#include <SPI.h>
#include <Ethernet.h>
#include <EthernetUdp.h>

//Mac Address for UDP-connection.
byte mac[] = { 0xE0, 0xDC, 0xA0, 0x1D, 0x55, 0xE3 };
unsigned int localPort = 8888; //UDP port.

//IP Addresses for the UDP connection
IPAddress myIP(192, 168, 200, 1); //IOT Module IP-address
IPAddress targetIP(192, 168, 200, 10); //PLC IP-address


const int espPacketSize = 2; //Size of incoming data from esp in bytes
const int udpPacketSize = 4; //Size of incoming data from udp in bytes

byte udpPacketBuffer[udpPacketSize]; //Buffer for holding data received from UDP connection
byte espPacketBuffer[espPacketSize]; //Buffer for holding data received from ESP connection

EthernetUDP Udp;


//Time variables for restricting test-transmissions to once every second. timeLast is the time of the last transmission, delayTime is how long to wait before transmitting again.
long timeLast;
long delayTime = 1000;


void setup() {
  //Open serial connection and await a connection
  Serial.begin(9600);
  Serial1.begin(9600);
  //Open Ethernet port
  Serial.println("Start Ethernet connection");
  Ethernet.begin(mac,myIP);
  //Open UDP port
  Serial.println("Start UDP protocol");
  Udp.begin(localPort);
  Serial.println("Done Setup");
}

void loop()
{
 //Check for received data on UDP connection, if there is any (>0 bytes received), read the data and write it to the ESP module via Serial connection.
  if(Udp.parsePacket() > 0)
  {
    Serial.print("Received from UDP: ");
    Udp.read(udpPacketBuffer,udpPacketSize);
    int spd = word(udpPacketBuffer[0],udpPacketBuffer[1]); //spd is the speed for the EV3 robot
    int dir = word(udpPacketBuffer[2],udpPacketBuffer[3]); //dir is the direction for the EV3 robot
    Serial.print(spd);
    Serial.print(", ");
    Serial.println(dir);
    //Serial.write(udpPacketBuffer,udpPacketSize);
  }
  //Check for received data on ESP connection, if there is any (>0 bytes received), read the data and write it to the UDP-connected device, via the Ethernet port.
  if(Serial1.readBytes((char*)espPacketBuffer, espPacketSize) > 0)
  {
    //Flip the received word. We don't know why, but the byte we get comes in the wrong order.
    byte espbuff = espPacketBuffer[0];
    espPacketBuffer[0] = espPacketBuffer[1];
    espPacketBuffer[1] = espbuff;
    //Send on UDP (to PLC)
    Udp.beginPacket(targetIP, localPort);
    Udp.write(espPacketBuffer, espPacketSize);
    Udp.endPacket();
    //Print to serial monitor
    Serial.print("Received from ESP: ");
    int dst = word(espPacketBuffer[0], espPacketBuffer[1]); //dst stands for data send transmitted
    Serial.println(dst);
  }
}