Dwinzo_dev/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx

import React, { useEffect, useRef, useState } from 'react'
import IKInstance from '../ikInstance/ikInstance';
import RoboticArmAnimator from '../animator/roboticArmAnimator';
import { usePauseButtonStore, usePlayButtonStore, useResetButtonStore } from '../../../../../store/usePlayButtonStore';
import { useArmBotStore } from '../../../../../store/simulation/useArmBotStore';
import armModel from "../../../../../assets/gltf-glb/rigged/ik_arm_4.glb";
import { useThree } from "@react-three/fiber";
import { useFloorItems } from '../../../../../store/store';
import useModuleStore from '../../../../../store/useModuleStore';
import * as THREE from "three";

function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {

    const [currentPhase, setCurrentPhase] = useState<(string)>("init");
    const [path, setPath] = useState<[number, number, number][]>([]);
    const [ikSolver, setIkSolver] = useState<any>(null);
    const { scene } = useThree();
    const restPosition = new THREE.Vector3(0, 1.75, -1.6);
    const targetBone = "Target";
    const groupRef = useRef<any>(null);
    const pauseTimeRef = useRef<number | null>(null);
    const isPausedRef = useRef<boolean>(false);
    let startTime: number;
    //zustand
    const { addCurrentAction, setArmBotActive, setArmBotState, removeCurrentAction } = useArmBotStore();
    const { floorItems } = useFloorItems();
    const { activeModule } = useModuleStore();
    const { isPlaying } = usePlayButtonStore();
    const { isReset, setReset } = useResetButtonStore();
    const { isPaused } = usePauseButtonStore();
    
    function firstFrame() {
        startTime = performance.now();
        step();
      }
    function step() {
        if (isPausedRef.current) {
            if (!pauseTimeRef.current) {
                pauseTimeRef.current = performance.now();
            }
            requestAnimationFrame(() => step());
            return;
        }
        if (pauseTimeRef.current) {
            const pauseDuration = performance.now() - pauseTimeRef.current;
            startTime += pauseDuration;
            pauseTimeRef.current = null;
        }
        const elapsedTime = performance.now() - startTime;
        if (elapsedTime < 1500) {
            // Wait until 1500ms has passed
            requestAnimationFrame(step);
            return;
        }
            if(currentPhase==="picking"){
           
                setArmBotActive(armBot.modelUuid, true);
                setArmBotState(armBot.modelUuid, "running");
                setCurrentPhase("start-to-end");
                startTime=0
                const startPoint = armBot.point.actions[0].process.startPoint;
                const endPoint = armBot.point.actions[0].process.endPoint;
                if (startPoint && endPoint) {
                    let curve = createCurveBetweenTwoPoints(
                        new THREE.Vector3(startPoint[0], startPoint[1], startPoint[2]),
                        new THREE.Vector3(endPoint[0], endPoint[1], endPoint[2]));
                    if (curve) {
                        logStatus(armBot.modelUuid, "picking the object");
                        setPath(curve.points.map(point => [point.x, point.y, point.z]))
                    }
                }
                logStatus(armBot.modelUuid, "Moving armBot from start point to end position.")
            }else if(currentPhase==="dropping"){
              
                setArmBotActive(armBot.modelUuid, true);
                setArmBotState(armBot.modelUuid, "running");
                setCurrentPhase("end-to-rest");
                startTime=0;
                const endPoint = armBot.point.actions[0].process.endPoint;
                if (endPoint) {
                    let curve = createCurveBetweenTwoPoints(new THREE.Vector3(endPoint[0], endPoint[1], endPoint[2]), restPosition);
                    if (curve) {
                        logStatus(armBot.modelUuid, "dropping the object");
                        setPath(curve.points.map(point => [point.x, point.y, point.z]));
                    }
                }
                logStatus(armBot.modelUuid, "Moving armBot from end point to rest position.")
            }
        
      }
      useEffect(() => {
        isPausedRef.current = isPaused;
    }, [isPaused]);

    useEffect(() => {
        let armItems = floorItems?.filter((val: any) =>
            val.modelUuid === "3abf5d46-b59e-4e6b-9c02-a4634b64b82d"
        );
        // Get the first matching item
        let armItem = armItems?.[0];
        if (armItem) {
            const targetMesh = scene?.getObjectByProperty("uuid", armItem.modelUuid);
            if (targetMesh) {
                targetMesh.visible = activeModule !== "simulation"
            }
        }
        const targetBones = ikSolver?.mesh.skeleton.bones.find(
            (b: any) => b.name === targetBone
        );
        if (isReset) {
            logStatus(armBot.modelUuid, "Simulation Play Reset Successfully")
            removeCurrentAction(armBot.modelUuid)
            setArmBotActive(armBot.modelUuid, true)
            setArmBotState(armBot.modelUuid, "running")
            setCurrentPhase("init-to-rest");
            isPausedRef.current=false
            pauseTimeRef.current=null
            isPausedRef.current=false
            startTime=0
            if (targetBones) {
                let curve = createCurveBetweenTwoPoints(targetBones.position, restPosition)
                if (curve) {
                    setPath(curve.points.map(point => [point.x, point.y, point.z]));
                }
            }
            setReset(false);
            logStatus(armBot.modelUuid, "Moving armBot from initial point to rest position.")
        }
        if (isPlaying) {
            //Moving armBot from initial point to rest position.
            if (!armBot?.isActive && armBot?.state == "idle" && currentPhase == "init") {
                setArmBotActive(armBot.modelUuid, true)
                setArmBotState(armBot.modelUuid, "running")
                setCurrentPhase("init-to-rest");
                if (targetBones) {
                    let curve = createCurveBetweenTwoPoints(targetBones.position, restPosition)
                    if (curve) {
                        setPath(curve.points.map(point => [point.x, point.y, point.z]));
                    }
                }
                logStatus(armBot.modelUuid, "Moving armBot from initial point to rest position.")
            }
            //Waiting for trigger.
            else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "rest" && !armBot.currentAction) {
                logStatus(armBot.modelUuid, "Waiting to trigger CurrentAction")
                const timeoutId = setTimeout(() => {
                    addCurrentAction(armBot.modelUuid, 'action-003');
                }, 3000);
                return () => clearTimeout(timeoutId);
            }
            else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "rest" && armBot.currentAction) {
                if (armBot.currentAction) {
                    setArmBotActive(armBot.modelUuid, true);
                    setArmBotState(armBot.modelUuid, "running");
                    setCurrentPhase("rest-to-start");
                    const startPoint = armBot.point.actions[0].process.startPoint;
                    if (startPoint) {
                        let curve = createCurveBetweenTwoPoints(restPosition, new THREE.Vector3(startPoint[0], startPoint[1], startPoint[2]));
                        if (curve) {
                            setPath(curve.points.map(point => [point.x, point.y, point.z]));
                        }
                    }
                }
                logStatus(armBot.modelUuid, "Moving armBot from rest point to start position.")
            }
            else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "picking" && armBot.currentAction) {
                requestAnimationFrame(firstFrame);
                // setArmBotActive(armBot.modelUuid, true);
                // setArmBotState(armBot.modelUuid, "running");
                // setCurrentPhase("start-to-end");
                // const startPoint = armBot.point.actions[0].process.startPoint;
                // const endPoint = armBot.point.actions[0].process.endPoint;
                // if (startPoint && endPoint) {
                //     let curve = createCurveBetweenTwoPoints(
                //         new THREE.Vector3(startPoint[0], startPoint[1], startPoint[2]),
                //         new THREE.Vector3(endPoint[0], endPoint[1], endPoint[2]));
                //     if (curve) {
                //         setTimeout(() => {
                //             logStatus(armBot.modelUuid, "picking the object");
                //             setPath(curve.points.map(point => [point.x, point.y, point.z]));
                //         }, 1500)
                //     }
                // }
                // logStatus(armBot.modelUuid, "Moving armBot from start point to end position.")
            }
            else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "dropping" && armBot.currentAction) {
                requestAnimationFrame(firstFrame);
                // setArmBotActive(armBot.modelUuid, true);
                // setArmBotState(armBot.modelUuid, "running");
                // setCurrentPhase("end-to-rest");
                // const endPoint = armBot.point.actions[0].process.endPoint;
                // if (endPoint) {
                //     let curve = createCurveBetweenTwoPoints(new THREE.Vector3(endPoint[0], endPoint[1], endPoint[2]), restPosition);
                //     if (curve) {
                //         setTimeout(() => {
                //             logStatus(armBot.modelUuid, "dropping the object");
                //             setPath(curve.points.map(point => [point.x, point.y, point.z]));
                //         }, 1500)
                //     }
                // }
                // logStatus(armBot.modelUuid, "Moving armBot from end point to rest position.")
            }
        } else {
            logStatus(armBot.modelUuid, "Simulation Play Stopped")
            setArmBotActive(armBot.modelUuid, false)
            setArmBotState(armBot.modelUuid, "idle")
            setCurrentPhase("init");
            setPath([])
            removeCurrentAction(armBot.modelUuid)
       
        }

    }, [currentPhase, armBot, isPlaying, ikSolver, isReset])


    function createCurveBetweenTwoPoints(p1: any, p2: any) {
        const mid = new THREE.Vector3().addVectors(p1, p2).multiplyScalar(0.5);
        // mid.y += 0.5;
        const points = [p1, mid, p2];
        return new THREE.CatmullRomCurve3(points);
    }


    const HandleCallback = () => {
        if (armBot.isActive && armBot.state == "running" && currentPhase == "init-to-rest") {
            logStatus(armBot.modelUuid, "Callback triggered: rest");
            setArmBotActive(armBot.modelUuid, false)
            setArmBotState(armBot.modelUuid, "idle")
            setCurrentPhase("rest");
            setPath([])
        }
        else if (armBot.isActive && armBot.state == "running" && currentPhase == "rest-to-start") {
            logStatus(armBot.modelUuid, "Callback triggered: pick.");
            setArmBotActive(armBot.modelUuid, false)
            setArmBotState(armBot.modelUuid, "idle")
            setCurrentPhase("picking");
            setPath([])
        }
        else if (armBot.isActive && armBot.state == "running" && currentPhase == "start-to-end") {
            logStatus(armBot.modelUuid, "Callback triggered: drop.");
            setArmBotActive(armBot.modelUuid, false)
            setArmBotState(armBot.modelUuid, "idle")
            setCurrentPhase("dropping");
            setPath([])
        }
        else if (armBot.isActive && armBot.state == "running" && currentPhase == "end-to-rest") {
            logStatus(armBot.modelUuid, "Callback triggered: rest, cycle completed.");
            setArmBotActive(armBot.modelUuid, false)
            setArmBotState(armBot.modelUuid, "idle")
            setCurrentPhase("rest");
            setPath([])
            removeCurrentAction(armBot.modelUuid)
        }
    }
    const logStatus = (id: string, status: string) => {
        // console.log('status: ', status);
    }

    return (
        <>
            <IKInstance modelUrl={armModel} setIkSolver={setIkSolver} ikSolver={ikSolver} armBot={armBot} groupRef={groupRef} />
            <RoboticArmAnimator HandleCallback={HandleCallback} restPosition={restPosition} ikSolver={ikSolver} targetBone={targetBone} armBot={armBot}
                logStatus={logStatus} path={path}currentPhase={currentPhase} />

        </>
    )
}

