Fixed the math Algorithm

I'm so happy :P
Stage 1 cleanign