#include "ld2420_radar.h"

Ld2420Radar::Ld2420Radar() {
    uart = nullptr;
    reset_runtime_state();
}

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

    if (config.median_window_size == 0) {
        config.median_window_size = 1;
    }
    if (config.median_window_size > max_supported_median_window) {
        config.median_window_size = max_supported_median_window;
    }

    line_buffer.reserve(96);

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

    reset_runtime_state();

    uint32_t now_ms = millis();
    state.last_update_ms = now_ms;
    activity_bucket_start_ms = now_ms;
}

void Ld2420Radar::update() {
    read_uart_lines();
    apply_presence_hold();
    update_online_state();
    update_activity_history();
    update_activity_outputs();
}

const RadarState &Ld2420Radar::get_state() const {
    return state;
}

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(state.activity_score);

    json += ",\"activity_score_current\":";
    json += String(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;
}

void Ld2420Radar::set_config(const RadarConfig &new_config) {
    config = new_config;

    if (config.median_window_size == 0) {
        config.median_window_size = 1;
    }
    if (config.median_window_size > max_supported_median_window) {
        config.median_window_size = max_supported_median_window;
    }
}

const RadarConfig &Ld2420Radar::get_config() const {
    return config;
}

void Ld2420Radar::reset_runtime_state() {
    state = RadarState();

    line_buffer = "";

    zone_window_count = 0;
    zone_window_index = 0;
    for (uint8_t i = 0; i < max_supported_median_window; i++) {
        zone_window[i] = 0;
    }

    has_filtered_zone = false;
    last_filtered_zone = 0;

    has_smoothed_distance = false;
    smoothed_distance_m = 0.0f;

    has_smoothed_speed = false;
    smoothed_speed_m_s = 0.0f;

    for (uint8_t i = 0; i < activity_bucket_count; i++) {
        activity_buckets[i] = 0;
    }

    activity_bucket_start_ms = 0;
    activity_bucket_accum_sum = 0;
    activity_bucket_accum_count = 0;
    activity_bucket_index = 0;
    activity_history_filled = false;
}

void Ld2420Radar::read_uart_lines() {
    if (uart == nullptr) {
        return;
    }

    while (uart->available()) {
        char c = static_cast<char>(uart->read());

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

            if (line_buffer.length() > 120) {
                line_buffer = "";
                state.parse_error_counter++;
            }
        }
    }
}

void Ld2420Radar::handle_line(const String &line_raw) {
    String line = line_raw;
    line.trim();

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

    state.line_counter++;
    state.last_update_ms = millis();

    if (line == "ON") {
        handle_on_line();
        return;
    }

    if (line == "OFF") {
        handle_off_line();
        return;
    }

    if (line.startsWith("Range ")) {
        handle_range_line(line);
        return;
    }

    if (config.enable_debug_unknown_lines) {
        Serial.print("unknown_line: ");
        Serial.println(line);
    }

    state.parse_error_counter++;
}

void Ld2420Radar::handle_on_line() {
    state.raw_presence = true;
    state.last_on_ms = state.last_update_ms;
}

void Ld2420Radar::handle_off_line() {
    state.raw_presence = false;
    state.last_off_ms = state.last_update_ms;
}

void Ld2420Radar::handle_range_line(const String &line) {
    String value_str = line.substring(6);
    value_str.trim();

    int raw_zone_signed = value_str.toInt();
    if (raw_zone_signed < 0) {
        state.parse_error_counter++;
        return;
    }

    uint16_t raw_zone = static_cast<uint16_t>(raw_zone_signed);

    if (!is_zone_valid(raw_zone)) {
        state.ignored_range_counter++;
        return;
    }

    uint32_t now_ms = millis();

    state.raw_distance_zone = raw_zone;

    push_zone_sample(raw_zone);

    uint16_t median_zone = get_median_zone();
    uint16_t filtered_zone = apply_zone_step_limit(median_zone);

    state.filtered_distance_zone = filtered_zone;

    float raw_distance_m = zone_to_distance_m(filtered_zone);
    float filtered_distance_m = apply_distance_smoothing(raw_distance_m);

    update_speed(filtered_distance_m, now_ms);

    state.distance_m = filtered_distance_m;
    state.last_range_ms = now_ms;

    /*
        Если пришёл Range, считаем это достаточным признаком присутствия.
    */
    state.raw_presence = true;
    state.last_on_ms = now_ms;
}

void Ld2420Radar::push_zone_sample(uint16_t zone) {
    zone_window[zone_window_index] = zone;
    zone_window_index = (zone_window_index + 1) % config.median_window_size;

    if (zone_window_count < config.median_window_size) {
        zone_window_count++;
    }
}

