#include <Ethernet.h>
#include <EthernetUDP.h>

/*
 * Tests the ability for the actuator box to send data to a python 
 * script on another machine connected to the LAN.
 */

#define ETHERNET_CS_PIN 25

byte mac[] = {
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
};
IPAddress ip(192, 168, 0, 115);
unsigned int local_port = 5000;


char packet_buffer[UDP_TX_PACKET_MAX_SIZE];  // buffer to hold incoming packet,
String data_request;
int packet_size;

EthernetUDP UDP;

void setup() {
  Serial.begin(9600);
  Serial.println("Beginning basic_ethernet_comms");
  
  Ethernet.init(ETHERNET_CS_PIN);
  Ethernet.begin(mac, ip);

  // Check for Ethernet hardware present
  if (Ethernet.hardwareStatus() == EthernetNoHardware) {
    Serial.println("Ethernet shield was not found.  Sorry, can't run without hardware. :(");
    while (true) {
      delay(1); // do nothing, no point running without Ethernet hardware
    }
  }
  if (Ethernet.linkStatus() == LinkOFF) {
    Serial.println("Ethernet cable is not connected.");
  }
  
  UDP.begin(local_port);
}

void loop() {
  packet_size = UDP.parsePacket();

  if (packet_size > 0) {
    Serial.println("Received UDP Packet");
    UDP.read(packet_buffer, UDP_TX_PACKET_MAX_SIZE);
    String data_request(packet_buffer);

    if (data_request == "servo_angle") {
      Serial.println("UDP Packet Requested Servo Angle");
      int angle = random(0, 91);
      String angle_string = String(angle);
      
      UDP.beginPacket(UDP.remoteIP(), UDP.remotePort());
      UDP.write(angle_string.c_str());
      UDP.endPacket();

      Serial.println("Sent " + String(angle) + " over UDP");
      Serial.println();
    }
  }
  memset(packet_buffer, 0, UDP_TX_PACKET_MAX_SIZE);
}