/*
    MASTER SKETCH: Movement Detector Sender (ESP-NOW) - FINAL VERSION with Speed Check
*/

#include <Wire.h> // Required for I2C communication with most sensors
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL343.h> 
#include <esp_now.h>
#include <WiFi.h>


// ===============================================================
// 1. GLOBAL CONFIGURATION & OBJECTS (Top of the file)
// ===============================================================

// --- Sensor Object Initialization ---
Adafruit_ADXL343 accel = Adafruit_ADXL343(12345); 


// --- Movement Detection Variables ---
const float MOVEMENT_THRESHOLD = .8; // Tune this value! (Delta magnitude threshold for general movement)
const float SPEED_THRESHOLD = 5.0;    // *** NEW: Threshold for "TOO FAST" (Tune this!) ***

// Tracking variables for the previous reading (MUST BE GLOBAL)
float last_accel_x = 0.0;
float last_accel_y = 0.0;
float last_accel_z = 0.0;


// State tracking flag (Controls if we print "DETECTED" or "SUBSIDED")
bool movement_detected_flag = false; 



// --- ESP-NOW MAC ADDRESS & STRUCTURES ---
uint8_t broadcastAddress[] = {0xb0, 0xa6, 0x04, 0x07, 0x29, 0x20};


typedef struct struct_message {
    char a[32]; // For status text ("Movement detected", "TOO FAST", etc.)
    int b;      // Integer (e.g., scaled reading)
    float c;    // Float (e.g., magnitude or raw float data)
    bool d;     // Boolean (Movement Detected Flag - used for general state tracking)
} struct_message;


struct_message myData;


esp_now_peer_info_t peerInfo;



// --- SENSOR VARIABLES (Used by the main loop for calculation) ---
float accelX = 0.0, accelY = 0.0, accelZ = 0.0; // Global variables for sensor readings



// ===============================================================
// 2. CALLBACK FUNCTION (MUST BE GLOBAL)
// ===============================================================
void OnDataSent(const wifi_tx_info_t *txInfo, esp_now_send_status_t status) { 
    Serial.print("\r\n[ESP-NOW Status] Last Packet Send Status:\t");
    Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}


// ===============================================================
// 3. SETUP FUNCTION (Initialization)
// ===============================================================
void setup() {
    // Initialize Serial Monitor
    Serial.begin(115200);
    
    // Set device as a Wi-Fi Station (Required for ESP-NOW)
    WiFi.mode(WIFI_STA);


    // --- Sensor Initialization ---
    Wire.begin(); // Initialize I2C bus for the sensor
    if(!accel.begin()) {
        Serial.println("ERROR: ADXL343 not found! Check wiring.");
    } else {
        accel.setRange(ADXL343_RANGE_16_G);
        Serial.println("ADXL343 Sensor Initialized Successfully.");
    }


    // 1. INITIALIZE ESP-NOW
    if (esp_now_init() != ESP_OK) {
        Serial.println("Error initializing ESP-NOW");
        return;
    }


    // 2. REGISTER SEND CALLBACK
    esp_now_register_send_cb(OnDataSent);
    
    // 3. CHANNEL SYNCHRONIZATION (CRITICAL FIX!)
    WiFi.setChannel(1); // <-- Use the channel that works for your setup!


    // 4. REGISTER PEER INFO
    memcpy(peerInfo.peer_addr, broadcastAddress, 6);
    peerInfo.channel = 0;  
    peerInfo.encrypt = false;
    
    if (esp_now_add_peer(&peerInfo) != ESP_OK){
        Serial.println("Failed to add peer");
        return;
    }
    Serial.println("\n=========================================");
    Serial.println("Sender Setup Complete. Ready to transmit.");
    Serial.println("=========================================\n");
}


