#include "uart.h"
#include <QApplication>
#include <QDebug>
#include <QSerialPort>
#include <QSettings>

namespace {

const int    kFrameLen = 24;
const quint8 kHead = 0x11;
const quint8 kTail = 0x22;

// 收不到帧头时缓冲上限，超过则只保留尾部一帧长度，避免长期涨下去
const int kBufMax = 1024;

// 小端无符号 16 位
inline int leU16(const char* p)
{
    return int(quint8(p[0]) | (quint8(p[1]) << 8));
}

// 摇杆量按协议范围 0~4096 收口（外部输入，越界值不采信）
inline int axis(const char* p)
{
    return qBound(0, leU16(p), 4096);
}

// 校验帧头、帧尾与和校验（BYTE0~BYTE22 之和的低 8 位）
bool parseFrame(const char* d, RcFrame& f)
{
    if (quint8(d[0]) != kHead || quint8(d[22]) != kTail)
        return false;

    quint8 sum = 0;
    for (int i = 0; i < 23; ++i)
        sum += quint8(d[i]);
    if (sum != quint8(d[23]))
        return false;

    const quint8 flags = quint8(d[1]);
    f.driveMode      = (flags & 0x01) != 0;
    f.armMode        = (flags & 0x02) != 0;
    f.armEnabled     = (flags & 0x04) != 0;
    f.chassisEnabled = (flags & 0x08) != 0;
    f.estop          = (flags & 0x10) != 0;

    f.gear  = axis(d + 3);
    f.steer = axis(d + 5);
    f.posZ  = axis(d + 7);
    f.posX  = axis(d + 9);
    f.posY  = axis(d + 11);
    f.roll  = axis(d + 13);
    f.pitch = axis(d + 15);
    f.yaw   = axis(d + 17);

    f.battery = qBound(0, int(quint8(d[19])), 100);
    return true;
}

} // namespace

RcCommunication::RcCommunication(QObject* parent)
    : QObject(parent)
{
    m_serial = new QSerialPort(this);
    connect(m_serial, &QSerialPort::readyRead, this, &RcCommunication::onReadyRead);

    // 与 AppContext 共用程序目录的 config.ini：默认构造读不到 rc/* 这些键
    // （本工程没有 setOrganizationName/setApplicationName）
    QSettings s(QApplication::applicationDirPath() + "/config.ini", QSettings::IniFormat);
    m_device = s.value("rc/device", "/dev/ttyS7").toString();
    m_baud   = s.value("rc/baud", 115200).toInt();

    // 协议发送周期 100ms，连续 500ms 无合法帧判为链路断开
    m_linkTimer = new QTimer(this);
    m_linkTimer->setInterval(500);
    connect(m_linkTimer, &QTimer::timeout, this, &RcCommunication::onLinkTimeout);
}

RcCommunication::~RcCommunication()
{
    stop();
}

bool RcCommunication::start()
{
    if (m_serial->isOpen())
        return true;

    m_serial->setPortName(m_device);
    m_serial->setBaudRate(m_baud);
    m_serial->setDataBits(QSerialPort::Data8);
    m_serial->setStopBits(QSerialPort::OneStop);
    m_serial->setParity(QSerialPort::NoParity);
    m_serial->setFlowControl(QSerialPort::NoFlowControl);

    if (!m_serial->open(QIODevice::ReadWrite)) {
        qWarning() << "遥控器串口打开失败:" << m_device << m_serial->errorString();
        return false;
    }
    m_buf.clear();
    m_linkTimer->start();
    return true;
}

void RcCommunication::stop()
{
    m_linkTimer->stop();
    if (m_serial->isOpen()) m_serial->close();
    m_buf.clear();
}

void RcCommunication::onReadyRead()
{
    m_buf.append(m_serial->readAll());

    // 逐字节扫描合法帧：容忍前导垃圾与粘包，未对齐时向后找下一个帧头。
    // 串口是字节流，一帧可能被切开，尾部不足一帧的残段留在 m_buf 等下次数据。
    int off = 0;
    while (off + kFrameLen <= m_buf.size()) {
        RcFrame f;
        if (parseFrame(m_buf.constData() + off, f)) {
            onFrame(f);
            off += kFrameLen;
        } else {
            ++off;
        }
    }
    if (off > 0) m_buf.remove(0, off);

    if (m_buf.size() > kBufMax) m_buf.remove(0, m_buf.size() - kFrameLen);
}

void RcCommunication::onFrame(const RcFrame& frame)
{
    m_linkTimer->start();   // 重置链路超时

    if (!m_linkOk) {
        m_linkOk = true;
        emit linkChanged(true);
    }
    emit frameReceived(frame);
}

void RcCommunication::onLinkTimeout()
{
    if (m_linkOk) {
        m_linkOk = false;
        emit linkChanged(false);
    }
}