/**
 * @file radar_manager.cpp
 * @brief R60ABD1 毫米波雷达管理模块实现
 *
 * 状态机逐字节解析 UART 帧, 提取生命体征数据
 */

#include "radar_manager.h"

// ============================================================
// 构造函数 & 析构函数
// ============================================================

RadarManager::RadarManager()
    : _serial(nullptr)
    , _state(STATE_IDLE)
    , _dataIndex(0)
    , _initialized(false)
    , _callback(nullptr)
    , _frameCount(0)
    , _errorCount(0)
    , _lastHeartBeat(0)
    , _lastBreathBeat(0)
    , _stablePresence(false)
    , _presenceCnt(0)
    , _lastRawPresence(false)
{
    memset(&_currentFrame, 0, sizeof(_currentFrame));
    memset(&_data, 0, sizeof(_data));
}

RadarManager::~RadarManager() {
}

// ============================================================
// 初始化
// ============================================================

bool RadarManager::begin() {
    _serial = &RADAR_UART_PORT;
    _serial->begin(RADAR_BAUD_RATE, SERIAL_8N1,
                   RADAR_RX_PIN, RADAR_TX_PIN);

    // 可选: 初始化雷达 GPIO 作为辅助
    pinMode(RADAR_GPIO1_PIN, INPUT);
    pinMode(RADAR_GPIO2_PIN, INPUT);

    _resetParser();
    memset(&_data, 0, sizeof(_data));
    _data.lastUpdate = millis();
    _stablePresence = false;
    _presenceCnt = 0;
    _lastRawPresence = false;

    // 不给雷达发任何命令，让它自己初始化并开始自动上报
    // R60ABD1 上电后通常需要 15-30 秒自检，之后自动输出数据
    Serial.println(F("[雷达] 雷达模块初始化完成, 等待自动上报..."));
    Serial.println(F("[雷达] (R60ABD1 自检约15-30秒，心率和呼吸需检测到静止人体后输出)"));
    return true;
}

// ============================================================
// 主循环 - 逐字节解析 UART 数据
// ============================================================

void RadarManager::loop() {
    while (_serial->available() > 0) {
        uint8_t byte = _serial->read();
        _parseByte(byte);
    }

    // 首次收到数据时打印提示
    static bool firstData = false;
    if (!firstData && _frameCount > 0) {
        firstData = true;
        Serial.printf("[雷达] ✅ 开始接收数据 (首帧)\n");
    }

    // 心率/呼吸数据过期 (>10秒无更新 → 标记无效)
    if (millis() - _lastHeartBeat > 10000) {
        _data.vitalValid = false;
        _data.heartRate  = 0;
    }
    if (millis() - _lastBreathBeat > 10000) {
        _data.vitalValid = false;
        _data.breathRate = 0;
    }

    // 超时检测：如果 10 秒还没收到任何数据，打印一次
    static uint32_t noDataWarn = 0;
    if (_frameCount == 0 && millis() - _data.lastUpdate > 10000) {
        if (millis() - noDataWarn > 10000) {
            noDataWarn = millis();
            Serial.println(F("[雷达] ⚠️ 10秒内未收到雷达数据, 请检查:"));
            Serial.println(F("[雷达]    1.雷达供电 5V?  2.TX→GPIO17 RX→GPIO1?  3.交叉了吗?"));
        }
    }
}

// ============================================================
// 帧解析状态机
// ============================================================

