Is the Floor Cleaner Home Yet?

A floor-cleaning robot is being tested on scripted patterns. Each command is a pair:

• ("turn", a): turn on the spot by a radians (positive is counter-clockwise);
• ("drive", d): drive d metres straight along the current heading (negative d reverses).

Driving d along heading \theta moves the robot by (d\cos\theta,\ d\sin\theta). Whether a pattern brings the robot back does not depend on where it starts, so take the start pose to be (0, 0, 0): the origin, facing the +x axis.

The robot counts as back at its start pose after a command when both hold:

• its distance from the start position is at most pos_tol metres, and
• its heading differs from the start heading by at most angle_tol radians, where the difference is wrapped into (-\pi, \pi] first (so a heading of 2\pi, a full turn, differs by 0).

Write first_return(commands, pos_tol, angle_tol) that returns the smallest k >= 1 such that the robot is back at its start pose after the first k commands, or None if that never happens.

Examples

Input:  commands = [("drive", 2), ("turn", math.pi / 2)] * 4, pos_tol = 1e-6, angle_tol = 1e-6
Output: 8
Explanation: a 2 m square. After 7 commands the robot is at the start position but faces
-pi/2 (straight down), so it is not yet back; the last quarter turn completes the pose.

Input:  commands = [("drive", 1), ("drive", -0.9995)], pos_tol = 0.001, angle_tol = 0.01
Output: 2
Explanation: it ends 0.0005 m from the start, facing the same way.

Input:  commands = [("turn", 1)], pos_tol = 0.1, angle_tol = 0.1
Output: None

Constraints

• no test puts the robot within 1e-9 of either tolerance, so < and <= give the same answer

Goals

• Track a pose through a command sequence and test it after every command
• Compare headings by their wrapped difference, so a full turn counts as no turn
• Accept a pose as "the same" when it is within a stated tolerance