#include <Adafruit_NeoPixel.h>

static const uint8_t LFA = 8;
static const uint8_t LFB = 9;
static const uint8_t LBA = 10;
static const uint8_t LBB = 11;
static const uint8_t RFA = 12;
static const uint8_t RFB = 13;
static const uint8_t RBA = 14;
static const uint8_t RBB = 15;

static const uint8_t BUZZER = 16;

static const uint8_t led = 25;

static const uint8_t Track0 = 26;   //ADC0
static const uint8_t Track1 = 27;   //ADC1
static const uint8_t Track2 = 28;   //ADC2

static const uint8_t TRIG = 6;
static const uint8_t ECHO = 7;

#define NUM_LEDS 15             // LED灯珠数量
#define LED_DT 17               // Arduino输出控制信号引脚
#define LED_TYPE WS2812         // LED灯带型号
#define COLOR_ORDER GRB         // RGB灯珠中红色、绿色、蓝色LED的排列顺序
uint8_t max_bright = 200;       // LED亮度控制变量，可使用数值为 0 ～ 255， 数值越大则光带亮度越高

byte RX_package[7] = {0};

uint32_t rx_timeout = 0;
byte rabbit_model = 0;
int Black_Line = 200;
uint64_t delaytime;

Adafruit_NeoPixel strip(NUM_LEDS, LED_DT, NEO_GRB + NEO_KHZ800);

enum em_dir
{
    stop = 0,
    forward,
    backward,
    turnleft,
    turnright,
    l_up,
    l_down,
    r_up,
    r_down,
    Contrarotate,   // 逆时针转
    Clockwise       // 顺时针转
};

struct st_control
{
    uint8_t key1;
    signed char X;
    signed char Y;
    signed char speed;
} control;

void setup()
{
    Serial1.begin(9600);
    Serial1.setTimeout(60);

    pinMode(LFA, OUTPUT);
    pinMode(LFB, OUTPUT);
    pinMode(LBA, OUTPUT);
    pinMode(LBB, OUTPUT);
    pinMode(RFA, OUTPUT);
    pinMode(RFB, OUTPUT);
    pinMode(RBA, OUTPUT);
    pinMode(RBB, OUTPUT);

    pinMode(BUZZER, OUTPUT);

    pinMode(led, OUTPUT);

    pinMode(TRIG, OUTPUT);
    pinMode(ECHO, INPUT);

    strip.begin();
    strip.setBrightness(max_bright);
    for (size_t i = 0; i < 15; i++)
    {
        strip.setPixelColor(i, 255, 0, 0);
        strip.show();
        delay(200);
        strip.clear();
        strip.show();
    }

    delaytime = millis();
}

// int T0;
// int T1;
// int T2;
void loop()
{
    RX_information();
    switch (rabbit_model)
    {
    case 1:
        obstacle_avoidance_model();
        break;
    case 2:
        follow_model();
        break;
    case 3:
        tracking_model();
        break;
    default:
        free_model();
        break;
    }
    // T0 = analogRead(Track0);
    // T1 = analogRead(Track1);
    // T2 = analogRead(Track2);
    // Serial.print("T0:");
    // Serial.print(T0);
    // Serial.print("--T1:");
    // Serial.print(T1);
    // Serial.print("--T2:");
    // Serial.println(T2);
    Serial1.print("-T2:");
}

void tracking_model()   // 循迹
{
    int T0 = analogRead(Track0);
    int T1 = analogRead(Track1);
    int T2 = analogRead(Track2);
    strip.fill(0x00ffff, 0, 15);
    strip.show();
    if (T0 > Black_Line && T1 <= Black_Line && T2 > Black_Line)//
    {
        car_movement(forward, control.speed);
    }
    else if (T0 <= Black_Line && T1 > Black_Line && T2 > Black_Line)//Contrarotate
    {
        car_movement(Contrarotate, control.speed);
    }
    else if (T0 <= Black_Line && T1 <= Black_Line && T2 > Black_Line)//Contrarotate
    {
        car_movement(Contrarotate, control.speed);
    }
    else if (T0 > Black_Line && T1 > Black_Line && T2 <= Black_Line)//Clockwise
    {
        car_movement(Clockwise, control.speed);
    }
    else if (T0 > Black_Line && T1 <= Black_Line && T2 <= Black_Line)//Clockwise
    {
        car_movement(Clockwise, control.speed);
    }
    else if (T0 > Black_Line && T1 > Black_Line && T2 > Black_Line)
    {
        car_movement(forward, control.speed);
    }
    else if (T0 <= Black_Line && T1 <= Black_Line && T2 <= Black_Line)
    {
        car_movement(stop, 0);
    }
}

