// FORI AGV Dashboard Client Application

// --- [UI Performance Optimization Config] ---
// Set to true to use web_video_server for video stream, false to use standard rosbridge
const USE_WEB_VIDEO_SERVER = true;
const WEB_VIDEO_SERVER_PORT = 8085; // Port for web_video_server (avoid conflict with ui_server 8080)

// --- [ROS2 Connection Setup] ---
const rosHost = window.location.hostname || 'localhost';
const ros = new ROSLIB.Ros({
    url: `ws://${rosHost}:9090`
});

const statusIndicator = document.getElementById('status-indicator');
const statusText = document.getElementById('status-text');

ros.on('connection', () => {
    statusIndicator.className = 'pulse-indicator green';
    statusText.innerText = 'Connected';
    console.log('Connected to rosbridge WebSocket server.');
    // Request map immediately via service (handles transient_local QoS on reconnect)
    setTimeout(() => requestMapFromService(), 1500);
});

ros.on('error', (error) => {
    statusIndicator.className = 'pulse-indicator red';
    statusText.innerText = 'Connection Error';
    console.error('Error connecting to rosbridge WebSocket:', error);
});

ros.on('close', () => {
    statusIndicator.className = 'pulse-indicator red';
    statusText.innerText = 'Disconnected';
    console.log('Connection to rosbridge WebSocket closed.');
});

// --- [ROS2 Topics & Message Types] ---
// Mode Control
const robotModePub = new ROSLIB.Topic({
    ros: ros,
    name: '/robot_mode',
    messageType: 'std_msgs/msg/String'
});

const robotModeStatusSub = new ROSLIB.Topic({
    ros: ros,
    name: '/robot_mode_status',
    messageType: 'std_msgs/msg/String'
});

// Waypoint configuration
const waypointPub = new ROSLIB.Topic({
    ros: ros,
    name: '/aruco_marker_waypoint',
    messageType: 'geometry_msgs/msg/PoseStamped'
});

// Nav2 goal configuration
const nav2GoalPub = new ROSLIB.Topic({
    ros: ros,
    name: '/goal_pose',
    messageType: 'geometry_msgs/msg/PoseStamped'
});

// Initial pose publisher (2D Pose Estimate)
const initialPosePub = new ROSLIB.Topic({
    ros: ros,
    name: '/initialpose',
    messageType: 'geometry_msgs/msg/PoseWithCovarianceStamped'
});

// Camera image stream (throttled to 10Hz if fallback is used)
const imageSub = new ROSLIB.Topic({
    ros: ros,
    name: '/aruco_detector/image/compressed',
    messageType: 'sensor_msgs/msg/CompressedImage',
    throttle_rate: 100
});

// ArUco marker pose subscriber
const arucoPoseSub = new ROSLIB.Topic({
    ros: ros,
    name: '/aruco_marker_pose',
    messageType: 'geometry_msgs/msg/PoseStamped',
    throttle_rate: 50
});

// Map subscriber
const mapSub = new ROSLIB.Topic({
    ros: ros,
    name: '/map',
    messageType: 'nav_msgs/msg/OccupancyGrid',
    throttle_rate: 2000
});

// Map service client - to request map from map_server after connection
const mapServiceClient = new ROSLIB.Service({
    ros: ros,
    name: '/map_server/map',
    serviceType: 'nav_msgs/srv/GetMap'
});

// Also try the GetMap service name used in Nav2 Humble
const getMapServiceClient = new ROSLIB.Service({
    ros: ros,
    name: '/map_server/get_map',
    serviceType: 'nav_msgs/srv/GetMap'
});

function requestMapFromService() {
    // Try /map_server/get_map first (Nav2 Humble standard)
    getMapServiceClient.callService(new ROSLIB.ServiceRequest({}), (result) => {
        if (result && result.map) {
            console.log('Map received via /map_server/get_map service.');
            processMapMessage(result.map);
        }
    }, (err) => {
        // Fallback: try /map_server/map
        mapServiceClient.callService(new ROSLIB.ServiceRequest({}), (result2) => {
            if (result2 && result2.map) {
                console.log('Map received via /map_server/map service.');
                processMapMessage(result2.map);
            }
        }, (err2) => {
            console.warn('Both map services unavailable, waiting for /map topic publish:', err2);
        });
    });
}

// Primary AMCL pose subscriber (Nav2 Map frame pose)
const amclPoseSub = new ROSLIB.Topic({
    ros: ros,
    name: '/amcl_pose',
    messageType: 'geometry_msgs/msg/PoseWithCovarianceStamped',
    throttle_rate: 50
});

// FAST-LIO2 LiDAR Odometry subscriber
const fastLioOdomSub = new ROSLIB.Topic({
    ros: ros,
    name: '/aft_mapped_to_init',
    messageType: 'nav_msgs/msg/Odometry',
    throttle_rate: 50
});

// Standard Odometry subscriber
const odomSub = new ROSLIB.Topic({
    ros: ros,
    name: '/odom',
    messageType: 'nav_msgs/msg/Odometry',
    throttle_rate: 50
});

