#include "esc_backend.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#ifdef DEBUG_MOTOR_TEST
#include <stdio.h>
#endif

/*
 * Hoverboard ESC Backend Implementation
 *
 * Adapts the Hoverboard EFeru FOC protocol to the ESC backend vtable.
 * UART2: PA2=TX, PA3=RX @ 115200 baud
 *
 * Packet: [0xABCD] [steer:i16] [speed:i16] [checksum:u16]
 * Checksum = start ^ steer ^ speed
 * Speed range: -1000 to +1000
 * Must send at >=50Hz or ESC times out.
 */

#define HOVERBOARD_START_FRAME  0xABCD
#define HOVERBOARD_BAUD         115200

typedef struct __attribute__((packed)) {
    uint16_t start;
    int16_t  steer;
    int16_t  speed;
    uint16_t checksum;
} hoverboard_cmd_t;

#ifdef DEBUG_MOTOR_TEST
UART_HandleTypeDef huart2;  /* non-static: exposed for jetson_uart.c R command */
#else
static UART_HandleTypeDef huart2;
#endif

/* Backend vtable instance */
static const esc_backend_t hoverboard_backend;

/*
 * Initialize UART2 for hoverboard communication.
 * Called once at startup via backend registration.
 */
static void hoverboard_backend_init(void) {
    /* Enable clocks */
    __HAL_RCC_UART5_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();

    /* PA2=TX, PA3=RX, AF7 for USART2 */
    GPIO_InitTypeDef gpio = {0};
    // UART5: PC12=TX, PD2=RX
    __HAL_RCC_GPIOD_CLK_ENABLE();
    gpio.Pin = GPIO_PIN_12;
    gpio.Mode = GPIO_MODE_AF_PP;
    gpio.Pull = GPIO_PULLUP;
    gpio.Speed = GPIO_SPEED_FREQ_HIGH;
    gpio.Alternate = GPIO_AF8_UART5;
    HAL_GPIO_Init(GPIOC, &gpio);

    // RX: PD2
    gpio.Pin = GPIO_PIN_2;
    gpio.Alternate = GPIO_AF8_UART5;
    HAL_GPIO_Init(GPIOD, &gpio);

    /* USART2 config */
    huart2.Instance = UART5;
    huart2.Init.BaudRate = HOVERBOARD_BAUD;
    huart2.Init.WordLength = UART_WORDLENGTH_8B;
    huart2.Init.StopBits = UART_STOPBITS_1;
    huart2.Init.Parity = UART_PARITY_NONE;
    huart2.Init.Mode = UART_MODE_TX_RX;
    huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart2.Init.OverSampling = UART_OVERSAMPLING_16;
    HAL_UART_Init(&huart2);

#ifdef DEBUG_MOTOR_TEST
    /* Diagnostic: report UART5 register state on USART6 after init */
    {
        extern void jetson_uart_send(const uint8_t *data, uint16_t len);
        char diag[128];
        uint32_t brr = UART5->BRR;
        uint32_t cr1 = UART5->CR1;
        uint32_t isr = UART5->ISR;
        uint32_t apb1 = HAL_RCC_GetPCLK1Freq();
        /* Also read GPIOC MODER to verify PC12 is in AF mode (bits 25:24 = 10) */
        uint32_t moder = GPIOC->MODER;
        uint32_t pc12_mode = (moder >> 24) & 0x3;  /* 0=input 1=output 2=AF 3=analog */
        uint32_t afr = GPIOC->AFR[1]; /* AFR high for pins 8-15 */
        uint32_t pc12_af = (afr >> 16) & 0xF; /* AF for pin 12 */
        int n = snprintf(diag, sizeof(diag),
            "UART5: BRR=%lu CR1=0x%lX ISR=0x%lX APB1=%luHz PC12mode=%lu AF=%lu\n",
            (unsigned long)brr, (unsigned long)cr1, (unsigned long)isr,
            (unsigned long)apb1, (unsigned long)pc12_mode, (unsigned long)pc12_af);
        /* Delay to let USART6 finish boot banner first */
        HAL_Delay(100);
        jetson_uart_send((uint8_t*)diag, n);
    }
#endif /* DEBUG_MOTOR_TEST */
}

/*
 * Send motor command via hoverboard protocol.
 * Called at ~50Hz from motor_driver_update().
 */
#ifdef DEBUG_MOTOR_TEST
static volatile uint32_t hover_tx_count = 0;
#endif

static void hoverboard_backend_send(int16_t speed, int16_t steer) {
    hoverboard_cmd_t cmd;
    cmd.start = HOVERBOARD_START_FRAME;
    cmd.steer = steer;
    cmd.speed = speed;
    cmd.checksum = cmd.start ^ cmd.steer ^ cmd.speed;

#ifdef DEBUG_MOTOR_TEST
    HAL_StatusTypeDef rc = HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
    hover_tx_count++;

    /* Debug: every 50th send, report status on USART6 */
    if (hover_tx_count % 50 == 1) {
        extern void jetson_uart_send(const uint8_t *data, uint16_t len);
        char dbg[64];
        int n = snprintf(dbg, sizeof(dbg), "ESC tx=%lu rc=%d spd=%d str=%d\n",
            (unsigned long)hover_tx_count, (int)rc, speed, steer);
        jetson_uart_send((uint8_t*)dbg, n);
    }
#else
    HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
#endif
}

/*
 * Emergency stop: send zero and disable motors.
 * Hoverboard will disable outputs on repeated zero packets.
 */
static void hoverboard_backend_estop(void) {
    hoverboard_backend_send(0, 0);
}

/*
 * Resume after estop (optional, hoverboard auto-resumes on non-zero command).
 */
static void hoverboard_backend_resume(void) {
    /* No action needed — next non-zero send will resume */
}

/*
 * Query telemetry from hoverboard.
 * Hoverboard protocol is command-only (no RX in this implementation).
 * Return zero telemetry stub; future: add RX feedback.
 */
static void hoverboard_backend_get_telemetry(esc_telemetry_t *out) {
    if (out) {
        out->speed = 0;
        out->steer = 0;
        out->voltage_mv = 0;
        out->current_ma = 0;
        out->temperature_c = 0;
        out->fault = 0;
    }
}

/* Hoverboard backend vtable */
static const esc_backend_t hoverboard_backend = {
    .init = hoverboard_backend_init,
    .send = hoverboard_backend_send,
    .estop = hoverboard_backend_estop,
    .resume = hoverboard_backend_resume,
    .get_telemetry = hoverboard_backend_get_telemetry,
};

/*
 * Public functions for backward compatibility.
 * These remain for existing code that calls hoverboard_init/hoverboard_send directly.
 */

void hoverboard_init(void) {
    esc_backend_register(&hoverboard_backend);
}

void hoverboard_send(int16_t speed, int16_t steer) {
    esc_send(speed, steer);
}