#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL343.h>


/* Assign a unique ID to this sensor at the same time */
Adafruit_ADXL343 accel = Adafruit_ADXL343(12345);


// --- Movement Detection Variables ---

// Threshold 1: Detects general movement (Original logic)
const float MOVEMENT_THRESHOLD = 0.8; // Example: Detect movement if magnitude change exceeds 0.8G

// Threshold 2: Detects *fast* movement (New Logic)
// This threshold determines how large the difference between consecutive readings must be 
// to trigger a "slow down" warning. You will need to tune this heavily!
const float FAST_MOVEMENT_THRESHOLD = 1.5; // Example: Trigger if change in acceleration vector magnitude exceeds 1.5G


// Tracking variables for the previous reading (for both general and fast detection)
float last_accel_x = 0.0;
float last_accel_y = 0.0;
float last_accel_z = 0.0;

// Variables to track the state of the alerts
bool movement_detected_flag = false;
bool fast_movement_warning_active = false;


void setup(void)
{
  Serial.begin(9600);
  while (!Serial);
  Serial.println("ADXL343 Movement Detector Initializing..."); Serial.println("");

  /* Initialise the sensor */
  if(!accel.begin())
  {
    Serial.println("Ooops, no ADXL343 detected ... Check your wiring!");
    while(1);
  }

  /* Set the range to whatever is appropriate for your project */
  accel.setRange(ADXL343_RANGE_16_G);


  Serial.println("ADXL343 initialized. Ready to detect movement.");
}


void loop(void)
{
  // 1. Read the current sensor data
  sensors_event_t event;
  accel.getEvent(&event);

  float current_x = event.acceleration.x;
  float current_y = event.acceleration.y;
  float current_z = event.acceleration.z;


  // 2. Calculate the magnitude of acceleration (Total force vector)
  // Magnitude = sqrt(X^2 + Y^2 + Z^2)
  float current_magnitude = sqrt(current_x * current_x + current_y * current_y + current_z * current_z);


  // 3. Calculate the change in magnitude since the last reading (General Movement Proxy)
  float last_magnitude = sqrt(last_accel_x * last_accel_x + last_accel_y * last_accel_y + last_accel_z * last_accel_z);
  // The difference between current and previous magnitude is a good indicator of change.
  float delta_magnitude = abs(current_magnitude - last_magnitude);


  // 4. *** NEW: Calculate the rate of change (Fast Movement Proxy) ***
  // This measures how much the *vector* has changed, not just the total force magnitude.
  float delta_x = abs(current_x - last_accel_x);
  float delta_y = abs(current_y - last_accel_y);
  float delta_z = abs(current_z - last_accel_z);

  // The magnitude of the change vector (how much the reading jumped)
  float rate_of_change_magnitude = sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z);


  // 5. Detection Logic & Output
  
  // --- A. Fast Movement Check (The new requirement) ---
  if (rate_of_change_magnitude > FAST_MOVEMENT_THRESHOLD) {
    if (!fast_movement_warning_active) {
      Serial.println("\n****************************");
      Serial.print("!!! WARNING: SLOW DOWN !!! ");
      Serial.print("Rate of Change Magnitude: ");
      Serial.print(rate_of_change_magnitude);
      Serial.println(" (Too fast!)");
      Serial.println("****************************\n");
      fast_movement_warning_active = true; // Set flag to prevent spamming
    }
  } else {
    // If the change is small, reset the warning flag
    if (fast_movement_warning_active) {
        Serial.println("\n[INFO] Movement rate stabilized.");
    }
    fast_movement_warning_active = false;
  }


  // --- B. General Movement Check (Original logic) ---
  if (delta_magnitude > MOVEMENT_THRESHOLD) {
    if (!movement_detected_flag) {
      Serial.println("=============================");
      Serial.print("MOVEMENT DETECTED! Delta Magnitude: ");
      Serial.println(delta_magnitude);
      Serial.println("=============================");
      movement_detected_flag = true; // Set flag to prevent spamming
    }
  } else {
    // If the change is small, reset the general movement flag
    if (movement_detected_flag) {
        Serial.println("[INFO] Movement subsided.");
    }
    movement_detected_flag = false;
  }


  // 6. Update state for the next loop iteration
  last_accel_x = current_x;
  last_accel_y = current_y;
  last_accel_z = current_z;


  // Small delay to keep readings stable and prevent overwhelming serial output
  delay(50); 
}
