/*
  BLE_Central_Device.ino
  https://docs.arduino.cc/tutorials/nano-33-ble-sense/ble-device-to-device

  This program uses the ArduinoBLE library to set-up an Arduino Nano 33 BLE Sense
  as a central device and looks for a specified service and characteristic in a
  peripheral device. If the specified service and characteristic is found in a
  peripheral device, the last detected value of the on-board gesture sensor of
  the Nano 33 BLE Sense, the APDS9960, is written in the specified characteristic.

  The circuit:
  - Arduino Nano 33 BLE Sense.

  This example code is in the public domain.
*/

#include <ArduinoBLE.h>
#include <Arduino_APDS9960.h>

// Unique UUID code for service
const char *deviceServiceUuid = "19b10000-e8f2-537e-4f6c-d104768a1214";
// Unique UUID code for characteristic
const char *deviceServiceCharacteristicUuid = "19b10001-e8f2-537e-4f6c-d104768a1214";

int gesture = -1;
int oldGestureValue = -1;

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

  // Initialize the APDS9960 sensor for gesture recognition
  if (!APDS.begin())
  {
    Serial.println("* Error initializing APDS9960 sensor!");
  }

  APDS.setGestureSensitivity(80);

  // Initialize BLE
  if (!BLE.begin())
  {
    Serial.println("* Starting Bluetooth® Low Energy module failed!");
    while (1)
      ;
  }

  /*
  // Set the name of the central device,
  // and start advertising to make itself
  // discoverable to other peripheral devices
  */
  BLE.setLocalName("Nano 33 BLE (Central)");
  BLE.advertise();

  Serial.println("Arduino Nano 33 BLE Sense (Central Device)");
  Serial.println(" ");
}

void loop()
{
  connectToPeripheral();
}

void connectToPeripheral()
{
  BLEDevice peripheral;

  Serial.println("- Discovering peripheral device...");

  // Scan for a peripheral with a specific service UUID
  // find available device and store in 'peripheral'
  do
  {
    BLE.scanForUuid(deviceServiceUuid);
    peripheral = BLE.available();
  } while (!peripheral);

  // Peripheral Info
  if (peripheral)
  {
    Serial.println("* Peripheral device found!");
    Serial.print("* Device MAC address: ");
    Serial.println(peripheral.address());
    Serial.print("* Device name: ");
    Serial.println(peripheral.localName());
    Serial.print("* Advertised service UUID: ");
    // This should be same as central service uuid
    Serial.println(peripheral.advertisedServiceUuid());
    Serial.println(" ");
    BLE.stopScan(); // stop scan when found

    // Control the peripheral once found
    controlPeripheral(peripheral);
  }
}

void controlPeripheral(BLEDevice peripheral)
{
  // Check if peripheral is connected
  Serial.println("- Connecting to peripheral device...");
  if (peripheral.connect())
  {
    Serial.println("* Connected to peripheral device!");
    Serial.println(" ");
  }
  else
  {
    Serial.println("* Connection to peripheral device failed!");
    Serial.println(" ");
    return;
  }

  // Check for peripheral's services and characteristics.
  Serial.println("- Discovering peripheral device attributes...");
  if (peripheral.discoverAttributes())
  {
    Serial.println("* Peripheral device attributes discovered!");
    Serial.println(" ");
  }
  else
  {
    Serial.println("* Peripheral device attributes discovery failed!");
    Serial.println(" ");
    peripheral.disconnect();
    return;
  }

  // Store peripheral's characteristic container to central's characteristic
  BLECharacteristic gestureCharacteristic = peripheral.characteristic(deviceServiceCharacteristicUuid);

  // Check if peripheral device initialized/writable characteristic
  if (!gestureCharacteristic)
  {
    Serial.println("* Peripheral device does not have gesture_type characteristic!");
    peripheral.disconnect();
    return;
  }
  else if (!gestureCharacteristic.canWrite())
  {
    Serial.println("* Peripheral does not have a writable gesture_type characteristic!");
    peripheral.disconnect();
    return;
  }

  // Write value to gesture_type characteristic
  while (peripheral.connected())
  {
    gesture = gestureDetectection();
    // Check if gesture changed from before
    if (oldGestureValue != gesture)
    {
      oldGestureValue = gesture;
      Serial.print("* Writing value to gesture_type characteristic: ");
      Serial.println(gesture);
      gestureCharacteristic.writeValue((byte)gesture);
      Serial.println("* Writing value to gesture_type characteristic done!");
      Serial.println(" ");
    }
  }
  Serial.println("- Peripheral device disconnected!");
}

