#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// #define WIRE Wire1  //only if display needs it.
#define SCREEN_WIDTH 128     // OLED display width, in pixels
#define SCREEN_HEIGHT 64     // OLED display height, in pixels
#define OLED_RESET -1        // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C  ///< See datasheet for Address; 0x3D for 128x64, 0x3C for OLED FeatherWing 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Screen buttons
const int buttonAPin = 0;
const int buttonBPin = 1;
const int buttonCPin = 2;
int buttonAState;
int buttonBState;
int buttonCState;
int lastButtonAState;
int lastButtonBState;
int lastButtonCState;
unsigned long lastDebounceATime = 0;
unsigned long lastDebounceBTime = 0;
unsigned long lastDebounceCTime = 0;
unsigned long debounceDelay = 50;

int userChannel = 1;
int userProgOffset = 0;

#include <MIDI.h>

#if defined(USE_TINYUSB_HOST) || !defined(USE_TINYUSB)
#error "Please use the Menu to select Tools->USB Stack: Adafruit TinyUSB"
#endif
#include "pio_usb.h"
#define HOST_PIN_DP 16  // Pin used as D+ for host, D- = D+ + 1
#include "EZ_USB_MIDI_HOST.h"

// USB Host object
Adafruit_USBH_Host USBHost;

USING_NAMESPACE_MIDI
USING_NAMESPACE_EZ_USB_MIDI_HOST

RPPICOMIDI_EZ_USB_MIDI_HOST_INSTANCE(usbhMIDI, MidiHostSettingsDefault)

Adafruit_USBD_MIDI usb_midi;                                  // USB MIDI object
MIDI_CREATE_INSTANCE(Adafruit_USBD_MIDI, usb_midi, MIDIusb);  // USB MIDI
MIDI_CREATE_INSTANCE(HardwareSerial, Serial1, MIDIuart);      // Serial MIDI over MIDI FeatherWing

static uint8_t midiDevAddr = 0;

static bool core0_booting = true;
static bool core1_booting = true;

/* MIDI IN MESSAGE REPORTING */
static void onMidiError(int8_t errCode) {
  Serial.printf("MIDI Errors: %s %s %s\r\n", (errCode & (1UL << ErrorParse)) ? "Parse" : "",
                (errCode & (1UL << ErrorActiveSensingTimeout)) ? "Active Sensing Timeout" : "",
                (errCode & (1UL << WarningSplitSysEx)) ? "Split SysEx" : "");
}
int last_cc_cntrl = 1;

static void onControlChange(Channel channel, byte controller, byte value) {
  MIDIusb.sendControlChange(controller, value, userChannel);
  MIDIuart.sendControlChange(controller, value, userChannel);
  Serial.printf("Ch %u CC#%u=%u\r\n", userChannel, controller, value);
  if (last_cc_cntrl != controller) {
    display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
    display.setTextWrap(false);
    display.setCursor(0, 12);
    display.printf("CC# %u                \r\n", controller);
    display.display();
    last_cc_cntrl = controller;
  }
}

static void onMidiClock() {
  Serial.printf("Clock\r\n");
  MIDIusb.sendClock();
  MIDIuart.sendClock();
}

static void onMidiStart() {
  Serial.printf("Start\r\n");
  MIDIusb.sendStart();
  MIDIuart.sendStart();
}

static void onMidiContinue() {
  Serial.printf("Cont\r\n");
  MIDIusb.sendContinue();
  MIDIuart.sendContinue();
}

static void onMidiStop() {
  Serial.printf("Stop\r\n");
  MIDIusb.sendStop();
  MIDIuart.sendStop();
}

static void onActiveSense() {
  Serial.printf("ASen\r\n");
}

static void onSystemReset() {
  Serial.printf("SysRst\r\n");
}

static void onMidiTick() {
  Serial.printf("Tick\r\n");
}

static void onMidiInWriteFail(uint8_t devAddr, uint8_t cable, bool fifoOverflow) {
  if (fifoOverflow)
    Serial.printf("Dev %u cable %u: MIDI IN FIFO overflow\r\n", devAddr, cable);
  else
    Serial.printf("Dev %u cable %u: MIDI IN FIFO error\r\n", devAddr, cable);
}

static void registerMidiInCallbacks() {
  auto intf = usbhMIDI.getInterfaceFromDeviceAndCable(midiDevAddr, 0);
  if (intf == nullptr)
    return;
  intf->setHandleControlChange(onControlChange);  // 0xB0
  intf->setHandleClock(onMidiClock);              // 0xF8
  // 0xF9 as 10ms Tick is not MIDI 1.0 standard but implemented in the Arduino MIDI Library
  intf->setHandleTick(onMidiTick);              // 0xF9
  intf->setHandleStart(onMidiStart);            // 0xFA
  intf->setHandleContinue(onMidiContinue);      // 0xFB
  intf->setHandleStop(onMidiStop);              // 0xFC
  intf->setHandleActiveSensing(onActiveSense);  // 0xFE
  intf->setHandleSystemReset(onSystemReset);    // 0xFF
  intf->setHandleError(onMidiError);

  auto dev = usbhMIDI.getDevFromDevAddr(midiDevAddr);
  if (dev == nullptr)
    return;
  dev->setOnMidiInWriteFail(onMidiInWriteFail);
}

