#include "MIDIUSB.h"
#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 period[NUM_VALVES] = {0, 0, 0};

// array of the current state of the valve pins (for tone periods)
uint8_t state[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
uint8_t putNote(int8_t note) {
  // set note
  for (int8_t i = 0; i < NUM_VALVES; i++) {
    if (notes[i] == -1) { // note on
      notes[i] = note;
      return i;
    } else if (notes[i] == note) { // note off
      notes[i] = -1;
      for (int8_t j = i; j < NUM_VALVES - 1; j++) {
        notes[j] = notes[j + 1];
      }
      notes[NUM_VALVES - 1] = -1;
      return i;
    }
  }

  // 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() {
  midiEventPacket_t rx;
  rx = MidiUSB.read();
  if (rx.header != 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.header & 0x09) == 0x09) { // handle note on
      uint8_t noteIdx = putNote(rx.byte2);
      uint16_t frq = midi2frq(rx.byte2);

      period[noteIdx] = 1000000 / frq / 2;

      #ifdef DEBUG
        Serial.println("ONNNNNNN " + String(rx.byte2) + " @ " + String(noteIdx));
      #endif
    } else if ((rx.header & 0x08) == 0x08) { // 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);
          period[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] + period[i] < nowMicros && notes[i] != -1) {
      lastNoteMicros[i] = nowMicros;
      state[i] ^= 1;
      digitalWrite(valves[i], state[i]);
    } else if (notes[i] == -1) { // be sure to end notes that should be ended
      digitalWrite(valves[i], LOW);
    }
  }
}