/*
  Arduino | coding-help
  OLED + DC MOTOR
  January 1, 2025 at 1:55 PM

  Kramik
  I HAVE A PROBLEM WHERE THE OLED FAILS TO ALLOCATE OR BEGIN.
  WHEN I RUN THE OLED CODE ALONE. IT WORKS. BUT WHEN I PUT THE MOTOR
  CODE FOR OBSTACLE AVOIDING IT DOESNT WORK
*/

//#include <NewPing.h>
#include <Servo.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#include "FluxGarage_RoboEyes.h"

// Motor control pins
#define RIGHT_MOTOR_IN1 5   // Right motor input 1
#define RIGHT_MOTOR_IN2 4   // Right motor input 2
#define LEFT_MOTOR_IN3 7    // Left motor input 3
#define LEFT_MOTOR_IN4 6   // Left motor input 4
#define ENABLE_RIGHT 3      // PWM enable pin for right motor
#define ENABLE_LEFT 9       // PWM enable pin for left motor
#define SPEED 255  // Speed (PWM value, range 0-255)
// Ultrasonic sensor
#define TRIGGER_PIN 11  // Trigger pin for the ultrasonic sensor
#define ECHO_PIN 10     // Echo pin for the ultrasonic sensor
#define SERVO_PIN 2
#define MAX_DISTANCE 50 // Maximum distance to check obstacles in cm

Servo servo;  // Servo motor for obstacle detection
roboEyes roboEyes; // create RoboEyes instance
//NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // Initialize ultrasonic sensor

int leftDistance;   // Distance measured to the left
int rightDistance;  // Distance measured to the right
int distance;
long duration;

void setup() {
  Serial.begin(9600);
  // Set motor control pins as outputs
  pinMode(RIGHT_MOTOR_IN1, OUTPUT);
  pinMode(RIGHT_MOTOR_IN2, OUTPUT);
  pinMode(LEFT_MOTOR_IN3, OUTPUT);
  pinMode(LEFT_MOTOR_IN4, OUTPUT);
  pinMode(ENABLE_RIGHT, OUTPUT);
  pinMode(ENABLE_LEFT, OUTPUT);
  servo.attach(SERVO_PIN);
  // Startup OLED Display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C or 0x3D
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  display.setRotation(2);
  display.clearDisplay();
  /*
    display.setTextSize(1);      // Normal 1:1 pixel scale
    display.setTextColor(SSD1306_WHITE); // Draw white text
    display.setCursor(0, 0);     // Start at top-left corner
    display.print("Ready!");
    display.display();
    delay(1000);
  */
  // Startup robo eyes
  roboEyes.begin(SCREEN_WIDTH, SCREEN_HEIGHT, 100); // screen-width, screen-height, max framerate
  // Define some automated eyes behaviour
  roboEyes.setAutoblinker(ON, 3, 2); // Start auto blinker animation cycle -> bool active, int interval, int variation -> turn on/off, set interval between each blink in full seconds, set range for random interval variation in full seconds
  roboEyes.setIdleMode(ON, 2, 2); // Start idle animation cycle (eyes looking in random directions) -> turn on/off, set interval between each eye repositioning in full seconds, set range for random time interval variation in full seconds
  // initialize
  Serial.println(F("Motor and Obstacle Avoidance Test: Starting..."));
  centerServo();  // Center the servo at the start
}

void loop() {
  roboEyes.update();

  int distance = getDistance(); // Get the current distance
  Serial.print(F("Distance: "));
  Serial.println(distance);

  // Handle obstacle detection
  if (distance <= 10) { // Too close to the object
    Serial.println(F("Object too close! Backing up."));
    stopMotors();
    moveBackward();
    delay(800);
  } else if (distance <= 20) { // Object detected in safe range
    Serial.println(F("Obstacle detected! Deciding direction to avoid."));
    stopMotors();
    decideTurnDirection();
    // No obstacle detected, move forward
  } else {
    Serial.println(F("No obstacle detected. Moving forward."));
    moveForward();
  }
  delay(500); // Reduce delay for faster reaction
}

