/*
 * jetson_uart.c — USART6 command interface for Jetson Orin
 *
 * Mirrors the USB CDC command protocol over UART so the Jetson can
 * arm, disarm, heartbeat, drive, and e-stop via hardware UART.
 * This fixes the CDC TX bug (Issue USB_CDC_BUG.md) by providing
 * a reliable non-USB path.
 *
 * USART6: PC6=TX, PC7=RX @ 921600 baud (matches Orin ttyTHS1)
 *
 * Command protocol (same as CDC):
 *   A        — arm request
 *   D        — disarm request
 *   E        — emergency stop
 *   Z        — clear e-stop
 *   H        — heartbeat (refresh timeout)
 *   C<s>,<t> — drive command: speed,steer (also refreshes heartbeat)
 *   G        — gyro recalibration
 */

#include "jetson_uart.h"
#include "jetson_cmd.h"
#include "config.h"
#include "safety.h"
#include "mpu6000.h"
#include "stm32f7xx_hal.h"
#include <stdio.h>

/* Shared flags — same ones CDC sets, main loop consumes */
extern volatile uint8_t  cdc_arm_request;
extern volatile uint8_t  cdc_disarm_request;
extern volatile uint8_t  cdc_recal_request;
extern volatile uint8_t  cdc_estop_request;
extern volatile uint8_t  cdc_estop_clear_request;
extern volatile uint8_t  cdc_streaming;
extern volatile char     cdc_cmd_buf[32];
extern volatile uint8_t  cdc_cmd_ready;

/* From jetson_cmd.h */
extern volatile uint8_t  jetson_cmd_ready;
extern volatile char     jetson_cmd_buf[32];
extern volatile uint32_t jetson_hb_tick;

static UART_HandleTypeDef huart6;
static uint8_t rx_byte;
static char line_buf[64];
static uint8_t line_idx = 0;

static void process_line(const char *buf, uint8_t len);

/* Keep FC awake on Jetson activity */
extern void power_mgmt_activity(void);

void jetson_uart_init(void) {
    __HAL_RCC_USART6_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();

    /* PC6=TX, PC7=RX, AF8 for USART6 */
    GPIO_InitTypeDef gpio = {0};
    gpio.Pin = GPIO_PIN_6 | GPIO_PIN_7;
    gpio.Mode = GPIO_MODE_AF_PP;
    gpio.Pull = GPIO_PULLUP;
    gpio.Speed = GPIO_SPEED_FREQ_HIGH;
    gpio.Alternate = GPIO_AF8_USART6;
    HAL_GPIO_Init(GPIOC, &gpio);

    huart6.Instance = USART6;
    huart6.Init.BaudRate = 921600;
    huart6.Init.WordLength = UART_WORDLENGTH_8B;
    huart6.Init.StopBits = UART_STOPBITS_1;
    huart6.Init.Parity = UART_PARITY_NONE;
    huart6.Init.Mode = UART_MODE_TX_RX;
    huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart6.Init.OverSampling = UART_OVERSAMPLING_16;
    HAL_UART_Init(&huart6);

    HAL_NVIC_SetPriority(USART6_IRQn, 4, 0);
    HAL_NVIC_EnableIRQ(USART6_IRQn);

    /* Start interrupt-driven receive */
    HAL_UART_Receive_IT(&huart6, &rx_byte, 1);

    /* Boot banner — confirms USART6 is alive */
    const char *banner = "SALTYLAB USART6 OK\n";
    HAL_UART_Transmit(&huart6, (const uint8_t *)banner, 19, 50);
}

/* Send telemetry/status back to Jetson */
void jetson_uart_send(const uint8_t *data, uint16_t len) {
    HAL_UART_Transmit(&huart6, (uint8_t *)data, len, 10);
}

