#include <SPI.h>
//#include "DynamixelSerial.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>

byte mac[] = {
  //0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0x01  // FEATHER 1
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0x02    // FEATHER 2
};

//IPAddress ip(192, 168, 133, 2);  // IP FEATHER1
//IPAddress outIp(192, 168, 133, 4); // PLAYER1
IPAddress ip(192, 168, 133, 6);  // IP FEATHER2
IPAddress outIp(192, 168, 133, 8); // PLAYER2

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

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;

uint8_t contador = 0;
unsigned long previousMillis = 0;        // will store last time LED was updated
const long interval = 120000;           //cambio cada 2min


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 ino non Hz

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

void scene( OSCMessage &msg )
{
      digitalWrite(13, HIGH);
      Serial.println("Scene received");

      if (msg.isInt(0)){        
        CMD[0] = msg.getInt(0);
        
        OSCMessage msg("/scene/change");
        msg.add((int32_t)CMD[0]);
        Udp.beginPacket(outIp, destPort);
        msg.send(Udp);
        Udp.endPacket();
        msg.empty();
        
      } //otherwise it's a floating point frequency in Hz

      Serial.println(CMD[0]);

}
 
void setup() {

  //Dynamixel.begin(1000000,5);  // Inicialize the servo at 1Mbps and Pin Control 5
  //Dynamixel.setCSlope(254, 64, 64);
  //Dynamixel.setCMargin(254, 3, 3);
  
  Serial.begin(115200);
  //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);
  
  //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;

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) 
  {
    // save the last time you blinked the LED
      previousMillis = currentMillis;
      contador++;
      if(contador > 5)
        contador = 1;

      Serial.print("scena: ");
      Serial.println(contador-1);
      
      OSCMessage msg("/scene/change");
      msg.add((int32_t)contador-1);
      Udp.beginPacket(outIp, destPort);
      msg.send(Udp);
      Udp.endPacket();
      msg.empty();
  }

  
  if ( (size = Udp.parsePacket()) > 0)
  {   

      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(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
        msg.dispatch("/scene", scene );    // 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(Cvalue != Cvalue_old)
  {
    Cvalue_old = Cvalue;
    if(Cvalue)
    {
      Serial.println("C->ON");
      contador++;
      if(contador > 5)
        contador = 1;

      Serial.print("scena: ");
      Serial.println(contador-1);
      
      OSCMessage msg("/scene/change");
      msg.add((int32_t)contador-1);
      Udp.beginPacket(outIp, destPort);
      msg.send(Udp);
      Udp.endPacket();
      msg.empty();
    }
  }
  
  if(Zvalue != Zvalue_old)
  {
    Zvalue_old = Zvalue;
    if(Zvalue)
    {
      Serial.println("Z->ON");
      contador--;
      if(contador < 1)
        contador = 5;

      Serial.print("scena: ");
      Serial.println(contador-1);
      
      OSCMessage msg("/scene/change");
      msg.add((int32_t)contador-1);
      Udp.beginPacket(outIp, destPort);
      msg.send(Udp);
      Udp.endPacket();
      msg.empty();
   
    }
  }

delay(20); // para que el mando no lea tan rapido

}