#include "stateMachine.h"
#include <SPI.h>
#include <Wire.h>
#include "display.h"
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MIDI.h>
#include "Rotary.h"

#define pushButton PIN_PE4
Rotary r = Rotary(PIN_PE5, PIN_PE6);

MIDI_CREATE_INSTANCE(HardwareSerial, Serial1, MyMIDI);



unsigned char result = r.process();

enum States{LEDs, CHANNEL, INSTRUMENT};

int counter = 0;
States State = LEDs;

const int DEBOUNCE_DELAY = 70;

// Variables will change:
int lastSteadyState = LOW;       // the previous steady state from the input pin
int lastFlickerableState = LOW;  // the previous flickerable state from the input pin
int currentState;                // the current reading from the input pin

// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled

bool Set = 1;
bool Led = 1;
bool Inst = 1;
int MIDIChannel = 0;

unsigned long previousMillis = 0;        
const long interval = 35; 
unsigned long currentMillis = millis();
bool tmp = 1;

void StateMachine(){
  currentMillis = millis();
  if(Inst == 0 && currentMillis - previousMillis >= interval){
      for (size_t i = 0; i < sizeof(myPins)/2  - 1; i++)
      {
        if (digitalRead(myPins[i]) == 1) digitalWrite(myPins[i],LOW);
      }      
  }
  
  switch (State)
  {
    case LEDs:
      currentState = digitalRead(pushButton);

      if (currentState != lastFlickerableState) {
        lastDebounceTime = millis();
        lastFlickerableState = currentState;
      }
      if ((millis() - lastDebounceTime) > DEBOUNCE_DELAY) {
        if (lastSteadyState == HIGH && currentState == LOW){
            display.clearDisplay(); 
            displayPrint("LEDs", 'L', 0, 2, 'W');           
            if(Led == 1) displayPrint("On", 'C', 2, 2, Set);
            else displayPrint("Off", 'C', 2, 2, Set);
            display.display();
        }
        else if (lastSteadyState == LOW && currentState == HIGH){
          Set = !Set;
          digitalWrite(PIN_PE3, Led);
        }
        lastSteadyState = currentState;
      }

      result = r.process();
      if (result == DIR_CW && Set == 0) {
        display.clearDisplay(); 
        displayPrint("CHANNEL", 'L', 0, 2, 'W'); 
        if(MIDIChannel != 0) displayPrint(MIDIChannel, 'C', 2, 2, 'W');
        else displayPrint("All", 'C', 2, 2, 'W');
        display.display();

        State = CHANNEL;
      }
      else if (result == DIR_CCW && Set == 0) {
        display.clearDisplay(); 
        displayPrint("INSTRUMENT", 'L', 0, 2, 'W'); 
        if(Inst == 1) displayPrint("Harm", 'C', 2, 2, 'W');
        else displayPrint("Meta", 'C', 2, 2, 'W');
        display.display();

        State = INSTRUMENT;
      }
      else if (result == DIR_CW && Set == 1) {
        Led = 1;
        display.clearDisplay(); 
        displayPrint("LEDs", 'L', 0, 2, 'W');           
        if(Led == 1) displayPrint("On", 'C', 2, 2, 'B');
        else displayPrint("Off", 'C', 2, 2, 'B');
        display.display();
      }
      else if (result == DIR_CCW && Set == 1){
        Led = 0;
        display.clearDisplay(); 
        displayPrint("LEDs", 'L', 0, 2, 'W');           
        if(Led == 1) displayPrint("On", 'C', 2, 2, 'B');
        else displayPrint("Off", 'C', 2, 2, 'B');
        display.display();
      }
    break;

    case CHANNEL:
      currentState = digitalRead(pushButton);

        if (currentState != lastFlickerableState) {
          lastDebounceTime = millis();
          lastFlickerableState = currentState;
        }
        if ((millis() - lastDebounceTime) > DEBOUNCE_DELAY) {
          if (lastSteadyState == HIGH && currentState == LOW){
            display.clearDisplay(); 
            displayPrint("CHANNEL", 'L', 0, 2, 'W');           
            if(MIDIChannel != 0) displayPrint(MIDIChannel, 'C', 2, 2, Set);
            else displayPrint("All", 'C', 2, 2, Set);
            display.display();
          }
          else if (lastSteadyState == LOW && currentState == HIGH){
            Set = !Set;
            MyMIDI.begin(MIDIChannel);
          }
          lastSteadyState = currentState;
        }

      result = r.process();
      if (result == DIR_CW && Set == 0) {
        display.clearDisplay(); 
        displayPrint("INSTRUMENT", 'L', 0, 2, 'W');           
        if(Inst == 1) displayPrint("Harm", 'C', 2, 2, 'W');
        else displayPrint("Meta", 'C', 2, 2, 'W');
        display.display();

        State = INSTRUMENT;
      }
      else if (result == DIR_CCW && Set == 0) {
        display.clearDisplay(); 
        displayPrint("LEDs", 'L', 0, 2, 'W');           
        if(Led == 1) displayPrint("On", 'C', 2, 2, 'W');
        else displayPrint("Off", 'C', 2, 2, 'W');
        display.display();

        State = LEDs;
      }
      else if (result == DIR_CW && Set == 1) {
        if(MIDIChannel != 16) MIDIChannel++;
        display.clearDisplay(); 
        displayPrint("CHANNEL", 'L', 0, 2, 'W');           
        if(MIDIChannel != 0) displayPrint(MIDIChannel, 'C', 2, 2, 'B');
        else displayPrint("All", 'C', 2, 2, 'B');
        display.display();
      }
      else if (result == DIR_CCW && Set == 1) {
        if(MIDIChannel != 0) MIDIChannel--;
        display.clearDisplay(); 
        displayPrint("CHANNEL", 'L', 0, 2, 'W');           
        if(MIDIChannel != 0) displayPrint(MIDIChannel, 'C', 2, 2, 'B');
        else displayPrint("All", 'C', 2, 2, 'B');
        display.display();
      }
    break;

    case INSTRUMENT:
      currentState = digitalRead(pushButton);

        if (currentState != lastFlickerableState) {
          lastDebounceTime = millis();
          lastFlickerableState = currentState;
        }
        if ((millis() - lastDebounceTime) > DEBOUNCE_DELAY) {
          if (lastSteadyState == HIGH && currentState == LOW){
              display.clearDisplay(); 
              displayPrint("INSTRUMENT", 'L', 0, 2, 'W');           
              if(tmp == 1) displayPrint("Harm", 'C', 2, 2, Set);
              else displayPrint("Meta", 'C', 2, 2, Set);
              display.display();
          }
          else if (lastSteadyState == LOW && currentState == HIGH){
            Set = !Set;
            if(tmp == 0) Inst = 0;
            else if (tmp == 1) Inst = 1;
          }
          lastSteadyState = currentState;
        }

      result = r.process();
      if (result == DIR_CW && Set == 0) {
        display.clearDisplay(); 
        displayPrint("LEDs", 'L', 0, 2, 'W');           
        if(Led == 1) displayPrint("On", 'C', 2, 2, 'W');
        else displayPrint("Off", 'C', 2, 2, 'W');
        display.display();

        State = LEDs;
      }
      else if (result == DIR_CCW && Set == 0) {
        display.clearDisplay(); 
        displayPrint("CHANNEL", 'L', 0, 2, 'W'); 
        if(MIDIChannel != 0) displayPrint(MIDIChannel, 'C', 2, 2, 'W');
        else displayPrint("All", 'C', 2, 2, 'W');
        display.display();

        State = CHANNEL;
      }
      else if (result == DIR_CW && Set == 1) {
        tmp = 1;
        display.clearDisplay(); 
        displayPrint("INSTRUMENT", 'L', 0, 2, 'W');           
        if(tmp == 1) displayPrint("Harm", 'C', 2, 2, 'B');
        else displayPrint("Meta", 'C', 2, 2, 'B');
        display.display();
      }
      else if (result == DIR_CCW && Set == 1) {
        tmp = 0;
        display.clearDisplay(); 
        displayPrint("INSTRUMENT", 'L', 0, 2, 'W');           
        if(tmp == 1) displayPrint("Harm", 'C', 2, 2, 'B');
        else displayPrint("Meta", 'C', 2, 2, 'B');
        display.display();
      }
    break;
  }
}