#include <stdint.h>
#include <Arduino.h>
#include <PluggableUSB.h>

#include "hid.hh" // Modified HID library: doesn't prefix each packet with ID
#include "instrument.hh"
#include "standard.hh"  // Standard pins

// If defined, we will check the wammy bar input
//#define WAMMY

// Maximum time between wammy bar updates.
#define UPDATE_INTERVAL_MS 20

// After an edge on a pin, stop listening for this long, to debounce it
#define SILENCE_INTERVAL_MS 40

// Your measured samples per frame, more or less
#define SAMPLES_PER_FRAME 127

// Some arithmetic for the compiler, to make the code fast
#define SAMPLES_PER_MS (SAMPLES_PER_FRAME / UPDATE_INTERVAL_MS)
#define SILENCE_SAMPLES (SAMPLES_PER_MS * SILENCE_INTERVAL_MS)

#if USB_VID != 0x1bad
#error USB_VID must be set to 0x1bad: see INSTALL.md
#endif

#if USB_PID == 0x0003
#define DRUM  // XBox
#elif USB_PID == 0x0004
#define GUITAR
#elif USB_PID == 0x0005
#define DRUM  // Wii RB1
#elif USB_PID == 0x3110
#define DRUM  // Wii RB2
#else
#error USB_PID not recognized: see INSTALL.md
#endif

InstrumentButtonState buttonState = {0};

void setup() {
  pinMode(STRUM_DOWN, INPUT_PULLUP);
  pinMode(STRUM_UP, INPUT_PULLUP);
  pinMode(TILT_SWITCH, INPUT_PULLUP);
  pinMode(BUTTON_GREEN, INPUT_PULLUP);
  pinMode(BUTTON_RED, INPUT_PULLUP);
  pinMode(BUTTON_YELLOW, INPUT_PULLUP);
  pinMode(BUTTON_BLUE, INPUT_PULLUP);
  pinMode(BUTTON_ORANGE, INPUT_PULLUP);
  pinMode(SOLO_GREEN, INPUT_PULLUP);
  pinMode(SOLO_RED, INPUT_PULLUP);
  pinMode(SOLO_YELLOW, INPUT_PULLUP);
  pinMode(SOLO_BLUE, INPUT_PULLUP);
  pinMode(SOLO_ORANGE, INPUT_PULLUP);
  pinMode(BUTTON_PLUS, INPUT_PULLUP);
  pinMode(BUTTON_MINUS, INPUT_PULLUP);
  pinMode(ANALOG_WAMMY, INPUT);
  pinMode(ANALOG_DPAD, INPUT);

  // Initialize HID
  static HIDSubDescriptor node(_hidReportDescriptor, sizeof(_hidReportDescriptor));
  HID().AppendDescriptor(&node);  

  buttonState.finalConstant = 0x0200020002000200;
}

// Order of pins in sample
uint8_t pins[] = {
  BUTTON_BLUE,
  BUTTON_GREEN,
  BUTTON_RED,
  BUTTON_YELLOW,
  BUTTON_ORANGE,
  TILT_SWITCH,
  STRUM_UP,        // Not in USB packet
  STRUM_DOWN,      // Not in USB packet
  BUTTON_MINUS,
  BUTTON_PLUS,
  SOLO_BLUE,       // Not in USB packet
  SOLO_GREEN,      // Not in USB packet
  SOLO_RED,        // Not in USB packet
  SOLO_YELLOW,     // Not in USB packet
  SOLO_ORANGE,     // Not in USB packet
};
#define npins (sizeof(pins) / sizeof(*pins))

// The 3.3v Pro Micro is on the slow side.
// Our strategy is to poll button state as quickly as possible,
// and hope we don't miss anything while we're doing USB stuff.
void loop() {
  uint16_t buttons = 0;
  uint16_t samples = 0;
  unsigned long next = 0;
  uint16_t silence[npins] = {0};

  while (1) {
    uint16_t edge = 0;

    samples++;

    for (uint8_t i = 0; i < npins; i++) {
      if (silence[i]) {
        silence[i]--;
      } else if (bitRead(buttons, i) != !digitalRead(pins[i])) {
        edge |= bit(i);
        silence[i] = SILENCE_SAMPLES;
      }
    }
    buttons ^= edge;

    // We've sampled everything. Is it time to do calculations and USB?
#ifdef WAMMY
    unsigned long now = millis();
    if (!edge && (next > now)) {
      continue;
    }
    next = now + UPDATE_INTERVAL_MS;
    buttonState.axis[2] = analogRead(ANALOG_WAMMY) / 4; // Wammy bar
#else
    if (!edge) {
      continue;
    }
#endif

    //
    // Calculate and send an HID update
    //
    uint16_t vbuttons = buttons; // We're going to mess with the button state
    
    buttonState.buttons = (vbuttons & 0b1100111111);     // +-..!OYRGB
#ifdef GUITAR
    buttonState.buttons |= (vbuttons >> 10) & 0b11111;   // Solo keys
    bitWrite(buttonState.buttons,  6, (vbuttons >> 10) & 0b11111);  // Solo modifier

    if (bitRead(vbuttons, 6)) {
      buttonState.hatAndConstant = 0; // up
    } else if bitRead(vbuttons, 7) {
      buttonState.hatAndConstant = 4; // down
    } else {
      buttonState.hatAndConstant = 8; // nothing
    }
#else // DRUMS

    // Hi hat pedal (SOLO_RED) makes yellow cymbal strike a blue cymbal strike
    if (bitRead(vbuttons, 12) && bitRead(vbuttons, 13)) {
      bitClear(vbuttons, 13);
      bitSet(vbuttons, 10);
    }

    buttonState.buttons |= (vbuttons >> 10) & 0b01011;   // Cymbals
    bitWrite(buttonState.buttons, 10, (vbuttons >>  0) & 0b01111);  // Drum pad modifier
    bitWrite(buttonState.buttons, 11, (vbuttons >> 10) & 0b01011);  // Cymbals modifier


    // rbdrum2midi wants these set: it ignores the button states.
    buttonState.velocity[0] = bitRead(buttonState.buttons, 3)?127:0; // Y
    buttonState.velocity[1] = bitRead(buttonState.buttons, 2)?127:0; // R 
    buttonState.velocity[2] = bitRead(buttonState.buttons, 1)?127:0; // G
    buttonState.velocity[3] = bitRead(buttonState.buttons, 0)?127:0; // B

    // Clone Hero 1.0.0.4080-final needs blue and yellow cymbals to send up and down on d-pad.
    // This is what the mysterous CymExt1 and CymExt2 mappings mean.
    // If these aren't set, all pads (except red) register as simultaneous drum and cymbal hits.
    if (bitRead(vbuttons, 13)) {
      buttonState.hatAndConstant = 0; // up
    } else if (bitRead(vbuttons, 10)) {
      buttonState.hatAndConstant = 4; // down
    } else {
      buttonState.hatAndConstant = 8; // nothing
    }
#endif

#ifdef DPAD
#error DPAD isn't implemented yet
#endif

#ifdef DEBUGY0
    // Log sample rate to the first Y axis
    buttonState.axis[1] = samples & 0xff;
#endif

    // Send an update
    HID().SendReport(0, (uint8_t *)&buttonState, 27);

    samples = 0;
  }
}