// Wheel Odometry subscriber
const wheelOdomSub = new ROSLIB.Topic({
    ros: ros,
    name: '/odom_wheels',
    messageType: 'nav_msgs/msg/Odometry',
    throttle_rate: 50
});

// Joint State subscriber (throttled to 5Hz to update RPM gauges efficiently)
const jointStateSub = new ROSLIB.Topic({
    ros: ros,
    name: '/joint_states',
    messageType: 'sensor_msgs/msg/JointState',
    throttle_rate: 200
});

// Battery State subscriber
const batterySub = new ROSLIB.Topic({
    ros: ros,
    name: '/battery_state',
    messageType: 'sensor_msgs/msg/BatteryState',
    throttle_rate: 500
});

batterySub.subscribe(function(msg) {
    // percentage is 0.0-1.0 range from sensor_msgs/BatteryState
    let pct;
    if (msg.percentage !== undefined && msg.percentage !== null && !isNaN(msg.percentage)) {
        // Normalize: if value > 1.0, it's already in percent form
        pct = msg.percentage > 1.0 ? Math.round(msg.percentage) : Math.round(msg.percentage * 100);
    } else if (msg.voltage && msg.voltage > 0) {
        // Fallback: estimate from voltage (24V LiFePO4 8S: 21.6V=0%, 28.0V=100%)
        const minV = 21.6, maxV = 28.0;
        pct = Math.round(Math.max(0, Math.min(100, (msg.voltage - minV) / (maxV - minV) * 100)));
    } else {
        return; // No valid data
    }

    const badge = document.getElementById('battery-badge');
    const bar = document.getElementById('battery-bar');
    const valDisplay = document.getElementById('battery-value-display');
    const ringFill = document.getElementById('battery-ring-fill');

    if (badge) badge.innerText = `${pct}%`;
    if (valDisplay) valDisplay.innerText = `${pct}%`;
    if (ringFill) {
        const strokeDash = 188 - Math.round((pct / 100) * 188);
        ringFill.setAttribute('stroke-dashoffset', strokeDash);
    }
    if (bar) bar.style.width = `${pct}%`;
});


// --- [Global State] ---
let robotPose = { x: 0.0, y: 0.0, yaw: 0.0 };
let parkingWaypoint = null; // Start as null so no marker is drawn at (0,0) by default
let nav2Goal = { x: 0.0, y: 0.0, yaw: 0.0 };
let nav2GoalActive = false;
let currentMode = 'nav2'; // 'nav2' or 'parking'
let mapData = null;
let mapInfo = null;
let feedbackRPMs = [0, 0, 0, 0]; // FL, FR, RL, RR

// Offscreen canvas for map pre-rendering performance and zoom transforms
const offscreenCanvas = document.createElement('canvas');
const offscreenCtx = offscreenCanvas.getContext('2d');

// Zoom and Pan states
let zoom = 1.0;
let panX = 0.0;
let panY = 0.0;
let isPanning = false;
let panStart = { x: 0, y: 0 };

// --- [Canvas Map Rendering] ---
const canvas = document.getElementById('map-canvas');
const ctx = canvas.getContext('2d');

// requestAnimationFrame throttle to avoid redundant redraws
let drawMapPending = false;
function drawMap() {
    if (drawMapPending) return; // Already queued
    drawMapPending = true;
    requestAnimationFrame(_doDrawMap);
}

function _doDrawMap() {
    drawMapPending = false;
    if (!mapData || !mapInfo) return;

    const w = mapInfo.width;
    const h = mapInfo.height;

    // Resize main canvas dimensions to match map aspect ratio
    if (canvas.width !== w || canvas.height !== h) {
        canvas.width = w;
        canvas.height = h;
    }

    // Clear main canvas
    ctx.clearRect(0, 0, canvas.width, canvas.height);

    ctx.save();
    // Apply zoom & pan transforms relative to center of canvas
    ctx.translate(canvas.width / 2 + panX, canvas.height / 2 + panY);
    ctx.scale(zoom, zoom);
    ctx.translate(-canvas.width / 2, -canvas.height / 2);

    // Draw the offscreen map
    ctx.drawImage(offscreenCanvas, 0, 0);
    ctx.restore();

    // Draw Parking Waypoint (Target)
    if (currentMode === 'parking' && parkingWaypoint) {
        drawTarget(parkingWaypoint.x, parkingWaypoint.y, parkingWaypoint.yaw, '#00d2ff');
    }

    // Draw Nav2 Goal Target (Yellow)
    if (currentMode === 'nav2' && nav2GoalActive) {
        drawTarget(nav2Goal.x, nav2Goal.y, nav2Goal.yaw, '#eab308');
    }

    // Draw Robot Pose (Triangle)
    drawRobot(robotPose.x, robotPose.y, robotPose.yaw, '#10b981');
}

