#include <avr/pgmspace.h>
#include <Arduino.h>
#include <IRremote.h>
#include <Servo.h>
#include <Adafruit_NeoPixel.h>
#include "TRSensors.h"
#include "ArduinoJson.h"

#define NTD1 294
#define NTD2 330
#define NTD3 350
#define NTD4 393
#define NTD5 441
#define NTD6 495
#define NTD7 556
#define NTDL1 147
#define NTDL2 165
#define NTDL3 175
#define NTDL4 196
#define NTDL5 221
#define NTDL6 248
#define NTDL7 278
#define NTDH1 589
#define NTDH2 661
#define NTDH3 700
#define NTDH4 786
#define NTDH5 882
#define NTDH6 990
#define NTDH7 112
    
#define IR_A 0xA2
#define IR_B 0x62
#define IR_C 0xE2
#define IR0 0x68
#define IR100 0x98
#define IR200 0xB0
#define IR1 0x30
#define IR2 0x18
#define IR3 0x7A
#define IR4 0x10
#define IR5 0x38
#define IR6 0x5A
#define IR7 0x42
#define IR8 0x4A
#define IR9 0x52

#define PWMA   45           //Left Motor Speed pin (ENA)
#define AIN2   40          //Motor-L forward (IN2).
#define AIN1   41          //Motor-L backward (IN1)
#define BIN1   36          //Motor-R forward (IN3)
#define BIN2   37          //Motor-R backward (IN4)
#define PWMB   46           //Right Motor Speed pin (ENB)
#define LED_PIN 13
#define RECV_PIN 7
#define RGB_PIN 24
#define TONE_PIN 23
#define BUTTON_PIN 25

#define  MODE_A 1
#define  MODE_B 2
#define  MODE_C 3

char cmd[80];
byte Num = 0;
String comdata = "";
uint8_t mode = MODE_A;
int Speed = 120;
char IRflag;
char Runflag = 0;
int pos = 90;
char step = 0;
static long lastTime;
static long lasRGBTttime;
uint16_t i, j;
uint8_t count = 0;
char Voltageflag = 0;
uint8_t Vcount = 0;

#define NUM_SENSORS 5
unsigned int sensorValues[NUM_SENSORS];
TRSensors trs =   TRSensors();

Adafruit_NeoPixel RGB = Adafruit_NeoPixel(2, RGB_PIN, NEO_GRB + NEO_KHZ800);

Servo myservo;
IRrecv irrecv(RECV_PIN);
decode_results results;


