/***************************************************
  This is a library for the CAP1188 I2C/SPI 8-chan Capacitive Sensor

  Designed specifically to work with the CAP1188 sensor from Adafruit
  ----> https://www.adafruit.com/products/1602

  These sensors use I2C/SPI to communicate, 2+ pins are required to
  interface
  Adafruit invests time and resources providing this open source code,
  please support Adafruit and open-source hardware by purchasing
  products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries.
  BSD license, all text above must be included in any redistribution
 ****************************************************/

#include <Wire.h>
#include <SPI.h>
#include <Adafruit_CAP1188.h>
#include "wiring_private.h"
//#include <Keyboard.h>

#include <Ethernet.h>
#include <EthernetUdp.h>
#include <OSCBundle.h>
#include <OSCBoards.h>
#include <OSCMessage.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, 0xED
};
IPAddress ip(192, 168, 1, 3);
IPAddress destIp(192, 168, 1, 2);
//IPAddress ip(10, 100, 50, 3);
//IPAddress destIp(10, 100, 50, 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;

#define  UDP_RX_PACKET_MAX_SIZE  8

uint32_t CMD[ 4 ]; // CMD + Ch + 2

// Use I2C, no reset pin!
Adafruit_CAP1188 cap1 = Adafruit_CAP1188();
Adafruit_CAP1188 cap2 = Adafruit_CAP1188();
Adafruit_CAP1188 cap3 = Adafruit_CAP1188();

bool ledFlag1;
bool ledFlag2;
bool ledFlag3;
bool ledFlag4;
bool ledFlag5;
bool ledFlag6;
bool ledFlag7;

int PWM[8] = { 0, 1, 13, 12, 11, 9, 6, 5 };

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

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

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

void scape(OSCMessage &msg)
{
  Serial.println("scape");
  //Keyboard.write(27);         // sends ASCII value 27, or SCAPE

}



void calibrate(OSCMessage &msg)
{
 Serial.println("calibration");
 
 if (!cap1.begin()) {
    Serial.println("CAP1188-29 not found");
    while (1);
  }
  Serial.println("CAP1188-29 found!");

  if (!cap2.begin(0x2B)) {
    Serial.println("CAP1188-28 not found");
    while (1);
  }
  Serial.println("CAP1188-28 found!");

  if (!cap3.begin(0x28)) {
    Serial.println("CAP1188-2B not found");
    while (1);
  }
  Serial.println("CAP1188-2B found!");
  
  cap1.calibration(0xFF);
  cap2.calibration(0xFF);
  cap3.calibration(0xFF);

}

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

  //Serial.print(CMD[0]);
  //Serial.print(",");
  //Serial.print(CMD[1]);
  //Serial.println();

  switch (CMD[0]) {
    case 1:
      analogWrite(PWM[7], CMD[1]);
      break;
    case 2:
      analogWrite(PWM[6], CMD[1]);
      break;
    case 3:
      analogWrite(PWM[5], CMD[1]);
      break;
    case 4:
      analogWrite(PWM[4], CMD[1]);
      break;
    case 5:
      analogWrite(PWM[3], CMD[1]);
      break;
    case 6:
      analogWrite(PWM[2], CMD[1]);
      break;
    case 7:
      analogWrite(PWM[1], CMD[1]);
      break;
    case 8:
      analogWrite(PWM[7], CMD[1]);
      analogWrite(PWM[6], CMD[1]);
      analogWrite(PWM[5], CMD[1]);
      analogWrite(PWM[4], CMD[1]);
      analogWrite(PWM[3], CMD[1]);
      analogWrite(PWM[2], CMD[1]);
      analogWrite(PWM[1], CMD[1]);
      break;
//    default: 
      // if nothing else matches, do the default
      // default is optional
//    break;
  }

  
}

void sendOsc( int sensor, int value )
{
  OSCMessage msg("/sensor");
  msg.add(sensor);
  msg.add(value);
  Udp.beginPacket(destIp, destPort);
  msg.send(Udp);
  Udp.endPacket();
  msg.empty();
}


void killapp( void )
{
  OSCMessage msg("/killapp");
  msg.add(1);
  //msg.add(value);
  Udp.beginPacket(destIp, destPort);
  msg.send(Udp);
  Udp.endPacket();
  msg.empty();
}