/* CONNECTION MANAGEMENT */
static void onMIDIconnect(uint8_t devAddr, uint8_t nInCables, uint8_t nOutCables) {
  Serial.printf("MIDI device at address %u has %u IN cables and %u OUT cables\r\n", devAddr, nInCables, nOutCables);
  midiDevAddr = devAddr;
  registerMidiInCallbacks();
}

static void onMIDIdisconnect(uint8_t devAddr) {
  Serial.printf("MIDI device at address %u unplugged\r\n", devAddr);
  midiDevAddr = 0;
}

/* MAIN LOOP FUNCTIONS */

static void blinkLED(void) {
  const uint32_t intervalMs = 1000;
  static uint32_t startMs = 0;

  static bool ledState = false;
  if (millis() - startMs < intervalMs)
    return;
  startMs += intervalMs;

  ledState = !ledState;
  digitalWrite(LED_BUILTIN, ledState ? HIGH : LOW);
}

// core1's setup
void setup1() {
#if ARDUINO_ADAFRUIT_FEATHER_RP2040_USB_HOST
  pinMode(18, OUTPUT);  // Sets pin USB_HOST_5V_POWER to HIGH to enable USB power
  digitalWrite(18, HIGH);
#endif

  // while(!Serial);   // wait for native usb
  Serial.println("Core1 setup to run TinyUSB host with pio-usb\r\n");

  // Check for CPU frequency, must be multiple of 120Mhz for bit-banging USB
  uint32_t cpu_hz = clock_get_hz(clk_sys);
  if (cpu_hz != 120000000UL && cpu_hz != 240000000UL) {
    delay(2000);  // wait for native usb
    Serial.printf("Error: CPU Clock = %lu, PIO USB require CPU clock must be multiple of 120 Mhz\r\n", cpu_hz);
    Serial.printf("Change your CPU Clock to either 120 or 240 Mhz in Menu->CPU Speed \r\n");
    while (1)
      delay(1);
  }

  pio_usb_configuration_t pio_cfg = PIO_USB_DEFAULT_CONFIG;
  pio_cfg.pin_dp = HOST_PIN_DP;

  USBHost.configure_pio_usb(1, &pio_cfg);
  // run host stack on controller (rhport) 1
  // Note: For rp2040 pico-pio-usb, calling USBHost.begin() on core1 will have most of the
  // host bit-banging processing work done in core1 to free up core0 for other work
  usbhMIDI.begin(&USBHost, 1, onMIDIconnect, onMIDIdisconnect);
  core1_booting = false;
  while (core0_booting)
    ;
}

// core1's loop
void loop1() {
  USBHost.task();
}

void setup() {
  TinyUSBDevice.setManufacturerDescriptor("LarsCo");
  TinyUSBDevice.setProductDescriptor("MIDI Masseuse");
  Serial.begin(115200);
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;  // Don't proceed, loop forever
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();
  delay(2000);  // Pause for 2 seconds
  // Clear the buffer
  display.clearDisplay();
  display.display();

  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
  display.setCursor(0, 0);
  display.print("USB MIDI Messenger");
  display.setCursor(0, 12);
  display.print("Ch x > 1  Note_ CC#_\r\n");
  display.setCursor(0, 24);
  display.printf("Progs %u-%u     \r\n", userProgOffset, (userProgOffset + 7));
  display.display();

  pinMode(buttonAPin, INPUT_PULLUP);
  pinMode(buttonBPin, INPUT_PULLUP);  // OLED button B as a 100k pullup on it on 128x32 FW
  pinMode(buttonCPin, INPUT_PULLUP);

  MIDIusb.begin();
  MIDIusb.turnThruOff();  // turn off echo

  MIDIuart.begin(MIDI_CHANNEL_OMNI);  // don't forget OMNI

  //     while(!Serial);   // wait for serial port
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.println("USB Host to MIDI Messenger\r\n");
  core0_booting = false;
  while (core1_booting)
    ;
}

void loop() {
  // Handle any incoming data; triggers MIDI IN callbacks
  usbhMIDI.readAll();
  // Do other processing that might generate pending MIDI OUT data

  // Tell the USB Host to send as much pending MIDI OUT data as possible
  usbhMIDI.writeFlushAll();

  // Do other non-USB host processing
  blinkLED();

  int readingA = digitalRead(buttonAPin);
  int readingB = digitalRead(buttonBPin);
  int readingC = digitalRead(buttonCPin);

  if (readingA != lastButtonAState) {
    lastDebounceATime = millis();
  }
  if (readingB != lastButtonBState) {
    lastDebounceBTime = millis();
  }
  if (readingC != lastButtonCState) {
    lastDebounceCTime = millis();
  }

  if ((millis() - lastDebounceATime) > debounceDelay) {
    if (readingA != buttonAState) {
      buttonAState = readingA;

      if (buttonAState == LOW)  // Botão pressionado
      {
        onControlChange(1, 48, 127);
        Serial.printf("PRE ON");
        display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
        display.setCursor(0, 12);
        display.printf("CC 48 = 127");
        display.display();
      } else if (buttonAState == HIGH)  // Botão solto
      {
        onControlChange(1, 48, 0);
        Serial.printf("PRE OFF");
        display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
        display.setCursor(0, 12);
        display.printf("CC 48 = 0 ");
        display.display();
      }
    }
  }

  lastButtonAState = readingA;
  lastButtonBState = readingB;
  lastButtonCState = readingC;
}