【格院】EP Lab 4 project（已经跑通版）：智能储物柜监控系统（I2C/TEM102/ADXL345/STM32/MBED OS）

写在前面的话：

不知道大家有没有做出来Lab 4的project呀？

我删除了之前的那个手搓版本，当前版本有更多的库依赖，并且调用了更多的Mbed OS的API来增加程序的稳定性（PS：请忽略我的代码中类似于volatile bool类型变量使用的操作，这就是专门提升灵活性而防止过度编译而做的操作，并且这个操作在当前程序中是安全的）

大家可以跑一下程序以后给我反馈是否OK，因为我好像是往某个库里面写了新东西的，但是我忘了是哪个了。。。。。

个人感觉本次的重难点有以下几个：
1.低功耗：Manual里面明确说到，我们需要设置一个低能耗的模式，并且用“中断”的策略来唤醒。

2.并行逻辑：在监测的同时需要对于用户的交互行为做出反应。

这一部分当然是可以做成轮询的检测的，但是不可避免的会消耗一定的处理器资源和处理时间，并且会带来一定的阻塞问题。所以这一部分可以做成并行访问，或者说，这一部分本来就该是并行访问，本来就该是同级时间。

这也是一个难点。it means ：做出来简单，做好难。

3.状态转移问题。

在我个人的project当中，我采取的是事件驱动的状态转移策略，我们可以发现的是：

在本项目中有几个我们需要注意的事件：用户按下按钮/倾斜角度过大/温度异常......

有几个我们需要注意的状态：低功耗（睡眠）模式/唤醒（输入密码）模式/报警模式/解锁模式

那么涉及到状态跳转到时候我们必须有很好的，稳定的，安全的跳转策略，对于当前状态的保存策略。那么稳定的ISR是必要的，在此基础上，我建议或者说强烈推荐使用Mutex，互斥锁来保护我们的栈。

4.系统鲁棒性和健壮性的提升。如何让我们的系统变得更加的安全和稳定？

智能储物系统使用说明

一、重要注意事项 ⚠️ (务必优先阅读！)

在开始操作和进一步开发之前，请务必注意以下几点，以确保系统正常工作并避免损坏元器件：

1. 共地 (Common Ground)：

   • 所有模块（如微控制器板、TMP102、ADXL345、蜂鸣器、LED等）的GND引脚必须连接在一起，形成共同的参考地。

2. I2C 通信：

   • 总线共享： TMP102温度传感器和ADXL345加速度传感器共享同一I2C总线。

   • 引脚连接：

     • SDA (数据线): 连接到微控制器 D4 引脚。

     • SCL (时钟线): 连接到微控制器 D5 引脚。

   • 上拉电阻： I2C总线的SDA和SCL线通常需要外部上拉电阻（例如4.7kΩ）连接到VCC，以确保通信稳定。您的开发板或传感器模块可能已内置。

   • TMP102 地址： 代码中配置的地址为 0x48 (7位地址)。

   • ADXL345 地址： 代码中配置的地址为 0x53 (7位地址，对应模块上的 0xA6 写 / 0xA7 读，如果 SDO/ALT ADDRESS 引脚接地)。

3. ADXL345 加速度传感器：

   • 通信方式： I2C (与TMP102共享D4, D5引脚)。

   • 配置参数：

     • 数据格式：全分辨率模式。

     • 测量量程：±16g。

     • 输出数据率：100Hz。

   • 检测方式： 系统通过周期性读取传感器数据（默认每秒1次）来检测倾斜和动态/震动，非中断驱动。

4. TMP102 温度传感器：

   • 通信方式： I2C (与ADXL345共享D4, D5引脚)。

   • 工作模式： 使用库默认配置。

   • 温度阈值：

     • 高温警报阈值：40.0°C

     • 低温警报阈值：0.0°C

5. 低功耗设计：

   • MCU sleep() 功能已集成在代码中。

   • 在进入睡眠模式 (STATE_SLEEP) 时，系统会停止周期性的传感器数据读取 (sensor_ticker) 和LED状态更新 (led_ticker) 以降低功耗。

6. 硬件连接 (基于代码中的Mbed引脚定义)：

   • I2C总线：

     • D4: I2C SDA (连接到 TMP102 SDA 和 ADXL345 SDA)

     • D5: I2C SCL (连接到 TMP102 SCL 和 ADXL345 SCL)

   • 用户按钮 (均配置为内部上拉电阻)：

     • A4: 唤醒按钮 / 密码输入 '1' (button_wake)

     • A0: 解锁按钮 / 密码输入 '2' (button_unlock)

   • 状态指示LED：

     • D2: 红色LED (redLED) - 用于警报或错误状态指示。

     • D3: 蓝色LED (blueLED) - 用于系统锁定或密码输入状态指示。

     • D10: 绿色LED (greenLED) - 用于系统解锁状态指示。

   • 蜂鸣器：

     • D6: 蜂鸣器 (buzzer) - 低电平有效 (代码中 buzzer = 1 为关闭)。