void setup() {

  for (int i = 0 ; i < 8 ; i++)
  {
    pinMode(PWM[i], OUTPUT);
    digitalWrite(PWM[i], HIGH);
  }

  delay(1000);
  
  Serial.begin(9600);

  //Keyboard.begin();
  //Keyboard.write(27);         // sends ASCII value 27, or SCAPE

  Serial.println("CAP1188 test!");

  // Initialize the sensor, if using i2c you can pass in the i2c address

  if (!cap1.begin()) {
    Serial.println("CAP1188-29 not found");
    while (1);
  }
  Serial.println("CAP1188-29 found!");

  if (!cap2.begin(0x2B)) {
    Serial.println("CAP1188-28 not found");
    while (1);
  }
  Serial.println("CAP1188-28 found!");

  if (!cap3.begin(0x28)) {
    Serial.println("CAP1188-2B not found");
    while (1);
  }
  Serial.println("CAP1188-2B found!");

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

  //cap1.sensibility(0x4F);
  //cap1.calibration(0xFF);
  //cap2.sensibility(0x4F);
  //cap2.calibration(0xFF);
  //cap3.sensibility(0x4F);
  //cap3.calibration(0xFF);
  
  delay(1000);
  killapp();
  
}

void loop() 
{

/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
/////////////////////////////////////////////////////

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


/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
/////////////////////////////////////////////////////

  uint8_t touched1 = cap1.touched();
  uint8_t touched2 = cap2.touched();
  uint8_t touched3 = cap3.touched();


  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval)
  {
    previousMillis = currentMillis;
    counter++;


    ///////////////////////

    if ( (touched1 & (1 << 0)) && !ledFlag1 ) // CAP1 pin
    {
      ledFlag1 = 1;
      Serial.println("sensor1 ON");
      digitalWrite(PWM[7], LOW);
      sendOsc(1, 1);
    }
    else if ( !(touched1 & (1 << 0)) && ledFlag1 )
    {
      ledFlag1 = 0;
      Serial.println("sensor1 OFF");
      digitalWrite(PWM[7], HIGH);
      sendOsc(1, 0);
    }

    if ( (touched1 & (1 << 5)) && !ledFlag2 ) // CAP1 pin
    {
      ledFlag2 = 1;
      Serial.println("sensor2 ON");
      digitalWrite(PWM[6], LOW);
      sendOsc(2, 1);
    }
    else if ( !(touched1 & (1 << 5)) && ledFlag2 )
    {
      ledFlag2 = 0;
      Serial.println("sensor2 OFF");
      digitalWrite(PWM[6], HIGH);
      sendOsc(2, 0);
    }

    //////////////////////

    if ( (touched2 & (1 << 0)) && !ledFlag3 ) // CAP1 pin
    {
      ledFlag3 = 1;
      Serial.println("sensor3 ON");
      digitalWrite(PWM[5], LOW);
      sendOsc(3, 1);
    }
    else if ( !(touched2 & (1 << 0)) && ledFlag3 )
    {
      ledFlag3 = 0;
      Serial.println("sensor3 OFF");
      digitalWrite(PWM[5], HIGH);
      sendOsc(3, 0);
    }

    if ( (touched2 & (1 << 4)) && !ledFlag4 ) // CAP1 pin
    {
      ledFlag4 = 1;
      Serial.println("sensor4 ON");
      digitalWrite(PWM[4], LOW);
      sendOsc(4, 1);
    }
    else if ( !(touched2 & (1 << 4)) && ledFlag4 )
    {
      ledFlag4 = 0;
      Serial.println("sensor4 OFF");
      digitalWrite(PWM[4], HIGH);
      sendOsc(4, 0);
    }

    if ( (touched2 & (1 << 7)) && !ledFlag5 ) // CAP1 pin
    {
      ledFlag5 = 1;
      Serial.println("sensor5 ON");
      digitalWrite(PWM[3], LOW);
      sendOsc(5, 1);
    }
    else if ( !(touched2 & (1 << 7)) && ledFlag5 )
    {
      ledFlag5 = 0;
      Serial.println("sensor5 OFF");
      digitalWrite(PWM[3], HIGH);
      sendOsc(5, 0);
    }

    //////////////////////////////////

    if ( (touched3 & (1 << 0)) && !ledFlag6 ) // CAP1 pin
    {
      ledFlag6 = 1;
      Serial.println("sensor6 ON");
      digitalWrite(PWM[2], LOW);
      sendOsc(6, 1);
    }
    else if ( !(touched3 & (1 << 0)) && ledFlag6 )
    {
      ledFlag6 = 0;
      Serial.println("sensor6 OFF");
      digitalWrite(PWM[2], HIGH);
      sendOsc(6, 0);
    }

    if ( (touched3 & (1 << 7)) && !ledFlag7 ) // CAP1 pin
    {
      ledFlag7 = 1;
      Serial.println("sensor7 ON");
      digitalWrite(PWM[1], LOW);
      sendOsc(7, 1);
    }
    else if ( !(touched3 & (1 << 7)) && ledFlag7 )
    {
      ledFlag7 = 0;
      Serial.println("sensor7 OFF");
      digitalWrite(PWM[1], HIGH);
      sendOsc(7, 0);
    }

///////////////////////////////////

    //Serial.println(counter);
    
  }

}