export default RoboticArmInstance;












// import React, { useEffect, useRef, useState } from 'react';
// import IKInstance from '../ikInstance/ikInstance';
// import RoboticArmAnimator from '../animator/roboticArmAnimator';
// import { usePlayButtonStore, useResetButtonStore } from '../../../../../store/usePlayButtonStore';
// import { useArmBotStore } from '../../../../../store/simulation/useArmBotStore';
// import armModel from "../../../../../assets/gltf-glb/rigged/ik_arm_4.glb";
// import { useThree } from "@react-three/fiber";
// import { useFloorItems } from '../../../../../store/store';
// import useModuleStore from '../../../../../store/useModuleStore';
// import * as THREE from "three";

// function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {

//     const [currentPhase, setCurrentPhase] = useState<string>("init");
//     const [path, setPath] = useState<[number, number, number][]>([]);
//     const [ikSolver, setIkSolver] = useState<any>(null);
//     const { scene } = useThree();
//     const restPosition = new THREE.Vector3(0, 1, -1.6);
//     const targetBone = "Target";
//     const groupRef = useRef<any>(null);

//     // Zustand Stores
//     const { addCurrentAction, setArmBotActive, setArmBotState, removeCurrentAction } = useArmBotStore();
//     const { floorItems } = useFloorItems();
//     const { activeModule } = useModuleStore();
//     const { isPlaying } = usePlayButtonStore();
//     const { isReset, setReset } = useResetButtonStore();

