#include "ld2420_radar.h"

Ld2420Radar::Ld2420Radar() {
    uart = nullptr;
    new_data_available = false;
    calibration_mode = false;

    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();

    state.timestamp_ms = millis();
    state.last_update_ms = state.timestamp_ms;
}

void Ld2420Radar::update() {
    state.timestamp_ms = millis();

    read_uart_lines();
    apply_presence_hold();
    update_stale_flag();
    update_direction();
    update_activity_score();
    update_confidence();
}

bool Ld2420Radar::has_new_data() const {
    return new_data_available;
}

void Ld2420Radar::clear_new_data_flag() {
    new_data_available = false;
}

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

String Ld2420Radar::get_state_json() {
    String json;
    json.reserve(512);

    json += "{";
    json += "\"timestamp_ms\":";
    json += String(state.timestamp_ms);

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

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

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

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

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

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

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

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

    json += ",\"radial_speed_m_s\":";
    json += String(state.radial_speed_m_s, 3);

    json += ",\"direction\":\"";
    json += direction_to_string(state.direction);
    json += "\"";

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

    json += ",\"confidence\":";
    json += String(state.confidence, 3);

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

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

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

    json += "}";

    state.state_json_cache = json;
    return state.state_json_cache;
}

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::set_calibration_mode(bool enabled) {
    calibration_mode = enabled;
}

bool Ld2420Radar::is_calibration_mode() const {
    return calibration_mode;
}

String Ld2420Radar::get_calibration_line() const {
    String line;
    line.reserve(128);

    line += "raw=";
    line += String(state.raw_distance_zone);

    line += " median=";
    line += String(state.median_distance_zone);

    line += " filtered=";
    line += String(state.filtered_distance_zone);

    line += " distance=";
    line += String(state.distance_m, 3);

    line += " speed=";
    line += String(state.radial_speed_m_s, 3);

    return line;
}

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

    line_buffer = "";
    new_data_available = false;

    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;
}

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.online = true;
    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;
    new_data_available = true;
}

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

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();
    state.median_distance_zone = 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, это уже полезный признак присутствия.
        Даже если отдельная строка ON отсутствует, будем считать,
        что радар видит цель.
    */
    state.raw_presence = true;
    state.last_on_ms = now_ms;

    new_data_available = true;
}

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::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::update_direction() {
    if (!state.presence && !state.raw_presence) {
        state.direction = RADAR_DIRECTION_UNKNOWN;
        return;
    }

    if (state.radial_speed_m_s > config.speed_epsilon_m_s) {
        state.direction = RADAR_DIRECTION_LEAVING;
    } else if (state.radial_speed_m_s < -config.speed_epsilon_m_s) {
        state.direction = RADAR_DIRECTION_APPROACHING;
    } else {
        state.direction = RADAR_DIRECTION_STATIC;
    }
}

void Ld2420Radar::update_activity_score() {
    if (!state.presence && !state.raw_presence) {
        state.activity_score = 0;
        return;
    }

    float abs_speed_m_s = fabs(state.radial_speed_m_s);
    uint8_t score = 1;

    if (abs_speed_m_s >= 0.03f) score = 2;
    if (abs_speed_m_s >= 0.07f) score = 3;
    if (abs_speed_m_s >= 0.12f) score = 4;
    if (abs_speed_m_s >= 0.20f) score = 5;
    if (abs_speed_m_s >= 0.30f) score = 6;
    if (abs_speed_m_s >= 0.45f) score = 7;
    if (abs_speed_m_s >= 0.65f) score = 8;
    if (abs_speed_m_s >= 0.90f) score = 9;
    if (abs_speed_m_s >= 1.20f) score = 10;

    state.activity_score = score;
}

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.activity_score = 0;
        state.radial_speed_m_s = 0.0f;
        state.direction = RADAR_DIRECTION_UNKNOWN;
    }
}

void Ld2420Radar::update_stale_flag() {
    uint32_t now_ms = millis();
    state.stale = !(state.last_update_ms > 0 && (now_ms - state.last_update_ms) < config.stale_after_ms);
}

void Ld2420Radar::update_confidence() {
    float conf = 0.0f;
    uint32_t now_ms = millis();

    if (state.online) {
        conf += 0.25f;
    }

    if (!state.stale) {
        conf += 0.25f;
    }

    if (state.presence) {
        conf += 0.20f;
    }

    if (state.filtered_distance_zone > 0) {
        conf += 0.15f;
    }

    if (zone_window_count >= 3) {
        conf += 0.15f;
    }

    if (state.parse_error_counter > 0) {
        conf -= 0.10f;
    }

    if (state.last_update_ms > 0 && (now_ms - state.last_update_ms) > config.stale_after_ms) {
        conf -= 0.20f;
    }

    if (conf < 0.0f) conf = 0.0f;
    if (conf > 1.0f) conf = 1.0f;

    state.confidence = conf;
}

bool Ld2420Radar::is_zone_valid(uint16_t zone) const {
    return zone >= config.min_valid_zone && zone <= config.max_valid_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;
}

const char *Ld2420Radar::direction_to_string(RadarDirection direction) const {
    switch (direction) {
        case RADAR_DIRECTION_STATIC:
            return "static";
        case RADAR_DIRECTION_APPROACHING:
            return "approaching";
        case RADAR_DIRECTION_LEAVING:
            return "leaving";
        default:
            return "unknown";
    }
}

String Ld2420Radar::escape_json_string(const String &value) const {
    String out;
    out.reserve(value.length() + 8);

    for (size_t i = 0; i < value.length(); i++) {
        char c = value.charAt(i);

        if (c == '\"') {
            out += "\\\"";
        } else if (c == '\\') {
            out += "\\\\";
        } else {
            out += c;
        }
    }

    return out;
}