二、系统操作说明 🎛️

本系统通过两个按钮（A4 和 A0）进行操作，并通过LED和蜂鸣器提供状态反馈。

1. 系统启动：

   • 正确连接所有硬件并为微控制器板上电。

   • 系统将自动进行初始化。您可以通过串口监视器（如Mbed OS默认波特率9600或代码中设置的速率）查看初始化信息和后续的状态输出。

   • 初始化完成后，系统会播放一声提示音，绿色LED闪烁一次。

   • 系统初始状态为 睡眠 (STATE_SLEEP)。按 唤醒按钮 (A4) 可唤醒系统。

2. 唤醒系统：

   • 在 睡眠 (STATE_SLEEP) 状态下，按一下 唤醒按钮 (A4)。

   • 系统将退出睡眠模式，进入 锁定 (STATE_LOCKED) 状态。蓝色LED会短暂亮一下然后熄灭，表示唤醒成功并进入锁定状态。

3. 密码输入与解锁/锁定：

   • 预设密码： 唤醒按钮 (A4) -> 解锁按钮 (A0) -> 唤醒按钮 (A4) -> 解锁按钮 (A0)。 (序列: 1, 2, 1, 2)

   • 进入密码输入模式 (STATE_CODE_ENTRY)：

     • 在 锁定 (STATE_LOCKED) 状态下，按任意一个密码按钮（A4 或 A0）即可开始输入密码。

     • 蓝色LED将变为快速闪烁，表示系统正在等待密码输入。

   • 输入反馈：

     • 每按下一个密码按钮，串口会打印输入的数字（1或2）和当前输入的位置。

   • 密码正确：

     • 当输入的4位序列与预设密码匹配时：

       • 系统进入 解锁 (STATE_UNLOCKED) 状态。

       • 绿色LED常亮。

       • 蜂鸣器发出两声短促的成功提示音。

       • 任何由传感器触发的警报状态将被解除（但如果物理条件仍然存在，警报可能会在解锁后重新触发）。

       • 密码错误尝试计数器将清零。

   • 密码错误：

     • 如果输入的4位序列不正确：

       • 蜂鸣器发出两声短促的错误提示音。

       • 密码错误尝试计数器增加。

       • 系统通常会返回到 锁定 (STATE_LOCKED) 状态（蓝色LED慢闪）。

     • 连续输错3次： 系统将进入 警报 (STATE_ALERT) 状态。红色LED快速闪烁，蜂鸣器发出10声长提示音。此时，系统仍处于锁定状态，需要正确输入密码才能解除警报并解锁。密码错误尝试计数器清零。

   • 密码输入超时 (5秒)：

     • 如果在开始输入密码后，5秒内没有完成4位密码的输入（或在输入过程中停顿超过5秒）：

       • 密码输入被取消。

       • 系统返回到 锁定 (STATE_LOCKED) 状态。

       • 蜂鸣器发出一声提示音。

       • 已输入的部分密码将被清空。

   • 从解锁状态锁定系统：

     • 在 解锁 (STATE_UNLOCKED) 状态下，按一下 唤醒按钮 (A4)。

     • 系统将进入 锁定 (STATE_LOCKED) 状态。

     • 绿色LED熄灭，蓝色LED开始慢闪。

     • 蜂鸣器发出一声提示音。

4. 锁定状态下的监控与警报 (STATE_LOCKED)： 系统在锁定状态下会周期性（默认每秒1次）监测温度和运动/倾斜状态。

   • 温度监控：

     • 如果温度传感器读数超过 40.0°C (高温) 或低于 0.0°C (低温)，或者传感器连续读取失败：

       • 系统进入 警报 (STATE_ALERT) 状态。

       • 红色LED快速闪烁，蜂鸣器发出3声提示音。

     • 自动解除： 如果温度恢复到正常范围内，temperature_alert标志会自动清除，系统会尝试从警报状态恢复到之前的状态（通常是 锁定 (STATE_LOCKED)）。

   • 动态/震动监控 (防篡改)：

     • 如果ADXL345检测到的三轴加速度绝对值之和超过阈值 ACCEL_TAMPER_THRESHOLD (250)：

       • 系统进入 警报 (STATE_ALERT) 状态。

       • 红色LED快速闪烁，蜂鸣器发出5声提示音。

   • 倾斜监控：

     • 如果ADXL345检测到的Z轴加速度绝对值小于阈值 ACCEL_TILT_THRESHOLD (150)，可能表示储物柜被倾斜：

       • 系统进入 警报 (STATE_ALERT) 状态。

       • 红色LED快速闪烁，蜂鸣器发出2声提示音。

