/* RANDOM GENERATED UUID
 * 
 * UUID generator:  https://www.uuidgenerator.net/
 * 
 * Binary to hex:   http://www.binaryhexconverter.com/binary-to-hex-converter
 * 
 * Hex to decimal:  http://www.binaryhexconverter.com/hex-to-decimal-converter
 * 
 * 
  e3ef0090-6b09-4f8a-a5c9-c0a7e9ddad83 ---> pack(IR,BATTERY,TILT,UP/DOWN,LEFT/RIGHT,q1,q2,q3,q4,q5,q6,q7,q8)    (notify)    13 bytes    

  e3ef0090-6b09-4f8a-a5c9-c0a7e9ddad86 ---> SENSOR PHYSICAL POSITION                                            (write)     1 bytes
  e3ef0090-6b09-4f8a-a5c9-c0a7e9ddad88 ---> SENSOR STATE                                                        (write)     1 bytes

  Complete Factory RESET:

  SF,2
  
  Create Private Service:

  Write                   08   Write value of characteristic with acknowledgment from client to server.
  Read                    02   Read value of characteristic. Value is sent from server to client.
  Notify                  10   Notify changes in value of characteristic.

EX:

  PZ                                    Clear all private service and characteristics settings
  PS,E3EF00906B094F8AA5C9C0A7E9DDAD83
  PC,E3EF00906B094F8AA5C9C0A7E9DDAD83,10,0D // Notify, 13 bytes

  OR

  PC,E3EF00906B094F8AA5C9C0A7E9DDAD83,08,02 // Write, 2 bytes
  
  R,1   // reboot
  LS    // list services

 *
 */

void initBTLERN4020(){

  delay(1000);

  digitalWrite(CMD, HIGH);

  if(!sendRN4020("GB", "3")){
    sendRN4020("SF,2", "AOK");    // RESET BTLE RN4020
    sendRN4020("SB,3", "AOK");    // Set the baud rate to 38400
    sendRN4020("R,1", "Reboot");  // Reboot BTLE RN4020

    Serial.println();
    Serial.println("RESET BTLE RN4020");
    Serial.println();
    
    delay(1000);
  }
  
  if(!sendRN4020("GN","hybridPLAY")){

    // Device settings
    sendRN4020("SS,C0000001","AOK");        // Activate Private Services
    sendRN4020("SR,24004000","AOK");        // Set device as peripheral, no direct advertisement and enable Apple Bueetooth
    sendRN4020("ST,0064,0002,0064","AOK");  // Set the interval to 125 ms, latency to 2, and time-out to 1 second
    sendRN4020("SN,hybridPLAY","AOK");      // Set the name of the device to "hybridPLAY"
    sendRN4020("SP,4","AOK");               // Set TX power value to -2.5
    sendRN4020("SB,3", "AOK");              // Set the baud rate to 38400

    // Private Characteristics
    sendRN4020("PZ", "AOK");                                          // Clear all private service and characteristics settings
    sendRN4020("PS,E3EF00906B094F8AA5C9C0A7E9DDAD83", "AOK");         // Create Private Service
    sendRN4020("PC,E3EF00906B094F8AA5C9C0A7E9DDAD83,10,0D", "AOK");   // Add Notify Characteristic, 13 bytes     (Sensor DATA)
    sendRN4020("PC,E3EF00906B094F8AA5C9C0A7E9DDAD86,08,01", "AOK");   // Add Write Characteristic,  1 byte       (Sensor POSITION)
    sendRN4020("PC,E3EF00906B094F8AA5C9C0A7E9DDAD88,08,01", "AOK");   // Add Write Characteristic,  1 byte       (Sensor STATE)
    
    sendRN4020("R,1", "Reboot");            // Reboot BTLE RN4020

    Serial.println();
    Serial.println("HYBRIDPLAY SENSOR FIRST INITIALIZE!");
    Serial.println();
    
    delay(1000);
  }

  if(debugMode){

    Serial.println(getSensorPosition());
    Serial.println(getSensorState());
    
  }
  
  digitalWrite(CMD, LOW);
  
}

void updateBTServices(){

  // PACK SENSOR DATA
  if(prevValIR != valIR || prevValBattery != valBattery || prevMoveDetected != moveDetected || prevteapotPacket[0] != teapotPacket[0] || prevteapotPacket[1] != teapotPacket[1] || prevteapotPacket[2] != teapotPacket[2] || prevteapotPacket[3] != teapotPacket[3] || prevteapotPacket[4] != teapotPacket[4] || prevteapotPacket[5] != teapotPacket[5] || prevteapotPacket[6] != teapotPacket[6] || prevteapotPacket[7] != teapotPacket[7]){

    digitalWrite(CMD, HIGH);
    
    Serial1.print("SUW,E3EF00906B094F8AA5C9C0A7E9DDAD83,");

    // IR             (00,64)HEX ---> (0,100)DEC
    Serial1.print(intToHexString(valIR));

    // BATTERY        (00,64)HEX ---> (0,100)DEC
    Serial1.print(intToHexString(valBattery));

    // TILT           (00,01)HEX ---> (0,1)DEC
    Serial1.print(intToHexString((int)moveDetected));

    // UP/DOWN        (00,01,02)HEX ---> (0,1,2)DEC ---> 0 UP - 1 OFF - 2 DOWN
    Serial1.print("01");

    // LEFT/RIGHT     (00,01,02)HEX ---> (0,1,2)DEC ---> 0 LEFT - 1 OFF - 2 RIGHT
    Serial1.print("01");

    // QUATERNIONS for Position in Space
    
    // Q1             (00,FF)HEX ---> (0,255)DEC
    Serial1.print(intToHexString(teapotPacket[0]));
    // Q2             (00,FF)HEX ---> (0,255)DEC
    Serial1.print(intToHexString(teapotPacket[1]));
    // Q3             (00,FF)HEX ---> (0,255)DEC
    Serial1.print(intToHexString(teapotPacket[2]));
    // Q4             (00,FF)HEX ---> (0,255)DEC
    Serial1.print(intToHexString(teapotPacket[3]));
    // Q5             (00,FF)HEX ---> (0,255)DEC
    Serial1.print(intToHexString(teapotPacket[4]));
    // Q6             (00,FF)HEX ---> (0,255)DEC
    Serial1.print(intToHexString(teapotPacket[5]));
    // Q7             (00,FF)HEX ---> (0,255)DEC
    Serial1.print(intToHexString(teapotPacket[6]));
    // Q8             (00,FF)HEX ---> (0,255)DEC
    Serial1.print(intToHexString(teapotPacket[7]));

    delay(2);
    Serial1.flush();
    
    Serial1.println();

    digitalWrite(CMD, LOW);

    // reset flags
    prevValIR = valIR;
    prevValBattery = valBattery;
    prevMoveDetected = moveDetected;
    prevteapotPacket[0] = teapotPacket[0];
    prevteapotPacket[1] = teapotPacket[1];
    prevteapotPacket[2] = teapotPacket[2];
    prevteapotPacket[3] = teapotPacket[3];
    prevteapotPacket[4] = teapotPacket[4];
    prevteapotPacket[5] = teapotPacket[5];
    prevteapotPacket[6] = teapotPacket[6];
    prevteapotPacket[7] = teapotPacket[7];
  }

  // Battery
  if((millis()-timerBattery) >= 60000){ // read battery level every minute
    timerBattery = millis();
    readBattery();
  }
  
}