//
// 触觉传感器串口协议解码器实现（中文注释版）。
// 使用要点：
// 1. 通过循环缓冲累积串口字节，按起始/结束符重组完整帧；
// 2. 使用 C++17 std::vector/std::optional 管理内存与返回值；
// 3. placement new（<new> 头文件）在已有内存上构造上下文对象。
//

#include "tacdec.h"

#include <algorithm> // std::find 等算法，用于搜索起始符
#include <cstddef>
#include <cstdint>
#include <new>       // placement new：在已分配内存上构造对象

namespace ffmsep::tactile { // 与头文件保持一致的命名空间层次

namespace { // 匿名命名空间，用于封装文件内部的工具常量与函数

constexpr std::size_t kMinimumFrameSize = 1   // start 起始符
    + 1                                       // address 设备地址
    + 1                                       // function 功能码
    + 1                                       // length 数据长度
    + 0                                       // payload 最短为 0
    + 2                                       // CRC 校验
    + 2;                                      // end markers 结束符

constexpr std::uint16_t kCrcInitial = 0xFFFF;        // CRC 初始值
constexpr std::uint16_t kCrcPolynomial = 0xA001;     // CRC-16/MODBUS 多项式 (LSB-first)

// 解码器私有上下文，跟随 CPCodecContext 的 priv_data 生命周期。
struct TactileDecoderContext {
    std::vector<std::uint8_t> fifo; // 环形缓冲，存储尚未解析的原始字节
    bool end_of_stream = false;     // 标记是否收到流结尾
    std::int64_t next_pts = 0;      // 输出帧的自增 pts
};

std::uint16_t crc16_modbus(const std::uint8_t* data, std::size_t length) {
    std::uint16_t crc = kCrcInitial;
    for (std::size_t i = 0; i < length; ++i) {
        crc ^= static_cast<std::uint16_t>(data[i]);
        for (int bit = 0; bit < 8; ++bit) {
            if ((crc & 0x0001U) != 0U) {
                crc = static_cast<std::uint16_t>((crc >> 1U) ^ kCrcPolynomial); // LSB 为 1 时异或多项式
            } else {
                crc = static_cast<std::uint16_t>(crc >> 1U); // 否则右移
            }
        }
    }
    return crc;
}

TactileDecoderContext* get_priv(CPCodecContext* ctx) {
    return ctx ? ctx->priv_as<TactileDecoderContext>() : nullptr; // 使用模板封装的安全转换
}

int tactile_init(CPCodecContext* ctx) {
    if (!ctx) {
        return CP_ERROR_INVALID_ARGUMENT;
    }
    if (!ctx->priv_data) {
        ctx->ensure_priv_storage(sizeof(TactileDecoderContext));
    }
    auto* storage = static_cast<TactileDecoderContext*>(ctx->priv_data);
    new (storage) TactileDecoderContext(); // placement new：在已分配的缓冲区中原地构造对象
    return CP_SUCCESS;
}

void tactile_close(CPCodecContext* ctx) {
    if (!ctx || !ctx->priv_data) {
        return;
    }
    if (auto* priv = get_priv(ctx); priv != nullptr) {
        priv->~TactileDecoderContext(); // 显式调用析构函数，释放内部 std::vector
    }
}

int tactile_send_packet(CPCodecContext* ctx, const CPPacket& packet) {
    auto* priv = get_priv(ctx);
    if (!priv) {
        return CP_ERROR_INVALID_STATE;
    }

    if (packet.flush) {
        priv->fifo.clear();      // flush: 清除所有缓存，回到初始状态
        priv->end_of_stream = false;
        priv->next_pts = 0;
    }

    if (!packet.payload.empty()) {
        priv->fifo.insert(priv->fifo.end(), packet.payload.begin(), packet.payload.end()); // 拼接新字节
    }

    if (packet.end_of_stream) {
        priv->end_of_stream = true; // 标记输入源已结束
    }

    return CP_SUCCESS;
}

std::size_t frame_length_from_payload(std::uint8_t payload_length) {
    return 1U + 1U + 1U + 1U + payload_length + 2U + 2U; // 计算完整帧总长度
}

const std::uint8_t* buffer_data(const std::vector<std::uint8_t>& buf) {
    return buf.empty() ? nullptr : buf.data();
}

int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
    auto* priv = get_priv(ctx);
    if (!priv) {
        return CP_ERROR_INVALID_STATE;
    }

    auto& buf = priv->fifo;

