#include <Wire.h>
#include <SPI.h>
#include "Ledpixel.h"
#include "nRF24L01.h"
#include "RF24.h"

//#include "printf.h"
#define mgc_on true
#define ble_on true
#define radio_on false
#define DEBUG  1
#define repair_led false

Ledpixel pixels;

int mode=0;

#if radio_on
  // Declarations for NRF24L01
  #define CE_PIN   3
  #define CSN_PIN  2
  
  int radioId = 'F'; // id fijo para la cama. Fuera del rango de los IDs DIP
  int channel = 0x4C;
  int id = 5;
  RF24 radio(CE_PIN, CSN_PIN); // Create a Radio
  // NOTE: the "LL" at the end of the constant is "LongLong" type
  //const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe
  const uint64_t pipe_lamp = 0xE8E8F0F0E1LL;
#endif


// initialize Hover
#if mgc_on
  #include "Hover.h"
  // Declarations for MGC3130
  int ts = 32;
  int reset = 33;
  // Enable or disable different modes. Only gestures and taps are enabled by default. 
  // Set the following four options to 0x01 if you want to capture every event. Note that the Serial console will be 'spammy'. 
  #define GESTUREMODE 0x01    //0x00 = disable gestures, 0x01 = enable gestures
  #define TOUCHMODE 0x00      //0x00 = disable touch, 0x01 = enable touch 
  #define TAPMODE 0x01        //0x00 = disable taps, 0x01 = enable taps, 0x02 = single taps only, 0x03 = double taps only
  #define POSITIONMODE 0x00   //0x00 = disable position tracking, 0x01 = enable position tracking
  Hover hover = Hover(ts, reset, GESTUREMODE, TOUCHMODE, TAPMODE, POSITIONMODE );    
#endif

// initialize Ble
#if ble_on
  #include <BLEDevice.h>
  #include <BLEServer.h>
  #include <BLEUtils.h>
  #include <BLE2902.h>
  BLEServer *pServer = NULL;
  BLECharacteristic * pTxCharacteristic;
  bool deviceConnected = false;
  bool oldDeviceConnected = false;
  uint8_t txValue = 0;
  
  // See the following for generating UUIDs:
  // https://www.uuidgenerator.net/
  
  #define SERVICE_UUID           "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID
  #define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
  #define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
  
  
  class MyServerCallbacks: public BLEServerCallbacks {
      void onConnect(BLEServer* pServer) {
        deviceConnected = true;
      };
  
      void onDisconnect(BLEServer* pServer) {
        deviceConnected = false;
      }
  };
  
  class MyCallbacks: public BLECharacteristicCallbacks {
      void onWrite(BLECharacteristic *pCharacteristic) {
        std::string rxValue = pCharacteristic->getValue();
  
        if (rxValue.length() > 0) {
          Serial.println("*********");
          Serial.print("Received Value: ");
          for (int i = 0; i < rxValue.length(); i++)
            Serial.print(rxValue[i]);
  
          Serial.println();
          Serial.println("*********");
        }
      }
  };
#endif

void setup()
{
  Serial.begin(2000000);  // start serial for output

  #if mgc_on
    delay(2000);
    Serial.println("Initializing Hover...please wait.");
    hover.begin(); 
  #else
    pinMode(32, INPUT);
    pinMode(33, INPUT);
    pinMode(22, INPUT);
    pinMode(23, INPUT);
  #endif
//  radio_ini();


  #if ble_on
    // Create the BLE Device
    BLEDevice::init("Lamp ble1");
  
    // Create the BLE Server
    pServer = BLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());
  
    // Create the BLE Service
    BLEService *pService = pServer->createService(SERVICE_UUID);
  
    // Create a BLE Characteristic
    pTxCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID_TX,
                      BLECharacteristic::PROPERTY_NOTIFY
                    );
                        
    pTxCharacteristic->addDescriptor(new BLE2902());
  
    BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(
                         CHARACTERISTIC_UUID_RX,
                        BLECharacteristic::PROPERTY_WRITE
                      );
  
    pRxCharacteristic->setCallbacks(new MyCallbacks());
  
    // Start the service
    pService->start();
  
    // Start advertising
    pServer->getAdvertising()->start();
    Serial.println("Waiting a client connection to notify...");
  #endif
  pixels.begin();
}

