// 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;

// constants for controll
const int distanceFromWall = 10; // desiered distance form the wall in cm
const int wallDistance = 20; // wall distance to start reacting
const int obstacleDistance = 30; // obstace distance
const int delayTime = 200; // to delay after doing a action to let the boat complete that action

// PID Controll variables
const float kp = 0.8; // proprotional constant
//const float ki = 0.1; // integral constant
//const float kd = 0.5; // derivative constant
int error = 0;
int correction = 0;
// int previousError = 0;
// int integral = 0;
// int derivative = 0;

// variables for controll
bool followLeft = true;
bool obstacleStateNew;
bool obstacleStateOld = false;

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);
// 
  if (frontDistance <= obstacleDistance) {
    obstacleStateNew = true;
  }
  else {
    obstacleStateNew = false;
  }
// if new obstacle then change the wall that will be followed
  if (obstacleStateOld == false && obstacleStateNew == true) {
    followLeft = !followLeft;
  }
  obstacleStateOld = obstacleStateNew;

  if (frontDistance > wallDistance) {
    if (followLeft) {
      leftDistance = getDistance(leftTrigger, leftEcho);
      // maintain a constant distance from the left wall
      error = distanceFromWall - leftDistance;
      // integral += error; // Accumulate error
      // derivative = error - previousError;
      // previousError = error;
      // int correction = (Kp * error) + (Ki * integral) + (Kd * derivative);
      correction = kp * error;
      Serial.print("Correction to move left: ");
      Serial.println(correction);
      moveWithCorrection(correction);
    }
    else {
      rightDistance = getDistance(rightTrigger, rightEcho);
      // maintain a constant distance from the right wall
      error = rightDistance - distanceFromWall;
      // integral += error; // Accumulate error
      // derivative = error - previousError;
      // previousError = error;
      // int correction = (Kp * error) + (Ki * integral) + (Kd * derivative);
      correction = kp * error;
      Serial.print("Correction to move right: ");
      Serial.println(correction);
      moveWithCorrection(correction);
    }

  }
  else {
    // if wall infront then stop and check left and right distance
    moveStop();
    leftDistance = getDistance(leftTrigger, leftEcho);
    rightDistance = getDistance(rightTrigger, rightEcho);
    if ( leftDistance > wallDistance || rightDistance > wallDistance) {
      // if  left or right is obsticales free then turn in the direction with more space
      if (leftDistance < rightDistance) {
        turnRight();
      }
      else {
        turnLeft();
      }
    }
  }
}