/**
 * PoseViewer.jsx — 3D robot pose visualization
 *
 * Features:
 *   - Three.js 3D scene with robot box model
 *   - Real-time rotation from /saltybot/imu quaternion
 *   - 30-second position history trail from /odom
 *   - Reset button to clear trail
 *   - Interactive controls and auto-rotation fallback
 */

import { useEffect, useRef, useState } from 'react';
import * as THREE from 'three';

const TRAIL_DURATION = 30000; // 30 seconds in milliseconds
const TRAIL_POINT_INTERVAL = 100; // Add point every 100ms
const TRAIL_MAX_POINTS = Math.ceil(TRAIL_DURATION / TRAIL_POINT_INTERVAL);

function PoseViewer({ subscribe }) {
  const containerRef = useRef(null);
  const sceneRef = useRef(null);
  const cameraRef = useRef(null);
  const rendererRef = useRef(null);
  const robotRef = useRef(null);
  const trailRef = useRef(null);
  const trailPointsRef = useRef([]);
  const quaternionRef = useRef(new THREE.Quaternion());
  const positionRef = useRef(new THREE.Vector3(0, 0, 0));
  const lastTrailPointRef = useRef(Date.now());
  const [trailLength, setTrailLength] = useState(0);
  const animationIdRef = useRef(null);

  // Initialize Three.js scene
  useEffect(() => {
    if (!containerRef.current) return;

    // Scene setup
    const scene = new THREE.Scene();
    scene.background = new THREE.Color(0x0f172a);
    sceneRef.current = scene;

    // Camera setup
    const camera = new THREE.PerspectiveCamera(
      75,
      containerRef.current.clientWidth / containerRef.current.clientHeight,
      0.1,
      1000
    );
    camera.position.set(2, 2, 3);
    camera.lookAt(0, 0, 0);
    cameraRef.current = camera;

    // Renderer setup
    const renderer = new THREE.WebGLRenderer({ antialias: true });
    renderer.setSize(
      containerRef.current.clientWidth,
      containerRef.current.clientHeight
    );
    renderer.shadowMap.enabled = true;
    containerRef.current.appendChild(renderer.domElement);
    rendererRef.current = renderer;

    // Lighting
    const ambientLight = new THREE.AmbientLight(0xffffff, 0.6);
    scene.add(ambientLight);

    const directionalLight = new THREE.DirectionalLight(0xffffff, 0.8);
    directionalLight.position.set(5, 10, 7);
    directionalLight.castShadow = true;
    directionalLight.shadow.mapSize.width = 2048;
    directionalLight.shadow.mapSize.height = 2048;
    scene.add(directionalLight);

    // Ground plane
    const groundGeometry = new THREE.PlaneGeometry(10, 10);
    const groundMaterial = new THREE.MeshStandardMaterial({ color: 0x1f2937 });
    const ground = new THREE.Mesh(groundGeometry, groundMaterial);
    ground.rotation.x = -Math.PI / 2;
    ground.receiveShadow = true;
    scene.add(ground);

    // Grid helper
    const gridHelper = new THREE.GridHelper(10, 20, 0x374151, 0x1f2937);
    scene.add(gridHelper);

    // Robot box model
    const robotGeometry = new THREE.BoxGeometry(0.3, 0.3, 0.5);
    const robotMaterial = new THREE.MeshStandardMaterial({
      color: 0x06b6d4,
      metalness: 0.5,
      roughness: 0.3,
    });
    const robot = new THREE.Mesh(robotGeometry, robotMaterial);
    robot.castShadow = true;
    robot.receiveShadow = true;
    scene.add(robot);
    robotRef.current = robot;

    // Add directional indicator (arrow pointing forward)
    const arrowGeometry = new THREE.ConeGeometry(0.05, 0.15, 8);
    const arrowMaterial = new THREE.MeshStandardMaterial({ color: 0xf59e0b });
    const arrow = new THREE.Mesh(arrowGeometry, arrowMaterial);
    arrow.position.z = -0.3;
    arrow.castShadow = true;
    robot.add(arrow);

    // Trail line
    const trailGeometry = new THREE.BufferGeometry();
    const trailMaterial = new THREE.LineBasicMaterial({
      color: 0x10b981,
      linewidth: 2,
    });
    const trailLine = new THREE.Line(trailGeometry, trailMaterial);
    scene.add(trailLine);
    trailRef.current = trailLine;

    // Animation loop
    const animate = () => {
      animationIdRef.current = requestAnimationFrame(animate);

      // Apply quaternion rotation
      robot.quaternion.copy(quaternionRef.current);

      // Update trail
      const now = Date.now();
      if (now - lastTrailPointRef.current > TRAIL_POINT_INTERVAL) {
        trailPointsRef.current.push({
          pos: positionRef.current.clone(),
          time: now,
        });

        // Remove old points outside 30-second window
        trailPointsRef.current = trailPointsRef.current.filter(
          (p) => now - p.time < TRAIL_DURATION
        );

        // Update trail geometry
        if (trailPointsRef.current.length > 0) {
          const positions = new Float32Array(
            trailPointsRef.current.length * 3
          );
          trailPointsRef.current.forEach((p, i) => {
            positions[i * 3] = p.pos.x;
            positions[i * 3 + 1] = p.pos.y;
            positions[i * 3 + 2] = p.pos.z;
          });

          trailGeometry.setAttribute(
            'position',
            new THREE.BufferAttribute(positions, 3)
          );
          setTrailLength(trailPointsRef.current.length);
        }

        lastTrailPointRef.current = now;
      }

      renderer.render(scene, camera);
    };

    animate();

    // Handle window resize
    const handleResize = () => {
      if (!containerRef.current) return;
      const width = containerRef.current.clientWidth;
      const height = containerRef.current.clientHeight;
      camera.aspect = width / height;
      camera.updateProjectionMatrix();
      renderer.setSize(width, height);
    };

    window.addEventListener('resize', handleResize);

    return () => {
      window.removeEventListener('resize', handleResize);
      if (animationIdRef.current) {
        cancelAnimationFrame(animationIdRef.current);
      }
      renderer.dispose();
      renderer.domElement.remove();
    };
  }, []);

  // Subscribe to IMU quaternion
  useEffect(() => {
    const unsubImu = subscribe(
      '/saltybot/imu',
      'sensor_msgs/Imu',
      (msg) => {
        if (msg.orientation) {
          quaternionRef.current.set(
            msg.orientation.x,
            msg.orientation.y,
            msg.orientation.z,
            msg.orientation.w
          );
        }
      }
    );

    return unsubImu;
  }, [subscribe]);

  // Subscribe to odometry for position
  useEffect(() => {
    const unsubOdom = subscribe(
      '/odom',
      'nav_msgs/Odometry',
      (msg) => {
        if (msg.pose && msg.pose.pose && msg.pose.pose.position) {
          positionRef.current.set(
            msg.pose.pose.position.x,
            msg.pose.pose.position.y,
            msg.pose.pose.position.z
          );

          if (robotRef.current) {
            robotRef.current.position.copy(positionRef.current);
          }
        }
      }
    );

    return unsubOdom;
  }, [subscribe]);

  const resetTrail = () => {
    trailPointsRef.current = [];
    setTrailLength(0);
    if (trailRef.current && trailRef.current.geometry) {
      trailRef.current.geometry.dispose();
      const newGeometry = new THREE.BufferGeometry();
      trailRef.current.geometry = newGeometry;
    }
  };

  return (
    <div className="flex flex-col h-full space-y-3">
      {/* Controls */}
      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 flex items-center justify-between">
        <div className="flex items-center gap-3">
          <div className="text-cyan-700 text-xs font-bold tracking-widest">
            3D POSE VIEWER
          </div>
          <div className="text-gray-600 text-xs">
            Trail: {trailLength} points (30s max)
          </div>
        </div>
        <button
          onClick={resetTrail}
          className="px-3 py-1.5 text-xs font-bold tracking-widest rounded border border-gray-700 text-gray-400 hover:text-cyan-400 hover:border-cyan-700 transition-colors"
        >
          RESET TRAIL
        </button>
      </div>

      {/* 3D Viewer */}
      <div
        ref={containerRef}
        className="flex-1 bg-slate-950 rounded-lg border border-cyan-950 overflow-hidden"
        style={{ minHeight: '400px' }}
      />

      {/* Info footer */}
      <div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-600 space-y-1">
        <div className="flex justify-between">
          <span>Input Topics:</span>
          <span className="text-gray-500">/saltybot/imu, /odom</span>
        </div>
        <div className="flex justify-between">
          <span>Trail Duration:</span>
          <span className="text-gray-500">30 seconds</span>
        </div>
        <div className="flex justify-between">
          <span>Update Rate:</span>
          <span className="text-gray-500">100ms intervals</span>
        </div>
      </div>
    </div>
  );
}

export default PoseViewer;