#include "ld2420_radar.h"

Ld2420Radar::Ld2420Radar() {
    reset_runtime_state();
}

bool Ld2420Radar::begin(HardwareSerial &serial, const RadarConfig &config) {
    serial_port = &serial;
    cfg = config;

    reset_runtime_state();

    serial_port -> begin(cfg.baud_rate, SERIAL_8N1, cfg.uart_rx_pin, cfg.uart_tx_pin);
    initialized = true;

    return true;
}

void Ld2420Radar::reset_runtime_state() {
    line_length = 0;
    line_buffer[0] = '\0';

    online = false;
    raw_presence = false;
    presence = false;

    raw_distance_m = 0.0f;
    filtered_distance_m = 0.0f;

    current_speed_m_s = 0.0f;
    activity_score_current = 0;

    last_frame_ms = 0;
    last_presence_ms = 0;
    last_distance_sample_ms = 0;

    last_distance_for_speed_m = 0.0f;
    has_distance_for_speed = false;

    second_scores_head = 0;
    second_scores_count = 0;
    last_second_push_ms = 0;

    for (size_t i = 0; i < SECOND_HISTORY_SIZE; ++i) {
        second_scores[i] = 0;
    }

    activity_score_dynamics = "constant";
}

void Ld2420Radar::update() {
    if (!initialized || serial_port == nullptr) {
        return;
    }

    while (serial_port -> available() > 0) {
        char ch = static_cast<char>(serial_port -> read());
        process_incoming_byte(ch);
    }

    uint32_t now_ms = millis();

    update_online_state(now_ms);
    update_presence_state(now_ms);
    update_second_history(now_ms);

    activity_score_dynamics = compute_dynamics();
}

void Ld2420Radar::process_incoming_byte(char ch) {
    if (ch == '\r' || ch == '\n') {
        if (line_length > 0) {
            line_buffer[line_length] = '\0';
            process_line(line_buffer);
            line_length = 0;
            line_buffer[0] = '\0';
        }
        return;
    }

    if (line_length + 1 >= LINE_BUFFER_SIZE) {
        line_length = 0;
        line_buffer[0] = '\0';
        return;
    }

    line_buffer[line_length++] = ch;
}

void Ld2420Radar::process_line(const char *line_cstr) {
    if (line_cstr == nullptr || line_cstr[0] == '\0') {
        return;
    }

    String line = String(line_cstr);
    line.trim();

    if (line.length() == 0) {
        return;
    }

    bool parsed = parse_ascii_line(line);

    if (parsed) {
        last_frame_ms = millis();
        online = true;
    } else if (cfg.enable_debug_unknown_lines) {
        Serial.print("LD2420 unknown line: ");
        Serial.println(line);
    }
}

