#include <pitches.h>
#include <Tone32.h>

#include <ESP32Servo.h> // ESP32Servo library installed by Library Manager
#include "ESC.h" // RC_ESP library installed by Library Manager
#include <analogWrite.h>
#include <ESP32Tone.h>
#include <ESP32PWM.h>

#include <LiquidCrystal_I2C.h>

//#include <digitalWriteFast.h>

#define RotateServo (25)
#define IRSensor (14)
#define ESC_PIN (13) // connected to ESC control wire
#define Button_1 (27)
#define Button_2 (32)
#define Button_3 (26)

#define MIN_SPEED 1244 // speed just slow enough to turn motor off
#define MAX_SPEED 1900 // speed where my motor drew 3.6 amps at 12v.

LiquidCrystal_I2C lcd(0x26,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display

Servo Rotate;     // create servo object to control the ESC

int angle = 0;
//int angleStep = 5;

int angleMin = 30;
int angleMax = 150;

ESC MotorESC (ESC_PIN, 1000, 2000, 500); // ESC_Name (PIN, Minimum Value, Maximum Value, Arm Value)

int Speed = 0;

int PlayTime = 31;

int countDownSec = PlayTime;

int Score = 0;

int IR_staus = 0;
int BT_staus = 0;
int waiting_staus = 0;
int rotate_staus = 0;
int motor_staus = 0;

int started = 0;

float waiting = 0;

int difficulty = 0;

int counter1 = 0;
int counter2 = 0;

unsigned long previousMillis = 0; 

const long interval = 1000;

void setup() {
  Serial.begin(115200);
  delay(1000);
  pinMode(ESC_PIN, OUTPUT);
  MotorESC.arm(); // Send the Arm command to ESC
  delay(5000); // Wait a while

  Rotate.attach(RotateServo);
  Rotate.write((angleMax + angleMin) / 2);
  
  randomSeed(analogRead(0));

  pinMode(IRSensor, INPUT); // sensor pin INPUT
  pinMode(Button_1  , INPUT_PULLUP);
  pinMode(Button_2  , INPUT_PULLUP);
  pinMode(Button_3  , INPUT_PULLUP);
  
  lcd.init();                      // initialize the lcd
  lcd.backlight();
  lcd.print(">   --------   <");
  lcd.setCursor(0,1);
  lcd.print(">START     EASY<");
  
//  \/   For Testing   \/
//  lcd.setCursor(0,1);
//  lcd.print("PUSHED: ");
//  lcd.print("ANGLE: ");
//  lcd.print("SPEED: ");
//  lcd.print("IR: ");
//  lcd.setCursor(6,1);
//  lcd.print("Count: ");

  // the following loop turns on the motor slowly, so get ready
  for (int i=0; i<350; i++){ // run speed from 840 to 1190
    MotorESC.speed(MIN_SPEED-200+i); // motor starts up about half way through loop
    delay(10);
  }
  MotorESC.speed(0);
}

void waitforuser() {
  if (waiting <= 1) {
    lcd.setCursor(0,0);
    lcd.print(">BALL-IN       <");
  } else if (waiting <= 2) {
    lcd.setCursor(0,0);
    lcd.print("> BALL-IN      <");
  } else if (waiting <= 3) {
    lcd.setCursor(0,0);
    lcd.print(">  BALL-IN     <");
  } else if (waiting <= 4) {
    lcd.setCursor(0,0);
    lcd.print(">   BALL-IN    <");
  } else if (waiting <= 5) {
    lcd.setCursor(0,0);
    lcd.print(">    BALL-IN   <");
  } else if (waiting <= 6) {
    lcd.setCursor(0,0);
    lcd.print(">     BALL-IN  <");
  } else if (waiting <= 7) {
    lcd.setCursor(0,0);
    lcd.print(">      BALL-IN <");
  } else if (waiting <= 8) {
    lcd.setCursor(0,0);
    lcd.print(">       BALL-IN<");
  } else {
    lcd.setCursor(0,0);
    lcd.print("> ERROR  ERROR <");
  }    
}

void showScore() {
  lcd.setCursor(7,0);
  lcd.print(Score);
}

void showTime() {
  lcd.setCursor(6,1);
  lcd.print(countDownSec);
  lcd.print("  ");
  Serial.println((String) "Sec: " + countDownSec);
}

void reset() {
  difficulty = 0;
  countDownSec = PlayTime;
  Score = 0;
  counter1 = 0;
  counter2 = 0;
  Speed = 0;
  MotorESC.speed(Speed);
  lcd.setCursor(0,1);
  lcd.print(">START     EASY<");
  started = 0;
}

void easy_mode() {
  if (counter1 == 80) {
    if (angle >= angleMax && rotate_staus != 1) {
      rotate_staus = 1;
    } else if (angle <= angleMin && rotate_staus != 0) {
      rotate_staus = 0;
    } else if (rotate_staus == 0) {
      angle = angle + random(2, 4);
    } else if (rotate_staus == 1) {
      angle = angle - random(2, 4);
    } 
    if (angle > angleMax) {
      angle = angleMax;
    } 
    if (angle < angleMin) {
      angle = angleMin;
    }
    Serial.println((String) "Angle: " + angle);
    Rotate.write(angle);
    counter1 = 0;
  }
  else {
    counter1++;
  }

  
  if (counter2 == 100) {
    if (Speed >= MAX_SPEED && motor_staus != 1) {
      motor_staus = 1;
    } else if (Speed <= MIN_SPEED && motor_staus != 0) {
      motor_staus = 0;
    } else if (motor_staus == 0) {
      Speed = Speed + 75;
    } else if (motor_staus == 1) {
      Speed = Speed - 75;
    } 
    if (Speed > MAX_SPEED) {
      Speed = MAX_SPEED;
    } 
    if (Speed < MIN_SPEED) {
      Speed = MIN_SPEED;
    }
    Serial.println((String) "Speed: " + Speed);
    MotorESC.speed(Speed);
    counter2 = 0;
  }
  else {
    counter2++;
  }
}

void normal_mode() {
  if (counter1 == 80) {
    if (angle >= angleMax && rotate_staus != 1) {
      rotate_staus = 1;
    } else if (angle <= angleMin && rotate_staus != 0) {
      rotate_staus = 0;
    } else if (rotate_staus == 0) {
      angle = angle + random(1, 8);
    } else if (rotate_staus == 1) {
      angle = angle - random(1, 8);
    } 
    if (angle > angleMax) {
      angle = angleMax;
    } 
    if (angle < angleMin) {
      angle = angleMin;
    }
    Serial.println((String) "Angle: " + angle);
    Rotate.write(angle);
    counter1 = 0;
  }
  else {
    counter1++;
  }

  if (counter2 == 100) {
    if (Speed >= MAX_SPEED && motor_staus != 1) {
      motor_staus = 1;
    } else if (Speed <= MIN_SPEED && motor_staus != 0) {
      motor_staus = 0;
    } else if (motor_staus == 0) {
      Speed = Speed + random(25, 100);
    } else if (motor_staus == 1) {
      Speed = Speed - random(25, 200);
    } 
    if (Speed > MAX_SPEED) {
      Speed = MAX_SPEED;
    } 
    if (Speed < MIN_SPEED) {
      Speed = MIN_SPEED;
    }
    Serial.println((String) "Speed: " + Speed);
    MotorESC.speed(Speed);
    counter2 = 0;
  }
  else {
    counter2++;
  }
}

void hard_mode() {
  if (counter1 == 80) {
    if (angle >= angleMax && rotate_staus != 1) {
      rotate_staus = 1;
    } else if (angle <= angleMin && rotate_staus != 0) {
      rotate_staus = 0;
    } else if (rotate_staus == 0) {
      angle = angle + random(1, 16);
    } else if (rotate_staus == 1) {
      angle = angle - random(1, 16);
    } 
    if (angle > angleMax) {
      angle = angleMax;
    } 
    if (angle < angleMin) {
      angle = angleMin;
    }
    Serial.println((String) "Angle: " + angle);
    Rotate.write(angle);
    counter1 = 0;
  }
  else {
    counter1++;
  }


  if (counter2 == 100) {
    if (Speed >= MAX_SPEED && motor_staus != 1) {
      motor_staus = 1;
    } else if (Speed <= MIN_SPEED && motor_staus != 0) {
      motor_staus = 0;
    } else if (motor_staus == 0) {
      Speed = Speed + random(50, 200);
    } else if (motor_staus == 1) {
      Speed = Speed - random(50, 200);
    } 
    if (Speed > MAX_SPEED) {
      Speed = MAX_SPEED;
    } 
    if (Speed < MIN_SPEED) {
      Speed = MIN_SPEED;
    }
    Serial.println((String) "Speed: " + Speed);
    MotorESC.speed(Speed);
    counter2 = 0;
  }
  else {
    counter2++;
  }
}

void loop() {
  
  unsigned long currentMillis = millis();
  
  int statusSensor = digitalRead(IRSensor);

  int Pushed_1 = digitalRead(Button_1);
  int Pushed_2 = digitalRead(Button_2);
  int Pushed_3 = digitalRead(Button_3);

  Serial.println(Pushed_1);
  Serial.println(Pushed_2);
  Serial.println(Pushed_3);
  
  if (started == 1) {
    if (currentMillis - previousMillis >= interval) {
      previousMillis = currentMillis;
      countDownSec--;
      if (countDownSec < 0) {
        countDownSec = 0;
      }
      showTime();
    }
    if (countDownSec > 0 && Pushed_1 != LOW && Pushed_3 == LOW) {
      if (difficulty == 0){         // easy mode
        easy_mode();
      } else if (difficulty == 1) { // normal mode
        normal_mode();
      } else if (difficulty == 2) { // hard mode
        hard_mode();
      } 
      Serial.println((String) "IR: " + statusSensor);
      if (statusSensor == 1 && IR_staus == 1) {
        IR_staus = 0;    
        showScore();
      } else if (statusSensor == 0 && IR_staus == 0) {
        Score++;
        IR_staus = 1; 
        showScore();
      } else {
        showScore();
      }
    } else {
      Speed = 0;
      MotorESC.speed(Speed);
      lcd.clear();
      lcd.setCursor(0,1);
      lcd.print("Gameover!");
      lcd.setCursor(0,0);
      lcd.print("Score: ");
      lcd.print(Score);
      lcd.setCursor(15,1);
      lcd.print("3");
      delay(1000);
      lcd.setCursor(15,1);
      lcd.print("2");
      delay(1000);
      lcd.setCursor(15,1);
      lcd.print("1");
      delay(1000);
      reset();
    }
  } else {
    if (Pushed_1 == LOW) {
      started = 1;
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Start In...");
      lcd.setCursor(0,1);
      lcd.print("3");
      delay(1000);
      lcd.setCursor(0,1);
      lcd.print("2");
      delay(1000);
      lcd.setCursor(0,1);
      lcd.print("1");
      delay(1000);
      lcd.clear();
      lcd.setCursor(0,1);
      lcd.print("Time:");
      lcd.setCursor(0,0);
      lcd.print("Score:");
    } else if (Pushed_2 == LOW) {
      if (difficulty == 0 && BT_staus == 0) {
        BT_staus = 1;
        difficulty = 1;   // easy > normal
        lcd.setCursor(9,1);
        lcd.print("NORMAL");
      } else if (difficulty == 1 && BT_staus == 0) {
        BT_staus = 1;
        difficulty = 2;   // normal > hard
        lcd.setCursor(9,1);
        lcd.print("  HARD");
      } else if (difficulty == 2 && BT_staus == 0) {
        BT_staus = 1;
        difficulty = 0;   // hard > easy
        lcd.setCursor(9,1);
        lcd.print("  EASY");
      }
    } else {
      BT_staus = 0;
      if (waiting == 8 && waiting_staus != 1) {
        waiting_staus = 1;
      } else if (waiting == 0 && waiting_staus != 0) {
        waiting_staus = 0;
      } else if (waiting_staus == 0) {
        waiting = waiting + 0.25;
        waitforuser();
      } else if (waiting_staus == 1)
        waiting = waiting - 0.25;
        waitforuser();
      } 
  }

//  \/   For Testing   \/

//  if (statusSensor == 1 && staus == 1) {
//    staus = 0;    
//    lcd.setCursor(4,1);
//    lcd.print(statusSensor);
//    lcd.setCursor(13,1);
//    lcd.print(Score);
//  } else if (statusSensor == 0 && staus == 0) {
//    Score = Score + 1;
//    staus = 1; 
//    lcd.setCursor(4,1);
//    lcd.print(statusSensor);
//    lcd.setCursor(13,1);
//    lcd.print(Score);
//  } else {
//    lcd.setCursor(4,1);
//    lcd.print(statusSensor);
//    lcd.setCursor(13,1);
//    lcd.print(Score);
//  }

  
//  if (Pushed_1 == LOW) {
//    lcd.setCursor(8,1);
//    lcd.print("LOW_1");
//    Serial.println(Pushed_1);
//    Serial.println(Pushed_2);
//    Serial.println(Pushed_3);
//  } else if (Pushed_2 == LOW) {
//    lcd.setCursor(8,1);
//    lcd.print("LOW_2");
//    Serial.println(Pushed_1);
//    Serial.println(Pushed_2);
//    Serial.println(Pushed_3);
//  } else if (Pushed_3 == LOW) {
//    lcd.setCursor(8,1);
//    lcd.print("LOW_3");
//    Serial.println(Pushed_1);
//    Serial.println(Pushed_2);
//    Serial.println(Pushed_3);
//  } else {
//    lcd.setCursor(8,1);
//    lcd.print("HIGH ");
//    Serial.println(Pushed_1);
//    Serial.println(Pushed_2);
//    Serial.println(Pushed_3);
//  }


//  Speed = 0;
//  lcd.setCursor(7,1);
//  lcd.print(Speed);
//  lcd.print("    ");
//  Serial.println(Speed);
//  MotorESC.speed(Speed);
//  delay(1000);
//  
//  Speed = MIN_SPEED;
//  lcd.setCursor(7,1);
//  lcd.print(Speed);
//  Serial.println(Speed);
//  MotorESC.speed(Speed);
//  delay(2000);
//  
//  Speed = (MIN_SPEED + MAX_SPEED) / 2;
//  lcd.setCursor(7,1);
//  lcd.print(Speed);
//  Serial.println(Speed);
//  MotorESC.speed(Speed);
//  delay(2000);
//
//  Speed = MAX_SPEED;
//  lcd.setCursor(7,1);
//  lcd.print(Speed);
//  Serial.println(Speed);
//  MotorESC.speed(Speed);
//  delay(2000);


//  for (int i=0; i<=180; i++){ // run speed from 840 to 1190
//    Rotate.write(i);
//    lcd.setCursor(7,1);
//    lcd.print(i);
//    Serial.println(i);
//    delay(20);
//  }
//
//  for (int i=180; i>=0; i--){ // run speed from 840 to 1190
//    Rotate.write(i);
//    lcd.setCursor(7,1);
//    lcd.print(i);
//    lcd.print(" ");
//    Serial.println(i);
//    delay(20);
//  }

//  delay(50);


//  for (int i=0; i<180; i++){ // run speed from 840 to 1190
//  int val = map(i, 0, 180, MIN_SPEED, MAX_SPEED);
//  MotorESC.speed(val); // motor starts up about half way through loop
//  lcd.setCursor(7,1);
//  lcd.print(val);
//  delay(10);
//  }

//  int val = 180;
//  val = map(val, 0, 180, MIN_SPEED, MAX_SPEED);
//  MotorESC.speed(val); // motor starts up about half way through loop
//  lcd.setCursor(7,1);
//  lcd.print(val);
//  delay(100);
  
}