#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <RTClib.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);
RTC_DS3231 rtc;
byte battery[8] = {
  B01110,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111
};

enum Mode { OFF, RF_FOLLOW, AUTONOMOUS, CRASH };
enum Mood { Normal, Sleepy, Sad, Energetic, Happy, Silly, Asleep };

Mode currentMode = AUTONOMOUS;
Mood currentMood = Normal;
String mode= "Auto";
String mood = "Normal";

bool deepSleep = false;
unsigned long lastMoodUpdate = 0;
unsigned long lastDisplayUpdate = 0;
unsigned long sleepyStartMillis = 0;
DateTime sleepyStartTime;

const int leftRF = 7;
const int rightRF = 6;
const unsigned long Duration = 200;
const int averagingCycles = 3;
const int pulseTolerance = 10;               
const int ldrPin = A3, battSensePin = A2;
const int LeftMotorForward = 13;
const int LeftMotorBackward = 12;
const int RightMotorForward = 9;
const int RightMotorBackward = 8;
const int enA = 10;
const int enB = 11;

const int trigPin = 3; 
const int echoPin = 2;

int distance = 0;
 
int motorSpeed = 180;

void setup() {
  Serial.begin(9600);
  lcd.init(); 
  lcd.backlight();
  rtc.begin();
  
  pinMode(RightMotorForward, OUTPUT);
  pinMode(LeftMotorForward, OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);

  pinMode(leftRF, INPUT);
  pinMode(rightRF, INPUT);
  
  pinMode (trigPin, OUTPUT);// Same as above, the TRIG pin will send the ultrasonic wave.
  pinMode (echoPin, INPUT); // The ECHO pin will recieve the rebounded wave, so it must be an input type.
  distance = rd();
  lcd.createChar(0, battery);
  updateMood();
  updateDisplay();
}

void loop() {
  handleBluetooth();
  handleMode();
  handleMood();
  handleDisplay();
}

void handleBluetooth() {
  if (Serial.available()) {
    char cmd = Serial.read();
    switch (cmd) {
      case 'R': currentMode = RF_FOLLOW; mode="Radio"; break;
      case 'X': currentMode = OFF; mode="Off"; break;
      case 'A': currentMode = AUTONOMOUS; mode="Auto"; break;
      case 'C': currentMode = CRASH; mode="Crash"; break;
    }  }
}

void handleMode() {
  checkDeepSleepWake();
  if (deepSleep || currentMode == OFF) {
    moveStop();
    return;
  }

  if (currentMode == RF_FOLLOW) rfFollowLogic();
  else if (currentMode == AUTONOMOUS) autonomousLogic();
  else if (currentMode == CRASH) crashLogic();
}

void handleMood() {
  if (millis() - lastMoodUpdate > 300000) {
    updateMood();
    lastMoodUpdate = millis();
  }
}
int readLightLevel() {
  return analogRead(ldrPin);
}