bool Ld2420Radar::parse_ascii_line(const String &raw_line) {
    String line = normalize_line(raw_line);
    line.toLowerCase();

    /*
        Поддерживаем разные возможные названия полей.
        Это сделано специально, чтобы не завязаться
        на единственный вариант строки.
    */
    static const char *presence_keys[] = {
        "presence", "pres", "occupancy", "occupied", "target",
        "detected", "detect", "exist", "state"
    };

    static const char *distance_meter_keys[] = {
        "distance_m", "dist_m", "dis_m", "range_m", "r_m", "meter", "meters"
    };

    static const char *distance_cm_keys[] = {
        "distance_cm", "dist_cm", "dis_cm", "range_cm", "cm"
    };

    static const char *distance_generic_keys[] = {
        "distance", "dist", "dis", "range", "r"
    };

    static const char *zone_keys[] = {
        "zone", "gate", "area", "distance_zone"
    };

    static const char *move_keys[] = {
        "move", "moving", "motion", "micro_move", "micro", "still"
    };

    bool got_anything = false;
    uint32_t now_ms = millis();

    bool explicit_presence = false;
    bool has_explicit_presence = extract_bool_by_keys(line, presence_keys, sizeof(presence_keys) / sizeof(presence_keys[0]), explicit_presence);

    float distance_m = 0.0f;
    bool has_distance_m = extract_number_by_keys(line, distance_meter_keys, sizeof(distance_meter_keys) / sizeof(distance_meter_keys[0]), distance_m);

    float distance_cm = 0.0f;
    bool has_distance_cm = extract_number_by_keys(line, distance_cm_keys, sizeof(distance_cm_keys) / sizeof(distance_cm_keys[0]), distance_cm);

    float distance_generic = 0.0f;
    bool has_distance_generic = extract_number_by_keys(line, distance_generic_keys, sizeof(distance_generic_keys) / sizeof(distance_generic_keys[0]), distance_generic);

    float zone = 0.0f;
    bool has_zone = extract_number_by_keys(line, zone_keys, sizeof(zone_keys) / sizeof(zone_keys[0]), zone);

    float move_value = 0.0f;
    bool has_move_value = extract_number_by_keys(line, move_keys, sizeof(move_keys) / sizeof(move_keys[0]), move_value);

    /*
        1. Приоритет:
           явная дистанция в метрах
           -> дистанция в сантиметрах
           -> generic distance
           -> zone/gate через калибровку
    */
    if (has_distance_m) {
        if (is_distance_plausible(distance_m)) {
            raw_distance_m = distance_m;
            apply_new_distance(distance_m, now_ms);
            got_anything = true;
        }
    } else if (has_distance_cm) {
        float converted = distance_cm / 100.0f;

        if (is_distance_plausible(converted)) {
            raw_distance_m = converted;
            apply_new_distance(converted, now_ms);
            got_anything = true;
        }
    } else if (has_distance_generic) {
        float converted = distance_generic;

        /*
            Если значение выглядит как сантиметры — конвертируем.
            Если выглядит как зона — позже это перекроется has_zone.
        */
        if (converted > 20.0f) {
            converted /= 100.0f;
        }

        if (is_distance_plausible(converted)) {
            raw_distance_m = converted;
            apply_new_distance(converted, now_ms);
            got_anything = true;
        }
    } else if (has_zone) {
        if (zone >= cfg.min_valid_zone && zone <= cfg.max_valid_zone) {
            float converted = zone_to_meters(zone);

            if (is_distance_plausible(converted)) {
                raw_distance_m = converted;
                apply_new_distance(converted, now_ms);
                got_anything = true;
            }
        }
    }

    /*
        2. Presence:
           - если есть явный флаг, используем его;
           - иначе presence выводим из валидной дистанции
             или из move_value > 0.
    */
    if (has_explicit_presence) {
        raw_presence = explicit_presence;
        got_anything = true;
    } else {
        bool inferred_presence = false;

        if (got_anything && filtered_distance_m > 0.01f) {
            inferred_presence = true;
        }

        if (has_move_value && move_value > 0.0f) {
            inferred_presence = true;
        }

        raw_presence = inferred_presence;
    }

    if (raw_presence) {
        last_presence_ms = now_ms;
    }

    activity_score_current = compute_current_activity_score(raw_presence, filtered_distance_m, current_speed_m_s);

    return got_anything || has_explicit_presence || has_move_value;
}

void Ld2420Radar::update_online_state(uint32_t now_ms) {
    if (last_frame_ms == 0) {
        online = false;
        return;
    }

    online = (now_ms - last_frame_ms <= cfg.stale_after_ms);
}

void Ld2420Radar::update_presence_state(uint32_t now_ms) {
    if (!online) {
        presence = false;
        return;
    }

    if (raw_presence) {
        presence = true;
        return;
    }

    if (last_presence_ms != 0 && (now_ms - last_presence_ms <= cfg.presence_hold_ms)) {
        presence = true;
        return;
    }

    presence = false;
}

void Ld2420Radar::update_second_history(uint32_t now_ms) {
    if (last_second_push_ms == 0) {
        last_second_push_ms = now_ms;
        return;
    }

    while (now_ms - last_second_push_ms >= 1000) {
        last_second_push_ms += 1000;

        uint8_t score_to_push = online ? activity_score_current : 0;

        second_scores[second_scores_head] = score_to_push;
        second_scores_head = (second_scores_head + 1) % SECOND_HISTORY_SIZE;

        if (second_scores_count < SECOND_HISTORY_SIZE) {
            ++second_scores_count;
        }
    }
}

void Ld2420Radar::apply_new_distance(float distance_m, uint32_t now_ms) {
    if (!is_distance_plausible(distance_m)) {
        return;
    }

    if (filtered_distance_m <= 0.0f) {
        filtered_distance_m = distance_m;
    } else {
        filtered_distance_m =
            (cfg.distance_ema_alpha * distance_m) +
            ((1.0f - cfg.distance_ema_alpha) * filtered_distance_m);
    }

    if (has_distance_for_speed && last_distance_sample_ms != 0 && now_ms > last_distance_sample_ms) {
        float dt_s = static_cast<float>(now_ms - last_distance_sample_ms) / 1000.0f;

        if (dt_s > 0.02f) {
            float speed = fabsf(distance_m - last_distance_for_speed_m) / dt_s;
            current_speed_m_s = speed;
        }
    } else {
        current_speed_m_s = 0.0f;
    }

    last_distance_for_speed_m = distance_m;
    last_distance_sample_ms = now_ms;
    has_distance_for_speed = true;
}

