Notes

import React, { useEffect, useRef, useState, useMemo, useCallback } from 'react';
import { useAmclPoseStore } from '@/hooks/service-hooks/amcl/amclposeR';
import { useLidarStore } from '@/hooks/service-hooks/lidar/lidarDataR';
import { useLidarStore2 } from '@/hooks/service-hooks/lidar/lidarDataR2';
import { useLidarStore3 } from '@/hooks/service-hooks/lidar/lidarDataR3';
import { useLidarStore4 } from '@/hooks/service-hooks/lidar/lidarDataR4';
import { useDisplayStore } from '@/stores/displayStore';
import { GridSize, BeamsLidar, LidarDataType } from "@/utilities/types";

// Define interfaces for our component props and state
interface Point {
x: number;
y: number;
}

interface TransformedData {
points: Point[];
lidarPose: Point;
}

interface LidarShapeProps {
mapResolution: number;
gridLocationInWorld: GridSize;
pointColor?: string;
pointDiameter?: number;
reflectorMode?: boolean;
canvasWidth: number;
canvasHeight: number;
robotPose?: number[];
lidarData: LidarDataType;
drawPolygon?: boolean;
}

interface LidarProps {
resolution: number;
gridWidth: number;
gridHeight: number;
gridLocation: GridSize;
}

// Canvas-based LidarShape component for much higher performance
export const LidarShape: React.FC<LidarShapeProps> = ({
mapResolution,
gridLocationInWorld,
pointColor = '#FF0000',
pointDiameter = 0.5,
reflectorMode = true,
canvasWidth,
canvasHeight,
robotPose = [0, 0, 0],
lidarData,
drawPolygon = true
}) => {
const canvasRef = useRef<HTMLCanvasElement>(null);
const [transformedData, setTransformedData] = useState<TransformedData>({
points: [],
lidarPose: {x: 0, y: 0}
});

// Transform points - optimized with useCallback for better performance
const transformPoints = useCallback((): TransformedData => {
if (!lidarData || !lidarData.Points_Base || !robotPose || robotPose.length < 3) {
return { points: [], lidarPose: {x: 0, y: 0} };
}

const points: Point[] = [];
const pointsBase = lidarData.Points_Base;
const cosTheta = Math.cos(robotPose[2]);
const sinTheta = Math.sin(robotPose[2]);
const rX = robotPose[0];
const rY = robotPose[1];
const gridX0 = gridLocationInWorld[0];
const gridY0 = gridLocationInWorld[1];
const invResolution = 1 / mapResolution; // Calculate once instead of dividing multiple times

// Process points in bulk for better performance
const batchSize = 100; // Process points in batches
for (let batchStart = 0; batchStart < pointsBase.length; batchStart += batchSize * 2) {
const batchEnd = Math.min(batchStart + batchSize * 2, pointsBase.length);

for (let i = batchStart; i < batchEnd; i += 2) {
if (i + 1 >= pointsBase.length) break;

// Transform from robot base to world coordinates
const worldX = rX + pointsBase[i] * cosTheta - pointsBase[i+1] * sinTheta;
const worldY = rY + pointsBase[i] * sinTheta + pointsBase[i+1] * cosTheta;

// Transform from world to grid coordinates with optimized math
const gridX = (worldY - gridY0) * invResolution;
const gridY = (worldX - gridX0) * invResolution;

// Only add points that are within the canvas
if (gridY > 0 && gridY <= canvasHeight && gridX > 0 && gridX <= canvasWidth) {
points.push({x: gridX, y: gridY});
}
}
}

// Calculate lidar origin in grid coordinates
let lidarPose: Point = {x: 0, y: 0};
if (lidarData.Position) {
const lidarOriginWorld = [
rX + lidarData.Position[0] * cosTheta - lidarData.Position[1] * sinTheta,
rY + lidarData.Position[0] * sinTheta + lidarData.Position[1] * cosTheta
];

lidarPose = {
x: (lidarOriginWorld[1] - gridY0) * invResolution,
y: (lidarOriginWorld[0] - gridX0) * invResolution
};
}

return { points, lidarPose };
}, [lidarData, robotPose, mapResolution, gridLocationInWorld, canvasWidth, canvasHeight]);

// Transform data when inputs change
useEffect(() => {
const newData = transformPoints();
setTransformedData(newData);
}, [transformPoints]);

// Draw on canvas when transformed data changes
useEffect(() => {
const canvas = canvasRef.current;
if (!canvas) return;

const ctx = canvas.getContext('2d');
if (!ctx) return;

const { points, lidarPose } = transformedData;
const radius = pointDiameter / 2;

// Clear canvas
ctx.clearRect(0, 0, canvasWidth, canvasHeight);

// Set point style
ctx.fillStyle = pointColor;

// Draw points using more efficient batch rendering
if (points.length > 0) {
// For large datasets, use sampling to improve performance
const maxPointsToRender = points.length > 5000 ? 1000 : points.length;
const step = Math.max(1, Math.ceil(points.length / maxPointsToRender));

for (let i = 0; i < points.length; i += step) {
const point = points[i];
ctx.beginPath();
ctx.arc(point.x, point.y, radius, 0, Math.PI * 2);
ctx.fill();
}
}

// Draw polygon if enabled
if (drawPolygon && points.length > 0) {
// Use fewer points for the polygon to improve performance
const maxPolygonPoints = 100;
const polygonStep = Math.max(1, Math.ceil(points.length / maxPolygonPoints));

ctx.beginPath();
ctx.moveTo(lidarPose.x, lidarPose.y);

for (let i = 0; i < points.length; i += polygonStep) {
ctx.lineTo(points[i].x, points[i].y);
}

ctx.closePath();
ctx.fillStyle = 'rgba(40, 0, 128, 0.2)';
ctx.strokeStyle = 'rgba(0, 0, 0, 0.1)';
ctx.lineWidth = 0.01;
ctx.fill();
ctx.stroke();
}
}, [transformedData, canvasWidth, canvasHeight, pointDiameter, pointColor, drawPolygon]);

return (
<canvas
ref={canvasRef}
width={canvasWidth}
height={canvasHeight}
className="absolute top-0 left-0"
style={{
position: 'absolute',
pointerEvents: 'none'
}}
/>
);
};

