#include "ld2420_radar.h"
#include <string.h>
#include <math.h>
#include <stdio.h>

/*
    =========================================================
    LD2420 Radar — реализация
    Протокол: Energy mode (бинарный фрейм, fw >= v1.5.4)

    Фрейм данных, 31 байт:
      [0..3]   Header   F4 F3 F2 F1
      [4..5]   Length   uint16_t LE
      [6]      Presence 0=нет 1=движение 2=стационарный
      [7..8]   Distance uint16_t LE (сантиметры)
      [9..24]  Gate[0..15] uint8_t
      [25..26] Padding  00 00
      [27..30] Footer   F8 F7 F6 F5

    Источник формата: ESPHome ld2420.cpp source
    =========================================================
*/

/* -------- constexpr definitions ------------------------- */

constexpr uint8_t Ld2420Radar::HDR[4];
constexpr uint8_t Ld2420Radar::FTR[4];


/* -------- begin ----------------------------------------- */

void Ld2420Radar::begin(HardwareSerial& serial, const RadarConfig& config) {
    _serial = &serial;
    _cfg    = config;

    /*  Ограничиваем окна  */
    if (_cfg.activity_avg_window_s    < 1)  _cfg.activity_avg_window_s    = 1;
    if (_cfg.activity_avg_window_s    > ACT_AVG_MAX) _cfg.activity_avg_window_s = ACT_AVG_MAX;
    if (_cfg.activity_trend_window_min < 1) _cfg.activity_trend_window_min = 1;
    if (_cfg.activity_trend_window_min > TREND_MAX) _cfg.activity_trend_window_min = TREND_MAX;
    if (_cfg.total_energy_max == 0)         _cfg.total_energy_max = 1;

    memset(_gate_energy,  0, sizeof(_gate_energy));
    memset(_act_avg_buf,  0, sizeof(_act_avg_buf));
    memset(_trend_buf,    0, sizeof(_trend_buf));
    memset(_rx_buf,       0, sizeof(_rx_buf));

    serial.begin(_cfg.baud_rate, SERIAL_8N1,
                 (int)_cfg.uart_rx_pin,
                 (int)_cfg.uart_tx_pin);
}


/* -------- update ---------------------------------------- */

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

    while (_serial->available()) {
        _feed_byte((uint8_t)_serial->read());
    }

    uint32_t now_ms = millis();

    /*  online  */
    _online = (_last_frame_ms > 0) &&
              (now_ms - _last_frame_ms < _cfg.stale_after_ms);

    /*  presence hold  */
    if (_last_on_ms > 0 && (now_ms - _last_on_ms < _cfg.presence_hold_ms)) {
        _presence = true;
    } else {
        _presence = false;
    }

    _tick_activity_avg(now_ms);
    _tick_trend(now_ms);
}


/* -------- _feed_byte ------------------------------------- */

/*
    Стратегия синхронизации:
    1. Ищем заголовок F4 F3 F2 F1 побайтово.
    2. После его захвата накапливаем оставшиеся (FRAME_SIZE - 4) байт.
    3. Проверяем футер. Если неверен — сбрасываем и ищем снова.
*/

void Ld2420Radar::_feed_byte(uint8_t b) {
    if (!_synced) {
        /*  Сдвигаем первые 4 байта в поисках заголовка  */
        _rx_buf[0] = _rx_buf[1];
        _rx_buf[1] = _rx_buf[2];
        _rx_buf[2] = _rx_buf[3];
        _rx_buf[3] = b;

        if (_rx_buf[0] == HDR[0] &&
            _rx_buf[1] == HDR[1] &&
            _rx_buf[2] == HDR[2] &&
            _rx_buf[3] == HDR[3]) {
            _synced = true;
            _rx_idx = 4;   // заголовок уже в буфере
        }
        return;
    }

    /*  Накапливаем остаток фрейма  */
    _rx_buf[_rx_idx++] = b;

    if (_rx_idx >= FRAME_SIZE) {
        if (_validate_footer()) {
            _process_frame();
        } else {
            if (_cfg.enable_debug_frames) {
                Serial.print("[LD2420] bad footer: ");
                for (uint8_t i = 0; i < FRAME_SIZE; i++) {
                    char hex[4];
                    snprintf(hex, sizeof(hex), "%02X ", _rx_buf[i]);
                    Serial.print(hex);
                }
                Serial.println();
            }
        }
        /*  В любом случае ищем следующий заголовок  */
        _synced = false;
        _rx_idx = 0;
        memset(_rx_buf, 0, sizeof(_rx_buf));
    }
}