uint8_t Ld2420Radar::compute_current_activity_score(bool detected_presence, float distance_m, float speed_m_s) const {
    if (!detected_presence) {
        return 0;
    }

    /*
        Основная идея:
        - наличие человека уже даёт маленький базовый балл;
        - движение по изменению дистанции даёт основной вклад;
        - очень близкий / очень дальний человек не должен ломать шкалу.
    */
    float base_score = 1.5f;

    float motion_score = 0.0f;
    if (speed_m_s > cfg.activity_min_speed_m_s) {
        float span = cfg.activity_max_speed_m_s - cfg.activity_min_speed_m_s;

        if (span < 0.001f) {
            span = 0.001f;
        }

        float normalized =
            (speed_m_s - cfg.activity_min_speed_m_s) / span;

        normalized = clampf(normalized, 0.0f, 1.0f);
        motion_score = normalized * 8.5f;
    }

    float distance_bonus = 0.0f;
    if (distance_m > 0.0f && distance_m < 6.0f) {
        distance_bonus = 0.5f;
    }

    int score = static_cast<int>(roundf(base_score + motion_score + distance_bonus));
    score = clampi(score, 0, 10);

    return static_cast<uint8_t>(score);
}

uint8_t Ld2420Radar::compute_minute_activity_score() const {
    if (second_scores_count == 0) {
        return 0;
    }

    uint16_t samples = min<uint16_t>(60, second_scores_count);
    uint32_t sum = 0;

    for (uint16_t i = 0; i < samples; ++i) {
        int idx = static_cast<int>(second_scores_head) - 1 - i;
        if (idx < 0) {
            idx += SECOND_HISTORY_SIZE;
        }

        sum += second_scores[idx];
    }

    int avg = static_cast<int>(roundf(static_cast<float>(sum) / static_cast<float>(samples)));
    avg = clampi(avg, 0, 10);

    return static_cast<uint8_t>(avg);
}

String Ld2420Radar::compute_dynamics() const {
    /*
        Оцениваем 10 минут по минутным средним.
    */
    float minute_avg[10];
    int valid_windows = 0;

    for (int minute_index = 0; minute_index < 10; ++minute_index) {
        int start_offset = minute_index * 60;
        int end_offset = start_offset + 60;

        if (second_scores_count < static_cast<uint16_t>(start_offset + 1)) {
            break;
        }

        uint32_t sum = 0;
        int count = 0;

        for (int sec = start_offset; sec < end_offset; ++sec) {
            if (second_scores_count < static_cast<uint16_t>(sec + 1)) {
                break;
            }

            int idx = static_cast<int>(second_scores_head) - 1 - sec;
            if (idx < 0) {
                idx += SECOND_HISTORY_SIZE;
            }

            sum += second_scores[idx];
            ++count;
        }

        if (count > 0) {
            minute_avg[valid_windows] = static_cast<float>(sum) / static_cast<float>(count);
            ++valid_windows;
        }
    }

    if (valid_windows < 3) {
        return "constant";
    }

    /*
        Разворачиваем хронологически: oldest -> newest
    */
    float ordered[10];
    for (int i = 0; i < valid_windows; ++i) {
        ordered[i] = minute_avg[valid_windows - 1 - i];
    }

    float sum_y = 0.0f;
    float min_y = ordered[0];
    float max_y = ordered[0];

    for (int i = 0; i < valid_windows; ++i) {
        sum_y += ordered[i];
        if (ordered[i] < min_y) min_y = ordered[i];
        if (ordered[i] > max_y) max_y = ordered[i];
    }

    float mean_y = sum_y / static_cast<float>(valid_windows);

    float variance = 0.0f;
    float sum_x = 0.0f;
    float sum_x2 = 0.0f;
    float sum_xy = 0.0f;

    for (int i = 0; i < valid_windows; ++i) {
        float x = static_cast<float>(i);
        float y = ordered[i];

        float dy = y - mean_y;
        variance += dy * dy;

        sum_x += x;
        sum_x2 += x * x;
        sum_xy += x * y;
    }

    float stddev = sqrtf(variance / static_cast<float>(valid_windows));
    float range = max_y - min_y;

    float n = static_cast<float>(valid_windows);
    float denom = (n * sum_x2) - (sum_x * sum_x);

    float slope = 0.0f;
    if (fabsf(denom) > 0.0001f) {
        slope = ((n * sum_xy) - (sum_x * sum_y)) / denom;
    }

    /*
        Классификация:
        - почти не меняется -> constant
        - заметный устойчивый рост -> increasing
        - заметное устойчивое падение -> decreasing
        - иначе -> variable
    */
    if (range <= 1.2f || stddev <= 0.6f) {
        return "constant";
    }

    if (slope >= 0.22f) {
        return "increasing";
    }

    if (slope <= -0.22f) {
        return "decreasing";
    }

    return "variable";
}

