#include <SPI.h>
//#include "wiring_private.h" 
#include <Ethernet.h>
#include <EthernetUdp.h>       

//#define echoPin1 12
#define echoPin1 9
#define echoPin2 6
#define echoPin3 11
//#define echoPin4 9
#define echoPin4 12

#define IP              11
#define SENSORQTY       2
#define PULSEDURATION   5000

#define DEBUG     0

int dipPin[7]  = { 1, 0, A5, 20, 21, 5, 13};


int sensor1, sensor2, sensor3, sensor4;

short flags1 = 1; 
short flags2 = 1;
short flags3 = 1;
short flags4 = 1;

// 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, 0xE0+IP
};

IPAddress ip(192, 168, 21, IP);  // Direccion local 

IPAddress destIp(192, 168, 21, 51); //  Direccion del servidor 192.168.21.51 (TIMELINE SERVER)

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;

#define  UDP_RX_PACKET_MAX_SIZE  8

//#define CE_PIN   A1
#define CSN_PIN  10

uint32_t CMD[ 8 ]; // CMD + PARAM
char packetBuffer[UDP_TX_PACKET_MAX_SIZE];  // buffer to hold incoming packet,
char ReplyBuffer[] = "Hello, i'm BOX XX  ";        // a string to send back
char sensorBuffer[] = "id:xx,sx:x  ";

unsigned long previousMillis1 = 0;       
unsigned long previousMillis2 = 0;     
unsigned long previousMillis3 = 0;    
unsigned long previousMillis4 = 0;        


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


byte id()
{
  int id = 0;
  for (byte i = 0; i < 4; i++ ) bitWrite(id, i, digitalRead(dipPin[i]));
  return id;
}


byte sensorNum()
{
  int id = 0;
  for (byte i = 4; i < 6; i++ ) bitWrite(id, i-4, digitalRead(dipPin[i]));
  return id;
}