// Gesture detection function
int gestureDetectection()
{
  if (APDS.gestureAvailable())
  {
    gesture = APDS.readGesture();

    switch (gesture)
    {
    case GESTURE_UP:
      Serial.println("- UP gesture detected");
      break;
    case GESTURE_DOWN:
      Serial.println("- DOWN gesture detected");
      break;
    case GESTURE_LEFT:
      Serial.println("- LEFT gesture detected");
      break;
    case GESTURE_RIGHT:
      Serial.println("- RIGHT gesture detected");
      break;
    default:
      Serial.println("- No gesture detected");
      break;
    }
  }
  return gesture;
}

/*
  BLE_Peripheral.ino

  This program uses the ArduinoBLE library to set-up an Arduino Nano 33 BLE
  as a peripheral device and specifies a service and a characteristic. Depending
  of the value of the specified characteristic, an on-board LED gets on.

  The circuit:
  - Arduino Nano 33 BLE.

  This example code is in the public domain.
*/

#include <ArduinoBLE.h>

enum
{
  GESTURE_NONE = -1,
  GESTURE_UP = 0,
  GESTURE_DOWN = 1,
  GESTURE_LEFT = 2,
  GESTURE_RIGHT = 3
};

// Unique UUID code for service
const char *deviceServiceUuid = "19b10000-e8f2-537e-4f6c-d104768a1214";
// Unique UUID code for characteristic
const char *deviceServiceCharacteristicUuid = "19b10001-e8f2-537e-4f6c-d104768a1214";

int gesture = -1;

// Define a BLE service and characteristic !! (done in peripheral)
BLEService gestureService(deviceServiceUuid);
BLEByteCharacteristic gestureCharacteristic(deviceServiceCharacteristicUuid, BLERead | BLEWrite);

void setup()
{
  Serial.begin(9600);
  while (!Serial)
    ;
  // Initialize LEDs and set their initial states
  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LEDR, HIGH);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, HIGH);
  digitalWrite(LED_BUILTIN, LOW);

  // Initialize the BLE module
  if (!BLE.begin())
  {
    Serial.println("- Starting Bluetooth® Low Energy module failed!");
    while (1)
      ;
  }

  /*
  // Set the name of the peripheral device,
  // and define the services and characteristics
  */
  BLE.setLocalName("Arduino Nano 33 BLE (Peripheral)");
  // the service that the peripheral will advertise
  // used to specify which service you want to actively advertise to other devices.
  BLE.setAdvertisedService(gestureService);
  // adds a characteristic to 'gestureService'
  gestureService.addCharacteristic(gestureCharacteristic);
  // adds 'gestureService' to the list of services provided by the peripheral, necessary to expose the service to central devices
  // registers the service with the BLE stack, making it available for central devices to discover when they connect to the peripheral.
  BLE.addService(gestureService);
  // initial value of 'gestureCharacteristic' set to -1
  gestureCharacteristic.writeValue(-1);
  // This function call starts the advertising process
  BLE.advertise();

  Serial.println("Nano 33 BLE (Peripheral Device)");
  Serial.println(" ");
}

void loop()
{
  // Checks for a central device's connection
  BLEDevice central = BLE.central();
  Serial.println("- Discovering central device...");
  delay(500);

  if (central)
  {
    Serial.println("* Connected to central device!");
    Serial.print("* Device MAC address: ");
    Serial.println(central.address());
    Serial.println(" ");

    while (central.connected())
    {
      // Check if characteristic was written to by central
      if (gestureCharacteristic.written())
      {
        // Update the LEDs based on the received gesture value
        gesture = gestureCharacteristic.value();
        writeGesture(gesture);
      }
    }

    Serial.println("* Disconnected to central device!");
  }
}

// Handle changes in the characteristic's value and update LEDs
void writeGesture(int gesture)
{
  Serial.println("- Characteristic <gesture_type> has changed!");

  switch (gesture)
  {
  case GESTURE_UP:
    Serial.println("* Actual value: UP (red LED on)");
    Serial.println(" ");
    digitalWrite(LEDR, LOW);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, HIGH);
    digitalWrite(LED_BUILTIN, LOW);
    break;
  case GESTURE_DOWN:
    Serial.println("* Actual value: DOWN (green LED on)");
    Serial.println(" ");
    digitalWrite(LEDR, HIGH);
    digitalWrite(LEDG, LOW);
    digitalWrite(LEDB, HIGH);
    digitalWrite(LED_BUILTIN, LOW);
    break;
  case GESTURE_LEFT:
    Serial.println("* Actual value: LEFT (blue LED on)");
    Serial.println(" ");
    digitalWrite(LEDR, HIGH);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, LOW);
    digitalWrite(LED_BUILTIN, LOW);
    break;
  case GESTURE_RIGHT:
    Serial.println("* Actual value: RIGHT (built-in LED on)");
    Serial.println(" ");
    digitalWrite(LEDR, HIGH);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, HIGH);
    digitalWrite(LED_BUILTIN, HIGH);
    break;
  default:
    digitalWrite(LEDR, HIGH);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, HIGH);
    digitalWrite(LED_BUILTIN, LOW);
    break;
  }
}