"""The back-to-front twin of a rig, which covers the same space with the textures turned round.

The head and torso boxes are centred on their pivots, so spinning either half a turn around its own
vertical axis leaves it covering the same space; only which face shows changes. The arms hang from
the torso's sides, so spinning the torso carries each arm to the other side. That twin (torso and
head facing backwards, arms swapped) is a separate local minimum the search can't walk out of, so
measure.py jumps to it directly, refines it, and keeps whichever scores lower. Only the colours tell
them apart.

In rig terms: the torso and head get R Ry(pi); each arm takes the other's rotation times Ry(pi) (a
half turn around its own length, which moves its off-centre box to the other side of its pivot); and
bends keep their shape with their direction turned half round, since the frame they're measured in
turned. The legs hang from the hips and don't change.
"""
import numpy as np

from model import INDEX, rotation_zyx

SPIN = np.diag([-1.0, 1.0, -1.0])  # Ry(pi)
ARMS = {"right_arm": "left_arm", "left_arm": "right_arm"}


def euler_zyx(r):
    """(pitch, yaw, roll) with rotation_zyx(pitch, yaw, roll) == r."""
    yaw = np.arcsin(np.clip(-r[2, 0], -1, 1))
    return np.arctan2(r[2, 1], r[2, 2]), yaw, np.arctan2(r[1, 0], r[0, 0])


def _spun(rig, source):
    g = lambda c: rig[INDEX[f"{source}_{c}"]]
    return euler_zyx(rotation_zyx(g("pitch"), g("yaw"), g("roll")) @ SPIN)


def flip_head(rig):
    """Just the head spun round: a cube on its pivot, so the same space facing the other way."""
    out = rig.copy()
    for c, v in zip(("pitch", "yaw", "roll"), _spun(rig, "head")):
        out[INDEX[f"head_{c}"]] = v
    return out


def twins(rig):
    """Every back-to-front twin worth trying: the torso with its arms, the head, and both."""
    return [flip(rig), flip_head(rig), flip_head(flip(rig))]


def flip(rig):
    out = rig.copy()
    for part, source in [("body", "body"), ("head", "head"), *ARMS.items()]:
        for c, v in zip(("pitch", "yaw", "roll"), _spun(rig, source)):
            out[INDEX[f"{part}_{c}"]] = v
    for part, source in [("body", "body"), *ARMS.items()]:
        out[INDEX[f"{part}_bend"]] = rig[INDEX[f"{source}_bend"]]
        out[INDEX[f"{part}_axis"]] = rig[INDEX[f"{source}_axis"]] + np.pi
    return out