//Make sure to install the library
#include <QTRSensors.h>
QTRSensors qtr;
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];

float Kp = 0.07; //set up the constants value
float Ki = 0.001;
float Kd = 0.6;
int P;
int I;
int D;

int lastError = 0;
boolean onoff = false;

//Increasing the maxspeed can damage the motors - at a value of 255 the 6V motors will receive 7,4 V
const uint8_t maxspeeda = 120;
const uint8_t maxspeedb = 120;
const uint8_t basespeeda = 100;
const uint8_t basespeedb = 100;

//Set up the drive motor carrier pins
int mode = 8;
/*int aphase = 9;
int aenbl = 6;
int bphase = 5;
int benbl = 3;*/

int aphase = 6;
int aenbl = 9;

int bphase = 3;
int benbl = 5;



//Set up the buttons pins
int buttoncalibrate = 17; //pin A3
int buttonstart = 2;

void setup() {
  Serial.begin(9600);
  qtr.setTypeRC();
  //Set up the sensor array pins
  qtr.setSensorPins((const uint8_t[]) {
    10, 11, 12, 14, 15, 16, 18, 19
  }, SensorCount);
  qtr.setEmitterPin(7);//LEDON PIN

  pinMode(mode, OUTPUT);
  pinMode(aphase, OUTPUT);
  pinMode(aenbl, OUTPUT);
  pinMode(bphase, OUTPUT);
  pinMode(benbl, OUTPUT);
  digitalWrite(mode, HIGH);

  delay(500);
  pinMode(LED_BUILTIN, OUTPUT);

  //Stephanos code - LED will blink for 3 seconds and then 
  /*   calibration will begin running for 10 seconds;
     this happens when our system boots. Giorgo i removed
     the functionality to calibrate through the button keypress.*/
  for (int counter=0; counter < 3;counter++){
    digitalWrite(LED_BUILTIN, HIGH);
    delay(500);
    digitalWrite(LED_BUILTIN, LOW);
    delay(500);
  }
  calibration();  
  //When calibration is compelted we understand that by blinking the LED for
   //5 times in a period of 2 seconds, faster than getting ready for calibration
   for (int counter=0; counter < 3;counter++){
    digitalWrite(LED_BUILTIN, HIGH);
    delay(200);
    digitalWrite(LED_BUILTIN, LOW);
    delay(200);
  }
  
  //modified by Stephanos - commented out
  /*boolean Ok = false;
  while (Ok == false) { //the loop won't start until the robot is calibrated
    if (digitalRead(buttoncalibrate) == HIGH) {
      calibration(); //calibrate the robot for 10 seconds
      Ok = true;
    }
  }*/
  
  forward_brake(0, 0);
}

void calibration() {
  digitalWrite(LED_BUILTIN, HIGH);
  for (uint16_t i = 0; i < 400; i++)
  {
    qtr.calibrate();
  }
  digitalWrite(LED_BUILTIN, LOW);
  Serial.print("Calibration COMPLETED!!!");
}


void loop() {
  /*if (digitalRead(buttonstart) == HIGH) {
    onoff = ! onoff;
    if (onoff = true) {
      delay(1000);//a delay when the robot starts
    }
    else {
      delay(50);
    }
  }*/
  delay(3000);// a small 3 seconds delay before starting our rebot
  PID_control();
  
  /*if (onoff == true) {
    PID_control();
  }
  else {
    forward_brake(0, 0);
  }*/
}


void forward_brake(int posa, int posb) {
  //set the appropriate values for aphase and bphase so that the robot goes straight
  digitalWrite(aphase, HIGH);
  digitalWrite(bphase, HIGH);
  analogWrite(aenbl, posa);
  analogWrite(benbl, posb);
}

void PID_control() {
  uint16_t position = qtr.readLineBlack(sensorValues);
  int error = 3500 - position;

  P = error;
  I = I + error;
  D = error - lastError;
  lastError = error;
  int motorspeed = P * Kp + I * Ki + D * Kd;

  int motorspeeda = basespeeda + motorspeed;
  int motorspeedb = basespeedb - motorspeed;

  if (motorspeeda > maxspeeda) {
    motorspeeda = maxspeeda;
  }
  if (motorspeedb > maxspeedb) {
    motorspeedb = maxspeedb;
  }
  if (motorspeeda < 0) {
    motorspeeda = 0;
  }
  if (motorspeedb < 0) {
    motorspeedb = 0;
  }
  Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb);
  forward_brake(motorspeeda, motorspeedb);
}