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

#include "hid.h" // Modified HID library: doesn't prefix each packet with ID
#include "instrument.h"

//#include "blue.h"  // Ginnie's blue guitar
#include "standard.h"  // Standard pins

InstrumentButtonState buttonState;
InstrumentButtonState lastState;

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(ANALOG_WAMMY, INPUT);
  pinMode(BUTTON_PLUS, INPUT_PULLUP);
  pinMode(BUTTON_MINUS, INPUT_PULLUP);

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

  buttonState.buttons = 0x0000;
  buttonState.hatAndConstant = 0x08;
  buttonState.axis[0] = 0;
  buttonState.axis[1] = 0;
  buttonState.axis[2] = 0;
  buttonState.axis[3] = 0;
  for (int i = 0; i < 12; i++) {
    buttonState.reserved1[i] = 0x0;
  }
  buttonState.finalConstant = 0x0200020002000200;
  buttonState.buttons = 0;
  buttonState.hatAndConstant = 0x08; // This apparently means "nothing pressed"
}

// The 3.3v Pro Micro is on the slow side.
// Our strategy is to poll button state as quickly as possible,
// send an update every 10ms,
// and hope we don't miss anything while we're doing USB stuff.
void loop() {
  static uint16_t buttons = 0;
  static unsigned long next = 0;
  unsigned long now = millis();

  buttons |= (0
    | (digitalRead(BUTTON_BLUE)   <<  0)
    | (digitalRead(BUTTON_GREEN)  <<  1)
    | (digitalRead(BUTTON_RED)    <<  2)
    | (digitalRead(BUTTON_YELLOW) <<  3)
    | (digitalRead(BUTTON_ORANGE) <<  4)
    | (digitalRead(TILT_SWITCH)   <<  5)
    | (digitalRead(STRUM_UP)      <<  6) // Not in USB packet
    | (digitalRead(STRUM_DOWN)    <<  7) // Not in USB packet
    | (digitalRead(BUTTON_MINUS)  <<  8)
    | (digitalRead(BUTTON_PLUS)   <<  9)
    | (digitalRead(SOLO_BLUE)     << 10) // Not in USB packet
    | (digitalRead(SOLO_GREEN)    << 11) // Not in USB packet
    | (digitalRead(SOLO_RED)      << 12) // Not in USB packet
    | (digitalRead(SOLO_YELLOW)   << 13) // Not in USB packet
    | (digitalRead(SOLO_ORANGE)   << 14) // Not in USB packet
  );

  if (next > now) {
    return;
  }

  // Open pullup reads 1, so invert everything we've read
  buttons = ~buttons;

  buttonState.buttons = (buttons & 0b1100111111);      // All directly-mappable bits
  buttonState.buttons |= ((buttons >> 10) & 0b11111);  // Solo keys
  bitWrite(buttonState.buttons, 6, (buttons >> 10) & 0b11111); // Solo bit

  // I don't understand why any rational engineer would have done this.
  if (bitRead(buttons, 6)) {
    buttonState.hatAndConstant = 0x00; // up
  } else if (false) {
    buttonState.hatAndConstant = 0x02; // right
  } else if (bitRead(buttons, 7)) {
    buttonState.hatAndConstant = 0x04; // down
  } else if (false) {
    buttonState.hatAndConstant = 0x06; // left
  } else {
    buttonState.hatAndConstant = 0x08; // nothing
  }

  buttonState.axis[2] = analogRead(ANALOG_WAMMY) / 4;

  // Send an update
  HID().SendReport(0, (uint8_t *)&buttonState, 27);
  
  next = now + 10;
  buttons = 0;
}