#ifdef ESP8266
#include <ESP8266WiFi.h>
#else
#include <WiFi.h>
#endif
#include <WiFiUdp.h>
#include <OSCMessage.h>
#include <OSCBundle.h>
#include <OSCData.h>

char ssid[] = "ROBOT";
char pass[] = "myrobotisawesome";

WiFiUDP Udp;

// remote IP (not needed for receive)
// Will be updated once the first package is received
// Gateway IP normally is 192.168.4.1
IPAddress outIp(192,168,4,2);

const unsigned int outPort = 9999;
const unsigned int localPort = 8888;

OSCErrorCode error;

// LED to reflect the active status of the robot
// Start "on"
unsigned int ledState = HIGH; 

typedef struct Packet {
  int16_t MagicWordLow;
  int16_t MagicWordHigh;
  
  int16_t Throttle;
  int16_t Steering;
  
  int16_t Kp;
  int16_t Ki;
  int16_t Kd;
  
  bool Enabled;
};

const int bufferSize = sizeof(Packet); // Size of Packet
uint8_t buffer[bufferSize];

Packet* pPacket = (Packet*)buffer;

void setup() {
  WiFi.softAP(ssid, pass);
  IPAddress apip = WiFi.softAPIP();
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(57600);
  Serial.println();
  Serial.print("AP IP: ");
  Serial.println(apip);
  Udp.begin(localPort);

  pPacket->MagicWordLow = 0x0DED;
  pPacket->MagicWordHigh = 0x0DEC;
}

void sendPacket()
{
  Serial.write(buffer, bufferSize);
}

void OSCMsgReceive()
{
  OSCMessage msgIN;
  int size;
  if ((size = Udp.parsePacket()) > 0)
  {
    while(size--)
    {
      msgIN.fill(Udp.read());
    }
    if (!msgIN.hasError())
    {
      msgIN.route("/Robot/Enable",toggleEnabled);
      msgIN.route("/Fader/Throttle",funcThrottle);
      msgIN.route("/Fader/Steering",funcSteering);
      msgIN.route("/Gain/Kp",funcKp);
      msgIN.route("/Gain/Ki",funcKi);
      msgIN.route("/Gain/Kd",funcKd);
    }
  }
}

void sendUdp(OSCMessage* pMessage)
{
  IPAddress remoteIp = Udp.remoteIP();
  //outIp = Udp.remoteIP();
  if (remoteIp != outIp)
  {
    outIp = remoteIp;
    Serial.print("New source address: ");
    Serial.println(outIp);
  }

  //send osc message back to controll object in TouchOSC
  //Local feedback is turned off in the TouchOSC interface.
  //The button is turned on in TouchOSC interface whe the conrol receives this message.
  Udp.beginPacket(Udp.remoteIP(), outPort);
  pMessage->send(Udp); // send the bytes
  Udp.endPacket(); // mark the end of the OSC Packet
  pMessage->empty(); // free space occupied by message
  
  sendPacket(); // Send on UART at the same time
}

void toggleEnabled(OSCMessage &msg, int addrOffset){
  ledState = !(boolean) msg.getFloat(0);
  OSCMessage msgOUT("/Robot/Enable");

  digitalWrite(BUILTIN_LED, ledState);

  msgOUT.add(!ledState);
  sendUdp(&msgOUT);
  pPacket->Enabled = ledState;

  Serial.println( ledState ? "Robot disabled" : "Robot enabled");
}

void funcThrottle(OSCMessage &msg, int addrOffset ){

  int value = msg.getFloat(0);
  OSCMessage msgOUT("/Fader/Throttle");
  msgOUT.add(value);
  sendUdp(&msgOUT);
  pPacket->Throttle = value;
}

void funcSteering(OSCMessage &msg, int addrOffset ){

  int value = msg.getFloat(0);
  OSCMessage msgOUT("/Fader/Steering");
  msgOUT.add(value);
  sendUdp(&msgOUT);
  pPacket->Steering = value;
}

void funcKp(OSCMessage &msg, int addrOffset ){

  int value = msg.getFloat(0);
  OSCMessage msgOUT("/Gain/Kp");
  msgOUT.add(value);
  sendUdp(&msgOUT);
  pPacket->Kp = value;

  // Redo this for label
  OSCMessage msgLABEL("/Gain/KpOut");
  msgLABEL.add(value);
  sendUdp(&msgLABEL);
}

void funcKi(OSCMessage &msg, int addrOffset ){

  int value = msg.getFloat(0);
  OSCMessage msgOUT("/Gain/Ki");
  msgOUT.add(value);
  sendUdp(&msgOUT);
  pPacket->Ki = value;

  // Redo this for label
  OSCMessage msgLABEL("/Gain/KiOut");
  msgLABEL.add(value);
  sendUdp(&msgLABEL);
}

void funcKd(OSCMessage &msg, int addrOffset ){

  int value = msg.getFloat(0);
  OSCMessage msgOUT("/Gain/Kd");
  msgOUT.add(value);
  sendUdp(&msgOUT);
  pPacket->Kd = value;

  // Redo this for label
  OSCMessage msgLABEL("/Gain/KdOut");
  msgLABEL.add(value);
  sendUdp(&msgLABEL);
}

void loop() {
  OSCMsgReceive();
}