#include <EEPROM.h>

#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 K_MAX 5
#define LEFT_MAX_SPEED int16_t(14 * K_MAX)
#define RIGHT_MAX_SPEED int16_t(15 * K_MAX)
#define K_MIN 5
#define LEFT_MIN_SPEED int16_t(14 * K_MIN)
#define RIGHT_MIN_SPEED int16_t(15 * K_MIN)

// #define CALIBRATION


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();

struct {
  uint16_t leftWhiteThreshold, leftBlackThreshold;
  uint16_t rightWhiteThreshold, rightBlackThreshold;
  uint16_t leftGrayThreshold, rightGrayThreshold;
} calib;

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

  delay(1000);
  #ifdef CALIBRATION
  calibrateSensors();
  EEPROM.put(0, calib);
  #else
  EEPROM.get(0, calib);
  #endif
}

void loop() {
  #ifdef CALIBRATION
  return;
  #endif

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

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

  delay(1);
  
  // int16_t left_sensor = analogRead(LEFT_SENSOR);
  // int16_t right_sensor = analogRead(RIGHT_SENSOR);
  // int16_t left_path = map(left_sensor, calib.leftWhiteThreshold, calib.leftBlackThreshold, 0, 1023);
  // int16_t right_path = map(right_sensor, calib.rightWhiteThreshold, calib.rightBlackThreshold, 0, 1023);

  // int16_t delta = map(left_path - right_path, -1023, 1023, LEFT_MIN_SPEED, LEFT_MAX_SPEED) / 1;

  // int16_t left_drive = (LEFT_MAX_SPEED + LEFT_MIN_SPEED) / 2 - delta - delta / 2;
  // int16_t right_drive = (LEFT_MAX_SPEED + LEFT_MIN_SPEED) / 2 - delta + delta / 2;

  // Serial.println(String(left_sensor) + "\t\t" + String(right_sensor) + "\t\t" + String(left_path) + "\t\t" + String(right_path) + "\t\t" + String(delta) + "\t\t" + String(left_drive) + "\t\t" + String(right_drive));
  // // analogWrite(LEFT_FORWARD, );
  // // analogWrite(RIGHT_FORWARD, );

  // delay(100);
  
}


void calibrateSensors() {
  uint16_t leftWhite, leftBlack;
  uint16_t rightWhite, rightBlack;

  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(String(leftThresholdSum) + " " + String(rightThresholdSum));
    delay(thresholdSumDelay / thresholdSumSize);
  }
  calib.leftWhiteThreshold = leftThresholdSum / thresholdSumSize;
  calib.rightWhiteThreshold = rightThresholdSum / thresholdSumSize;
  Serial.print("\nWhite threshold: ");
  Serial.println(String(calib.leftWhiteThreshold) + " " + String(calib.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);
  }
  calib.leftBlackThreshold = leftThresholdSum / thresholdSumSize;
  calib.rightBlackThreshold = rightThresholdSum / thresholdSumSize;
  Serial.print("\nBlack threshold: ");
  Serial.println(String(calib.leftBlackThreshold) + " " + String(calib.rightBlackThreshold));

  turnForward(100);

  calib.leftGrayThreshold = (calib.leftWhiteThreshold + calib.leftBlackThreshold) / 2;
  calib.rightGrayThreshold = (calib.rightWhiteThreshold + calib.rightBlackThreshold) / 2;
  Serial.print("\nGray threshold: ");
  Serial.println(String(calib.leftGrayThreshold) + " " + String(calib.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_MIN_SPEED);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, RIGHT_MIN_SPEED);
  if (del != -1) {
    delay(del);
    turnOff();
  }
}


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