import * as THREE from 'three';
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
//import URDFLoader from './urdf/ExtendedURDFLoader.js';
import { createRotationSector, createAngleIndicator, createJointLabel } from './JointVisualiser.js';
import ExtendedURDFLoader from './urdf/ExtendedURDFLoader.js';
import { OBJLoader } from 'three/examples/jsm/loaders/OBJLoader.js';
import { MTLLoader } from 'three/examples/jsm/loaders/MTLLoader.js';
import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js';

const gltfLoader = new GLTFLoader();


export class URDFEditor {
  constructor(canvas) {
    this.canvas = canvas;

    this.scene = new THREE.Scene();
    this.scene.background = new THREE.Color(0xaaaaaa);

    this.camera = new THREE.PerspectiveCamera(45, canvas.clientWidth / canvas.clientHeight, 0.1, 100);
    this.camera.position.set(2, 2, 2);
    this.camera.lookAt(0, 0, 0);

    this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
    this.renderer.setSize(canvas.clientWidth, canvas.clientHeight);

    this.overlayCanvas = document.getElementById('overlay-canvas');
    this.overlayCtx = this.overlayCanvas.getContext('2d');

    // Match size to renderer
    const resizeOverlay = () => {
      const { clientWidth, clientHeight } = this.renderer.domElement;
      this.overlayCanvas.width = clientWidth;
      this.overlayCanvas.height = clientHeight;
    };
    window.addEventListener('resize', resizeOverlay);
    resizeOverlay();


    this.controls = new OrbitControls(this.camera, this.renderer.domElement);
    this.controls.target.set(0, 0.5, 0);
    this.controls.update();

    this.raycaster = new THREE.Raycaster();
    this.mouse = new THREE.Vector2();

    this.hoveredJoint = null;
    this.draggedJoint = null;
    this.worldAxis = null;
    this.lastX = null;
    this.isDragging = false;
    this.jointAngles = {};

    this.loader = new ExtendedURDFLoader();
    this.loader.loadMeshCb = (path, manager, onComplete) => {
      if (path.endsWith('.glb') || path.endsWith('.gltf')) {
        const gltfLoader = new GLTFLoader(manager);
        gltfLoader.load(path, (gltf) => {
          onComplete(gltf.scene);
        });
      } else {
        // fall back to default behavior (STL, DAE)
        URDFLoader.prototype.loadMeshCb.call(this.loader, path, manager, onComplete);
      }
    };

    this.loader.packages = {
      'Little_Sophia_Face': '/robots/LittleSophia'
    };
    this.loader.parseVisual = true;
    this.loader.parseCollision = false;

    this.setupScene();
    this.loadURDF();
    this.setupEvents();



    // Add some lights (optional if using MeshBasicMaterial)
    const ambient = new THREE.AmbientLight(0x404040);
    this.scene.add(ambient);
    const dirLight = new THREE.DirectionalLight(0xffffff, 1);
    dirLight.position.set(5, 5, 5);
    this.scene.add(dirLight);

    // this.mesh = null;
    // gltfLoader.load('meshes/Little_Sophia.glb', (gltf) => {
    //   const model = gltf.scene;
    //   model.scale.set(0.001, 0.001, 0.001); // adjust if needed
    //   model.rotation.y = Math.PI; // radians (180°)
    //   this.scene.add(model);

    //   // Optional: fit camera to model
    //   const box = new THREE.Box3().setFromObject(model);
    //   const center = box.getCenter(new THREE.Vector3());
    //   const size = box.getSize(new THREE.Vector3()).length();
    // });


    this.animate();
  }

  setupScene() {
    this.scene.add(new THREE.AmbientLight(0xffffff, 0.6));
    const light1 = new THREE.DirectionalLight(0xffffff, 0.8);
    light1.position.set(15, 10, 7);
    this.scene.add(light1);

    const light2 = new THREE.DirectionalLight(0xffffff, 0.8);
    light2.position.set(-15, 10, -7);
    this.scene.add(light2);

    this.scene.add(new THREE.AxesHelper(1));
    this.scene.add(new THREE.GridHelper(2, 10));
  }