void setup() 
{


  if(DEBUG)
  {
    Serial.begin(115200);
    while (!Serial) {
      ; // wait for serial port to connect. Needed for Leonardo only
    }
  }

  
  pinMode(echoPin1, INPUT);
  pinMode(echoPin2, INPUT);
  pinMode(echoPin3, INPUT);
  pinMode(echoPin4, INPUT);


  for (int i = 0; i < 7; i++)
  {
    pinMode(dipPin[i], INPUT);
  }


  if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware
  {
    mac[5] = 0xE0 + id();
    IPAddress ip2(192, 168, 21, id());  // Direccion local en modo manual DIP    
    Ethernet.begin(mac, ip2);
  }
  else
    Ethernet.begin(mac, ip); //  // Direccion local en modo HARDCODED



   if(DEBUG)
   {
      Serial.println("IP ADDRESS: ");
      Serial.println(Ethernet.localIP());
      
      byte macBuffer[6];  // create a buffer to hold the MAC address
      Ethernet.MACAddress(macBuffer); // fill the buffer
      Serial.print("The MAC address is: ");
      for (byte octet = 0; octet < 6; octet++) 
      {
        Serial.print(macBuffer[octet], HEX);
        if (octet < 5) {
          Serial.print('-');
        }
      }
      Serial.println();
   }

    
/*
  // 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);

  if(DEBUG)
  {
    Serial.println("---- DIP CONFIG -----");
    Serial.print("id (dip): ");
    Serial.println(id());  
    Serial.print("Mode (dip): ");
    Serial.println(digitalRead(dipPin[6]));
    Serial.print("Sensor QTY (dip): ");
    Serial.println(sensorNum());
    Serial.println("---------");
  }


  
}


void reboot()
{
  if(DEBUG)
    Serial.println("reset"); 
  resetBoard();
}


void check_sensor1()
{
  sensor1 = digitalRead(echoPin1);
  
  if( (sensor1) & flags1 )
  {
      Serial.println("sensor 1: OFF -------- RAW");
      flags1 = 0;

  }
  else if ( (!sensor1) & !flags1 )
  {
      Serial.println("sensor 1: ON -------- RAW");
      flags1 = 1;
  }
}


void check_sensor1_pulse()
{
  sensor1 = digitalRead(echoPin1);

  if ( (sensor1) & flags1 )  // DETECTO EL CERO Y CUENTO TIEMPO PARA PULSO OFF
  {
      //Serial.println("sensor 1: OFF -------- RAW ");
      flags1 = 0;
      previousMillis1 = millis();
      
  }
  else if ( (!sensor1) & !flags1 ) // DETECTO FLANCO POSITIVO
  {
      if(DEBUG)
        Serial.println("sensor 1: ON -------- RAW ");
        
      flags1 = 1;
      //previousMillis1 = millis();
      
      if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware                 
          sprintf(sensorBuffer, "id:%d,s1:1\r\n", id());
      else
          sprintf(sensorBuffer, "id:%d,s1:1\r\n", IP);

      Udp.beginPacket(destIp, destPort);
      //Udp.write("id");
      //Udp.write( String(IP) );
      //Udp.write("S1:1\r\n");
      Udp.write(sensorBuffer);
      Udp.endPacket();
      
      delay(30);
      
  }
  else if( ( millis() - previousMillis1 > PULSEDURATION) & !flags1 )
  {
      if(DEBUG)
        Serial.println("sensor 1: OFF -------- PULSE ");
      
      flags1 = 1;

      if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware                 
          sprintf(sensorBuffer, "id:%d,s1:0\r\n", id());
      else
          sprintf(sensorBuffer, "id:%d,s1:0\r\n", IP);

      Udp.beginPacket(destIp, destPort);

      //Udp.write("id");
      //Udp.write(IP);
      //Udp.write("S1:0\r\n");
      
      Udp.write(sensorBuffer);
      Udp.endPacket();
      
      delay(30);

  } 
  
}


void check_sensor2()
{
  sensor2 = digitalRead(echoPin2);
  
  if( (sensor2) & flags2 )
  {
      if(DEBUG)
        Serial.println("sensor 2: OFF --------------- ");
      
      flags2 = 0;
  }
  else if ( (!sensor2) & !flags2 )
  {
      if(DEBUG)
        Serial.println("sensor 2: ON ");
        
      flags2 = 1;
  }
}



void check_sensor2_pulse()
{
  sensor2 = digitalRead(echoPin2);

  if ( (sensor2) & flags2 )  // DETECTO EL CERO Y CUENTO TIEMPO PARA PULSO OFF
  {
      //Serial.println("sensor 2: OFF -------- RAW ");
      flags2 = 0;
      previousMillis2 = millis();
      
  }
  else if ( (!sensor2) & !flags2 ) // DETECTO FLANCO POSITIVO
  {
      if(DEBUG)
        Serial.println("sensor 2: ON -------- RAW ");
        
      flags2 = 1;
      //previousMillis2 = millis();
      
      Udp.beginPacket(destIp, destPort);
      //Udp.write("S2:1");
      
      if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware                 
          sprintf(sensorBuffer, "id:%d,s2:1\r\n", id());
      else
          sprintf(sensorBuffer, "id:%d,s2:1\r\n", IP);
          
      Udp.write(sensorBuffer);
      Udp.endPacket();
      
      delay(30);
      
  }
  else if( ( millis() - previousMillis2 > PULSEDURATION) & !flags2 )
  {
      if(DEBUG)
        Serial.println("sensor 2: OFF -------- PULSE ");
      
      flags2 = 1;

      if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware                 
          sprintf(sensorBuffer, "id:%d,s2:0\r\n", id());
      else
          sprintf(sensorBuffer, "id:%d,s2:0\r\n", IP);

      Udp.beginPacket(destIp, destPort);
      //Udp.write("S2:0");
      Udp.write(sensorBuffer);
      Udp.endPacket();
      
      delay(30);

  } 
  
}



void check_sensor3()
{
  sensor3 = digitalRead(echoPin3);
  
  if( (sensor3) & flags3 )
  {
      if(DEBUG)
        Serial.println("sensor 3: OFF --------------- ");
        
      flags3 = 0;
  }
  else if ( (!sensor3) & !flags3 )
  {
      if(DEBUG)
        Serial.println("sensor 3: ON ");
        
      flags3 = 1;
  }
}



void check_sensor3_pulse()
{
  sensor3 = digitalRead(echoPin3);

  if ( (sensor3) & flags3 )  // DETECTO EL CERO Y CUENTO TIEMPO PARA PULSO OFF
  {
      //Serial.println("sensor 3: OFF -------- RAW ");
      flags3 = 0;
      previousMillis3 = millis();
      
  }
  else if ( (!sensor3) & !flags3 ) // DETECTO FLANCO POSITIVO
  {
      if(DEBUG)
        Serial.println("sensor 3: ON -------- RAW ");
        
      flags3 = 1;
      //previousMillis3 = millis();
      if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware                 
          sprintf(sensorBuffer, "id:%d,s3:1\r\n", id());
      else
          sprintf(sensorBuffer, "id:%d,s3:1\r\n", IP);

      Udp.beginPacket(destIp, destPort);
      //Udp.write("S3:1");
      
      Udp.write(sensorBuffer);
      Udp.endPacket();
      
      delay(30);
      
  }
  else if( ( millis() - previousMillis3 > PULSEDURATION) & !flags3 )
  {
      if(DEBUG)
        Serial.println("sensor 3: OFF -------- PULSE ");
      
      flags3 = 1;
      
      if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware                 
          sprintf(sensorBuffer, "id:%d,s3:0\r\n", id());
      else
          sprintf(sensorBuffer, "id:%d,s3:0\r\n", IP);

      Udp.beginPacket(destIp, destPort);
      //Udp.write("S3:0");

      Udp.write(sensorBuffer);
      Udp.endPacket();
      
      delay(30);

  } 
  
}


void check_sensor4()
{
  sensor4 = digitalRead(echoPin4);
  
  if( (sensor4) & flags4 )
  {
      if(DEBUG)
        Serial.println("sensor 4: OFF --------------- ");
        
      flags4 = 0;
  }
  else if ( (!sensor4) & !flags4 )
  {
      if(DEBUG) 
        Serial.println("sensor 4: ON ");
        
      flags4 = 1;
  }
}



void check_sensor4_pulse()
{
  sensor4 = digitalRead(echoPin4);

  if ( (sensor4) & flags4 )  // DETECTO EL CERO Y CUENTO TIEMPO PARA PULSO OFF
  {
      //Serial.println("sensor 2: OFF -------- RAW ");
      flags4 = 0;
      previousMillis4 = millis();
      
  }
  else if ( (!sensor4) & !flags4 ) // DETECTO FLANCO POSITIVO
  {
      if(DEBUG)
        Serial.println("sensor 4: ON -------- RAW ");
        
      flags4 = 1;
      //previousMillis4 = millis();
      if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware                 
          sprintf(sensorBuffer, "id:%d,s4:1\r\n", id());
      else
          sprintf(sensorBuffer, "id:%d,s4:1\r\n", IP);

      Udp.beginPacket(destIp, destPort);
      //Udp.write("S4:1");
      
      Udp.write(sensorBuffer);
      Udp.endPacket();
      
      delay(30);
      
  }
  else if( ( millis() - previousMillis4 > PULSEDURATION) & !flags4 )
  {
      if(DEBUG)
        Serial.println("sensor 4: OFF -------- PULSE ");
      
      flags4 = 1;
      sprintf(sensorBuffer, "id:%d,s4:0\r\n", IP);

      Udp.beginPacket(destIp, destPort);
      //Udp.write("S4:0");
      
      if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware                 
          sprintf(sensorBuffer, "id:%d,s4:0\r\n", id());
      else
          sprintf(sensorBuffer, "id:%d,s4:0\r\n", IP);

      Udp.write(sensorBuffer);
      Udp.endPacket();
      
      delay(30);

  } 
  
}



void loop() 
{

  ////////////////////////////////////////////////////////////
  /////// RUTINA TRATAMIENTO DE SENSORES    /////////////
  ////////////////////////////////////////////////////////////

  if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware
  {
  switch (sensorNum())  // en modo manual chequeo bits de numero de sensores activos
    {
      case 0:
        check_sensor1_pulse();
        check_sensor2_pulse();
        break;
      case 1:
        check_sensor1_pulse();
        check_sensor2_pulse();
        break;
      case 2:
        check_sensor1_pulse();
        check_sensor2_pulse();
        check_sensor3_pulse();
        break;
      case 3:
        check_sensor1_pulse();
        check_sensor2_pulse();
        check_sensor3_pulse();
        check_sensor4_pulse();
        break;
    }
  } 
  else  // modo hardcoded en firmware y flash eeprom con UDP config
  {
    switch (SENSORQTY)  // en modo hardcoded
    {
      case 2:
        check_sensor1_pulse();
        check_sensor2_pulse();
        break;
      case 3:
        check_sensor1_pulse();
        check_sensor2_pulse();
        check_sensor3_pulse();
        break;
      case 4:
        check_sensor1_pulse();
        check_sensor2_pulse();
        check_sensor3_pulse();
        check_sensor4_pulse();
        break;
    }
  }
  

    
  
  ////////////////////////////////////////////////////////////
  /////// RUTINA TRATAMIENTO DE COMANDOS  UDP    /////////////
  ////////////////////////////////////////////////////////////

  
  int packetSize;
  
  if ( (packetSize = Udp.parsePacket()) > 0)
  {   
      //Serial.print("mensaje recibido: ");
      //Serial.println(size);
      while (packetSize--)
      {
        IPAddress remote = Udp.remoteIP();

        if(DEBUG)
        {
            Serial.print("Received packet of size ");
            Serial.println(packetSize);
            Serial.print("From ");
            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 packetBuffer
        Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);

        if(DEBUG)
        {
          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(), destPort);
        
        if(digitalRead(dipPin[6])) // si modo DIP manual -> pin dip7 HIGH -> LED 13 ON por hardware
          sprintf(ReplyBuffer, "Hello, i'm BOX:%d\r\n", id());
        else
          sprintf(ReplyBuffer, "Hello, i'm BOX:%d\r\n", IP); 
          
        Udp.write(ReplyBuffer);
        Udp.endPacket();
        
      }

     
  }

  

  //delay(10);

  

}