#include <Encoder.h>
#include "src/Lanes.h"
#include <Arduino.h>


const int Lane1Pin = 9;    //Output Channel 1
const int Lane2Pin = 10;   //Output Channel 2
const int Lane3Pin = 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;
long Lane1Pos, Lane2Pos, Lane3Pos, Lane1Time, Lane2Time, Lane3Time = 0;

//Lanes are 4 dimensions 0 = Step Time, 1 = Step Voltage, 2 = Curve type (Linear, Expo, Log, Sine, etc) 3 = Curve Parameter.
int Lane1[4][16];
int Lane2[4][16];
int Lane3[4][16];

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);

  //Initialize the Lanes
  for(int i = 0; i < 16; i++){
    Lane1[0][i] = random(0, 96);
    Lane1[1][i] = random(0, 255);
    Lane1[2][i] = random(0, 2);
    Lane1[3][i] = random(0,255);
    Lane2[0][i] = random(0, 96);
    Lane2[1][i] = random(0, 255);
    Lane2[2][i] = random(0, 2);
    Lane2[3][i] = random(0,255);
    Lane3[0][i] = random(0, 96);
    Lane3[1][i] = random(0, 255);
    Lane3[2][i] = random(0, 2);
    Lane3[3][i] = random(0,255);
  }

  analogWrite( Lane1Pin, Lane1[1][0]);
  analogWrite( Lane2Pin, Lane2[1][0]);
  analogWrite( Lane3Pin, Lane3[1][0]);

  // 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;
      E1Click = true;
      }
    }

    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;
    }

    if ((ClockTime - Lane1Time) > Lane1[0][Lane1Pos]){
      Lane1Pos = (Lane1Pos + 1) & B00001111;
      Lane1Time = ClockTime;
      analogWrite(Lane1Pin, Lane1[1][Lane1Pos]);
    }
    if ((ClockTime - Lane2Time) > Lane2[0][Lane2Pos]){
      Lane2Pos = (Lane2Pos + 1) & B00001111;
      Lane2Time = ClockTime;
      analogWrite(Lane2Pin, Lane2[1][Lane2Pos]);
    }
    if ((ClockTime - Lane3Time) > Lane3[0][Lane3Pos]){
      Lane3Pos = (Lane3Pos + 1) & B00001111;
      Lane3Time = ClockTime;
      analogWrite(Lane3Pin, Lane3[1][Lane3Pos]);
    }
}

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