// 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.');
});

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 (throttled to 1Hz since maps update infrequently)
const mapSub = new ROSLIB.Topic({
    ros: ros,
    name: '/map',
    messageType: 'nav_msgs/msg/OccupancyGrid',
    throttle_rate: 1000
});

// Odometry subscriber (throttled to 10Hz for smoother rendering without CPU spikes)
const odomSub = new ROSLIB.Topic({
    ros: ros,
    name: '/odom_wheels',
    messageType: 'nav_msgs/msg/Odometry',
    throttle_rate: 100
});

// 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) {
    const voltage = msg.voltage ? msg.voltage.toFixed(1) : '26.4';
    const pct = Math.round((msg.percentage !== undefined ? msg.percentage : 0.85) * 100);
    
    const badge = document.getElementById('battery-badge');
    const bar = document.getElementById('battery-bar');
    
    if (badge) {
        badge.innerText = `${pct}% (${voltage}V)`;
        if (pct >= 50) {
            badge.style.background = 'rgba(34, 197, 94, 0.2)';
            badge.style.color = '#4ade80';
            badge.style.borderColor = 'rgba(74, 222, 128, 0.4)';
        } else if (pct >= 20) {
            badge.style.background = 'rgba(234, 179, 8, 0.2)';
            badge.style.color = '#facc15';
            badge.style.borderColor = 'rgba(250, 204, 21, 0.4)';
        } else {
            badge.style.background = 'rgba(239, 68, 68, 0.2)';
            badge.style.color = '#f87171';
            badge.style.borderColor = 'rgba(248, 113, 113, 0.4)';
        }
    }
    
    if (bar) {
        bar.style.width = `${pct}%`;
        if (pct >= 50) {
            bar.style.background = 'linear-gradient(90deg, #22c55e, #4ade80)';
        } else if (pct >= 20) {
            bar.style.background = 'linear-gradient(90deg, #eab308, #facc15)';
        } else {
            bar.style.background = 'linear-gradient(90deg, #ef4444, #f87171)';
        }
    }
});


// --- [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');

function drawMap() {
    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 = color;
    ctx.shadowBlur = 10;
    ctx.shadowColor = color;
    ctx.beginPath();
    ctx.moveTo(10, 0);
    ctx.lineTo(-8, -6);
    ctx.lineTo(-4, 0);
    ctx.lineTo(-8, 6);
    ctx.closePath();
    ctx.fill();
    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] ---
// Map listener
mapSub.subscribe((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) {
            r = 15; g = 18; b = 32; a = 255;
        } else if (val === 100) {
            r = 157; g = 78; b = 221; a = 255;
        } else {
            r = 6; g = 6; b = 10; 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();
});

// Odom listener
odomSub.subscribe((message) => {
    const pose = message.pose.pose;
    robotPose.x = pose.position.x;
    robotPose.y = pose.position.y;

    // Quaternion to Euler yaw
    const q = pose.orientation;
    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);
    robotPose.yaw = Math.atan2(siny_cosp, cosy_cosp);

    // Update speeds
    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`;

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

    drawMap();
});

// 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);
});

// 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';
        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';
        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';
        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';
        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
    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;
    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
    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
    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 });

// 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) {
    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: [
                0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
                0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                0.0, 0.0, 0.0, 0.06853891945200942, 0.0, 0.0,
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
            ]
        }
    });

    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`);
    drawMap();
}