5. 解除警报 (STATE_ALERT)：

   • 通过密码： 在任何警报状态下，正确输入预设密码将使系统进入 解锁 (STATE_UNLOCKED) 状态。这会使系统脱离当前的 警报 (STATE_ALERT) 状态。

   • 温度警报自动解除： 如上所述，如果温度恢复正常，温度警报会自动解除。

   • 篡改/倾斜警报： tamper_alert 和 tilt_alert 标志在当前代码逻辑中，一旦被设置，并不会因为物理条件的恢复而自动清除其标志位。因此，即使扰动停止，系统可能仍处于警报状态。主要通过输入正确密码来使系统脱离警报状态并解锁。如果解锁后物理扰动条件依然存在，警报可能会重新触发。

6. MCU睡眠 (STATE_SLEEP)：

   • 在 锁定 (STATE_LOCKED) 或 解锁 (STATE_UNLOCKED) 状态下，如果系统空闲（无按钮操作）超过30秒 (IDLE_TIMEOUT_MS)：

     • 系统将自动进入低功耗的 睡眠 (STATE_SLEEP) 模式。

     • 所有LED熄灭，蜂鸣器关闭。

     • 周期性的传感器数据读取和LED更新将暂停。

   • 在 警报 (STATE_ALERT) 状态下，如果空闲超过60秒（两倍的 IDLE_TIMEOUT_MS），系统也会尝试进入睡眠模式。

   • 按 唤醒按钮 (A4) 可从睡眠模式唤醒系统，唤醒后系统将进入 锁定 (STATE_LOCKED) 状态。

三、程序重点功能

1. 环境监控 (基于TMP102)：

   • 通过I2C总线实时读取环境温度。

   • 具备高温 (40.0°C) 和低温 (0.0°C) 阈值警报功能。

   • 温度警报在物理条件恢复正常后可以自动解除。

2. 防篡改及倾斜检测 (基于ADXL345)：

   • 通过I2C总线获取三轴加速度数据。

   • 实现动态/震动检测（基于三轴加速度绝对值之和判断是否超出 ACCEL_TAMPER_THRESHOLD）。

   • 实现倾斜检测（基于Z轴加速度绝对值判断是否低于 ACCEL_TILT_THRESHOLD）。

3. 用户认证与交互：

   • 采用双按钮 (A4 和 A0) 输入4位序列密码 (1,2,1,2)。

   • 包含按钮软件去抖逻辑。

   • 具有密码输入超时机制 (5秒)。

   • 限制密码错误尝试次数（连续3次错误触发警报）。

   • 通过LED和蜂鸣器提供输入和状态反馈。

4. 实时反馈与警报系统：

   • 使用红色LED指示警报/错误状态。

   • 使用蓝色LED指示锁定状态和密码输入过程。

   • 使用绿色LED指示解锁状态。

   • 使用蜂鸣器提供操作成功、失败、警报等多种声音提示。

5. 多状态系统管理：

   • 使用 SystemState 枚举（STATE_SLEEP, STATE_LOCKED, STATE_UNLOCKED, STATE_CODE_ENTRY, STATE_ALERT）清晰地管理系统运行的各种模式。

   • 通过 previous_state 变量记录警报前的状态，以便在警报解除后恢复。

6. 低功耗操作：

   • 集成Mbed OS的 sleep() 低功耗功能。

   • 在空闲时自动进入睡眠模式，并在睡眠期间停止不必要的周期性任务（传感器读取、LED更新）以节省能源。

   • 通过按钮中断唤醒。

7. 事件驱动与并发处理：

   • 使用Mbed OS的 EventQueue 将中断服务程序（ISR）中的耗时操作（如按钮逻辑处理）推迟到专门的事件处理线程中执行，提高了系统的响应性和稳定性。

   • 使用 Ticker 实现周期性的传感器数据采集和LED状态更新。

   • 使用 Mutex 保护共享变量（如系统状态、传感器数据）的线程安全访问。

四、代码

#include "mbed.h"
#include "TMP102.h"
#include "ADXL345_I2C.h"
#include "rtos.h"
#include "platform/mbed_power_mgmt.h"
#include <chrono> // Required for std::chrono
#include <cstdarg> // Required for va_list

// === System Configuration ===
// Temperature thresholds and error value
const float TEMP_HIGH_THRESHOLD = 40.0f; // Celsius
const float TEMP_LOW_THRESHOLD = 0.0f;   // Celsius
const float TEMP_ERROR_VALUE = -999.0f;  // Value indicating sensor read error

