#include <Wire.h>
#include <MIDIUSB.h>
#include <MIDI.h>

#define COMMAND_HEADER  0 // Value of I2C command message header
#define CMD_RESET       0   // Reset command
#define DEBOUNCE_TIME   50 // Time in ms to ignore switch value change after previous change
#define ROT_THRESHOLD   10 // Threshold at which to change rotational scaling
#define ROT_FAST_SCALE  10 // Factor to multiply rotational change rate for fast scrolling

enum CONTROLLER_TYPE
{
    CONTROLLER_TYPE_SWITCH,
    CONTROLLER_TYPE_ENCODER
};

struct Controller
{
    bool dirty = false; // True if value has changed since last I2C read
    int16_t value = 0; // Value
    uint8_t count = 0; // Used to filter controller
    uint8_t code = 0; // Used to filter encoder controller
    uint8_t type; // Controller type (see CONTROLLER_TYPE)
    uint32_t time; // Time of last update (ms since boot)
    int16_t getValue() // Get the controller value with any required processing
    {
        int nValue = value;
        switch(type)
        {
        case CONTROLLER_TYPE_ENCODER:
            nValue = value;
            value = 0; // Reset relative position
            break;
        case CONTROLLER_TYPE_SWITCH:
            nValue = value;
        }
        return nValue;
    }
};

static const uint8_t anValid[] = {0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0}; // Table of valid encoder states
static const uint8_t SwitchInputPins[] = {10,16,14,15}; 
static const uint8_t EncoderInputPins[] = {4,5,6,7,18,19,20,21};
static const uint8_t InterruptPin = 9;
static const uint8_t MAX_CONTROLLERS = sizeof(SwitchInputPins) + sizeof(EncoderInputPins) / 2; // Quantity of controllers
static const uint8_t SWITCH_REG_START = 65; // Offset of on / off switch I2C register
static const uint8_t ENCODER_REG_START = 115; // Offset of rotary encoder I2C register
static const uint8_t SWITCH_START = 0; // Offset of on / off switch controls in controller table
static const uint8_t ENCODER_START = sizeof(SwitchInputPins); // Offset of rotary encoder controls in controller table
Controller g_anControllers[MAX_CONTROLLERS]; // Array of controller objects

uint8_t g_nI2Cregister = 0; // Address of I2C register requested by I2C master (index of controller - 1 based)
uint8_t g_nLastRead = 0; // The index of the controller last read

MIDI_CREATE_DEFAULT_INSTANCE();


void setup() {
    MIDI.begin(MIDI_CHANNEL_OFF);
    pinMode(LED_BUILTIN_TX,INPUT); //swithc rx and tx leds of, so they don't blink on midi
    pinMode(LED_BUILTIN_RX,INPUT);

    //Serial.stop;
    /*
    Serial.begin(9600);
    Serial.println("Starting");
    Serial.print("Controllers: ");
    Serial.println(MAX_CONTROLLERS);
    Serial.print("Encoders start at ");
    Serial.println(ENCODER_START);
    */
    
    for(uint8_t i = 0; i < sizeof(SwitchInputPins); ++i)
        pinMode(SwitchInputPins[i], INPUT_PULLUP);
    for(uint8_t i = 0; i < sizeof(EncoderInputPins); ++i)
        pinMode(EncoderInputPins[i], INPUT_PULLUP);

    pinMode(InterruptPin, OUTPUT);

    for(int i = 0; i < ENCODER_START; ++i)
        g_anControllers[i].type = CONTROLLER_TYPE_SWITCH;
    for(int i = ENCODER_START; i < MAX_CONTROLLERS; ++i)
        g_anControllers[i].type = CONTROLLER_TYPE_ENCODER;
    
    Wire.begin(8); // I2C address is 8
    Wire.onRequest(onI2Crequest);
    Wire.onReceive(onI2Creceive);

}