//     // Generate path according to constraints
//     const generateArmPath = (
//         startPoint: [number, number, number],
//         endPoint: [number, number, number],
//         restPos: THREE.Vector3 = new THREE.Vector3(0, 1, -1.6)
//     ): [number, number, number][] => {
//         const path: [number, number, number][] = [];

//         const startVec = new THREE.Vector3(...startPoint);
//         const endVec = new THREE.Vector3(...endPoint);

//         // Helper functions
//         const addLinePoints = (from: THREE.Vector3, to: THREE.Vector3, segments: number) => {
//             for (let i = 0; i <= segments; i++) {
//                 const t = i / segments;
//                 const point = new THREE.Vector3().lerpVectors(from, to, t);
//                 path.push([point.x, point.y, point.z]);
//             }
//         };

//         const addCurveFromTo = (from: THREE.Vector3, to: THREE.Vector3, segments: number = 15) => {
//             const mid = new THREE.Vector3()
//                 .addVectors(from, to)
//                 .multiplyScalar(0.5)
//                 .setY(Math.max(from.y, to.y) + 0.5);
//             const curve = new THREE.CatmullRomCurve3([from, mid, to]);
//             const points = curve.getPoints(segments);
//             for (const p of points) {
//                 path.push([p.x, p.y, p.z]);
//             }
//         };

