#include "hoverboard.h"
#include "config.h"
#include "stm32f7xx_hal.h"

static UART_HandleTypeDef huart2;

void hoverboard_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);
}

void hoverboard_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);
}