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

import React, { useEffect, useState, useRef } 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 { usePlayButtonStore } from "../../../store/usePlayButtonStore";
import { usePlayAgv } from "../../../store/store";

interface PathNavigatorProps {
  navMesh: any;
  pathPoints: any;
  id: string;
  speed: number;
  bufferTime: number;
  hitCount: number;
  processes: any[];
  agvRef: any;
}
interface AGVData {
  processId: string;
  vehicleId: string;
  hitCount: number;
  totalHits: number;
}
type Phase = "initial" | "toDrop" | "toPickup";

export default function PathNavigator({
  navMesh,
  pathPoints,
  id,
  speed,
  bufferTime,
  hitCount,
  processes,
  agvRef,
}: PathNavigatorProps) {
  const [currentPhase, setCurrentPhase] = useState<Phase>("initial");
  // console.log('agvRef: ', agvRef);

  const [path, setPath] = useState<[number, number, number][]>([]);

  // const [currentPhase, setCurrentPhase] = useState<"initial" | "loop">(
  //   "initial"
  // );
  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 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 { scene } = useThree();
  const { isPlaying } = usePlayButtonStore();
  const { PlayAgv, setPlayAgv } = usePlayAgv();

  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");
    setPickupDropPath([]);
    setDropPickupPath([]);
    distancesRef.current = [];
    totalDistanceRef.current = 0;
    progressRef.current = 0;
    isWaiting.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 = {
      x: object.position.x,
      y: object.position.y,
      z: object.position.z,
    };

    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]);

  // Add these refs outside the useFrame if not already present:
  const startPointReached = useRef(false);

  useFrame((_, delta) => {});

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

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

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

    if (isPlaying && !hasStarted.current) {
      hasStarted.current = false;
      startPointReached.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 hitCount === currentAgv.expectedHitCount;
    };

    // Step 1: Snap to start point on first play
    if (isPlaying && !hasStarted.current && toPickupPath.length > 0) {
      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;
      startPointReached.current = true;
      progressRef.current = 0;

      return;
    }

    // Step 2: Wait at start point for AGV readiness (only if expected hit count is not met)
    if (
      isPlaying &&
      startPointReached.current &&
      path.length === 0 &&
      currentAgv?.isplaying
    ) {
      if (!isAgvReady()) {
        return; // Prevent transitioning to the next phase if AGV is not ready
      }

      setPath([...toPickupPath]); // Start path transition once the AGV is ready
      setCurrentPhase("toDrop");
      progressRef.current = 0;
      startPointReached.current = false;

      return;
    }

    if (path.length < 2) return;

    progressRef.current += delta * speed;

    let covered = progressRef.current;
    let accumulated = 0;
    let index = 0;

    if (distancesRef.current.length !== path.length - 1) {
      distancesRef.current = [];
      totalDistanceRef.current = 0;

      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 distance = start.distanceTo(end);
        distancesRef.current.push(distance);
        totalDistanceRef.current += distance;
      }
    }

    while (
      index < distancesRef.current.length &&
      covered > accumulated + distancesRef.current[index]
    ) {
      accumulated += distancesRef.current[index];
      index++;
    }

    // AGV has completed its path
    if (index >= distancesRef.current.length) {
      progressRef.current = totalDistanceRef.current;

      timeoutRef.current = setTimeout(() => {
        if (!isAgvReady()) {
          isWaiting.current = false;
          return;
        }

        let nextPath = [];
        let nextPhase = currentPhase;

        if (currentPhase === "toDrop") {
          nextPath = dropPickupPath;
          nextPhase = "toPickup";
        } else if (currentPhase === "toPickup") {
          nextPath = pickupDropPath;
          nextPhase = "toDrop";
        } else {
          nextPath = pickupDropPath;
          nextPhase = "toDrop";
        }

        setPath([...nextPath]);
        setCurrentPhase(nextPhase);
        progressRef.current = 0;
        isWaiting.current = false;
        distancesRef.current = [];

        // Decrease the expected count if AGV is ready and has completed its path
        if (currentAgv) {
          currentAgv.expectedCount = Math.max(0, currentAgv.expectedCount - 1); // Decrease but ensure it's not negative
          console.log(
            "Decreased expected count to: ",
            currentAgv.expectedCount
          );
        }

        if (agvRef.current) {
          agvRef.current = agvRef.current.map((agv: AGVData) =>
            agv.vehicleId === id ? { ...agv, hitCount: null } : agv
          );
        }

        // 🔁 Reset and wait again after reaching start
        if (currentPhase === "toPickup" && currentAgv) {
          currentAgv.isplaying = false;
          setPath([]);
          startPointReached.current = true;
          progressRef.current = 0;
        }
      }, bufferTime * 1000);

      return;
    }

    // Step 4: Interpolate position and rotation
    const start = new THREE.Vector3(...path[index]);
    const end = new THREE.Vector3(...path[index + 1]);
    const dist = distancesRef.current[index];

    if (!dist || dist === 0) return;

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

    const direction = new THREE.Vector3().subVectors(end, start).normalize();
    object.rotation.y = Math.atan2(direction.x, direction.z);
  });

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

  return (
    <group name="path-navigator-lines" visible={!isPlaying}>
      {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>
  );
}