Working main loop, missing implementations for next_angle and next_origin