// Convert ROS meters coordinate (x,y) to Canvas pixels index (u,v)
defRosToCanvas = (rx, ry) => {
    if (!mapInfo) return { u: 0, v: 0 };
    const res = mapInfo.resolution;
    const originX = mapInfo.origin.position.x;
    const originY = mapInfo.origin.position.y;

    // Raw pixels coordinates
    const u_raw = (rx - originX) / res;
    const v_raw = canvas.height - ((ry - originY) / res);

    // Apply zoom & pan transformations
    const u = (u_raw - canvas.width / 2) * zoom + canvas.width / 2 + panX;
    const v = (v_raw - canvas.height / 2) * zoom + canvas.height / 2 + panY;
    return { u, v };
};

// Convert Canvas pixels index (u,v) back to ROS meters coordinate (x,y)
defCanvasToRos = (u, v) => {
    if (!mapInfo) return { rx: 0, ry: 0 };
    const res = mapInfo.resolution;
    const originX = mapInfo.origin.position.x;
    const originY = mapInfo.origin.position.y;

    // Reverse zoom & pan transformations
    const u_raw = (u - panX - canvas.width / 2) / zoom + canvas.width / 2;
    const v_raw = (v - panY - canvas.height / 2) / zoom + canvas.height / 2;

    const rx = (u_raw * res) + originX;
    const ry = ((canvas.height - v_raw) * res) + originY;
    return { rx, ry };
};

function drawRobot(rx, ry, ryaw, color) {
    const pt = defRosToCanvas(rx, ry);

    ctx.save();
    ctx.translate(pt.u, pt.v);
    // Draw robot pointing relative to Yaw
    // Canvas rotation is clockwise. In ROS, yaw increases counter-clockwise.
    // So we negate yaw to align correctly.
    ctx.rotate(-ryaw);

    // Draw triangle
    ctx.fillStyle = '#22A774';
    ctx.strokeStyle = '#047857';
    ctx.lineWidth = 2;
    ctx.shadowBlur = 6;
    ctx.shadowColor = 'rgba(34, 167, 116, 0.4)';
    ctx.beginPath();
    ctx.moveTo(12, 0);
    ctx.lineTo(-9, -7);
    ctx.lineTo(-5, 0);
    ctx.lineTo(-9, 7);
    ctx.closePath();
    ctx.fill();
    ctx.stroke();
    ctx.restore();
}

function drawTarget(rx, ry, ryaw, color) {
    const pt = defRosToCanvas(rx, ry);

    ctx.save();
    ctx.translate(pt.u, pt.v);
    ctx.rotate(-ryaw);

    // Draw target marker
    ctx.strokeStyle = color;
    ctx.lineWidth = 2;
    ctx.shadowBlur = 8;
    ctx.shadowColor = color;

    ctx.beginPath();
    ctx.arc(0, 0, 7, 0, 2 * Math.PI);
    ctx.stroke();

    // Draw heading line
    ctx.beginPath();
    ctx.moveTo(0, 0);
    ctx.lineTo(12, 0);
    ctx.stroke();

    ctx.restore();
}

// --- [Subscribe Listeners] ---
// processMapMessage: shared handler for both /map topic and /map_server/map service
function processMapMessage(message) {
    mapData = message.data;
    mapInfo = message.info;

    const w = mapInfo.width;
    const h = mapInfo.height;

    // Re-initialize offscreen canvas sizes if map sizes change
    if (offscreenCanvas.width !== w || offscreenCanvas.height !== h) {
        offscreenCanvas.width = w;
        offscreenCanvas.height = h;
    }

    // Draw occupancy grid grid-by-grid onto offscreen canvas
    const imgData = offscreenCtx.createImageData(w, h);
    for (let i = 0; i < mapData.length; i++) {
        const val = mapData[i];
        let r, g, b, a;
        if (val === 0) {
            // Free space: Clean crisp white
            r = 255; g = 255; b = 255; a = 255;
        } else if (val === 100) {
            // Occupied wall: Dark slate
            r = 30; g = 41; b = 59; a = 255;
        } else {
            // Unknown area: Soft light gray
            r = 226; g = 232; b = 240; a = 255;
        }

        const col = i % w;
        const row = h - 1 - Math.floor(i / w);
        const pixelIdx = (row * w + col) * 4;

        imgData.data[pixelIdx] = r;
        imgData.data[pixelIdx + 1] = g;
        imgData.data[pixelIdx + 2] = b;
        imgData.data[pixelIdx + 3] = a;
    }
    offscreenCtx.putImageData(imgData, 0, 0);

    drawMap();
}

// Map listener - handles live updates
mapSub.subscribe((message) => {
    processMapMessage(message);
});

// --- [Pose Tracking] ---
// Strategy: Use AMCL pose (map frame) as primary. Fall back to wheel odom ONLY if AMCL
// has not been received for 3+ seconds. NEVER mix frame origins.
let amclActive = false;
let lastAmclTime = 0;

