/*
  UDPSendReceive.pde:
 This sketch receives UDP message strings, prints them to the serial port
 and sends an "acknowledge" string back to the sender

 A Processing sketch is included at the end of file that can be used to send
 and received messages for testing with a computer.

 created 21 Aug 2010
 by Michael Margolis

 This code is in the public domain.
 */


#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
#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, 60);
  
  unsigned int localPort = 8888;      // local port to listen on
  
  // buffers for receiving and sending data
  char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; //buffer to hold incoming packet,
  char  ReplyBuffer[] = "acknowledged";       // a string to send back
  
  // An EthernetUDP instance to let us send and receive packets over UDP
  EthernetUDP Udp;
#endif

#define CE_PIN   A1
#define CSN_PIN  A0

int radioId = 'F'; // id fijo para la cama. Fuera del rango de los IDs DIP

/*-----( Declare objects )-----*/
RF24 radio(CE_PIN, CSN_PIN); // Create a Radio
/*-----( Declare Variables )-----*/
uint8_t CMD[ 32 ]; // CMD + Ch + 6

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

char c;
String inputString;
bool stringComplete;
int i;

int R = 0;
int G = 0;
int B = 0;

int r1 = 0;
int r2 = 0;
int r3 = 0;
int r4 = 0;

void setup() {
    // Open serial communications and wait for port to open:
  Serial.begin(9600);
  //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 = 1;
  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 loop() {

  #if ETHERNET_ON
      // if there's data available, read a packet
      int packetSize = Udp.parsePacket();
      if (packetSize)
      {
        Serial.print("Received packet of size ");
        Serial.println(packetSize);
        Serial.print("From ");
        IPAddress remote = Udp.remoteIP();
        for (int i = 0; i < 4; i++)
        {
          Serial.print(remote[i], DEC);
          if (i < 3)
          {
            Serial.print(".");
          }
        }
        Serial.print(", port ");
        Serial.println(Udp.remotePort());
    
        // read the packet into packetBufffer
        Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
        Serial.println("Contents:");
        Serial.println(packetBuffer);
    
        // send a reply, to the IP address and port that sent us the packet we received
        Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
        Udp.write(ReplyBuffer);
        Udp.endPacket();
      }
      delay(10);
   #endif

  ////////////////////////////////////////////////////////////
  /////// RUTINA TRATAMIENTO DE STRINGS DEL UART   ///////////
  ////////////////////////////////////////////////////////////

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

      if ((c == '\r') || (c == '\n'))
      {
        if (inputString == "ping")
        {
          Serial.println("received ping");
        }
        else if (inputString.startsWith("ch:"))
        {
          Serial.print("canal: ");
          String str = inputString.substring(inputString.lastIndexOf(":") + 1);
          ch = str.toInt();
          Serial.println(ch);
          radio.setChannel(ch); // canal para sala developpers. Por defecto es el canal 76 (showroom)
        }
        else if (inputString.startsWith("radio:")) // 'radio:' cmd , ch, data1, data2, data3
        {
          // CMD: 'D', 'A', 'U' // antiguos 'p', 's', 'r' , 'b'
          //radio:D,0,1,1 // digital DVK & USB
          //radio:A,0,255,0,0  // rojo DVK
          //radio:B,F,6,255,255,0 rojo CAMA
          //radio:B,F,1 STOP MOTORES

          #if debug
            Serial.println("received radio->" + inputString);
          #endif
          
            String str = inputString.substring(inputString.indexOf(":") + 1);
            
          #if debug
            Serial.println("str->" + str);
          #endif

          String command = str.substring(0, str.indexOf(','));
          command.trim();
          CMD[0] = command[0];

          int index = 1;
          int commaIndex = 0;
          do {
            commaIndex = str.indexOf(',', commaIndex + 1);
            String tmp = str.substring(commaIndex + 1, str.indexOf(',', commaIndex + 1));
            tmp.trim();
            if (isNumeric(tmp)) {
              CMD[index] = tmp.toInt();
            } else {
              CMD[index] = tmp[0];
            }
            index++;
          } while (str.lastIndexOf(',') != commaIndex);

          for (int i = 0; i < index; i++) {
            Serial.println((int)CMD[i]);
          }

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

        }

        //Serial.println(inputString);
        inputString = String();

      }
      else
        inputString += c;

    }
}

/*Todo esto sobra*/

boolean isNumeric(String str) {
  for (char i = 0; i < str.length(); i++) {
    if ( !(isDigit(str.charAt(i)) )) {
      return false;
    }
  }
  return true;
}