uint16_t Ld2420Radar::get_median_zone() const {
    if (zone_window_count == 0) {
        return 0;
    }

    uint16_t temp[max_supported_median_window];

    for (uint8_t i = 0; i < zone_window_count; i++) {
        temp[i] = zone_window[i];
    }

    for (uint8_t i = 0; i < zone_window_count; i++) {
        for (uint8_t j = i + 1; j < zone_window_count; j++) {
            if (temp[j] < temp[i]) {
                uint16_t swap_tmp = temp[i];
                temp[i] = temp[j];
                temp[j] = swap_tmp;
            }
        }
    }

    return temp[zone_window_count / 2];
}

uint16_t Ld2420Radar::apply_zone_step_limit(uint16_t candidate_zone) {
    if (!has_filtered_zone) {
        has_filtered_zone = true;
        last_filtered_zone = candidate_zone;
        return candidate_zone;
    }

    int delta = static_cast<int>(candidate_zone) - static_cast<int>(last_filtered_zone);

    if (delta > static_cast<int>(config.max_zone_step_per_sample)) {
        candidate_zone = last_filtered_zone + config.max_zone_step_per_sample;
    } else if (delta < -static_cast<int>(config.max_zone_step_per_sample)) {
        candidate_zone = last_filtered_zone - config.max_zone_step_per_sample;
    }

    last_filtered_zone = candidate_zone;
    return candidate_zone;
}

float Ld2420Radar::zone_to_distance_m(uint16_t zone) const {
    float distance_m = static_cast<float>(zone) * config.zone_to_meter_k + config.zone_to_meter_b;

    if (distance_m < 0.0f) {
        distance_m = 0.0f;
    }

    return distance_m;
}

float Ld2420Radar::apply_distance_smoothing(float new_distance_m) {
    if (!has_smoothed_distance) {
        has_smoothed_distance = true;
        smoothed_distance_m = new_distance_m;
        return smoothed_distance_m;
    }

    smoothed_distance_m =
        config.distance_ema_alpha * new_distance_m +
        (1.0f - config.distance_ema_alpha) * smoothed_distance_m;

    return smoothed_distance_m;
}

void Ld2420Radar::update_speed(float filtered_distance_m, uint32_t now_ms) {
    if (state.last_range_ms == 0 || now_ms <= state.last_range_ms || state.distance_m < 0.0f) {
        state.radial_speed_m_s = 0.0f;
        has_smoothed_speed = false;
        smoothed_speed_m_s = 0.0f;
        return;
    }

    float dt_s = static_cast<float>(now_ms - state.last_range_ms) / 1000.0f;
    if (dt_s <= 0.0f) {
        return;
    }

    float raw_speed_m_s = (filtered_distance_m - state.distance_m) / dt_s;

    if (!has_smoothed_speed) {
        has_smoothed_speed = true;
        smoothed_speed_m_s = raw_speed_m_s;
    } else {
        smoothed_speed_m_s =
            config.speed_ema_alpha * raw_speed_m_s +
            (1.0f - config.speed_ema_alpha) * smoothed_speed_m_s;
    }

    state.radial_speed_m_s = smoothed_speed_m_s;
}

void Ld2420Radar::apply_presence_hold() {
    uint32_t now_ms = millis();

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

    if (state.last_on_ms > 0 && (now_ms - state.last_on_ms) <= config.presence_hold_ms) {
        state.presence = true;
    } else {
        state.presence = false;
        state.radial_speed_m_s = 0.0f;
    }
}

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

    state.stale = !(state.last_update_ms > 0 && (now_ms - state.last_update_ms) < config.stale_after_ms);
    state.online = !state.stale;
}

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

    if (abs_speed_m_s <= config.activity_min_speed_m_s) {
        return 1;
    }

    if (abs_speed_m_s >= config.activity_max_speed_m_s) {
        return 10;
    }

    float normalized =
        (abs_speed_m_s - config.activity_min_speed_m_s) /
        (config.activity_max_speed_m_s - config.activity_min_speed_m_s);

    int score = 1 + static_cast<int>(normalized * 9.0f + 0.5f);

    if (score < 1) score = 1;
    if (score > 10) score = 10;

    return static_cast<uint8_t>(score);
}

void Ld2420Radar::update_activity_history() {
    uint32_t now_ms = millis();

    if (activity_bucket_start_ms == 0) {
        activity_bucket_start_ms = now_ms;
    }

    /*
        Каждый update складываем текущий score в накопитель текущего 10-секундного окна.
    */
    uint8_t current_score = speed_to_activity_score(fabs(state.radial_speed_m_s));
    state.activity_score_current = current_score;

    activity_bucket_accum_sum += current_score;
    activity_bucket_accum_count++;

    /*
        Если прошло 10 секунд — фиксируем бакет.
        Если прошло сразу несколько бакетов подряд, тоже корректно продвигаем окно.
    */
    while ((now_ms - activity_bucket_start_ms) >= activity_bucket_duration_ms) {
        commit_current_activity_bucket(activity_bucket_start_ms + activity_bucket_duration_ms);
    }
}