void loop()
{
  #if repair_led
    for(int i=0; i<36; i++) pixels.st_strip(i, 0, 0, 0, 255);
    pixels.refresh();
  #else
    mgc();
    ble();
    pixels.refresh_leds();
  #endif
//  pixels.demo_leds();
  //radio_read();
}


  
void mgc() {
  #if mgc_on
    unsigned long currentMillis = millis();    // used for updating 3D position coordinates. Set to update every 150ms by default. 
    // read incoming data stream from Hover
    hover.readI2CData();
  
    
    // retreive a gesture, touch, or position                
    Gesture g = hover.getGesture();
    Touch t = hover.getTouch();
    Position p = hover.getPosition();        
    
    // print Gesture data
    if ( g.gestureID != 0){
      Serial.print("Event: "); Serial.print(g.gestureType); Serial.print("\t");          
      Serial.print("Gesture ID: "); Serial.print(g.gestureID,HEX); Serial.print("\t");
      Serial.print("Value: "); Serial.print(g.gestureValue,DEC); Serial.println("");
      if (g.gestureID==2)
        {
          switch (mode) {
            case 0:
              pixels.mode1(g.gestureValue);
              break;
            case 1:
              pixels.mode2(g.gestureValue);
              break;
            case 2:
              break;
            case 3:
              break;
            case 4:
              break;
            case 5:
              break;
          } 
        }
      else if (g.gestureID==1)
        {
          select_mode(g.gestureValue);
        }
    }     
    // print Touch data 
    if ( t.touchID != 0){  
      Serial.print("Event: "); Serial.print(t.touchType); Serial.print("\t");      
      Serial.print("Touch ID: "); Serial.print(t.touchID,HEX); Serial.print("\t");
      Serial.print("Value: "); Serial.print(t.touchValue,HEX); Serial.println("");
    }
  #endif
}

void ble()
  {
    #if ble_on
//      if (deviceConnected) {
//          if (Serial.available())
//            {
//              txValue = Serial.read();;
//              pTxCharacteristic->setValue(&txValue, 1);
//              pTxCharacteristic->notify();
//            }
//  //        pTxCharacteristic->setValue(&txValue, 1);
//  //        pTxCharacteristic->notify();
//  //        txValue++;
//        delay(10); // bluetooth stack will go into congestion, if too many packets are sent
//      }
    
        // disconnecting
        if (!deviceConnected && oldDeviceConnected) {
            delay(500); // give the bluetooth stack the chance to get things ready
            pServer->startAdvertising(); // restart advertising
            Serial.println("start advertising");
            oldDeviceConnected = deviceConnected;
        }
        // connecting
        if (deviceConnected && !oldDeviceConnected) {
        // do stuff here on connecting
            oldDeviceConnected = deviceConnected;
        }
    #endif
  }
void radio_ini()
  {
    #if radio_on
      radioId = id;
      radio.begin();
//      printf_begin();
      radio.setDataRate( RF24_250KBPS );
      radio.setPALevel( RF24_PA_MAX );
      radio.setAutoAck(0);
      radio.setPayloadSize(8);
      radio.setChannel(channel); 
      radio.openReadingPipe(1, pipe_lamp);
      radio.startListening();
      radio.printDetails();
    #endif
  }


/*-----( Declare Variables )-----*/
uint8_t CMD[ 32 ]; // CMD + Ch + 6

void radio_read()
  {
    #if radio_on
      if ( radio.available() )
        {
          // Dump the payloads until we've gotten everything
          // bool done = false;
          // while (!done)
          while (radio.available())
          {
            // Fetch the payload, and see if this was the last one.
            //done = radio.read( CMD, sizeof(CMD) );
            radio.read( CMD, sizeof(CMD) );
  
            if (DEBUG)
            {
  
              Serial.println("Got command");
  
              for (int i = 0 ; i < 8 ; i++)
              {
                Serial.print(CMD[i]);
                Serial.print(",");
              }
              Serial.println();
            }
  
            if ( (CMD[0] == 'A') && ( CMD[1] == id ) ) // RGB command
            {
              if (DEBUG)
              {
                Serial.print("Color: ");
                Serial.print(CMD[2]);
                Serial.print(" ");
                Serial.print(CMD[3]);
                Serial.print(" ");
                Serial.print(CMD[4]);
                Serial.print(" ");
                Serial.println(CMD[5]);
              }
              Serial.println("Color");
  //            analogWrite(RED, CMD[2]);
  //            analogWrite(GREEN, CMD[3]);
  //            analogWrite(BLUE, CMD[4]);
  //            analogWrite(WHITE, CMD[5]);
            }
          }
        }
     #endif
  }

void select_mode(int value)
  {
    if (value==1) 
      {
        mode++;
        if (mode>5) mode=0;
      }
    else if (value==2)
      {
        mode--;
        if (mode<0) mode=5;
      }
    pixels.select(mode);
    pixels.refresh();
    //st_refresh = true;
  }