function applySmoothedPose(newX, newY, newYaw) {
    // Strong smoothing to prevent UI jitter (alpha=0.4 = 40% new, 60% old)
    const alpha = 0.4;
    robotPose.x = alpha * newX + (1 - alpha) * robotPose.x;
    robotPose.y = alpha * newY + (1 - alpha) * robotPose.y;

    // Yaw wrap-around aware interpolation
    let diffYaw = newYaw - robotPose.yaw;
    while (diffYaw > Math.PI) diffYaw -= 2 * Math.PI;
    while (diffYaw < -Math.PI) diffYaw += 2 * Math.PI;
    robotPose.yaw += alpha * diffYaw;

    const poseVal = document.getElementById('val-pose');
    if (poseVal) {
        let yawDeg = Math.round(robotPose.yaw * 180 / Math.PI);
        if (yawDeg < 0) yawDeg += 360;
        poseVal.innerHTML = `X: ${robotPose.x.toFixed(2)}m &nbsp;|&nbsp; Y: ${robotPose.y.toFixed(2)}m &nbsp;|&nbsp; Yaw: ${yawDeg}°`;
    }
    drawMap();
}

function quatToYaw(q) {
    const siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    const cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    return Math.atan2(siny_cosp, cosy_cosp);
}

// 1. AMCL Pose Listener - PRIMARY source for Real AGV mode (map frame, most accurate)
amclPoseSub.subscribe((message) => {
    amclActive = true;
    lastAmclTime = Date.now();
    const pose = message.pose.pose;
    applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation));
});

// 2. Wheel Odometry - FALLBACK only when AMCL is not active (simulation/odom frame)
wheelOdomSub.subscribe((message) => {
    // Only use wheel odom if AMCL hasn't been received in last 3 seconds
    if (Date.now() - lastAmclTime < 3000) return;
    const pose = message.pose.pose;
    applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation));

    const twist = message.twist.twist;
    document.getElementById('val-linear').innerText = `${twist.linear.x.toFixed(2)} m/s`;
    document.getElementById('val-angular').innerText = `${twist.angular.z.toFixed(2)} rad/s`;
});

// 3. Standard /odom - Only update velocity display, never pose (to avoid frame mixing)
odomSub.subscribe((message) => {
    const twist = message.twist.twist;
    document.getElementById('val-linear').innerText = `${twist.linear.x.toFixed(2)} m/s`;
    document.getElementById('val-angular').innerText = `${twist.angular.z.toFixed(2)} rad/s`;
});

// 4. FAST-LIO2 - Only velocity display, AMCL gives more accurate map-frame pose
fastLioOdomSub.subscribe((message) => {
    if (!amclActive) {
        // If no AMCL available (e.g. before localization converges), use FAST-LIO pose
        if (Date.now() - lastAmclTime > 3000) {
            const pose = message.pose.pose;
            applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation));
        }
    }
});

// Camera stream setup with automatic fallback
const cameraStreamImg = document.getElementById('camera-stream');
let rosbridgeCameraActive = false;

function startRosbridgeCameraFallback() {
    if (rosbridgeCameraActive) return;
    rosbridgeCameraActive = true;
    console.warn('Falling back to ROS Bridge WebSocket camera stream (base64).');
    imageSub.subscribe((message) => {
        cameraStreamImg.src = "data:image/jpeg;base64," + message.data;
    });
}

function startWebVideoStream() {
    const videoStreamUrl = `http://${rosHost}:${WEB_VIDEO_SERVER_PORT}/stream?topic=/aruco_detector/image&type=mjpeg&transport=compressed`;
    cameraStreamImg.src = videoStreamUrl;
    cameraStreamImg.onerror = () => {
        // web_video_server is down — retry after 3s, then fall back to rosbridge
        console.warn(`web_video_server unreachable. Retrying in 3s...`);
        cameraStreamImg.src = '';
        setTimeout(() => {
            // Try once more before giving up and switching to rosbridge
            const retryImg = new Image();
            retryImg.onload = () => {
                // Server is back up — restore the stream
                console.log('web_video_server recovered. Restoring MJPEG stream.');
                cameraStreamImg.src = videoStreamUrl;
                cameraStreamImg.onerror = startWebVideoStream; // Re-attach error handler
            };
            retryImg.onerror = () => {
                console.warn('web_video_server still down. Switching to ROS Bridge fallback.');
                startRosbridgeCameraFallback();
            };
            retryImg.src = `http://${rosHost}:${WEB_VIDEO_SERVER_PORT}/`;
        }, 3000);
    };
    console.log(`Subscribed to camera stream via web_video_server: ${videoStreamUrl}`);
}

if (USE_WEB_VIDEO_SERVER) {
    startWebVideoStream();
} else {
    console.log("Subscribing to camera stream via ROS Bridge WebSocket (base64 fallback).");
    startRosbridgeCameraFallback();
}