void Ld2420Radar::commit_current_activity_bucket(uint32_t next_bucket_start_ms) {
    uint8_t average_score = 0;

    if (activity_bucket_accum_count > 0) {
        average_score = static_cast<uint8_t>(
            activity_bucket_accum_sum / activity_bucket_accum_count
        );
    }

    activity_buckets[activity_bucket_index] = average_score;

    activity_bucket_index++;
    if (activity_bucket_index >= activity_bucket_count) {
        activity_bucket_index = 0;
        activity_history_filled = true;
    }

    activity_bucket_accum_sum = 0;
    activity_bucket_accum_count = 0;
    activity_bucket_start_ms = next_bucket_start_ms;
}

void Ld2420Radar::update_activity_outputs() {
    /*
        activity_score = средняя активность за последнюю минуту.
        Так как бакет = 10 секунд, берём последние 6 бакетов.
    */
    uint8_t minute_bucket_count = 6;
    uint32_t sum = 0;
    uint8_t count = 0;

    for (uint8_t i = 0; i < minute_bucket_count; i++) {
        int index = static_cast<int>(activity_bucket_index) - 1 - i;
        if (index < 0) {
            index += activity_bucket_count;
        }

        /*
            Если история ещё не заполнена, не читаем незаписанные хвосты.
        */
        if (!activity_history_filled) {
            if (index >= static_cast<int>(activity_bucket_index)) {
                continue;
            }
        }

        sum += activity_buckets[index];
        count++;
    }

    if (count > 0) {
        state.activity_score = static_cast<uint8_t>(sum / count);
    } else {
        state.activity_score = state.activity_score_current;
    }

    state.activity_score_dynamics = calculate_activity_dynamics();
}

RadarActivityDynamics Ld2420Radar::calculate_activity_dynamics() const {
    /*
        Для оценки динамики за 10 минут используем 60 бакетов по 10 секунд.
        Делим их на две половины:
        - первые 5 минут
        - последние 5 минут

        Если разница средних заметна — increasing/decreasing.
        Если внутри ряда слишком большой разброс — variable.
        Иначе constant.
    */
    uint8_t available_count = activity_history_filled ? activity_bucket_count : activity_bucket_index;

    if (available_count < 12) {
        return RADAR_ACTIVITY_DYNAMICS_CONSTANT;
    }

    uint8_t half = available_count / 2;

    float first_sum = 0.0f;
    float second_sum = 0.0f;

    for (uint8_t i = 0; i < half; i++) {
        int index = static_cast<int>(activity_bucket_index) - available_count + i;
        while (index < 0) {
            index += activity_bucket_count;
        }
        first_sum += activity_buckets[index];
    }

    for (uint8_t i = half; i < available_count; i++) {
        int index = static_cast<int>(activity_bucket_index) - available_count + i;
        while (index < 0) {
            index += activity_bucket_count;
        }
        second_sum += activity_buckets[index];
    }

    float first_avg = first_sum / half;
    float second_avg = second_sum / (available_count - half);
    float delta = second_avg - first_avg;

    /*
        Проверка на "variable":
        если средний размах скачков между соседними бакетами большой,
        динамика считается рваной.
    */
    float diff_sum = 0.0f;
    uint8_t diff_count = 0;

    for (uint8_t i = 1; i < available_count; i++) {
        int prev_index = static_cast<int>(activity_bucket_index) - available_count + (i - 1);
        int curr_index = static_cast<int>(activity_bucket_index) - available_count + i;

        while (prev_index < 0) prev_index += activity_bucket_count;
        while (curr_index < 0) curr_index += activity_bucket_count;

        diff_sum += abs(static_cast<int>(activity_buckets[curr_index]) - static_cast<int>(activity_buckets[prev_index]));
        diff_count++;
    }

    float avg_diff = (diff_count > 0) ? (diff_sum / diff_count) : 0.0f;

    if (avg_diff >= 2.5f && fabs(delta) < 2.0f) {
        return RADAR_ACTIVITY_DYNAMICS_VARIABLE;
    }

    if (delta >= 1.5f) {
        return RADAR_ACTIVITY_DYNAMICS_INCREASING;
    }

    if (delta <= -1.5f) {
        return RADAR_ACTIVITY_DYNAMICS_DECREASING;
    }

    return RADAR_ACTIVITY_DYNAMICS_CONSTANT;
}

const char *Ld2420Radar::activity_dynamics_to_string(RadarActivityDynamics dynamics) const {
    switch (dynamics) {
        case RADAR_ACTIVITY_DYNAMICS_INCREASING:
            return "increasing";
        case RADAR_ACTIVITY_DYNAMICS_DECREASING:
            return "decreasing";
        case RADAR_ACTIVITY_DYNAMICS_VARIABLE:
            return "variable";
        default:
            return "constant";
    }
}

bool Ld2420Radar::is_zone_valid(uint16_t zone) const {
    return zone >= config.min_valid_zone && zone <= config.max_valid_zone;
}