#include "system/includes.h"
#include "radar_manager.h"
#include <string.h>
#include "app_log.h"

static struct radar_sensor_data current_data;
static struct radar_sensor_data consumed_data;

static OS_MUTEX radar_manager_mutex;
static u8 radar_manager_mutex_ready;
static u8 has_consumed_snapshot;


static u16 abs_diff_u16(u16 a, u16 b)
{
    return (a >= b) ? (u16)(a - b) : (u16)(b - a);
}


static u8 abs_diff_u8(u8 a, u8 b)
{
    return (a >= b) ? (u8)(a - b) : (u8)(b - a);
}


/*
 * 保留 ESP32 当前最终业务判定：
 * heart:  > 0 && < 200 bpm
 * breath: > 0 && <= 60 /min
 *
 * AC79 内部使用 x10 整数。
 */
static u8 heart_rate_valid(u16 value_x10)
{
    return (value_x10 > 0 && value_x10 < 2000) ? 1 : 0;
}


static u8 breath_rate_valid(u16 value_x10)
{
    return (value_x10 > 0 && value_x10 <= 600) ? 1 : 0;
}


/*
 * 调用者必须持有 radar_manager_mutex。
 */
static void build_change_flags_locked(
    struct radar_change_flags *flags
)
{
    memset(flags, 0, sizeof(*flags));

    if (!has_consumed_snapshot) {

        if (current_data.last_update_ms != 0) {
            flags->any_changed = 1;
        }

        return;
    }


    flags->presence_changed =
        current_data.presence != consumed_data.presence;

    flags->motion_changed =
        current_data.motion != consumed_data.motion;

    flags->sleep_state_changed =
        current_data.sleep_state != consumed_data.sleep_state;

    /*
     * ESP32 当前 meaningful-change 阈值：
     * body movement >= 5
     * distance      >= 10 cm
     * heart         >= 3 bpm
     * breath        >= 2 /min
     */
    flags->body_movement_changed =
        abs_diff_u8(
            current_data.body_movement,
            consumed_data.body_movement
        ) >= 5;

    flags->distance_changed =
        abs_diff_u16(
            current_data.distance_cm,
            consumed_data.distance_cm
        ) >= 10;

    flags->heart_rate_changed =
        abs_diff_u16(
            current_data.heart_rate_x10,
            consumed_data.heart_rate_x10
        ) >= 30;

    flags->breath_rate_changed =
        abs_diff_u16(
            current_data.breath_rate_x10,
            consumed_data.breath_rate_x10
        ) >= 20;

    flags->abnormal_state_changed =
        current_data.abnormal_state !=
        consumed_data.abnormal_state;


    flags->any_changed =
        flags->presence_changed ||
        flags->motion_changed ||
        flags->sleep_state_changed ||
        flags->body_movement_changed ||
        flags->distance_changed ||
        flags->heart_rate_changed ||
        flags->breath_rate_changed ||
        flags->abnormal_state_changed;
}


int radar_manager_init(void)
{
    memset(&current_data, 0, sizeof(current_data));
    memset(&consumed_data, 0, sizeof(consumed_data));

    has_consumed_snapshot = 0;

    if (!radar_manager_mutex_ready) {
        os_mutex_create(&radar_manager_mutex);
        radar_manager_mutex_ready = 1;
    }

    APP_LOG("[RADAR_MGR] init success\n");

    return 0;
}


int radar_manager_update(
    const struct radar_manager_input *input
)
{
    if (!input) {
        return -1;
    }

    if (!radar_manager_mutex_ready) {
        return -2;
    }


    os_mutex_pend(&radar_manager_mutex, 0);


    current_data.presence = input->presence;
    current_data.motion = input->motion;
    current_data.body_movement = input->body_movement;

    current_data.distance_cm = input->distance_cm;

    current_data.heart_rate_x10 = input->heart_rate_x10;
    current_data.breath_rate_x10 = input->breath_rate_x10;

    current_data.heart_valid =
        heart_rate_valid(input->heart_rate_x10);

    current_data.breath_valid =
        breath_rate_valid(input->breath_rate_x10);

    current_data.sleep_state = input->sleep_state;

    current_data.pos_x_mm = input->pos_x_mm;
    current_data.pos_y_mm = input->pos_y_mm;
    current_data.pos_z_mm = input->pos_z_mm;

    current_data.abnormal_state = input->abnormal_state;

    current_data.last_update_ms = input->update_ms;


    os_mutex_post(&radar_manager_mutex);


    return 0;
}


int radar_manager_get_sensor_data(
    struct radar_sensor_data *out
)
{
    if (!out) {
        return -1;
    }

    if (!radar_manager_mutex_ready) {
        return -2;
    }


    os_mutex_pend(&radar_manager_mutex, 0);

    memcpy(out, &current_data, sizeof(*out));

    os_mutex_post(&radar_manager_mutex);


    return 0;
}


int radar_manager_get_change_flags(
    struct radar_change_flags *out
)
{
    if (!out) {
        return -1;
    }

    if (!radar_manager_mutex_ready) {
        return -2;
    }


    os_mutex_pend(&radar_manager_mutex, 0);

    build_change_flags_locked(out);

    os_mutex_post(&radar_manager_mutex);


    return 0;
}


int radar_manager_get_report_snapshot(
    u32 now_ms,
    u32 max_age_ms,
    struct radar_report_snapshot *out
)
{
    struct radar_change_flags flags;
    u32 age;

    if (!out) {
        return -1;
    }

    if (!radar_manager_mutex_ready) {
        return -2;
    }


    memset(out, 0, sizeof(*out));


    os_mutex_pend(&radar_manager_mutex, 0);


    memcpy(
        &out->sensor,
        &current_data,
        sizeof(out->sensor)
    );

    build_change_flags_locked(&flags);


    out->has_person =
        current_data.presence != 0;

    /*
     * 先保持 ESP32 当前语义：
     * presence != 0 或 sleep_state > 0
     */
    out->bed_occupied =
        (current_data.presence != 0) ||
        (current_data.sleep_state > 0);

    out->has_valid_vitals =
        current_data.heart_valid ||
        current_data.breath_valid;

    out->has_meaningful_change =
        flags.any_changed;


    if (current_data.last_update_ms != 0) {

        age =
            (u32)(
                now_ms -
                current_data.last_update_ms
            );

        out->sample_age_ms = age;

        if (age <= max_age_ms) {
            out->has_fresh_sample = 1;
        }
    }


    os_mutex_post(&radar_manager_mutex);


    return 0;
}


int radar_manager_mark_snapshot_consumed(void)
{
    if (!radar_manager_mutex_ready) {
        return -1;
    }


    os_mutex_pend(&radar_manager_mutex, 0);

    memcpy(
        &consumed_data,
        &current_data,
        sizeof(consumed_data)
    );

    has_consumed_snapshot = 1;

    os_mutex_post(&radar_manager_mutex);


    return 0;
}