#include <Encoder.h>
#include <Arduino.h>


const int Channel1 = 9;    //Output Channel 1
const int Channel2 = 10;   //Output Channel 2
const int Channel3 = 11;   //Output Channel 3
const int Enc1P1 = 2;   //Encoder 1 Pin 1 to Interrupt
const int Enc2P1 = 3;   //Encoder 2 Pin 1 to Interrupt
const int Enc1P2 = 14;  //Encoder 1 Pin 2 to non-Interrupting pin because we only have 2
const int Enc2P2 = 15;  //Encoder 2 Pin 2 to non-Interrupting pin because we only have 2
const int Enc1Btn = 16; //Encoder 1 Button
const int Enc2Btn = 17; //Encoder 2 Button
const int ClockIn = 4; //Clock In
const int ClockDetect = 7; //Detect clock jack
const int DebounceTime = 10;  //Debounce time in ms

Encoder LeftEnc( Enc1P1, Enc1P2);
Encoder RightEnc( Enc2P1, Enc2P2);

int E1 = 0;
int E2 = 0;
bool E1Btn = false;
bool E2Btn = false;
bool ClockState = false;
bool E1Prev, E2Prev, CDPrev, E1Click, E2Click, CDIn = false;
unsigned int E1Bounce, E2Bounce, CDBounce = 0;
unsigned long ClockPrev = 0;
byte Input = 0;
int PpQN = 24;
float Clock = 120;
float ClockTick = (1/((Clock * PpQN)/60)) * 1000;
unsigned long ClockTime = 0;
unsigned long LastStepTime = 0;
long EncLeft, EncRight = 0;

void setup() {
  
  //Open Serial for output prior to installing a screen
  Serial.begin( 115200 );
  Serial.println("Env Gen");
  randomSeed(analogRead(A7));

  pinMode(Enc1Btn, INPUT_PULLUP);
  pinMode(Enc2Btn, INPUT_PULLUP);  
  pinMode(ClockIn, INPUT);
  pinMode(ClockDetect, INPUT_PULLUP);

  E1Btn = digitalRead(Enc1Btn);
  E1Prev = E1Btn;
  E2Btn = digitalRead(Enc2Btn);
  E2Prev = E2Btn;
  ClockState = digitalRead(ClockIn);
  CDPrev = digitalRead(ClockDetect);

  // Timer0 is already used for millis() - we'll just interrupt somewhere
  // in the middle and call the "Compare A" function below
  OCR0A = 0xAF;
  TIMSK0 |= _BV(OCIE0A);

}

void loop() {


    long newLEnc = LeftEnc.read();
    long newREnc = RightEnc.read();

    if (digitalRead(Enc1Btn) != E1Btn){
      if (E1Bounce == 0 & E1Click == false){
      Serial.println("E1Btn State Change");
      E1Btn = !E1Btn;
      E1Bounce = DebounceTime;
      E1Click = true;
      }
    }

    if (digitalRead(Enc2Btn) != E2Btn){
      if (E2Bounce == 0 & E2Click == false){
      Serial.println("E2Btn State Change");
      E2Btn = !E2Btn;
      E2Bounce = DebounceTime;
      E2Click = true;
      }
    }

    //false click handler
    E1Click = false;
    E2Click = false;

    bool newCD = digitalRead(ClockDetect);

    if (newCD != CDPrev){
      if (CDBounce == 0){
      Serial.println("Clock Jack Status Change");
      CDPrev = CDIn = newCD;
      CDBounce = DebounceTime << 4;
      }
    }

    if ( digitalRead(ClockIn) != ClockState){
        ClockState = !ClockState;
        if (ClockState){
          unsigned long tmpClock = micros();
          float clkInTick = tmpClock - ClockPrev;
          float  newBPM = ((1.0/(clkInTick/1000000.0)) * 60.0)/(float)PpQN;
          ClockPrev = tmpClock;

          if (abs(Clock - newBPM) > 0.5){
            Clock = newBPM;
            String outputBPM = "New BPM: ";
            outputBPM.concat(newBPM);
            Serial.println(outputBPM);
            outputBPM = "Clock Tick: ";
            outputBPM.concat(clkInTick);
            Serial.println(outputBPM);
            ClockTick = (1/((Clock * PpQN)/60)) * 1000;
          }
        }
    }

    if (newLEnc != EncLeft || newREnc != EncRight){
      String output = "Left Enc Pos: ";
      output.concat(newLEnc);
      output.concat( ", Right Enc Pos: ");
      output.concat(newREnc);
      Serial.print(output);

      if (newLEnc != EncLeft){
        Clock = Clock + (((float)EncLeft - (float)newLEnc)/40.0);
      } else{
        Clock = Clock + ((EncRight - newREnc) * 2.5);
      }

      ClockTick = (1/((Clock * PpQN)/60)) * 1000;
      EncLeft = newLEnc;
      EncRight = newREnc;
      output = " Clock: ";
      output.concat(Clock);
      output.concat( " Clocktick: ");
      output.concat( ClockTick);

      Serial.println(output);

    }

    unsigned long currentTime = millis();

    if ((currentTime - LastStepTime) > ClockTick){
      ClockTime++;
      LastStepTime = currentTime;
    }}
    

// Interrupt is called once a millisecond, 
SIGNAL(TIMER0_COMPA_vect) 
{
  E1Bounce = (E1Bounce - 1) & 0b10000000;
  E2Bounce = (E2Bounce - 1) & 0b10000000;
  CDBounce = (CDBounce - 1) & 0b10000000;
}