Still incorrect, noticible problem is that as you rotate too far one direction the computed lengths of the links changes which shouldn't happen?