From Cells to Wheel Commands

A planner hands a two-wheeled robot its route as a list of grid cells (row, col). The robot cannot follow cells, but it can do two things: turn in place by an angle, and drive straight for a distance. Translate the route into such commands.

Coordinates: the grid lies flat on the floor with x pointing along increasing col and y pointing towards decreasing row (row 0 is at the top of the map, the far wall). Headings are in radians, counter-clockwise from +x: a step to the next column (right) has heading 0, a step to the row above (up) has heading \pi/2, a step up and right \pi/4. Each cell is cell metres wide; a straight step is cell metres long and a diagonal one \sqrt{2} times as long. Consecutive cells of the route always touch by a side or a corner.

Cut the route into legs: maximal runs of consecutive steps in the same direction. For each leg, in order, emit one pair (turn, distance): first turn from the heading the robot has (initially heading) to the leg's heading, then drive the whole leg. Every turn is wrapped into (-\pi, \pi], so it takes the shorter way round, and an exact half turn is +\pi. A turn of 0.0 is still emitted.

Write drive_commands(path, heading, cell) that returns the list of (turn, distance) tuples; a route of one cell needs no commands, so return [].

Examples

Input:  path = [(2, 0), (2, 1), (2, 2), (1, 3), (0, 3)], heading = 0.0, cell = 0.5
Output: [(0.0, 1.0), (0.7853981633974483, 0.7071067811865476), (0.7853981633974483, 0.5)]
Explanation: two steps right (already facing right), one step up-right after a quarter
of a right angle to the left, one step up after another.

Input:  path = [(1, 1), (2, 1)], heading = 1.5707963267948966, cell = 0.25
Output: [(3.141592653589793, 0.25)]
Explanation: facing up, the robot must face down: -pi wrapped into (-pi, pi] is +pi.

Input:  path = [(3, 3)], heading = 0.0, cell = 1.0
Output: []

Constraints

• heading is in (-\pi, \pi]; floats are compared with a tolerance of 1e-6

Goals

• Turn a planned list of cells into turn-in-place and drive-straight commands
• Merge steps in the same direction into one drive
• Wrap each turn into $(-\pi, \pi]$, the shorter way round