void follow_model() // 跟随
{
    int Avoidance_distance = SR04(TRIG, ECHO);
    strip.fill(0x8B00FF, 0, 15);
    strip.show();
    if (Avoidance_distance <= 15)
    {
        buzzer_sound(100);
        car_movement(backward, control.speed);
    }
    else if (15 <= Avoidance_distance && Avoidance_distance <= 20) 
    {
        car_movement(stop, 0);  // stop
    } 
    else if (20 <= Avoidance_distance && Avoidance_distance <= 35)
    { 
        car_movement(forward, control.speed);
    } 
    else 
    {
        car_movement(stop, 0);  // stop
    }
}

void obstacle_avoidance_model() // 避障模式
{
    int Avoidance_distance = SR04(TRIG, ECHO);
    strip.fill(0x0000ff, 0, 15);
    strip.show();
    if (Avoidance_distance <= 25) 
    {
        buzzer_sound(100);
        if (Avoidance_distance <= 15) 
        {
            car_movement(stop, 0);  // stop
            if(true == delay100ms(3))
              return;
        }
        else
        {
            car_movement(stop, 0);  // stop
            if(true == delay100ms(3))
              return;
            car_movement(backward, control.speed);
            if(true == delay100ms(2))
              return;
            car_movement(Contrarotate, control.speed);
            if(true == delay100ms(3))
              return;
        }
    }
    else
    {
        car_movement(forward, control.speed);
    }
}

bool delay100ms(uint32_t ms)
{
  for(uint32_t i=ms; i>0; i--)
  {
    RX_information();
    delay(100);
    if(control.key1 == 0x10 || control.key1 == 0x40 || control.key1 == 0x80)
    {
      return true;
    }
  }
  return false;
}

