Sorta fixed but it cant solve IK. Probably those distances I am calculating are not correct.