#include "stm32f7xx_hal.h"
#include "usbd_core.h"
#include "usbd_cdc.h"
#include "usbd_cdc_if.h"
#include "usbd_desc.h"
#include "mpu6000.h"
#include "balance.h"
#include "hoverboard.h"
#include "motor_driver.h"
#include "mode_manager.h"
#include "config.h"
#include "status.h"
#include "safety.h"
#include "crsf.h"
#include "i2c1.h"
#include "bmp280.h"
#include "mag.h"
#include "bno055.h"
#include "jetson_cmd.h"
#include "jetson_uart.h"
#include "jlink.h"
#include "ota.h"
#include "audio.h"
#include "buzzer.h"
#include "led.h"
#include "servo.h"
#include "ina219.h"
#include "ultrasonic.h"
#include "power_mgmt.h"
#include "battery.h"
#include "coulomb_counter.h"
#include "watchdog.h"
#include "pid_flash.h"
#include "fault_handler.h"
#include "can_driver.h"
#include "servo_bus.h"
#include "gimbal.h"
#include "lvc.h"
#include <math.h>
#include <string.h>
#include <stdio.h>

extern volatile uint8_t  cdc_streaming;      /* set by S command in CDC RX */
extern volatile uint8_t  cdc_arm_request;    /* set by A command */
extern volatile uint8_t  cdc_disarm_request; /* set by D command */
extern volatile uint8_t  cdc_recal_request;  /* set by G command */
extern volatile uint32_t cdc_rx_count;       /* total CDC packets received */
extern volatile uint8_t  cdc_estop_request;
extern volatile uint8_t  cdc_estop_clear_request;

/* Direct motor test (set by W command in jetson_uart.c) */
volatile int16_t direct_test_speed = 0;
volatile int16_t direct_test_steer = 0;

/* BNO055 active flag (set if BNO055 initialized successfully) */
static bool bno055_active = false;

/* Watchdog reset flag (set if MCU was reset by IWDG timeout) */
static bool g_watchdog_reset_detected = false;

/*
 * Apply a PID tuning command string from the USB terminal.
 * Format: P<kp>  I<ki>  D<kd>  T<setpoint_deg>  M<max_speed>  ?
 * Returns a short ack string written into `reply` (max reply_sz bytes).
 */
static void apply_pid_cmd(balance_t *b, const char *cmd, char *reply, int reply_sz) {
    float val = 0.0f;
    /* Simple ASCII float parse — avoids sscanf in hot path */
    if (cmd[0] == '?') {
        snprintf(reply, reply_sz,
            "{\"kp\":%d,\"ki\":%d,\"kd\":%d,\"sp\":%d,\"ms\":%d}\n",
            (int)(b->kp * 1000), (int)(b->ki * 1000), (int)(b->kd * 1000),
            (int)(b->setpoint * 10), (int)b->max_speed);
        return;
    }
    /* Parse float after the command letter */
    if (sscanf(cmd + 1, "%f", &val) != 1) {
        snprintf(reply, reply_sz, "{\"err\":\"bad_fmt\"}\n");
        return;
    }
    switch (cmd[0]) {
        case 'P': if (val >= 0.0f && val <= 500.0f) b->kp = val;        break;
        case 'I': if (val >= 0.0f && val <= 50.0f)  b->ki = val;        break;
        case 'K': /* alias for D */ /* fall through */
        case 'D': if (val >= 0.0f && val <= 50.0f)  b->kd = val;        break;
        case 'T': if (val >= -20.0f && val <= 20.0f) b->setpoint = val; break;
        case 'M': if (val >= 0.0f && val <= 1000.0f) b->max_speed = (int16_t)val; break;
    }
    snprintf(reply, reply_sz,
        "{\"kp\":%d,\"ki\":%d,\"kd\":%d,\"sp\":%d,\"ms\":%d}\n",
        (int)(b->kp * 1000), (int)(b->ki * 1000), (int)(b->kd * 1000),
        (int)(b->setpoint * 10), (int)b->max_speed);
}

USBD_HandleTypeDef hUsbDevice;

