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

#define DEBUG
#undef DEBUG

#define VALVE_1 7
#define VALVE_2 8
#define VALVE_3 9

#define NUM_VALVES 3

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

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

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

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

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

// 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

  // initialize valve output pins
  for (uint8_t i = 0; i < NUM_VALVES; i++) {
    pinMode(valves[i], 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 = 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;
}

void loop() {
  // 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) { // handle note on
      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)) { // 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
    } 
    #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 
  }

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