void updateMood() {
  int lightLevel = readLightLevel();
  DateTime now = rtc.now();
  
  float batt = readBattery();

  if (!deepSleep && lightLevel < 200) {
    if (now.hour() >= 22 || now.hour() < 7) {
      if (currentMood != Sleepy) sleepyStartMillis = millis();
      currentMood = Sleepy;
      mood = "Sleepy";
      if (millis() - sleepyStartMillis >= 1800000) {
      deepSleep = true;
      sleepyStartTime = now;
      currentMood = Asleep;
      mood = "Asleep";
      motorSpeed = 120;
    }
    } else {
      currentMood = Sad;
      mood = "Sad";
      motorSpeed = 140;
    }
    return;
  }
  else if (!deepSleep && (((batt < 10) && (batt <= 30)) && (now.hour() >= 22 || now.hour() < 8))) {
    if (currentMood != Sleepy) sleepyStartMillis = millis();
    currentMood = Sleepy;
    mood = "Sleepy";
    motorSpeed = 120;
    if (millis() - sleepyStartMillis >= 1800000) {
      deepSleep = true;
      sleepyStartTime = now;
      currentMood = Asleep;
      mood = "Asleep";
    }
  }else if (!deepSleep && batt > 80 || (now.hour() >= 10 && now.hour() < 13)) {
    currentMood = Energetic;
    mood = "Energetic";
    motorSpeed = 225;
  } else if (!deepSleep && (now.hour() >= 9 && now.hour() < 15)) {
    currentMood = Happy;
    mood = "Happy";
    motorSpeed = 200;
  } else if(!deepSleep && (now.hour() >= 22 || now.hour() < 9)){
    currentMood = Normal;
    mood = "Normal";
    motorSpeed = 180;
  } else {
    currentMood = (random(0,10) >= 7) ? (random(0,10) >= 7) ? Happy : Silly : Normal;
    mood = (currentMood==Silly || currentMood == Happy) ? (currentMood == Happy) ? "Happy" : "Silly" : "Normal";
    motorSpeed = (currentMood == Silly) ? random(190,220) : 180;
  }
}

void handleDisplay() {
  if (millis() - lastDisplayUpdate > 10000) {
    updateDisplay();
    lastDisplayUpdate = millis();
  }
}

void updateDisplay() {
  lcd.clear();
  DateTime now = rtc.now();
  lcd.setCursor(0, 0); lcd.print(now.hour()); lcd.print(":");
  if (now.minute() < 10) lcd.print("0"); lcd.print(now.minute());
  int x=15;
  if(readBattery<100)x=16;
  else if(readBattery()<10)x=17;
  lcd.setCursor(x, 0);lcd.write(byte(0));
  lcd.print((int)readBattery()); lcd.print("%");
  drawExpression();
  lcd.setCursor(0, 3); lcd.print(mood);
  lcd.setCursor(15, 3); lcd.print(mode);
}

void drawExpression() {
  if(currentMood == Asleep){
    int y = random(0,100);
    if(y>=50){
      lcd.setCursor(8,1);lcd.print("Yawn");
      lcd.setCursor(8,2);lcd.print("....");
      delay(random(2000,5000));
    }
  }
  switch(currentMood) {
    case Sleepy: lcd.setCursor(8,1);lcd.print("-"); lcd.setCursor(10,1);lcd.print("-");lcd.setCursor(9,2);lcd.print("-"); break;
    case Sad: lcd.setCursor(8,1);lcd.print("T"); lcd.setCursor(10,1);lcd.print("T");lcd.setCursor(9,2);lcd.print("-"); break;
    case Energetic: lcd.setCursor(8,1);lcd.print("O"); lcd.setCursor(10,1);lcd.print("O");lcd.setCursor(9,2);lcd.print("-"); break;
    case Happy: lcd.setCursor(8,1);lcd.print("^"); lcd.setCursor(10,1);lcd.print("^");lcd.setCursor(9,2);lcd.print("-"); break;
    case Silly: lcd.setCursor(8,1);lcd.print("o"); lcd.setCursor(10,1);lcd.print("O");lcd.setCursor(9,2);lcd.print("u"); break;
    case Asleep: lcd.setCursor(8,1);lcd.print("-"); lcd.setCursor(10,1);lcd.print("-");lcd.setCursor(9,2);lcd.print("."); break;
    default: lcd.setCursor(8,1);lcd.print("o"); lcd.setCursor(10,1);lcd.print("o");lcd.setCursor(9,2);lcd.print("_"); break;
  }
}

void checkDeepSleepWake() {
  if (deepSleep) {
    DateTime now = rtc.now();
    TimeSpan sleepDuration = now - sleepyStartTime;
    if ((readBattery() > 30) || (sleepDuration.totalseconds() >= 21600)) {
      deepSleep = false;
      sleepyStartMillis = 0;
    }
  }
}