/*
  // Function to get the distance from the ultrasonic sensor
  int getDistance() {
  delay(50);
  int cm = sonar.ping_cm();
  if (cm == 0) {
  cm = 100; // Default to 100 cm if no echo received
  }
  return cm;
  }
*/

int getDistance() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.0344 / 2;
  //Serial.print("Distance: ");
  //Serial.print(distance);
  //Serial.println(" cm");
  //delay(100);
  return int(distance);
}


// Function to decide which direction to turn
void decideTurnDirection() {
  lookLeft();
  lookRight();
  delay(100);

  // Compare distances and turn in the direction with more space
  if (rightDistance <= leftDistance) {
    Serial.println(F("Turning Left"));
    turnLeft();
  } else {
    Serial.println(F("Turning Right"));
    turnRight();
  }
  delay(100);
}

// Function to look left and measure distance
void lookLeft() {
  //servo.attach(9);  // Reattach the servo for movement
  servo.write(180); // Turn servo to the left
  delay(500);
  leftDistance = getDistance(); // Measure distance
  delay(100);
  centerServo(); // Center the servo
  Serial.print(F("Left distance: "));
  Serial.println(leftDistance);
}

// Function to look right and measure distance
void lookRight() {
  //servo.attach(9);  // Reattach the servo for movement
  servo.write(0);   // Turn servo to the right
  delay(500);
  rightDistance = getDistance(); // Measure distance
  delay(100);
  centerServo(); // Center the servo
  Serial.print(F("Right distance: "));
  Serial.println(rightDistance);
}

// Motor control functions
void moveForward() {
  digitalWrite(RIGHT_MOTOR_IN1, HIGH);
  digitalWrite(RIGHT_MOTOR_IN2, LOW);
  digitalWrite(LEFT_MOTOR_IN3, HIGH);
  digitalWrite(LEFT_MOTOR_IN4, LOW);
  analogWrite(ENABLE_RIGHT, SPEED);
  analogWrite(ENABLE_LEFT, SPEED);
}

void moveBackward() {
  digitalWrite(RIGHT_MOTOR_IN1, LOW);
  digitalWrite(RIGHT_MOTOR_IN2, HIGH);
  digitalWrite(LEFT_MOTOR_IN3, LOW);
  digitalWrite(LEFT_MOTOR_IN4, HIGH);
  analogWrite(ENABLE_RIGHT, SPEED);
  analogWrite(ENABLE_LEFT, SPEED);
}

void moveLeft() {
  digitalWrite(RIGHT_MOTOR_IN1, HIGH); // Right motor forward
  digitalWrite(RIGHT_MOTOR_IN2, LOW);
  digitalWrite(LEFT_MOTOR_IN3, LOW);   // Stop left motor
  digitalWrite(LEFT_MOTOR_IN4, HIGH); // Left motor backward
  analogWrite(ENABLE_RIGHT, SPEED);
  analogWrite(ENABLE_LEFT, SPEED);
}

void moveRight() {
  digitalWrite(RIGHT_MOTOR_IN1, LOW);  // Right motor backward
  digitalWrite(RIGHT_MOTOR_IN2, HIGH);
  digitalWrite(LEFT_MOTOR_IN3, HIGH);  // Left motor forward
  digitalWrite(LEFT_MOTOR_IN4, LOW);
  analogWrite(ENABLE_RIGHT, SPEED);
  analogWrite(ENABLE_LEFT, SPEED);
}

// Turn left by moving motors accordingly
void turnLeft() {
  moveLeft();
  delay(800);  // Adjust timing as necessary
  moveForward();
  delay(300);
}

// Turn right by moving motors accordingly
void turnRight() {
  moveRight();
  delay(800);  // Adjust timing as necessary
  moveForward();
  delay(300);
}

void stopMotors() {
  digitalWrite(RIGHT_MOTOR_IN1, LOW);
  digitalWrite(RIGHT_MOTOR_IN2, LOW);
  digitalWrite(LEFT_MOTOR_IN3, LOW);
  digitalWrite(LEFT_MOTOR_IN4, LOW);
}



// Function to center the servo and then turn it off
void centerServo() {
  servo.write(90);  // Center the servo
  delay(100);
  //servo.detach();   // Detach the servo to turn it off
}