#include "RTClib.h"
RTC_DS3231 rtc;
char daysOfTheWeek[7][12] = { "Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday" };
//set time for auto mode
const int setHour = 8;
int currentHour;

const int buttonPin = 11;  // the pin that the pushbutton is attached to
const int ledPin1 = 13;    // the pin that the LED is attached to
const int ledPin2 = 12;
int buttonPushCounter = 0;  // counter for the number of button presses
int buttonState = 0;        // current state of the button
int lastButtonState = 0;    // previous state of the button

//Ultrasonics
//Front
const int echo1 = 23;
const int trig1 = 22;
long dur1;
int frontDist;

//Back
const int echo2 = 25;
const int trig2 = 24;
long dur2;
int backDist;

//Left
const int echo3 = 27;
const int trig3 = 26;
long dur3;
int leftDist;

//Right
const int echo4 = 29;
const int trig4 = 28;
long dur4;
int rightDist;

//Mop
const int echo5 = 31;
const int trig5 = 30;
long dur5;
int mopDist;

//Vac
const int echo6 = 33;
const int trig6 = 32;
long dur6;
int vacDist;

//buzzer
const int buzzer = 10;

//var
const int d = 40;
const int mopWaterLevel = 10;
const int garbageLevel = 3;

void setup() {
  // initialize serial communication:
  Serial.begin(115200);
  // initialize the button pin as a input:
  pinMode(buttonPin, INPUT);
  // initialize the LED as an output:
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);

  //Ultrasonic Pins
  pinMode(trig1, OUTPUT);  //u-1
  pinMode(echo1, INPUT);   //u-1
  pinMode(trig2, OUTPUT);  //u-2
  pinMode(echo2, INPUT);   //u-2
  pinMode(trig3, OUTPUT);  //u-3
  pinMode(echo3, INPUT);   //u-3
  pinMode(trig4, OUTPUT);  //u-4
  pinMode(echo4, INPUT);   //u-4
  pinMode(trig5, OUTPUT);  //u-5
  pinMode(echo5, INPUT);   //u-5
  pinMode(trig6, OUTPUT);  //u-6
  pinMode(echo6, INPUT);   //u-6

  if (!rtc.begin()) {
    Serial.println("Couldn't find RTC");
    Serial.flush();
    while (1) delay(10);
    if (rtc.lostPower()) {
      Serial.println("RTC lost power, let's set the time!");
      // When time needs to be set on a new device, or after a power loss, the
      // following line sets the RTC to the date & time this sketch was compiled
      rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
      // This line sets the RTC with an explicit date & time, for example to set
      // January 21, 2014 at 3am you would call:
      // rtc.adjust(DateTime(2014, 1, 21, 3, 0, 0));
    }

  }
}

void loop() {
  // read the pushbutton input pin:
  digitalWrite(ledPin1, LOW);
  digitalWrite(ledPin2, LOW);
  delay(250);
  buttonState = digitalRead(buttonPin);

  // compare the buttonState to its previous state
  if (buttonState != lastButtonState) {
    // if the state has changed, increment the counter
    if (buttonState == HIGH) {
      // if the current state is HIGH then the button went from off to on:
      buttonPushCounter++;
      Serial.print("Number of button pushes: ");
      Serial.println(buttonPushCounter);
      Serial.println("Button 1 on");
    } else {
      // if the current state is LOW then the button went from on to off:
      Serial.println("Button 1 off");
    }
    // Delay a little bit to avoid bouncing
    delay(50);
  }
  // save the current state as the last state, for next time through the loop
  lastButtonState = buttonState;

  if (buttonPushCounter > 0)
  {
    while (buttonPushCounter % 2 == 0) {
      Serial.println("Auto Mode Selected...");
      digitalWrite(ledPin1, HIGH);
      digitalWrite(ledPin2, LOW);
      DateTime now = rtc.now();
      Serial.print("Date: ");
      Serial.print(now.year(), DEC);
      Serial.print('/');
      Serial.print(now.month(), DEC);
      Serial.print('/');
      Serial.println(now.day(), DEC);

      Serial.print("Day: ");
      Serial.println(daysOfTheWeek[now.dayOfTheWeek()]);

      Serial.print("Time: ");
      currentHour = now.hour();
      Serial.print(currentHour, DEC);
      Serial.print(':');
      Serial.print(now.minute(), DEC);
      Serial.print(':');
      Serial.print(now.second(), DEC);
      Serial.println();
      Serial.println();
      delay(1000);
    }
    while (buttonPushCounter % 2 != 0) {
      Serial.println("Manual Mode Selected...");
      digitalWrite(ledPin1, LOW);
      digitalWrite(ledPin2, HIGH);

      getFrontDist();
      getMopDist();
      getVacDist();

      while (mopDist > mopWaterLevel || vacDist < garbageLevel)
      {
        Serial.println("Low Water Level... OR Garbage full");
        stockAlert();
        stopVaccumNmop();
        stopRobot();
        getMopDist();
        getVacDist();
      }

      if (frontDist > d) {
        startVaccum();
        startMop();
        while (frontDist > d)
        {
          getFrontDist();
          forward();

          if (frontDist < d)
          {
            Serial.println("frontDist < 10...");
            stopRobot();
            break;
          }
        }
      }
      if (frontDist < d)
      {
        stopRobot();
        Serial.println("Finding another way...");
        getBackDist();
        getLeftDist();
        getRightDist();

        if (leftDist > d)
        {
          Serial.println("Can not move Froward X ^^^ X");
          Serial.println("Looking Left  <<<");
          turnLeft();
          stopRobot();
        }
        else if (rightDist > d)
        {
          Serial.println("Can not move Left X <<< X");
          Serial.println("Looking Right  >>>");
          turnRight();
          stopRobot();
        }
        else if (backDist > d)
        {
          Serial.println("Can not move Right X >>> X");
          Serial.println("Looking Back  vvv");
          backward();
          delay(5000);
          stopRobot();
        }
        else {
          Serial.println("No way to go...XXX");
          movementStop();
          stopVaccumNmop();
          stopRobot();
        }
      }
    }
  }
  else {
    Serial.println("To Select Mode Press Button...");
    Serial.println("->  Press 1 time for Manual Mode");
    Serial.println("->  Press 2 times for Auto Mode");
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, HIGH);
    Serial.println();
  }
  Serial.println();
  delay(250);
}
void startVaccum()
{
  Serial.println("Vac Started...");
  digitalWrite(47, HIGH); //vac
}
void startMop()
{
  Serial.println("Mop Started...");
  digitalWrite(46, HIGH); //mop water
  digitalWrite(45, HIGH); //mop rotate
}
void stopVaccumNmop() {
  Serial.println("Vaccum and Mop Stopped...");
  digitalWrite(47, LOW);
  digitalWrite(45, LOW);
  digitalWrite(46, LOW);
}
void movementStop() {
  Serial.println("Buzzer: Robot Stopped - Wait 10s");
  tone(buzzer, 1000);  //Send 1KHz sound signal
  delay(10000);        //for 1 sec
}