float readBattery() {
  int raw = analogRead(battSensePin);
  float voltage = ((raw / 1023.0)*5.0)*2.6;
  float batteryMin = 10.5;
  float batteryMax = 12.6;
  voltage = constrain(voltage, batteryMin, batteryMax);
  float Percent = ((voltage - batteryMin) / (batteryMax - batteryMin)) * 100.0;
  Percent = constrain(Percent, 0, 100);
  return Percent;
}

void rfFollowLogic() {
  int totalLeftPulses = 0;
  int totalRightPulses = 0;
  obstacleavoid();
  // Averaging loop
  for (int i = 0; i < averagingCycles; i++) {
    totalLeftPulses += countPulses(leftRF, Duration);
    totalRightPulses += countPulses(rightRF, Duration);
    delay(20); // Small delay between samples
  }

  // Calculate averages
  int avgLeft = totalLeftPulses / averagingCycles;
  int avgRight = totalRightPulses / averagingCycles;
  int diff = avgLeft - avgRight;
  // Decision logic
  if (abs(diff) < pulseTolerance) {
    moveForward();
    delay(500);
    moveStop();
  } else if (diff > pulseTolerance) {
    turnLeft();
    delay(500);
    moveStop();
  } else {
    turnRight();
    delay(500);
    moveStop();
  }
  delay(100);

}

int countPulses(int pin, unsigned long duration) {
  unsigned long start = millis();
  int count = 0;
  while (millis() - start < duration) {
    if (digitalRead(pin) == HIGH) {
      count++;
      delayMicroseconds(200); // De-bounce and sample timing
    }
  }
  return count;
}

void autonomousLogic() {
  obstacleavoid();
  moveForward();
  if(currentMood == Silly){
    delay(500);
    int r=random(0,100);
    if(r>=50){
      int y=random(0,100);
      if(y>=50){
        moveStop();
        turnRight();
        delay(5000);
        moveStop();
      }
      else{
        moveStop();
        turnLeft();
        delay(5000);
        moveStop();
      }
    }
  }
}

void crashLogic() {
  obstaclecrash();
}
void obstacleavoid(){
  int distanceRight = 0;
  int distanceLeft = 0;
  distance = rd();
  
  while (distance < 15){
    moveStop();
    delay(300);
    moveBackward();
    delay(500);
    moveStop();
    delay(300);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);

    if (distance < distanceRight){
      turnRight();
      delay(500);
      moveStop();
      delay(300);
      break;
    }
    else if (distance < distanceLeft){
      turnLeft();
      delay(500);
      moveStop();
      delay(300);
      break;
    }
    distance = rd();
  }
}

void obstaclecrash(){
  int distanceRight = 0;
  int distanceLeft = 0;
  distance = rd();
  if (distance > 30){
    moveStop();
    delay(300);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);

    if (distance <= distanceLeft){
      turnRight();
      moveStop();
    }
    else if (distance <= distanceRight){
      turnLeft();
      moveStop();
    }
  }
  moveForward();
}

int rd(){  
  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  long duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  int d = (duration * 0.034 / 2);
  return d;
}
int lookRight(){ 
  turnRight();
  delay(500);
  moveStop();
  int dist = rd();
  delay(500);
  turnLeft();
  delay (500);
  moveStop();
  delay(100);
  return dist;
}

int lookLeft(){  
  turnLeft();
  delay(500);
  moveStop();
  int dist = rd();
  delay(500);
  turnRight();
  delay (500);
  moveStop();
  delay(100);
  return dist;
}

void moveStop(){
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(enA,0);
  digitalWrite(enB,0);
}

void moveForward(){
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW); 
  digitalWrite(enA,motorSpeed);
  digitalWrite(enB,motorSpeed);
}

void moveBackward(){
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(enA,motorSpeed);
  digitalWrite(enB,motorSpeed);
}

void turnRight(){
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(enA,225);
  digitalWrite(enB,225);
}

void turnLeft(){
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(enA,225);
  digitalWrite(enB,225);
}