#include <Wire.h>

// =============================
// PINS
// =============================

#define TRIG_PIN A0
#define ECHO_PIN A1
#define BUZZER_PIN A2

#define MPU_ADDR 0x68


// =============================
// FALL THRESHOLDS
// =============================

// MPU6050 ±2g:
// 16384 counts = 1g

#define FREEFALL_SQ 67108864UL
// (0.5g)^2 * 16384^2

#define IMPACT_SQ 1677721600UL
// (2.5g)^2 * 16384^2

#define INACTIVITY_MIN_SQ 150994944UL
// (0.75g)^2 * 16384^2

#define INACTIVITY_MAX_SQ 419430400UL
// (1.25g)^2 * 16384^2


// =============================
// FALL STATE
// =============================

bool possibleFall = false;
bool impactDetected = false;

unsigned long freefallTime = 0;
unsigned long impactTime = 0;


// =============================
// MPU READ
// =============================

bool readAcceleration(int16_t &ax,
                      int16_t &ay,
                      int16_t &az) {

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission();

  Wire.requestFrom(MPU_ADDR, 6);

  if (Wire.available() < 6) {
    return false;
  }

  ax = (Wire.read() << 8) | Wire.read();
  ay = (Wire.read() << 8) | Wire.read();
  az = (Wire.read() << 8) | Wire.read();

  return true;
}


// =============================
// ULTRASONIC
// =============================

long getDistance() {

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);

  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);

  if (duration == 0) {
    return -1;
  }

  return duration / 58;
}


// =============================
// SETUP
// =============================

void setup() {

  Serial.begin(115200);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(BUZZER_PIN, OUTPUT);

  noTone(BUZZER_PIN);

  Wire.begin();

  delay(500);

  Serial.println("Walking Stick Ready");

  // Check MPU
  Wire.beginTransmission(MPU_ADDR);

  if (Wire.endTransmission() == 0) {
    Serial.println("MPU6050 OK");
  } else {
    Serial.println("MPU6050 ERROR");
  }
}


// =============================
// MAIN LOOP
// =============================

void loop() {

  int16_t ax;
  int16_t ay;
  int16_t az;

  // -----------------------------
  // READ MPU
  // -----------------------------

  if (!readAcceleration(ax, ay, az)) {

    Serial.println("MPU READ ERROR");

    delay(100);

    return;
  }


  // -----------------------------
  // TOTAL ACCELERATION SQUARED
  // -----------------------------

  uint32_t ax2 = (int32_t)ax * ax;
  uint32_t ay2 = (int32_t)ay * ay;
  uint32_t az2 = (int32_t)az * az;

  uint32_t accelSquared = ax2 + ay2 + az2;


  // -----------------------------
  // FALL DETECTION
  // -----------------------------

  unsigned long now = millis();

  // Stage 1: Free fall
  if (!possibleFall && !impactDetected) {

    if (accelSquared < FREEFALL_SQ) {

      possibleFall = true;

      freefallTime = now;

      Serial.println("POSSIBLE FREE FALL");
    }
  }


  // Stage 2: Impact
  if (possibleFall && !impactDetected) {

    if (now - freefallTime > 1000) {

      possibleFall = false;

      Serial.println("Free fall cancelled");
    }

    else if (accelSquared > IMPACT_SQ) {

      impactDetected = true;

      impactTime = now;

      Serial.println("IMPACT DETECTED");
    }
  }


  // Stage 3: Inactivity
  if (impactDetected) {

    if (now - impactTime > 1500) {

      if (accelSquared >= INACTIVITY_MIN_SQ &&
          accelSquared <= INACTIVITY_MAX_SQ) {

        Serial.println("************************");
        Serial.println("     FALL DETECTED");
        Serial.println("************************");

        tone(BUZZER_PIN, 2000);

        delay(3000);

        noTone(BUZZER_PIN);

        possibleFall = false;
        impactDetected = false;
      }

      else {

        Serial.println("Movement detected");
        
        possibleFall = false;
        impactDetected = false;
      }
    }
  }


  // -----------------------------
  // ULTRASONIC
  // -----------------------------

  long distance = getDistance();


  // -----------------------------
  // SERIAL OUTPUT
  // -----------------------------

  Serial.print("AX=");
  Serial.print(ax);

  Serial.print(" AY=");
  Serial.print(ay);

  Serial.print(" AZ=");
  Serial.print(az);

  Serial.print(" | Distance=");

  Serial.print(distance);

  Serial.println(" cm");


  // -----------------------------
  // OBSTACLE WARNING
  // -----------------------------

  if (distance > 0 && distance < 30) {

    tone(BUZZER_PIN, 1000);

  }

  else if (!impactDetected) {

    noTone(BUZZER_PIN);
  }


  delay(100);
}