#include <Encoder.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 = 20; //Clock In
const int ClockDetect = 21; //Detect clock jack

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

int E1 = 0;
int E2 = 0;
bool E1Btn = false;
bool E2Btn = false;
bool ClockState = false;
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));

  //Clear 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]);

}

void loop() {

    delay(1);

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

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