import {enhanceConvertedMjcf,groundConvertedMjcf} from './urdfToMjcf';

const encode=(value:string)=>new TextEncoder().encode(value);
const decode=(value:Uint8Array)=>new TextDecoder().decode(value);

describe('groundConvertedMjcf',()=>{
  it('抬升所有根 body，并在 z=0 添加地面',()=>{
    const result=decode(groundConvertedMjcf(encode('<mujoco><worldbody><body name="robot" pos="1 2 0.1"><geom type="box" size="1 1 1"/></body></worldbody></mujoco>'),-0.4,'fixed'));
    const document=new DOMParser().parseFromString(result,'application/xml');
    expect(document.querySelector('body[name="robot"]')?.getAttribute('pos')).toBe('1 2 0.5');
    expect(document.querySelector('geom[name="__platform_ground__"]')).toMatchObject({tagName:'geom'});
    expect(document.querySelector('geom[name="__platform_ground__"]')?.getAttribute('group')).toBe('5');
    expect(document.querySelector('freejoint')).toBeNull();
  });

  it('为浮动基座的每个世界根 body 添加 freejoint',()=>{
    const result=decode(groundConvertedMjcf(encode('<mujoco><worldbody><body name="robot"><geom type="sphere" size="1"/></body></worldbody></mujoco>'),-1,'floating'));
    const document=new DOMParser().parseFromString(result,'application/xml');
    expect(document.querySelector('body[name="robot"] > freejoint')?.getAttribute('name')).toBe('__platform_base_freejoint__');
    expect(document.querySelector('body[name="robot"]')?.getAttribute('pos')).toBe('0 0 1');
  });
});

describe('enhanceConvertedMjcf',()=>{
  it('为可驱动关节补充 motor，并跳过已有驱动器和 ball joint',()=>{
    const source='<mujoco><worldbody><body name="robot"><joint name="shoulder" type="hinge"/><body><joint name="slider" type="slide"/><joint name="ball" type="ball"/><joint name="parent_driven"/></body></body></worldbody><actuator><position name="existing" joint="shoulder"/><motor name="parent_motor" jointinparent="parent_driven"/></actuator></mujoco>';
    const result=enhanceConvertedMjcf(encode(source),{addActuators:true,addSensors:false,sensorType:'camera'});
    const document=new DOMParser().parseFromString(decode(result.data),'application/xml');
    expect(result.actuatorCount).toBe(1);
    expect(document.querySelector('motor[joint="slider"]')?.getAttribute('ctrllimited')).toBe('false');
    expect(document.querySelector('motor[joint="slider"]')?.hasAttribute('ctrlrange')).toBe(false);
    expect(document.querySelector('motor[joint="slider"]')?.getAttribute('name')).toBe('slider_motor');
    expect(document.querySelector('motor[joint="slider"]')?.getAttribute('forcerange')).toBe('-100 100');
    expect(document.querySelector('joint[name="slider"]')?.getAttribute('stiffness')).toBe('0');
    expect(document.querySelector('joint[name="slider"]')?.getAttribute('damping')).toBe('0');
    expect(document.querySelectorAll('[joint="shoulder"]')).toHaveLength(1);
    expect(document.querySelectorAll('[jointinparent="parent_driven"], [joint="parent_driven"]')).toHaveLength(1);
    expect(document.querySelector('[joint="ball"]')).toBeNull();
  });

  it('将可调摄像头固连到指定机器人 body',()=>{
    const result=enhanceConvertedMjcf(encode('<mujoco><worldbody><body name="base"><body name="head"/></body></worldbody></mujoco>'),{addActuators:false,addSensors:true,sensorType:'camera',cameraMountBody:'head',cameraPosition:[.2,0,.1],cameraDirection:'+X'});
    const document=new DOMParser().parseFromString(decode(result.data),'application/xml');
    const camera=document.querySelector('body[name="head"] > camera');
    expect(result.cameraAdded).toBe(true);
    expect(camera?.getAttribute('mode')).toBe('fixed');
    expect(camera?.getAttribute('pos')).toBe('0.2 0 0.1');
    expect(camera?.getAttribute('xyaxes')).toBe('0 -1 0 0 0 1');
    expect(camera?.getAttribute('resolution')).toBe('640 480');
    const repeated=enhanceConvertedMjcf(result.data,{addActuators:false,addSensors:true,sensorType:'camera'});
    expect(new DOMParser().parseFromString(decode(repeated.data),'application/xml').querySelectorAll('camera')).toHaveLength(1);
  });

  it('关闭选项时不修改 actuator 和 camera',()=>{
    const result=enhanceConvertedMjcf(encode('<mujoco><worldbody><body name="robot"><joint name="joint"/></body></worldbody></mujoco>'),{addActuators:false,addSensors:false,sensorType:'camera'});
    const document=new DOMParser().parseFromString(decode(result.data),'application/xml');
    expect(document.querySelector('actuator')).toBeNull();
    expect(document.querySelector('camera')).toBeNull();
  });
});