// Accelerometer thresholds for tamper and tilt detection
const int ACCEL_TAMPER_THRESHOLD = 250; // Magnitude threshold for tamper
const int ACCEL_TILT_THRESHOLD = 150;   // Z-axis threshold for tilt. Note: This value depends on the sensor's scale and output format.

// Unlock code configuration
const int UNLOCK_CODE_LENGTH = 4;     // Length of the unlock code
const int UNLOCK_TIMEOUT_MS = 5000;   // Timeout for code entry in milliseconds
const int DEBOUNCE_MS = 50;           // Debounce time for buttons in milliseconds

// System timers
const int IDLE_TIMEOUT_MS = 30000;    // Timeout to enter sleep mode in milliseconds
const int SENSOR_UPDATE_MS = 1000;    // Interval for sensor updates in milliseconds

// === Hardware Interfaces ===
// I2C bus and sensors
I2C i2c(D4, D5); // SDA, SCL
TMP102 tempSensor(D4, D5, 0x48); // Temperature sensor - TMP102 constructor handles the address shift
ADXL345_I2C accel(D4, D5);       // Accelerometer (using default I2C pins, library handles address)

// User interface buttons with internal pull-up resistors
InterruptIn button_unlock(A0, PullUp); // Button for entering '2' in the code or locking
InterruptIn button_wake(A4, PullUp);   // Button for waking up or entering '1' in the code

// Status indicators: LEDs and Buzzer
DigitalOut redLED(D2);    // Red LED for alerts/errors
DigitalOut blueLED(D3);   // Blue LED for locked/code entry status
DigitalOut greenLED(D10); // Green LED for unlocked status
DigitalOut buzzer(D6);    // Buzzer for audible feedback (active low, so 1 is off)

// === System States ===
enum SystemState {
    STATE_SLEEP,        // Low-power sleep mode
    STATE_LOCKED,       // System is locked and monitoring
    STATE_UNLOCKED,     // System is unlocked
    STATE_CODE_ENTRY,   // User is entering the unlock code
    STATE_ALERT         // System is in an alert state (tamper, temp, etc.)
};

// === Thread-safe Global Variables ===
// Mutex to protect access to current_state and previous_state
Mutex state_mutex;
SystemState current_state = STATE_SLEEP;  // Current operational state of the system
SystemState previous_state = STATE_SLEEP; // Previous state before entering an alert

// Mutex to protect access to sensor data
Mutex sensor_mutex;
float current_temperature = 25.0f;     // Last read temperature value
int16_t accel_readings[3] = {0, 0, 0}; // Last read accelerometer values (x, y, z)
int motion_magnitude = 0;              // Calculated motion magnitude

// Volatile alert flags, can be set by sensor checks
volatile bool temperature_alert = false; // True if temperature is out of bounds
volatile bool tamper_alert = false;      // True if tampering motion is detected
volatile bool tilt_alert = false;        // True if significant tilt is detected

// Access control variables
uint8_t unlock_code[UNLOCK_CODE_LENGTH] = {1, 2, 1, 2}; // The correct unlock code (Wake, Unlock, Wake, Unlock)
uint8_t code_entry[UNLOCK_CODE_LENGTH];                 // Buffer for user's code input
volatile int code_position = 0;                         // Current position in code_entry buffer
volatile int failed_attempts = 0;                       // Counter for incorrect code entries

// EventQueue for deferring ISR work to a separate thread
EventQueue event_queue(32 * EVENTS_EVENT_SIZE);
Thread event_thread(osPriorityNormal, OS_STACK_SIZE, nullptr, "event_thread"); // Thread to run the event queue

// Mutex for thread-safe printing
Mutex printf_mutex;

// Thread-safe printf wrapper
void safe_printf(const char* format, ...) {
    printf_mutex.lock();
    va_list args;
    va_start(args, format);
    vprintf(format, args);
    va_end(args);
    printf_mutex.unlock();
}

// Timers for various system functions
Timer idle_timer;        // Tracks idle time to enter sleep mode
Timer code_timer;        // Tracks time during code entry
Timer last_wake_press;   // Used for debouncing wake button
Timer last_unlock_press; // Used for debouncing unlock button

// Timer state tracking (since is_started() doesn't exist in Mbed OS 6)
bool code_timer_running = false;
bool debug_timer_running = false;

// Tickers for periodic tasks
Ticker sensor_ticker; // Calls sensor update handler periodically
Ticker led_ticker;    // Calls LED update handler periodically

// ADXL345 I2C address for direct I2C operations
// Using a different name to avoid conflict with the library's macro
const int ADXL345_ADDR = (0x53 << 1);