  async loadURDF() {
    this.urdfPath = '/robots/LittleSophia/urdf/LittleSophia.urdf';

    const urdfText = await fetch(this.urdfPath).then(res => res.text());
    const robot = await this.loader.loadFromString(urdfText);

    robot.rotation.x = -Math.PI / 2;
    this.scene.add(robot);
    this.robot = robot;

    for (const jointName in this.robot.joints) {
      console.log(jointName, this.robot.joints[jointName].transmission);
    }
  }



  findObjectByName(root, name) {
    let result = null;
    root.traverse(child => {
      if (child.name === name) {
        result = child;
      }
    });
    return result;
  }


  setupEvents() {
    this.canvas.addEventListener('pointermove', this.onPointerMove.bind(this));
    this.canvas.addEventListener('pointerdown', this.onPointerDown.bind(this));
    this.canvas.addEventListener('pointerup', this.onPointerUp.bind(this));
  }

  onPointerMove(event) {
    this.mouse.x = (event.clientX / this.canvas.clientWidth) * 2 - 1;
    this.mouse.y = -(event.clientY / this.canvas.clientHeight) * 2 + 1;

    this.raycaster.setFromCamera(this.mouse, this.camera);

    if (!this.robot) return;
    const intersects = this.raycaster.intersectObjects(this.robot.children, true);

    if (this.isDragging && this.draggedJoint && this.worldAxis) {
      const deltaX = event.clientX - this.lastX;
      this.lastX = event.clientX;
      const angleDelta = deltaX * 0.005;

      const jointName = this.draggedJoint.name;

      // ✅ Use transmission-derived limits if available
      const { lower, upper } = getJointLimits(jointName, this.robot);

      const currentAngle = this.jointAngles[jointName] ?? 0;
      const proposedAngle = currentAngle + angleDelta;

      // ✅ Clamp to safe range
      const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle));
      const actualDelta = clampedAngle - currentAngle;

      this.draggedJoint.rotateOnAxis(this.worldAxis, actualDelta);
      this.jointAngles[jointName] = clampedAngle;

      // ✅ Update gizmo indicator
      const gizmo = this.draggedJoint.userData.gizmo;
      if (gizmo?.indicator) this.draggedJoint.parent.remove(gizmo.indicator);