/* -------- _validate_footer ------------------------------ */

bool Ld2420Radar::_validate_footer() const {
    return _rx_buf[FRAME_FOOTER_OFF + 0] == FTR[0] &&
           _rx_buf[FRAME_FOOTER_OFF + 1] == FTR[1] &&
           _rx_buf[FRAME_FOOTER_OFF + 2] == FTR[2] &&
           _rx_buf[FRAME_FOOTER_OFF + 3] == FTR[3];
}


/* -------- _process_frame -------------------------------- */

void Ld2420Radar::_process_frame() {
    uint32_t now_ms = millis();
    _last_frame_ms = now_ms;

    /*  Поле Presence  */
    uint8_t presence_raw = _rx_buf[FRAME_PRESENCE_OFF];
    bool has_target = (presence_raw == 0x01 || presence_raw == 0x02);

    if (has_target) {
        _last_on_ms = now_ms;
    }

    /*  Дистанция (uint16_t LE, сантиметры → метры)  */
    uint16_t dist_cm = (uint16_t)(_rx_buf[FRAME_DIST_OFFSET]) |
                       (uint16_t)(_rx_buf[FRAME_DIST_OFFSET + 1]) << 8;
    float raw_m = (float)dist_cm / 100.0f;

    if (has_target && dist_cm > 0) {
        if (_dist_ema < 0.0f) {
            _dist_ema = raw_m;
        } else {
            _dist_ema = _cfg.distance_ema_alpha * raw_m
                      + (1.0f - _cfg.distance_ema_alpha) * _dist_ema;
        }
        _distance_m = _dist_ema;
    } else if (!has_target) {
        _dist_ema   = -1.0f;
        _distance_m = 0.0f;
    }

    /*  Энергии ворот Gate[0..15]  */
    uint32_t total_energy = 0;
    for (uint8_t g = 0; g < LD2420_TOTAL_GATES; g++) {
        _gate_energy[g] = _rx_buf[FRAME_GATE_OFFSET + g];
        total_energy += _gate_energy[g];
    }

    /*
        activity_score_current:
        Суммарная энергия по всем воротам нормируется к 0–10.
        total_energy_max соответствует score = 10.
    */
    if (total_energy == 0) {
        _activity_score_current = 0;
    } else {
        float score_f = (float)total_energy / (float)_cfg.total_energy_max * 10.0f;
        if (score_f > 10.0f) score_f = 10.0f;
        _activity_score_current = (uint8_t)(score_f + 0.5f);
    }

    if (_cfg.enable_debug_frames) {
        Serial.print("[LD2420] pres=");
        Serial.print(presence_raw);
        Serial.print(" dist_cm=");
        Serial.print(dist_cm);
        Serial.print(" energy_sum=");
        Serial.print(total_energy);
        Serial.print(" score_cur=");
        Serial.println(_activity_score_current);
    }
}


/* -------- _tick_activity_avg ---------------------------- */