void setup() {
  Serial.begin(115200);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT);
  myservo.attach(A3);
  myservo.write(pos);
  digitalWrite(LED_PIN, HIGH);
  delay(300);
  digitalWrite(LED_PIN, LOW);
  RGB.begin();
  RGB.setBrightness(30);
  RGB.setColor(0, 255, 0, 0);
  RGB.show();
  tone(TONE_PIN, NTD1, 300);
  delay(600);
  RGB.setColor(0, 0, 255, 0);
  RGB.show();
  tone(TONE_PIN, NTD2, 300);
  delay(600);
  RGB.setColor(0, 0, 0, 255);
  RGB.show();
  tone(TONE_PIN, NTD3, 300);
  delay(600);
  RGB.setColor(0, 255, 0, 0);
  RGB.show();
  irrecv.enableIRIn();
  pinMode(PWMA, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  stop();
  Serial.println("start ...");
}
void getIR()
{
  static long lastIRTime = millis();
  uint8_t  irRead;
  if (irrecv.decode(&results))
  {
    if (results.value & 0xFF + (results.value >> 8) & 0xFF == 0xFF)
    {
      irRead = (results.value >> 8) & 0xFF;
      Serial.println(irRead, HEX);
      IRflag = 1;
      switch ((results.value >> 8) & 0xFF)
      {
        case IR_A:
          mode = MODE_A;
          tone(TONE_PIN, NTD1, 300);
          RGB.setColor(0, 255, 0, 0);
          RGB.show();
          pos = 90;
          Speed = 100;
          myservo.write(pos);
          break;
        case IR_B:
          mode = MODE_B;
          tone(TONE_PIN, NTD2, 300);
          RGB.setColor(0, 0, 255, 0);
          RGB.show();
          pos = 90;
          Speed = 100;
          myservo.write(pos);
          stop();
          break;
        case IR_C:
          mode = MODE_C;
          tone(TONE_PIN, NTD3, 300);
          RGB.setColor(0, 0, 0, 255);
          RGB.show();
          pos = 90;
          Speed = 100;
          myservo.write(pos);
          break;
        case IR0:
          Speed = 100;
          break;
        case IR100:
          Speed = 200;
          break;
       case IR200:
          Speed = 250;
          break;
        case IR1:
          tone(TONE_PIN, NTD1, 300);
          step = 2;
          break;
        case IR2:
          tone(TONE_PIN, NTD2, 300);
           if (mode == MODE_A)
          {
            forward();
            RGB.setColor(0, 255, 0, 0);
            RGB.show();
            //buzzer.tone(NTD4, 300);
          }
          break;
        case IR3:
          tone(TONE_PIN, NTD3, 300);
          step = -2;
          break;
        case IR4:
          tone(TONE_PIN, NTD4, 300);
          if (mode == MODE_A)
          {
            left();
            RGB.setColor(0, 255, 255, 0);
            RGB.show();
          }
          break;
        case IR5:
          tone(TONE_PIN, NTD5, 300);
          if (mode == MODE_A)
          {
            stop();
            //buzzer.tone(NTD4, 300);
            RGB.setColor(0, 0, 0, 0);
            RGB.show();
          }
          break;
        case IR6:
          if (mode == MODE_A)
          {
            right();
            RGB.setColor(0, 0, 0, 255);
            RGB.show();
          }
          tone(TONE_PIN, NTD6, 300);
          break;
        case IR7:
          tone(TONE_PIN, NTD7, 300);
          break;
        case IR8:
          if (mode == MODE_A)
          {
            backward();
            RGB.setColor(0, 0, 255, 0);
            RGB.show();
          }
          tone(TONE_PIN, NTDH1, 300);
          break;
        case IR9:
          tone(TONE_PIN, NTDH2, 300);
          break;
        default:
          IRflag = 0;
      }
    }
    lastIRTime = millis();
    irrecv.resume(); // Receive the next value
  }
  else
  {
    if ((millis() - lastIRTime > 200) && IRflag)
    {
      stop();
      step = 0;
      IRflag = 0;
    }
  }
}
void serialHandle() {

  while (Serial.available() > 0)
  {
    cmd[Num++] = Serial.read();
    comdata += char(cmd[Num - 1]);
    delay(2);
  }
  if (comdata.length() > 0)
  {
    StaticJsonBuffer<80> jsonBuffer;
    JsonObject& DecodeData = jsonBuffer.parseObject(cmd);

    if (DecodeData.success())
    {
      char * jsonData;
      if (mode == MODE_A)
      {
        jsonData = DecodeData["Forward"];
        if (strcmp(jsonData, "Down") == 0) {
          RGB.setColor(0, 0xFF0000);
          RGB.show();
          forward();
          tone(TONE_PIN, NTD1, 300);
        }
        else if (strcmp(jsonData, "Up") == 0)
        {
          RGB.setColor(0, 0x000000);
          RGB.show();
          stop();
        }

        jsonData = DecodeData["Backward"];
        if (strcmp(jsonData, "Down") == 0) {
          RGB.setColor(0, 0x00FF00);
          RGB.show();
          backward();
          tone(TONE_PIN, NTD2, 300);
        }
        else if (strcmp(jsonData, "Up") == 0)
        {
          RGB.setColor(0, 0x000000);
          RGB.show();
          stop();
        }

        jsonData = DecodeData["Left"];
        if (strcmp(jsonData, "Down") == 0) {
          RGB.setColor(0, 0xFFFF00);
          RGB.show();
          left();
          tone(TONE_PIN, NTD3, 300);
        }
        else if (strcmp(jsonData, "Up") == 0)
        {
          RGB.setColor(0, 0x000000);
          RGB.show();
          stop();
        }

        jsonData = DecodeData["Right"];
        if (strcmp(jsonData, "Down") == 0) {
          RGB.setColor(0, 0x0000FF);
          RGB.show();
          right();
          tone(TONE_PIN, NTD4, 300);
        }
        else if (strcmp(jsonData, "Up") == 0)
        {
          RGB.setColor(0, 0x000000);
          RGB.show();
          stop();
        }

        jsonData = DecodeData["ServoLeft"];
        if (strcmp(jsonData, "Down") == 0) {
          step = 2;
        }
        else if (strcmp(jsonData, "Up") == 0)
        {
          step = 0;
        }
        jsonData = DecodeData["ServoRight"];
        if (strcmp(jsonData, "Down") == 0) {
          step = -2; 
        }
        else if (strcmp(jsonData, "Up") == 0)
        {
          step = 0;
        }
      }

      jsonData = DecodeData["Mode"];
      if (strcmp(jsonData, "Low") == 0) {
        mode = MODE_A;
        tone(TONE_PIN, NTD1, 300);
        RGB.setColor(0, 255, 0, 0);
        RGB.show();
        stop();
        Speed = 100;
      }
      else if (strcmp(jsonData, "Medium") == 0)
      {
        mode = MODE_B;
        tone(TONE_PIN, NTD2, 300);
        RGB.setColor(0, 0, 255, 0);
        RGB.show();
        pos = 90;
        myservo.write(pos);
        stop();
        Speed = 100;
      }
      else if (strcmp(jsonData, "High") == 0)
      {
        mode = MODE_C;
        tone(TONE_PIN, NTD3, 300);
        RGB.setColor(0, 0, 0, 255);
        RGB.show();
        pos = 90;
        myservo.write(pos);
        Speed = 100;
      }

      jsonData = DecodeData["Low"];
      if (strcmp(jsonData, "Down") == 0) {
        Speed = 100;
      }
      jsonData = DecodeData["Medium"];
      if (strcmp(jsonData, "Down") == 0) {
        Speed = 200;
      }
      jsonData = DecodeData["High"];
      if (strcmp(jsonData, "Down") == 0) {
        Speed = 250;
      }
    }
    else
    {
      Serial.println("failed");
    }
    Num = 0;
    comdata = "";
  }
}
void forward()
{
  analogWrite(PWMA, Speed);
  analogWrite(PWMB, Speed);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  Runflag = 1;
}