// Main Lidar component with performance optimizations
const Lidar: React.FC<LidarProps> = ({
resolution,
gridWidth,
gridHeight,
gridLocation,
}) => {
// Get data from stores with proper typing
const pose = useAmclPoseStore(state => state.amclPoseData);
const lidarData = useLidarStore(state => state.lidarData);
const lidarData2 = useLidarStore2(state => state.lidarData);
const lidarData3 = useLidarStore3(state => state.lidarData);
const lidarData4 = useLidarStore4(state => state.lidarData);

const displayLidar1 = useDisplayStore((state) => state.lidar1);
const displayLidar2 = useDisplayStore((state) => state.lidar2);
const displayLidar3 = useDisplayStore((state) => state.lidar3);
const displayLidar4 = useDisplayStore((state) => state.lidar4);

// Create common props to reduce duplication - use useMemo for memoization
const commonProps = useMemo(() => ({
mapResolution: resolution,
gridLocationInWorld: gridLocation,
pointColor: "#FF0000",
pointDiameter: 0.5,
reflectorMode: true,
canvasWidth: gridWidth,
canvasHeight: gridHeight,
robotPose: pose?.PoseEstimate,
drawPolygon: true
}), [resolution, gridLocation, gridWidth, gridHeight, pose?.PoseEstimate]);

// Use state with useCallback to debounce rapid changes
const [animationFrame, setAnimationFrame] = useState<number | null>(null);

// Debounced render function
const debouncedRender = useCallback(() => {
if (animationFrame !== null) {
cancelAnimationFrame(animationFrame);
}

const frameId = requestAnimationFrame(() => {
setAnimationFrame(null);
});

setAnimationFrame(frameId);
}, []);

// Trigger the debounced render when data changes
useEffect(() => {
debouncedRender();

return () => {
if (animationFrame !== null) {
cancelAnimationFrame(animationFrame);
}
};
}, [lidarData, lidarData2, lidarData3, lidarData4, debouncedRender]);

// Only render visible lidars
return (
<div
className="absolute top-0 z-40 bg-transparent"
style={{
width: `${gridWidth}px`,
height: `${gridHeight}px`,
}}
>
{displayLidar1 && lidarData && (
<LidarShape
{...commonProps}
lidarData={lidarData}
key="lidar1"
/>
)}
{displayLidar2 && lidarData2 && (
<LidarShape
{...commonProps}
lidarData={lidarData2}
key="lidar2"
/>
)}
{displayLidar3 && lidarData3 && (
<LidarShape
{...commonProps}
lidarData={lidarData3}
key="lidar3"
/>
)}
{displayLidar4 && lidarData4 && (
<LidarShape
{...commonProps}
lidarData={lidarData4}
key="lidar4"
/>
)}
</div>
);
};

export default Lidar;