// ArUco pose subscription for real-time UI feedback
let arucoTimeout = null;
arucoPoseSub.subscribe((message) => {
    if (arucoTimeout) {
        clearTimeout(arucoTimeout);
    }

    const statusBadge = document.getElementById('aruco-status');
    statusBadge.innerText = '인식됨 (Detected)';
    statusBadge.className = 'aruco-status-badge connected';

    // Parse ID from frame_id
    const markerId = message.header.frame_id.replace('aruco_marker_', '');
    const x = message.pose.position.x;
    const y = message.pose.position.y;
    const z = message.pose.position.z;

    // Convert quaternion to Euler angles (Roll, Pitch, Yaw)
    const q = message.pose.orientation;
    
    // roll (x-axis rotation)
    const sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    const cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    const roll = Math.atan2(sinr_cosp, cosr_cosp) * 180 / Math.PI;
    
    // pitch (y-axis rotation)
    const sinp = 2 * (q.w * q.y - q.z * q.x);
    let pitch = 0;
    if (Math.abs(sinp) >= 1) {
        pitch = Math.sign(sinp) * 90;
    } else {
        pitch = Math.asin(sinp) * 180 / Math.PI;
    }
    
    // yaw (z-axis rotation)
    const siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    const cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    const yaw = Math.atan2(siny_cosp, cosy_cosp) * 180 / Math.PI;

    document.getElementById('aruco-id').innerText = markerId;
    document.getElementById('aruco-x').innerText = x.toFixed(3) + ' m';
    document.getElementById('aruco-y').innerText = y.toFixed(3) + ' m';
    document.getElementById('aruco-z').innerText = z.toFixed(3) + ' m';
    document.getElementById('aruco-yaw').innerText = yaw.toFixed(1) + '°';
    document.getElementById('aruco-rp').innerText = roll.toFixed(1) + '° / ' + pitch.toFixed(1) + '°';

    // Reset UI if no new message within 1 second
    arucoTimeout = setTimeout(() => {
        statusBadge.innerText = '미인식 (No Marker)';
        statusBadge.className = 'aruco-status-badge disconnected';
        document.getElementById('aruco-id').innerText = '-';
        document.getElementById('aruco-x').innerText = '-';
        document.getElementById('aruco-y').innerText = '-';
        document.getElementById('aruco-z').innerText = '-';
        document.getElementById('aruco-yaw').innerText = '-';
        document.getElementById('aruco-rp').innerText = '-';
    }, 1000);
});

// Helper to update top mode summary card
function updateTopModeText(modeStr) {
    const topEl = document.getElementById('val-state-top');
    if (topEl) {
        topEl.innerText = modeStr;
    }
}

// Robot state listener
robotModeStatusSub.subscribe((message) => {
    document.getElementById('val-state').innerText = message.data;
    const status = message.data.toLowerCase();

    // Update Environment Badge
    const envBadge = document.getElementById('env-badge');
    if (status.includes('simulation')) {
        envBadge.innerText = 'Simulation Mode';
        envBadge.className = 'env-badge sim';
    } else if (status.includes('real')) {
        envBadge.innerText = 'Real AGV Mode';
        envBadge.className = 'env-badge real';
    }

    if (status.includes('mode: nav2')) {
        currentMode = 'nav2';
        updateTopModeText('Nav2');
        document.getElementById('btn-mode-nav2').className = 'btn btn-primary active';
        document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
        document.getElementById('btn-mode-parking').className = 'btn btn-primary';
        document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
    } else if (status.includes('mode: patrol')) {
        currentMode = 'patrol';
        updateTopModeText('Patrol');
        document.getElementById('btn-mode-patrol').className = 'btn btn-primary active';
        document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
        document.getElementById('btn-mode-parking').className = 'btn btn-primary';
        document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
    } else if (status.includes('mode: parking')) {
        currentMode = 'parking';
        updateTopModeText('Parking');
        document.getElementById('btn-mode-parking').className = 'btn btn-primary active';
        document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
        document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
        document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
    } else if (status.includes('mode: stop') || status.includes('state: estop')) {
        currentMode = 'stop';
        updateTopModeText('STOP');
        document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
        document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
        document.getElementById('btn-mode-parking').className = 'btn btn-primary';
        document.getElementById('btn-emergency-stop').className = 'btn btn-danger active';
    }
});

// Joint State listener (RPM updates)
jointStateSub.subscribe((message) => {
    // Get velocities in rad/s, convert to RPM (RPM = rad/s * 60 / (2 * pi))
    // Joint indices might vary, we average or read them based on names
    const names = message.name;
    const vel = message.velocity;

    names.forEach((name, idx) => {
        const rpm = Math.round(vel[idx] * 60 / (2 * Math.PI));

        let barId = '';
        let valId = '';
        if (name.includes('front_left')) { barId = 'bar-fl'; valId = 'val-fl'; }
        else if (name.includes('front_right')) { barId = 'bar-fr'; valId = 'val-fr'; }
        else if (name.includes('rear_left')) { barId = 'bar-rl'; valId = 'val-rl'; }
        else if (name.includes('rear_right')) { barId = 'bar-rr'; valId = 'val-rr'; }

        if (barId && valId) {
            document.getElementById(valId).innerText = `${rpm} RPM`;
            // Scale bar width (max 250 RPM is 100% width)
            const pct = Math.min(Math.abs(rpm) / 250 * 100, 100);
            document.getElementById(barId).style.width = `${pct}%`;
        }
    });
});


// --- [UI Interaction Actions] ---

// Toggle Nav2 mode
document.getElementById('btn-mode-nav2').addEventListener('click', () => {
    currentMode = 'nav2';
    nav2GoalActive = false;
    parkingWaypoint = null; // Clear waypoint on UI
    updateTopModeText('Nav2');
    document.getElementById('btn-mode-nav2').className = 'btn btn-primary active';
    document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
    document.getElementById('btn-mode-parking').className = 'btn btn-primary';

    const msg = new ROSLIB.Message({ data: 'nav2' });
    robotModePub.publish(msg);
});