// === Function Prototypes ===
void safe_printf(const char* format, ...);
void init_hardware();
void enter_sleep_mode();
void exit_sleep_mode();
void update_sensors();       // Will be called from event queue
void check_temperature();
void check_motion();
void check_alerts();
void process_code_entry();
void handle_unlock();
void handle_lock();
void update_leds();          // Will be called from event queue
void sound_alert(int beeps);
void deferred_wake_handler();
void deferred_unlock_handler();
// New ISR handlers for tickers
void sensor_ticker_isr_handler();
void led_ticker_isr_handler();
bool read_i2c_safe(I2C &i2c_bus, int addr, char *data, int len, bool repeated = false);
bool write_i2c_safe(I2C &i2c_bus, int addr, const char *data, int len, bool repeated = false);


// === Safe I2C Communication Wrappers ===
bool read_i2c_safe(I2C &i2c_bus, int addr, char *data, int len, bool repeated) {
    int result = i2c_bus.read(addr, data, len, repeated);
    if (result != 0) {
        safe_printf("I2C read error from address 0x%X: %d\n", addr, result);
        return false;
    }
    return true;
}

bool write_i2c_safe(I2C &i2c_bus, int addr, const char *data, int len, bool repeated) {
    int result = i2c_bus.write(addr, data, len, repeated);
    if (result != 0) {
        safe_printf("I2C write error to address 0x%X: %d\n", addr, result);
        return false;
    }
    return true;
}

// === Interrupt Service Routines (Minimal to defer work) ===
void wake_button_isr() {
    // Use elapsed_time() for Mbed OS 6+
    if (std::chrono::duration_cast<std::chrono::milliseconds>(last_wake_press.elapsed_time()).count() > DEBOUNCE_MS) {
        event_queue.call(deferred_wake_handler);
        last_wake_press.reset(); // Reset after registering the press
    }
}

void unlock_button_isr() {
    if (std::chrono::duration_cast<std::chrono::milliseconds>(last_unlock_press.elapsed_time()).count() > DEBOUNCE_MS) {
        event_queue.call(deferred_unlock_handler);
        last_unlock_press.reset();
    }
}

// ISR handlers for tickers, they post tasks to the event queue
void sensor_ticker_isr_handler() {
    event_queue.call(update_sensors);
}

void led_ticker_isr_handler() {
    event_queue.call(update_leds);
}


// === Deferred Handlers (Executed by Event Queue Thread) ===
void deferred_wake_handler() {
    state_mutex.lock();
    SystemState state_snapshot = current_state;
    state_mutex.unlock();
    
    switch (state_snapshot) {
        case STATE_SLEEP:
            exit_sleep_mode();
            break;
        case STATE_LOCKED:
            state_mutex.lock();
            current_state = STATE_CODE_ENTRY;
            code_position = 0;
            code_timer.reset(); 
            code_timer.start();
            code_timer_running = true;
            code_entry[code_position++] = 1;
            safe_printf("Code Entry Started. Input: 1\n");
            state_mutex.unlock();
            break;
        case STATE_UNLOCKED:
            handle_lock();
            break;
        case STATE_CODE_ENTRY:
            if (code_position < UNLOCK_CODE_LENGTH) {
                code_entry[code_position++] = 1;
                safe_printf("Code Entry. Input: 1, Position: %d\n", code_position);
            }
            code_timer.reset(); // Reset timeout on new input
            break;
        default: break;
    }
    idle_timer.reset();
}

void deferred_unlock_handler() {
    state_mutex.lock();
    SystemState state_snapshot = current_state;
    state_mutex.unlock();
    
    switch (state_snapshot) {
        case STATE_LOCKED:
            state_mutex.lock();
            current_state = STATE_CODE_ENTRY;
            code_position = 0;
            code_timer.reset(); 
            code_timer.start();
            code_timer_running = true;
            code_entry[code_position++] = 2;
            safe_printf("Code Entry Started. Input: 2\n");
            state_mutex.unlock();
            break;
        case STATE_CODE_ENTRY:
            if (code_position < UNLOCK_CODE_LENGTH) {
                code_entry[code_position++] = 2;
                safe_printf("Code Entry. Input: 2, Position: %d\n", code_position);
            }
            code_timer.reset(); // Reset timeout on new input
            break;
        default: break;
    }
    idle_timer.reset();
}

