#include "bluetooth.h"

#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>

#include "control.h"
#include "hcsr04.h"
#include "infrared.h"
#include "line_seek.h"
#include "timer.h"
#include "usart.h"

#define BUFFER_SIZE 255
#define MESSAGE_SIZE 19
#define CMD_NUM 9

static int8_t rxBuffer[BUFFER_SIZE], txBuffer[BUFFER_SIZE];
static uint16_t txLen;
static uint8_t cmdIndex;
static uint8_t timer_event_id = -1;
void sendSensor(void)
{
    // static uint8_t line_seek[5];
    // strcpy(line_seek, LineSeek_GetStatusStr());
    // for (uint8_t i = 0; i < 5; ++i)
    //     line_seek[i] ^= 1;
    // BLUETOOTH_Send("$4WD,CSB%u,PV-,GS-,LF%s,HW%hhu%hhu,GM-#",
    //                (uint32_t)round((double)sonor_distance / 10),
    //                line_seek,
    //                !INFRARED_GetL(),
    //                !INFRARED_GetR());
}

void BLUETOOTH_Init(void)
{
    HAL_UARTEx_ReceiveToIdle_DMA(&huart2, rxBuffer, 100);
    TIMER_AddLoopEvent(EVENT_BLUETOOTH, sendSensor, 200);
}

void BLUETOOTH_Send(const char *str, ...)
{
    va_list args;
    va_start(args, str);
    vsprintf(txBuffer, str, args);
    va_end(args);
    HAL_UART_Transmit_DMA(&huart2, txBuffer, strlen(txBuffer));
}

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    if (huart == &huart2 && rxBuffer[0] == '$' && rxBuffer[Size - 1] == '#')
    {
        rxBuffer[Size] = '\0';
        // 通用协议
        if (mode == MODE_REMOTE && rxBuffer[2] == ',')
        {
            cmdIndex = 0;
            for (uint8_t i = 1; i < MESSAGE_SIZE; i += 2)
                if (rxBuffer[i] == '0')
                    ++cmdIndex;
                else
                    break;
            if (cmdIndex == CMD_NUM)
                cmdIndex = 0;
            CONTROL_CommonCmd(cmdIndex, rxBuffer[cmdIndex * 2 + 1] - '0');
        }
        else
        {
            static uint16_t num[6];

            // 模式选择
            if (rxBuffer[5] == 'M')
            {
                sscanf(rxBuffer + 5, "%*4s%c%c", num, num + 1);
                CONTROL_Mode(num[0] - '0', num[1] - '0');
            }
            else if (mode == MODE_REMOTE)
                switch (rxBuffer[5])
                {
                        // 机械手
                    case 'S':
                        break;
                        // 舵机云台
                    case 'P':
                        sscanf(rxBuffer + 5, "%*3s%hu", num);
                        CONTROL_Servo(180 - num[0]);
                        break;
                        // 七彩灯
                    case 'C':
                        sscanf(rxBuffer + 5, "%*3s%hu,%*3s%hu,%*3s%hu", num, num + 1, num + 2);
                        CONTROL_RGB(num[0], num[1], num[2]);
                        break;
                }
        }
        BLUETOOTH_Init();
    }
}