// Motor Driver Pins.
#define PWM_A_PIN 5
#define PWM_B_PIN 10
#define IN_A_PIN 6
#define IN_B_PIN 9

#define LeftBaseSpeed 65 // Base Left speed for forward speed when line is centered.
#define RightBaseSpeed 55 // Base Right speed for forward speed when line is centered.

// Wall Sensors Pins.
#define LEFT_SENSOR A3
#define FORWARD_SENSOR A6
#define RIGHT_SENSOR A7

int RightMotorSpeed = 0;

int LeftMotorSpeed = 0;
int SpeedUpdate = 0;


int minus = -15;
int baseLeft = 80-minus;
int baseForward = 110-minus;
int baseRight = 74-minus;

int minLeft = 40, maxLeft = 160;
int minRight = 26, maxRight = 160;

int sensorValues[3];

int nonWallLeft = 0;
int nonWallRight = 0;

bool isLeftWall = true;
bool isRightWall = true;

int Error = 0;
int LastError = 0;

// P and D koefs.
float Kp = 2.3;
float Kd = 10;
float turningKp = 0.8;
float truningKd = 20;

void setup() {
  pinMode(IN_A_PIN, OUTPUT);
  pinMode(IN_B_PIN, OUTPUT);

  baseLeft = map(baseLeft, minLeft, maxLeft, 0, 100);
  baseRight = map(baseRight, minRight, maxRight, 0, 100);

  nonWallLeft = minLeft - 20;
  nonWallRight = minRight - 15;

  Serial.begin(9600);
}

void loop() {
  delay(10);
  isLeftWall = true;
  isRightWall = true;

  if (analogRead(LEFT_SENSOR) <= nonWallLeft) {
    isLeftWall = false;
  }

  if (analogRead(RIGHT_SENSOR) <= nonWallRight) {
    isRightWall = false;
  }

  sensorValues[0] = map(analogRead(LEFT_SENSOR), minLeft, maxLeft, 0, 100);
  sensorValues[1] = analogRead(FORWARD_SENSOR);
  sensorValues[2] = map(analogRead(RIGHT_SENSOR), minRight, maxRight, 0, 100);

  Serial.print(sensorValues[0]);Serial.print('\t');
  Serial.print(sensorValues[1]);Serial.print('\t');
  Serial.print(sensorValues[2]);Serial.print('\n');
  
  // 🛑 ПЕРЕВІРКА НА ТУПИК (стіну попереду)
  if (sensorValues[1] > 100 and sensorValues[0]>10 and sensorValues[2]>-10) {
    // їхати назад
    int rotateSpeed = 125;
    digitalWrite(6, LOW);
    analogWrite(5, ( 255 - rotateSpeed ));
    digitalWrite(10, LOW);
    analogWrite(9, ( 255 - rotateSpeed ));
    Serial.println("тупик");
    delay(100);
    return; // пропускаємо звичайну поведінку
  }

  // Звичайна поведінка
  calculateError();

  float p = isRightWall && isLeftWall ? Kp : turningKp;
  float d = isRightWall && isLeftWall ? Kd : truningKd;

  SpeedUpdate = p * Error + d * (Error - LastError);
  LastError = Error;

  speedUpdate();
  motorControl(RightMotorSpeed, LeftMotorSpeed);
}

// Sets speed for each motor.
void motorControl(int RightMotorSpeed, int LeftMotorSpeed) {
  if (RightMotorSpeed <= 0) {
    RightMotorSpeed = abs(RightMotorSpeed);
    analogWrite(PWM_B_PIN, 255 - RightMotorSpeed);
    digitalWrite(IN_B_PIN, HIGH);
  }
  else {
    analogWrite(PWM_B_PIN, RightMotorSpeed);
    digitalWrite(IN_B_PIN, LOW);
  }

  if (LeftMotorSpeed <= 0) {
    LeftMotorSpeed = abs(LeftMotorSpeed);
    analogWrite(PWM_A_PIN, 255 - LeftMotorSpeed);
    digitalWrite(IN_A_PIN, HIGH);
  }
  else {
    analogWrite(PWM_A_PIN, LeftMotorSpeed);
    digitalWrite(IN_A_PIN, LOW);
  }
}

// calculates Error value.
void calculateError() {
  Error = 0;
  if (isLeftWall && isRightWall) {
    Error = (sensorValues[0] - baseLeft) + (sensorValues[2] - baseRight)  * (-1);
  } else if (isLeftWall) {
    if (sensorValues[1] > baseForward) {
      Error = (sensorValues[0] - baseLeft) + (sensorValues[1] - baseForward);
    } else {
      Error = (sensorValues[0] - baseLeft);
    }
  } else if (isRightWall) {
    if (sensorValues[1] > baseForward) {
      Error = (sensorValues[2] - baseRight) * (-1) + (sensorValues[1] - baseForward) * (-1);
    } else {
      Error = (sensorValues[2] - baseRight) * (-1);
    }
  }
}

// Updates motors speed on SpeedUpdate value.
void speedUpdate() {
  RightMotorSpeed = RightBaseSpeed - SpeedUpdate;
  LeftMotorSpeed = LeftBaseSpeed + SpeedUpdate;

  // Limiting Min Max PWM values.
  RightMotorSpeed = constrain(RightMotorSpeed, -RightBaseSpeed, RightBaseSpeed);
  LeftMotorSpeed = constrain(LeftMotorSpeed, -LeftBaseSpeed, LeftBaseSpeed);
}