Made changes to the code, it's running faster now. Errors are low ~ 1e-3 maximum