// Toggle Patrol mode
document.getElementById('btn-mode-patrol').addEventListener('click', () => {
    currentMode = 'patrol';
    parkingWaypoint = null;
    nav2GoalActive = false;
    updateTopModeText('Patrol');
    document.getElementById('btn-mode-patrol').className = 'btn btn-primary active';
    document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
    document.getElementById('btn-mode-parking').className = 'btn btn-primary';

    const msg = new ROSLIB.Message({ data: 'patrol' });
    robotModePub.publish(msg);
});

// Toggle Parking mode
document.getElementById('btn-mode-parking').addEventListener('click', () => {
    currentMode = 'parking';
    parkingWaypoint = null; // Clear waypoint on UI until set by user
    updateTopModeText('Parking');
    document.getElementById('btn-mode-parking').className = 'btn btn-primary active';
    document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
    document.getElementById('btn-mode-patrol').className = 'btn btn-primary';

    const msg = new ROSLIB.Message({ data: 'parking' });
    robotModePub.publish(msg);
});

// Toggle 2D Pose Estimate Mode
let poseEstimateMode = false;
document.getElementById('btn-pose-estimate').addEventListener('click', () => {
    poseEstimateMode = true;
    document.getElementById('btn-pose-estimate').className = 'btn btn-secondary active';
    document.getElementById('btn-pose-estimate').innerText = '📍 지도를 클릭/드래그하여 로봇 위치 지정...';
    nav2GoalActive = false;
    drawMap();
});

// Trigger Emergency Stop (ESTOP)
document.getElementById('btn-emergency-stop').addEventListener('click', () => {
    currentMode = 'stop';
    nav2GoalActive = false;
    parkingWaypoint = null; // Clear waypoint on UI
    updateTopModeText('STOP');
    document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
    document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
    document.getElementById('btn-mode-parking').className = 'btn btn-primary';
    document.getElementById('btn-emergency-stop').className = 'btn btn-danger active';

    const msg = new ROSLIB.Message({ data: 'stop' });
    robotModePub.publish(msg);
});

// Send updated Waypoint manually from inputs
document.getElementById('btn-set-wp').addEventListener('click', () => {
    const x = parseFloat(document.getElementById('wp-x').value);
    const y = parseFloat(document.getElementById('wp-y').value);
    const yawDeg = parseFloat(document.getElementById('wp-yaw').value);
    const yawRad = yawDeg * Math.PI / 180.0;

    if (currentMode === 'nav2') {
        nav2Goal.x = x;
        nav2Goal.y = y;
        nav2Goal.yaw = yawRad;
        nav2GoalActive = true;
        publishNav2Goal();
    } else {
        parkingWaypoint = { x: x, y: y, yaw: yawRad };
        publishWaypoint();
    }
});

// Drag-to-steer (Rviz2 style) mouse handlers
// Drag-to-steer and Pan/Zoom mouse handlers
let isDragging = false;
let dragStartCoords = { x: 0, y: 0 };
let dragStartRos = { x: 0, y: 0 };
let currentDragYaw = 0.0;

// Prevent standard context menu on canvas to allow right-click panning
canvas.addEventListener('contextmenu', (event) => {
    event.preventDefault();
});

canvas.addEventListener('mousedown', (event) => {
    if (!mapInfo) return;

    // Right click OR Shift + Left click starts Panning
    if (event.button === 2 || (event.button === 0 && event.shiftKey)) {
        isPanning = true;
        panStart.x = event.clientX;
        panStart.y = event.clientY;
        return;
    }

    // Standard Left Click starts Drag-to-steer
    if (event.button === 0) {
        isDragging = true;

        const rect = canvas.getBoundingClientRect();
        const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height);
        const dx_padding = (rect.width - canvas.width * scale) / 2;
        const dy_padding = (rect.height - canvas.height * scale) / 2;

        const clickU = (event.clientX - rect.left - dx_padding) / scale;
        const clickV = (event.clientY - rect.top - dy_padding) / scale;

        dragStartCoords.x = clickU;
        dragStartCoords.y = clickV;

        const coords = defCanvasToRos(clickU, clickV);
        dragStartRos.x = coords.rx;
        dragStartRos.y = coords.ry;

        document.getElementById('wp-x').value = coords.rx.toFixed(2);
        document.getElementById('wp-y').value = coords.ry.toFixed(2);

        const yawDeg = parseFloat(document.getElementById('wp-yaw').value) || 0.0;
        currentDragYaw = yawDeg * Math.PI / 180.0;

        if (poseEstimateMode) {
            // Update local robotPose estimate representation
            robotPose.x = coords.rx;
            robotPose.y = coords.ry;
            robotPose.yaw = currentDragYaw;
        } else if (currentMode === 'nav2') {
            nav2Goal.x = coords.rx;
            nav2Goal.y = coords.ry;
            nav2Goal.yaw = currentDragYaw;
            nav2GoalActive = true;
        } else {
            parkingWaypoint = { x: coords.rx, y: coords.ry, yaw: currentDragYaw };
        }
        drawMap();
    }
});

