"""Mirror calibrated O30 V10; retain existing V9 filenames for compatibility."""
from pathlib import Path
import hashlib
import json
import shutil
import struct
import xml.etree.ElementTree as ET

import numpy as np

ROOT = Path(__file__).resolve().parents[2]
MODEL = ROOT / 'src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O30'
SOURCE = MODEL / 'linker_hand_o30_right/linkerhand_o30_right_zero_v10_calibrated.urdf'
OUTPUT = ROOT / 'assets/exports/O30_V10_left_right'
RIGHT = OUTPUT / 'right/linkerhand_o30_right_v10.urdf'
LEFT = OUTPUT / 'left/linkerhand_o30_left_v10.urdf'


def mirror_stl(source, target):
    raw = source.read_bytes()
    count = struct.unpack_from('<I', raw, 80)[0]
    if len(raw) != 84 + 50 * count:
        raise ValueError(f'Expected binary STL: {source}')
    dtype = np.dtype([('normal', '<f4', (3,)), ('vertices', '<f4', (3, 3)), ('attr', '<u2')])
    triangles = np.frombuffer(raw, dtype=dtype, offset=84).copy()
    triangles['normal'][:, 1] *= -1
    triangles['vertices'][:, :, 1] *= -1
    # Reflection reverses handedness: swap vertex winding to retain outward normals.
    triangles['vertices'] = triangles['vertices'][:, [0, 2, 1], :]
    target.write_bytes(raw[:84] + triangles.tobytes())


def build():
    for side in ['right', 'left']:
        (OUTPUT / side / 'meshes').mkdir(parents=True, exist_ok=True)
    shutil.copy2(SOURCE, RIGHT)
    tree = ET.parse(SOURCE)
    root = tree.getroot()
    root.set('name', 'linkerhand_o30_left_v10_mirrored')
    def reflect_attr(element, attr, signs):
        values = np.fromstring(element.get(attr, '0 0 0'), sep=' ')
        element.set(attr, ' '.join(format(float(v), '.16g') for v in values * signs))
    for origin in root.findall('.//origin'):
        reflect_attr(origin, 'xyz', [1, -1, 1])
        # S R(r,p,y) S = R(-r,p,-y), S=diag(1,-1,1).
        reflect_attr(origin, 'rpy', [-1, 1, -1])
    for axis in root.findall('.//axis'):
        # Angular vectors are axial vectors, transformed by det(S)*S.
        reflect_attr(axis, 'xyz', [-1, 1, -1])
    for inertia in root.findall('.//inertia'):
        for name in ['ixy', 'iyz']:
            inertia.set(name, format(-float(inertia.get(name)), '.16g'))
    for mesh in root.findall('.//mesh'):
        filename = mesh.get('filename')
        if not filename.startswith('meshes/'):
            raise ValueError(f'Expected relative mesh path: {filename}')
    for filename in sorted({m.get('filename') for m in root.findall('.//mesh')}):
        shutil.copy2(SOURCE.parent / filename, RIGHT.parent / filename)
        mirror_stl(SOURCE.parent / filename, LEFT.parent / filename)
    ET.indent(tree, space='  ')
    tree.write(LEFT, encoding='utf-8', xml_declaration=True)
    samples = json.loads((ROOT / 'tools/o30_right_calibration/samples_v9.json').read_text())
    shutil.copy2(ROOT / 'archive/exports/O30_right_V10_linear/joint_order.txt', OUTPUT / 'sdk_joint_order.txt')
    # External commands use SDK order; radians below are internal reference targets.
    sdk_order = ['thumb_cmc_roll','thumb_cmc_yaw','index_mcp_roll','middle_mcp_roll',
                 'ring_mcp_roll','pinky_mcp_roll','thumb_mcp','index_mcp_pitch',
                 'middle_mcp_pitch','ring_mcp_pitch','pinky_mcp_pitch','index_pip',
                 'middle_pip','ring_pip','pinky_pip','thumb_ip','index_dip',
                 'middle_dip','ring_dip','pinky_dip']
    order = [j.get('name') for j in root.findall('joint') if j.get('type') == 'revolute']
    commands = {}
    for sample in samples['samples']:
        angles = {}
        for name, command in zip(sdk_order, sample['command']):
            limit = root.find(f"joint[@name='{name}']/limit")
            lo, hi = float(limit.get('lower')), float(limit.get('upper'))
            angles[name] = lo + (hi - lo) * command / 255
        commands[sample['finger']] = dict(sdk_command=sample['command'], left_ros_radians=[angles[n] for n in order])
    zero = json.loads((ROOT / 'archive/exports/O30_right_V10_linear/zero_command.json').read_text())
    zero_angles = {}
    for name, value in zip(sdk_order, zero):
        limit = root.find(f"joint[@name='{name}']/limit")
        lo, hi = float(limit.get('lower')), float(limit.get('upper'))
        zero_angles[name] = lo + (hi - lo) * value / 255
    (OUTPUT / 'commands.json').write_text(json.dumps(dict(
        asset_version='V10', command_topic='/cb_left_hand_control_cmd',
        command_units='SDK 0..255; send sdk_command, not left_ros_radians',
        sdk_joint_order=sdk_order, left_ros_order=order, pinches=commands,
        zero=dict(sdk_command=zero, left_ros_radians=[zero_angles[n] for n in order])), indent=2)+'\n')
    (OUTPUT / 'version.json').write_text(json.dumps(dict(
        asset_version='V10', source=str(SOURCE.relative_to(ROOT)),
        source_sha256=hashlib.sha256(SOURCE.read_bytes()).hexdigest(),
        note='V9 directory and filenames retained; both assets now contain V10 calibration'), indent=2)+'\n')
    print(OUTPUT)


if __name__ == '__main__':
    build()