    while (true) {
        if (buf.empty()) {
            if (priv->end_of_stream) {
                priv->end_of_stream = false;
                return CP_ERROR_EOF; // 没有数据且流结束
            }
            return CP_ERROR_EAGAIN; // 告诉调用者需要更多数据
        }

        // Discard bytes until start byte is found.
        auto start_it = std::find(buf.begin(), buf.end(), kStartByte);
        if (start_it == buf.end()) {
            buf.clear();
            if (priv->end_of_stream) {
                priv->end_of_stream = false;
                return CP_ERROR_EOF;
            }
            return CP_ERROR_EAGAIN;
        }
        if (start_it != buf.begin()) {
            buf.erase(buf.begin(), start_it); // 丢掉起始符前的噪声
        }

        if (buf.size() < kMinimumFrameSize) {
            if (priv->end_of_stream) {
                // Incomplete frame at end of stream: drop it and report EOF.
                buf.clear();
                priv->end_of_stream = false;
                return CP_ERROR_EOF;
            }
            return CP_ERROR_EAGAIN; // 帧尚未完整，继续等待
        }

        const std::uint8_t* data = buffer_data(buf);
        const std::uint8_t address = data[1];
        const std::uint8_t function = data[2];
        const std::uint8_t payload_length = data[3];

        const std::size_t total_frame_length = frame_length_from_payload(payload_length); // 根据长度字段推算完整帧尺寸
        if (buf.size() < total_frame_length) {
            if (priv->end_of_stream) {
                // Not enough data before stream end: treat as EOF and drop buffer.
                buf.clear();
                priv->end_of_stream = false;
                return CP_ERROR_EOF;
            }
            return CP_ERROR_EAGAIN;
        }

        const std::size_t payload_offset = 4U;
        const std::size_t crc_offset = payload_offset + payload_length;
        const std::size_t end_offset = crc_offset + 2U; // CRC 后紧接 0x0D 0x0A

        const std::uint8_t crc_lo = data[crc_offset];
        const std::uint8_t crc_hi = data[crc_offset + 1U];
        const std::uint16_t crc_value = static_cast<std::uint16_t>(crc_lo) |
            static_cast<std::uint16_t>(crc_hi << 8U);

        const std::uint8_t end_first = data[end_offset];
        const std::uint8_t end_second = data[end_offset + 1U];

        if (end_first != kEndByteFirst || end_second != kEndByteSecond) {
            // Invalid end marker, drop start byte and retry.
            buf.erase(buf.begin());
            continue;
        }

        const std::size_t crc_region_length = 3U + payload_length; // address + function + length + payload
        const std::uint16_t computed_crc = crc16_modbus(data + 1U, crc_region_length);
        if (computed_crc != crc_value) {
            buf.erase(buf.begin()); // CRC 校验失败，丢弃该起始符并重新同步
            continue;
        }

        (void)address;  // 当前实现仅检查 CRC，不直接使用地址/功能码
        (void)function;

        frame.data.assign(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length));
        frame.pts = priv->next_pts++;
        frame.key_frame = true;
        frame.valid = true;

        buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length)); // 移除已消费的内容

        return CP_SUCCESS;
    }
}

const CPCodec kTactileCodec {
    "tactile_serial",
    "Framed tactile sensor serial protocol decoder",
    CPMediaType::Data,
    CPCodecID::Tactile,
    sizeof(TactileDecoderContext),
    &tactile_init,
    &tactile_close,
    &tactile_send_packet,
    &tactile_receive_frame
};

} // namespace

// 将底层 CPFrame 转换为协议专用的 TactileFrame，失败时返回 std::nullopt。
std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
    if (!frame.valid || frame.data.size() < kMinimumFrameSize) {
        return std::nullopt;
    }
    const auto* bytes = frame.data.data();
    const std::size_t size = frame.data.size();

    if (bytes[0] != kStartByte) {
        return std::nullopt;
    }
    if (bytes[size - 2] != kEndByteFirst || bytes[size - 1] != kEndByteSecond) {
        return std::nullopt;
    }

    if (size < 4U) {
        return std::nullopt;
    }

    const std::uint8_t length = bytes[3];
    if (frame_length_from_payload(length) != size) {
        return std::nullopt;
    }

    const std::uint8_t address = bytes[1];
    const FunctionCode function = static_cast<FunctionCode>(bytes[2]);
    const std::size_t payload_offset = 4U;

    TactileFrame parsed{};
    parsed.device_address = address;
    parsed.function = function;
    parsed.data_length = length;
    parsed.payload.assign(bytes + payload_offset, bytes + payload_offset + length);
    return parsed;
}

// 将数据域按照小端排列的 16 位压力值序列解析为整数数组。
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
    if (frame.payload.empty() || (frame.payload.size() % 2U != 0U)) {
        return {};
    }
    std::vector<std::uint16_t> values;
    values.reserve(frame.payload.size() / 2U);
    for (std::size_t idx = 0; idx + 1U < frame.payload.size(); idx += 2U) {
        const std::uint16_t value = static_cast<std::uint16_t>(
            static_cast<std::uint16_t>(frame.payload[idx]) |
            static_cast<std::uint16_t>(frame.payload[idx + 1U] << 8U));
        values.push_back(value);
    }
    return values;
}

// 解析矩阵尺寸载荷：两个字节分别为长边/短边。
std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame) {
    if (frame.payload.size() != 2U) {
        return std::nullopt;
    }
    MatrixSize size{};
    size.long_edge = frame.payload[0];
    size.short_edge = frame.payload[1];
    return size;
}

// 解析矩阵坐标：协议格式与尺寸一致，直接复用逻辑。
std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame) {
    return parse_matrix_size_payload(frame);
}

// 提供对外查询：返回触觉协议对应的 CPCodec 描述。
const CPCodec* tactile_codec() {
    return &kTactileCodec;
}

// 将触觉协议编解码器注册到全局列表，供 cpcodec_find_decoder 使用。
void register_tactile_codec() {
    cpcodec_register(&kTactileCodec);
}

} // namespace ffmsep::tactile