#include <SPI.h>
#include "wiring_private.h" // pinPeripheral() function

#define ETHERNET_ON true
#define RADIOSEND   true

#if ETHERNET_ON
  #include <Ethernet2.h>
  #include <EthernetUdp2.h>         // UDP library from: bjoern@cs.stanford.edu 12/30/2008
  #include <OSCBundle.h>
  #include <OSCBoards.h>
  #include <OSCMessage.h>
#endif

#include "nRF24L01MOD.h"
#include "RF24MOD.h"
#include "printfMOD.h"


#if ETHERNET_ON
  // 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);
  
  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;

#endif

#define CE_PIN   A1
#define CSN_PIN  A0

int radioId; 

RF24 radio(CE_PIN, CSN_PIN); // Create a Radio

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

// NOTE: the "LL" at the end of the constant is "LongLong" type
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe
int ch = 76; // Por defecto es el canal 76 (showroom)
char c;
int i;
int bitCounter=0;


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


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
  //}
  #if ETHERNET_ON
    // start the Ethernet and UDP:
    Ethernet.begin(mac, ip);
    Udp.begin(localPort);

  #endif


  radioId = 0; // MASTER
  radio.begin();
  printf_begin();
  radio.setDataRate( RF24_250KBPS );
  radio.setPALevel( RF24_PA_MAX );
  radio.setAutoAck(0);
  radio.setPayloadSize(8);
  radio.setChannel(ch); 
  #if RADIOSEND
    radio.openWritingPipe(pipe);
  #else
    radio.openReadingPipe(1, pipe);
    radio.startListening();
  #endif
  radio.printDetails();
  
}




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


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

      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
      if (msg.isInt(2)){        
        CMD[2] = msg.getInt(2);
      } //otherwise it's a floating point frequency in Hz
      if (msg.isInt(3)){        
        CMD[3] = msg.getInt(3);

        radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA
        //delay(1);
        
        //Serial.print(CMD[0]);€
        //Serial.print(",");
        //Serial.print(CMD[1]);
        //Serial.print(",");
        //Serial.print(CMD[2]);
        //Serial.print(",");
        //Serial.print(CMD[3]);
        //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();

  #if ETHERNET_ON

      //unsigned long currentMillis = millis();

      //if (currentMillis - previousMillis >= interval) 
      //{
      //    previousMillis = currentMillis;
          
          OSCMessage msg;
          int size;
          
          if ( (size = Udp.parsePacket()) > 0)
          {   
              //Serial.print("mensaje recibido: ");
              //Serial.println(size);

              while (size--)
                msg.fill(Udp.read());
              

              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("/cmd", cmdProcess );    // this is how it is marked on the silkscreen
              }
              else 
              {
                error = msg.getError();
                Serial.print("error: ");
                Serial.println(error);
              }
          }

          //delay(15);
      //}

   #endif
*/

  ////////////////////////////////////////////////////////////
  /////// 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.print(CMD[0]);€
        //Serial.print(",");
        //Serial.print(CMD[1]);
        //Serial.print(",");
        //Serial.print(CMD[2]);
        //Serial.print(",");
        //Serial.print(CMD[3]);
        //Serial.println();
      }

    }


}