void RadarManager::_parseByte(uint8_t byte) {
    switch (_state) {

    case STATE_IDLE:
        // 等待帧头第一个字节 0x53
        if (byte == FRAME_HEADER_1) {
            _resetParser();
            _currentFrame.data[0] = byte;
            _state = STATE_GOT_HEADER1;
        }
        break;

    case STATE_GOT_HEADER1:
        // 等待帧头第二个字节 0x59
        if (byte == FRAME_HEADER_2) {
            _currentFrame.data[1] = byte;
            _state = STATE_GOT_HEADER2;
        } else {
            // 不是 0x59, 重新开始
            _errorCount++;
            _state = STATE_IDLE;
            if (byte == FRAME_HEADER_1) {
                // 连续 0x53, 重新开始
                _currentFrame.data[0] = byte;
                _state = STATE_GOT_HEADER1;
            }
        }
        break;

    case STATE_GOT_HEADER2:
        // 读取控制字
        _currentFrame.control = byte;
        _state = STATE_GOT_CONTROL;
        break;

    case STATE_GOT_CONTROL:
        // 读取命令字
        _currentFrame.command = byte;
        _state = STATE_GOT_COMMAND;
        break;

    case STATE_GOT_COMMAND:
        // 读取长度高字节
        _currentFrame.data_length = ((uint16_t)byte) << 8;
        _state = STATE_GOT_LEN_H;
        break;

    case STATE_GOT_LEN_H:
        // 读取长度低字节
        _currentFrame.data_length |= byte;

        // 长度安全检查
        if (_currentFrame.data_length > RADAR_FRAME_BUF_SIZE) {
            Serial.printf("[雷达] 帧数据长度异常: %d\n",
                          _currentFrame.data_length);
            _errorCount++;
            _state = STATE_IDLE;
            return;
        }

        _dataIndex = 0;

        // 如果数据长度为 0, 直接跳到校验状态
        if (_currentFrame.data_length == 0) {
            _state = STATE_GOT_DATA;
        } else {
            _state = STATE_GOT_LEN_L;
        }
        break;

    case STATE_GOT_LEN_L:
        // 读取数据字节
        _currentFrame.data[_dataIndex++] = byte;

        if (_dataIndex >= _currentFrame.data_length) {
            _state = STATE_GOT_DATA;
        }
        break;

    case STATE_GOT_DATA:
        // 读取校验码
        _currentFrame.checksum = byte;
        _state = STATE_GOT_CHECKSUM;
        break;

    case STATE_GOT_CHECKSUM:
        // 等待帧尾第一个字节 0x54
        if (byte == FRAME_TAIL_1) {
            _state = STATE_GOT_TAIL1;
        } else {
            _errorCount++;
            _state = STATE_IDLE;
        }
        break;

    case STATE_GOT_TAIL1:
        // 等待帧尾第二个字节 0x43
        if (byte == FRAME_TAIL_2) {
            _state = STATE_FRAME_COMPLETE;
            _currentFrame.timestamp = millis();
            _processCompleteFrame();
            _state = STATE_IDLE;
        } else {
            _errorCount++;
            _state = STATE_IDLE;
        }
        break;

    default:
        _state = STATE_IDLE;
        break;
    }
}

// ============================================================
// 帧处理
// ============================================================

void RadarManager::_processCompleteFrame() {
    _frameCount++;

    // 校验
    if (!_validateChecksum()) {
        return;
    }

    _currentFrame.valid = true;

    // 根据控制字提取数据
    switch (_currentFrame.control) {
    case CTRL_PRESENCE:
        _extractPresence(_currentFrame);
        break;
    case CTRL_BREATHING:
        _extractBreathing(_currentFrame);
        break;
    case CTRL_HEART_RATE:
        _extractHeartRate(_currentFrame);
        break;
    case CTRL_FALL:
        _extractFall(_currentFrame);    // AFD1 专用跌倒
        break;
    case CTRL_SLEEP:
        _extractSleep(_currentFrame);
        break;
    case CTRL_WORK_STATUS:
        _extractWorkStatus(_currentFrame);
        break;
    case CTRL_DETECT_RANGE:
        _extractDetectRange(_currentFrame);
        break;
    default:
        Serial.printf("[雷达] 未识别控制字=0x%02X 长度=%d\n",
                      _currentFrame.control, _currentFrame.data_length);
        // 打印数据内容辅助分析
        Serial.print("       数据: ");
        for (int i = 0; i < _currentFrame.data_length && i < 16; i++) {
            Serial.printf("%02X ", _currentFrame.data[i]);
        }
        Serial.println();
        break;
    }
}

// ============================================================
// 数据提取函数
// ============================================================

