Added getAngleFromMotion (still not perfect) and cleaned up some codes