#include <bluefruit.h>
#include <Wire.h>

const uint8_t LED_SERVICE_UUID[] =
{
    0x6C, 0xCE, 0x98, 0x91, 0xB9, 0xB3, 0x11, 0x87,
    0x9E, 0x47, 0xF5, 0x67, 0x01, 0x00, 0x4B, 0xE5
};

const uint8_t LED_CHR_UUID[] =
{
    0x6C, 0xCE, 0x98, 0x91, 0xB9, 0xB3, 0x11, 0x87,
    0x9E, 0x47, 0xF5, 0x67, 0x02, 0x00, 0x4B, 0xE5
};

BLEService        lbs(LED_SERVICE_UUID);
BLECharacteristic lsbLED(LED_CHR_UUID);
BLEConnection* connection;

void ble_start_advertisement(void);

void ble_start_advertisement(void)
{
    // Advertising packet
    Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
    Bluefruit.Advertising.addTxPower();
    Bluefruit.Advertising.addName();

    /* Start Advertising
    * - Enable auto advertising if disconnected
    * - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
    * - Timeout for fast mode is 30 seconds
    * - Start(timeout) with timeout = 0 will advertise forever (until connected)
    *
    * For recommended advertising interval
    * https://developer.apple.com/library/content/qa/qa1931/_index.html
    */
    Bluefruit.Advertising.restartOnDisconnect(true);
    Bluefruit.Advertising.setInterval(32, 244); // in unit of 0.625 ms
    Bluefruit.Advertising.setFastTimeout(30);   // number of seconds in fast mode
    Bluefruit.Advertising.start(0);             // 0 = Don't stop advertising after n seconds
}

void setup()
{
  Serial.begin(9600);
  while ( !Serial ) delay(10);

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(LED_CONN, OUTPUT);
  digitalWrite(LED_BUILTIN, 0); // led off
  digitalWrite(LED_CONN, 0); // led off

  Serial.println("Bluetooth connection reciever");

  uint8_t mac[6] = {0};
  char deviceName[16] = {0};  

  Serial.println("-------------------------------------");
  Bluefruit.begin(4,0);
  Bluefruit.getAddr(mac);
  sprintf(deviceName, "RAK4630-%2X%2X%2X", mac[0], mac[1], mac[2]);
  Bluefruit.setName(deviceName);
  Serial.println("LED TEST");
  digitalWrite(LED_CONN, 1);

  // Configure callbacks
  Bluefruit.Periph.setConnectCallback(connect_callback);
  Bluefruit.Periph.setDisconnectCallback(disconnect_callback);

  lbs.begin();
  lsbLED.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
  lsbLED.setPermission(SECMODE_OPEN, SECMODE_OPEN);
  lsbLED.setFixedLen(1);
  lsbLED.begin();
  lsbLED.write8(0x00); // led = off
  lsbLED.setWriteCallback(led_write_callback);

  ble_start_advertisement();

  Serial.println("Started Advertising");

}


void disconnect_callback(uint16_t conn_handle, uint8_t reason)
{
  (void) conn_handle;
  (void) reason;

  Serial.println();
  Serial.print("Disconnected, reason = 0x");
  Serial.println(reason, HEX);
  lsbLED.write8(0x0);

}

// callback invoked when central connects
void connect_callback(uint16_t conn_handle)
{
  // Get the reference to current connection
  connection = Bluefruit.Connection(conn_handle);

  char central_name[32] = { 0 };
  connection->getPeerName(central_name, sizeof(central_name));

  Serial.print("Connected to ");
  Serial.println(central_name);
  Bluefruit.Advertising.start(0);
}

void led_write_callback(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len)
{
  (void) conn_hdl;
  (void) chr;
  (void) len; // len should be 1

  // data = 1 -> LED = On
  // data = 0 -> LED = Off
  digitalWrite(LED_BUILTIN,data[0]&1);
  Serial.print("Data: ");
  Serial.println(data[0],HEX);
  lsbLED.write8(0x01);
}

void loop()
{

}