// Tim781S LiDAR Visualization JavaScript

let canvas, ctx;
let centerX, centerY;
let scale = 20; // pixels per meter
let maxDisplay = 20; // max display range in meters

function initLidarCanvas() {
    canvas = document.getElementById('lidarCanvas');
    if (!canvas) {
        console.error('LiDAR canvas not found');
        return;
    }

    ctx = canvas.getContext('2d');
    centerX = canvas.width / 2;
    centerY = canvas.height / 2;

    // Draw initial grid
    drawGrid();

    console.log('LiDAR canvas initialized');
}

function drawGrid() {
    if (!ctx) return;

    ctx.clearRect(0, 0, canvas.width, canvas.height);

    // Draw background
    ctx.fillStyle = '#0a0a0a';
    ctx.fillRect(0, 0, canvas.width, canvas.height);

    // Draw grid circles (range rings)
    ctx.strokeStyle = '#2a2a2a';
    ctx.lineWidth = 1;

    for (let r = 5; r <= maxDisplay; r += 5) {
        ctx.beginPath();
        ctx.arc(centerX, centerY, r * scale, 0, 2 * Math.PI);
        ctx.stroke();

        // Draw range labels
        ctx.fillStyle = '#666';
        ctx.font = '12px monospace';
        ctx.fillText(r + 'm', centerX + 5, centerY - r * scale + 15);
    }

    // Draw axes
    ctx.strokeStyle = '#444';
    ctx.lineWidth = 2;

    // X axis
    ctx.beginPath();
    ctx.moveTo(0, centerY);
    ctx.lineTo(canvas.width, centerY);
    ctx.stroke();

    // Y axis
    ctx.beginPath();
    ctx.moveTo(centerX, 0);
    ctx.lineTo(centerX, canvas.height);
    ctx.stroke();

    // Draw robot position (center circle)
    ctx.fillStyle = '#00ff88';
    ctx.beginPath();
    ctx.arc(centerX, centerY, 5, 0, 2 * Math.PI);
    ctx.fill();

    // Draw forward direction indicator
    ctx.strokeStyle = '#00ff88';
    ctx.lineWidth = 3;
    ctx.beginPath();
    ctx.moveTo(centerX, centerY);
    ctx.lineTo(centerX, centerY - 30);
    ctx.stroke();
}

function drawLidarScan(points) {
    if (!ctx || !Array.isArray(points)) return;

    // Redraw grid first
    drawGrid();

    // Draw scan points
    points.forEach(point => {
        const angle = point.angle;
        const range = point.range;

        // Convert polar to Cartesian (LiDAR coordinate frame)
        // In LiDAR frame: 0° = forward (up on screen)
        // We need to rotate 90° so forward matches up on screen
        const x = centerX + (range * scale * Math.sin(angle));
        const y = centerY - (range * scale * Math.cos(angle));

        // Color based on distance (closer = brighter green, farther = darker)
        const intensity = 1.0 - (range / maxDisplay);
        const r = Math.floor(0 * intensity);
        const g = Math.floor(255 * intensity);
        const b = Math.floor(136 * intensity);

        // Draw point
        ctx.fillStyle = `rgb(${r}, ${g}, ${b})`;
        ctx.beginPath();
        ctx.arc(x, y, 2, 0, 2 * Math.PI);
        ctx.fill();
    });

    // Draw connection lines (optional - creates solid appearance)
    if (points.length > 1) {
        ctx.strokeStyle = 'rgba(0, 255, 136, 0.3)';
        ctx.lineWidth = 1;
        ctx.beginPath();

        points.forEach((point, index) => {
            const angle = point.angle;
            const range = point.range;
            const x = centerX + (range * scale * Math.sin(angle));
            const y = centerY - (range * scale * Math.cos(angle));

            if (index === 0) {
                ctx.moveTo(x, y);
            } else {
                ctx.lineTo(x, y);
            }
        });

        ctx.stroke();
    }
}

// Make functions available globally
window.initLidarCanvas = initLidarCanvas;
window.drawLidarScan = drawLidarScan;