#include <QTRSensors.h>

#define NUM_SENSORS   6     // number of sensors used
#define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading
#define EMITTER_PIN   QTR_NO_EMITTER_PIN 

QTRSensorsAnalog qtra((unsigned char[]) {0, 1, 2, 3, 4, 5},
  NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];

int PWMA = 3;
int PWMB = 11;

int max_speed = 200;

int proportional, Setpoint, integral, derivative, last_proportional, error_value, Kp, Ki, Kd, right_speed, left_speed;
double pozitie;

int MOTOR1_PIN1 = 12;
int MOTOR1_PIN2 = 3;

int MOTOR2_PIN1 = 13;
int MOTOR2_PIN2 = 11;

// int led = 13;

void setup() {
  Setpoint = 128;
  /* pid tuning parameters */
  Kp = 2.2; // 2.5
  Ki = 0.9; // 0.9
  Kd = 6;  // 6

    pinMode(MOTOR1_PIN1, OUTPUT);
  pinMode(MOTOR1_PIN2, OUTPUT);
  pinMode(MOTOR2_PIN1, OUTPUT);
  pinMode(MOTOR2_PIN2, OUTPUT);
  // pinMode(led, OUTPUT);

  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);

  delay(500);
 // digitalWrite(led, HIGH);    // turn on Arduino's LED to indicate we are in calibration mode

  for (int i = 0; i < 400; i++)  // make the calibration take about 10 seconds
  {
    qtra.calibrate();       // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  }
 // digitalWrite(led, LOW);     // turn off Arduino's LED to indicate we are through with calibration


  // print the calibration minimum values measured when emitters were on
  Serial.begin(9600);
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtra.calibratedMinimumOn[i]);
    Serial.print(' ');
  }
  Serial.println();

  // print the calibration maximum values measured when emitters were on
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtra.calibratedMaximumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
  Serial.println();
  delay(1000);
}


void loop() {

  /*if (irrecv.decode(&results)) {
   switch (results.value)
   {
   case 0x506A9BC7 :
   ir=0; //stop
   break;
   case 0xDBCB3A41 :
   ir=1; //start
   break;
   }
   irrecv.resume(); // Receive the next value
   }*/
  //if (ir) {
  digitalWrite(MOTOR1_PIN1, HIGH);
  digitalWrite(MOTOR1_PIN2, LOW);
  digitalWrite(MOTOR2_PIN1, LOW);
  digitalWrite(MOTOR2_PIN2, HIGH);

  pid_calc();
  calc_turn();
  motor_drive(right_speed, left_speed); 
  //}
  /* else {
   digitalWrite(MOTOR1_PIN1, LOW);
   digitalWrite(MOTOR1_PIN2, LOW);
   digitalWrite(MOTOR2_PIN1, LOW);
   digitalWrite(MOTOR2_PIN2, LOW);
   }*/

}

void pid_calc() {
  pozitie = qtra.readLine(sensorValues);
  pozitie = map(pozitie,0,5000,0,255);
  proportional = pozitie - Setpoint;
  integral = integral + proportional;
  derivative = proportional - last_proportional;
  last_proportional = proportional;
  error_value = int(proportional * Kp + integral * Ki + derivative * Kd);
}

void calc_turn() { //Restricting the error value between +256.
  if (error_value < -max_speed) {
    error_value = -max_speed;
  }
  if (error_value > max_speed) {
    error_value = max_speed;
  }
  // If error_value is less than zero calculate right turn speed values
  if (error_value < 0) {
    right_speed = max_speed + error_value;
    left_speed = max_speed;
  }
  // If error_value is greater than zero calculate left turn values
  else {
    right_speed = max_speed;
    left_speed = max_speed - error_value;
  }
}

void motor_drive(int right_speed, int left_speed) { // Drive motors according to the calculated values for a turn
  analogWrite(PWMA, right_speed);
  analogWrite(PWMB, left_speed);
}