Fixed logical error in algorithm
Fixed some bugs but still have to work a lot out