//Nano BLE - UWB Receiver Code

#include <ArduinoBLE.h>
#include <Servo.h>
#include <Adafruit_NeoPixel.h>

Servo myservo;  // create servo object to control a servo

//int pos0 = 0;    // variable to store the servo position
int pos = 0;
int servo = 3;


#define LED_PIN D3
#define LED_COUNT 16

Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

void setup() {
  Serial.begin(115200);

  myservo.attach(A2);  // attaches the servo on pin 9 to the servo object  

  strip.begin();
  strip.show();
  strip.setBrightness(100);

  // begin initialization
  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");

    while (1);
  }

  Serial.println("BLE Central - Receiver");
  Serial.println("Make sure to turn on the device.");

  // start scanning for peripheral
  BLE.scan();
  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);

  digitalWrite(LED_BUILTIN, LOW);         
  digitalWrite(LEDR, HIGH);     
  digitalWrite(LEDG, HIGH);     
  digitalWrite(LEDB, HIGH);       
}

void loop() {
  // check if a peripheral has been discovered
  BLEDevice peripheral = BLE.available();

  if (peripheral) {
    // discovered a peripheral, print out address, local name, and advertised service
    Serial.print("Found ");
    Serial.print(peripheral.address());
    Serial.print(" '");
    Serial.print(peripheral.localName());
    Serial.print("' ");
    Serial.print(peripheral.advertisedServiceUuid());
    Serial.println();

    setRingColor(false);
    delay(200);
    setRingColor(true);
    delay(200);
    // Check if the peripheral is a POSSensor, the local name will be:
    // "POSSensor"
    if (peripheral.localName() == "POSSensor2") {  
      // stop scanning
      BLE.stopScan();

      monitorBLEperipheral(peripheral);

      // peripheral disconnected, start scanning again
      BLE.scan();
    }
  }
}

void monitorBLEperipheral(BLEDevice peripheral) {
  // connect to the peripheral
  Serial.println("Connecting ...");
  if (peripheral.connect()) {
    Serial.println("Connected");
  } else {
    Serial.println("Failed to connect!");
    return;
  }

  // discover peripheral attributes
  Serial.println("Discovering service 0xffe0 ...");
  if (peripheral.discoverService("11a953ef-0809-4d89-9a56-25ab22841ttt")) {
    Serial.println("Service discovered");
  } else {
    Serial.println("Attribute discovery failed.");
    peripheral.disconnect();

    while (1);
    return;
  }

  // retrieve the POSSensorData characteristic
  BLECharacteristic POSSensorData2 = peripheral.characteristic("e624a94b-4a14-41f3-ae53-ec99c6025999");
  
  // subscribe to the simple key characteristic
  Serial.println("Subscribing to POSSensorData characteristic ...");
  if (!POSSensorData2) {
    Serial.println("no POSSensorData characteristic found!");
    peripheral.disconnect();
    return;
  } 
    else if (!POSSensorData2.canSubscribe()) {
    Serial.println("POSSensorData characteristic is not subscribable!");
    peripheral.disconnect();
    return;
  } 
    else if (!POSSensorData2.subscribe()) {
    Serial.println("subscription failed!");
    peripheral.disconnect();
    return;
  } 
    else {
    Serial.println("Subscribed to POSSensorData characteristic");
  }

  while (peripheral.connected()) {
    // while the peripheral is connected

    // check if the value of the simple key characteristic has been updated
    if (POSSensorData2.valueUpdated()) {

// THIS SECTION CONVERTS THE RECEIVED CHARACTERISTIC FROM UNSIGNED CHAR TO STRING // 
      String str;
      int length = POSSensorData2.valueLength();
      const uint8_t* val = POSSensorData2.value();
      str.reserve(length);

      for (int i = 0; i<length; i++){
        str += (char)val[i];
      }

      Serial.println(str);

      if (str.indexOf("POS") != -1) {
        int posIndex = str.indexOf("POS");
        int firstComma = str.indexOf(',', posIndex + 1);
        int secondComma = str.indexOf(',', firstComma + 1);
        int thirdComma = str.indexOf(',', secondComma + 1);

        float x = str.substring(firstComma + 1, secondComma).toFloat();
        float y = str.substring(secondComma + 1, thirdComma).toFloat();
        float z = str.substring(thirdComma + 1, str.indexOf(',', thirdComma + 1)).toFloat();        
        
        pos = 180 - x/2.85 * 180;
        myservo.write(pos);
        delay(100);

void setRingColor(bool off) {
  for (int i = 0; i < strip.numPixels(); i++) {
    if (pos/3 < pos< pos *2/3 ) {
           strip.setPixelColor(i, strip.Color(off ? 0 : 0, 0, 255));
        }
        else {
            strip.setPixelColor(i, strip.Color(off ? 0 : 0, 0, 255));
        }
      strip.show();  
  }
        

        // if (0 <= x && x <= 2.85 && 0 <= y && y <= 3.15) {
        //   digitalWrite(LED_BUILTIN, HIGH);
        //   digitalWrite(LEDR, HIGH);
        //   digitalWrite(LEDG, HIGH);
        //   digitalWrite(LEDB, HIGH);
        // }

        // if (0 <= x && x <= 2.85 && 3.15 <= y && y <= 6.3) {
        //   digitalWrite(LED_BUILTIN, LOW);
        //   digitalWrite(LEDR, LOW);
        //   digitalWrite(LEDG, HIGH);
        //   digitalWrite(LEDB, HIGH);
        // }

        // if (2.85 <= x && x <= 5.7 && 0 <= y && y <= 3.15) {
        //   digitalWrite(LED_BUILTIN, LOW);
        //   digitalWrite(LEDR, HIGH);
        //   digitalWrite(LEDG, LOW);
        //   digitalWrite(LEDB, HIGH);
        // }

        // if (2.85 <= x && x <= 5.7 && 3.15 <= y && y <= 6.3) {
        //   digitalWrite(LED_BUILTIN, LOW);
        //   digitalWrite(LEDR, HIGH);
        //   digitalWrite(LEDG, HIGH);
        //   digitalWrite(LEDB, LOW);
        // }
        
        Serial.print("x: ");
        Serial.print(x);
        Serial.print(", y: ");
        Serial.print(y);
        Serial.print(", z: ");
        Serial.println(z);
      } else {
        Serial.println("POS not found in the data string");
      }      
    }
  }
  

  Serial.println("BLE disconnected!");
}