Fixed some code, Still need to fix the math.