void backward()
{
  analogWrite(PWMA, Speed);
  analogWrite(PWMB, Speed);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  Runflag = 1;
}

void right()
{
  analogWrite(PWMA, 80);
  analogWrite(PWMB, 80);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  Runflag = 1;
}

void left()
{
  analogWrite(PWMA, 80);
  analogWrite(PWMB, 80);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  Runflag = 1;
}

void stop()
{
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);
  Runflag = 0;
}

uint32_t Wheel(byte WheelPos) {
  if (WheelPos < 85) {
    return RGB.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
  } else if (WheelPos < 170) {
    WheelPos -= 85;
    return RGB.Color(255 - WheelPos * 3, 0, WheelPos * 3);
  } else {
    WheelPos -= 170;
    return RGB.Color(0, WheelPos * 3, 255 - WheelPos * 3);
  }
}

float getDistanceCm() {
  int TRIG = A4;
  int ECHO = 4;
  pinMode(ECHO, INPUT);    // Define the ultrasonic echo input pin
  pinMode(TRIG, OUTPUT);   // Define the ultrasonic trigger input pin

  digitalWrite(TRIG, LOW);   // set trig pin low 2μs
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);  // set trig pin 10μs , at last 10us
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);    // set trig pin low
  // noInterrupts();
  float Fdistance = pulseIn(ECHO, HIGH, 20000); // Read echo pin high level time(us)
  // interrupts();
  return  Fdistance = Fdistance / 58;    //Y m=（X s*344）/2
}

void modeA()
{

}

void modeB()
{
  float d = getDistanceCm();
  float Ld;
  float Rd;
  static long time = millis();
  randomSeed(analogRead(6));
  uint8_t randNumber = random(2);
  if (d > 10 || d == 0)
  {
    forward();
  }
  else if ((d > 3) && (d < 10))
  {
    stop();
    myservo.write(0);
    delay(300);
    Rd = getDistanceCm();
    myservo.write(180);
    delay(500);
    Ld = getDistanceCm();
    myservo.write(90);
    if (Ld > d || Rd > d)
    {
      if (Rd > Ld) {
        right();
        delay(500);
      } else
      {
        left();
        delay(500);
      }
      stop();
      delay(300);
    }
  }
  else
  {
    right();
    delay(1000);
  }
  delay(100);
}

void modeC()
{
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);

  // Get the position of the line.
  unsigned int position = trs.readLine(sensorValues);

  // The "proportional" term should be 0 when we are on the line.
  int proportional = (int)position - 2000;
  Serial.println(proportional);

  // improve performance.
  int power_difference = proportional / 10;

  // to a negative value.
  const int maximum = 150;

  if (power_difference > maximum)
    power_difference = maximum;
  if (power_difference < - maximum)
    power_difference = - maximum;
  if (power_difference < 0)
  {
    analogWrite(PWMB, maximum + power_difference);
    analogWrite(PWMA, maximum);
  }
  else
  {
    analogWrite(PWMB, maximum);
    analogWrite(PWMA, maximum - power_difference);
  }
}

void loop() {
  getIR();
  serialHandle();
  // put your main code here, to run repeatedly:
  if (millis() - lastTime > 20) {
    lastTime = millis();
    if (step !=  0) {
      pos += step;
      if (pos > 180)pos = 180;
      if (pos < 0)pos = 0;
      myservo.write(pos);
    }
    if (!Runflag)
    {
      for (i = 0; i < RGB.numPixels(); i++) {
        RGB.setPixelColor(i, Wheel(((i * 256 / RGB.numPixels()) + j) & 255));
      }
      RGB.show();
      if (j++ > 256 * 5) j = 0;
    }
    if (++ count > 10)
    {
      count = 0;
      if (digitalRead(LED_PIN) == LOW) {
        digitalWrite(LED_PIN, HIGH);
      } else {
        digitalWrite(LED_PIN, LOW);
      }
      if (++ Vcount > 10)
      {
        Vcount = 0;
        float Voltage = analogRead(15) * 5000.0 / 1024 * 2 + 270;
        Serial.print("The ADC value is: ");
        Serial.print(Voltage, 0);
        Serial.println(" mV");
      }
    }
  }
  switch (mode)
  {
    case MODE_A:
      modeA();
      break;
    case MODE_B:
      modeB();
      break;
    case MODE_C:
      modeC();
      break;
  }
}
