#!/usr/bin/env node
/**
 * LLUAV SDK Demo: Real-time State Monitor (Node.js)
 *
 * Demonstrates:
 *   - Auto-reconnect & heartbeat
 *   - Periodic task scheduler
 *   - Graceful shutdown with Ctrl+C
 *
 * Usage:
 *   node demo_state_monitor.js [host] [port] [password]
 */

const { LLUAVClient, LLUAVCommandError } = require('../src/client');

function formatPose(j) {
    const lp = j.lidar_pose || {};
    return `POS(${lp.x ?? '?'},${lp.y ?? '?'},${lp.z ?? '?'}) YAW=${lp.yaw ?? '?'} ROLL=${lp.roll ?? '?'} PITCH=${lp.pitch ?? '?'}`;
}

function formatHud(j) {
    const pos = j.uav_pos_enu || {};
    const extra = j.extra || {};
    return `HUD ENU(${pos.x?.toFixed(2) ?? '?'},${pos.y?.toFixed(2) ?? '?'},${pos.z?.toFixed(2) ?? '?'}) Vol=${extra.volume_est ?? 'N/A'}`;
}

function formatState(j) {
    const st = j.state || {};
    return `STATE armed=${st.armed ?? '?'} ctrl=${st.ctrl_state ?? '?'} batt=${st.battery_v ?? '?'}V`;
}

async function main() {
    const host = process.argv[2] || 'localhost';
    const port = parseInt(process.argv[3] || '53001', 10);
    const password = process.argv[4] || null;

    const client = new LLUAVClient(host, port, password, 10000, {
        autoReconnect: true,
        heartbeatInterval: 5000,
    });

    let running = true;
    process.on('SIGINT', () => {
        console.log('\nStopping monitor...');
        running = false;
    });

    try {
        console.log(`Connecting to ${client.url} ...`);
        await client.connect();
        console.log('Connected. Press Ctrl+C to stop.\n');

        // Register periodic task for state polling
        client.addPeriodicTask('statePoll', 2, 'state');

        while (running) {
            await new Promise(r => setTimeout(r, 2000));
            if (!running) break;

            try {
                const pose = await client.getLidarPose();
                console.log(`[POSE]  ${formatPose(pose)}`);
            } catch (e) {
                console.log(`[POSE]  Error: ${e.message}`);
            }

            try {
                const hud = await client.getHud1();
                console.log(`[HUD]   ${formatHud(hud)}`);
            } catch (e) {
                console.log(`[HUD]   Error: ${e.message}`);
            }

            try {
                const state = await client.getState();
                console.log(`[STATE] ${formatState(state)}`);
            } catch (e) {
                console.log(`[STATE] Error: ${e.message}`);
            }
            console.log();
        }

        await client.disconnect();
        console.log('Disconnected.');
    } catch (e) {
        console.error('Fatal error:', e.message);
        process.exit(1);
    }
}

main();