// === Hardware Initialization ===
void init_hardware() {
    safe_printf("Initializing I2C bus...\n");
    i2c.frequency(400000);

    safe_printf("Initializing LEDs and Buzzer...\n");
    redLED = 0; blueLED = 0; greenLED = 0;
    buzzer = 1; // Off (active low)

    safe_printf("Initializing Temperature Sensor...\n");
    if (tempSensor.isConnected()) {
        safe_printf("TMP102 detected at address 0x48\n");
        tempSensor.configure(); // Use default configuration
    } else {
        safe_printf("ERROR: TMP102 not found at address 0x48!\n");
    }

    safe_printf("Initializing Accelerometer ADXL345...\n");
    // Initialize ADXL345. The ADXL345_I2C library should handle setup.
    // If specific setup is needed (like power mode, range), call library methods.
    // Example of direct I2C commands:
    char cmd[2];
    // Put ADXL345 into standby mode to change settings
    cmd[0] = 0x2D; // POWER_CTL register address
    cmd[1] = 0x00; // Standby mode
    if (!write_i2c_safe(i2c, ADXL345_ADDR, cmd, 2)) {
        safe_printf("Error: Failed to set ADXL345 to standby.\n");
    }
    ThisThread::sleep_for(10ms);

    // Set data format: Full resolution, +/-16g
    cmd[0] = 0x31; // DATA_FORMAT register address
    cmd[1] = 0x0B; // Full res, +/-16g
     if (!write_i2c_safe(i2c, ADXL345_ADDR, cmd, 2)) {
        safe_printf("Error: Failed to set ADXL345 data format.\n");
    }

    // Set data rate: 100Hz
    cmd[0] = 0x2C; // BW_RATE register address
    cmd[1] = 0x0A; // 100Hz output rate
    if (!write_i2c_safe(i2c, ADXL345_ADDR, cmd, 2)) {
         safe_printf("Error: Failed to set ADXL345 data rate.\n");
    }
    
    // Enable measurement mode
    cmd[0] = 0x2D; // POWER_CTL register address
    cmd[1] = 0x08; // Measurement mode
    if (!write_i2c_safe(i2c, ADXL345_ADDR, cmd, 2)) {
        safe_printf("Error: Failed to enable ADXL345 measurement mode.\n");
    }
    // It's often better to use library-provided functions for setup if they exist, e.g., accel.setPowerControl(0x08);

    safe_printf("Initializing buttons...\n");
    button_wake.fall(&wake_button_isr);
    button_unlock.fall(&unlock_button_isr);

    safe_printf("Starting timers...\n");
    idle_timer.start();
    last_wake_press.start();
    last_unlock_press.start();
    last_wake_press.reset(); 
    last_unlock_press.reset();

    safe_printf("Starting event queue thread...\n");
    event_thread.start(callback(&event_queue, &EventQueue::dispatch_forever));

    safe_printf("Scheduling periodic tasks...\n");
    // Attach new ISR handlers for tickers
    sensor_ticker.attach(&sensor_ticker_isr_handler, std::chrono::milliseconds(SENSOR_UPDATE_MS));
    led_ticker.attach(&led_ticker_isr_handler, 50ms);


    safe_printf("Performing initial sensor reading...\n");
    event_queue.call(update_sensors); // Perform initial read via event queue

    sound_alert(1);
    greenLED = 1; ThisThread::sleep_for(200ms); greenLED = 0;
}

// === Power Management ===
void enter_sleep_mode() {
    state_mutex.lock();
    if (current_state == STATE_SLEEP) {
        state_mutex.unlock(); return;
    }
    safe_printf("Entering sleep mode...\n");
    current_state = STATE_SLEEP;
    state_mutex.unlock();

    redLED = 0; blueLED = 0; greenLED = 0; buzzer = 1;
    sensor_ticker.detach();
    led_ticker.detach();
}

void exit_sleep_mode() {
    state_mutex.lock();
    if (current_state != STATE_SLEEP) {
        state_mutex.unlock(); return;
    }
    safe_printf("Exiting sleep mode...\n");
    current_state = STATE_LOCKED;
    state_mutex.unlock();

    sensor_ticker.attach(&sensor_ticker_isr_handler, std::chrono::milliseconds(SENSOR_UPDATE_MS));
    led_ticker.attach(&led_ticker_isr_handler, 50ms);
    
    event_queue.call(update_sensors);
    idle_timer.reset();

    blueLED = 1; ThisThread::sleep_for(100ms); blueLED = 0;
}

// === Sensor Functions (Called by Event Queue) ===
void update_sensors() {
    state_mutex.lock();
    SystemState state_snapshot = current_state;
    state_mutex.unlock();

    if (state_snapshot != STATE_SLEEP) {
        check_temperature();
        check_motion();
        check_alerts(); // Check alerts after updating sensor values
    }
}

void check_temperature() {
    float temp = tempSensor.read();
    static int error_count = 0; // Static to persist across calls. Moved here for clarity.

    sensor_mutex.lock();
    // Check for exact error value; floating point comparisons can be tricky.
    if (temp < (TEMP_ERROR_VALUE + 0.1f) && temp > (TEMP_ERROR_VALUE - 0.1f) ) {
        error_count++;
        if (error_count > 3) {
            if (!temperature_alert) safe_printf("Temp sensor error persisted. Alert set.\n");
            temperature_alert = true;
            current_temperature = TEMP_ERROR_VALUE; 
        }
    } else {
        error_count = 0; // Reset on a good read
        current_temperature = temp;
        if (temp > TEMP_HIGH_THRESHOLD || temp < TEMP_LOW_THRESHOLD) {
            if (!temperature_alert) safe_printf("Temperature alert: %.1fC\n", temp);
            temperature_alert = true;
        } else {
            if (temperature_alert) safe_printf("Temperature normal: %.1fC\n", temp);
            temperature_alert = false;
        }
    }
    sensor_mutex.unlock();
}

