#include "ld2420_radar.h"

/* ------------------------------------------------------------------ */
/* ------------------------  Инициализация  --------------------------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::begin(Stream &uart, const RadarConfig &cfg) {
    _uart = &uart;
    _cfg = cfg;

    /* Установим параметры UART */
    pinMode(_cfg.uart_rx_pin, INPUT_PULLUP);
    pinMode(_cfg.uart_tx_pin, OUTPUT);
    // На ESP32 Serial2 уже привязан к этим пинам, но оставляем для совместимости
}

/* ------------------------------------------------------------------ */
/* --------------------------  Основной цикл  ------------------------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::update() {
    if (!_uart) return;

    /* Читаем все доступные байты */
    while (_uart->available()) {
        char c = _uart->read();
        if (c == '\n' || c == '\r') {
            if (_line_pos > 0) {
                _parse_line(_line_buf, _line_pos);
                _line_pos = 0;
            }
        } else {
            if (_line_pos < LINE_BUF_SIZE - 1) {
                _line_buf[_line_pos++] = c;
            } else {
                /* Буфер переполнен – сбрасываем */
                _line_pos = 0;
            }
        }
    }

    _update_online_state();
}

/* ------------------------------------------------------------------ */
/* ------------------------  Парсинг строки  -----------------------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::_parse_line(const char *line, size_t len) {
    if (len < 5) return;          // минимальная длина +<zone>,<dist>

    /* Время прихода */
    _last_line_ms = millis();

    /* Если строка начинается с '+' – это данные */
    if (line[0] == '+') {
        const char *p = line + 1;
        uint16_t zone = 0;
        float distance_m = NAN, speed_m_s = NAN;

        /* Парсим <zone> */
        while (*p && *p != ',') {
            if (!isdigit(*p)) return;          // Неверный формат
            zone = zone * 10 + (*p - '0');
            ++p;
        }
        if (*p != ',') return;
        ++p;

        /* Парсим <distance> (число с плавающей точкой) */
        char dist_str[16];
        size_t i = 0;
        while (*p && *p != ',' && i < sizeof(dist_str)-1) {
            dist_str[i++] = *p++;
        }
        if (*p != ',') return;
        ++p;
        dist_str[i] = '\0';
        distance_m = atof(dist_str);

        /* Парсим <speed> (число с плавающей точкой) */
        char speed_str[16];
        i = 0;
        while (*p && i < sizeof(speed_str)-1) {
            speed_str[i++] = *p++;
        }
        speed_str[i] = '\0';
        speed_m_s = atof(speed_str);

        /* Обновляем состояние */
        _update_presence_state(zone);
        _update_distance_speed(distance_m, speed_m_s);
    } else if (line[0] == '-') {
        /* Ошибка от радара – просто игнорируем */
        return;
    } else {
        /* Неизвестная строка */
        if (_cfg.enable_debug_unknown_lines) {
            Serial.printf("Unknown radar line: %.*s\n", (int)len, line);
        }
    }
}

/* ------------------------------------------------------------------ */
/* -----------------------  Обновление online  ----------------------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::_update_online_state() {
    uint32_t now = millis();
    _state.online = (now - _last_line_ms <= _cfg.stale_after_ms);
}

/* ------------------------------------------------------------------ */
/* ---------------------  Обновление presence  ---------------------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::_update_presence_state(uint16_t zone) {
    if (zone < _cfg.min_valid_zone || zone > _cfg.max_valid_zone) return;

    /* Если пришел первый сигнал, запоминаем время */
    if (!_state.presence) {
        _presence_start_ms = millis();
    }
    _state.presence = true;
}

/* ------------------------------------------------------------------ */
/* --------------------  Обновление расстояния/скорости ----------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::_update_distance_speed(float distance_m, float speed_m_s) {
    /* Сглаживаем дистанцию */
    if (isnan(_state.distance_m)) _state.distance_m = distance_m;
    else
        _state.distance_m =
            _cfg.distance_ema_alpha * distance_m +
            (1.0f - _cfg.distance_ema_alpha) * _state.distance_m;

    /* Сглаживаем скорость */
    float speed_smooth = 0.0f;
    if (!isnan(speed_m_s)) {
        static float prev_speed = 0.0f;
        speed_smooth =
            _cfg.speed_ema_alpha * speed_m_s +
            (1.0f - _cfg.speed_ema_alpha) * prev_speed;
        prev_speed = speed_smooth;
    }

    /* Обновляем активность */
    _calc_activity_score_current();
    /* Сохраняем в историю */
    _activity_hist[_hist_idx++] = _state.activity_score_current;
    if (_hist_idx >= ACTIVITY_HISTORY) {
        _hist_idx = 0;
        _hist_filled = true;
    }

    /* Вычисляем среднюю активность за минуту (последние N значений) */
    _calc_activity_score_average();
    _determine_dynamics();
}