// ===============================================================
// 4. SENSOR READING FUNCTION (Replaces the simulation)
// ===============================================================
void readAccelerometerData() {
    sensors_event_t event;
    if (accel.getEvent(&event)) { // Check if reading was successful
        float current_x = event.acceleration.x;
        float current_y = event.acceleration.y;
        float current_z = event.acceleration.z;


        // Calculate the magnitude of acceleration (Total force vector)
        float current_magnitude = sqrt(current_x * current_x + current_y * current_y + current_z * current_z);


        // Calculate the change in magnitude since the last reading
        float last_magnitude = sqrt(last_accel_x * last_accel_x + last_accel_y * last_accel_y + last_accel_z * last_accel_z);
        float delta_magnitude = abs(current_magnitude - last_magnitude);


        // Update the global variables used by the main loop() logic
        accelX = current_x;
        accelY = current_y;
        accelZ = current_z;
        
        // Print for debugging purposes (optional)
        Serial.printf("Sensor Read: X=%.2f, Y=%.2f, Z=%.2f | Delta Mag: %.2f\n", accelX, accelY, accelZ, delta_magnitude);


    } else {
        Serial.println("ERROR reading sensor data!");
    }
}


// ===============================================================
// 5. MAIN LOOP (State Machine & Sending)
// ===============================================================
void loop() {
    // --- STEP A: Read Sensor Data and Calculate Change ---
    readAccelerometerData(); 

    // Recalculate delta magnitude based on the updated global variables from readAccelerometerData()
    float current_magnitude = sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);
    float last_magnitude = sqrt(last_accel_x * last_accel_x + last_accel_y * last_accel_y + last_accel_z * last_accel_z);
    float current_delta_magnitude = abs(current_magnitude - last_magnitude);


    // --- STEP B: State Change Detection (The core logic) ---
    char status_string[32] = ""; // Buffer to hold the status message
    bool send_movement_flag = false;

    if (current_delta_magnitude > SPEED_THRESHOLD) {
        // 1. TOO FAST STATE
        strcpy(status_string, "TOO FAST");
        send_movement_flag = true;
        
        if (!movement_detected_flag) {
            Serial.println("\n========================================");
            Serial.println("!!! SPEED EXCEEDED (TOO FAST) !!!"); // <-- PRINT ON CHANGE
            Serial.println("========================================\n");
        }

    } else if (current_delta_magnitude > MOVEMENT_THRESHOLD) {
        // 2. NORMAL MOVEMENT DETECTED STATE
        strcpy(status_string, "Movement detected");
        send_movement_flag = true;

        if (!movement_detected_flag) {
            Serial.println("\n========================================");
            Serial.println("!!! MOVEMENT DETECTED !!!"); // <-- PRINT ON CHANGE
            Serial.println("========================================\n");
        }

    } else {
        // 3. SUBSIDED STATE (Below both thresholds)
        strcpy(status_string, "Movement subsided");
        send_movement_flag = false;

        if (movement_detected_flag) {
            Serial.println("\n========================================");
            Serial.println("!!! MOVEMENT SUBSIDED !!!"); // <-- PRINT ON CHANGE
            Serial.println("========================================\n");
        }
    }


    // --- Populate Payload based on determined state ---
    if (send_movement_flag) {
        myData.a[0] = '\0'; // Clear status string buffer for safety
        strncpy(myData.a, status_string, 31); // Copy the correct status
        myData.b = 1;                               // Movement is active
        myData.c = current_magnitude;               // Send the actual magnitude
        myData.d = true;                             // Flag set to true
    } else {
        // Subsided or still state
        strcpy(myData.a, "Movement subsided"); // Always send this if not moving fast/moving
        myData.b = 0;                            // Movement is inactive
        myData.c = current_magnitude;            // Send the low magnitude
        myData.d = false;                         // Flag set to false
    }


    // --- STEP C: Send Data (Network Transmission) ---
    esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &myData, sizeof(myData));
    
    if (result == ESP_OK) {
        Serial.printf("Data packet sent successfully. Status: %s\n", myData.a); 
    } else {
        Serial.println("ERROR: Failed to send data packet."); 
    }


    // --- STEP D: Update State for Next Cycle & Sensor History ---
    // The flag is now set based on the highest detected state (TOO FAST > MOVEMENT > STILL)
    if (current_delta_magnitude > SPEED_THRESHOLD) {
        movement_detected_flag = true; // If too fast, we are definitely "moving"
    } else if (current_delta_magnitude > MOVEMENT_THRESHOLD) {
        movement_detected_flag = true; // Normal movement
    } else {
        movement_detected_flag = false; // Subsided
    }

    // Update history for the next loop iteration
    last_accel_x = accelX;
    last_accel_y = accelY;
    last_accel_z = accelZ;


    // Wait for a short period to keep the loop responsive but not overwhelming.
    delay(100); // Reduced delay slightly to better capture rapid changes
}