void check_motion() {
    int16_t pDataXYZ[3] = {0, 0, 0};

    sensor_mutex.lock();
    // ADXL345_I2C::getOutput is typically void. It populates pDataXYZ.
    // No return value to check like `== 0`.
    // Assume success if no I2C error was thrown/handled by underlying library calls.
    accel.getOutput(pDataXYZ); 
    
    accel_readings[0] = pDataXYZ[0];
    accel_readings[1] = pDataXYZ[1];
    accel_readings[2] = pDataXYZ[2];

    motion_magnitude = abs(accel_readings[0]) + abs(accel_readings[1]) + abs(accel_readings[2]);

    if (motion_magnitude > ACCEL_TAMPER_THRESHOLD) {
        if (!tamper_alert) safe_printf("Tamper detected! Magnitude: %d\n", motion_magnitude);
        tamper_alert = true;
    } else {
        // tamper_alert = false; // Consider how/when to reset tamper alerts
    }

    // Tilt detection: if Z-axis reading is small, it might be tilted.
    // This threshold is sensitive to the sensor's scale and orientation.
    if (abs(accel_readings[2]) < ACCEL_TILT_THRESHOLD) {
        if (!tilt_alert) safe_printf("Tilt detected! Z-axis: %d\n", accel_readings[2]);
        tilt_alert = true;
    } else {
        // tilt_alert = false; // Consider how/when to reset tilt alerts
    }
    sensor_mutex.unlock();
}

// === Alert Management ===
void check_alerts() {
    bool any_alert_active = temperature_alert || tamper_alert || tilt_alert;

    state_mutex.lock();
    SystemState state_snapshot = current_state; // Snapshot before potential change
    SystemState previous_state_snapshot = previous_state; // Snapshot before potential change
    state_mutex.unlock();


    if (any_alert_active && state_snapshot != STATE_ALERT) {
        safe_printf("ALERT TRIGGERED! Temp:%d, Tamper:%d, Tilt:%d. Prev state was %d\n", 
               temperature_alert, tamper_alert, tilt_alert, state_snapshot);
        state_mutex.lock();
        previous_state = state_snapshot; // Save state before alert
        current_state = STATE_ALERT;
        state_mutex.unlock();

        if (temperature_alert) event_queue.call(sound_alert, 3);
        else if (tamper_alert) event_queue.call(sound_alert, 5);
        else if (tilt_alert) event_queue.call(sound_alert, 2);
        
    } else if (!any_alert_active && state_snapshot == STATE_ALERT) {
        safe_printf("Alerts cleared. Returning to previous state: %d\n", previous_state_snapshot);
        state_mutex.lock();
        current_state = previous_state_snapshot; // Restore the genuinely previous state
        // Explicitly reset alert flags now that they are handled
        temperature_alert = false; 
        tamper_alert = false;
        tilt_alert = false;
        state_mutex.unlock();
    }
}

void sound_alert(int beeps) {
    for (int i = 0; i < beeps; i++) {
        buzzer = 0; ThisThread::sleep_for(100ms);
        buzzer = 1; ThisThread::sleep_for(100ms);
    }
}

// === Access Control ===
void process_code_entry() {
    if (code_position >= UNLOCK_CODE_LENGTH) {
        bool correct = true;
        for (int i = 0; i < UNLOCK_CODE_LENGTH; i++) {
            if (code_entry[i] != unlock_code[i]) {
                correct = false; break;
            }
        }
        if (correct) {
            safe_printf("Unlock code correct.\n");
            handle_unlock();
        } else {
            safe_printf("Unlock code incorrect.\n");
            failed_attempts++;
            event_queue.call(sound_alert, 2);
            if (failed_attempts >= 3) {
                safe_printf("Too many failed attempts. Entering alert.\n");
                state_mutex.lock();
                previous_state = STATE_LOCKED; 
                current_state = STATE_ALERT;
                state_mutex.unlock();
                event_queue.call(sound_alert, 10);
                failed_attempts = 0;
            } else { // Only go to locked if not entering alert due to failed attempts
                 state_mutex.lock();
                 current_state = STATE_LOCKED;
                 state_mutex.unlock();
            }
        }
        code_position = 0;
        code_timer.stop();
        code_timer_running = false;
    }

    if (code_timer_running && std::chrono::duration_cast<std::chrono::milliseconds>(code_timer.elapsed_time()).count() > UNLOCK_TIMEOUT_MS && code_position > 0) {
        safe_printf("Code entry timed out.\n");
        state_mutex.lock();
        current_state = STATE_LOCKED;
        state_mutex.unlock();
        code_position = 0;
        event_queue.call(sound_alert, 1);
        code_timer.stop();
        code_timer_running = false;
    }
}