String Ld2420Radar::get_state_json() const {
    String json = "{";
    json += "\"online\":";
    json += (online ? "true" : "false");
    json += ",";
    json += "\"presence\":";
    json += (presence ? "true" : "false");
    json += ",";
    json += "\"activity_score\":";
    json += String(compute_minute_activity_score());
    json += ",";
    json += "\"activity_score_current\":";
    json += String(activity_score_current);
    json += ",";
    json += "\"activity_score_dynamics\":\"";
    json += activity_score_dynamics;
    json += "\",";
    json += "\"distance_m\":";
    json += String((presence && online) ? filtered_distance_m : 0.0f, 2);
    json += "}";

    return json;
}

String Ld2420Radar::normalize_line(const String &line) const {
    String out = line;

    out.replace('\t', ' ');
    out.replace(';', ' ');
    out.replace('|', ' ');
    out.replace(',', ' ');
    out.replace(':', '=');

    while (out.indexOf("  ") >= 0) {
        out.replace("  ", " ");
    }

    out.trim();
    return out;
}

bool Ld2420Radar::extract_number_by_keys(const String &line, const char *keys[], size_t key_count, float &value_out) const {
    for (size_t i = 0; i < key_count; ++i) {
        if (parse_number_after_key(line, String(keys[i]), value_out)) {
            return true;
        }
    }
    return false;
}

bool Ld2420Radar::extract_bool_by_keys(const String &line, const char *keys[], size_t key_count, bool &value_out) const {
    for (size_t i = 0; i < key_count; ++i) {
        if (parse_bool_after_key(line, String(keys[i]), value_out)) {
            return true;
        }
    }
    return false;
}

bool Ld2420Radar::parse_number_after_key(const String &line, const String &key, float &value_out) const {
    int key_pos = line.indexOf(key);
    if (key_pos < 0) {
        return false;
    }

    int eq_pos = line.indexOf('=', key_pos + key.length());
    if (eq_pos < 0) {
        return false;
    }

    int start = eq_pos + 1;
    while (start < static_cast<int>(line.length()) && line[start] == ' ') {
        ++start;
    }

    if (start >= static_cast<int>(line.length())) {
        return false;
    }

    int end = start;
    bool has_digit = false;

    if (line[end] == '+' || line[end] == '-') {
        ++end;
    }

    while (end < static_cast<int>(line.length())) {
        char ch = line[end];

        if ((ch >= '0' && ch <= '9') || ch == '.') {
            has_digit = true;
            ++end;
            continue;
        }

        break;
    }

    if (!has_digit) {
        return false;
    }

    value_out = line.substring(start, end).toFloat();
    return true;
}

bool Ld2420Radar::parse_bool_after_key(const String &line, const String &key, bool &value_out) const {
    int key_pos = line.indexOf(key);
    if (key_pos < 0) {
        return false;
    }

    int eq_pos = line.indexOf('=', key_pos + key.length());
    if (eq_pos < 0) {
        return false;
    }

    int start = eq_pos + 1;
    while (start < static_cast<int>(line.length()) && line[start] == ' ') {
        ++start;
    }

    if (start >= static_cast<int>(line.length())) {
        return false;
    }

    String tail = line.substring(start);
    tail.trim();
    tail.toLowerCase();

    if (tail.startsWith("1") || tail.startsWith("true") || tail.startsWith("on") || tail.startsWith("yes") || tail.startsWith("detect")) {
        value_out = true;
        return true;
    }

    if (tail.startsWith("0") || tail.startsWith("false") || tail.startsWith("off") || tail.startsWith("no") || tail.startsWith("clear")) {
        value_out = false;
        return true;
    }

    return false;
}

float Ld2420Radar::zone_to_meters(float zone) const {
    return (zone * cfg.zone_to_meter_k) + cfg.zone_to_meter_b;
}

bool Ld2420Radar::is_distance_plausible(float distance_m) const {
    return distance_m >= 0.0f && distance_m <= 8.5f;
}

float Ld2420Radar::clampf(float value, float min_value, float max_value) const {
    if (value < min_value) return min_value;
    if (value > max_value) return max_value;
    return value;
}

int Ld2420Radar::clampi(int value, int min_value, int max_value) const {
    if (value < min_value) return min_value;
    if (value > max_value) return max_value;
    return value;
}