canvas.addEventListener('mousemove', (event) => {
    const rect = canvas.getBoundingClientRect();
    const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height);

    if (isPanning) {
        const dx = event.clientX - panStart.x;
        const dy = event.clientY - panStart.y;

        // Pan dynamically mapped to canvas pixels and zoom factor
        panX += dx / (scale * zoom);
        panY += dy / (scale * zoom);

        panStart.x = event.clientX;
        panStart.y = event.clientY;
        drawMap();
        return;
    }

    if (!isDragging || !mapInfo) return;

    const dx_padding = (rect.width - canvas.width * scale) / 2;
    const dy_padding = (rect.height - canvas.height * scale) / 2;

    const clickU = (event.clientX - rect.left - dx_padding) / scale;
    const clickV = (event.clientY - rect.top - dy_padding) / scale;

    const dx = clickU - dragStartCoords.x;
    const dy = clickV - dragStartCoords.y;

    if (Math.sqrt(dx * dx + dy * dy) > 8) { // Drag threshold
        currentDragYaw = Math.atan2(-dy, dx);

        let yawDeg = Math.round(currentDragYaw * 180.0 / Math.PI);
        if (yawDeg < 0) yawDeg += 360;
        document.getElementById('wp-yaw').value = yawDeg;

        if (poseEstimateMode) {
            robotPose.yaw = currentDragYaw;
        } else if (currentMode === 'nav2') {
            nav2Goal.yaw = currentDragYaw;
        } else {
            if (!parkingWaypoint) {
                parkingWaypoint = { x: dragStartRos.x, y: dragStartRos.y, yaw: currentDragYaw };
            }
            parkingWaypoint.yaw = currentDragYaw;
        }

        drawMap();

        // Render drag line (drawn in screen space)
        ctx.strokeStyle = '#f43f5e';
        ctx.lineWidth = 2;
        ctx.beginPath();
        ctx.moveTo(dragStartCoords.x, dragStartCoords.y);
        ctx.lineTo(clickU, clickV);
        ctx.stroke();
    }
});

canvas.addEventListener('mouseup', (event) => {
    if (isPanning) {
        isPanning = false;
        return;
    }

    if (!isDragging) return;
    isDragging = false;

    if (poseEstimateMode) {
        publishInitialPose(dragStartRos.x, dragStartRos.y, currentDragYaw);
        poseEstimateMode = false;
        document.getElementById('btn-pose-estimate').className = 'btn btn-secondary';
        document.getElementById('btn-pose-estimate').innerText = '📍 로봇 위치 초기화 (Pose Estimate)';
    } else if (currentMode === 'nav2') {
        publishNav2Goal();
    } else {
        publishWaypoint();
    }
});

// Scroll Wheel Zoom
canvas.addEventListener('wheel', (event) => {
    if (!mapInfo) return;
    event.preventDefault();

    const zoomFactor = 1.1;
    if (event.deltaY < 0) {
        zoom *= zoomFactor;
    } else {
        zoom /= zoomFactor;
    }

    // Zoom limit boundaries (0.3x to 8x)
    zoom = Math.max(0.3, Math.min(zoom, 8.0));
    drawMap();
}, { passive: false });

// Mobile Touch Interactions (touchstart, touchmove, touchend)
let touchStartCoords = { x: 0, y: 0 };
let isTouchDragging = false;

canvas.addEventListener('touchstart', (e) => {
    if (e.touches.length === 1) {
        e.preventDefault();
        const touch = e.touches[0];
        const rect = canvas.getBoundingClientRect();
        const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height);
        const dx_padding = (rect.width - canvas.width * scale) / 2;
        const dy_padding = (rect.height - canvas.height * scale) / 2;

        const clickU = (touch.clientX - rect.left - dx_padding) / scale;
        const clickV = (touch.clientY - rect.top - dy_padding) / scale;

        touchStartCoords.x = touch.clientX;
        touchStartCoords.y = touch.clientY;
        dragStartCoords.x = clickU;
        dragStartCoords.y = clickV;

        const coords = defCanvasToRos(clickU, clickV);
        dragStartRos.x = coords.rx;
        dragStartRos.y = coords.ry;

        document.getElementById('wp-x').value = coords.rx.toFixed(2);
        document.getElementById('wp-y').value = coords.ry.toFixed(2);

        isTouchDragging = true;

        if (poseEstimateMode) {
            robotPose.x = coords.rx;
            robotPose.y = coords.ry;
        } else if (currentMode === 'nav2') {
            nav2Goal.x = coords.rx;
            nav2Goal.y = coords.ry;
            nav2GoalActive = true;
        } else {
            parkingWaypoint = { x: coords.rx, y: coords.ry, yaw: 0.0 };
        }
        drawMap();
    }
}, { passive: false });

