#include <Encoder.h>
//#include <MIDIUSB.h>

#define ENCODER_USE_INTERRUPTS

// encoder declaration and properties
  const byte n_enc = 1; // number of encoders
  Encoder myEnc[n_enc] = {{2, 3}};  // declaring encoder and pins (encoder.h library)
  byte hwCH = 0;  // encoder midi channel
  byte hwCC[n_enc] = {1};  // encoder cc channel
//

// variables for storing encoder value
  int newHwVal[n_enc] = {0};
  int oldHwVal[n_enc] = {0};
  int minHwVal = 0;
  int maxHwVal = 16384;        // stores maximum encoder value - 16383 for 14-bit range
  float encSense = 0.25;          // encoder sensitivity - no. values output per detent = (4 * encSense)
  const byte encThreshold = 1; // for hysterisis check
  
  byte hwMSB[n_enc] = {0};
  byte hwLSB[n_enc] = {0};
  byte oldHwLSB[n_enc] = {0};
//  

// variables for determining if the encoder is moving to prevent feedback loop when reading and writing hardware / software
  unsigned long encLastRxTime[n_enc] = {0};
  boolean encRxStarted[n_enc] = {false};
  byte encRxTimeout = 50;
//

// stores enc midi cc value from sw (see void handleControlChange)
  int swVal[n_enc] = {0};  // enc cc val from sw
  int swMSB[n_enc] = {0};
  int swLSB[n_enc] = {0};
  byte swCC = 0;  // enc midi cc channel from sw
  byte swCH = 0;  // enc midi channel from sw
//

// low pass filter variables
  float lpfVal[n_enc] = {0};
  const float alpha = 0.5;
  unsigned long lastLpfFollowTime[n_enc] = {0};

  static int errorMinThreshold = 12;
  static int errorMaxThreshold = 50;
  static int maxLpfTimeout = 2;
  static int minLpfTimeout = 0.0007;
//

void readHwVal(int i) {
  
  int val = myEnc[i].read();

  // Hysteresis to stop jitter
  if (abs (val - getHwVal(i)) > encThreshold) {
    setHwVal(i, val * encSense);
  } 
}

void setHwVal(int i, int val) {

  //val = lpf(i, val);

  if (val != oldHwVal[i]) {

    if (val > maxHwVal) {
      myEnc[i].write(maxHwVal / encSense);
    
    } else if (val < minHwVal) {
      myEnc[i].write(minHwVal);
    
    } else {
      newHwVal[i] = val;
    } 
    
    oldHwVal[i] = val;
  }
}

int getHwVal(int i) {
  return newHwVal[i]; 
}

// MIDIUSB library for handling incoming midi
  // void MIDIread() {
  //   midiEventPacket_t rx = MidiUSB.read();

  //   switch (rx.header) {
  //     case 0:
  //       break;  // No pending events

  //     case 0x9:
  //       // handleNoteOn(
  //       //   rx.byte1 & 0xF,  //channel
  //       //   rx.byte2,        //pitch
  //       //   rx.byte3         //velocity
  //       // );
  //       break;

  //     case 0x8:
  //       // handleNoteOff(
  //       //   rx.byte1 & 0xF,  //channel
  //       //   rx.byte2,        //pitch
  //       //   rx.byte3         //velocity
  //       // );
  //       break;

  //     case 0xB:
  //       handleControlChange(rx.byte1 & 0xF,  // channel
  //                           rx.byte2,        // cc
  //                           rx.byte3         // value
  //       );
  //       break;

  //     case 0xE:
  //       //      handlePitchBend(
  //       //        rx.byte1 & 0xF,  //channel
  //       //        rx.byte2,        //LSB
  //       //        rx.byte3         //MSB
  //       //      );
  //       break;

  //     case 0xC:
  //       // handleProgramChange(
  //       //   rx.byte1 & 0xF,  //channel
  //       //   rx.byte2         //program number
  //       // );
  //       break;

  //     case 0xD:
  //       // handleAftertouch(
  //       //   rx.byte1 & 0xF,  //channel
  //       //   rx.byte2         //pressure
  //       // );
  //       break;

  //       // case 0xF:
  //       //   {
  //       //     // The first byte of a system exclusive message is always 0xF0
  //       //     if (rx.byte1 == 0xF0) {
  //       //       // We have a valid system exclusive message
  //       //       byte* data = rx.data;
  //       //       unsigned int length = rx.byte2;
  //       //       handleSysEx(data, length);
  //       //     }
  //       //   }
  //       //   break;
  //   }

  //   //  if (rx.header != 0) {
  //   //    Serial.print("Unhandled MIDI message: ");
  //   //    Serial.print(rx.byte1 & 0xF, DEC);
  //   //    Serial.print("-");
  //   //    Serial.print(rx.byte1, DEC);
  //   //    Serial.print("-");
  //   //    Serial.print(rx.byte2, DEC);
  //   //    Serial.print("-");
  //   //    Serial.println(rx.byte3, DEC);
  //   //  }
  // }

  // called in MIDIread(), stores sw val if midi ch and cc ch match hw -> startMidiRx
  // void handleControlChange(byte channel, byte number, byte value) {
    
  //   float fval = value;  // stores software midi CC value
  //   swCC = number;    // stores software midi CC channel
  //   swCH = channel;   // stores software midi channel

  //   for (int i = 0; i < n_enc; i++) { 
  //     if ((swCC == hwCC[i]) && (swCH == hwCH)) {
  //       setSwMSB(i, fval); //***
  //     }

  //     if ((swCC == (hwCC[i] + 32)) && (swCH == hwCH)) {
  //       setSwLSB(i, fval); //***
  //       startMidiRx(i);
  //     }
  //   }


  // }

  // sends midi cc in sendValueToAbleton using MIDIUSB library
  // void sendControlChange(byte channel, byte control, byte value) {
  //   midiEventPacket_t event = {0x0B, 0xB0 | channel, control, value};
  //   MidiUSB.sendMIDI(event);
  // }
