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

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

// 小车默认速度
static int8_t speed = 20;
uint8_t mode = MODE_REMOTE;

void carMove(uint8_t state);
void carSpin(uint8_t state);
void callBuzzer(uint8_t state);
void setSpeed(uint8_t state);
void setServo(uint8_t state);
// void sing(uint8_t state);
void setLed(uint8_t state);
// void fire(uint8_t state);

void CONTROL_CommonCmd(uint8_t type, uint8_t state)
{
    switch (type)
    {
    case CMD_MOVE:
        carMove(state);
        break;
    case CMD_ROTATE:
        carSpin(state);
        break;
    case CMD_BUZZER:
        callBuzzer(state);
        break;
    case CMD_SPEED:
        setSpeed(state);
        break;
    case CMD_SERVO:
        setServo(state);
        break;
    // case CMD_SING:
    //     break;
    case CMD_LED:
        setLed(state);
        break;
    // case CMD_FIRE:
    //     break;
    case CMD_SERVO_RESET:
        SERVO_Init();
        break;
    }
}

inline void CONTROL_Servo(uint8_t degree)
{
    SERVO_Rotate(degree);
}

inline 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 new_mode, uint8_t state)
{
    if (state == 0)
    {
        mode = MODE_STOP;
        MOTOR_Stop();
    }
    else
    {
        mode = new_mode;
        switch (new_mode)
        {
        case MODE_REMOTE:
            speed = 20;
            MOTOR_SetDuty(speed, speed);
            MOTOR_Init();
            break;
        case MODE_PATROL:
            break;
        case MODE_AVOID:
            break;
        default:
            mode = MODE_STOP;
            MOTOR_Stop();
            break;
        }
    }
}

void carMove(uint8_t state)
{
    if (state == 0)
        MOTOR_Stop();
    else
    {
        MOTOR_Init();
        switch (state)
        {
            // 前进
        case 1:
            MOTOR_SetDuty(speed, speed);
            break;
            // 后退
        case 2:
            MOTOR_SetDuty(-speed, -speed);
            break;
            // 左转
        case 3:
            MOTOR_SetDuty(0, speed);
            break;
            // 右转
        case 4:
            MOTOR_SetDuty(speed, 0);
            break;
        }
    }
}

inline void carSpin(uint8_t state)
{
    if (state == 1)
        MOTOR_SetDuty(speed, -speed);
    else if (state == 2)
        MOTOR_SetDuty(-speed, speed);
}

inline void callBuzzer(uint8_t state)
{
    BUZZER_StartTimed(5);
}

void setSpeed(uint8_t state)
{
    if (state == 1)
        speed += 10;
    else if (state == 2)
        speed -= 10;
    if (speed > 100)
        speed = 100;
    else if (speed < 20)
        speed = 20;
}

void setServo(uint8_t state)
{
    if (state == 1)
        SERVO_Rotate(180);
    else if (state == 2)
        SERVO_Rotate(0);
}

void setLed(uint8_t state)
{
    LED_Start(LED_ALL);
    switch (state)
    {
        // 白
    case 1:
        LED_SetDuty(255, 255, 255);
        break;
        // 红
    case 2:
        LED_SetDuty(255, 0, 0);
        break;
        // 绿
    case 3:
        LED_SetDuty(0, 255, 0);
        break;
        // 蓝
    case 4:
        LED_SetDuty(0, 0, 255);
        break;
        // 青
    case 5:
        LED_SetDuty(0, 255, 255);
        break;
        // 品红
    case 6:
        LED_SetDuty(255, 0, 255);
        break;
        // 黄
    case 7:
        LED_SetDuty(255, 255, 0);
        break;
        // 关
    default:
        LED_SetDuty(0, 0, 0);
        break;
    }
}