#include "DynamixelSerial.h"
#include <SPI.h>
#include "wiring_private.h" 

#include <WiiChuck.h>
Accessory nunchuck1;

#define ETHERNET_ON true

#include <Ethernet.h>
#include <EthernetUdp.h>       
#include <OSCBundle.h>
#include <OSCBoards.h>
#include <OSCMessage.h>

// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
byte mac[] = {
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
};
IPAddress ip(192, 168, 1, 3); 
IPAddress destIp(192, 168, 1, 2);
//IPAddress ip(10, 100, 50, 3); 
//IPAddress destIp(10, 100, 50, 2);

unsigned int localPort = 8888;      // local port to listen on
unsigned int  destPort = 9999;      // TO SET SENDING PORT

// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP Udp;
OSCErrorCode error;

#define  UDP_RX_PACKET_MAX_SIZE  8

uint32_t CMD[ 8 ]; // CMD + Ch + 2

uint8_t rele1 = A1;
uint8_t rele2 = A0;

uint8_t Zvalue;
uint8_t Cvalue;
uint8_t Zvalue_old = 0;
uint8_t Cvalue_old = 0;

void resetBoard()
{
  //Serial.println("reset");
  NVIC_SystemReset();       // esta funcion en teoria si funciona en SAMD
}


void test(OSCMessage &msg)
{
  digitalWrite(13, HIGH);
  Serial.println("test");  
  digitalWrite(13, LOW);
}

void reboot(OSCMessage &msg)
{
  digitalWrite(13, HIGH);
  Serial.println("reset"); 
  resetBoard();
  digitalWrite(13, LOW);
}


void cmdProcess( OSCMessage &msg )
{
      unsigned int frequency = 0;
      digitalWrite(13, HIGH);
      //Serial.println("CMD received");

      if (msg.isInt(0)){        
        CMD[0] = msg.getInt(0);
      } //otherwise it's a floating point frequency in Hz
      if (msg.isInt(1)){        
        CMD[1] = msg.getInt(1);
      } //otherwise it's a floating point frequency in Hz

      Serial.print(CMD[0]);
      Serial.print(",");
      Serial.print(CMD[1]);
      Serial.println();
      digitalWrite(13, LOW);
      
}
 


void setup() {

  Dynamixel.begin(1000000,5);  // Inicialize the servo at 1Mbps and Pin Control 5
  //Dynamixel.setID(1,2);

  Dynamixel.setCSlope(1, 128, 128);
  Dynamixel.setCSlope(2, 128, 128);
  //Dynamixel.setCMargin
  
  Serial.begin(512000);
  //delay(4000);
  //while (!Serial) {
    //; // wait for serial port to connect. Needed for Leonardo only
  //}

    // start the Ethernet and UDP:
  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(localPort);

  //nunchuck1.begin();
  //if (nunchuck1.type == Unknown) {
  //  nunchuck1.type = NUNCHUCK;
  //}
  
  pinMode(rele1, OUTPUT);
  pinMode(rele2, OUTPUT);
  
  nunchuck1.begin();
  if (nunchuck1.type == Unknown_) { // Modificado en la libreria para que no de error con la Ethernet.h
    nunchuck1.type = NUNCHUCK;
  }
  
}


void loop() {

  OSCMessage msg;
  int size;
  
  if ( (size = Udp.parsePacket()) > 0)
  {   
      //Serial.print("mensaje recibido: ");
      //Serial.println(size);
      while (size--)
      {
      //  msg.fill(Udp.read());
        uint8_t packetBuffer[UDP_RX_PACKET_MAX_SIZE];
        Udp.read(packetBuffer, UDP_RX_PACKET_MAX_SIZE);
        msg.fill(packetBuffer, UDP_RX_PACKET_MAX_SIZE);
      }

      if (!msg.hasError()) 
      {
        //Serial.println("packetGuay");   
        //Serial.println(msg.match("/test"));

        msg.dispatch("/test", test );    // this is how it is€ marked on the silkscreen
        msg.dispatch("/reset", reboot );    // this is how it is€ marked on the silkscreen
        msg.dispatch("/cmd", cmdProcess );    // this is how it is marked on the silkscreen
      }
      else 
      {
        error = msg.getError();
        //Serial.print("error: ");
        //Serial.println(error);
      }
  }

  nunchuck1.readData();    // Read inputs and update maps
  uint8_t joystickValueX = nunchuck1.values[0];
  uint8_t joystickValueY = nunchuck1.values[1];
  Zvalue = nunchuck1.values[10];
  Cvalue = nunchuck1.values[11];

//  Serial.print("X: ");
//  Serial.print(joystickValueX);
//  Serial.print(" Y: ");
//  Serial.print(joystickValueY);
//  Serial.print(" Z: ");
//  Serial.print(Zvalue);  
//  Serial.print(" C: ");
//  Serial.println(Cvalue);  
  
  if(Zvalue != Zvalue_old)
  {
    Zvalue_old = Zvalue;
    if(Zvalue)
    {
      Serial.println("Z ON");
      Dynamixel.moveSpeed(2, 900, 512);
      delay(1);
    }
    else
    {
      Serial.println("Z OFF");
      Dynamixel.moveSpeed(2, 100, 512); 
      delay(1);
    } 
  }

  if(Cvalue != Cvalue_old)
  {
    Cvalue_old = Cvalue;
    if(Cvalue)
    {
      Serial.println("C ON");
      Dynamixel.moveSpeed(1, 1023, 1023);
      delay(1);
    }
    else
    {
      Serial.println("C OFF");
      Dynamixel.moveSpeed(1, 0, 1023);
      delay(1); 
    } 

  }
  
  //Dynamixel.move(1,random(200,800));  // Move the Servo radomly from 200 to 800
  //Dynamixel.move(1,joystickValueX*4);  // Move the Servo radomly from 200 to 800

  //Serial.print("Posicion: ");
  //Serial.println(Dynamixel.readPosition(1));
  //digitalWrite(rele1, HIGH);
  //digitalWrite(rele2, HIGH);
  //delay(500);
  //digitalWrite(rele1, LOW);
  //digitalWrite(rele2, LOW);
  //delay(500);

}