//

// convert and send cc to ableton
void sendValueToAbleton(int i, int val) {
  
  hwMSB[i] = val / 128;
  hwLSB[i] = val % 128;

  if (hwLSB[i] != oldHwLSB[i]) {
    // sendControlChange(hwCH, hwCC[i], hwMSB[i]);
    // MidiUSB.flush();
    Serial.print("encoder " + String(i+1));
    Serial.print("    ");

    Serial.print("hwVal: ");
    Serial.print(val);
    Serial.print("    ");
    
    Serial.print("MSB: ");
    Serial.print(hwMSB[i]);
    Serial.print("    ");
    
    // sendControlChange(hwCH, (hwCC[i] + 32), hwLSB[i]);
    // MidiUSB.flush();
    Serial.print("LSB: ");
    Serial.println(hwLSB[i]);

   oldHwLSB[i] = hwLSB[i];
  }

}

void setSwMSB(int i, int val) {
  swMSB[i] = val; 
}

void setSwLSB(int i, int val) {
  swLSB[i] = val; 
}

int getSwVal(int i) {

  swVal[i] = (swMSB[i] << 7) | swLSB[i];
  return swVal[i]; 
}

void startMidiRx(int i) {
  
  encRxStarted[i] = true;
  encLastRxTime[i] = millis();  // Stores the previous time
}

// returns true when midi is received within encRxTimeout
bool isReceivingMidi(int i) {
  
  if (encRxStarted[i] == true) {
    if ((millis() - encLastRxTime[i]) > encRxTimeout) {
      encRxStarted[i] = false;
    }
  }

  return encRxStarted[i];
}

// returns result of math operation // Used to adjust incoming software value for encSense
float lerp(float x1, float x2, float ratio) { 
  return x1 + (x2 - x1) * ratio; 
}

float lpf(int i, float target) {

  float timeout = getTimeout(target, lpfVal[i]);

  if ((millis() - lastLpfFollowTime[i]) > timeout) {
    lastLpfFollowTime[i] = millis();
    lpfVal[i] += alpha * (target - lpfVal[i]);

    return lpfVal[i];
  }
}

float getTimeout(int target, int follower) {
  
  float timeout;
  float error = abs(target - follower);

  timeout = mapWithThresh(error, errorMinThreshold, errorMaxThreshold, maxLpfTimeout, minLpfTimeout);

  return timeout;
}

int mapWithThresh(int val, int xMin, int xMax, int yMin, int yMax) {
  if (val > xMax) {
    return yMax;
  } else if (val < xMin) {
    return yMin;
  } else {
    return map(val, xMin, xMax, yMin, yMax);
  }
}

void encoders() {
 
 for (int i = 0; i < n_enc; i++) { 

    readHwVal(i); // reads hardware value, stops jitter, stores value in newHwVal (uses interrupts)
  
    if (isReceivingMidi(i)) {
      myEnc[i].write(getSwVal(i));
    
    } else {
      sendValueToAbleton(i, getHwVal(i));
    }

    isReceivingMidi(i);
  }
}

void setup() {
  
  Serial.begin(115200);
  Serial.println("<Arduino is ready>");
  Serial.println("");

  Serial.print("alpha == ");
  Serial.println(alpha);
  Serial.println("");
  Serial.print("i.e. encoder sensitivity is ");
  Serial.print(4 * encSense);
  Serial.println(" values per detent");
  Serial.println("");

  Serial.print("If error < ");
  Serial.print(errorMinThreshold);
  Serial.print(", the time for value changes is ");
  Serial.print(maxLpfTimeout);
  Serial.println("ms");
  Serial.println("");

  Serial.print("If error > ");
  Serial.print(errorMaxThreshold);
  Serial.print(", the time for value changes is ");
  Serial.print(minLpfTimeout);  
  Serial.println("ms");
  Serial.println("");

  Serial.print("If ");
  Serial.print(errorMinThreshold);
  Serial.print(" < ");
  Serial.print("error < ");
  Serial.print(errorMaxThreshold);
  Serial.print(", the time for value changes is mapped between the thresholds");
  Serial.println("");
}

void loop() {  
  //MIDIread();  // read software value -> lerp -> if midi receiving from software on encoder CH & CC -> starts timer
  encoders();
}