"""L20 assets and JSON-calibrated passive coupling for dex-retargeting."""
from pathlib import Path
import json
import xml.etree.ElementTree as ET
import numpy as np
from dex_retargeting.kinematics_adaptor import MimicJointKinematicAdaptor
import l20_model_source as source

ROOT = Path(__file__).resolve().parents[1]
source.REPO_ROOT = ROOT / 'third_party/l20_assets'
ASSET = source.REPO_ROOT / 'L20/RIGHT'
FINGERS = ['thumb','index','middle','ring','pinky']
TIPS = np.array([4,8,12,16,20])
CHAINS = [(1,2,3,4),(5,6,7,8),(9,10,11,12),(13,14,15,16),(17,18,19,20)]

def palm_basis(p):
    z=p[9]-p[0]; z=z/np.linalg.norm(z)
    y=p[5]-p[17]; y=y-y.dot(z)*z; y=y/np.linalg.norm(y)
    return np.column_stack([np.cross(y,z),y,z])

class Calibration:
    def __init__(self):
        self.raw=json.loads((ASSET/'g20_right_G20.json').read_text())
        self.tables=self.raw['joints']
        urdf=ET.parse(ASSET/'linkerhand_g20_right.urdf').getroot()
        self.urdf_limits={j.get('name'):np.array([float(j.find('limit').get(k)) for k in ['lower','upper']]) for j in urdf.findall('joint')}
        self.active=sorted([n for n,x in self.tables.items() if not x.get('passive',False)],key=lambda n:self.tables[n]['motor_index'])
        self.passive=[n for n in self.tables if n not in self.active]
        self.values={n:np.array(x['angle_rad'])+x.get('zero_angles',{}).get('urdf_zero_offset_rad',0) for n,x in self.tables.items()}
        self.valid_commands={}
        self.bounds={}
        for n in self.active:
            motor=self.tables[n]['motor_index']
            valid=np.ones(256,dtype=bool)
            for name,x in self.tables.items():
                if x['motor_index']==motor:
                    low,high=self.urdf_limits[name]
                    valid &= (self.values[name]>=low)&(self.values[name]<=high)
            commands=np.flatnonzero(valid)
            assert len(commands)>1 and np.all(np.diff(commands)==1),n
            self.valid_commands[n]=commands
            self.bounds[n]=[self.values[n][commands].min(),self.values[n][commands].max()]
        self.curves={}
        for n in self.passive:
            motor=self.tables[n]['motor_index']
            active=next(a for a in self.active if self.tables[a]['motor_index']==motor)
            order=np.argsort(self.values[active])
            x=self.values[active][order]; y=self.values[n][order]
            x,idx=np.unique(x,return_index=True)
            self.curves[n]=(active,x,y[idx])

    def passive_value(self,name,q):
        _,x,y=self.curves[name]
        i=np.clip(np.searchsorted(x,q,side='right')-1,0,len(x)-2)
        slope=(y[i+1]-y[i])/(x[i+1]-x[i])
        return float(np.interp(q,x,y)),float(slope)

    def command(self,q_by_name):
        cmd=np.array(self.raw['baseline_command_u8'],dtype=np.uint8)
        for n in self.active:
            allowed=self.valid_commands[n]
            cmd[self.tables[n]['motor_index']]=allowed[np.argmin(np.abs(self.values[n][allowed]-q_by_name[n]))]
        return cmd

    def decode(self,commands,joint_names):
        return np.array([self.values[n][commands[self.tables[n]['motor_index']]] for n in joint_names])

class CalibratedAdaptor(MimicJointKinematicAdaptor):
    def __init__(self,robot,calibration):
        c=calibration
        parents=[c.curves[n][0] for n in c.passive]
        super().__init__(robot,c.active,parents,c.passive,[1.]*len(parents),[0.]*len(parents))
        self.calibration=c

    def forward_qpos(self,pin_qpos):
        for i,n in enumerate(self.calibration.passive):
            value,slope=self.calibration.passive_value(n,pin_qpos[self.idx_pin2source[i]])
            pin_qpos[self.idx_pin2mimic[i]]=value
            self.multipliers[i]=slope
        return pin_qpos

def build_assets(out, side='right'):
    out.mkdir(parents=True,exist_ok=True)
    asset=source.REPO_ROOT/'L20'/side.upper()
    fixed=source.build_model(side,out)
    mj=ET.parse(fixed)
    # Preserve both the original URDF and JSON. Adapt only generated files.
    eq=mj.getroot().find('equality')
    if eq is not None: mj.getroot().remove(eq)
    mj.write(fixed)
    tree=ET.parse(asset/f'linkerhand_g20_{side}.urdf')
    root=tree.getroot()
    for mesh in root.iter('mesh'): mesh.set('filename',str(asset/mesh.get('filename')))
    for j in root.findall('joint'):
        mimic=j.find('mimic')
        if mimic is not None:j.remove(mimic)
    for body in mj.getroot().iter('body'):
        for site in body.findall('site'):
            name=site.get('name')
            ET.SubElement(root,'link',name=name)
            j=ET.SubElement(root,'joint',name=name+'_fixed',type='fixed')
            ET.SubElement(j,'parent',link=body.get('name'));ET.SubElement(j,'child',link=name)
            ET.SubElement(j,'origin',xyz=site.get('pos'),rpy='0 0 0')
    path=out/'l20_dex.urdf';tree.write(path)
    return fixed,path