#include <Servo.h>
#include <PID_v1.h>
#include <NewPing.h>

// LDR Sensor
const int LDR_PIN = A0;
const int BOARD_RESOLUTION = 1024;

// Ultrasonic Sensor
#define TRIG_PIN A1
#define ECHO_PIN A2
#define MAX_DISTANCE 200
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// Steering Servo
Servo steeringServo;
const int STEERING_PIN = 10;

// DC Motors (simulated with Servos)
Servo leftFrontMotor, rightFrontMotor, leftRearMotor, rightRearMotor;
const int LEFT_FRONT_PIN = 3;
const int RIGHT_FRONT_PIN = 5;
const int LEFT_REAR_PIN = 6;
const int RIGHT_REAR_PIN = 9;

// PID for steering
double Setpoint, Input, Output;
double Kp = 2.0, Ki = 5.0, Kd = 1.0;
PID steeringPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

int distance = 100;
boolean goesForward = false;

void setup() {
  Serial.begin(9600);
  
  // LDR setup
  pinMode(LDR_PIN, INPUT);
  
  // Steering Servo setup
  steeringServo.attach(STEERING_PIN);
  steeringServo.write(90);  // Center position
  
  // DC Motors setup (simulated with Servos)
  leftFrontMotor.attach(LEFT_FRONT_PIN);
  rightFrontMotor.attach(RIGHT_FRONT_PIN);
  leftRearMotor.attach(LEFT_REAR_PIN);
  rightRearMotor.attach(RIGHT_REAR_PIN);
  
  // PID setup
  Setpoint = 90;  // Center position
  Input = steeringServo.read();
  steeringPID.SetMode(AUTOMATIC);
  steeringPID.SetOutputLimits(0, 180);
  
  delay(2000);
}

void loop() {
  // LDR reading
  int ldrValue = analogRead(LDR_PIN);
  if (ldrValue < BOARD_RESOLUTION / 2) {
    Serial.println("It is DARK!");
  } else {
    Serial.println("It is LIGHT!");
  }
  
  // Ultrasonic reading
  distance = sonar.ping_cm();
  if (distance == 0) distance = MAX_DISTANCE;
  Serial.print("Distance: ");
  Serial.println(distance);
  
  // Obstacle avoidance logic
  if (distance <= 20) {
    moveStop();
    delay(300);
    moveBackward();
    delay(400);
    moveStop();
    delay(300);
    int distanceRight = lookRight();
    delay(300);
    int distanceLeft = lookLeft();
    delay(300);
    
    if (distanceRight >= distanceLeft) {
      turnRight();
    } else {
      turnLeft();
    }
  } else {
    moveForward();
  }
  
  // PID control for steering
  Input = steeringServo.read();
  steeringPID.Compute();
  steeringServo.write(Output);
  
  delay(50);
}

void moveStop() {
  setMotorSpeed(90, 90, 90, 90);  // 90 is stop for Servo
}

void moveForward() {
  if (!goesForward) {
    goesForward = true;
    setMotorSpeed(180, 180, 180, 180);  // Full speed forward
  }
}

void moveBackward() {
  goesForward = false;
  setMotorSpeed(0, 0, 0, 0);  // Full speed backward
}

void turnRight() {
  setMotorSpeed(180, 0, 180, 0);
  delay(500);
  moveForward();
}

void turnLeft() {
  setMotorSpeed(0, 180, 0, 180);
  delay(500);
  moveForward();
}

void setMotorSpeed(int lf, int rf, int lr, int rr) {
  leftFrontMotor.write(lf);
  rightFrontMotor.write(rf);
  leftRearMotor.write(lr);
  rightRearMotor.write(rr);
}

int lookRight() {
  steeringServo.write(50);
  delay(500);
  int distance = sonar.ping_cm();
  delay(100);
  steeringServo.write(90);
  return distance;
}

int lookLeft() {
  steeringServo.write(130);
  delay(500);
  int distance = sonar.ping_cm();
  delay(100);
  steeringServo.write(90);
  return distance;
}