#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";

const long sendInterval = 100; // in ms

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;

  uint8_t checksum;
  
} __attribute__ ((__packed__));

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

Packet* pPacket = (Packet*)buffer;

void calcChecksum()
{
  uint16_t sum = 0;
  
  for (int i = 4; i < (bufferSize - 1); i++)
  {
    sum += buffer[i];
  }

  pPacket->checksum = (uint8_t)(sum & 0xFF);
  //Serial.print("sum: ");
  //Serial.println(sum);
  //Serial.print("checksum: ");
  //Serial.println(pPacket->checksum);
}

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

  // Initialize the packet
  pPacket->MagicWordLow = 0x0DED;
  pPacket->MagicWordHigh = 0x0DEC;

  pPacket->Throttle = 1;
  pPacket->Steering = 2;
  pPacket->Kp = 3;
  pPacket->Ki = 4;
  pPacket->Kd = 5;
  pPacket->Enabled = false;
  pPacket->checksum = 0;
}

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

  calcChecksum();
}

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

  static unsigned long previousMillis = 0;

  long currentMillis = millis();

  if (currentMillis - previousMillis >= sendInterval)
  {
    // If something can be printed there is enough time
    // between iterations
    Serial.println("Sending data");
    previousMillis = currentMillis;
    sendPacket(); // Send on UART
  }
}