#include "ld2420_radar.h"

#include <math.h>

/*
    =========================
    UTILS
    =========================
*/

static uint8_t clamp_score_0_10(int value) {
    if (value < 0) return 0;
    if (value > 10) return 10;
    return (uint8_t)value;
}


/*
    =========================
    CTOR
    =========================
*/

Ld2420Radar::Ld2420Radar() {
    memset(&state, 0, sizeof(state));
}


/*
    =========================
    INIT
    =========================
*/

void Ld2420Radar::begin(HardwareSerial &uart_port, const RadarConfig &cfg) {
    uart = &uart_port;
    config = cfg;

    uart -> begin(config.baud_rate, SERIAL_8N1, config.uart_rx_pin, config.uart_tx_pin);

    last_data_time_ms = 0;
    last_presence_time_ms = 0;

    last_distance_m = 0.0f;
    last_update_time_ms = millis();

    memset(activity_history, 0, sizeof(activity_history));
    activity_history_index = 0;
    activity_history_filled = false;
}


/*
    =========================
    MAIN UPDATE
    =========================
*/

void Ld2420Radar::update() {
    read_uart();
    update_online_state();
    update_presence();
    update_motion();
    update_activity();
}


/*
    =========================
    UART PARSING
    =========================
*/

void Ld2420Radar::read_uart() {
    while (uart -> available()) {
        char c = uart -> read();

        if (c == '\n') {
            process_line(line_buffer);
            line_buffer = "";
        } else if (c != '\r') {
            line_buffer += c;
        }
    }
}


void Ld2420Radar::process_line(const String &line) {
    /*
        Ожидаем формат типа:
        "Range 42"
    */

    if (line.startsWith("Range ")) {
        int zone = line.substring(6).toInt();

        if (zone >= config.min_valid_zone && zone <= config.max_valid_zone) {
            handle_range(zone);
        }

        last_data_time_ms = millis();
    }
}


/*
    =========================
    RANGE HANDLING
    =========================
*/

void Ld2420Radar::handle_range(int zone) {
    float distance = config.zone_to_meter_k * zone + config.zone_to_meter_b;

    /*
        EMA сглаживание дистанции
    */
    state.distance_m =
        config.distance_ema_alpha * distance +
        (1.0f - config.distance_ema_alpha) * state.distance_m;

    last_presence_time_ms = millis();
    state.presence = true;
}


/*
    =========================
    ONLINE / PRESENCE
    =========================
*/

void Ld2420Radar::update_online_state() {
    uint32_t now = millis();

    state.online = (now - last_data_time_ms <= config.stale_after_ms);
}


void Ld2420Radar::update_presence() {
    uint32_t now = millis();

    if (now - last_presence_time_ms > config.presence_hold_ms) {
        state.presence = false;
    }
}


/*
    =========================
    MOTION / SPEED
    =========================
*/

void Ld2420Radar::update_motion() {
    uint32_t now = millis();
    float dt = (now - last_update_time_ms) / 1000.0f;

    if (dt <= 0.0f) return;

    float speed = (state.distance_m - last_distance_m) / dt;

    /*
        EMA по скорости
    */
    state.speed_m_s =
        config.speed_ema_alpha * speed +
        (1.0f - config.speed_ema_alpha) * state.speed_m_s;

    last_distance_m = state.distance_m;
    last_update_time_ms = now;
}


/*
    =========================
    ACTIVITY
    =========================
*/

uint8_t Ld2420Radar::speed_to_activity_score(float abs_speed) const {
    if (!state.presence) return 0;

    if (abs_speed <= config.activity_min_speed_m_s) return 1;
    if (abs_speed >= config.activity_max_speed_m_s) return 10;

    float norm =
        (abs_speed - config.activity_min_speed_m_s) /
        (config.activity_max_speed_m_s - config.activity_min_speed_m_s);

    int score = 1 + (int)(norm * 9.0f + 0.5f);

    return clamp_score_0_10(score);
}


void Ld2420Radar::push_activity_sample(uint8_t score) {
    activity_history[activity_history_index] = score;

    activity_history_index++;
    if (activity_history_index >= ACTIVITY_HISTORY_SIZE) {
        activity_history_index = 0;
        activity_history_filled = true;
    }
}


uint8_t Ld2420Radar::calculate_activity_average() const {
    uint32_t sum = 0;
    uint16_t count = 0;

    uint16_t max_count = activity_history_filled
        ? ACTIVITY_HISTORY_SIZE
        : activity_history_index;

    for (uint16_t i = 0; i < max_count; i++) {
        sum += activity_history[i];
        count++;
    }

    if (count == 0) return 0;

    return clamp_score_0_10(sum / count);
}


ActivityDynamics Ld2420Radar::calculate_activity_dynamics() const {
    uint16_t max_count = activity_history_filled
        ? ACTIVITY_HISTORY_SIZE
        : activity_history_index;

    if (max_count < 20) {
        return ACTIVITY_DYNAMICS_CONSTANT;
    }

    uint16_t half = max_count / 2;

    uint32_t sum_old = 0;
    uint32_t sum_new = 0;

    for (uint16_t i = 0; i < half; i++) {
        sum_old += activity_history[i];
    }

    for (uint16_t i = half; i < max_count; i++) {
        sum_new += activity_history[i];
    }

    float avg_old = (float)sum_old / half;
    float avg_new = (float)sum_new / (max_count - half);

    float diff = avg_new - avg_old;

    /*
        variance → детекция "variable"
    */
    float variance = 0;
    float avg_total = (avg_old + avg_new) * 0.5f;

    for (uint16_t i = 0; i < max_count; i++) {
        float d = activity_history[i] - avg_total;
        variance += d * d;
    }
    variance /= max_count;

    if (variance > 6.0f) {
        return ACTIVITY_DYNAMICS_VARIABLE;
    }

    if (fabs(diff) < 0.5f) {
        return ACTIVITY_DYNAMICS_CONSTANT;
    }

    if (diff > 0) {
        return ACTIVITY_DYNAMICS_INCREASING;
    } else {
        return ACTIVITY_DYNAMICS_DECREASING;
    }
}


void Ld2420Radar::update_activity() {
    float abs_speed = fabs(state.speed_m_s);

    state.activity_score_current = speed_to_activity_score(abs_speed);

    push_activity_sample(state.activity_score_current);

    state.activity_score = calculate_activity_average();

    state.activity_score_dynamics = calculate_activity_dynamics();
}


/*
    =========================
    JSON OUTPUT
    =========================
*/

String Ld2420Radar::get_state_json() const {
    String json;
    json.reserve(192);

    json += "{";

    json += "\"online\":";
    json += (state.online ? "true" : "false");

    json += ",\"presence\":";
    json += (state.presence ? "true" : "false");

    json += ",\"activity_score\":";
    json += String(clamp_score_0_10(state.activity_score));

    json += ",\"activity_score_current\":";
    json += String(clamp_score_0_10(state.activity_score_current));

    json += ",\"activity_score_dynamics\":\"";
    json += activity_dynamics_to_string(state.activity_score_dynamics);
    json += "\"";

    json += ",\"distance_m\":";
    json += String(state.distance_m, 2);

    json += "}";

    return json;
}


/*
    =========================
    HELPERS
    =========================
*/

const char *Ld2420Radar::activity_dynamics_to_string(ActivityDynamics d) const {
    switch (d) {
        case ACTIVITY_DYNAMICS_INCREASING: return "increasing";
        case ACTIVITY_DYNAMICS_DECREASING: return "decreasing";
        case ACTIVITY_DYNAMICS_VARIABLE:   return "variable";
        default:                           return "constant";
    }
}