//         // Step 1: Move vertically to align Y with start point
//         const step1Start = startVec.clone().setY(restPos.y);
//         const step1End = startVec.clone();
//         addLinePoints(step1Start, step1End, 10);

//         // Step 2: Move horizontally XZ to reach start point
//         const step2Start = step1End.clone();
//         const step2End = startVec.clone(); // ✅ Fixed here
//         addLinePoints(step2Start, step2End, 10);
//         addLinePoints(step1End, step2End, 10);

//         // Optional Step 3: Move to rest/midpoint if needed
//         const distanceToRest = startVec.distanceTo(restPos);
//         if (distanceToRest > 1) {
//             addCurveFromTo(step2End, restPos, 15);
//         }

//         // Step 4: Move up/down to align Y with end point
//         const step4Start = endVec.clone().setY(restPos.y);
//         const step4End = endVec.clone();
//         addLinePoints(step4Start, step4End, 10);

//         // Step 5: Move horizontally XZ to end point
//         addLinePoints(step4End, endVec.clone(), 10);

//         // Step 6: Return to rest or mid-position after dropping
//         const finalRestHeight = restPos.clone().setY(endVec.y + 0.5);
//         addCurveFromTo(endVec, finalRestHeight, 15);

//         return path;
//     };

//     useEffect(() => {
//         let armItems = floorItems?.filter((val: any) =>
//             val.modeluuid === armBot.modelUuid
//         );
//         let armItem = armItems?.[0];
//         if (armItem) {
//             const targetMesh = scene?.getObjectByProperty("uuid", armItem.modeluuid);
//             if (targetMesh) {
//                 targetMesh.visible = activeModule !== "simulation";
//             }
//         }

