//#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <wiring_private.h> // pinPeripheral() function
#include <Adafruit_CAP1188.h>
#include "mcp_can.h"
#include "mcp2515_can.h"

#define DEBUG         0
#define TEST          0
#define ETHERNET_ON   1
#define ID            0x00

#if ETHERNET_ON
  #include <Ethernet.h>
  #include <EthernetUdp.h>         // UDP library from: bjoern@cs.stanford.edu 12/30/2008
#endif

#define  UDP_RX_PACKET_MAX_SIZE  8

#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, 4, 2); // localIP
  IPAddress destIp(192, 168, 4, 133); // Brightsign player
  //IPAddress destIp(192, 168, 4, 3); // mac

  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;

  char packetBuffer[UDP_TX_PACKET_MAX_SIZE];  // buffer to hold incoming packet,

#endif

int verde = 6;
int rojo = 5;
int blanco = 9;

int inc = 1;

const long timewindow = 120000;           // interval at which to blink (milliseconds)

int brightness;
bool touchedFlag = 0;
bool externTouch = 0;
bool modul1, modul2;
unsigned long currentMillis = 0;
unsigned long previousMillis = 0;        // will store last time LED was updated

const int SPI_CS_PIN = A5; // pin A5 Feather M0
const int CAN_RESET_PIN = A3; // pin A3 Feather M0

mcp2515_can CAN(SPI_CS_PIN);
unsigned char stmp[8] = {0, 0, 0, 0, 0, 0, 0, 0};
unsigned char len = 8;
unsigned char buf[8];

int sensorAct = 0;
int sensorActLast = 0;
int countSensor = 0;


SPIClass SPI_CAN(&sercom1, 12,  13,  11, SPI_PAD_0_SCK_1, SERCOM_RX_PAD_3);

// Reset Pin is used for I2C or SPI
//#define CAP1188_RESET 9

// Use I2C, no reset pin!
Adafruit_CAP1188 cap = Adafruit_CAP1188();

// Or...Use I2C, with reset pin
// Adafruit_CAP1188 cap = Adafruit_CAP1188(CAP1188_RESET);

void setup()
{

  pinMode(verde, OUTPUT);
  pinMode(rojo, OUTPUT);
  pinMode(blanco, OUTPUT);
  
  Serial.begin(9600);
  if(DEBUG)
  {
    while (!Serial) {
        delay(1);
    }
  }

  #if ETHERNET_ON
  
    // 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);
    if(DEBUG)
      Serial.println("Ethernet Ready!");

  #endif

  
  if(DEBUG)
    Serial.println("CAP1188 test!");
  
  analogWrite(verde, 255);
  analogWrite(rojo, 255);  
  analogWrite(blanco, 255);


  if (!cap.begin(0x29))
  {
    if (!cap.begin()) {
      if(DEBUG)
        Serial.println("CAP1188 not found");
        
      //while (1)
      //;
    }
  }
  if(DEBUG)
    Serial.println("CAP1188 found!");

  delay(1000); // esperar 1s a que arranque toda la electronica

  cap.sensibility(0x2F);  // 32x sensibility
  cap.calibration(0xFF);   // force calibration
  
  //cap.sensibility(0x4F);  // 8x sensibility
  //cap.writeRegister(0x2A, 0x00); // multiple touch off
  //cap.writeRegister(0x38, 0x11); // threshold noise 50%
 
  // CAN RESET PIN
  pinMode(CAN_RESET_PIN, OUTPUT);
  digitalWrite(CAN_RESET_PIN, HIGH);
  CAN.setSPI(&SPI_CAN);
  delay(100);

  while (CAN_OK != CAN.begin(CAN_500KBPS))
  { // init can bus : baudrate = 500k
    if(DEBUG)
    {
      Serial.println("CAN BUS Shield init fail");
      Serial.println(" Init CAN BUS Shield again");
      delay(100);
    }
  }
  if(DEBUG)
    Serial.println("CAN BUS Shield init ok!");

  //delay(1000);
  previousMillis = millis();
  
}

