#pragma once
/* orin_serial.h — Orin↔ESP32-S3 BALANCE USB/UART serial protocol (bd-66hx)
 *
 * Frame layout (matches bd-wim1 esp32_balance_protocol.py exactly):
 *   [0xAA][LEN][TYPE][PAYLOAD × LEN bytes][CRC8-SMBUS]
 *   CRC covers LEN + TYPE + PAYLOAD bytes.
 *   All multi-byte payload fields are big-endian.
 *
 * Physical: UART0 → CH343 USB-serial → Orin /dev/esp32-balance  @ 460800 baud
 */

#include <stdint.h>
#include <stdbool.h>
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"

/* ── Frame constants ── */
#define ORIN_SYNC           0xAAu
#define ORIN_MAX_PAYLOAD    62u

/* ── Command types: Orin → ESP32 ── */
#define CMD_HEARTBEAT       0x01u
#define CMD_DRIVE           0x02u   /* int16 speed + int16 steer, BE */
#define CMD_ESTOP           0x03u   /* uint8: 1=assert, 0=clear */
#define CMD_ARM             0x04u   /* uint8: 1=arm, 0=disarm */
#define CMD_PID             0x05u   /* float32 kp, ki, kd, BE */

/* ── Telemetry types: ESP32 → Orin ── */
#define TELEM_STATUS        0x80u   /* status @ 10 Hz */
#define TELEM_VESC_LEFT     0x81u   /* VESC ID 56 telemetry @ 10 Hz */
#define TELEM_VESC_RIGHT    0x82u   /* VESC ID 68 telemetry @ 10 Hz */
#define RESP_ACK            0xA0u
#define RESP_NACK           0xA1u

/* ── NACK error codes ── */
#define ERR_BAD_CRC         0x01u
#define ERR_BAD_LEN         0x02u
#define ERR_ESTOP_ACTIVE    0x03u
#define ERR_DISARMED        0x04u

/* ── Balance state (mirrored from TELEM_STATUS.balance_state) ── */
typedef enum {
    BAL_DISARMED   = 0,
    BAL_ARMED      = 1,
    BAL_TILT_FAULT = 2,
    BAL_ESTOP      = 3,
} bal_state_t;

/* ── Shared state written by RX task, consumed by main/vesc tasks ── */
typedef struct {
    volatile int16_t  speed;        /* -1000..+1000 */
    volatile int16_t  steer;        /* -1000..+1000 */
    volatile uint32_t updated_ms;   /* esp_timer tick at last CMD_DRIVE */
} orin_drive_t;

typedef struct {
    volatile float    kp, ki, kd;
    volatile bool     updated;
} orin_pid_t;

typedef struct {
    volatile bool     armed;
    volatile bool     estop;
    volatile uint32_t hb_last_ms;   /* esp_timer tick at last CMD_HEARTBEAT/CMD_DRIVE */
} orin_control_t;

/* ── TX frame queue item ── */
typedef struct {
    uint8_t type;
    uint8_t len;
    uint8_t payload[ORIN_MAX_PAYLOAD];
} orin_tx_frame_t;

/* ── Globals (defined in orin_serial.c, extern here) ── */
extern orin_drive_t   g_orin_drive;
extern orin_pid_t     g_orin_pid;
extern orin_control_t g_orin_ctrl;

/* ── API ── */
void orin_serial_init(void);

/* Tasks — pass tx_queue as arg to both */
void orin_serial_rx_task(void *arg);   /* arg = QueueHandle_t tx_queue */
void orin_serial_tx_task(void *arg);   /* arg = QueueHandle_t tx_queue */

/* Enqueue outbound frames */
void orin_send_status(QueueHandle_t q,
                      int16_t pitch_x10, int16_t motor_cmd,
                      uint16_t vbat_mv, bal_state_t state, uint8_t flags);
void orin_send_vesc(QueueHandle_t q, uint8_t telem_type,
                    int32_t erpm, uint16_t voltage_mv,
                    int16_t current_ma, uint16_t temp_c_x10);
void orin_send_ack(QueueHandle_t q, uint8_t cmd_type);
void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err);