      const newIndicator = createAngleIndicator(this.worldAxis, clampedAngle);
      newIndicator.position.copy(this.draggedJoint.position);
      this.draggedJoint.parent.add(newIndicator);
      this.draggedJoint.userData.gizmo.indicator = newIndicator;
    }

    // Hover highlighting
    if (intersects.length > 0) {
      const target = intersects[0].object;
      if (this.hoveredJoint?.material?.emissive) {
        this.hoveredJoint.material.emissive.setHex(0x000000);
      }
      this.hoveredJoint = target;
      if (this.hoveredJoint.material?.emissive) {
        this.hoveredJoint.material.emissive.setHex(0x333333);
      }
    } else {
      if (this.hoveredJoint?.material?.emissive) {
        this.hoveredJoint.material.emissive.setHex(0x000000);
      }
      this.hoveredJoint = null;
    }
  }


  onPointerDown(event) {
    if (!this.hoveredJoint) return;

    this.isDragging = true;
    this.lastX = event.clientX;
    this.controls.enabled = false;

    this.draggedJoint = this.findJointAncestor(this.hoveredJoint);
    if (!this.draggedJoint) return;

    const jointName = this.draggedJoint.name;
    const jointData = this.robot.joints?.[jointName];
    if (!(jointData?.axis instanceof THREE.Vector3)) return;

    const axis = jointData.axis.clone();
    if (axis.lengthSq() === 0) return;

    this.worldAxis = axis.normalize();

    // ✅ Use transmission-derived limits if available
    const { lower, upper } = getJointLimits(jointName, this.robot);
    console.log(lower, upper);

    const jointTransmission = this.robot.joints[jointName].transmission;
    const encoderRange = jointTransmission.encoderRange / jointTransmission.mechanicalReduction;
    const sector = createRotationSector(this.worldAxis, lower, upper, encoderRange);
    const indicator = createAngleIndicator(this.worldAxis, this.jointAngles[jointName] ?? 0);

    sector.position.copy(this.draggedJoint.position);
    indicator.position.copy(this.draggedJoint.position);

    this.draggedJoint.parent.add(sector);
    this.draggedJoint.parent.add(indicator);
    this.draggedJoint.userData.gizmo = { sector, indicator };
  }


  onPointerUp() {
    this.isDragging = false;
    this.controls.enabled = true;

    const gizmo = this.draggedJoint?.userData.gizmo;
    if (gizmo) {
      this.draggedJoint.parent.remove(gizmo.sector);
      this.draggedJoint.parent.remove(gizmo.indicator);
    }

    this.draggedJoint = null;
    this.worldAxis = null;
  }

  findJointAncestor(object) {
    while (object && object.parent) {
      if (object.type === 'URDFJoint') return object;
      object = object.parent;
    }
    return null;
  }

  animate() {
    requestAnimationFrame(() => this.animate());
    this.drawJointOverlay(); // ✅ fixed overlay
    //console.log(this.robot.joints)
    this.renderer.render(this.scene, this.camera);
    //console.log(this.mesh);
  }



  drawJointOverlay() {
    const ctx = this.overlayCtx;
    if (!ctx || !this.robot?.joints) return;

    ctx.clearRect(0, 0, this.overlayCanvas.width, this.overlayCanvas.height);
    ctx.fillStyle = 'rgba(0, 0, 0, 0.5)';
    ctx.fillRect(0, 0, 300, this.overlayCanvas.height);

    ctx.font = '14px monospace';
    ctx.fillStyle = '#0f0';
    ctx.textBaseline = 'top';

    let y = 10;
    const nameX = 10;
    const angleX = 220; // adjust for spacing

    for (const jointName in this.robot.joints) {
      const jointData = this.robot.joints[jointName];
      const jointObject = this.findObjectByName(this.robot, jointName);
      if (!jointData || !jointObject) continue;

      const angle = this.jointAngles?.[jointName] ?? 0;
      const degrees = (angle * 180 / Math.PI).toFixed(1);

      const rotatedChild = jointObject.children?.[0];
      const childName = rotatedChild?.name || '(unnamed)';

      ctx.fillText(childName, nameX, y);
      const angleStr = `${degrees}°`;
      const angleWidth = ctx.measureText(angleStr).width;
      ctx.fillText(angleStr, angleX - angleWidth, y);

      const t = jointData.transmission;
      if (t) {
        //const actuatorAngleDeg = (degrees / t.mechanicalReduction) + 4096 / 2;
        const ticks = radiansToTicks(angle, t).toFixed(0);
        const isValid = ticks >= t.encoderValidMin && ticks <= t.encoderValidMax;

        //ctx.fillStyle = isValid ? '#0f0' : '#f00';
        ctx.fillText(`${ticks}`, nameX + 250, y);
      }

      y += 18;
    }
  }
}

function ticksToRadians(ticks, actuator) {
  const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75
  const centered = ticks - actuator.encoderTicks / 2; // shift so mid = 0
  const actuatorDeg = centered / ticksPerDeg;
  const jointDeg = actuatorDeg / actuator.mechanicalReduction;
  return jointDeg * (Math.PI / 180); // convert degrees → radians
}

function radiansToTicks(radians, actuator) {
  const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75
  const jointDeg = radians * (180 / Math.PI); // radians → degrees
  const actuatorDeg = jointDeg * actuator.mechanicalReduction; // apply reduction
  const centered = actuatorDeg * ticksPerDeg; // convert to ticks offset from mid
  const ticks = centered + actuator.encoderTicks / 2; // shift back to full range
  return ticks;
}


function getJointLimits(jointName, robot) {
  const transmission = robot.joints[jointName].transmission;
  //console.log(transmission.encoderValidMin, transmission.encoderValidMax);
  const lowerRad = ticksToRadians(transmission.encoderValidMin, transmission);
  const upperRad = ticksToRadians(transmission.encoderValidMax, transmission);
  //console.log(lowerRad, upperRad);
  return { lower: lowerRad, upper: upperRad };
}

function getJointTransmission(jointName, robot) {
  return robot.joints[jointName].transmission;
}

function loadObjWithMtl(objPath, mtlPath, onLoad) {
  const mtlLoader = new MTLLoader();
  mtlLoader.load(mtlPath, (materials) => {
    materials.preload();
    const objLoader = new OBJLoader();
    objLoader.setMaterials(materials);
    objLoader.load(objPath, (object) => {
      onLoad(object);
    });
  });
}