Dwinzo_dev/app/src/modules/builder/agv/pathNavigator.tsx

import React, { useCallback, useEffect, useRef, useState } from "react";
import * as THREE from "three";
import { NavMeshQuery } from "@recast-navigation/core";
import { NavMesh as RecastNavMesh } from "@recast-navigation/core";
import { useFrame, useThree } from "@react-three/fiber";
import { Line } from "@react-three/drei";
interface Pair {
  x: number;
  y: number;
  z: number;
}
interface PathProps {
  navMesh: RecastNavMesh | null; // The navigation mesh
  pathPoints: Pair[] | undefined; // Array of points (or undefined)
}
const PathNavigator = ({ navMesh, pathPoints }: PathProps) => {
  const { scene, raycaster, gl } = useThree();

  const [path, setPath] = useState<THREE.Vector3[]>([]); // Path is an array of THREE.Vector3
  const [points, setSelectedPoints] = useState<THREE.Vector3[]>([]); // Path is an array of THREE.Vector3
  const progressRef = useRef<number>(0);
  const meshRef = useRef<THREE.Mesh>(null!);
  const handleClick = useCallback(() => {
    if (!navMesh) return;

    const intersects = raycaster.intersectObjects(scene.children, true);
    if (intersects.length > 0) {
      const { point } = intersects[0];
      const newPoint = { x: point.x, y: 0, z: point.z };

      setSelectedPoints((prevPoints: THREE.Vector3[]) => {
        if (prevPoints.length === 2) {
          // If two points already exist, replace them with the new point
          return [new THREE.Vector3(newPoint.x, newPoint.y, newPoint.z)];
        }
        // Otherwise, append the new point to the array
        return [
          ...prevPoints,
          new THREE.Vector3(newPoint.x, newPoint.y, newPoint.z),
        ];
      });
    }
  }, [navMesh, scene]);
  React.useEffect(() => {
    if (points?.length === 2 && navMesh) {
      const [start, end] = points;
      console.log("start: ", start);
      console.log("end: ", end);

      const navMeshQuery = new NavMeshQuery(navMesh);
      console.log("navMeshQuery: ", navMeshQuery);

      const { path } = navMeshQuery.computePath(start, end);
      console.log("paths: ", path);

      //   if (path.length > 0) {
      //     setPath(
      //       path.map((point) => {
      //         const newY = point.y + 0.1; // Increment the y-coordinate
      //         return new THREE.Vector3(point.x, newY, point.z); // Create a new Vector3
      //       })
      //     );
      //     progressRef.current = 0;
      //   }
    }
  }, [points,]);

  useFrame((_, delta) => {
    if (path.length > 1 && meshRef.current) {
      const speed = 3;
      progressRef.current += delta * speed;
      let totalDistance = 0;
      const distances = [];
      for (let i = 0; i < path.length - 1; i++) {
        const start = new THREE.Vector3(...path[i]);
        const end = new THREE.Vector3(...path[i + 1]);
        const segmentDistance = start.distanceTo(end);
        distances.push(segmentDistance);
        totalDistance += segmentDistance;
      }

      let coveredDistance = progressRef.current;
      let accumulatedDistance = 0;
      let index = 0;

      while (
        index < distances.length &&
        coveredDistance > accumulatedDistance + distances[index]
      ) {
        accumulatedDistance += distances[index];
        index++;
      }

      if (index < distances.length) {
        const start = new THREE.Vector3(...path[index]);
        const end = new THREE.Vector3(...path[index + 1]);
        const segmentDistance = distances[index];

        const t = (coveredDistance - accumulatedDistance) / segmentDistance;
        const position = start.lerp(end, t);
        meshRef.current.position.copy(position);

        const direction = new THREE.Vector3()
          .subVectors(end, start)
          .normalize();
        const targetQuaternion = new THREE.Quaternion().setFromUnitVectors(
          new THREE.Vector3(0, 0, 1),
          direction
        );
        meshRef.current.quaternion.slerp(targetQuaternion, 0.1);
      } else {
        progressRef.current = totalDistance;
      }
    }
  });

  useEffect(() => {
    gl.domElement.addEventListener("click", handleClick);
    return () => gl.domElement.removeEventListener("click", handleClick);
  }, [handleClick]);
  return (
    <>
      {path.length > 0 && <Line points={path} color="blue" lineWidth={3} />}
      {path.length > 0 && (
        // <primitive
        //   ref={gltfRef}
        //   object={gltfClone}
        //   position={path.length > 0 ? path[0] : [0, 0.1, 0]}
        //   scale={[0.5, 0.5, 0.5]}
        // />
        <mesh ref={meshRef} position={path.length > 0 ? path[0] : [0, 0.1, 0]}>
          <boxGeometry args={[1, 1, 1]} />
          <meshNormalMaterial />
        </mesh>
      )}
    </>
  );
};

export default PathNavigator;