/** @brief  Get the next controller who's values has changed since last I2C request
 *  @retval uint8_t Index of controller (1 based) or 0 for none
 *  @note   Avoids one controller hogging bus by starting scan after last read controller
 */
uint8_t getDirty()
{
    // Iterate through all controllers looking for changed, starting at controller after last read then wrapping round
    for(uint8_t i = g_nLastRead; i < MAX_CONTROLLERS; ++i)
    {
        if(g_anControllers[i].dirty)
        {
            digitalWrite(InterruptPin, LOW);
            if(i < ENCODER_START) {
              return(i + SWITCH_REG_START);
            }
            else {
              return(i - ENCODER_START + ENCODER_REG_START);
            }
        }
    }
    for(uint8_t i = 0; i < g_nLastRead; ++i)
    {
        if(g_anControllers[i].dirty)
        {
            digitalWrite(InterruptPin, LOW);
            if(i < ENCODER_START) {
              return(i + SWITCH_REG_START);
            }
            else {
              return(i - ENCODER_START + ENCODER_REG_START);
            }
        }
    }
    digitalWrite(InterruptPin, HIGH);
    return 0;
}

/** @brief  Reset all controller values
 *  @todo   Should we set values to current readings?
 */
void reset()
{
    for(uint8_t i = 0; i < MAX_CONTROLLERS; ++i)
    {
        g_anControllers[i].value = 0;
        g_anControllers[i].dirty = false;
    }
    g_nLastRead = 0;
    digitalWrite(InterruptPin, HIGH);
    //Serial.println("RESET");
}

void sendReg(uint8_t nReg)
{
    //Wire.flush();
    uint16_t nValue = 0;
    uint8_t data[2];
    if(nReg >= SWITCH_REG_START && nReg < SWITCH_REG_START + sizeof(SwitchInputPins))
    {
      nValue = g_anControllers[nReg - SWITCH_REG_START].value;
      g_anControllers[nReg - SWITCH_REG_START].dirty = false;
      g_nLastRead = nReg - SWITCH_REG_START + 1;
    }
    else if(nReg >= ENCODER_REG_START && nReg < ENCODER_REG_START + sizeof(EncoderInputPins) / 2)
    {
      nValue = g_anControllers[nReg - ENCODER_REG_START + ENCODER_START].value;
      g_anControllers[nReg - ENCODER_REG_START + ENCODER_START].dirty = false;
      g_anControllers[nReg - ENCODER_REG_START + ENCODER_START].value = 0;
      g_nLastRead = nReg - ENCODER_REG_START + ENCODER_START + 1;
    }
    if(g_nLastRead >= MAX_CONTROLLERS)
      g_nLastRead = 0;
    data[0] = nValue & 0xFF;
    data[1] = (nValue & 0xFF00) >> 8;
    Wire.write(data, 2);
    getDirty();
    /*
    Serial.println();
    Serial.print("I2C Tx reg: ");
    Serial.print(nReg);
    Serial.print(" value: ");
    Serial.print(nValue);
    Serial.print(":");
    Serial.print(g_anControllers[nValue].value);
    Serial.print(":");
    Serial.print(data[0]);
    Serial.print(data[1]);
    Serial.println();
    */
}

void onI2Crequest() {
    if(g_nI2Cregister)
    {
        // Send value of requested controller
        sendReg(g_nI2Cregister);
        //Serial.println(g_nI2Cregister);
        g_nI2Cregister = 0;
        return;
    }
    // Send index of first changed controller or zero if all clean
    //Wire.flush(); // Seem to need to flush I2C to ensure single byte of data is sent correctly
    Wire.write(getDirty());
}

void onI2Creceive(int nBytes) {
    if(nBytes < 1)
        return;
    int nValue = Wire.read();
    if(nValue == COMMAND_HEADER && nBytes > 1)
    {
        nValue = Wire.read();
        switch(nValue)
        {
            case CMD_RESET:
                reset();
                break;
        }
    }
    else
    {
        g_nI2Cregister = nValue;
    }
    while(Wire.available())
        Wire.read(); // Clear buffer of unexpected bytes
}

