// Use from 0 to 4. Higher number, more debugging messages and memory usage.
#define _WIFIMGR_LOGLEVEL_    4
#define DEBUG 1

#include <WiFi.h>
#include <WiFiUdp.h>
#include <WiFiManager.h>
#include "debug.h"
#include <Arduino.h>
#include <ESPmDNS.h>
#include <OSCMessage.h>
#include <OSCBundle.h>
#include <OSCData.h>
#include <MIDI.h>
#include "OSC2Midi.h"
#include <HardwareSerial.h>
#include <SoftwareSerial.h>
#include <LiquidCrystal_I2C.h>

#define MDNS_NAME "osc2midi"
#define AP_SSID_NAME "OSC2MIDI"
#define AP_PASSWORD "osc2midi"
#define AP_SSID_CONFIG_NAME "OSC2MIDI-Config"
#define AP_CONFIG_PASSWORD "osc2midi"
#define MDNS_NAME "osc2midi"
#define SOFT_SERIAL_RX 18
#define SOFT_SERIAL_TX 19
#define AP_TIMEOUT 120
#define AP_DATA_RESET_PIN 35
#define AP_MODE_PIN 34
#define LCD_I2C_ADDR 0x27
#define LCD_COL 16
#define LCD_ROW 2
#define UDP_RECV_PORT 8000
#define UDP_SEND_PORT 9000
#define K_RATE 200

void OSCToMidiCC(OSCMessage &msg, int offset);
void MidiCCToOSC(uint8_t channel, uint8_t number, uint8_t value);

WiFiUDP udp;
IPAddress clientIP;
LiquidCrystal_I2C lcd(LCD_I2C_ADDR, LCD_COL, LCD_ROW);
HardwareSerial midi1(2); // RX: 16, TX: 17
#ifndef D5
#define D5 (SOFT_SERIAL_RX)
#define D6 (SOFT_SERIAL_TX)
//#define D7 (23)
//#define D8 (5)
//#define TX (1)
#endif
SoftwareSerial midi2;
uint32_t k_rate_last = millis();
bool ap_mode_state = digitalRead(AP_MODE_PIN);

MIDI_CREATE_INSTANCE(HardwareSerial, midi1, MIDI1);

void setup()
{
  pinMode(AP_DATA_RESET_PIN, INPUT_PULLUP);
  pinMode(AP_MODE_PIN, INPUT_PULLUP);

  Serial.begin(115200);
  Serial.setDebugOutput(true);
  Serial.println("<start>");

  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.noCursor();
  lcd.setCursor(0, 0);
  lcd.print("* OSC2MIDI *");
  delay(1000);

  if (ap_mode_state == LOW)
  {
    Serial.println("Mode Access-Point");
    WiFi.softAP(AP_SSID_NAME, AP_PASSWORD);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Mode AP");
    lcd.setCursor(0, 1);
    lcd.print(WiFi.softAPIP());
  }
  else
  {
    Serial.println("Mode Client");

    WiFi.mode(WIFI_STA); // explicitly set mode, esp defaults to STA+AP

    //WiFiManager, Local intialization. Once its business is done, there is no need to keep it around
    WiFiManager wm;

    if (digitalRead(AP_DATA_RESET_PIN) != LOW)
    {
      wm.resetSettings();
      Serial.println("Resetting AP data");
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Resetting AP data");
      delay(2000);
      //ESP.restart();
    }

    // Automatically connect using saved credentials,
    // if connection fails, it starts an access point with the specified name ( "AutoConnectAP"),
    // if empty will auto generate SSID, if password is blank it will be anonymous AP (wm.autoConnect())
    // then goes into a blocking loop awaiting configuration and will return success result

    // res = wm.autoConnect(); // auto generated AP name from chipid
    // res = wm.autoConnect("AutoConnectAP"); // anonymous ap
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Mode Config-AP");
    lcd.setCursor(0, 1);
    lcd.print("192.168.4.1");

    if (!wm.autoConnect(AP_SSID_CONFIG_NAME, AP_CONFIG_PASSWORD))
    {
      Serial.println("Failed to connect");
      lcd.print("Failed");
      delay(2000);
      ESP.restart();
    }
    else {
      //if you get here you have connected to the WiFi
      Serial.println("Connected");

      if (!MDNS.begin(MDNS_NAME))
      {
        Serial.println("Error setting up MDNS responder!");
      }
      else
      {
        Serial.println("mDNS started.");
      }
    }

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Mode WiFi client");
    lcd.setCursor(0, 1);
    lcd.print(WiFi.localIP());
  }

  udp.begin(UDP_RECV_PORT);
  Serial.print("Listening for UDP packets on port ");
  Serial.println(UDP_RECV_PORT);

  midi1.begin(31250); // 16,17
  midi2.begin(31250, SWSERIAL_8N1, D5, D6, false, 95, 11);
  midi2.enableIntTx(false);

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

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

  size_t size = udp.parsePacket();

  while (udp.available())
  {
    IPAddress tmpIP = udp.remoteIP();

    // Check if there are any OSC packets to handle
    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"
    if (clientIP == tmpIP)
    {
      clientIP = tmpIP;
      Serial.print("Connection from: ");
      Serial.print(clientIP);
    }
  }

  // Check if there are any CC messages from synth itself
  if (MIDI1.read())
  {
    DEBUG_MSG("MIDI-IN[1] Type: ");
    DEBUG_MSG("%3d", MIDI1.getType());
    DEBUG_MSG(" Data1: ");
    DEBUG_MSG("%3d", MIDI1.getData1());
    DEBUG_MSG(" Data2: ");
    DEBUG_MSG("%3d", MIDI1.getData2());
    DEBUG_MSG(" Channel: ");
    DEBUG_MSG("%0d", MIDI1.getChannel());
    DEBUG_MSG("\n");
  }

  // MIDI-Merger from (Soft-)MIDI2 to MIDI1
  if (midi2.available() > 0)
  {
    while (midi2.available() > 0)
    {
      Serial.print("MIDI-IN[2]: ");
      Serial.println(midi2.peek(), DEC);
      midi1.write(midi2.read());
    }
  }

  // Do something at control rate
  if (k_rate_last < (millis() - K_RATE))
  {
    if (ap_mode_state != digitalRead(AP_MODE_PIN))
      ESP.restart();
  }
}

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);
    MIDI1.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);
    MIDI1.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);
    MIDI1.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, UDP_SEND_PORT);
    msg.send(udp);
    udp.endPacket();
  }
}