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

#define ETHERNET_ON true
#define RADIOSEND   true

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

#include "nRF24L01MOD.h"
#include "RF24MOD.h"
#include "printfMOD.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, 0xEA
};
IPAddress ip(192, 168, 1, 3);  // rango gaspar
//IPAddress ip(192, 168, 133, 3);  // rango protopixel (estrella)
//PAddress ip(10, 1, 10, 3);  // rango robot (joguines)

IPAddress destIp(192, 168, 1, 2); // mac pero no se usa
//IPAddress ip(10, 100, 50, 3); 
//IPAddress destIp(10, 100, 50, 2);
//IPAddress destIp(10, 1, 10, 2);  // rango robot (joguines)

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

#define CE_PIN   A1
#define CSN_PIN  A0

int radioId; 
RF24 radio(CE_PIN, CSN_PIN); // Create a Radio
uint32_t CMD[ 8 ]; // CMD + PARAM
// NOTE: the "LL" at the end of the constant is "LongLong" type
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe

//int ch = 0x4C ;  // ESTRELLA
int ch = 0x76 ; // JOGUINES
//int ch = 0x20 ; // GASPAR

char c;
int i;
int bitCounter=0;


unsigned long previousMillis = 0;        // will store last time LED was updated
const long interval = 10;  

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




void setup() 
{
    // Open serial communications and wait for port to open:
  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);

  radio.begin();
  //printf_begin();
  radio.setDataRate( RF24_250KBPS );
  radio.setPALevel( RF24_PA_MAX );
  radio.setAutoAck(0);
  radio.setPayloadSize(8);
  radio.setChannel(ch); 
  radio.openWritingPipe(pipe);
  radio.printDetails();
  
}


void test(OSCMessage &msg)
{
  Serial.println("test");  
}

void reboot(OSCMessage &msg)
{
  Serial.println("reset"); 
  resetBoard();
}


void cmdProcess( OSCMessage &msg )
{
      unsigned int frequency = 0;

      //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
     
    
      // CMD lo pilla directamente ya que parte el array automatico en 8 grupos de 8bits
        radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA
        delay(1);
       
        //Serial.print(CMD[0]);
        //Serial.print(",");
        //Serial.print(CMD[1]);
        //Serial.println();
      
}
 


void loop() {


  ////////////////////////////////////////////////////////////
  /////// RUTINA TRATAMIENTO DE COMANDOS  OSC    /////////////
  ////////////////////////////////////////////////////////////

    // SEND A OSC MESSAGE

    //OSCMessage msg("/test");
    //msg.add("hello, osc.");
    //Udp.beginPacket(outIp, outPort);
    //msg.send(Udp);
    //Udp.endPacket();
    //msg.empty();

//unsigned long currentMillis = millis();

//if (currentMillis - previousMillis >= interval) 
//{
//    previousMillis = currentMillis;
  
  
/*
  CMD[0] = CMD[0]+1;
  if(CMD[0] > 255)
    CMD[0] = 0;
    
  radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA
  delay(1000);
  
  Serial.println(CMD[0]);
*/
  
  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);
      }
  }

  //delay(15);
//}

  ////////////////////////////////////////////////////////////
  /////// RUTINA TRATAMIENTO DE COMANDOS  UART   /////////////
  ////////////////////////////////////////////////////////////

/*
  if (Serial.available())
    {
      c = Serial.read();
      //Serial.print(c);

      if(c > 200) // CMD received
      {
        CMD[0] = c;
        bitCounter=0;
      }
      else
      {
        if(bitCounter == 1)
          CMD[1] = c;
        else if(bitCounter == 2)
          CMD[2] = c;
        else if(bitCounter == 3)
          CMD[3] = c;

      } 

      bitCounter++;

      if(bitCounter == 4)
      {

        radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA

        // Serial.write(CMD[0]);
        // Serial.write(CMD[1]);
        // Serial.write(CMD[2]);
        // Serial.write(CMD[3]);
        // Serial.println();
      }

    }

*/

}