#include "esc_backend.h"
#include "config.h"
#include "stm32f7xx_hal.h"

/*
 * 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;

static UART_HandleTypeDef huart2;

/* 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_USART2_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();

    /* PA2=TX, PA3=RX, AF7 for USART2 */
    GPIO_InitTypeDef gpio = {0};
    gpio.Pin = GPIO_PIN_2 | GPIO_PIN_3;
    gpio.Mode = GPIO_MODE_AF_PP;
    gpio.Pull = GPIO_PULLUP;
    gpio.Speed = GPIO_SPEED_FREQ_HIGH;
    gpio.Alternate = GPIO_AF7_USART2;
    HAL_GPIO_Init(GPIOA, &gpio);

    /* USART2 config */
    huart2.Instance = USART2;
    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);
}

/*
 * Send motor command via hoverboard protocol.
 * Called at ~50Hz from motor_driver_update().
 */
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;

    HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
}

/*
 * 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);
}