void RadarManager::_extractPresence(const RadarFrame& frame) {
    uint8_t raw = frame.data_length > 0 ? frame.data[0] : 0;
    bool rawPresence = false;

    // cmd 0x00 (len=1): 有人状态 (1=有人 0=无人)
    if (frame.command == 0x00 && frame.data_length >= 1) {
        rawPresence = (raw == 1);
        _data.radarValid = true;
        _data.lastUpdate = millis();
    }
    // cmd 0x01 (len=1): 运动状态/体动 (值越大越活跃)
    else if (frame.command == 0x01 && frame.data_length >= 1) {
        _data.body       = raw;
        _data.motion     = (raw >= 1);
        _data.radarValid = true;
        _data.lastUpdate = millis();
        _notifyUpdate();
        return;  // 运动帧不含有人信息, 跳过防抖
    }
    // cmd 0x03 (len=1): 有人状态备用 (非0=有人)
    else if (frame.command == 0x03 && frame.data_length >= 1) {
        rawPresence = (raw != 0);
        _data.radarValid = true;
        _data.lastUpdate = millis();
    }
    // cmd 0x04 (len=2): [0]=presence [1]=motion
    else if (frame.command == 0x04 && frame.data_length >= 2) {
        rawPresence = (frame.data[0] != 0);
        _data.motion = (frame.data[1] == 0);
        _data.radarValid = true;
        _data.lastUpdate = millis();
    }
    // cmd 0x05 (len=6): 完整包
    else if (frame.command == 0x05 && frame.data_length >= 6) {
        rawPresence = (frame.data[0] != 0);
        _data.motion     = (frame.data[1] == 0);
        _data.distance   = ((uint16_t)frame.data[2] << 8) | frame.data[3];
        _data.body       = frame.data[4];
        _data.radarValid = true;
        _data.lastUpdate = millis();
    } else {
        return;  // 未知命令
    }

    // 防抖: 连续 PRESENCE_HYSTERESIS 次一致才更新
    if (rawPresence == _lastRawPresence) {
        if (_presenceCnt < PRESENCE_HYSTERESIS) _presenceCnt++;
    } else {
        _presenceCnt = 0;
        _lastRawPresence = rawPresence;
    }
    if (_presenceCnt >= PRESENCE_HYSTERESIS && rawPresence != _data.presence) {
        _data.presence = rawPresence;
        _stablePresence = rawPresence;
    }
    _notifyUpdate();
}

void RadarManager::_extractBreathing(const RadarFrame& frame) {
    if (frame.data_length >= 1) {
        uint8_t rate = frame.data[0];
        if (rate != 0x80 && rate != 0x00 && rate <= 40) {
            _data.breathRate  = rate;
            _data.vitalValid  = true;
            _data.presence    = true;
            _data.radarValid  = true;
            _lastBreathBeat   = millis();
        }
        _data.lastUpdate  = millis();
        _notifyUpdate();
    }
}

void RadarManager::_extractHeartRate(const RadarFrame& frame) {
    if (frame.data_length >= 1) {
        uint8_t rate = frame.data[0];
        if (rate != 0x80 && rate != 0x00 && rate >= 40) {
            _data.heartRate   = rate;
            _data.vitalValid  = true;
            _data.presence    = true;
            _data.radarValid  = true;
            _lastHeartBeat    = millis();
        }
        _data.lastUpdate  = millis();
        _notifyUpdate();
    }
}

// AFD1 专用: 跌倒检测 (控制字 0x83)
void RadarManager::_extractFall(const RadarFrame& frame) {
    if (frame.data_length >= 1) {
        _data.fall       = (frame.data[0] != 0);
        _data.radarValid = true;
        _data.lastUpdate = millis();
        _notifyUpdate();
    }
}

void RadarManager::_extractSleep(const RadarFrame& frame) {
    // 从睡眠数据提取: cmd=0x00/0x01 为入床离床状态
    if (frame.data_length >= 1) {
        // data[0]: 0=离床/无驻留, 非0=入床/驻留
        if (frame.command == 0x00 || frame.command == 0x01) {
            _data.resident   = (frame.data[0] != 0);
            _data.lastUpdate  = millis();
            _notifyUpdate();
        }
    }

    if (frame.data_length >= 3) {
        _data.sleepState = frame.data[1];   // 0=清醒 1=浅睡 2=深睡
        _data.sleepScore = frame.data[2];   // 0~100
        _data.lastUpdate = millis();
        _notifyUpdate();
    }

    if (frame.data_length >= 4) {
        _data.fall     = (frame.data[3] & 0x01) != 0;
        _data.resident = (frame.data[3] & 0x02) != 0;
        _data.lastUpdate = millis();
        _notifyUpdate();
    }
}

