#ifndef POINT_CLOUD_CONVERTER_H
#define POINT_CLOUD_CONVERTER_H

#include <string>
#include <vector>
#include <memory>

/**
 * @brief 简单的 3D 点结构
 */
struct Point3D
{
    float x, y, z;
    Point3D() : x(0), y(0), z(0) {}
    Point3D(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {}
};

/**
 * @brief 带颜色的 3D 点结构
 */
struct Point3DRGB
{
    float x, y, z;
    uint8_t r, g, b;
    Point3DRGB() : x(0), y(0), z(0), r(255), g(255), b(255) {}
    Point3DRGB(float _x, float _y, float _z, uint8_t _r = 255, uint8_t _g = 255, uint8_t _b = 255)
        : x(_x), y(_y), z(_z), r(_r), g(_g), b(_b) {}
};

/**
 * @brief 简单的点云容器
 */
template<typename PointT>
class SimplePointCloud
{
public:
    std::vector<PointT> points;
    std::vector<int> lineIndices;  // 每个点所属的线索引

    void clear() { points.clear(); lineIndices.clear(); }
    size_t size() const { return points.size(); }
    bool empty() const { return points.empty(); }
    void reserve(size_t n) { points.reserve(n); lineIndices.reserve(n); }
    void push_back(const PointT& pt) { points.push_back(pt); }
    void push_back(const PointT& pt, int lineIdx) {
        points.push_back(pt);
        lineIndices.push_back(lineIdx);
    }
};

using PointCloudXYZ = SimplePointCloud<Point3D>;
using PointCloudXYZRGB = SimplePointCloud<Point3DRGB>;

/**
 * @brief 点云数据转换器
 * 负责加载 txt 和 pcd 格式的点云文件
 */
class PointCloudConverter
{
public:
    PointCloudConverter();
    ~PointCloudConverter();

    /**
     * @brief 从 txt 文件加载点云（使用 CloudUtils）
     */
    int loadFromTxt(const std::string& fileName, PointCloudXYZ& cloud);
    int loadFromTxt(const std::string& fileName, PointCloudXYZRGB& cloud);

    /**
     * @brief 从 pcd 文件加载点云（简单 ASCII/Binary 解析）
     */
    int loadFromPcd(const std::string& fileName, PointCloudXYZ& cloud);
    int loadFromPcd(const std::string& fileName, PointCloudXYZRGB& cloud);

    /**
     * @brief 根据文件扩展名自动选择加载方式
     */
    int loadFromFile(const std::string& fileName, PointCloudXYZ& cloud);
    int loadFromFile(const std::string& fileName, PointCloudXYZRGB& cloud);

    /**
     * @brief 保存点云到 txt 文件（使用 CloudUtils）
     */
    int saveToTxt(const std::string& fileName, const PointCloudXYZ& cloud, int lineNum, int linePtNum);

    /**
     * @brief 旋转点云（行列转置 + 交换XY）
     * @param cloud 输入点云
     * @param rotatedCloud 输出旋转后的点云
     * @param lineNum 原始线数
     * @param linePtNum 原始每线点数
     * @param newLineNum 输出新的线数
     * @param newLinePtNum 输出新的每线点数
     */
    int rotateCloud(const PointCloudXYZ& cloud, PointCloudXYZ& rotatedCloud,
                    int lineNum, int linePtNum, int& newLineNum, int& newLinePtNum);

    /**
     * @brief 获取最后的错误信息
     */
    std::string getLastError() const { return m_lastError; }

    /**
     * @brief 获取加载的点云数量
     */
    size_t getLoadedPointCount() const { return m_loadedPointCount; }

    /**
     * @brief 获取加载的线数量
     */
    int getLoadedLineCount() const { return m_loadedLineCount; }

private:
    /**
     * @brief 获取文件扩展名（小写）
     */
    std::string getFileExtension(const std::string& fileName);

    /**
     * @brief 解析 PCD 文件头
     */
    struct PcdHeader
    {
        int width = 0;
        int height = 1;
        int points = 0;
        bool isBinary = false;
        bool hasRgb = false;
        std::vector<std::string> fields;
        std::vector<int> fieldSizes;
        std::vector<char> fieldTypes;
        int pointSize = 0;
    };

    bool parsePcdHeader(std::ifstream& file, PcdHeader& header);

    std::string m_lastError;
    size_t m_loadedPointCount;
    int m_loadedLineCount;
};

#endif // POINT_CLOUD_CONVERTER_H