#!/usr/bin/env node
/**
 * LLUAV SDK Demo: Automated Flight Mission (Node.js)
 *
 * Demonstrates a complete flight workflow:
 *   1. Connect & login
 *   2. Preflight check
 *   3. Get initial state
 *   4. Takeoff
 *   5. Set setpoint (NED)
 *   6. RTL
 *
 * Usage:
 *   node demo_flight_mission.js [host] [port] [password]
 */

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

async function sleep(ms) {
    return new Promise(r => setTimeout(r, ms));
}

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

    try {
        console.log('==================================================');
        console.log('LLUAV Flight Mission Demo (Node.js)');
        console.log('==================================================');

        // 1. Connect
        console.log('\n[1/6] Connecting...');
        await client.connect();
        console.log(`      Connected to ${client.url}`);

        // 2. Preflight check
        console.log('\n[2/6] Running preflight check...');
        try {
            const preflight = await client.preflightCheck();
            const ret = preflight.preflight_check ?? -1;
            const msg = preflight.preflight_check_msg ?? 'Unknown';
            console.log(`      Result: ${msg} (code=${ret})`);
            if (ret !== 0) {
                console.log('      WARNING: Preflight check failed!');
            }
        } catch (e) {
            console.log(`      Preflight check error: ${e.message}`);
        }

        // 3. Initial state
        console.log('\n[3/6] Getting initial state...');
        const state = await client.getState();
        const st = state.state || {};
        console.log(`      Armed: ${st.armed ?? '?'}  CtrlState: ${st.ctrl_state ?? '?'}`);

        // 4. Takeoff
        console.log('\n[4/6] Commanding TAKEOFF...');
        const takeoffResp = await client.takeoff();
        console.log(`      Takeoff acknowledged: ret=${takeoffResp.ret ?? '?'}`);
        console.log('      Waiting 5 seconds for climb...');
        await sleep(5000);

        // 5. Set setpoint
        const spX = 5.0, spY = 0.0, spZ = -5.0, spYaw = 0.0;
        console.log(`\n[5/6] Setting setpoint NED: x=${spX}, y=${spY}, z=${spZ}, yaw=${spYaw}`);
        const spResp = await client.setSpNed(spX, spY, spZ, spYaw);
        console.log(`      Setpoint acknowledged: ret=${spResp.ret ?? '?'}`);
        console.log('      Waiting 5 seconds...');
        await sleep(5000);

        // 6. RTL
        console.log('\n[6/6] Commanding RTL...');
        const rtlResp = await client.rtl();
        console.log(`      RTL acknowledged: ret=${rtlResp.ret ?? '?'}`);

        console.log('\n==================================================');
        console.log('Mission demo completed.');
        console.log('==================================================');

        await client.disconnect();
    } catch (e) {
        if (e instanceof LLUAVCommandError) {
            console.error(`Command failed: ${e.message} (ret=${e.retCode})`);
            console.error('Response:', JSON.stringify(e.response, null, 2));
        } else {
            console.error('Error:', e.message);
        }
        process.exit(1);
    }
}

main();
