#include "control.h"
#include "hcsr04.h"
#include "led.h"
#include "motor.h"

#define CMD_MOVE 0
#define CMD_ROTATE 1
#define CMD_BUZZER 2
#define CMD_SPEED 3
// #define CMD_SHAKE 4
// #define CMD_SING 5
#define CMD_LED 6
// #define CMD_FIRE 7
// #define CMD_SERVO 8

void move(uint8_t state);
void rotate(uint8_t state);
void buzzer(uint8_t state);
void speed(uint8_t state);
// void shake(uint8_t state);
// void sing(uint8_t state);
void led(uint8_t state);
// void fire(uint8_t state);
// void servo(uint8_t state);

void CONTROL_CommonCmd(uint8_t type, uint8_t state)
{
    switch (type)
    {
    case CMD_MOVE:
        move(state);
        break;
    case CMD_ROTATE:
        rotate(state);
        break;
    case CMD_BUZZER:
        buzzer(state);
        break;
    case CMD_SPEED:
        speed(state);
        break;
    // case CMD_SHAKE:
    //     break;
    // case CMD_SING:
    //     break;
    case CMD_LED:
        led(state);
        break;
    // case CMD_FIRE:
    //     break;
    // case CMD_SERVO:
    //     break;
    }
}

void CONTROL_RGB(uint8_t r, uint8_t g, uint8_t b)
{
    LED_Start(LED_ALL);
    LED_SetDuty(r, g, b);
}

void CONTROL_Mode(uint8_t mode, uint8_t state)
{
}

void move(uint8_t state)
{
}

void rotate(uint8_t state)
{
}

void buzzer(uint8_t state)
{
    if (state)
        HAL_GPIO_WritePin(FM_K2_POWERC_GPIO_Port, FM_K2_POWERC_Pin, GPIO_PIN_RESET);
    else
        HAL_GPIO_WritePin(FM_K2_POWERC_GPIO_Port, FM_K2_POWERC_Pin, GPIO_PIN_SET);
}

void speed(uint8_t state)
{
}

void led(uint8_t state)
{
}