import * as THREE from 'three';
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
import URDFLoader from './urdf/URDFLoader.js';

// Setup scene, camera, renderer
const canvas = document.getElementById('urdf-canvas');
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
renderer.setSize(window.innerWidth, window.innerHeight);

const scene = new THREE.Scene();
scene.background = new THREE.Color(0x111111);

const camera = new THREE.PerspectiveCamera(45, window.innerWidth / window.innerHeight, 0.1, 100);
camera.position.set(2, 2, 2);
camera.lookAt(0, 0, 0);

// Lights
scene.add(new THREE.AmbientLight(0xffffff, 0.6));
const directional = new THREE.DirectionalLight(0xffffff, 0.8);
directional.position.set(5, 10, 7);
scene.add(directional);

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

// Helpers
scene.add(new THREE.AxesHelper(1));
const gridHelper = new THREE.GridHelper(2, 10);
gridHelper.rotation.x = Math.PI / 2;
gridHelper.position.z = 0;
scene.add(gridHelper);

// Raycasting
const raycaster = new THREE.Raycaster();
const mouse = new THREE.Vector2();
let hoveredJoint = null;
let isDragging = false;
let lastX = null;
let worldAxis = null;

let draggedJoint = null;


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



// Load URDF
const loader = new URDFLoader();
loader.packages = { '': './urdf/' };

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

loader.load('./urdf/sample.urdf', robot => {
    //scene.rotation.x = -Math.PI / 2;
    robot.rotation.x = -Math.PI / 2;
    scene.add(robot);
    document.getElementById('status').textContent = 'URDF Loaded';

    loader.parseVisual = true;
    loader.parseCollision = false;

    // Add AxesHelpers to all meshes
    robot.traverse(obj => {
        if (obj.isMesh) obj.add(new THREE.AxesHelper(0.05));
    });

    // Setup pointer events
    canvas.addEventListener('pointermove', event => {
        mouse.x = (event.clientX / canvas.clientWidth) * 2 - 1;
        mouse.y = -(event.clientY / canvas.clientHeight) * 2 + 1;

        raycaster.setFromCamera(mouse, camera);
        const intersects = raycaster.intersectObjects(robot.children, true);

        if (isDragging && draggedJoint && worldAxis) {
            const deltaX = event.clientX - lastX;
            lastX = event.clientX;
            const angle = deltaX * 0.005;
            console.log(worldAxis, angle);
            draggedJoint.rotateOnAxis(worldAxis, angle);
        }


        if (intersects.length > 0) {
            const target = intersects[0].object;

            // Reset previous highlight
            if (
                hoveredJoint &&
                hoveredJoint.material &&
                'emissive' in hoveredJoint.material &&
                typeof hoveredJoint.material.emissive.setHex === 'function'
            ) {
                hoveredJoint.material.emissive.setHex(0x000000);
            }

            hoveredJoint = target;

            // Highlight new joint
            if (
                hoveredJoint.material &&
                'emissive' in hoveredJoint.material &&
                typeof hoveredJoint.material.emissive.setHex === 'function'
            ) {
                hoveredJoint.material.emissive.setHex(0x333333);
            }

        } else {
            if (
                hoveredJoint &&
                hoveredJoint.material &&
                'emissive' in hoveredJoint.material &&
                typeof hoveredJoint.material.emissive.setHex === 'function'
            ) {
                hoveredJoint.material.emissive.setHex(0x000000);
            }
            hoveredJoint = null;
        }

    });

    canvas.addEventListener('pointerdown', event => {
        if (!hoveredJoint) return;

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

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

        const jointName = draggedJoint.name;
        const jointData = robot.joints?.[jointName];

        // ✅ Check if axis is a valid THREE.Vector3
        if (!(jointData?.axis instanceof THREE.Vector3)) {
            console.warn(`Invalid axis for joint "${jointName}":`, jointData?.axis);
            return;
        }

        const urdfAxis = jointData.axis.clone();
        if (urdfAxis.lengthSq() === 0) {
            console.warn(`Zero-length axis for joint "${jointName}"`);
            return;
        }

        worldAxis = urdfAxis.normalize();

        console.log(`Dragging joint "${jointName}" on axis`, worldAxis.toArray());
    });






    canvas.addEventListener('pointerup', () => {
        isDragging = false;
        controls.enabled = true;
        draggedJoint = null;
        worldAxis = null;
    });


    animate();
});

function animate() {
    requestAnimationFrame(animate);
    renderer.render(scene, camera);
}