//Official Client
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEClient.h>
#include <BLEServer.h>
#include <Arduino.h>

//Pin numbers
//motors pin numbers
int AIN2 = D10;
int AIN1 = D9;
int BIN1 = D8;
int BIN2 = D7;

int LED = D6;

BLEClient*  pClient; //Manages the client connection
bool doconnect = false; //Create a boolean variable for when it is/isn't connected

//BLE Server name (name of the other Xiao)
#define bleServerName "Danisita_BBQ"

//Address of the peripheral device
static BLEAddress *pServerAddress;

BLEUUID serviceUUID("181A"); //Service UUID (same as the other Xiao)
BLEUUID charUUID("2A59");    //Characteristic UUID (same as the other Xiao)

uint8_t Joy; //Char variable for the received messages

//Callback function in the case of an advertising
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) {//Method for advertised device
    if (advertisedDevice.getName() == bleServerName) { //Compare advertiser name
      advertisedDevice.getScan()->stop(); //Stop scan, we found what we are looking for
      pServerAddress = new BLEAddress(advertisedDevice.getAddress()); //Address of advertised device
      Serial.println("Device found. Connecting!");
    }
  }
};


void setup() {
  //pin mode for the motors information
  pinMode(AIN2, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);

  pinMode(LED, OUTPUT);

  Serial.begin(115200); //Serial communication to observe prints in serial monitor

  Serial.println("Starting BLE client...");

  BLEDevice::init("Danisita_BBQ_Client"); //Create the BLE device with its name

  BLEScan* pBLEScan = BLEDevice::getScan(); //Method to scan
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks()); //Call function for callbacks
  pBLEScan->setActiveScan(true); //Establish an active scanning (repeat it until connected)
  pBLEScan->start(30); //Begin scanning during 30 seconds

  pClient = BLEDevice::createClient(); //Create a client
  
  pClient->connect(*pServerAddress); //Method to connect to the server with the retrieved address (line 28)
  Serial.println(" - Connected to server");

  //Get the reference of the searched service
  BLERemoteService* pRemoteService = pClient->getService(serviceUUID); 
  if (pRemoteService == nullptr) { //Case in which the service UUID isn't found
    Serial.print("Failed to find our service UUID: ");
    Serial.println(serviceUUID.toString().c_str());
    return;
  }
    //Get the reference of the searched characteristic
  BLERemoteCharacteristic* pCharacteristic = pRemoteService->getCharacteristic(charUUID); 
  if (pCharacteristic == nullptr) { //Case in which the cahracteristic isn't found
    Serial.print("Failed to find our characteristic UUID");
    return;
  }

  Serial.println(" - Found characteristics");
  
  //Notification for the first time:
  pCharacteristic->registerForNotify([](BLERemoteCharacteristic* pBLERemoteCharacteristic, 
  uint8_t* pData, size_t length, bool isNotify) {
    Serial.println("Notify received");
    Serial.print("Value: ");
    Serial.println(*pData); //pData is the message received
  });

  doconnect = true; //Flag to indicate the connection
  
}

void loop() {
  if (doconnect) {
    digitalWrite(LED,HIGH);
    //Look for the service UUID and returns if the remote service exists
    BLERemoteService* pRemoteService = pClient->getService(serviceUUID); 
    //Look for the service UUID and returns if the remote characteristic exists
    BLERemoteCharacteristic* pCharacteristic = pRemoteService->getCharacteristic(charUUID);
     //Registers the notifications
    pCharacteristic->registerForNotify([](BLERemoteCharacteristic* pBLERemoteCharacteristic, 
    uint8_t* pData, size_t length, bool isNotify){
      Serial.println("Notify received");
      Serial.print("Value: ");
      Joy = *pData; //Save the message received in a variable (that's easier for me to visualize it)
      Serial.println(Joy); //Print it in the serial monitor
      
      if(Joy == 0){ //STOP
        digitalWrite(LED,LOW);

        digitalWrite(AIN2, LOW);
        digitalWrite(AIN1, LOW);
        digitalWrite(BIN1, LOW);
        digitalWrite(BIN2, LOW);
      }
      else if(Joy == 1){ //UP
        digitalWrite(LED,HIGH);

        digitalWrite(AIN2, HIGH);
        digitalWrite(AIN1, LOW);
        digitalWrite(BIN1, HIGH);
        digitalWrite(BIN2, LOW);
      }
      else if(Joy == 2){ //RIGHT
        digitalWrite(LED,HIGH);

        digitalWrite(AIN2, HIGH);
        digitalWrite(AIN1, LOW);
        digitalWrite(BIN1, LOW);
        digitalWrite(BIN2, HIGH);
      }
      else if(Joy == 3){ //DOWN
        digitalWrite(LED,HIGH);

        digitalWrite(AIN2, LOW);
        digitalWrite(AIN1, HIGH);
        digitalWrite(BIN1, LOW);
        digitalWrite(BIN2, HIGH);
      }
      else if(Joy == 4){ //LEFT
        digitalWrite(LED,HIGH);

        digitalWrite(AIN2, LOW);
        digitalWrite(AIN1, HIGH);
        digitalWrite(BIN1, HIGH);
        digitalWrite(BIN2, LOW);
      }
      else{ //STOP (just in case)
        digitalWrite(LED,LOW);
        
        digitalWrite(AIN2, LOW);
        digitalWrite(AIN1, LOW);
        digitalWrite(BIN1, LOW);
        digitalWrite(BIN2, LOW);
      }
      });
  }
  
  delay(1000);
  }