void free_model()       // 自由遥控模式
{
    if (control.key1 == 0x01)   // clockwise
    {
        car_movement(Clockwise, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.key1 == 0x02)  // counterclockwise
    {
        car_movement(Contrarotate, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.key1 == 0x04)  // buzzer
    {
        buzzer_sound(1);
    }
    else if (control.X <= 60 && control.X >= -60 && control.Y >= 60)  // forward
    {
        car_movement(forward, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.X <= 60 && control.X >= -60 && control.Y <= -60)  // backward
    {
        car_movement(backward, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.Y <= 60 && control.Y >= -60 && control.X <= -60)  // Shift to the left
    {
        car_movement(turnleft, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.Y <= 60 && control.Y >= -60 && control.X >= 60)  // Shift to the right
    {
        car_movement(turnright, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.Y >= 60 && control.X <= -60)  // upper left
    {
        car_movement(l_up, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.Y <= -60 && control.X <= -60)  // lower left
    {
        car_movement(l_down, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.Y >= 60 && control.X >= 60)  // upper right
    {
        car_movement(r_up, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else if (control.Y <= -60 && control.X >= 60)  // lower right
    {
        car_movement(r_down, control.speed);
        strip.fill(0x00ff00, 0, 15);
        strip.show();
    }
    else
    {
        car_movement(stop, 0);  // stop
        if ((millis() - delaytime) > 1000)
        {
            delaytime = millis();
            strip.fill(random(1,16777215), 0, 15);
            strip.show();
        }
    }
}

void buzzer_sound(unsigned int time_ms)
{
    while (time_ms)
    {
        time_ms--;
        digitalWrite(BUZZER, 1);
        delayMicroseconds(600);
        digitalWrite(BUZZER, 0);
        delayMicroseconds(400);
    } 
}

float SR04(int Trig, int Echo)
{
    digitalWrite(Trig, LOW);
    delayMicroseconds(2);
    digitalWrite(Trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(Trig, LOW);
    float distance = pulseIn(Echo, HIGH) / 58.00;
    delay(10);
    return distance;
}

void RX_information()
{
    if (Serial1.available())
    {
        Serial1.readBytes(RX_package, 7);
        if (RX_package[0] == 0xA5 && RX_package[6] == 0x5A)
        {
            control.key1 = RX_package[1];
            control.X = RX_package[2];
            control.Y = RX_package[3];
            control.speed = RX_package[4];
            switch (control.key1)
            {
            case 0x10:
                rabbit_model = 0;   // Free remote control mode
            break;
            case 0x20:
                rabbit_model = 1;   // OA
                break;
            case 0x40:
                rabbit_model = 2;   // Follow
                break;
            case 0x80:
                rabbit_model = 3;   // tracking
                break;
            } 
        }
        else
        {
            digitalWrite(led, !digitalRead(led));
            control.X = 0;
            control.Y = 0;
            Serial1.flush();
        }
    }
    else        // 超时600ms没有数据接收清空 Timeout 600ms No data receive clearing
    {
        delay(1);   
        rx_timeout++;
        if (rx_timeout > 600)
        {
            rx_timeout = 0;
            digitalWrite(led, !digitalRead(led));
            control.X = 0;
            control.Y = 0;
            Serial1.flush();
        }
    }
}

void car_movement(em_dir direction, byte speed)
{
    speed = speed + 128;
    switch (direction)
    {
    case forward:
        analogWrite(LFA, speed);
        analogWrite(LFB, 0);
        analogWrite(LBA, speed);
        analogWrite(LBB, 0);
        analogWrite(RFA, 0);
        analogWrite(RFB, speed);
        analogWrite(RBA, 0);
        analogWrite(RBB, speed);
        break;
    case backward:
        analogWrite(LFA, 0);
        analogWrite(LFB, speed);
        analogWrite(LBA, 0);
        analogWrite(LBB, speed);
        analogWrite(RFA, speed);
        analogWrite(RFB, 0);
        analogWrite(RBA, speed);
        analogWrite(RBB, 0);
        break;
    case turnleft:
        analogWrite(LFA, 0);
        analogWrite(LFB, speed);
        analogWrite(LBA, speed);
        analogWrite(LBB, 0);
        analogWrite(RFA, 0);
        analogWrite(RFB, speed);
        analogWrite(RBA, speed);
        analogWrite(RBB, 0);
        break;
    case turnright:
        analogWrite(LFA, speed);
        analogWrite(LFB, 0);
        analogWrite(LBA, 0);
        analogWrite(LBB, speed);
        analogWrite(RFA, speed);
        analogWrite(RFB, 0);
        analogWrite(RBA, 0);
        analogWrite(RBB, speed);
        break;
    case l_up:
        analogWrite(LFA, 0);
        analogWrite(LFB, 0);
        analogWrite(LBA, speed);
        analogWrite(LBB, 0);
        analogWrite(RFA, 0);
        analogWrite(RFB, speed);
        analogWrite(RBA, 0);
        analogWrite(RBB, 0);
        break;
    case l_down:
        analogWrite(LFA, 0);
        analogWrite(LFB, speed);
        analogWrite(LBA, 0);
        analogWrite(LBB, 0);
        analogWrite(RFA, 0);
        analogWrite(RFB, 0);
        analogWrite(RBA, speed);
        analogWrite(RBB, 0);
        break;
    case r_up:
        analogWrite(LFA, speed);
        analogWrite(LFB, 0);
        analogWrite(LBA, 0);
        analogWrite(LBB, 0);
        analogWrite(RFA, 0);
        analogWrite(RFB, 0);
        analogWrite(RBA, 0);
        analogWrite(RBB, speed);
        break;
    case r_down:
        analogWrite(LFA, 0);
        analogWrite(LFB, 0);
        analogWrite(LBA, 0);
        analogWrite(LBB, speed);
        analogWrite(RFA, speed);
        analogWrite(RFB, 0);
        analogWrite(RBA, 0);
        analogWrite(RBB, 0);
        break;
    case Clockwise:
        analogWrite(LFA, speed);
        analogWrite(LFB, 0);
        analogWrite(LBA, speed);
        analogWrite(LBB, 0);
        analogWrite(RFA, speed);
        analogWrite(RFB, 0);
        analogWrite(RBA, speed);
        analogWrite(RBB, 0);
        break;
    case Contrarotate:
        analogWrite(LFA, 0);
        analogWrite(LFB, speed);
        analogWrite(LBA, 0);
        analogWrite(LBB, speed);
        analogWrite(RFA, 0);
        analogWrite(RFB, speed);
        analogWrite(RBA, 0);
        analogWrite(RBB, speed);
        break;
    default:
        analogWrite(LFA, 255);
        analogWrite(LFB, 255);
        analogWrite(LBA, 255);
        analogWrite(LBB, 255);
        analogWrite(RFA, 255);
        analogWrite(RFB, 255);
        analogWrite(RBA, 255);
        analogWrite(RBB, 255);
        break;
    }
}