void Ld2420Radar::_tick_activity_avg(uint32_t now_ms) {
    if (_act_avg_last_ms == 0) { _act_avg_last_ms = now_ms; return; }
    if (now_ms - _act_avg_last_ms < 1000) return;
    _act_avg_last_ms = now_ms;

    uint8_t win = _cfg.activity_avg_window_s;
    _act_avg_buf[_act_avg_idx] = _activity_score_current;
    _act_avg_idx = (_act_avg_idx + 1) % win;
    if (_act_avg_count < win) _act_avg_count++;

    uint32_t sum = 0;
    for (uint8_t i = 0; i < _act_avg_count; i++) sum += _act_avg_buf[i];
    _activity_score = (uint8_t)(sum / _act_avg_count);
}


/* -------- _tick_trend ----------------------------------- */

void Ld2420Radar::_tick_trend(uint32_t now_ms) {
    if (_trend_last_ms == 0) { _trend_last_ms = now_ms; return; }
    if (now_ms - _trend_last_ms < 60000u) return;
    _trend_last_ms = now_ms;

    uint8_t win = _cfg.activity_trend_window_min;
    _trend_buf[_trend_idx] = _activity_score;
    _trend_idx = (_trend_idx + 1) % win;
    if (_trend_count < win) _trend_count++;

    _compute_trend();
}


/* -------- _compute_trend -------------------------------- */

void Ld2420Radar::_compute_trend() {
    uint8_t n = _trend_count;
    if (n < 2) { _trend_slope = 0; _trend_std_dev = 0.0f; return; }

    uint8_t win = _cfg.activity_trend_window_min;

    float sum_x = 0, sum_y = 0, sum_xx = 0, sum_xy = 0, mean_y = 0;

    for (uint8_t i = 0; i < n; i++) {
        /*  Хронологический порядок: старый → новый  */
        uint8_t buf_i = (uint8_t)((_trend_idx - n + i + win) % win);
        float x = (float)i;
        float y = (float)_trend_buf[buf_i];
        sum_x  += x;
        sum_y  += y;
        sum_xx += x * x;
        sum_xy += x * y;
        mean_y += y;
    }

    float fn = (float)n;
    float denom = fn * sum_xx - sum_x * sum_x;

    if (fabsf(denom) < 1e-6f) {
        _trend_slope = 0;
    } else {
        float slope = (fn * sum_xy - sum_x * sum_y) / denom;
        /*  Значимый порог: ±0.30 балл/мин  */
        if      (slope >  0.30f) _trend_slope =  1;
        else if (slope < -0.30f) _trend_slope = -1;
        else                     _trend_slope =  0;
    }

    /*  Стандартное отклонение для детектирования "variable"  */
    mean_y /= fn;
    float var = 0.0f;
    for (uint8_t i = 0; i < n; i++) {
        uint8_t buf_i = (uint8_t)((_trend_idx - n + i + win) % win);
        float d = (float)_trend_buf[buf_i] - mean_y;
        var += d * d;
    }
    _trend_std_dev = sqrtf(var / fn);
}


/* -------- get_activity_dynamics ------------------------- */

const char* Ld2420Radar::get_activity_dynamics() const {
    if (_trend_count < 2) return "constant";

    /*
        "variable" = высокий разброс без чёткого тренда.
        Порог StdDev > 2.5 при нулевом или слабом слоупе.
    */
    if (_trend_std_dev > 2.5f && _trend_slope == 0) return "variable";

    if      (_trend_slope >  0) return "increasing";
    else if (_trend_slope <  0) return "decreasing";
    else                        return "constant";
}


/* -------- get_state_json -------------------------------- */

const char* Ld2420Radar::get_state_json() {
    snprintf(_json_buf, sizeof(_json_buf),
        "{"
        "\"online\":%s,"
        "\"presence\":%s,"
        "\"activity_score\":%u,"
        "\"activity_score_current\":%u,"
        "\"activity_score_dynamics\":\"%s\","
        "\"distance_m\":%.2f"
        "}",
        _online   ? "true" : "false",
        _presence ? "true" : "false",
        (unsigned)_activity_score,
        (unsigned)_activity_score_current,
        get_activity_dynamics(),
        _distance_m
    );
    return _json_buf;
}