#include <Adafruit_NeoPixel.h>
#include <Servo.h>

int left_motor_pin1 = 12;
int left_motor_pin2 = 11;
int right_motor_pin1 = 10;
int right_motor_pin2 = 9;
int left_pwm_pin = 13;
int right_pwm_pin = 8;
// HC-SR04 sensor pins
int front_trig_pin = 6;
int front_echo_pin = 7;

Servo turretServo;
int servoPin = 3;

// HC-SR04 sensor constants
const float SOUND_SPEED = 343.2;
const float THRESHOLD_DISTANCE = 60.0;

const int HISTORY_SIZE = 5;

float left_distance_history[HISTORY_SIZE];
float center_distance_history[HISTORY_SIZE];
float right_distance_history[HISTORY_SIZE];
int history_index = 0;

// NeoPixel strip settings
int numPixels = 13;
int ledPin = 2;
Adafruit_NeoPixel strip = Adafruit_NeoPixel(numPixels, ledPin, NEO_GRB + NEO_KHZ800);
void setColor(uint8_t r, uint8_t g, uint8_t b) {
  for (int i = 0; i < strip.numPixels(); i++) {
    strip.setPixelColor(i, strip.Color(r, g, b));
  }
  strip.show();
}

float read_distance(int trig_pin, int echo_pin) {
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);

  long duration = pulseIn(echo_pin, HIGH);
  float distance = (duration * SOUND_SPEED) / 20000;

  // Apply a ceiling to the distance value
  if (distance > 200) {
    distance = 150;
  }

  return distance;
}

void update_history(float history[], float new_value) {
  history[history_index] = new_value;
}

float calculate_average(float history[]) {
  float sum = 0;
  for (int i = 0; i < HISTORY_SIZE; i++) {
    sum += history[i];
  }
  return sum / HISTORY_SIZE;
}



void initialize_pins() {
  // Set the motor pins as outputs
  pinMode(left_motor_pin1, OUTPUT);
  pinMode(left_motor_pin2, OUTPUT);
  pinMode(right_motor_pin1, OUTPUT);
  pinMode(right_motor_pin2, OUTPUT);
  pinMode(left_pwm_pin, OUTPUT);
  pinMode(right_pwm_pin, OUTPUT);

  // Set the HC-SR04 pins as outputs and inputs
  pinMode(front_trig_pin, OUTPUT);
  pinMode(front_echo_pin, INPUT);
}

void setup() {
  Serial.begin(9600);
  initialize_pins();
  turretServo.attach(servoPin);
  strip.begin();           // Initialize the NeoPixel strip
  strip.show();            // Turn off all pixels
  setColor(0, 0, 255);     // Set default color to blue

}

