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

import React, { useEffect, useState, useRef, useMemo } from "react";
import * as THREE from "three";
import { useFrame, useThree } from "@react-three/fiber";
import { NavMeshQuery } from "@recast-navigation/core";
import { Line } from "@react-three/drei";
import {
  useAnimationPlaySpeed,
  usePlayButtonStore,
} from "../../../store/usePlayButtonStore";
import { usePlayAgv } from "../../../store/store";

interface PathNavigatorProps {
  navMesh: any;
  pathPoints: any;
  id: string;
  speed: number;
  globalSpeed: number;
  bufferTime: number;
  hitCount: number;
  processes: any[];
  agvRef: any;
  MaterialRef: any;
}
interface AGVData {
  processId: string;
  vehicleId: string;
  hitCount: number;
  totalHits: number;
}
type Phase = "initial" | "toDrop" | "toPickup";
type MaterialType = "Box" | "Crate";
export default function PathNavigator({
  navMesh,
  pathPoints,
  id,
  speed,
  globalSpeed,
  bufferTime,
  hitCount,
  processes,
  agvRef,
  MaterialRef,
}: PathNavigatorProps) {
  const [currentPhase, setCurrentPhase] = useState<Phase>("initial");
  const [path, setPath] = useState<[number, number, number][]>([]);
  const [toPickupPath, setToPickupPath] = useState<[number, number, number][]>(
    []
  );
  const [pickupDropPath, setPickupDropPath] = useState<
    [number, number, number][]
  >([]);
  const [dropPickupPath, setDropPickupPath] = useState<
    [number, number, number][]
  >([]);
  const [initialPosition, setInitialPosition] = useState<THREE.Vector3 | null>(
    null
  );
  const [initialRotation, setInitialRotation] = useState<THREE.Euler | null>(
    null
  );
  const [boxVisible, setBoxVisible] = useState(false);

  const distancesRef = useRef<number[]>([]);
  const totalDistanceRef = useRef(0);
  const progressRef = useRef(0);
  const isWaiting = useRef(false);
  const timeoutRef = useRef<NodeJS.Timeout | null>(null);
  const hasStarted = useRef(false);
  const hasReachedPickup = useRef(false);

  const { scene } = useThree();
  const { isPlaying } = usePlayButtonStore();
  const { PlayAgv, setPlayAgv } = usePlayAgv();

  const boxRef = useRef<THREE.Mesh | null>(null);

  const baseMaterials = useMemo(
    () => ({
      Box: new THREE.MeshStandardMaterial({ color: 0x8b4513 }),
      Crate: new THREE.MeshStandardMaterial({ color: 0x00ff00 }),
      Default: new THREE.MeshStandardMaterial({ color: 0xcccccc }),
    }),
    []
  );

  useEffect(() => {
    const object = scene.getObjectByProperty("uuid", id);
    if (object) {
      setInitialPosition(object.position.clone());
      setInitialRotation(object.rotation.clone());
    }
  }, [scene, id]);

  const computePath = (start: any, end: any) => {
    try {
      const navMeshQuery = new NavMeshQuery(navMesh);
      const { path: segmentPath } = navMeshQuery.computePath(start, end);
      return (
        segmentPath?.map(
          ({ x, y, z }) => [x, y + 0.1, z] as [number, number, number]
        ) || []
      );
    } catch {
      return [];
    }
  };

  const resetState = () => {
    if (timeoutRef.current) {
      clearTimeout(timeoutRef.current);
      timeoutRef.current = null;
    }

    setPath([]);
    setCurrentPhase("initial");
    setToPickupPath([]);
    setPickupDropPath([]);
    setDropPickupPath([]);
    setBoxVisible(false);
    distancesRef.current = [];
    totalDistanceRef.current = 0;
    progressRef.current = 0;
    isWaiting.current = false;
    hasStarted.current = false;
    hasReachedPickup.current = false;

    if (initialPosition && initialRotation) {
      const object = scene.getObjectByProperty("uuid", id);
      if (object) {
        object.position.copy(initialPosition);
        object.rotation.copy(initialRotation);
      }
    }
  };

  useEffect(() => {
    if (!isPlaying) {
      resetState();
    }

    if (!navMesh || pathPoints.length < 2) return;

    const [pickup, drop] = pathPoints.slice(-2);

    const object = scene.getObjectByProperty("uuid", id);
    if (!object) return;

    const currentPosition = object.position;

    const toPickupPath = computePath(currentPosition, pickup);
    const pickupToDropPath = computePath(pickup, drop);
    const dropToPickupPath = computePath(drop, pickup);

    if (
      toPickupPath.length &&
      pickupToDropPath.length &&
      dropToPickupPath.length
    ) {
      setPickupDropPath(pickupToDropPath);
      setDropPickupPath(dropToPickupPath);
      setToPickupPath(toPickupPath);
      setPath(toPickupPath);
      setCurrentPhase("initial");
    }
  }, [navMesh, pathPoints, hitCount, isPlaying, PlayAgv]);

  useEffect(() => {
    if (path.length < 2) return;

    let total = 0;
    const segmentDistances = path.slice(0, -1).map((point, i) => {
      const dist = new THREE.Vector3(...point).distanceTo(
        new THREE.Vector3(...path[i + 1])
      );
      total += dist;
      return dist;
    });

    distancesRef.current = segmentDistances;
    totalDistanceRef.current = total;
    progressRef.current = 0;
    isWaiting.current = false;
  }, [path]);

  function logAgvStatus(id: string, status: string) {
    // console.log(
    //   `AGV ${id}: ${status}`

    // );
  }

  function findProcessByTargetModelUUID(processes: any, targetModelUUID: any) {
    for (const process of processes) {
      for (const path of process.paths) {
        for (const point of path.points) {
          if (
            point.connections?.targets?.some(
              (target: any) => target.modelUUID === targetModelUUID
            )
          ) {
            return process.id;
          }
        }
      }
    }
    return null;
  }

  useEffect(() => {
    if (!scene || !boxRef || !processes || !MaterialRef.current) return;

    const existingObject = scene.getObjectByProperty("uuid", id);
    if (!existingObject) return;

    if (boxRef.current?.parent) {
      boxRef.current.parent.remove(boxRef.current);
      boxRef.current = null;
    }

    if (boxVisible) {
      const matchedProcess = findProcessByTargetModelUUID(processes, id);
      let materialType: "Box" | "Crate" | "Default" = "Default";

      if (matchedProcess) {
        const materialEntry = MaterialRef.current.find((item: any) =>
          item.objects.some((obj: any) => obj.processId === matchedProcess)
        );
        if (materialEntry) {
          materialType = materialEntry.material;
        }
      }

      const boxGeometry = new THREE.BoxGeometry(0.5, 0.5, 0.5);
      const boxMesh = new THREE.Mesh(boxGeometry, baseMaterials[materialType]);
      boxMesh.position.y = 1;
      boxMesh.name = `box-${id}`;
      existingObject.add(boxMesh);
      boxRef.current = boxMesh;
    }

    return () => {
      if (boxRef.current?.parent) {
        boxRef.current.parent.remove(boxRef.current);
      }
    };
  }, [processes, MaterialRef, boxVisible, scene, id, baseMaterials]);

  useFrame((_, delta) => {
    const currentAgv = (agvRef.current || []).find(
      (agv: AGVData) => agv.vehicleId === id
    );

    if (!scene || !id || !isPlaying) return;

    const object = scene.getObjectByProperty("uuid", id);
    if (!object) return;

    if (isPlaying && !hasStarted.current) {
      hasStarted.current = false;
      progressRef.current = 0;
      isWaiting.current = false;
      if (timeoutRef.current) {
        clearTimeout(timeoutRef.current);
        timeoutRef.current = null;
      }
    }

    const isAgvReady = () => {
      if (!agvRef.current || agvRef.current.length === 0) return false;
      if (!currentAgv) return false;

      return currentAgv.isActive && hitCount >= currentAgv.maxHitCount;
    };

    if (isPlaying && !hasStarted.current && toPickupPath.length > 0) {
      setBoxVisible(false);
      const startPoint = new THREE.Vector3(...toPickupPath[0]);
      object.position.copy(startPoint);

      if (toPickupPath.length > 1) {
        const nextPoint = new THREE.Vector3(...toPickupPath[1]);
        const direction = nextPoint.clone().sub(startPoint).normalize();
        object.rotation.y = Math.atan2(direction.x, direction.z);
      }

      hasStarted.current = true;
      progressRef.current = 0;
      hasReachedPickup.current = false;
      setToPickupPath(toPickupPath.slice(-1));
      logAgvStatus(id, "Started from station, heading to pickup");
      return;
    }

    if (isPlaying && currentPhase === "initial" && !hasReachedPickup.current) {
      const reached = moveAlongPath(
        object,
        path,
        distancesRef.current,
        speed,
        delta,
        progressRef
      );

      if (reached) {
        hasReachedPickup.current = true;
        if (currentAgv) {
          currentAgv.status = "picking";
        }
        logAgvStatus(id, "Reached pickup point, Waiting for material");
      }
      return;
    }

    if (isPlaying && currentAgv?.isActive && currentPhase === "initial") {
      if (!isAgvReady()) return;
      setTimeout(() => {
        setBoxVisible(true);
        setPath([...pickupDropPath]);
        setCurrentPhase("toDrop");
        progressRef.current = 0;
        logAgvStatus(id, "Started from pickup point, heading to drop point");
        if (currentAgv) {
          currentAgv.status = "toDrop";
        }
      }, 0);
      return;
    }

    if (isPlaying && currentPhase === "toDrop") {
      const reached = moveAlongPath(
        object,
        path,
        distancesRef.current,
        speed,
        delta,
        progressRef
      );

      if (reached && !isWaiting.current) {
        isWaiting.current = true;
        logAgvStatus(id, "Reached drop point");
        if (currentAgv) {
          currentAgv.status = "droping";
          currentAgv.hitCount = currentAgv.hitCount--;
        }
        timeoutRef.current = setTimeout(() => {
          setPath([...dropPickupPath]);
          setCurrentPhase("toPickup");
          progressRef.current = 0;
          isWaiting.current = false;
          setBoxVisible(false);
          if (currentAgv) {
            currentAgv.status = "toPickup";
          }
          logAgvStatus(
            id,
            "Started from droping point, heading to pickup point"
          );
        }, bufferTime * 1000);
      }
      return;
    }

    if (isPlaying && currentPhase === "toPickup") {
      const reached = moveAlongPath(
        object,
        path,
        distancesRef.current,
        speed,
        delta,
        progressRef
      );

      if (reached) {
        if (currentAgv) {
          currentAgv.isActive = false;
        }
        setCurrentPhase("initial");
        if (currentAgv) {
          currentAgv.status = "picking";
        }
        logAgvStatus(id, "Reached pickup point again, cycle complete");
      }
      return;
    }

    moveAlongPath(
      object,
      path,
      distancesRef.current,
      speed,
      delta,
      progressRef
    );
  });

  function moveAlongPath(
    object: THREE.Object3D,
    path: [number, number, number][],
    distances: number[],
    speed: number,
    delta: number,
    progressRef: React.MutableRefObject<number>
  ): boolean {
    if (path.length < 2) return false;

    progressRef.current += delta * (speed * globalSpeed);
    let covered = progressRef.current;
    let accumulated = 0;
    let index = 0;

    for (; index < distances.length; index++) {
      const dist = distances[index];
      if (accumulated + dist >= covered) break;
      accumulated += dist;
    }

    if (index >= path.length - 1) {
      if (path.length > 1) {
        const lastDirection = new THREE.Vector3(...path[path.length - 1])
          .sub(new THREE.Vector3(...path[path.length - 2]))
          .normalize();
        object.rotation.y = Math.atan2(lastDirection.x, lastDirection.z);
      }
      return true;
    }

    const start = new THREE.Vector3(...path[index]);
    const end = new THREE.Vector3(...path[index + 1]);
    const dist = distances[index];

    const t = THREE.MathUtils.clamp((covered - accumulated) / dist, 0, 1);
    object.position.copy(start.clone().lerp(end, t));

    if (dist > 0.1) {
      const targetDirection = end.clone().sub(start).normalize();
      const targetRotationY = Math.atan2(targetDirection.x, targetDirection.z);

      const rotationSpeed = Math.min(5 * delta, 1);
      object.rotation.y = THREE.MathUtils.lerp(
        object.rotation.y,
        targetRotationY,
        rotationSpeed
      );
    }

    return false;
  }

  useEffect(() => {
    return () => {
      if (timeoutRef.current) {
        clearTimeout(timeoutRef.current);
      }
    };
  }, []);

  return (
    <group name="path-navigator-lines">
      {toPickupPath.length > 0 && (
        <Line
          points={toPickupPath}
          color="blue"
          lineWidth={3}
          dashed
          dashSize={0.75}
          dashScale={2}
        />
      )}

      {pickupDropPath.length > 0 && (
        <Line points={pickupDropPath} color="red" lineWidth={3} />
      )}

      {dropPickupPath.length > 0 && (
        <Line points={dropPickupPath} color="red" lineWidth={3} />
      )}
    </group>
  );
}