#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

#define TRIGGER_PIN  12  
#define ECHO_PIN     11  
#define MAX_DISTANCE 400 
#define ALERT_DISTANCE 100 

Servo myservo;
const int servoPin = 3;
const int BUZZER = A0;
MPU6050 mpu;

void setup() {
  Serial.begin(9600);
  // Setup ultrasonic sensor
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  // Setup buzzer
  pinMode(BUZZER, OUTPUT);
  // Setup servomotor
  myservo.attach(servoPin);
  // Setup accelerometer and gyroscope
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1); // Loop forever if connection failed
  } else {
    Serial.println("MPU6050 connection successful");
  }
}

// Find device functionality
void findDevice(){
 if (Serial.available() > 0) {
    String input = Serial.readString();
    input.trim();
    Serial.println(input);

    if (input.equals("find device")) {
      Serial.println("buzzer is on...");
      tone(BUZZER, 1000); 
    }
    else if (input.equals("stop find")) {
      Serial.println("buzzer is off...");
      noTone(BUZZER);
    }
  }
}

void activateObstacleHapticSignal(Servo servo){
  for (int angle = 90; angle <= 120; angle += 5) {
    servo.write(angle);              
    delay(20);                         
  }
  for (int angle = 120; angle >= 60; angle -= 5) {
    servo.write(angle);              
    delay(20);                         
  }
}

void detectObstacle(){
  digitalWrite(TRIGGER_PIN, LOW);  // Clear the trigger
  delayMicroseconds(2);            // Settle time
  digitalWrite(TRIGGER_PIN, HIGH); // Set the trigger pin to high state for 10 microseconds
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);  // Bring back the trigger pin to low

  long duration = pulseIn(ECHO_PIN, HIGH); // Read the echo pin, returns the sound wave travel time in microseconds
  int distance = duration * 0.034 / 2;     // Calculating the distance

  Serial.print("Distance: ");
  Serial.print(distance); // Print the distance on the Serial Monitor
  Serial.println(" cm");

  if (distance <= ALERT_DISTANCE && distance > 0) { 
    activateObstacleHapticSignal(myservo);
  }

  delay(100); // Delay a bit to stabilize the sensor
}

void getUserCoordinates(){
  int16_t ax, ay, az;
  int16_t gx, gy, gz;

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  Serial.print("Accel X: ");
  Serial.print(ax);
  Serial.print(" / Y: ");
  Serial.print(ay);
  Serial.print(" / Z: ");
  Serial.println(az);

  Serial.print("Gyro X: ");
  Serial.print(gx);
  Serial.print(" / Y: ");
  Serial.print(gy);
  Serial.print(" / Z: ");
  Serial.println(gz);

  delay(1000); // Delay a second before next loop
}

void loop() {
  findDevice();
  detectObstacle();
  getUserCoordinates();
}



#include <Arduino_LSM9DS1.h>
#include <Wire.h>
#include <Adafruit_DRV2605.h>

#define DRV2605_ADDR 0x5A // I2C address of the motor controller

Adafruit_DRV2605 drv; // Create an instance of the motor controller object

void setup() {
   Wire.begin(); // Initialise I2C communication
  Serial.begin(9600);
  drv.begin(DRV2605_ADDR); // Initialise the motor controller
  while (!Serial);

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }
  
  Serial.println("IMU initialized!");
}

void readUserCoordinates(){
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;

  if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable() && IMU.magneticFieldAvailable()) {
    IMU.readAcceleration(ax, ay, az);
    IMU.readGyroscope(gx, gy, gz);
    IMU.readMagneticField(mx, my, mz);

    // send the real time coordinates to the Google Maps API
    sendCoordinates();

    Serial.print("Accelerometer (m/s^2): ");
    Serial.print(ax); Serial.print(", ");
    Serial.print(ay); Serial.print(", ");
    Serial.println(az);

    Serial.print("Gyroscope (deg/s): ");
    Serial.print(gx); Serial.print(", ");
    Serial.print(gy); Serial.print(", ");
    Serial.println(gz);

    Serial.print("Magnetometer (uT): ");
    Serial.print(mx); Serial.print(", ");
    Serial.print(my); Serial.print(", ");
    Serial.println(mz);

    Serial.println();
  }

  delay(100); // Adjust delay as needed
}

void setVibrationPattern(uint8_t pattern) {
  drv.setWaveform(pattern); 
}

void vibrateNTimes(uint8_t numTimes) {
  // Set the motor controller to the intensity selected by the user and that is being fetched from the database
  setVibrationPattern(fetchedPattern); 
  for (uint8_t i = 0; i < numTimes; i++) {
    delay(100); // Vibration duration
  }
}

void loop() {
  // Given that the glasses would be communicating with the Google Maps API, 
  // when they get signaled that the user has reached the final destination, then send three haptic signals to the user
  while(communicatingWithMapsAPI){
    if(destinationReached){
      vibrateNTimes(3);
    }
    else if (reachedTurn){
      vibrateNTimes(2);
    }
  }
}

#include <Wire.h>
#include <VL53L1X.h>
#include <Wire.h>
#include <Adafruit_DRV2605.h>

#define DRV2605_ADDR 0x5A // I2C address of the motor controller
#define TRIGGER_PIN  12  
#define ECHO_PIN     11  

Adafruit_DRV2605 drv; // Create an instance of the motor controller object

VL53L1X sensor;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  drv.begin(DRV2605_ADDR);
  // Set ultrasonic sensor up
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT); 
  // Set ToF sensor up 
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1);
  }

  sensor.setDistanceMode(VL53L1X::Long);
  sensor.setMeasurementTimingBudget(50000);  // Set timing budget to 50 ms

  Serial.println("Sensor initialized!");
}

void setVibrationPattern(uint8_t pattern) {
  drv.setWaveform(pattern); 
}

void vibrateNTimes(uint8_t numTimes) {
  // Set the motor controller to the intensity selected by the user and that is being fetched from the database
  setVibrationPattern(fetchedPattern); 
  for (uint8_t i = 0; i < numTimes; i++) {
    delay(100); // Vibration duration
  }
}

void readDataFromToF(){
  sensor.startContinuous(50);  // Start continuous measurement with 50 ms delay between readings

  // Read distance data
  while (!sensor.dataReady()) {
    delay(5);
  }
  uint16_t distance = sensor.read();

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" mm");

  delay(1000);  // Wait for 1 second before taking the next reading
}

void readDataFromUltrasonic(){
  digitalWrite(TRIGGER_PIN, LOW);  // Clear the trigger
  delayMicroseconds(2);            // Settle time
  digitalWrite(TRIGGER_PIN, HIGH); // Set the trigger pin to high state for 10 microseconds
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);  // Bring back the trigger pin to low

  long duration = pulseIn(ECHO_PIN, HIGH); // Read the echo pin, returns the sound wave travel time in microseconds
  int distance = duration * 0.034 / 2;     // Calculating the distance

  Serial.print("Distance: ");
  Serial.print(distance); // Print the distance on the Serial Monitor
  Serial.println(" cm");

  delay(100); // Delay a bit to stabilize the sensor
}

void loop() {
  //Combines information from the ultrasonic and ToF sensors to detect obstacles
  // Uses the vibrateNTimes function to then send the corresponding haptic signals
  detectObstacle(); 
}