#include <Bounce2.h>

/* 
 DESCRIPTION
 ====================
 Reads the 2 switches in an encoder, 
 determines direction,
 updates counter.
 No interrupts.
 Switches debounced, didn't test first, just did it anyway.
 */

//Encoder 2 (16,17)
//Encoder 3 (9,10)

const byte ENCODER_PINA= 16;
//const byte LED_PINA= 12;
const byte ENCODER_PINB= 17;
//const byte LED_PINB= 11;

int valueA; //debounced encoder switch reads
int valueB;

bool motionDetected = false;
int grossCounter = 0; // total steps
int nettCounter = 0;  // cw-ccw
int fullRevolutions = 0;
int surplusSteps = 0; //part revs
bool CW;

byte cyclesPerRev =20;  //check encoder datasheet

// Instantiate 2 Bounce object
Bounce debouncerA = Bounce(); 
Bounce debouncerB = Bounce(); 

// setup ********************************************
void setup() {
  Serial.begin(9600);
  Serial.println("Setup");
  // Setup the buttons
  pinMode(ENCODER_PINA,INPUT_PULLUP);
  pinMode(ENCODER_PINB,INPUT_PULLUP);


  // After setting up the button, setup debouncer
  debouncerA.attach(ENCODER_PINA);
  debouncerA.interval(1);
  debouncerB.attach(ENCODER_PINB);
  debouncerB.interval(1);

  //Setup the LED
//  pinMode(LED_PINA,OUTPUT);
//  pinMode(LED_PINB,OUTPUT);
  Serial.println("Setup done");
}

// loop *****************************************
void loop() {

  // Update the debouncers
  doDebounce();

  // Read the encoder switches
  doEncoderRead();

  // Update LEDs and serial print A, a, B, b
//  updateLEDs();

  //determine direction and update counter

  updateCounter();

} //loop

// my functions **************************************************
void doDebounce()
{
  debouncerA.update();
  debouncerB.update();
} //doDebounce

void doEncoderRead()
{
  valueA = debouncerA.read();
  valueB = debouncerB.read();
} //doEncoderRead

void updateLEDs()
{

  if ( valueA == HIGH) {
//    digitalWrite(LED_PINA, HIGH );
    //Serial.print("A");

  } 
  else {
//    digitalWrite(LED_PINA, LOW );
    //Serial.print("a");
  }

  if ( valueB == HIGH) {
//    digitalWrite(LED_PINB, HIGH );
    //Serial.println("B");
  } 
  else {
//    digitalWrite(LED_PINB, LOW );
    //Serial.println("b");  
  }

} //updateLEDs

void updateCounter()
{

  /*
  the possibilites are:
   
   AB: in a detent
   if just arrived, update counter, clear motiondetected
   otherwise do nothing
   Ab: start of CW or end of CCW
   if start, set CW bool and set motionDetected
   if at end (know becasue motionDetected already set), do nothing
   aB: start of CCW or end of CW
   if start, clear CW bool and set motionDetected
   if at end (know becasue motionDetected already set), do nothing
   ab: in middle of either CW or CCW, do nothing
   */

  if (valueA && valueB && motionDetected ) //in a detent and just arrived
  {
    if (CW)
    {
      grossCounter= grossCounter + 1;
      nettCounter= nettCounter + 1;
    }
    else //CCW
    {
      grossCounter= grossCounter + 1;
      nettCounter= nettCounter - 1;
    }
    motionDetected = false;
    Serial.print("grossCounter: ");
    Serial.println(grossCounter);
    Serial.print("nettCounter: ");
    Serial.println(nettCounter);
    
    fullRevolutions = nettCounter / cyclesPerRev; 
    surplusSteps = nettCounter % cyclesPerRev;
    
    Serial.print("Nett position: ");
    Serial.print(fullRevolutions);
    Serial.print(" + ");
    Serial.println(surplusSteps);
    Serial.println(" ");
    
    


  }

  if (valueA && !valueB && !motionDetected ) // just started CW
  {
    CW= true;
    motionDetected=true;
    Serial.println("CW");

  }

  if (!valueA && valueB && !motionDetected )  //just started CCW
  {
    CW= false;
    motionDetected=true;
    Serial.println("CCW");

  }
} //updateCounter