static void SystemClock_Config(void) {
    RCC_OscInitTypeDef osc = {0};
    RCC_ClkInitTypeDef clk = {0};
    RCC_PeriphCLKInitTypeDef pclk = {0};
    __HAL_RCC_PWR_CLK_ENABLE();
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
    osc.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    osc.HSEState = RCC_HSE_ON;
    osc.PLL.PLLState = RCC_PLL_ON;
    osc.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    osc.PLL.PLLM = 8; osc.PLL.PLLN = 432; osc.PLL.PLLP = 2; osc.PLL.PLLQ = 9;
    if (HAL_RCC_OscConfig(&osc) != HAL_OK) {
        osc.OscillatorType = RCC_OSCILLATORTYPE_HSI;
        osc.HSIState = RCC_HSI_ON; osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
        osc.HSEState = RCC_HSE_OFF; osc.PLL.PLLSource = RCC_PLLSOURCE_HSI; osc.PLL.PLLM = 16;
        if (HAL_RCC_OscConfig(&osc) != HAL_OK) while (1);
    }
    HAL_PWREx_EnableOverDrive();
    pclk.PeriphClockSelection = RCC_PERIPHCLK_CLK48 | RCC_PERIPHCLK_I2C1;
    pclk.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP;
    pclk.PLLSAI.PLLSAIN = 384; pclk.PLLSAI.PLLSAIQ = 7; pclk.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8;
    pclk.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
    if (HAL_RCCEx_PeriphCLKConfig(&pclk) != HAL_OK) while (1);
    clk.ClockType = RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
    clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    clk.AHBCLKDivider = RCC_SYSCLK_DIV1;
    clk.APB1CLKDivider = RCC_HCLK_DIV4; clk.APB2CLKDivider = RCC_HCLK_DIV2;
    HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7);
}

extern PCD_HandleTypeDef hpcd;
void OTG_FS_IRQHandler(void) { HAL_PCD_IRQHandler(&hpcd); }
void SysTick_Handler(void) { HAL_IncTick(); }

