Dwinzo_dev/app/src/modules/IIOTTemp/RTSimulation/IIOTSimulation/ArmAnimator.tsx

import { useFrame, useThree } from '@react-three/fiber';
import React, { useEffect, useRef, useState } from 'react'
import * as THREE from "three";
import { useAnimationPlaySpeed } from '../../../../store/usePlayButtonStore';


type PointWithDegree = {
    position: [number, number, number];
    degree: number;
};

function ArmAnimator({ armBot, ikSolver, setIkSolver, targetBone, restPosition, path, assetName, data }: any) {
    const { scene } = useThree();
    const progressRef = useRef(0);
    const curveRef = useRef<THREE.Vector3[] | null>(null);
    const totalDistanceRef = useRef(0);
    const startTimeRef = useRef<number | null>(null);
    const segmentDistancesRef = useRef<number[]>([]);
    const [currentPath, setCurrentPath] = useState<[number, number, number][]>([]);
    const [circlePoints, setCirclePoints] = useState<[number, number, number][]>([]);
    const [circlePointsWithDegrees, setCirclePointsWithDegrees] = useState<PointWithDegree[]>([]);
    const [customCurvePoints, setCustomCurvePoints] = useState<THREE.Vector3[] | null>(null);
    let curveHeight = 1.75
    const CIRCLE_RADIUS = 1.6
    const { speed } = useAnimationPlaySpeed();
    let duration = 5000

    const [isRunning, setIsRunning] = useState(false);
    const lastPercentageRef = useRef<number>(0);
    const targetPercentageRef = useRef<number>(0);
    const interpolationStartTimeRef = useRef<number>(performance.now());

    useEffect(() => {

        setCurrentPath(path);
    }, [path]);

    // Handle circle points based on armBot position
    useEffect(() => {
        const points = generateRingPoints(CIRCLE_RADIUS, 64)
        setCirclePoints(points);
    }, [armBot.position]);

    //Generate Circle Points
    function generateRingPoints(radius: any, segments: any) {
        const points: [number, number, number][] = [];
        for (let i = 0; i < segments; i++) {
            // Calculate angle for current segment
            const angle = (i / segments) * Math.PI * 2;
            // Calculate x and z coordinates (y remains the same for a flat ring)
            const x = Math.cos(angle) * radius;
            const z = Math.sin(angle) * radius;
            points.push([x, 1.5, z]);
        }
        return points;
    }

    //Generate CirclePoints with Angle
    function generateRingPointsWithDegrees(radius: number, segments: number, initialRotation: [number, number, number]) {
        const points: { position: [number, number, number]; degree: number }[] = [];
        for (let i = 0; i < segments; i++) {
            const angleRadians = (i / segments) * Math.PI * 2;
            const x = Math.cos(angleRadians) * radius;
            const z = Math.sin(angleRadians) * radius;
            const degree = (angleRadians * 180) / Math.PI; // Convert radians to degrees
            points.push({
                position: [x, 1.5, z],
                degree,
            });
        }
        return points;
    }
    // Handle circle points based on armBot position
    useEffect(() => {
        const points = generateRingPointsWithDegrees(CIRCLE_RADIUS, 64, armBot.rotation);
        setCirclePointsWithDegrees(points)
    }, [armBot.rotation]);

    // Function for find nearest Circlepoints Index
    const findNearestIndex = (nearestPoint: [number, number, number], points: [number, number, number][], epsilon = 1e-6) => {
        for (let i = 0; i < points.length; i++) {
            const [x, y, z] = points[i];
            if (
                Math.abs(x - nearestPoint[0]) < epsilon &&
                Math.abs(y - nearestPoint[1]) < epsilon &&
                Math.abs(z - nearestPoint[2]) < epsilon
            ) {
                return i; // Found the matching index
            }
        }
        return -1; // Not found
    };

    //function to find nearest Circlepoints 
    const findNearest = (target: [number, number, number]) => {
        return circlePoints.reduce((nearest, point) => {
            const distance = Math.hypot(target[0] - point[0], target[1] - point[1], target[2] - point[2]);
            const nearestDistance = Math.hypot(target[0] - nearest[0], target[1] - nearest[1], target[2] - nearest[2]);
            return distance < nearestDistance ? point : nearest;
        }, circlePoints[0]);
    };

    // Helper function to collect points and check forbidden degrees
    const collectArcPoints = (startIdx: number, endIdx: number, clockwise: boolean) => {
        const totalSegments = 64;
        const arcPoints: [number, number, number][] = [];
        let i = startIdx;

        while (i !== (endIdx + (clockwise ? 1 : -1) + totalSegments) % totalSegments) {
            const { degree, position } = circlePointsWithDegrees[i];
            // Skip over
            arcPoints.push(position);
            i = (i + (clockwise ? 1 : -1) + totalSegments) % totalSegments;
        }
        return arcPoints;
    };

    //Range to restrict angle 
    const hasForbiddenDegrees = (arc: [number, number, number][]) => {
        return arc.some(p => {
            const idx = findNearestIndex(p, circlePoints);
            const degree = circlePointsWithDegrees[idx]?.degree || 0;
            return degree >= 271 && degree <= 300;  // Forbidden range: 271° to 300°
        });
    };

    // Handle nearest points and final path (including arc points)
    useEffect(() => {
        if (circlePoints.length > 0 && currentPath.length > 0) {

            const start = currentPath[0];
            const end = currentPath[currentPath.length - 1];

            const raisedStart = [start[0], start[1] + 0.5, start[2]] as [number, number, number];

            const raisedEnd = [end[0], end[1] + 0.5, end[2]] as [number, number, number];


            const nearestToStart = findNearest(raisedStart);
            const nearestToEnd = findNearest(raisedEnd);

            const indexOfNearestStart = findNearestIndex(nearestToStart, circlePoints);
            const indexOfNearestEnd = findNearestIndex(nearestToEnd, circlePoints);

            const totalSegments = 64;
            const clockwiseDistance = (indexOfNearestEnd - indexOfNearestStart + totalSegments) % totalSegments;
            const counterClockwiseDistance = (indexOfNearestStart - indexOfNearestEnd + totalSegments) % totalSegments;

            // Try both directions
            const arcClockwise = collectArcPoints(indexOfNearestStart, indexOfNearestEnd, true);
            const arcCounterClockwise = collectArcPoints(indexOfNearestStart, indexOfNearestEnd, false);

            const clockwiseForbidden = hasForbiddenDegrees(arcClockwise);
            const counterClockwiseForbidden = hasForbiddenDegrees(arcCounterClockwise);

            let arcPoints: [number, number, number][] = [];

            if (!clockwiseForbidden && (clockwiseDistance <= counterClockwiseDistance || counterClockwiseForbidden)) {
                arcPoints = arcClockwise;
            } else {
                arcPoints = arcCounterClockwise;
            }

            const pathVectors = [
                new THREE.Vector3(start[0], start[1], start[2]),
                new THREE.Vector3(start[0], curveHeight, start[2]),
                new THREE.Vector3(nearestToStart[0], curveHeight, nearestToStart[2]),
                ...arcPoints.map(point => new THREE.Vector3(point[0], curveHeight, point[2])),
                new THREE.Vector3(nearestToEnd[0], curveHeight, nearestToEnd[2]),
                new THREE.Vector3(end[0], curveHeight, end[2]),
                new THREE.Vector3(end[0], end[1], end[2])
            ];

            const pathSegments: [THREE.Vector3, THREE.Vector3][] = [];
            for (let i = 0; i < pathVectors.length - 1; i++) {
                pathSegments.push([pathVectors[i], pathVectors[i + 1]]);
            }

            const segmentDistances = pathSegments.map(([p1, p2]) => p1.distanceTo(p2));
            segmentDistancesRef.current = segmentDistances;
            const totalDistance = segmentDistances.reduce((sum, d) => sum + d, 0);
            totalDistanceRef.current = totalDistance;


            setCustomCurvePoints(pathVectors);
        }
    }, [circlePoints, currentPath]);

    // Frame update for animation
    useFrame((state, delta) => {

        if (!startTimeRef.current || !isRunning) return;


        if (!ikSolver || !customCurvePoints || customCurvePoints.length === 0) return;


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

        if (!bone) return;


        const now = performance.now();
        const elapsed = now - interpolationStartTimeRef.current;
        const duration = 2000;
        const t = Math.min(elapsed / duration, 1);
        const interpolatedPercentage =
            lastPercentageRef.current +
            (targetPercentageRef.current - lastPercentageRef.current) * t;
        const progress = Math.min(interpolatedPercentage / 100, 1);

        const distances = segmentDistancesRef.current;
        const totalDistance = totalDistanceRef.current;

        const coveredDistance = progress * totalDistance;
        // console.log('coveredDistance: ', coveredDistance);

        // Traverse segments to find current position
        let index = 0;
        let accumulatedDistance = 0;

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

        if (index < distances.length) {
            const startPoint = customCurvePoints[index];
            const endPoint = customCurvePoints[index + 1];
            const segmentDistance = distances[index];
            const t = (coveredDistance - accumulatedDistance) / segmentDistance;



            if (startPoint && endPoint) {
                const position = startPoint.clone().lerp(endPoint, t);
                bone.position.copy(position);
            }
        }

        ikSolver.update();

        // Reset at the end
        if (progress >= 1) {
            setCurrentPath([]);
            setCustomCurvePoints([]);
            curveRef.current = null;
            progressRef.current = 0;
            startTimeRef.current = null;
        }
        if (currentPath.length === 0 && bone) {
            bone.position.copy(bone.position);
            ikSolver.update();
        }
    });

    useEffect(() => {
        if (data.assetName !== assetName) return;
        if (data.state === 'running' && data.percentage !== undefined) {
            console.log('data.percentage: ', data.percentage);
            if (data.percentage === 0) {
                startTimeRef.current = performance.now();
                lastPercentageRef.current = 0;
                targetPercentageRef.current = 0
            } else {
                lastPercentageRef.current = targetPercentageRef.current;
            }
            targetPercentageRef.current = data.percentage;
            interpolationStartTimeRef.current = performance.now();
            setIsRunning(true);
        } else {

            setIsRunning(false);
        }
    }, [data, assetName]);

    return (
        <></>
    )
}

export default ArmAnimator