#define DEBUG 1

#include "debug.h"
#include <Arduino.h>
//#include <ESP8266WiFi.h>
#include <WiFiManager.h>
#include <WiFiUdp.h>
#include <OSCMessage.h>
#include <OSCBundle.h>
#include <OSCData.h>
#include <MIDI.h>
#include "OSC2Midi.h"

void MidiCCToOSC(uint8_t channel, uint8_t number, uint8_t value);

WiFiUDP udp;

/**
   source address of last OSC message for midi2osc messages.
*/
IPAddress clientIP;

/*IPAddress local_IP(192,168,4,2);
  IPAddress gateway(192,168,4,1);
  IPAddress subnet(255,255,255,0);*/

/*
   UART    RX IO   TX IO   CTS   RTS
   UART0   GPIO3   GPIO1   N/A   N/A
   UART1   GPIO9   GPIO10  GPIO6   GPIO11
   UART2   GPIO16  GPIO17  GPIO8   GPIO7
*/

MIDI_CREATE_INSTANCE(HardwareSerial, Serial2, MIDI); // Pins on ESP32: RX: 6, TX: 7
//MIDI_CREATE_INSTANCE(HardwareSerial, Serial1, MIDI2); // Pins on ESP32: RX: ?, TX: ?

void setup()
{
  Serial2.begin(31250);

  Serial.begin(115200);
  Serial.setDebugOutput(true);

  DEBUG_MSG("\nHello OSC2Midi!\n");

  WiFi.softAP("OSC2Midi", "1357924680");

  DEBUG_MSG("\nAP IP address: %s\n", WiFi.softAPIP().toString().c_str());

  udp.begin(8000);

  MIDI.begin(MIDI_CHANNEL_OMNI);
  MIDI.setHandleControlChange(MidiCCToOSC);
  MIDI.turnThruOff();
}

void OSCToMidiCC(OSCMessage &msg, int offset)
{
  char address[100] = { 0 };
  uint8_t cc, value;
  uint8_t midichannel;

  msg.getAddress(address, offset, sizeof(address));
  midichannel = getMIDIChannel(address);
  //midichannel--;

  if (msg.size() == 1 && msg.isFloat(0))
  {
    // Single or multi control with sending one value
    cc = getCC(address);
    value = round(msg.getFloat(0));
    value = value > 127 ? 127 : value;
    DEBUG_MSG("MSG: %s\tChannel: %u\t\tCC: %u\tValue: %u\n", address, midichannel, cc, value);
    MIDI.sendControlChange(cc, value, midichannel);
  }
  else if (msg.size() == 2 && msg.isFloat(0) && msg.isFloat(1))
  {
    // XY pad, two values
    cc = getVar(address, 1);
    value = round(msg.getFloat(0));
    value = value > 127 ? 127 : value;
    DEBUG_MSG("MSG: %s\tChannel: %u\t\tCC: %u\tValue: %u\n", address, midichannel, cc, value);
    MIDI.sendControlChange(cc, value, midichannel);
    cc = getVar(address, 2);
    value = round(msg.getFloat(1));
    value = value > 127 ? 127 : value;
    DEBUG_MSG("MSG: %s\tChannel: %u\t\tCC: %u\tValue: %u\n", address, midichannel, cc, value);
    MIDI.sendControlChange(cc, value, midichannel);
  }
  else
  {
    DEBUG_MSG("Cannot handle: %s\n", address);
  }
}

void MidiCCToOSC(uint8_t channel, uint8_t number, uint8_t val)
{
  char buffer[1024];

  snprintf(buffer, sizeof(buffer), "/midi/cc/%u/%u", channel, number);

  DEBUG_MSG("MidiCCToOsc: %s %f\n", buffer, val * 1.0);

  if (clientIP)
  {
    OSCMessage msg = OSCMessage(buffer);
    msg.add(val);

    udp.beginPacket(clientIP, 9000);
    msg.send(udp);
    udp.endPacket();
  }
}

void loop()
{
  OSCMessage msg;
  uint8_t buffer[1024];
  uint16_t outPort;

  // Check if there are any OSC packets to handle
  size_t size = udp.parsePacket();

  if (size > 0 && size <= 1024)
  {
    udp.read(buffer, size);
    msg.fill(buffer, size);

    if (!msg.hasError())
    {
      DEBUG_OSC_MESSAGE(msg);
      msg.route("/midi/cc", OSCToMidiCC);
      //msg.route("/midi/sysex", OSCToMidiSYSEX);
      //msg.route("/midi/note", OSCToMidiNote);
    }
    else
    {
      DEBUG_MSG("Error parsing OSC message: %d\n", msg.getError());
    }

    // Keep track of the client IP address for "talking back"
    clientIP = udp.remoteIP();

    udp.flush();
  }

  // Check if there are any CC messages from synth itself
  MIDI.read();
}