void readEncoder() {
    int8_t nDir = 0; // Direction of rotation [-1, 0, +1]

    for(uint8_t n = 0; n < sizeof(EncoderInputPins) / 2; ++n) {
        int nEncoder = n + ENCODER_START;
        int nClk = digitalRead(EncoderInputPins[n*2]);
        if(!nClk && !g_anControllers[nEncoder].code)
            continue; // Ignore encoder if clock not asserted or not mid-decode
        int nData = digitalRead(EncoderInputPins[n*2+1]);
        g_anControllers[nEncoder].code <<= 2;
        if(nData)
            g_anControllers[nEncoder].code |= 0x02;
        if(nClk)
            g_anControllers[nEncoder].code |= 0x01;
        g_anControllers[nEncoder].code &= 0x0f;
        // If valid then add to 8-bit history validate rotation codes and process
        if(anValid[g_anControllers[nEncoder].code]) {
            g_anControllers[nEncoder].count <<= 4;
            g_anControllers[nEncoder].count |= g_anControllers[nEncoder].code;
            if(g_anControllers[nEncoder].count == 0xd4)
                nDir = +1;
            else if(g_anControllers[nEncoder].count == 0x17)
                nDir = -1;
            if(nDir)
            {
                //Inhibit fast rotation as it doesn't really work well
                //uint32_t lNow = millis();
                //if(lNow >= g_anControllers[nEncoder].time + ROT_THRESHOLD)
                    g_anControllers[nEncoder].value += nDir; //Slow rotation
                //else
                    //g_anControllers[nEncoder].value += nDir * ROT_FAST_SCALE; //Fast rotation
                //g_anControllers[nEncoder].time = lNow;
                g_anControllers[nEncoder].code = 0;
                g_anControllers[nEncoder].dirty = true;
                /*
                Serial.print("Encoder ");
                Serial.print(nEncoder);
                Serial.print(":");
                Serial.print(g_anControllers[nEncoder].value);
                Serial.println("");
                */
            }
        }
    }
}

void readSwitch() {
    for(uint8_t n = 0; n < sizeof(SwitchInputPins); ++n) {
        int nSwitch = n;
        int nValue = !digitalRead(SwitchInputPins[n]);//*(-1))+1;
        if(nValue != g_anControllers[nSwitch].value && millis() > g_anControllers[nSwitch].time + DEBOUNCE_TIME)
        {
            //  Note: Initial testing suggests that debounce is not required but it is added to handle noisy switches
            g_anControllers[nSwitch].value = nValue;
            g_anControllers[nSwitch].time = millis();
            g_anControllers[nSwitch].dirty = true;
            /*
            Serial.print("Switch ");
            Serial.print(SWITCH_START + nSwitch);
            Serial.print(":");
            Serial.print(g_anControllers[SWITCH_START + nSwitch].value);
            Serial.println("");
            */
        }
    }
}

void loop() {
  readSwitch();
  readEncoder();
  getDirty();
  USBtoMIDI();
}

void USBtoMIDI() {
  midiEventPacket_t rx;
  do {
    rx = MidiUSB.read();
    if (rx.header != 0) {
      MIDI.send(rx.byte1, rx.byte2, rx.byte3, 0x01);     
      
      /*
      Serial.print("Received: ");
      Serial.print(rx.header, HEX);
      Serial.print("-");
      Serial.print(rx.byte1, DEC);
      Serial.print("-");
      Serial.print(rx.byte2, DEC);
      Serial.print("-");
      Serial.println(rx.byte3, DEC);
      Serial.println(rx.byte1-(rx.header << 4), DEC); //Channel
      */
      
    }
  } while (rx.header != 0);
}