void RadarManager::_extractWorkStatus(const RadarFrame& frame) {
    if (frame.command == WS_INIT_COMPLETE && frame.data_length >= 1) {
        if (frame.data[0] == 0x0F) {
            _initialized = true;
            Serial.println(F("[雷达] ✅ 雷达初始化完成"));
        }
    }
}

void RadarManager::_extractDetectRange(const RadarFrame& frame) {
    // 探测范围信息
    if (frame.data_length >= 1) {
        Serial.printf("[雷达] 探测范围状态: 0x%02X\n", frame.data[0]);
    }
}

// ============================================================
// 校验码计算 & 验证
// ============================================================

uint8_t RadarManager::_calcChecksum(const uint8_t* data, uint16_t len) {
    uint16_t sum = 0;
    for (uint16_t i = 0; i < len; i++) {
        sum += data[i];
    }
    return (uint8_t)(sum & 0xFF);
}

bool RadarManager::_validateChecksum() {
    // 计算: 帧头(2) + 控制字(1) + 命令字(1) + 长度(2) + 数据(N) 求和取低8位
    uint8_t header[6];
    header[0] = FRAME_HEADER_1;
    header[1] = FRAME_HEADER_2;
    header[2] = _currentFrame.control;
    header[3] = _currentFrame.command;
    header[4] = (_currentFrame.data_length >> 8) & 0xFF;
    header[5] = _currentFrame.data_length & 0xFF;

    uint16_t sum = 0;
    for (int i = 0; i < 6; i++) sum += header[i];
    for (uint16_t i = 0; i < _currentFrame.data_length; i++) sum += _currentFrame.data[i];

    uint8_t expected = (uint8_t)(sum & 0xFF);
    return (expected == _currentFrame.checksum);
}

// ============================================================
// 解析器重置
// ============================================================

void RadarManager::_resetParser() {
    _state     = STATE_IDLE;
    _dataIndex = 0;
    memset(&_currentFrame, 0, sizeof(_currentFrame));
}

// ============================================================
// 获取最新数据 (线程安全版本)
// ============================================================

RadarRawData RadarManager::getData() const {
    return _data;
}

// ============================================================
// 回调注册
// ============================================================

void RadarManager::onDataUpdate(RadarCallback cb) {
    _callback = cb;
}

void RadarManager::_notifyUpdate() {
    if (_callback) {
        _callback(_data);
    }
}

// ============================================================
// 发送命令到雷达
// ============================================================

void RadarManager::sendCommand(uint8_t control, uint8_t command,
                                const uint8_t* data, uint16_t len) {
    if (!_serial) return;

    // 构建帧
    uint8_t frame[RADAR_FRAME_BUF_SIZE + 9];
    uint16_t idx = 0;

    frame[idx++] = FRAME_HEADER_1;
    frame[idx++] = FRAME_HEADER_2;
    frame[idx++] = control;
    frame[idx++] = command;
    frame[idx++] = (len >> 8) & 0xFF;
    frame[idx++] = len & 0xFF;

    if (data && len > 0) {
        memcpy(&frame[idx], data, len);
        idx += len;
    }

    // 校验: 帧头(2) + 控制(1) + 命令(1) + 长度(2) + 数据(N)
    uint16_t sum = 0;
    for (uint16_t i = 0; i < idx; i++) sum += frame[i];
    frame[idx++] = (uint8_t)(sum & 0xFF);

    frame[idx++] = FRAME_TAIL_1;
    frame[idx++] = FRAME_TAIL_2;

    _serial->write(frame, idx);
    _serial->flush();
}

void RadarManager::queryInitStatus() {
    // 查询初始化状态: 控制字 0x05, 命令字 0x81, 数据 0x0F
    uint8_t d = 0x0F;
    sendCommand(CTRL_WORK_STATUS, WS_INIT_QUERY, &d, 1);
}

void RadarManager::queryDetectRange() {
    sendCommand(CTRL_DETECT_RANGE, CMD_QUERY);
}