//         const targetBones = ikSolver?.mesh.skeleton.bones.find(
//             (b: any) => b.name === targetBone
//         );

//         if (isReset) {
//             logStatus(armBot.modelUuid, "Simulation Play Reset Successfully");
//             removeCurrentAction(armBot.modelUuid);
//             setArmBotActive(armBot.modelUuid, true);
//             setArmBotState(armBot.modelUuid, "running");
//             setCurrentPhase("init-to-rest");

//             if (targetBones) {
//                 const startPoint = new THREE.Vector3(targetBones.position.x, targetBones.position.y, targetBones.position.z);
//                 const curve = new THREE.CatmullRomCurve3([
//                     startPoint,
//                     startPoint.clone().setY(restPosition.y + 0.5),
//                     restPosition.clone(),
//                 ]);
//                 setPath(curve.getPoints(30).map(p => [p.x, p.y, p.z]));
//             }

//             setReset(false);
//             logStatus(armBot.modelUuid, "Moving armBot from initial point to rest position.");
//         }

//         if (isPlaying) {
//             // Phase 1: Initial -> Rest
//             if (!armBot.isActive && armBot.state === "idle" && currentPhase === "init") {
//               setArmBotActive(armBot.modelUuid, true);
//               setArmBotState(armBot.modelUuid, "running");
//               setCurrentPhase("init-to-rest");
          
//               if (targetBones) {
//                 const startPoint = new THREE.Vector3(targetBones.position.x, targetBones.position.y, targetBones.position.z);
//                 const curve = new THREE.CatmullRomCurve3([
//                   startPoint,
//                   startPoint.clone().setY(restPosition.y + 0.5),
//                   restPosition.clone(),
//                 ]);
//                 setPath(curve.getPoints(30).map(p => [p.x, p.y, p.z]));
//               }
          
//               logStatus(armBot.modelUuid, "Moving armBot from initial point to rest position.");
//             }
          
//             // Phase 2: Wait for trigger
//             else if (!armBot.isActive && armBot.state === "idle" && currentPhase === "rest" && !armBot.currentAction) {
//               logStatus(armBot.modelUuid, "Waiting to trigger CurrentAction");
//               const timeoutId = setTimeout(() => {
//                 addCurrentAction(armBot.modelUuid, 'action-003');
//               }, 3000);
//               return () => clearTimeout(timeoutId);
//             }
          
//             // Phase 3: Rest -> Start Point
//             else if (!armBot.isActive && armBot.state === "idle" && currentPhase === "rest" && armBot.currentAction) {
//               setArmBotActive(armBot.modelUuid, true);
//               setArmBotState(armBot.modelUuid, "running");
//               setCurrentPhase("rest-to-start");
          
//               const startPoint = armBot.point.actions[0].process.startPoint;
//               if (startPoint) {
//                 const start = new THREE.Vector3(startPoint[0], startPoint[1], startPoint[2]);
//                 const curve = new THREE.CatmullRomCurve3([restPosition, restPosition.clone().setY(start.y), start]);
//                 setPath(curve.getPoints(30).map(p => [p.x, p.y, p.z]));
//               }
          
//               logStatus(armBot.modelUuid, "Moving armBot from rest point to start position.");
//             }
          
//             // Phase 4: Start -> End (Pick & Move Object)
//             else if (!armBot.isActive && armBot.state === "idle" && currentPhase === "picking" && armBot.currentAction) {
//               setArmBotActive(armBot.modelUuid, true);
//               setArmBotState(armBot.modelUuid, "running");
//               setCurrentPhase("start-to-end");
          
//               const startPoint = armBot.point.actions[0].process.startPoint;
//               const endPoint = armBot.point.actions[0].process.endPoint;
          
