// Use from 0 to 4. Higher number, more debugging messages and memory usage.
#define _WIFIMGR_LOGLEVEL_  1
#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>
#include <looper.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 SOFT_SERIAL_RX        18
#define SOFT_SERIAL_TX        19
#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 KRATE_MODE            200
#define KRATE_STATE           2000

void OSCToMidiCC(OSCMessage &msg, int offset);
void MidiCCToOSC(uint8_t channel, uint8_t number, uint8_t value);
void change_midi_state(uint8_t midichannel, uint8_t cc, uint8_t value);
void show_midi_state(void);
void set_midi_state(void);
void check_mode(void);

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;
bool ap_mode_state = digitalRead(AP_MODE_PIN);
int8_t state[16][128];

looper sched;

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(F("OSC2MIDI (c)2020 H. Wirtz <wirtz@parasitstudio.de>"));

  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.noCursor();
  lcd.setCursor(2, 0);
  lcd.print(F("* OSC2MIDI *"));
  lcd.setCursor(0, 1);
  lcd.print(F("(c)parasiTstudio"));
  delay(1000);

  if (ap_mode_state == LOW)
  {
    DEBUG_MSG("Mode Access-Point\n");

    if (!WiFi.softAPConfig(IPAddress(192, 168, 4, 1), IPAddress(192, 168, 4, 1), IPAddress(255, 255, 255, 0))) {
      DEBUG_MSG("AP Config Failed\n");
    }

    if (!WiFi.softAP(AP_SSID_NAME, AP_PASSWORD))
    {
      DEBUG_MSG("Failed to start AP\n");
      lcd.print(F("Failed "));
      delay(1000);
      lcd.print(F("- restart"));
      delay(1000);
      ESP.restart();
    }

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print(F("Mode AP"));
    lcd.setCursor(0, 1);
    lcd.print(WiFi.softAPIP());
  }
  else
  {
    DEBUG_MSG("Mode Client\n");

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

    WiFiManager wm;

    if (digitalRead(AP_DATA_RESET_PIN) != LOW)
    {
      wm.resetSettings();
      DEBUG_MSG("Resetting AP data\n");
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print(F("Resetting AP data"));
      delay(2000);
    }

    // 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

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print(F("Mode Config-AP"));
    lcd.setCursor(0, 1);
    lcd.print(F("192.168.4.1"));

    if (!wm.autoConnect(AP_SSID_CONFIG_NAME, AP_CONFIG_PASSWORD))
    {
      DEBUG_MSG("Failed to connect\n");
      lcd.print(F("Failed"));
      delay(1000);
      lcd.print(F("- restart"));
      delay(1000);
      ESP.restart();
    }
    else {
      //if you get here you have connected to the WiFi
      DEBUG_MSG("Connected\n");

      if (!MDNS.begin(MDNS_NAME))
      {
        DEBUG_MSG("Error setting up MDNS responder!\n");
      }
      else
      {
        DEBUG_MSG("mDNS started.\n");
      }
    }

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

  memset(state, -1, 16 * 128);

  udp.begin(UDP_RECV_PORT);
  DEBUG_MSG("Listening for UDP packets on port %d\n", 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();

  //set_midi_state();

  sched.addJob(check_mode, KRATE_MODE);
  sched.addJob(show_midi_state, KRATE_STATE);
}

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;
      DEBUG_MSG("Connection from: %s", 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)
    {
      DEBUG_MSG("MIDI-IN[2]: %03d\n", midi2.peek());
      midi1.write(midi2.read());
    }
  }

  sched.scheduler();
}

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);

  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);
    change_midi_state(midichannel, cc, value);
  }
  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);
    change_midi_state(midichannel, cc, value);
    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);
    change_midi_state(midichannel, cc, value);
  }
  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();
  }
}

void change_midi_state(uint8_t midichannel, uint8_t cc, uint8_t value)
{
  state[midichannel - 1][cc - 1] = value;
}

void show_midi_state(void)
{
  uint8_t m, c;

  DEBUG_MSG("Current MIDI state:\n");

  for (m = 0; m < 16; m++)
  {
    DEBUG_MSG("MIDI-Channel %d\n", m + 1);
    for (c = 0; c < 128; c++)
    {
      if (state[m][c] >= 0)
      {
        DEBUG_MSG("\tCC: %03d = %03d\n", c, state[m][c]);
      }
    }
  }
}

/*
  void set_midi_state(void)
  {
  uint8_t tmp;
  uint8_t m;
  uint16_t i;

  DEBUG_MSG("Send MIDI state:\n");

  for (m = 0; m < 16; m++);
  {
    if (state[m].size() > 0)
    {
      DEBUG_MSG("MIDI-Channel %d\n");
      for (i = 0; i < state[m].size(); i++)
      {
        tmp = state[m].get(i);
        MIDI1.sendControlChange(tmp >> 4, tmp & 0x0f, m);
        DEBUG_MSG("\tSet state of CC: %03d = %03d\n", tmp >> 4, tmp & 0x0f);
      }
    }
  }
  }
*/

void check_mode(void)
{
  if (ap_mode_state != digitalRead(AP_MODE_PIN))
  {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Changing mode...");
    delay(500);
    ESP.restart();
  }
}