#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// === IR Sensor Pins ===
#define IR_LEFT   16
#define IR_CENTER 17
#define IR_RIGHT  18

// === Ultrasonic Sensor Pins ===
#define US_FRONT_TRIG 4
#define US_FRONT_ECHO 5
#define US_LEFT_TRIG  6
#define US_LEFT_ECHO  7
#define US_RIGHT_TRIG 8
#define US_RIGHT_ECHO 9

// === Motor LED Pins (Simulated L298N) ===
#define IN1 10  // Left forward
#define IN2 11  // Left backward
#define IN3 12  // Right forward
#define IN4 13  // Right backward

// === MPU6050 Sensor ===
Adafruit_MPU6050 mpu;

void setup() {
  Serial.begin(115200);

  // === IR Sensors ===
  pinMode(IR_LEFT, INPUT);
  pinMode(IR_CENTER, INPUT);
  pinMode(IR_RIGHT, INPUT);

  // === Ultrasonic Sensors ===
  pinMode(US_FRONT_TRIG, OUTPUT);
  pinMode(US_FRONT_ECHO, INPUT);
  pinMode(US_LEFT_TRIG, OUTPUT);
  pinMode(US_LEFT_ECHO, INPUT);
  pinMode(US_RIGHT_TRIG, OUTPUT);
  pinMode(US_RIGHT_ECHO, INPUT);

  // === Motor Pins (LEDs) ===
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // === MPU6050 Init ===
  Wire.begin();  // Uses GP0 (SDA) and GP1 (SCL) on Pico
  if (!mpu.begin()) {
    Serial.println("MPU6050 not found!");
    while (1);
  }
  Serial.println("MPU6050 ready!");
}

long readUltrasonic(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000); // Timeout 30ms
  long distance = duration * 0.034 / 2;
  return distance;
}

void loop() {
  // === Read Ultrasonic Sensors ===
  int distFront = readUltrasonic(US_FRONT_TRIG, US_FRONT_ECHO);
  int distLeft  = readUltrasonic(US_LEFT_TRIG, US_LEFT_ECHO);
  int distRight = readUltrasonic(US_RIGHT_TRIG, US_RIGHT_ECHO);

  Serial.print("Ultrasonic - Front: ");
  Serial.print(distFront);
  Serial.print(" cm | Left: ");
  Serial.print(distLeft);
  Serial.print(" cm | Right: ");
  Serial.println(distRight);

  // === Obstacle Avoidance Logic ===
  if (distFront > 0 && distFront < 15) {
    stopMotors();
    delay(300);
    if (distLeft > distRight) {
      turnLeft();
    } else {
      turnRight();
    }
    delay(400);
  } else {
    lineFollowing();
  }

  // === Slope Detection from MPU6050 ===
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float zAccel = a.acceleration.z;

  Serial.print("Z-accel: ");
  Serial.print(zAccel);
  Serial.println(" m/s²");

  if (zAccel < 8.0) {
    Serial.println("Slope detected! (adjust speed)");
  }

  delay(100);
}

// === Motor LED Control ===
void forward() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void stopMotors() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void turnLeft() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void turnRight() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

// === Line Following Logic ===
void lineFollowing() {
  bool left = digitalRead(IR_LEFT) == LOW;
  bool center = digitalRead(IR_CENTER) == LOW;
  bool right = digitalRead(IR_RIGHT) == LOW;

  if (center && !left && !right) {
    forward();
  } else if (left) {
    turnLeft();
  } else if (right) {
    turnRight();
  } else {
    stopMotors(); // Line lost
  }
}