Fixed the easy parts of the algorithm. Bring on the math!

Signed-off-by: Joppe Widstam <joppewidstam@hotmail.com>