void loop() {
  // Read the distance in the center direction
  turretServo.write(90); // Center
  delay(500);
  float center_distance1 = read_distance(front_trig_pin, front_echo_pin);
  delay(100);
  float center_distance2 = read_distance(front_trig_pin, front_echo_pin);
  delay(100);
  float center_distance3 = read_distance(front_trig_pin, front_echo_pin);
  delay(100);
  float center_distance4 = read_distance(front_trig_pin, front_echo_pin);
  delay(100);
  float center_distance5 = read_distance(front_trig_pin, front_echo_pin);
  float center_avg_distance = (center_distance1 + center_distance2 + center_distance3 + center_distance4 + center_distance5) / 5.0;
  update_history(center_distance_history, center_avg_distance);

  Serial.print("Center average distance: ");
  Serial.println(center_avg_distance);
if (center_avg_distance <= 15.0) { // Check if the center distance is less than or equal to 15 cm
    drive_backward(500);
    turn_around(500); // Drive backward if the condition is met
}

  else if (center_avg_distance <= THRESHOLD_DISTANCE) {
    stop();
    delay(500); // Wait for the robot to come to a complete stop
    // Read the distances in left and right directions
    turretServo.write(45); // Left
    delay(1000);
    float left_distance1 = read_distance(front_trig_pin, front_echo_pin);
    delay(500); // Wait for the sensor to settle
    float left_distance2 = read_distance(front_trig_pin, front_echo_pin);
    delay(500); // Wait for the sensor to settle
    float left_distance3 = read_distance(front_trig_pin, front_echo_pin);
    delay(500); // Wait for the sensor to settle
    float left_distance4 = read_distance(front_trig_pin, front_echo_pin);
    delay(500); // Wait for the sensor to settle
    float left_distance5 = read_distance(front_trig_pin, front_echo_pin);
    float left_avg_distance = (left_distance1 + left_distance2 + left_distance3 + left_distance4 + left_distance5) / 5.0;

    Serial.print("Left average distance: ");
    Serial.println(left_avg_distance);

    turretServo.write(135); // Right
    delay(1000);
    float right_distance1 = read_distance(front_trig_pin, front_echo_pin);
    delay(500); // Wait for the sensor to settle
    float right_distance2 = read_distance(front_trig_pin, front_echo_pin);
    delay(500); // Wait for the sensor to settle
    float right_distance3 = read_distance(front_trig_pin, front_echo_pin);
    delay(500); // Wait for the sensor to settle
    float right_distance4 = read_distance(front_trig_pin, front_echo_pin);
    delay(500); // Wait for the sensor to settle
    float right_distance5 = read_distance(front_trig_pin, front_echo_pin);
    float right_avg_distance = (right_distance1 + right_distance2 + right_distance3 + right_distance4 + right_distance5) / 5.0;

    Serial.print("Right average distance: ");
    Serial.println(right_avg_distance);

    turretServo.write(90); // Reset servo to the center position
    delay(1000);

  // Choose the direction with the greatest average distance
  if (left_avg_distance > (THRESHOLD_DISTANCE / 2) && left_avg_distance > right_avg_distance) {
    drive_backward(200);
    turn_left(400);
  } else if (right_avg_distance > (THRESHOLD_DISTANCE / 2) {
    drive_backward(200);
    turn_right(400);
  } else {
    // All three average distances are below the threshold
    drive_backward(500);
    turn_around(500);
  }
 else {
  drive_forward();
}
  //history_index = (history_index + 1) % HISTORY_SIZE;
  //delay(50);
}



void drive_forward() {
  Serial.println("Driving forward");
  digitalWrite(left_motor_pin1, LOW);
  digitalWrite(left_motor_pin2, HIGH);
  digitalWrite(right_motor_pin1, HIGH);
  digitalWrite(right_motor_pin2, LOW);

  analogWrite(left_pwm_pin, 140);
  analogWrite(right_pwm_pin, 140);
  delay(50);
  setColor(0, 0, 255);
}

void drive_backward(int duration) {
  Serial.println("Driving backward");
  digitalWrite(left_motor_pin1, HIGH);
  digitalWrite(left_motor_pin2, LOW);
  digitalWrite(right_motor_pin1, LOW);
  digitalWrite(right_motor_pin2, HIGH);

  analogWrite(left_pwm_pin, 140);
  analogWrite(right_pwm_pin, 140);
  delay(duration);
}

void turn_left(int duration) {
  Serial.println("Turning left");
  digitalWrite(left_motor_pin1, LOW);
  digitalWrite(left_motor_pin2, LOW);
  digitalWrite(right_motor_pin1, HIGH);
  digitalWrite(right_motor_pin2, LOW);

  analogWrite(left_pwm_pin, 140);
  analogWrite(right_pwm_pin, 140);
  setColor(0, 255, 0);
  delay(duration);
  
}

void turn_right(int duration) {
  Serial.println("Turning right");
  digitalWrite(left_motor_pin1, LOW);
  digitalWrite(left_motor_pin2, HIGH);
  digitalWrite(right_motor_pin1, LOW);
  digitalWrite(right_motor_pin2, LOW);

  analogWrite(left_pwm_pin, 140);
  analogWrite(right_pwm_pin, 140);
  setColor(255, 0, 0);
  delay(duration);
}

void turn_around(int duration) {
    Serial.println("Turning Around");
  digitalWrite(left_motor_pin1, LOW);
  digitalWrite(left_motor_pin2, HIGH);
  digitalWrite(right_motor_pin1, LOW);
  digitalWrite(right_motor_pin2, HIGH);

  analogWrite(left_pwm_pin, 140);
  analogWrite(right_pwm_pin, 140);
  setColor(255, 255, 255);
  delay(duration);
}

void stop() {
  digitalWrite(left_motor_pin1, LOW);
  digitalWrite(left_motor_pin2, LOW);
  digitalWrite(right_motor_pin1, LOW);
  digitalWrite(right_motor_pin2, LOW);
  delay(500); // Wait for 50 milliseconds before resuming
}