int main(void) {
    SCB_EnableICache();
    /* DCache stays ON — MPU Region 0 in usbd_conf.c marks USB buffers non-cacheable. */
    checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */
    HAL_Init();
    SystemClock_Config();

    /* Fault recovery handler (Issue #565) — must be first, before safety_init() */
    fault_handler_init();

    /* Detect watchdog reset (Issue #300) — must be before safety_init() */
    g_watchdog_reset_detected = watchdog_was_reset_by_watchdog();

    /* USB CDC */
    USBD_Init(&hUsbDevice, &SaltyLab_Desc, 0);
    USBD_RegisterClass(&hUsbDevice, &USBD_CDC);
    USBD_CDC_RegisterInterface(&hUsbDevice, &USBD_CDC_fops);
    USBD_Start(&hUsbDevice);

    status_init();
    HAL_Delay(3000);  /* Wait for USB host to enumerate */

    /* Log watchdog reset event if it occurred (Issue #300) */
    if (g_watchdog_reset_detected) {
        printf("[WATCHDOG] MCU reset by IWDG timeout detected. Main loop may have hung.\n");
    }

    /* Init IMU (MPU6000 via SPI1 — mpu6000.c wraps icm42688 + complementary filter) */
    int imu_ret = mpu6000_init() ? 0 : -1;

    /*
     * Gyro bias calibration — hold board still for ~1s.
     * Must run before safety_init() (IWDG not yet started).
     * Blocks arming and streaming until complete.
     */
    if (imu_ret == 0) mpu6000_calibrate();

    /* Init hoverboard ESC UART */
    hoverboard_init();

    /* Init balance controller */
    balance_t bal;
    balance_init(&bal);

    /* Load PID from flash if previously auto-tuned (Issue #531) */
    {
        float flash_kp, flash_ki, flash_kd;
        if (pid_flash_load(&flash_kp, &flash_ki, &flash_kd)) {
            bal.kp = flash_kp;
            bal.ki = flash_ki;
            bal.kd = flash_kd;
            printf("[PID] Loaded from flash: Kp=%.3f Ki=%.3f Kd=%.3f\n",
                   (double)flash_kp, (double)flash_ki, (double)flash_kd);
        }
    }

    /* Init motor driver */
    motor_driver_t motors;
    motor_driver_init(&motors);

    /* Init CRSF/ELRS receiver on UART4 (PA0/PA1) with DMA */
    crsf_init();

    /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
    jlink_init();

    /* Init CAN2 BLDC motor controller bus — PB12/PB13, 500 kbps (Issue #597) */
    can_driver_init();

    /* Send fault log summary on boot if a prior fault was recorded (Issue #565) */
    if (fault_get_last_type() != FAULT_NONE) {
        fault_log_entry_t fle;
        memset(&fle, 0, sizeof(fle));
        jlink_tlm_fault_log_t ftlm;
        memset(&ftlm, 0, sizeof(ftlm));
        ftlm.entry_count = fault_log_get_count();
        if (fault_log_read(0u, &fle)) {
            ftlm.fault_type   = fle.fault_type;
            ftlm.reset_count  = fle.reset_count;
            ftlm.timestamp_ms = fle.timestamp_ms;
            ftlm.pc           = fle.pc;
            ftlm.lr           = fle.lr;
            ftlm.cfsr         = fle.cfsr;
            ftlm.hfsr         = fle.hfsr;
        }
        jlink_send_fault_log(&ftlm);
        printf("[FAULT] Prior fault type=%u count=%u PC=0x%08lX\n",
               (unsigned)ftlm.fault_type, (unsigned)ftlm.entry_count,
               (unsigned long)ftlm.pc);
    }

    /* Init Jetson UART command interface on USART6 (PC6/PC7) at 921600 baud.
     * Mirrors CDC command protocol over hardware UART (fixes USB CDC TX bug). */
    jetson_uart_init();

    /* Init I2S3 audio amplifier (PC10/PA15/PB5, mute=PC5) */
    audio_init();
    audio_play_tone(AUDIO_TONE_STARTUP);

    /* Init piezo buzzer driver (TIM4_CH3 PWM on PB2, Issue #189) */
    buzzer_init();
    buzzer_play_melody(MELODY_STARTUP);

    /* Init WS2812B NeoPixel LED ring (TIM3_CH1 PWM on PB4, Issue #193) */
    led_init();
    led_set_state(LED_STATE_BOOT);

    /* Init power management — STOP-mode sleep/wake, wake EXTIs configured */
    power_mgmt_init();

    /* Init servo pan-tilt driver for camera head (TIM4 PWM on PB6/PB7, Issue #206) */
    servo_init();

    /* Init ST3215 serial bus servo driver (USART3 half-duplex PB10, Issue #547) */
    servo_bus_init();

    /* Init pan/tilt gimbal (ST3215 servos, centers both axes on boot) */
    gimbal_t gimbal;
    gimbal_init(&gimbal);

    /* Init HC-SR04 ultrasonic distance sensor (TIM1 input capture on PA1, Issue #243) */
    ultrasonic_init();

    /* Init mode manager (RC/autonomous blend; CH6 mode switch) */
    mode_manager_t mode;
    mode_manager_init(&mode);

    /* Init battery ADC (PC1/ADC3 — Vbat divider 11:1) for CRSF telemetry */
    battery_init();

    /* Init LVC: low voltage cutoff state machine (Issue #613) */
    lvc_init();

    /* Probe I2C1 for optional sensors — skip gracefully if not found */
    int baro_chip = 0;  /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
    mag_type_t mag_type = MAG_NONE;
    if (i2c1_init() == 0) {
        int chip = bmp280_init();
        baro_chip = (chip > 0) ? chip : 0;
        mag_type = mag_init();
        ina219_init();  /* Init INA219 dual motor current monitoring (Issue #214) */
    }

    /*
     * IWDG starts AFTER all peripheral inits — avoids reset during mpu6000_init()
     * which takes ~510ms (well above the 50ms WATCHDOG_TIMEOUT_MS).
     * Once started, safety_refresh() MUST be called every WATCHDOG_TIMEOUT_MS
     * or MCU resets. IWDG cannot be stopped once started (hardware enforced).
     */
    safety_init();

    char buf[320];
    int len;
    uint32_t tx_count = 0;   /* telemetry frames sent to host */

    IMUData imu;
    uint32_t send_tick = 0;
    uint32_t balance_tick = 0;
    uint32_t esc_tick = 0;
    uint32_t crsf_telem_tick = 0;  /* CRSF uplink telemetry TX timer */
    uint32_t jlink_tlm_tick = 0;   /* Jetson binary telemetry TX timer */
    uint32_t pm_tlm_tick    = 0;   /* JLINK_TLM_POWER transmit timer */
    uint32_t can_cmd_tick   = 0;   /* CAN velocity command TX timer (Issue #597) */
    uint32_t can_tlm_tick   = 0;   /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */
    uint32_t lvc_tlm_tick   = 0;   /* JLINK_TLM_LVC transmit timer (Issue #613) */
    uint8_t  pm_pwm_phase   = 0;   /* Software PWM counter for sleep LED */
    const float dt = 1.0f / PID_LOOP_HZ;  /* 1ms at 1kHz */

    while (1) {
        if (imu_ret == 0) mpu6000_read(&imu);

        uint32_t now = HAL_GetTick();

        /* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */
        safety_refresh();

        /* Advance audio tone sequencer (non-blocking, call every tick) */
        audio_tick(now);

        /* Advance buzzer pattern sequencer (non-blocking, call every tick) */
        buzzer_tick(now);

        /* Advance LED animation sequencer (non-blocking, call every tick) */
        led_tick(now);

        /* Fault recovery LED blink code (Issue #565; self-disables after 10 s) */
        fault_led_tick(now);

        /* Servo pan-tilt animation tick — updates smooth sweeps */
        servo_tick(now);

        /* Gimbal ST3215 feedback polling tick (self-throttles to GIMBAL_TLM_HZ) */
        gimbal_tick(&gimbal, now);

        /* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
        battery_accumulate_coulombs();

        /* LVC: update low-voltage protection state machine (Issue #613) */
        lvc_tick(now, battery_read_mv());

        /* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness.
         * pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */
        pm_pwm_phase++;
        {
            uint8_t pm_bright = power_mgmt_led_brightness();
            if (pm_bright > 0u) {
                bool led_on = (pm_pwm_phase < pm_bright);
                HAL_GPIO_WritePin(LED1_PORT, LED1_PIN,
                    led_on ? GPIO_PIN_RESET : GPIO_PIN_SET);
            }
        }

        /* Power manager tick — may block in WFI (STOP mode) when disarmed */
        if (bal.state != BALANCE_ARMED) {
            power_mgmt_tick(now);
        }

        /* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
        mode_manager_update(&mode, now);

        if (cdc_estop_request) {
            EstopSource src = (cdc_estop_request == 1) ? ESTOP_REMOTE : ESTOP_CELLULAR_TIMEOUT;
            cdc_estop_request = 0;
            safety_remote_estop(src);
            safety_arm_cancel();
            balance_disarm(&bal);
            motor_driver_estop(&motors);
        }
        if (cdc_estop_clear_request) {
            cdc_estop_clear_request = 0;
            if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
                safety_remote_estop_clear();
        }

        /* Drain JLink DMA circular buffer and parse frames */
        jlink_process();

        /* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */
        can_driver_process();

        /* Handle JLink one-shot flags from Jetson binary protocol */
        if (jlink_state.estop_req) {
            jlink_state.estop_req = 0u;
            safety_remote_estop(ESTOP_REMOTE);
            safety_arm_cancel();
            balance_disarm(&bal);
            motor_driver_estop(&motors);
        }
        if (jlink_state.arm_req) {
            jlink_state.arm_req = 0u;
            if (!safety_remote_estop_active() &&
                    mpu6000_is_calibrated() &&
                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
                safety_arm_start(now);
            }
        }
        if (jlink_state.disarm_req) {
            jlink_state.disarm_req = 0u;
            safety_arm_cancel();
            balance_disarm(&bal);
        }
        if (jlink_state.pid_updated) {
            jlink_state.pid_updated = 0u;
            bal.kp = jlink_state.pid_kp;
            bal.ki = jlink_state.pid_ki;
            bal.kd = jlink_state.pid_kd;
        }
        if (jlink_state.dfu_req) {
            jlink_state.dfu_req = 0u;
            /* ota_enter_dfu() is a no-op (returns false) when armed;
             * never returns when disarmed — MCU resets into DFU mode. */
            ota_enter_dfu(bal.state == BALANCE_ARMED);
        }
        if (jlink_state.sleep_req) {
            jlink_state.sleep_req = 0u;
            power_mgmt_request_sleep();
        }
        /* GIMBAL_POS: forward new pan/tilt command to gimbal (Issue #547) */
        if (jlink_state.gimbal_updated) {
            jlink_state.gimbal_updated = 0u;
            gimbal_set_pos(&gimbal,
                           (int16_t)jlink_state.gimbal_pan_x10,
                           (int16_t)jlink_state.gimbal_tilt_x10,
                           jlink_state.gimbal_speed);
        }

        /* PID_SAVE: persist current gains to flash, reply with result (Issue #531).
         * Only allowed while disarmed -- flash sector erase stalls CPU for ~1 s. */
        if (jlink_state.pid_save_req && bal.state != BALANCE_ARMED) {
            jlink_state.pid_save_req = 0u;
            bool save_ok = pid_flash_save(bal.kp, bal.ki, bal.kd);
            jlink_tlm_pid_result_t pid_res;
            pid_res.kp       = bal.kp;
            pid_res.ki       = bal.ki;
            pid_res.kd       = bal.kd;
            pid_res.saved_ok = save_ok ? 1u : 0u;
            jlink_send_pid_result(&pid_res);
            printf("[PID] Flash save %s: Kp=%.3f Ki=%.3f Kd=%.3f\n",
                   save_ok ? "OK" : "FAIL",
                   (double)bal.kp, (double)bal.ki, (double)bal.kd);
        }

        /* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
        if (jlink_state.fault_log_req) {
            jlink_state.fault_log_req = 0u;
            fault_log_entry_t fle;
            memset(&fle, 0, sizeof(fle));
            jlink_tlm_fault_log_t ftlm;
            memset(&ftlm, 0, sizeof(ftlm));
            ftlm.entry_count = fault_log_get_count();
            if (fault_log_read(0u, &fle)) {
                ftlm.fault_type   = fle.fault_type;
                ftlm.reset_count  = fle.reset_count;
                ftlm.timestamp_ms = fle.timestamp_ms;
                ftlm.pc           = fle.pc;
                ftlm.lr           = fle.lr;
                ftlm.cfsr         = fle.cfsr;
                ftlm.hfsr         = fle.hfsr;
            }
            jlink_send_fault_log(&ftlm);
        }

        /* CAN_STATS: send telemetry on demand or at CAN_TLM_HZ (1 Hz) (Issue #597) */
        {
            bool send_can = (jlink_state.can_stats_req != 0u);
            if (!send_can && (now - can_tlm_tick >= (1000u / CAN_TLM_HZ))) {
                send_can = true;
            }
            if (send_can) {
                jlink_state.can_stats_req = 0u;
                can_tlm_tick = now;
                can_stats_t cs;
                can_driver_get_stats(&cs);
                can_feedback_t fb0, fb1;
                jlink_tlm_can_stats_t ctlm;
                memset(&ctlm, 0, sizeof(ctlm));
                ctlm.tx_count  = cs.tx_count;
                ctlm.rx_count  = cs.rx_count;
                ctlm.err_count = cs.err_count;
                ctlm.bus_off   = cs.bus_off;
                if (can_driver_get_feedback(CAN_NODE_LEFT, &fb0)) {
                    ctlm.vel0_rpm    = fb0.velocity_rpm;
                    ctlm.node_faults |= (fb0.fault ? 1u : 0u);
                }
                if (can_driver_get_feedback(CAN_NODE_RIGHT, &fb1)) {
                    ctlm.vel1_rpm    = fb1.velocity_rpm;
                    ctlm.node_faults |= (fb1.fault ? 2u : 0u);
                }
                jlink_send_can_stats(&ctlm);
            }
        }

        /* Power management: CRSF/JLink activity or armed state resets idle timer */
        if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) ||
                jlink_is_active(now) ||
                bal.state == BALANCE_ARMED) {
            power_mgmt_activity();
        }

        /* RC CH5 kill switch: emergency stop if RC is alive and CH5 off.
         * Issue #512: RC becomes optional override — kill switch triggers estop,
         * not disarm, so Jetson-armed robots remain armed when RC disconnects.
         * Emergency stop kills motors immediately but allows re-arm. */
        if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
            motor_driver_estop(&motors);
        }
        /* RC failsafe: disarm if signal lost AFTER RC was previously alive.
         * Prevents auto-disarm in USB-only mode (crsf_state.last_rx_ms == 0). */
        if (bal.state == BALANCE_ARMED &&
                crsf_state.last_rx_ms != 0 &&
                (now - crsf_state.last_rx_ms) > CRSF_FAILSAFE_MS) {
            safety_arm_cancel();
            balance_disarm(&bal);
        }

        /* Tilt fault buzzer alert (one-shot on fault edge) */
        safety_alert_tilt_fault(bal.state == BALANCE_TILT_FAULT);

        /* RC arm/disarm via CH5 switch (CRSF) — edge detect with hold interlock.
         * Issue #512: RC becomes optional override. Falling edge only disarms if RC
         * explicitly requested it (CH5 off while RC alive). RC disconnect doesn't
         * disarm Jetson-controlled robots; Jetson timeout disarm (in main loop) handles it. */
        {
            static bool s_rc_armed_prev = false;
            bool rc_armed_now = safety_rc_alive(now) && crsf_state.armed;
            if (rc_armed_now && !s_rc_armed_prev) {
                /* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */
                if (!safety_remote_estop_active() &&
                        mpu6000_is_calibrated() &&
                        bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
                    safety_arm_start(now);
                }
            }
            if (!rc_armed_now && s_rc_armed_prev && safety_rc_alive(now)) {
                /* Falling edge with RC still alive: RC explicitly de-armed.
                 * Cancel pending arm or disarm if already armed. */
                safety_arm_cancel();
                if (bal.state == BALANCE_ARMED) balance_disarm(&bal);
            }
            /* Note: RC disconnect (crsf_state.last_rx_ms == 0 after being alive) is handled
             * by failsafe timer below, NOT by this edge detector. */
            s_rc_armed_prev = rc_armed_now;
        }

        /* Handle arm/disarm requests from USB with arm-hold interlock */
        if (cdc_arm_request) {
            cdc_arm_request = 0;
            if (!safety_remote_estop_active() &&
                    mpu6000_is_calibrated() &&
                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
                safety_arm_start(now);
            }
        }
        /* Complete arming once hold timer expires */
        if (safety_arm_ready(now) && bal.state == BALANCE_DISARMED) {
            safety_arm_cancel();
            balance_arm(&bal);
            audio_play_tone(AUDIO_TONE_ARM);
        }
        if (cdc_disarm_request) {
            cdc_disarm_request = 0;
            safety_arm_cancel();
            balance_disarm(&bal);
        }

        /* Gyro recalibration — disarm first, then re-sample bias (~1s blocked) */
        if (cdc_recal_request) {
            cdc_recal_request = 0;
            safety_arm_cancel();
            balance_disarm(&bal);
            if (imu_ret == 0) mpu6000_calibrate();
        }

        /* Handle PID tuning commands from USB (P/I/D/T/M/?) */
        if (cdc_cmd_ready) {
            cdc_cmd_ready = 0;
            char reply[96];
            apply_pid_cmd(&bal, (const char *)cdc_cmd_buf, reply, sizeof(reply));
            CDC_Transmit((uint8_t *)reply, strlen(reply));
        }

        /* Handle Jetson C<speed>,<steer> command (parsed here, not in ISR) */
        if (jetson_cmd_ready) {
            jetson_cmd_ready = 0;
            jetson_cmd_process();
        }

        /*
         * Balance PID — 1kHz with MPU6000/SPI, ~250Hz with BNO055/I2C.
         * BNO055 read (~3ms) is placed inside the gate so it doesn't block
         * the full main loop; the gap between ticks is naturally longer.
         * jlink takes priority for Jetson speed offset; falls back to CDC.
         */
        if (imu_ret == 0 && now - balance_tick >= 1) {
            if (bno055_active) bno055_read(&imu);  /* I2C read inside gate */
            balance_tick = now;
            float base_sp = bal.setpoint;
            if (jlink_is_active(now))
                bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
            else if (jetson_cmd_is_active(now))
                bal.setpoint += jetson_cmd_sp_offset();
            balance_update(&bal, &imu, dt);
            bal.setpoint = base_sp;
        }

        /* Latch estop on tilt fault or disarm */
        {
            static uint8_t s_prev_tilt_fault = 0;
            uint8_t tilt_now = (bal.state == BALANCE_TILT_FAULT) ? 1u : 0u;
            if (tilt_now && !s_prev_tilt_fault)
                audio_play_tone(AUDIO_TONE_FAULT);
            s_prev_tilt_fault = tilt_now;
        }
        if (bal.state == BALANCE_TILT_FAULT) {
            motor_driver_estop(&motors);
        } else if (bal.state == BALANCE_DISARMED && motors.estop &&
                   !safety_remote_estop_active()) {
            motor_driver_estop_clear(&motors);
        }

        /* LVC cutoff: disarm and estop on undervoltage latch (Issue #613) */
        if (lvc_is_cutoff() && bal.state == BALANCE_ARMED) {
            safety_arm_cancel();
            balance_disarm(&bal);
            motor_driver_estop(&motors);
        }

        /* Feed autonomous steer from Jetson into mode manager.
         * jlink takes priority over legacy CDC jetson_cmd.
         * mode_manager_get_steer() blends it with RC steer per active mode. */
        if (jlink_is_active(now))
            mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0);
        else if (jetson_cmd_is_active(now))
            mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);

        /* Send to hoverboard ESC at 50Hz (every 20ms) */
        if (now - esc_tick >= 20) {
            esc_tick = now;
            if (bal.state == BALANCE_ARMED) {
                /* Blended steer (RC ↔ auto per mode) + RC speed bias */
                int16_t steer      = mode_manager_get_steer(&mode);
                int16_t spd_bias   = mode_manager_get_speed_bias(&mode);
                int32_t speed      = (int32_t)bal.motor_cmd + spd_bias;
                /* LVC power scaling: 100% normal, 50% critical, 0% cutoff (Issue #613) */
                uint8_t lvc_scale = lvc_get_power_scale();
                if (lvc_scale < 100u) {
                    speed = (speed * (int32_t)lvc_scale) / 100;
                }
                if (speed >  1000) speed =  1000;
                if (speed < -1000) speed = -1000;
                motor_driver_update(&motors, (int16_t)speed, steer, now);
            } else if (direct_test_speed != 0 || direct_test_steer != 0) {
                /* Direct motor test mode — set by W command */
                esc_send(direct_test_speed, direct_test_steer);
            } else {
                /* Always send zero while disarmed to prevent ESC timeout */
                motor_driver_update(&motors, 0, 0, now);
            }
        }

        /* CAN BLDC: enable/disable follows arm state (Issue #597) */
        {
            static bool s_can_enabled = false;
            bool armed_now = (bal.state == BALANCE_ARMED);
            if (armed_now && !s_can_enabled) {
                can_driver_send_enable(CAN_NODE_LEFT,  true);
                can_driver_send_enable(CAN_NODE_RIGHT, true);
                s_can_enabled = true;
            } else if (!armed_now && s_can_enabled) {
                can_driver_send_enable(CAN_NODE_LEFT,  false);
                can_driver_send_enable(CAN_NODE_RIGHT, false);
                s_can_enabled = false;
            }
        }

        /* CAN BLDC: send velocity/torque commands at CAN_TX_RATE_HZ (100 Hz) (Issue #597)
         * Converts balance PID output + blended steer to per-wheel RPM.
         * Left  wheel: speed_rpm + steer_rpm (differential drive)
         * Right wheel: speed_rpm - steer_rpm */
        if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) {
            can_cmd_tick = now;
            int16_t can_steer = mode_manager_get_steer(&mode);
            int32_t speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
            int32_t steer_rpm = (int32_t)can_steer    * CAN_RPM_SCALE / 2;
            can_cmd_t cmd_l = { (int16_t)(speed_rpm + steer_rpm), 0 };
            can_cmd_t cmd_r = { (int16_t)(speed_rpm - steer_rpm), 0 };
            if (bal.state != BALANCE_ARMED) {
                cmd_l.velocity_rpm = 0;
                cmd_r.velocity_rpm = 0;
            }
            can_driver_send_cmd(CAN_NODE_LEFT,  &cmd_l);
            can_driver_send_cmd(CAN_NODE_RIGHT, &cmd_r);
        }

        /* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.
         * Sends battery sensor frame (0x08) and flight mode frame (0x21)
         * back to ELRS TX module so the pilot's handset OSD shows Vbat + state. */
        if (now - crsf_telem_tick >= (1000u / CRSF_TELEMETRY_HZ)) {
            crsf_telem_tick = now;
            uint32_t vbat_mv  = battery_read_mv();
            /* Use coulomb-based SoC if available, fallback to voltage-based */
            uint8_t soc_pct = battery_get_soc_coulomb();
            if (soc_pct == 255) {
                soc_pct = battery_estimate_pct(vbat_mv);
            }
            crsf_send_battery(vbat_mv, coulomb_counter_get_remaining_mah(), soc_pct);
            crsf_send_flight_mode(bal.state == BALANCE_ARMED);
        }

        /* JLink STATUS telemetry at JLINK_TLM_HZ (50Hz) back to Jetson */
        if (now - jlink_tlm_tick >= (1000u / JLINK_TLM_HZ)) {
            jlink_tlm_tick = now;
            jlink_tlm_status_t tlm;
            tlm.pitch_x10    = (int16_t)(bal.pitch_deg * 10.0f);
            tlm.roll_x10     = (int16_t)(imu.roll * 10.0f);
            tlm.yaw_x10      = (int16_t)(imu.yaw  * 10.0f);
            tlm.motor_cmd    = bal.motor_cmd;
            uint32_t vbat    = battery_read_mv();
            tlm.vbat_mv      = (vbat > 65535u) ? 65535u : (uint16_t)vbat;
            tlm.rssi_dbm     = crsf_state.rssi_dbm;
            tlm.link_quality = crsf_state.link_quality;
            tlm.balance_state = (uint8_t)bal.state;
            tlm.rc_armed     = crsf_state.armed ? 1u : 0u;
            tlm.mode         = (uint8_t)mode_manager_active(&mode);
            EstopSource _es  = safety_get_estop();
            tlm.estop        = (uint8_t)_es;
            /* Use coulomb-based SoC if available, fallback to voltage-based */
            uint8_t soc = battery_get_soc_coulomb();
            tlm.soc_pct = (soc == 255) ? battery_estimate_pct(vbat) : soc;
            tlm.fw_major     = FW_MAJOR;
            tlm.fw_minor     = FW_MINOR;
            tlm.fw_patch     = FW_PATCH;
            jlink_send_telemetry(&tlm);

            /* Send gimbal state at same 50 Hz cadence (Issue #547) */
            jlink_tlm_gimbal_state_t gtlm;
            gtlm.pan_x10        = gimbal.fb_pan_x10;
            gtlm.tilt_x10       = gimbal.fb_tilt_x10;
            gtlm.pan_speed_raw  = gimbal.fb_pan_speed;
            gtlm.tilt_speed_raw = gimbal.fb_tilt_speed;
            gtlm.torque_en      = gimbal.torque_enabled ? 1u : 0u;
            uint32_t rx_total   = gimbal.rx_ok + gimbal.rx_err;
            gtlm.rx_err_pct     = (rx_total > 0u)
                                      ? (uint8_t)(gimbal.rx_err * 100u / rx_total)
                                      : 0u;
            jlink_send_gimbal_state(&gtlm);
        }

        /* JLINK_TLM_POWER telemetry at PM_TLM_HZ (1 Hz) */
        if (now - pm_tlm_tick >= (1000u / PM_TLM_HZ)) {
            pm_tlm_tick = now;
            jlink_tlm_power_t pow;
            pow.power_state  = (uint8_t)power_mgmt_state();
            pow.est_total_ma = power_mgmt_current_ma();
            pow.est_audio_ma = (uint16_t)(power_mgmt_state() == PM_SLEEPING ? 0u : PM_CURRENT_AUDIO_MA);
            pow.est_osd_ma   = (uint16_t)(power_mgmt_state() == PM_SLEEPING ? 0u : PM_CURRENT_OSD_MA);
            pow.idle_ms      = power_mgmt_idle_ms();
            jlink_send_power_telemetry(&pow);
        }

        /* JLINK_TLM_LVC telemetry at LVC_TLM_HZ (1 Hz) -- battery voltage + protection state (Issue #613) */
        if (now - lvc_tlm_tick >= (1000u / LVC_TLM_HZ)) {
            lvc_tlm_tick = now;
            uint32_t lvc_vbat = battery_read_mv();
            jlink_tlm_lvc_t ltlm;
            ltlm.voltage_mv       = (lvc_vbat > 65535u) ? 65535u : (uint16_t)lvc_vbat;
            ltlm.protection_state = (uint8_t)lvc_get_state();
            if (lvc_vbat == 0u) {
                ltlm.percent = 255u;
            } else if (lvc_vbat <= LVC_CUTOFF_MV) {
                ltlm.percent = 0u;
            } else if (lvc_vbat >= LVC_WARNING_MV) {
                ltlm.percent = 100u;
            } else {
                ltlm.percent = (uint8_t)(((lvc_vbat - LVC_CUTOFF_MV) * 100u) /
                                         (LVC_WARNING_MV - LVC_CUTOFF_MV));
            }
            jlink_send_lvc_tlm(&ltlm);
        }

        /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
        if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
            send_tick = now;
            if (imu_ret == 0) {
                float err = bal.setpoint - bal.pitch_deg;
                /* Build JSON incrementally; append optional sensor fields */
                char *p = buf;
                int rem = (int)sizeof(buf);
                int n;
                n = snprintf(p, rem,
                    "{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d,\"y\":%d",
                    (int)(bal.pitch_deg * 10),   /* pitch degrees x10 */
                    (int)(imu.roll * 10),        /* roll degrees x10 */
                    (int)(err * 10),             /* PID error x10 */
                    (int)(bal.integral * 10),    /* integral x10 (windup monitor) */
                    (int)bal.motor_cmd,          /* ESC command -1000..+1000 */
                    (int)bal.state,
                    (int)(imu.yaw * 10));        /* yaw degrees x10 (gyro-integrated) */
                p += n; rem -= n;
                n = snprintf(p, rem, ",\"md\":%d",
                    (int)mode_manager_active(&mode));  /* 0=MANUAL,1=ASSISTED,2=AUTO */
                p += n; rem -= n;
                if (mag_type != MAG_NONE) {
                    int16_t hd = mag_read_heading();
                    if (hd >= 0)
                        n = snprintf(p, rem, ",\"hd\":%d", hd); /* heading deg×10 */
                    else
                        n = snprintf(p, rem, ",\"hd\":-1");      /* not ready */
                    p += n; rem -= n;
                }
                if (baro_chip) {
                    int32_t pres_pa; int16_t temp_x10;
                    bmp280_read(&pres_pa, &temp_x10);
                    int32_t alt_cm = bmp280_pressure_to_alt_cm(pres_pa);
                    /* alt cm, temp °C×10, pressure hPa×10 (Pa÷10 = hPa×10) */
                    n = snprintf(p, rem, ",\"alt\":%ld,\"t\":%d,\"pa\":%ld",
                        (long)alt_cm, (int)temp_x10, (long)(pres_pa / 10));
                    p += n; rem -= n;
                    if (baro_chip == 0x60) {  /* BME280: add humidity %RH×10 */
                        int16_t hum_x10 = bmp280_read_humidity();
                        if (hum_x10 >= 0) {
                            n = snprintf(p, rem, ",\"h\":%d", (int)hum_x10);
                            p += n; rem -= n;
                        }
                    }
                }
                if (safety_rc_alive(now)) {
                    /* RSSI, link quality, and CH1–CH4 mapped to µs (1000–2000) */
                    n = snprintf(p, rem, ",\"rssi\":%d,\"lq\":%d"
                        ",\"ch1\":%d,\"ch2\":%d,\"ch3\":%d,\"ch4\":%d",
                        (int)crsf_state.rssi_dbm, (int)crsf_state.link_quality,
                        (int)crsf_to_range(crsf_state.channels[0], 1000, 2000),
                        (int)crsf_to_range(crsf_state.channels[1], 1000, 2000),
                        (int)crsf_to_range(crsf_state.channels[2], 1000, 2000),
                        (int)crsf_to_range(crsf_state.channels[3], 1000, 2000));
                    p += n; rem -= n;
                }
                /* Jetson active flag, USB TX/RX packet counters */
                int es; EstopSource _rs = safety_get_estop();
                if (_rs >= ESTOP_REMOTE) es = (int)_rs;
                else if (bal.state == BALANCE_TILT_FAULT) es = ESTOP_TILT;
                else es = ESTOP_CLEAR;
                /* BNO055 calibration status and temperature when active */
                if (bno055_active) {
                    uint8_t cs = bno055_calib_status();
                    n = snprintf(p, rem, ",\"bno_cs\":%d,\"bno_t\":%d",
                        (int)cs, (int)bno055_temperature());
                    p += n; rem -= n;
                }
                n = snprintf(p, rem, ",\"ja\":%d,\"jl\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n",
                    jetson_cmd_is_active(now) ? 1 : 0,
                    jlink_is_active(now) ? 1 : 0,
                    (unsigned)tx_count,
                    (unsigned)cdc_rx_count, es);
                len = (int)(p + n - buf);
            } else {
                len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret);
            }
            CDC_Transmit((uint8_t *)buf, len);
            tx_count++;
        }

        status_update(now, (imu_ret == 0),
                      (bal.state == BALANCE_ARMED),
                      (bal.state == BALANCE_TILT_FAULT),
                      safety_remote_estop_active());
        HAL_Delay(1);
    }
}