void loop()
{

  
 /////////// GESTION DE LOS EVENTOS CAPACITIVOS /////////////////

 
  uint8_t touched = cap.touched();

  if ( (touched & 0x7F) && (!touchedFlag) && (!externTouch) )   // solo si no hay bloque externo
  {

    //if((touched & 0x80))
    //  Serial.println("pad 8");
      
    touchedFlag = 1;
    previousMillis = currentMillis ; // pongo a cero el timer cuando pulso
    
    for (uint8_t i = 0; i < 7; i++)  // Del 1 al 7, reservamos la 8 para el ID
    {
      if (touched & (1 << i))
      {
        if(DEBUG)
        {
          Serial.print("C");
          Serial.print(i + 1);
          Serial.print("\t");
        }
        stmp[i] = 1;
        
        if(i>3)
          modul1 = 1;
        else if(i<3)
          modul2 = 1;
          
        break;
      }
    }
    if(DEBUG)
      Serial.println();

    stmp[7] = ID; 
    for(int i = 0; i<7; i++)    // Send data by UDP
    {
        if( stmp[i])
        {
          Udp.beginPacket(destIp, destPort);
          Udp.print("C");
          Udp.print(stmp[7]);
          Udp.print(i+1);
          Udp.endPacket();
          break;
        }
    }

    // CAN DE FEEDBACK PARA QUE LOS OTROS DEVICES QUE SEPAN QUE SE HAN DE BLOQUEAR
    stmp[7] = ID; 
    CAN.sendMsgBuf(ID, 0, 8, stmp); // send data:  id = 0x00, standrad frame, data len = 8, stmp: data buf

    ////// DETECCION DE CUELGUES DEL SENSOR Y AUTOCALIBRACION LOCAL
    for(int i = 0; i<7; i++)    // check pads repetitions
    {
      if( stmp[i]) // si hay algun sensor activo
      {        
        sensorAct = stmp[7]*10+i+1; // Detectar que sensor es el que se ha lanzado
        if(sensorAct == sensorActLast)
          countSensor++;
        else
        {
          countSensor=0;
        }
        sensorActLast = sensorAct;
      }

    }
    if(DEBUG)
    {
            Serial.println("-----------------------------");
            Serial.print("SensorAct: ");
            Serial.print(sensorAct);
            Serial.print("  CountSensor: ");
            Serial.println(countSensor);           
    }
    //delay(50); // Evita enviar varios mensajes seguidos. Experimentar con este tiempo!

  }
  else
  {
    for (uint8_t i = 0; i < 7; i++)  // Del 1 al 7, reservamos la 8 para el ID
      stmp[i] = 0;
  }


 /////////// TIMER PARA CONTROL DE LEDS /////////////////

  currentMillis = millis();

  if ( (currentMillis - previousMillis >= timewindow) && (touchedFlag) ) 
  {
    touchedFlag = 0;
    modul1 = 0;
    modul2 = 0;
    
    analogWrite(verde, 255);
    analogWrite(rojo, 255);

    brightness = 0;
    
    if(DEBUG)
      Serial.println("Sensors unblocked");
      
  }
  else if ( (currentMillis - previousMillis < timewindow) && (touchedFlag) )
  {
    // animacion LED
    if(!externTouch) // mientras no haya touch externo por red o can
    {
      brightness = brightness + inc;

      if(modul1)
      {
        analogWrite(verde, brightness); // modul1
      }
      else if(modul2)
      {
        //analogWrite(verde, brightness);
        analogWrite(rojo, brightness); // modul2
      }
      delay(5);
      if(brightness > 150)
          inc = -1;
      else if(brightness <= 0)
          inc = 1;
    }

  }
  


 /////////// TX CAN TEST  /////////////////
/*
 if(TEST)
 {
  
  stmp[7] = stmp[7]+1;
  if(stmp[7] == 100)
  {
      stmp[7] = 0;
      stmp[6] = stmp[6] + 1;
      
      if(stmp[6] == 100)
      {
          stmp[6] = 0;
          stmp[5] = stmp[6] + 1;
      }
  }
 }
*/

  /////////// TX CAN SEND  /////////////////
/*
  if(DEBUG)
  {
    Serial.println("-----------------------------");
    Serial.print("Send data from ID: 0x0");
    Serial.println(ID);
 
    
    for(int i = 0; i<len; i++)    // print the data
    {
       if(DEBUG)
       {
        Serial.print(stmp[i], HEX);
        Serial.print("\t");
       }
    }
    Serial.println();
  }
  */
  
 
 /////////// RX CAN  /////////////////

  if(CAN_MSGAVAIL == CAN.checkReceive())    // check if data coming
  {
      CAN.readMsgBuf(&len, buf);  // read data,  len: data length, buf: data buf

      externTouch = 1; // bloqueo animacion cuando recibo un can del vecino
      
      if(DEBUG)
      {
        //unsigned long canId = CAN.getCanId();
        Serial.println("-----------------------------");
        Serial.print("Get data from ID: 0x0");
        Serial.println(buf[7], HEX);
  
        for(int i = 0; i<7; i++)    // print the data
        {
            Serial.print(buf[i], HEX);
            Serial.print("\t");
        }
        Serial.println();
        Serial.print ("Extern TOUCH: ");
        Serial.println(externTouch);
      }

      //// DETECTOR DE CUELGUES DE SENSOR Y AUTOCALIBRACION DEL RESTO DE MODULOS
      for(int i = 0; i<7; i++)    // check pads repetitions
      {
        if( buf[i]) // si hay algun sensor activo
        {        
          sensorAct = buf[7]*10+i+1; // Detectar que sensor es el que se ha lanzado
          
          if(sensorAct == sensorActLast)
            countSensor++;
          else
          {
            countSensor=0;
          }
          sensorActLast = sensorAct;
       
          if(DEBUG)
          {
            Serial.println("-----------------------------");
            Serial.print("SensorAct: ");
            Serial.print(sensorAct);
            Serial.print("  CountSensor: ");
            Serial.println(countSensor);           
          }
          
        }
        
      }

      for(int i = 0; i<len; i++)    // Send data by UDP
      {
          if( buf[i])
          {
            Udp.beginPacket(destIp, destPort);
            Udp.print("C");
            Udp.print(buf[7]);
            Udp.print(i+1);
            Udp.endPacket();
            break;
          }
      }
  
  }

 /////////// UDP FEEDBACK /////////////////


  int packetSize = Udp.parsePacket();
  if(packetSize)
  {
    if (DEBUG) 
    {
      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);
  
    if(DEBUG)
    {
        Serial.println("Contents:");
        Serial.println(packetBuffer);
    }

    if (strcmp(packetBuffer, "END") == 0)  // comparo string que llega con el "END"
    {
        if(DEBUG)
            Serial.println("Reset LEDs");

        //previousMillis = currentMillis;
        touchedFlag=0; //activa sensores
        modul1 = 0;
        modul2 = 0;
        
        externTouch = 0; //desbloquea sensores OJO SI ESTO NO VA, NO VA NADA!!
    
        analogWrite(verde, 255);
        analogWrite(rojo, 255);
        
        stmp[7] = 0xFF; 
        CAN.sendMsgBuf(ID, 0, 8, stmp); // Broadcast Feedback con "byte 7 = 0xFF"
    }
    else if (strcmp(packetBuffer, "CAL") == 0)  // comparo string que llega con el "END"
    {
        if(DEBUG)
            Serial.println("Calibrate sensors");

        cap.calibration(0xFF);   // force calibration

        stmp[7] = 0x7F;
        CAN.sendMsgBuf(ID, 0, 8, stmp); // Broadcast Feedback con "byte 7 = 0xFF"
    }
    
    
  }

/////// AUTOCALIBRACIÓN POR EXCESO DE SALTOS DEL MISMO SENSOR

  if(countSensor > 1) // numero maximo de saltos del mismo sensor 
  {
     cap.calibration(0xFF);  // force calibration
     stmp[7] = 0x7F;
     CAN.sendMsgBuf(ID, 0, 8, stmp); // Broadcast Feedback con "byte 7 = 0xFF"
     countSensor = 0;
     if (DEBUG) 
     {
        Serial.println("Force calibration!");
     }
  }
  
 
}