#define LEFT_FORWARD 3
#define LEFT_BACKWARD 5
#define RIGHT_FORWARD 6
#define RIGHT_BACKWARD 11

#define LEFT_SENSOR A0
#define RIGHT_SENSOR A7

#define LEFT_MAX_SPEED 125
#define RIGHT_MAX_SPEED 150
#define NULL_SPEED 0

float Kp, Ki, Kd;
float integral, pError;

float computePID(float setpoint, float input, float dt) {
    float error = setpoint - input;
    float derivative = (error - pError) / dt;
    pError = error;
    integral += error * dt;
    return error * Kp + integral * Ki + derivative * Kd;
}


void turnOff(int del = 0);
void turnRight(int del = -1);
void turnLeft(int del = -1);
void rotateRight(int del = -1);
void rotateLeft(int del = -1);
void turnForward(int del = -1);
void turnBackward(int del = -1);
void calibrateSensors();

uint16_t leftWhiteThreshold, leftBlackThreshold;
uint16_t leftWhite, leftBlack;
uint16_t rightWhiteThreshold, rightBlackThreshold;
uint16_t rightWhite, rightBlack;
uint16_t leftGrayThreshold, rightGrayThreshold;

void setup() {
  Serial.begin (9600);
  pinMode (LEFT_FORWARD, OUTPUT);
  pinMode (LEFT_BACKWARD, OUTPUT);
  pinMode (RIGHT_FORWARD, OUTPUT);
  pinMode (RIGHT_BACKWARD, OUTPUT);

  turnOff();

  pinMode (LEFT_SENSOR, INPUT);
  pinMode (RIGHT_SENSOR, INPUT);

  calibrateSensors();
  delay(5000);
}

void loop() {
  uint16_t leftSensor = analogRead(LEFT_SENSOR);
  uint16_t rightSensor = analogRead(RIGHT_SENSOR);
  Serial.println(leftSensor);
  Serial.println(" ");
  Serial.println(rightSensor);

  if (leftSensor > leftGrayThreshold && rightSensor < rightGrayThreshold) {
    turnLeft();
  }
  else if (leftSensor < leftGrayThreshold && rightSensor > rightGrayThreshold) {
    turnRight();
  }
  else {
    turnForward();
  }

  delay(1);
  
}


void calibrateSensors() {
  char thresholdSumSize = 100;
  uint16_t thresholdSumDelay = 5000;
  uint32_t leftThresholdSum = 0;
  uint32_t rightThresholdSum = 0;

  for (char i = 0; i < thresholdSumSize; ++i) {
    leftThresholdSum += analogRead(LEFT_SENSOR);
    rightThresholdSum += analogRead(RIGHT_SENSOR);
    Serial.println("\nWhite calibration:");
    Serial.println(leftThresholdSum + " " + String(rightThresholdSum));
    delay(thresholdSumDelay / thresholdSumSize);
  }
  leftWhiteThreshold = leftThresholdSum / thresholdSumSize;
  rightWhiteThreshold = rightThresholdSum / thresholdSumSize;
  Serial.print("\nWhite threshold: ");
  Serial.println(String(leftWhiteThreshold) + " " + String(rightWhiteThreshold));

  turnForward(100);
  delay(5000);

  leftThresholdSum = 0;
  rightThresholdSum = 0;
  for (char i = 0; i < thresholdSumSize; ++i) {
    leftThresholdSum += analogRead(LEFT_SENSOR);
    rightThresholdSum += analogRead(RIGHT_SENSOR);
    Serial.println("\nBlack calibration:");
    Serial.println(String(leftThresholdSum) + " " + String(rightThresholdSum));
    delay(thresholdSumDelay / thresholdSumSize);
  }
  leftBlackThreshold = leftThresholdSum / thresholdSumSize;
  rightBlackThreshold = rightThresholdSum / thresholdSumSize;
  Serial.print("\nBlack threshold: ");
  Serial.println(String(leftBlackThreshold) + " " + String(rightBlackThreshold));

  turnForward(100);

  leftGrayThreshold = (leftWhiteThreshold + leftBlackThreshold) / 2;
  rightGrayThreshold = (rightWhiteThreshold + rightBlackThreshold) / 2;
  Serial.print("\nGray threshold: ");
  Serial.println(String(leftGrayThreshold) + " " + String(rightGrayThreshold));
}

void turnOff(int del) {
  analogWrite(LEFT_FORWARD, 0);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, 0);
  delay(del);
}

void turnRight(int del) {
  analogWrite(LEFT_FORWARD, LEFT_MAX_SPEED);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, 0);
  if (del != -1) {
    delay(del);
    turnOff();
  }
}

void turnLeft(int del) {
  analogWrite(LEFT_FORWARD, 0);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, RIGHT_MAX_SPEED);
  if (del != -1) {
    delay(del);
    turnOff();
  }
}

void rotateRight(int del) {
  analogWrite(LEFT_FORWARD, LEFT_MAX_SPEED);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, RIGHT_MAX_SPEED);
  analogWrite(RIGHT_FORWARD, 0);
  if (del != -1) {
    delay(del);
    turnOff();
  }
}

void rotateLeft(int del) {
  analogWrite(LEFT_FORWARD, 0);
  analogWrite(LEFT_BACKWARD, LEFT_MAX_SPEED);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, RIGHT_MAX_SPEED);
  if (del != -1) {
    delay(del);
    turnOff();
  }
}

void turnForward(int del) {
  analogWrite(LEFT_FORWARD, LEFT_MAX_SPEED);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, RIGHT_MAX_SPEED);
  if (del != -1) {
    delay(del);
    turnOff();
  }
}


void turnBackward(int del) {
  analogWrite(LEFT_FORWARD, 0);
  analogWrite(LEFT_BACKWARD, LEFT_MAX_SPEED);
  analogWrite(RIGHT_BACKWARD, RIGHT_MAX_SPEED);
  analogWrite(RIGHT_FORWARD, 0);
  if (del != -1) {
    delay(del);
    turnOff();
  }
}