#include "MIDIUSB.h" // see https://github.com/arduino-libraries/MIDIUSB
#include <math.h>

#define DEBUG
#undef DEBUG

#define MIDI_IN_PIN 0

#define OVERALL_VELOCITY_PIN 10

#define VALVE_1 7     // 6mm
#define VALVE_2 6     // 6mm
#define VALVE_3 9     // 6mm
#define VALVE_4 8     // 4mm
#define VALVE_5 5     // 4mm

#define NUM_VALVES 5

// array of currently active notes, index 0 is the oldest one
int16_t notes[NUM_VALVES] = {-1, -1, -1, -1, -1};

// array of the length of the tone period required to create the tone
uint16_t periods[NUM_VALVES] = {0, 0, 0, 0, 0};

// array of the current state of the valve pins (for tone periods)
uint8_t states[NUM_VALVES] = {0, 0, 0, 0, 0};

// array for the period timer helpers
uint64_t lastNoteMicros[NUM_VALVES] = {0, 0, 0, 0, 0};

// array of the valve pin numbers
const uint8_t valves[NUM_VALVES] = {VALVE_1, VALVE_2, VALVE_3, VALVE_4, VALVE_5};

// MIDI parameter overview
// First parameter is the event type (0x0B = control change).
// Second parameter is the event type, combined with the channel.
// Third parameter is the control number number (0-119).
// Fourth parameter is the control value (0-127).

void setup() {
  #ifdef DEBUG
    Serial.begin(115200);
  #endif

  Serial1.begin(31250); // MIDI baud rate

  // initialize valve output pins
  for (uint8_t i = 0; i < NUM_VALVES; i++) {
    pinMode(valves[i], OUTPUT);
  }

  pinMode(OVERALL_VELOCITY_PIN, OUTPUT);
}

// calculates the frequency according to the midi note number
uint16_t midi2frq(uint8_t number) {
  #ifdef DEBUG
    Serial.print(number);
    Serial.print(" | ");
  #endif

  double _n = (double)((double)number - (double)69);

  #ifdef DEBUG
    Serial.print(_n);
    Serial.print(" | ");
  #endif

  double d = 0;

  /*if (number < 48) { // c3
    d = pow((double)2, _n / (double)12) * (double)440 * (double) 1.015;
  } else if (number < 57) { // c4
    d = pow((double)2, _n / (double)12) * (double)440 * (double) 1.02;
  } else if (number < 60) { // c5
    d = pow((double)2, _n / (double)12) * (double)440 * (double) 1.025;
  } else {
    d = pow((double)2, _n / (double)12) * (double)440 * (double) 1.04;
  }*/

  d = pow((double)2, _n / (double)12) * (double)440;

  #ifdef DEBUG
    Serial.println(d);
  #endif

  return (uint16_t)d;
}

// puts the note into the notes array or removes it if already present
int8_t putNote(int8_t note, uint8_t state) {
  // set note
  for (uint8_t i = 0; i < NUM_VALVES; i++) {
    if (notes[i] == -1 && state == 1) { // note on
      notes[i] = note;
      return i;
    } else if (notes[i] == note && state == 0) { // note off
      //notes[i] = -1;
      for (uint8_t j = i; j < NUM_VALVES - 1; j++) {
        notes[j] = notes[j + 1];
        periods[j] = periods[j + 1];
        states[j] = states[j + 1];
        lastNoteMicros[j] = lastNoteMicros[j + 1];
      }
      notes[NUM_VALVES - 1] = -1;
      periods[NUM_VALVES - 1] = 0;
      states[NUM_VALVES - 1] = 0;
      lastNoteMicros[NUM_VALVES - 1] = 0;
      return i;
    }
  }

  if (state == 0) {
    return -1;
  }

  // remove oldest note and shift other notes
  for (int8_t i = 0; i < NUM_VALVES - 1; i++) {
    notes[i] = notes[i + 1];
  }

  notes[NUM_VALVES - 1] = note;
  return NUM_VALVES - 1;
}

int counter = 0;

void loop() {
  if (counter == 100) { 
    counter = 0;
    // handle USB MIDI IN
    midiEventPacket_t rx;
    rx = MidiUSB.read();
    if (rx.byte1 != 0) {
      #ifdef DEBUG
        char buffer[256];
        sprintf(buffer, "header: 0x%X  b1: %d  b2: %d  b3: %d", rx.header, rx.byte1,
                rx.byte2, rx.byte3);
        Serial.println(buffer);
      #endif
  
      if ((rx.byte1 & 0xF0) == 0x90 && rx.byte3 > 0 && rx.byte2 < 84) { // handle note on // 84 = C6 = 1046.5Hz
        int8_t noteIdx = putNote(rx.byte2, 1);
        uint16_t frq = midi2frq(rx.byte2);
  
        periods[noteIdx] = 1000000 / frq / 2;
  
        #ifdef DEBUG
          Serial.println("ONNNNNNN " + String(rx.byte2) + " @ " + String(noteIdx));
        #endif
      } else if (((rx.byte1 & 0xF0) == 0x80 || ((rx.byte1 & 0xF0) == 0x90 && rx.byte3 == 0)) && rx.byte2 < 84) { // handle note off
        #ifdef DEBUG
          int8_t idx = 0;
        #endif
  
        for (int8_t i = 0; i < NUM_VALVES; i++) {
          if (notes[i] != -1 && notes[i] == rx.byte2) {
            putNote(rx.byte2, 0);
            //periods[i] = 0;
  
            #ifdef DEBUG
              idx = i;
            #endif
  
            break;
          }
        }
  
        #ifdef DEBUG
          Serial.println("OFFFFFFFF " + String(rx.byte2) + " @ " + String(idx));
        #endif
      } else if ((rx.byte1 & 0xF0) == 0xB0) { // handle CC
        if (rx.byte2 == 7) { // CC 7 = Volume
          analogWrite(OVERALL_VELOCITY_PIN, 255 - (rx.byte3 * 2));
        }
      }
      #ifdef DEBUG
        else {
          char buffer[256];
          sprintf(buffer, "header: 0x%X  b1: %d  b2: %d  b3: %d", rx.byte1, rx.byte2, rx.byte3); 
          Serial.println(buffer);
        }
      #endif 
    }
  } else {
    counter++;
  }

/*  // handle Hardware MIDI IN
  if (Serial1.available() >= 3) {
    uint8_t statusByte = Serial1.read();
    uint8_t dataByte1 = Serial1.read();
    uint8_t dataByte2 = Serial1.read();

    if ((statusByte & 0x09) == 0x09) { // handle note on

    } else if ((statusByte & 0x08) == 0x08) { // handle note off

    }
  }
*/

  // get current microseconds
  uint64_t nowMicros = micros();

  // check for notes that require attention
  for (uint8_t i = 0; i < NUM_VALVES; i++) {
    if (lastNoteMicros[i] + periods[i] < nowMicros && notes[i] != -1) {
      lastNoteMicros[i] = nowMicros;
      states[i] ^= 1;
      digitalWrite(valves[i], states[i]);
    } else if (notes[i] == -1) { // be sure to end notes that should be ended
      digitalWrite(valves[i], LOW);
    }
  }
}