//               if (startPoint && endPoint) {
//                 const path = generateArmPath(startPoint, endPoint, restPosition);
//                 logStatus(armBot.modelUuid, "Waiting to pick.");
//                 setTimeout(()=>{
//                     setPath(path);
//                 },3000)
//               }
          
//               logStatus(armBot.modelUuid, "Moving armBot from start point to end position.");
//             }
          
//             // Phase 5: End -> Rest (Drop & Return)
//             else if (!armBot.isActive && armBot.state === "idle" && currentPhase === "dropping" && armBot.currentAction) {
//               setArmBotActive(armBot.modelUuid, true);
//               setArmBotState(armBot.modelUuid, "running");
//               setCurrentPhase("end-to-rest");
          
//               const endPoint = armBot.point.actions[0].process.endPoint;
//               if (endPoint) {
//                 const start = new THREE.Vector3(endPoint[0], endPoint[1], endPoint[2]);
//                 const curve = new THREE.CatmullRomCurve3([start, start.clone().setY(restPosition.y), restPosition]);
//                 logStatus(armBot.modelUuid, "Waiting to drop.");
//                 setTimeout(()=>{
//                     setPath(curve.getPoints(30).map(p => [p.x, p.y, p.z]));
//                 },3000)
               
//               }
          
//               logStatus(armBot.modelUuid, "Moving armBot from end point to rest position.");
//             }
//           } else {
//             logStatus(armBot.modelUuid, "Simulation Play Stopped");
//             setArmBotActive(armBot.modelUuid, false);
//             setArmBotState(armBot.modelUuid, "idle");
//             setCurrentPhase("init");
//             setPath([]);
//             removeCurrentAction(armBot.modelUuid);
//         }

//     }, [currentPhase, armBot, isPlaying, ikSolver, isReset]);

//     const HandleCallback = () => {
//         if (armBot.isActive && armBot.state === "running" && currentPhase === "init-to-rest") {
//             logStatus(armBot.modelUuid, "Callback triggered: rest");
//             setArmBotActive(armBot.modelUuid, false);
//             setArmBotState(armBot.modelUuid, "idle");
//             setCurrentPhase("rest");
//             setPath([]);
//         } else if (armBot.isActive && armBot.state === "running" && currentPhase === "rest-to-start") {
//             logStatus(armBot.modelUuid, "Callback triggered: pick.");
//             setArmBotActive(armBot.modelUuid, false);
//             setArmBotState(armBot.modelUuid, "idle");
//             setCurrentPhase("picking");
//             setPath([]);
//         } else if (armBot.isActive && armBot.state === "running" && currentPhase === "start-to-end") {
//             logStatus(armBot.modelUuid, "Callback triggered: drop.");
//             setArmBotActive(armBot.modelUuid, false);
//             setArmBotState(armBot.modelUuid, "idle");
//             setCurrentPhase("dropping");
//             setPath([]);
//         } else if (armBot.isActive && armBot.state === "running" && currentPhase === "end-to-rest") {
//             logStatus(armBot.modelUuid, "Callback triggered: rest, cycle completed.");
//             setArmBotActive(armBot.modelUuid, false);
//             setArmBotState(armBot.modelUuid, "idle");
//             setCurrentPhase("rest");
//             setPath([]);
//             removeCurrentAction(armBot.modelUuid);
//         }
//     };

//     const logStatus = (id: string, status: string) => {
//         console.log( status);
//         // console.log(`status [${id}]: ${status}`);
//     };

//     return (
//         <>
//             <IKInstance modelUrl={armModel} setIkSolver={setIkSolver} ikSolver={ikSolver} armBot={armBot} groupRef={groupRef} />
//             <RoboticArmAnimator
//                 HandleCallback={HandleCallback}
//                 restPosition={restPosition}
//                 ikSolver={ikSolver}
//                 targetBone={targetBone}
//                 armBot={armBot}
//                 logStatus={logStatus}
//                 path={path}
//             />
//         </>
//     );
// }

// export default RoboticArmInstance;