//https://github.com/pablopeza/TB6612FNG_ESP32
//https://www.upesy.com/blogs/tutorials/how-to-use-gpio-pins-of-esp32-with-arduino#
//https://github.com/pololu/qtr-sensors-arduino/releases

#include <TB6612_ESP32.h>
#include <QTRSensors.h>
//1. In the Arduino IDE, open "Manage Libraries...".
//2. Search for "QTRSensors" bu Pololu.
//3. Click the QTRSensors entry in the list.
//4. Click "Install".
#define LED_BUILTIN 2

#define AIN1 5   // ESP32 Pin D13 to TB6612FNG Pin AIN1
#define AIN2 18  // ESP32 Pin D14 to TB6612FNG Pin AIN2
#define PWMA 19  // ESP32 Pin D26 to TB6612FNG Pin PWMA

#define BIN1 16  // ESP32 Pin D12 to TB6612FNG Pin BIN1
#define BIN2 4   // ESP32 Pin D27 to TB6612FNG Pin BIN2
#define PWMB 15  // ESP32 Pin D25 to TB6612FNG Pin PWMB

#define STBY 17  // ESP32 Pin D33 to TB6612FNG Pin STBY

#define KP 1                   //experiment to determine this, start by something small that just makes your bot follow the line at a slow speed
#define KD 2                   //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd)
#define M1_minumum_speed 120   //minimum speed of the Motor1
#define M2_minumum_speed 120   //minimum speed of the Motor2
#define M1_maksimum_speed 130  //max. speed of the Motor1
#define M2_maksimum_speed 130  //max. speed of the Motor2
#define MIDDLE_SENSOR 26       //number of middle sensor PIN used
#define TIMEOUT 2500           //waits for 2500 us for sensor outputs to go low
#define DEBUG 1

const int buttonPin = 32;  //All GPIO pins (except GPIO36, GPIO39, GPIO34, and GPIO35 pins)
const int ledPin = 2;
// State of the push button
int buttonState = 1;


QTRSensors qtr;
const uint8_t SensorCount = 5;
uint16_t sensorValues[SensorCount];
int threshold[SensorCount];

// line up with function names like forward.  Value can be 1 or -1
const int offsetA = 1;
const int offsetB = 1;

Motor motor1 = Motor(AIN1, AIN2, PWMA, offsetA, STBY, 5000, 8, 1);
Motor motor2 = Motor(BIN1, BIN2, PWMB, offsetB, STBY, 5000, 8, 2);

//const int line_pin[5] = { 14, 27, 26, 25, 33 };
//int line_val[5] = { 0 };

int lastError = 0;
int last_proportional = 0;
int integral = 3;



void setup() {
  Serial.begin(9600);
  Serial.println("Iniciando sensores ...");
  qtr.setTypeRC();
  qtr.setSensorPins((const uint8_t[]){ 14, 27, 26, 25, 33 }, SensorCount);

  //Set the pin as an input pullup
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  Serial.println("Iniciando ...");
  Serial.print(buttonState);

  brake(motor1, motor2);

  while (buttonState == HIGH) {
    buttonState = digitalRead(buttonPin);
    Serial.print(buttonState);
    digitalWrite(ledPin, HIGH);
    delay(100);
    digitalWrite(ledPin, LOW);
    delay(100);
  }
  manual_calibration();
  buttonState = 1;
}

void readSensors() {
  // read calibrated sensor values and obtain a measure of the line position
  // from 0 to 5000 (for a white line, use readLineWhite() instead)
  uint16_t position = qtr.readLineBlack(sensorValues);
  Serial.println(position);
  // print the sensor values as numbers from 0 to 1000, where 0 means maximum
  // reflectance and 1000 means minimum reflectance, followed by the line
  // position
  for (uint8_t i = 0; i < SensorCount; i++) {
    Serial.print(sensorValues[i]);
    Serial.print('\t');
  }
  int error = position - 2000;
  int motorSpeed = KP * error + KD * (error - lastError);
  lastError = error;

  int leftMotorSpeed = M1_minumum_speed + motorSpeed;
  int rightMotorSpeed = M2_minumum_speed - motorSpeed;

  set_motors(leftMotorSpeed, rightMotorSpeed);
}

void set_motors(int motor1speed, int motor2speed) {
  if (motor1speed > M1_maksimum_speed) motor1speed = M1_maksimum_speed;
  if (motor2speed > M2_maksimum_speed) motor2speed = M2_maksimum_speed;
  if (motor1speed < 0) motor1speed = 0;
  if (motor2speed < 0) motor2speed = 0;
  motor1.drive(motor1speed);
  motor2.drive(motor2speed);
}

void loop() {
  waiting_start();
  readSensors();
}

void waiting_start() {
  while (buttonState == HIGH) {
    buttonState = digitalRead(buttonPin);
    Serial.print(buttonState);
    digitalWrite(ledPin, HIGH);
    delay(100);
    digitalWrite(ledPin, LOW);
    delay(100);
  }
}

void manual_calibration() {
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);  // turn on Arduino's LED to indicate we are in calibration mode
  // 2.5 ms RC read timeout (default) * 10 reads per calibrate() call
  // = ~25 ms per calibrate() call.
  // Call calibrate() 400 times to make calibration take about 10 seconds.
  for (uint16_t i = 0; i < 400; i++) {
    qtr.calibrate();
  }
  digitalWrite(LED_BUILTIN, LOW);  // turn off Arduino's LED to indicate we are through with calibration

  // print the calibration minimum values measured when emitters were ont
  for (uint8_t i = 0; i < SensorCount; i++) {
    Serial.print(qtr.calibrationOn.minimum[i]);
    Serial.print(' ');
  }
  Serial.println();

  // print the calibration maximum values measured when emitters were on
  for (uint8_t i = 0; i < SensorCount; i++) {
    Serial.print(qtr.calibrationOn.maximum[i]);
    Serial.print(' ');
  }
  Serial.println();
  Serial.println();
  delay(1000);
}