canvas.addEventListener('touchmove', (e) => {
    if (!isTouchDragging || e.touches.length !== 1) return;
    e.preventDefault();
    const touch = e.touches[0];
    const rect = canvas.getBoundingClientRect();
    const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height);

    const dx = touch.clientX - touchStartCoords.x;
    const dy = touch.clientY - touchStartCoords.y;

    if (Math.abs(dx) > 5 || Math.abs(dy) > 5) {
        currentDragYaw = Math.atan2(-dy, dx);
        let yawDeg = Math.round(currentDragYaw * 180.0 / Math.PI);
        if (yawDeg < 0) yawDeg += 360;
        document.getElementById('wp-yaw').value = yawDeg;

        if (poseEstimateMode) {
            robotPose.yaw = currentDragYaw;
        } else if (currentMode === 'nav2') {
            nav2Goal.yaw = currentDragYaw;
        } else {
            if (parkingWaypoint) parkingWaypoint.yaw = currentDragYaw;
        }
        drawMap();
    }
}, { passive: false });

canvas.addEventListener('touchend', (e) => {
    if (isTouchDragging) {
        isTouchDragging = false;
        if (poseEstimateMode) {
            publishInitialPose(dragStartRos.x, dragStartRos.y, currentDragYaw);
            poseEstimateMode = false;
            document.getElementById('btn-pose-estimate').className = 'btn btn-secondary';
            document.getElementById('btn-pose-estimate').innerText = '📍 로봇 위치 초기화 (Pose Estimate)';
        } else if (currentMode === 'nav2') {
            publishNav2Goal();
        } else {
            publishWaypoint();
        }
    }
});

// Double-click to reset zoom & pan translation
canvas.addEventListener('dblclick', () => {
    zoom = 1.0;
    panX = 0.0;
    panY = 0.0;
    drawMap();
});

function publishWaypoint() {
    const yaw = parkingWaypoint.yaw;

    const msg = new ROSLIB.Message({
        header: {
            frame_id: 'map',
            stamp: { secs: 0, nsecs: 0 } // Rosbridge populates timestamp if zero
        },
        pose: {
            position: { x: parkingWaypoint.x, y: parkingWaypoint.y, z: 0.0 },
            orientation: {
                x: 0.0,
                y: 0.0,
                z: Math.sin(yaw * 0.5),
                w: Math.cos(yaw * 0.5)
            }
        }
    });

    waypointPub.publish(msg);
    console.log(`Published new waypoint coordinates: x=${parkingWaypoint.x.toFixed(2)}, y=${parkingWaypoint.y.toFixed(2)}, yaw=${(yaw * 180 / Math.PI).toFixed(0)}deg`);
    drawMap();
}

function publishNav2Goal() {
    const yaw = nav2Goal.yaw;

    const msg = new ROSLIB.Message({
        header: {
            frame_id: 'map',
            stamp: { secs: 0, nsecs: 0 }
        },
        pose: {
            position: { x: nav2Goal.x, y: nav2Goal.y, z: 0.0 },
            orientation: {
                x: 0.0,
                y: 0.0,
                z: Math.sin(yaw * 0.5),
                w: Math.cos(yaw * 0.5)
            }
        }
    });

    nav2GoalPub.publish(msg);
    console.log(`Published Nav2 Goal: x=${nav2Goal.x.toFixed(2)}, y=${nav2Goal.y.toFixed(2)}, yaw=${(yaw * 180 / Math.PI).toFixed(0)}deg`);
    drawMap();
}

function publishInitialPose(x, y, yaw) {
    // Covariance is 6x6 row-major, indices [0][0]=x, [1][1]=y, [5][5]=yaw
    const cov = new Array(36).fill(0.0);
    cov[0]  = 0.25;                    // xx uncertainty (0.5m std dev)
    cov[7]  = 0.25;                    // yy uncertainty (0.5m std dev)
    cov[35] = 0.06853891945200942;     // yaw*yaw uncertainty (~15deg std dev)

    const msg = new ROSLIB.Message({
        header: {
            frame_id: 'map',
            stamp: { secs: 0, nsecs: 0 }
        },
        pose: {
            pose: {
                position: { x: x, y: y, z: 0.0 },
                orientation: {
                    x: 0.0,
                    y: 0.0,
                    z: Math.sin(yaw * 0.5),
                    w: Math.cos(yaw * 0.5)
                }
            },
            covariance: cov
        }
    });

    initialPosePub.publish(msg);
    console.log(`Published Initial Pose Reset: x=${x.toFixed(2)}, y=${y.toFixed(2)}, yaw=${(yaw * 180 / Math.PI).toFixed(0)}deg`);
}

// --- [Mobile UI Functions] ---

// Toggle Mobile Drawer Sidebar
function toggleMobileSidebar() {
    const sidebar = document.getElementById('appSidebar');
    const overlay = document.getElementById('sidebarOverlay');
    if (sidebar && overlay) {
        sidebar.classList.toggle('mobile-open');
        overlay.classList.toggle('active');
    }
}

if ('serviceWorker' in navigator) {
    window.addEventListener('load', () => {
        navigator.serviceWorker.register('./sw.js')
            .then(reg => console.log('PWA ServiceWorker registered:', reg.scope))
            .catch(err => console.warn('PWA ServiceWorker registration failed:', err));
    });
}