/* ------------------------------------------------------------------ */
/* --------------------  Расчёт текущей активности  ----------------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::_calc_activity_score_current() {
    /* Прямое линейное преобразование скорости к шкале 0…10 */
    float speed = _cfg.speed_ema_alpha * _state.distance_m; // placeholder
    if (speed < _cfg.activity_min_speed_m_s) {
        _state.activity_score_current = 0;
    } else if (speed >= _cfg.activity_max_speed_m_s) {
        _state.activity_score_current = 10;
    } else {
        float v = (speed - _cfg.activity_min_speed_m_s) /
                  (_cfg.activity_max_speed_m_s - _cfg.activity_min_speed_m_s);
        _state.activity_score_current = round(v * 10.0f);
    }
}

/* ------------------------------------------------------------------ */
/* ---------------------  Средняя активность за минуту -------------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::_calc_activity_score_average() {
    if (!_hist_filled && _hist_idx == 0) return; // не хватает данных

    uint8_t sum = 0;
    uint8_t cnt = _hist_filled ? ACTIVITY_HISTORY : _hist_idx;
    for (uint8_t i = 0; i < cnt; ++i)
        sum += _activity_hist[i];
    _state.activity_score = round((float)sum / cnt);
}

/* ------------------------------------------------------------------ */
/* -----------------------  Динамика активности  --------------------*/
/* ------------------------------------------------------------------ */

void Ld2420Radar::_determine_dynamics() {
    /* Сравниваем последние два значения (или средние) */
    uint8_t last2[ACTIVITY_HISTORY];
    if (_hist_filled) {
        for (int i = 0; i < ACTIVITY_HISTORY; ++i)
            last2[i] = _activity_hist[(ACTIVITY_HISTORY - 1 - i)];
    } else {
        for (uint8_t i = 0; i < _hist_idx; ++i)
            last2[i] = _activity_hist[(_hist_idx - 1 - i)];
    }

    /* Считаем среднее за последние 5 значений */
    uint8_t cnt = min<uint8_t>(5, ACTIVITY_HISTORY);
    uint8_t avg_prev = 0, avg_curr = 0;
    for (uint8_t i = 0; i < cnt; ++i) {
        avg_prev += last2[i];
        if (i >= cnt/2) avg_curr += last2[i];
    }
    avg_prev /= cnt;
    avg_curr /= cnt;

    /* Выбираем динамику */
    if (avg_curr > avg_prev + 1)
        _state.activity_score_dynamics = "increasing";
    else if (avg_curr < avg_prev - 1)
        _state.activity_score_dynamics = "decreasing";
    else
        _state.activity_score_dynamics = "constant";
}

/* ------------------------------------------------------------------ */
/* --------------------------  JSON‑вывод  --------------------------*/
/* ------------------------------------------------------------------ */

String Ld2420Radar::get_state_json() const {
    String s;
    s.reserve(200);
    s += "{";

    s += "\"online\": ";
    s += _state.online ? "true" : "false";
    s += ", ";

    s += "\"presence\": ";
    s += _state.presence ? "true" : "false";
    s += ", ";

    s += "\"activity_score\": ";
    s += String(_state.activity_score);
    s += ", ";

    s += "\"activity_score_current\": ";
    s += String(_state.activity_score_current);
    s += ", ";

    s += "\"activity_score_dynamics\": \"";
    s += _state.activity_score_dynamics;
    s += "\", ";

    s += "\"distance_m\": ";
    if (isnan(_state.distance_m)) {
        s += "null";
    } else {
        s += String(_state.distance_m, 2);   // 2 знака после запятой
    }

    s += "}";
    return s;
}