#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 = 30; // 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 PacketOut {
  int16_t MagicWordLow;
  int16_t MagicWordHigh;
  
  int16_t Throttle;
  int16_t Steering;

  int16_t SpeedKp;
  int16_t SpeedKi;
  
  int16_t AngleKp;
  int16_t AngleKi;
  int16_t AngleKd;
  
  bool Enabled;

  bool SpeedControl;

  uint8_t checksum;
  
} __attribute__ ((__packed__));

struct PacketIn {
  
  int16_t BatteryLevel;
  
  int16_t Mode;
  
  uint8_t checksum;
  
} __attribute__ ((__packed__));

const int bufferOutSize = sizeof(PacketOut); // Size of Packet
uint8_t bufferOut[bufferOutSize];
PacketOut* pPacketOut = (PacketOut*)bufferOut;

const int bufferInSize = sizeof(PacketIn);
uint8_t bufferIn[bufferInSize];
PacketIn* pPacketIn = (PacketIn*)bufferIn;

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

  pPacketOut->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(250000);
  Serial.println();
  Serial.print("AP IP: ");
  Serial.println(apip);
  Udp.begin(localPort);

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

  pPacketOut->Throttle = 1;
  pPacketOut->Steering = 2;
  pPacketOut->SpeedKp = 3;
  pPacketOut->SpeedKi = 4;
  pPacketOut->AngleKp = 3;
  pPacketOut->AngleKi = 4;
  pPacketOut->AngleKd = 5;
  pPacketOut->Enabled = false;
  pPacketOut->SpeedControl = true;
  pPacketOut->checksum = 0;
}

void sendPacket()
{
  calcChecksum();
  Serial.write(bufferOut, bufferOutSize);
}

void OSCMsgReceive()
{
  OSCMessage msgIN;
  int size;
  if ((size = Udp.parsePacket()) > 0)
  {
    while(size--)
    {
      msgIN.fill(Udp.read());
    }
    if (!msgIN.hasError())
    {
      msgIN.route("/Robot/Enable",funcEnabled);
      msgIN.route("/Fader/Throttle",funcThrottle);
      msgIN.route("/Fader/Steering",funcSteering);
      
      msgIN.route("/Gain/Speed/Kp",funcSpeedKp);
      msgIN.route("/Gain/Speed/Ki",funcSpeedKi);
      
      msgIN.route("/Gain/Angle/Kp",funcAngleKp);
      msgIN.route("/Gain/Angle/Ki",funcAngleKi);
      msgIN.route("/Gain/Angle/Kd",funcAngleKd);

      msgIN.route("/SpeedControl",funcSpeedControl);
    }
  }
}

void sendUdp(OSCMessage* pMessage)
{
  IPAddress remoteIp = Udp.remoteIP();
  
  /* Address is exchanged for 0.0.0.0 so when
   *  forwarding from the other board the 
   *  package does not reach the phone
   *  TODO: Check for address other than 0.0.0.0
   *  before saving
  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(outIp, outPort);
  pMessage->send(Udp); // send the bytes
  Udp.endPacket(); // mark the end of the OSC Packet
  pMessage->empty(); // free space occupied by message
}

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

  digitalWrite(BUILTIN_LED, ledState);

  msgOUT.add(!ledState);
  sendUdp(&msgOUT);
  pPacketOut->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);
  pPacketOut->Throttle = value;
}

void funcSteering(OSCMessage &msg, int addrOffset ){

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

void funcSpeedKp(OSCMessage &msg, int addrOffset ){

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

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

void funcSpeedKi(OSCMessage &msg, int addrOffset ){

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

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

void funcAngleKp(OSCMessage &msg, int addrOffset ){

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

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

void funcAngleKi(OSCMessage &msg, int addrOffset ){

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

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

void funcAngleKd(OSCMessage &msg, int addrOffset ){

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

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

void funcSpeedControl(OSCMessage &msg, int addrOffset ){

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

// Send OSC packets updating touchOSC on the mobile
void remotePresentation()
{
  OSCMessage msgLABEL("/battery");
  msgLABEL.add(pPacketIn->BatteryLevel);
  sendUdp(&msgLABEL);

  // TODO: Add the desired data to be forwarded from pPacketIn
}

bool parsePacket()
{
  static uint8_t magicWord[4];
  static uint32_t* pMagicWord = (uint32_t*)magicWord;
  
  static uint8_t byteCounter = 0;
  static const uint8_t payloadSize = sizeof(PacketIn);
  static uint8_t payload[payloadSize];
  static PacketIn* pPacketInWorker = (PacketIn*)payload;

  uint8_t newByte = Serial.read();
  
  // We have the header correct
  if (*pMagicWord == 0xDEC0DED)
  {
    payload[byteCounter++] = newByte;

    static uint16_t sum = 0;
  
    if (byteCounter >= payloadSize)
    {
        *pMagicWord = 0; // Unvalidate the magic word
        byteCounter = 0; // Reset the read counter

        uint8_t checksum = sum & 0xFF;
        sum = 0;

        if (checksum == pPacketInWorker->checksum)
        {
          // If robot enabled we blink 
          // when receiving data
          if (pPacketOut->Enabled)
          {
            ledState = !ledState;
            digitalWrite(BUILTIN_LED, ledState);
          }

          *pPacketIn = *((PacketIn*)payload);

          remotePresentation();
          
          return true; // new package available
        }
    }
    sum +=newByte; // placed here not to include the checksum
  }
  else
  {
    *pMagicWord = *pMagicWord >> 8 | newByte << 8*3; // Works on revo
    //Serial.print("Byte: ");
    //Serial.println(newByte);
  }
  
  return false;
}

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
  }
  
  if (Serial.available())
  {
    parsePacket();
  }
  
}