/* ISR callback — accumulate bytes into line buffer, process on \n */
void jetson_uart_rx_callback(UART_HandleTypeDef *huart) {
    if (huart != &huart6) return;

    /* Any UART activity keeps the FC awake (prevents STOP mode) */
    power_mgmt_activity();

    if (rx_byte == '\n' || rx_byte == '\r') {
        if (line_idx > 0) {
            line_buf[line_idx] = '\0';
            process_line(line_buf, line_idx);
            line_idx = 0;
        }
    } else if (line_idx < sizeof(line_buf) - 1) {
        line_buf[line_idx++] = (char)rx_byte;
    } else {
        line_idx = 0;  /* overflow — reset */
    }

    /* Re-arm receive */
    HAL_UART_Receive_IT(&huart6, &rx_byte, 1);
}

/*
 * Process a complete line. Same command set as CDC_Receive.
 * Single-char commands work without newline too (for compatibility).
 */
static void process_line(const char *buf, uint8_t len) {
    if (len < 1) return;

    switch (buf[0]) {
        case 'A': cdc_arm_request = 1;            
            /* Send arm attempt status for debugging */
            {
                extern bool mpu6000_is_calibrated(void);
                extern float bal_pitch_deg_get(void);  /* we'll add this */
                char dbg[80];
                int n = snprintf(dbg, sizeof(dbg), "ARM_REQ cal=%d estop=%d\n",
                    mpu6000_is_calibrated() ? 1 : 0,
                    safety_remote_estop_active() ? 1 : 0);
                HAL_UART_Transmit(&huart6, (uint8_t*)dbg, n, 10);
            }
            break;
        case 'D': cdc_disarm_request = 1;         break;
        case 'G': cdc_recal_request = 1;          break;
        case 'E': cdc_estop_request = 1;          break;
        case 'F': cdc_estop_request = 2;          break;
        case 'Z': cdc_estop_clear_request = 1;    break;
        case 'S': cdc_streaming = !cdc_streaming;  break;

        case 'H':
            jetson_hb_tick = HAL_GetTick();
            break;

        case 'C': {
            uint8_t copy_len = len < 31 ? len : 31;
            for (uint8_t i = 0; i < copy_len; i++)
                jetson_cmd_buf[i] = buf[i];
            jetson_cmd_buf[copy_len] = '\0';
            jetson_hb_tick = HAL_GetTick();
            jetson_cmd_ready = 1;
            break;
        }

        /* Direct motor test: W<speed>,<steer> — bypasses balance PID,
         * sends directly to ESC. For bench testing and diagnostics only.
         * Does NOT require arming. ESC watchdog stops motors if commands stop. */
        case 'W': {
            int spd = 0, str = 0;
            if (len > 1 && sscanf(buf + 1, "%d,%d", &spd, &str) >= 1) {
                /* Clamp to safe bench test range */
                if (spd > 100) spd = 100;
                if (spd < -100) spd = -100;
                if (str > 100) str = 100;
                if (str < -100) str = -100;
                /* Set persistent direct test globals — main loop sends at 50Hz */
                extern volatile int16_t direct_test_speed;
                extern volatile int16_t direct_test_steer;
                direct_test_speed = (int16_t)spd;
                direct_test_steer = (int16_t)str;
                char ack[32];
                int n = snprintf(ack, sizeof(ack), "W:%d,%d\n", spd, str);
                HAL_UART_Transmit(&huart6, (uint8_t*)ack, n, 10);
            }
            break;
        }

#ifdef DEBUG_MOTOR_TEST
        /* GPIO test: reconfigure PC12 as plain GPIO output, set HIGH for 10s, then restore UART */
        case 'X': {
            GPIO_InitTypeDef gpio = {0};
            gpio.Pin = GPIO_PIN_12;
            gpio.Mode = GPIO_MODE_OUTPUT_PP;
            gpio.Pull = GPIO_NOPULL;
            gpio.Speed = GPIO_SPEED_FREQ_HIGH;
            HAL_GPIO_Init(GPIOC, &gpio);
            HAL_GPIO_WritePin(GPIOC, GPIO_PIN_12, GPIO_PIN_SET);
            char msg[] = "PC12=HIGH 10s, measure T5 pad\n";
            HAL_UART_Transmit(&huart6, (uint8_t*)msg, sizeof(msg)-1, 10);
            HAL_Delay(10000);
            /* Restore to UART AF */
            gpio.Mode = GPIO_MODE_AF_PP;
            gpio.Pull = GPIO_PULLUP;
            gpio.Alternate = GPIO_AF8_UART5;
            HAL_GPIO_Init(GPIOC, &gpio);
            char done[] = "PC12 restored to UART5\n";
            HAL_UART_Transmit(&huart6, (uint8_t*)done, sizeof(done)-1, 10);
            break;
        }

        /* Baud rate sweep test: try multiple baud rates on UART5, 3s each */
        case 'R': {
            extern UART_HandleTypeDef huart2;
            uint16_t start = 0xABCD;
            int16_t steer = 0, speed = 50;
            uint16_t checksum = start ^ (uint16_t)steer ^ (uint16_t)speed;
            uint8_t pkt[8];
            pkt[0] = start & 0xFF; pkt[1] = (start >> 8) & 0xFF;
            pkt[2] = steer & 0xFF; pkt[3] = (steer >> 8) & 0xFF;
            pkt[4] = speed & 0xFF; pkt[5] = (speed >> 8) & 0xFF;
            pkt[6] = checksum & 0xFF; pkt[7] = (checksum >> 8) & 0xFF;

            /* Also report current BRR and APB1 freq */
            uint32_t brr = UART5->BRR;
            uint32_t apb1 = HAL_RCC_GetPCLK1Freq();
            char info[80];
            int n = snprintf(info, sizeof(info), "BRR=%lu APB1=%lu\n",
                (unsigned long)brr, (unsigned long)apb1);
            HAL_UART_Transmit(&huart6, (uint8_t*)info, n, 20);

            const uint32_t bauds[] = {115200, 38400, 9600, 19200, 57600, 230400};
            for (int b = 0; b < 6; b++) {
                /* Reconfigure baud */
                huart2.Init.BaudRate = bauds[b];
                HAL_UART_Init(&huart2);

                char msg[40];
                int mn = snprintf(msg, sizeof(msg), "BAUD %lu...\n", (unsigned long)bauds[b]);
                HAL_UART_Transmit(&huart6, (uint8_t*)msg, mn, 20);

                /* Send for 3 sec */
                for (int i = 0; i < 150; i++) {
                    HAL_UART_Transmit(&huart2, pkt, 8, 5);
                    HAL_Delay(20);
                }
                /* Brief stop between bauds */
                HAL_Delay(500);
            }

            /* Stop motors, restore 115200 */
            speed = 0; checksum = start;
            pkt[4] = 0; pkt[5] = 0;
            pkt[6] = checksum & 0xFF; pkt[7] = (checksum >> 8) & 0xFF;
            huart2.Init.BaudRate = 115200;
            HAL_UART_Init(&huart2);
            for (int i = 0; i < 50; i++) {
                HAL_UART_Transmit(&huart2, pkt, 8, 5);
                HAL_Delay(20);
            }
            char ack[] = "R:done\n";
            HAL_UART_Transmit(&huart6, (uint8_t*)ack, 7, 10);
            break;
        }
#endif /* DEBUG_MOTOR_TEST */

        case '?': {
            /* Status dump for debugging */
            char st[128];
            int n = snprintf(st, sizeof(st),
                "ST cal=%d estop=%d pitch=? hb=%lu\n",
                mpu6000_is_calibrated() ? 1 : 0,
                safety_remote_estop_active() ? 1 : 0,
                (unsigned long)jetson_hb_tick);
            HAL_UART_Transmit(&huart6, (uint8_t*)st, n, 20);
            break;
        }

        /* PID tuning commands */
        case 'P': case 'I': case 'K': case 'T': case 'M': {
            uint8_t copy_len = len < 31 ? len : 31;
            for (uint8_t i = 0; i < copy_len; i++)
                cdc_cmd_buf[i] = buf[i];
            cdc_cmd_buf[copy_len] = '\0';
            cdc_cmd_ready = 1;
            break;
        }

        default: break;
    }
}

/* IRQ handler — call from stm32 interrupt vector */
void USART6_IRQHandler(void) {
    HAL_UART_IRQHandler(&huart6);
}