#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
 
  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, 0, (LEFT_MAX_SPEED - LEFT_MIN_SPEED) / 2);

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

  Serial.println("A: " + String(calib.leftWhiteThreshold) + "\t\t" + String(calib.leftBlackThreshold) + "\t\t" + String(calib.rightWhiteThreshold) + "\t\t" + String(calib.rightBlackThreshold) + "\t\t" + String(left_sensor) + "\t\t" + String(right_sensor));
  Serial.println("B: " + 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();
  }
}