#include "quadrature_encoder.pio.h"
// LCD2004 and Pi Pico!
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);  // set the LCD address to 0x27 for a 16 chars and 2 line display

//
int new_value,new_value2, delta, old_value = 0;
const uint PIN_AB_EN1 = 9;
const uint PIN_BA_EN1 = 10;
const uint PIN_AB_EN2 = 11;
const uint PIN_BA_EN2 = 12;
PIO pio = pio0;


const uint sm = 0;


// Base pin to connect the A phase of the encoder.
// The B phase must be connected to the next pin



// note: thanks to two's complement arithmetic delta will always
// be correct even when new_value wraps around MAXINT / MININT



void setup() {
  // put your setup code here, to run once:
  pinMode(PIN_AB_EN1, INPUT_PULLUP);
  pinMode(PIN_BA_EN1, INPUT_PULLUP);
  pinMode(PIN_AB_EN2, INPUT_PULLUP);
  pinMode(PIN_BA_EN2, INPUT_PULLUP);
  Serial1.begin(115200);

  // we don't really need to keep the offset, as this program must be loaded
  // at offset 0
  //pio_add_program(pio, &quadrature_encoder_program);
  pio_add_program(pio0, &quadrature_encoder_program);
  pio_add_program(pio1, &quadrature_encoder_program);
  //quadrature_encoder_program_init(pio, 0, PIN_AB, 0);
  quadrature_encoder_program_init(pio0, 0, PIN_AB_EN1, 0);	// Pins 0 and 1
  quadrature_encoder_program_init(pio0, 1, PIN_AB_EN2, 0);	// Pins 2 and 3, etc.

  /*
    quadrature_encoder_program_init(pio1, 1, offset1, 10, 0);
    quadrature_encoder_program_init(pio1, 2, offset1, 12, 0);
    quadrature_encoder_program_init(pio1, 3, offset1, 14, 0);
  */
}

void loop() {
  // put your main code here, to run repeatedly:
  new_value = quadrature_encoder_get_count(pio0, 0) / 4;
  new_value2 = quadrature_encoder_get_count(pio0, 1) / 4;
  delta = new_value - old_value;
  old_value = new_value;
  //Serial.print(position %8d, delta %6d\n", new_value, delta);
  Serial1.print ("Position: ");
  Serial1.println (new_value );
  Serial1.print ("Position2: ");
  Serial1.println (new_value2 );

  delay(10);
}