void handle_unlock() {
    state_mutex.lock();
    current_state = STATE_UNLOCKED;
    state_mutex.unlock();
    failed_attempts = 0;
    greenLED = 1;
    buzzer = 0; ThisThread::sleep_for(150ms); buzzer = 1; ThisThread::sleep_for(100ms);
    buzzer = 0; ThisThread::sleep_for(150ms); buzzer = 1;
    idle_timer.reset();
}

void handle_lock() {
    state_mutex.lock();
    current_state = STATE_LOCKED;
    state_mutex.unlock();
    safe_printf("System locked.\n");
    greenLED = 0;
    event_queue.call(sound_alert, 1);
    idle_timer.reset();
}

// === LED Status Updates (Called by Event Queue) ===
void update_leds() {
    static uint32_t led_counter = 0;
    led_counter++;

    state_mutex.lock();
    SystemState state_snapshot = current_state;
    state_mutex.unlock();

    redLED = 0; blueLED = 0; greenLED = 0; // Reset LEDs

    switch (state_snapshot) {
        case STATE_SLEEP: break; // All off
        case STATE_LOCKED: blueLED = (led_counter % 20 < 10); break; // Slow blue blink
        case STATE_UNLOCKED: greenLED = 1; break; // Solid green
        case STATE_CODE_ENTRY: blueLED = (led_counter % 10 < 5); break; // Faster blue blink
        case STATE_ALERT: redLED = (led_counter % 4 < 2); break; // Fast red flash
    }
}

// === Main Program ===
int main() {
    safe_printf("\n=== Smart Locker System begin ===\n");
    safe_printf("Mbed OS version: %d.%d.%d\n", MBED_MAJOR_VERSION, MBED_MINOR_VERSION, MBED_PATCH_VERSION);
    
    init_hardware();

    safe_printf("System ready.\n");
    safe_printf("Default unlock code sequence: Wake, Unlock, Wake, Unlock (Button A4, A0, A4, A0)\n\n");

    // Debug timer for periodic status updates
    Timer debug_timer;
    debug_timer.start();
    debug_timer_running = true;

    while (true) {
        state_mutex.lock();
        SystemState state_snapshot = current_state;
        state_mutex.unlock();

        switch (state_snapshot) {
            case STATE_CODE_ENTRY:
                process_code_entry();
                break;
            case STATE_LOCKED:
            case STATE_UNLOCKED:
                if (std::chrono::duration_cast<std::chrono::milliseconds>(idle_timer.elapsed_time()).count() > IDLE_TIMEOUT_MS) {
                    safe_printf("Idle timeout reached.\n");
                    if (state_snapshot == STATE_UNLOCKED) handle_lock();
                    enter_sleep_mode();
                }
                break;
            case STATE_ALERT:
                if (std::chrono::duration_cast<std::chrono::milliseconds>(idle_timer.elapsed_time()).count() > IDLE_TIMEOUT_MS * 2) {
                    safe_printf("Alert state idle timeout. Entering sleep.\n");
                    enter_sleep_mode();
                }
                break;
            case STATE_SLEEP:
                // Handled by sleep() call at the end of the loop
                break;
            default: break;
        }
        
        if (state_snapshot != STATE_SLEEP) {
             check_alerts(); 
        }

        if (debug_timer_running && std::chrono::duration_cast<std::chrono::seconds>(debug_timer.elapsed_time()).count() >= 5) {
            debug_timer.reset();
            sensor_mutex.lock();
            float temp_val = current_temperature;
            int motion_val = motion_magnitude;
            bool temp_alert_val = temperature_alert; // snapshot volatile bools
            bool tamper_alert_val = tamper_alert;
            bool tilt_alert_val = tilt_alert;
            sensor_mutex.unlock();
            
            state_mutex.lock();
            SystemState current_s = current_state;
            state_mutex.unlock();
            
            safe_printf("Status: T=%.1fC, Motion=%d, State=%d, Alerts(T:%d,M:%d,L:%d)\n",
                   temp_val, motion_val, current_s,
                   temp_alert_val, tamper_alert_val, tilt_alert_val);
        }

        if (state_snapshot == STATE_SLEEP) {
            sleep(); // Mbed OS sleep, wakes on interrupt
        } else {
            ThisThread::sleep_for(50ms);
        }
    }
}