//#include <ArduinoLowPower.h>
//#include <avr/power.h>
#include <avr/sleep.h>


volatile bool startStopState = true;
volatile byte chaseHideState = HIGH; //HIGH for CHASING mode, LOW - HIDING mode

const byte motorDriverEnablePin = 7;
const byte startStopButtonPin = 2;
const byte chaseHideButtonPin = 3;
const byte rightLdrPin = A2;
const byte leftLdrPin = A3;
const byte rightMotorPin[] = {5, 4};
const byte leftMotorPin[] = {15, 14}; //A1, A0
int trigPin = 11;      // trig pin of HC-SR04
int echoPin = 10;     // Echo pin of HC-SR04

int distance,duration;


void setup()
{
  pinMode(motorDriverEnablePin, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(startStopButtonPin, INPUT_PULLUP);
  pinMode(chaseHideButtonPin, INPUT_PULLUP);
  pinMode(rightLdrPin, INPUT);
  pinMode(leftLdrPin, INPUT);
  pinMode(rightMotorPin[0], OUTPUT);
  pinMode(rightMotorPin[1], OUTPUT);
  pinMode(leftMotorPin[0], OUTPUT);
  pinMode(leftMotorPin[1], OUTPUT);
  pinMode(trigPin, OUTPUT);         // set trig pin as output
  pinMode(echoPin, INPUT);          //set echo pin as input to capture reflected waves

  attachInterrupt(digitalPinToInterrupt(startStopButtonPin), startStopButtonPressed, FALLING);
  attachInterrupt(digitalPinToInterrupt(chaseHideButtonPin), chaseHideButtonRelesed, RISING);

  digitalWrite(rightLdrPin, HIGH); //A2 -> activate internal resistor
  digitalWrite(leftLdrPin, HIGH);  //A3 -> activate internal resistor

  Serial.begin(115200);
}

void loop()
{
  if(startStopState) //Start
  {
    signed rightLdrVal = 1023 - analogRead(rightLdrPin);
    signed leftLdrVal = 1023 - analogRead(leftLdrPin);
    signed ldrDiffVal = abs(leftLdrVal - rightLdrVal);

    digitalWrite(trigPin, HIGH);
    delay(1);
    digitalWrite(trigPin, LOW);
    // Measure the pulse input in echo pin
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    // Distance is half the duration divided by 29.1 (from datasheet)
    distance = (duration/2) / 29.1;
    Serial.print("Distance: ");
    Serial.print(distance);



    Serial.print("\tLeft LDR: ");
    Serial.print(leftLdrVal);
    Serial.print("\tRight LDR: ");
    Serial.print(rightLdrVal);
    Serial.print("\tLDR Diff: ");
    Serial.print(ldrDiffVal);
    Serial.print("\tMode: ");

    digitalWrite(motorDriverEnablePin, HIGH);

    if(chaseHideState) //CHASING mode
    {
      Serial.print("CHASE");
      Serial.print("\tAction: ");


      if(ldrDiffVal <= 10 && leftLdrVal > 800 && rightLdrVal > 800)
        driveForward();
      else if(leftLdrVal > rightLdrVal && ldrDiffVal > 10 && leftLdrVal > 800 && rightLdrVal > 800)
        turnLeft();
      else if(rightLdrVal > leftLdrVal && ldrDiffVal > 10 && leftLdrVal > 800 && rightLdrVal > 800)
        turnRight();
      else
        stop();
    }
    else //HIDING mode
    {
      Serial.print("HIDE");
      Serial.print("\tAction: ");

      if(ldrDiffVal <= 10 && leftLdrVal > 800 && rightLdrVal > 800)
        driveBackward();
      else if(leftLdrVal > rightLdrVal && ldrDiffVal > 10 && leftLdrVal > 800 && rightLdrVal > 800)
        turnRightBackward();
      else if(rightLdrVal > leftLdrVal && ldrDiffVal > 10 && leftLdrVal > 800 && rightLdrVal > 800)
        turnLeftBackward();
      else
        stop();
    }
  }
  else //Stop
  {
    digitalWrite(motorDriverEnablePin, LOW);
    Serial.println("Sleeping...");
    //Sleep...
    goToSleep();
  }
}

void goToSleep()
{
  byte adcsra = ADCSRA;          //save the ADC Control and Status Register A
  ADCSRA = 0;                    //disable the ADC
    /*
    EICRA = _BV(ISC01);            //configure INT0 to trigger on falling edge
    EIMSK = _BV(INT0);             //enable INT0
    set_sleep_mode(SLEEP_MODE_IDLE);     //-> The least power savings
    set_sleep_mode(SLEEP_MODE_ADC);
    set_sleep_mode(SLEEP_MODE_PWR_SAVE);
    set_sleep_mode(SLEEP_MODE_STANDBY);
    set_sleep_mode(SLEEP_MODE_PWR_DOWN); //-> The most power savings
    */
  set_sleep_mode(SLEEP_MODE_IDLE);
  cli();                         //stop interrupts to ensure the BOD timed sequence executes as required
  sleep_enable();
  //disable brown-out detection while sleeping (20-25µA)
    /*
    uint8_t mcucr1 = MCUCR | _BV(BODS) | _BV(BODSE);
    uint8_t mcucr2 = mcucr1 & ~_BV(BODSE);
    MCUCR = mcucr1;
    MCUCR = mcucr2;
    */
  sleep_bod_disable();           //for AVR-GCC 4.3.3 and later, this is equivalent to the previous 4 lines of code
  sei();                         //ensure interrupts enabled so we can wake up again
  sleep_cpu();                   //go to sleep
  sleep_disable();               //wake up here
  ADCSRA = adcsra;               //restore ADCSRA
}


//ISR#0
void startStopButtonPressed()
{
  startStopState = !startStopState;
}
//ISR#1
void chaseHideButtonRelesed()
{
  chaseHideState = !chaseHideState;
}

void stop()
{
  digitalWrite(rightMotorPin[0], LOW);
  digitalWrite(rightMotorPin[1], LOW);
  digitalWrite(leftMotorPin[0], LOW);
  digitalWrite(leftMotorPin[1], LOW);
  Serial.println("Stop");
}

void driveForward()
{
  digitalWrite(rightMotorPin[0], HIGH); //D5	  -> INPUT#3
  digitalWrite(rightMotorPin[1], LOW);  //D4	  -> INPUT#4
  digitalWrite(leftMotorPin[0], LOW);   //D15(A1) -> INPUT#1
  digitalWrite(leftMotorPin[1], HIGH);  //D14(A0) -> INPUT#2
  Serial.println("Forward");
}

void driveBackward()
{
  digitalWrite(rightMotorPin[0], LOW);  //D5	  -> INPUT#3
  digitalWrite(rightMotorPin[1], HIGH); //D4	  -> INPUT#4
  digitalWrite(leftMotorPin[0], HIGH);  //D15(A1) -> INPUT#1
  digitalWrite(leftMotorPin[1], LOW);   //D14(A0) -> INPUT#2
  Serial.println("Backward");
}

void turnRight()
{
  digitalWrite(rightMotorPin[0], LOW);  //D5	  -> INPUT#3
  digitalWrite(rightMotorPin[1], LOW);  //D4	  -> INPUT#4
  digitalWrite(leftMotorPin[0], LOW);   //D15(A1) -> INPUT#1
  digitalWrite(leftMotorPin[1], HIGH);  //D14(A0) -> INPUT#2
  Serial.println("Right");
}

void turnRightBackward()
{
  digitalWrite(rightMotorPin[0], LOW);  //D5	  -> INPUT#3
  digitalWrite(rightMotorPin[1], LOW);  //D4	  -> INPUT#4
  digitalWrite(leftMotorPin[0], HIGH);  //D15(A1) -> INPUT#1
  digitalWrite(leftMotorPin[1], LOW);   //D14(A0) -> INPUT#2
  Serial.println("Right Backward");
}

void turnLeft()
{
  //analogWrite(rightMotorPin[0], 1);   //D5	  -> INPUT#3 //turnLeft -> min speed
  digitalWrite(rightMotorPin[0], HIGH); //D5	  -> INPUT#3
  digitalWrite(rightMotorPin[1], LOW);  //D4	  -> INPUT#4
  digitalWrite(leftMotorPin[0], LOW);   //D15(A1) -> INPUT#1
  digitalWrite(leftMotorPin[1], LOW);   //D14(A0) -> INPUT#2
  Serial.println("Left");
}

void turnLeftBackward()
{
  digitalWrite(rightMotorPin[0], LOW);  //D5	  -> INPUT#3
  digitalWrite(rightMotorPin[1], HIGH); //D4	  -> INPUT#4
  digitalWrite(leftMotorPin[0], LOW);   //D15(A1) -> INPUT#1
  digitalWrite(leftMotorPin[1], LOW);   //D14(A0) -> INPUT#2
  Serial.println("Left Backward");
}

void turnCounterclockwise()
{
  digitalWrite(rightMotorPin[0], HIGH); //D5	  -> INPUT#3
  digitalWrite(rightMotorPin[1], LOW);  //D4	  -> INPUT#4
  digitalWrite(leftMotorPin[0], HIGH);  //D15(A1) -> INPUT#1
  digitalWrite(leftMotorPin[1], LOW);   //D14(A0) -> INPUT#2
  Serial.println("Counterclockwise");
}

void turnClockwise()
{
  digitalWrite(rightMotorPin[0], LOW);  //D5	  -> INPUT#3
  digitalWrite(rightMotorPin[1], HIGH); //D4	  -> INPUT#4
  digitalWrite(leftMotorPin[0], LOW);   //D15(A1) -> INPUT#1
  digitalWrite(leftMotorPin[1], HIGH);  //D14(A0) -> INPUT#2
  Serial.println("Clockwise");
}