void stockAlert() {
  Serial.println("Buzzer: Stock Alert - Wait 8s");
  //Stock Alert
  for (int i = 0; i <= 5; i++) {
    noTone(buzzer);
    tone(buzzer, 440);
    delay(1000);
    noTone(buzzer);
    delay(500);
  }
}
void stopRobot()
{
  Serial.println("Stopping Robot   ---");
  digitalWrite(48, LOW); //w1
  digitalWrite(49, LOW); //w2
  digitalWrite(50, LOW);  //w3
  digitalWrite(51, LOW);  //w4

}
void forward()
{
  Serial.println("Moving Forward...   ^^^");
  digitalWrite(48, HIGH); //w1
  digitalWrite(49, HIGH); //w2
  digitalWrite(50, LOW);  //w3
  digitalWrite(51, LOW);  //w4
}
void backward()
{
  Serial.println("Moving Backward...    vvv");
  digitalWrite(48, LOW); //w1
  digitalWrite(49, LOW); //w2
  digitalWrite(50, HIGH);  //w3
  digitalWrite(51, HIGH);  //w4
}
void turnLeft()
{
  Serial.println("Turning Left...     <<<");
  digitalWrite(48, LOW); //w1
  digitalWrite(49, HIGH); //w2
  digitalWrite(50, LOW);  //w3
  digitalWrite(51, HIGH);  //w4
}
void turnRight()
{
  Serial.println("Turning Right...      >>>");
  digitalWrite(48, HIGH); //w1
  digitalWrite(49, LOW); //w2
  digitalWrite(50, HIGH);  //w3
  digitalWrite(51, LOW);  //w4
}

void getFrontDist()
{
  //Front Dist
  digitalWrite(trig1, LOW);
  delayMicroseconds(2);
  digitalWrite(trig1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig1, LOW);
  dur1 = pulseIn(echo1, HIGH);
  frontDist = dur1 * 0.034 / 2;
  Serial.print("Front Dist: ");
  Serial.print(frontDist);
  Serial.println(" cm");
}
void getBackDist()
{
  //back dist
  digitalWrite(trig2, LOW);
  delayMicroseconds(2);
  digitalWrite(trig2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig2, LOW);
  dur2 = pulseIn(echo2, HIGH);
  backDist = dur2 * 0.034 / 2;
  Serial.print("back Dist: ");
  Serial.print(backDist);
  Serial.println(" cm");
}
void getLeftDist()
{
  //left dist
  digitalWrite(trig3, LOW);
  delayMicroseconds(2);
  digitalWrite(trig3, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig3, LOW);
  dur3 = pulseIn(echo3, HIGH);
  leftDist = dur3 * 0.034 / 2;
  Serial.print("left Dist: ");
  Serial.print(leftDist);
  Serial.println(" cm");
}
void getRightDist()
{
  //right dist
  digitalWrite(trig4, LOW);
  delayMicroseconds(2);
  digitalWrite(trig4, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig4, LOW);
  dur4 = pulseIn(echo4, HIGH);
  rightDist = dur4 * 0.034 / 2;
  Serial.print("right Dist: ");
  Serial.print(rightDist);
  Serial.println(" cm");
}
void getMopDist()
{ //mop dist
  digitalWrite(trig5, LOW);
  delayMicroseconds(2);
  digitalWrite(trig5, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig5, LOW);
  dur5 = pulseIn(echo5, HIGH);
  mopDist = dur5 * 0.034 / 2;
  Serial.print("mop Dist: ");
  Serial.print(mopDist);
  Serial.println(" cm");
}
void getVacDist()
{ //vac dist
  digitalWrite(trig6, LOW);
  delayMicroseconds(2);
  digitalWrite(trig6, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig6, LOW);
  dur6 = pulseIn(echo6, HIGH);
  vacDist = dur6 * 0.034 / 2;
  Serial.print("vac Dist: ");
  Serial.print(vacDist);
  Serial.println(" cm");
  delay(1000);
  Serial.println();
}