#include <Adafruit_MPU6050.h>

Adafruit_MPU6050 mpu;
sensors_event_t event;

// PROPERTIES
float distance_walked, distance_driven;

float get_distance() {
  // Distance travelled - in practice, this would be read from sensors
  return 100;
}

// acceleration at which movement is considered driving
float driving_threshold = 10;
bool driving = false;
bool walking = false;
float start_distance, stop_time;
int max_stop_time = 5000;

void push_bluetooth(String event_name, float data) {
  // Helper function to mimic a Bluetooth service being pushed to the client
  Serial.print(event_name);
  Serial.print(": ");
  Serial.println(data);
}

bool zero_movement() {
  return (event.acceleration.x == 0 &&
          event.acceleration.y == 0 &&
          event.acceleration.z == 0);
}

float acceleration_mag() {
  return sqrt(pow(event.acceleration.x,2) +
              pow(event.acceleration.y,2) +
              pow(event.acceleration.z,2));
}

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

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  distance_walked = get_distance();
  distance_driven = get_distance();
}

void loop() {
  mpu.getAccelerometerSensor()->getEvent(&event);

  // check if driving
  if (driving || walking) {
    // if not stopped and movement is 0, begin stop-timer
    if (stop_time == 0) {
        if (zero_movement()) {
          stop_time = millis();
      }
    // otherwise, if stopped:
    } else { 
      // first check if movement is not 0
      if (!zero_movement()) {
          // if so, reset stop-timer
          stop_time = 0;
        }
      // then check if stop time exceeds threshold
      if (millis() - stop_time > max_stop_time) {
        // if so, reset stop-timer, publish event, and disable flag
        stop_time = 0;
        if (driving) {
          driving = false;
          stop_time = 0;
          distance_driven = get_distance() - start_distance;
          push_bluetooth("drive_complete", distance_driven);
        } else {
          walking = false;
          stop_time = 0;
          distance_walked = get_distance() - start_distance;
          push_bluetooth("walk_complete", distance_walked);
        }
      }
    }
  }

  // if not driving or walking, check for any movement
  if (!driving && !walking) {
    if (!zero_movement()) {
        if (acceleration_mag() > driving_threshold) {
          driving = true;
          start_distance = get_distance();
          stop_time = 0;
        } else {
          walking = true;
          start_distance = get_distance();
          stop_time = 0;
        }
    }
  }

  Serial.print("Driving: ");
  Serial.print(driving);
  Serial.print(", Walking: ");
  Serial.print(walking);
  Serial.print(", Accel. Mag.:");
  Serial.print(acceleration_mag());
  Serial.print("m/s^2, Stop Time: ");
  Serial.print(stop_time == 0 ? stop_time : millis() - stop_time);
  Serial.println("ms");
  delay(500);
}