// connections with motor driver
// left motor
#define leftEnable 3 // PWM pin
#define leftIn1 2
#define leftIn2 4
// right motor
#define rightEnable 5 // PWM pin
#define rightIn1 6
#define rightIn2 7

// connections with ultrasonic sensor
// left ultrasonic sensor
#define leftTrigger 13
#define leftEcho 12
// front ultrasonic sensor
#define frontTrigger 11
#define frontEcho 10
// right ultrasonic sensor
#define rightTrigger 9
#define rightEcho 8

// variables for ultrasonic sensor
int leftDistance;
int frontDistance;
int rightDistance;

// variables for motor driver
byte leftSpeed = 200; // current motor speed when moving forward
byte rightSpeed = 200;
const byte maxSpeed = 255;

// variables for controll
const int distanceFormWall = 10; // desiered distance form the wall in cm
const float kp = 0.8; // proprotional constant
const int objectDistance = 20; // object distance to start reacting
const int delayTime = 200; // to delay after doing a action to let the boat complete that action

void setup() {

  pinMode(leftTrigger, OUTPUT);
  pinMode(leftEcho, INPUT);
  pinMode(frontTrigger, OUTPUT);
  pinMode(frontEcho, INPUT);
  pinMode(rightTrigger, OUTPUT);
  pinMode(rightEcho, INPUT);

  for (int i = 2; i <= 7; i++) {
    pinMode(i, OUTPUT);
  }

  Serial.begin(115200);

}

// function to get distance form a ultrasonic sensor
int getDistance(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  // send a 10 microsecond pulse to trigger the sensor
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  // measure the duration on HIGH on echo pin
  long duration = pulseIn(echo, HIGH);
  int distance = duration / 58; // to get the distance in cm

  return distance;
}

// function to move the boat with correction
void moveWithCorrection(int correction) {
  // add the correction to the motor speed within the constrains 0 to max
  leftSpeed = constrain(leftSpeed + correction, 0, maxSpeed);
  rightSpeed = constrain(rightSpeed - correction, 0, maxSpeed);
  Serial.println("Move forward with correction");
  Serial.print("left Speed: ");
  Serial.print(leftSpeed);
  Serial.print("    right Speed: ");
  Serial.println(rightSpeed);

  analogWrite(leftEnable, leftSpeed);
  analogWrite(rightEnable, rightSpeed);

// move both motors in the forward direction
  digitalWrite(leftIn1, HIGH);
  digitalWrite(leftIn2, LOW);
  digitalWrite(rightIn1, HIGH);
  digitalWrite(rightIn2, LOW);

}

// stop the boat
void moveStop() {
  digitalWrite(leftIn1, LOW);
  digitalWrite(leftIn2, LOW);
  digitalWrite(rightIn1, LOW);
  digitalWrite(rightIn2, LOW);
  Serial.println("Motor stop");
  delay(delayTime);
}

// turn the boat right
void turnRight() {
  analogWrite(leftEnable, maxSpeed);
  analogWrite(rightEnable, maxSpeed);
  // move left motor in the forward direction
  digitalWrite(leftIn1, HIGH);
  digitalWrite(leftIn2, LOW);
  // move right motor to the backward direction
  digitalWrite(rightIn1, LOW);
  digitalWrite(rightIn2, HIGH);  

  Serial.println("Turn right");
  delay(delayTime);
}

// turn the boat left
void turnLeft() {
  analogWrite(leftEnable, maxSpeed);
  analogWrite(rightEnable, maxSpeed);
  // move right motor in the forward direction
  digitalWrite(rightIn1, HIGH);
  digitalWrite(rightIn2, LOW);
  // move left motor to the backward direction
  digitalWrite(leftIn1, LOW);
  digitalWrite(leftIn2, HIGH);  

  Serial.println("turn left");
  delay(delayTime);
}

void loop() {

  frontDistance = getDistance(frontTrigger, frontEcho);
  // move forward if the front is clear of obstecals
  if (frontDistance > objectDistance) {
    leftDistance = getDistance(leftTrigger, leftEcho);
    // maintain a constant distance from the left wall
    int error = distanceFormWall - leftDistance;
    int correction = kp * error;
    Serial.print("Correction: ");
    Serial.println(correction);
    moveWithCorrection(correction);
  }
  else {
    // if object infront then stop and check left and right distance
    moveStop();
    leftDistance = getDistance(leftTrigger, leftEcho);
    rightDistance = getDistance(rightTrigger, rightEcho);
    if ( leftDistance > objectDistance || rightDistance > objectDistance) {
      // if  left or right is obsticales free then turn in the direction with more space 
      if (leftDistance < rightDistance) {
        turnRight();
      }
      else {
        turnLeft();
      }
    }
  }
}