Edited a lot of errors in the math and in the implementation and changed variable names. should work now