#include <Encoder.h>
// Motor A
const byte  ENA = 13;
const byte  IN1 = 12;
const byte  IN2 = 14;

// Motor B
const byte  ENB = 25;
const byte  IN3 = 27;
const byte  IN4 = 26;
const byte  A_0 = 36;
const byte  A_1 = 39;

int xx = 0, yy = 0; // x&y potentiometers

Encoder left(19,16);  // 32,33
Encoder right(18,17); // 34,35

void setup ()
{
  // Declaramos todos los pines como salidas
  pinMode (ENA, OUTPUT);
  pinMode (ENB, OUTPUT);
  pinMode (IN1, OUTPUT);
  pinMode (IN2, OUTPUT);
  pinMode (IN3, OUTPUT);
  pinMode (IN4, OUTPUT);
  pinMode (A_0, INPUT);
  pinMode (A_1, INPUT);
  Serial.begin(115200);
  Serial.print("Encoder Positions\nleft, right, ave, diff, d_avg, d_diff\n");
}

void writeA(int a1, int a2) {
  digitalWrite(IN1, a1);
  digitalWrite(IN2, a2);
}
void writeB(int a3, int a4) {
  digitalWrite(IN3, a3);
  digitalWrite(IN4, a4);
}

void loop ()
{
  static int lastXX, lastYY;
  xx = (analogRead(A_0)-512) / 8;
  yy = (analogRead(A_1) - 512) / 2;

  int speedA = constrain(yy - xx, -255, 255);
  int speedB = constrain(yy + xx, -255, 255);

  if (speedA >= 0 ) {
    writeA(1, 0);
    analogWrite(ENA, speedA);
  }
  else
  {
    writeA(0, 1);
    analogWrite(ENA, -speedA);
  }
  if (speedB >= 0 ) {
    writeB(1, 0);
    analogWrite(ENB, speedB);
  }
  else
  {
    writeB(0, 1);
    analogWrite(ENB, -speedB);
  }

  if (false && (xx != lastXX || yy != lastYY)) {
    lastXX = xx;
    lastYY = yy;
    Serial.print(xx);
    Serial.print(",");
    Serial.print(yy);
    Serial.print(" ");
  }
  left.read();
  right.read();
  report();
}

void report(){
  auto now = millis();
  const typeof(now) interval = 1000;
  static typeof(now) last = -interval;
  static long last_avg, last_diff;
  if(now - last >= interval){
    last+= interval;
    long lval = -left.read();
    long rval= right.read();
    long avg = (lval+rval)/2;
    long diff = (lval-rval)/2;
    long delta_avg = avg - last_avg;
    long delta_diff = diff - last_diff;
    last_avg = avg;
    last_diff = diff;
    Serial.print(lval);
    Serial.print(" ");
    Serial.print(rval);
    Serial.print(" ");
    Serial.print(avg);
    Serial.print(" ");
    Serial.print(diff);
    Serial.print(" ");
    Serial.print(delta_avg);
    Serial.print(" ");
    Serial.print(delta_diff);
    Serial.println();
  }
}