Measure/include/message/unpacket.hpp

#pragma once

#include <cmath>
#include <map>
#include "defines.hpp"

class CloudPointUnpacket {
public:
    template<typename T>
    bool unpacket(unsigned char *data, size_t size, T &cloud) {
        pcl::PointCloud<pcl::PointALL>::Ptr all(new pcl::PointCloud<pcl::PointALL>);
        return unpacketFactory(data, size, all) && transformed(all, cloud);
    }

private:
    static bool unpacketFactory(unsigned char *data, size_t size, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
        if (!checkPacket(data, size)) {
            return false;
        }

        switch (data[2] << 8 | data[3]) {
            case ZX::PointXYZ_TYPE:
                DLOG(INFO) << "Unpack PointXYZ_TYPE";
                return unpacketXYZ(data, size, cloud);
            case ZX::PointXYZI_TYPE:
                DLOG(INFO) << "Unpack PointXYZ_TYPE";
                return unpacketXYZI(data, size, cloud);
            case ZX::PointXYZPYR_TYPE:
                DLOG(INFO) << "Unpack PointXYZPYR_TYPE";
                return unpacketXYZPYR(data, size, cloud);
            case ZX::PointRYR_TYPE:
                DLOG(INFO) << "Unpack PointRYR_TYPE";
                return unpacketRYR(data, size, cloud);
            case ZX::LIDAR_RS_HELIOS_16P:
//                DLOG(INFO) << "Unpack LIDAR_RS_HELIOS_16P";
                return unpacketRYR(data, size, cloud, 0.25);
            default:
                LOG(WARNING) << "Data types are not compatible.";
                return false;
        }
    }

    static bool unpacketXYZ(unsigned char *data, size_t size, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
        return false;
    }

    static bool unpacketXYZI(unsigned char *data, size_t size, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
        return false;
    }

    static bool unpacketXYZPYR(unsigned char *data, size_t size, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
        return false;
    }

    static bool unpacketRYR(const unsigned char *data, size_t size, pcl::PointCloud<pcl::PointALL>::Ptr &cloud, double acc = 1) {
        pcl::PointALL point{};
        double distance, roll_radian, yaw_radian;
        for (int i = ZX::PACKET_HEAD_SIZE; i < size - ZX::PACKET_END_SIZE; i += 7) {
            distance = (float) ((data[i + 0] << 8 | data[i + 1]) * acc) * 0.01; // 当前距离精度为m，去掉0.01为cm
            point.roll = (double) ((data[i + 2] << 8 | data[i + 3]) * 0.01);
            point.yaw = (double) ((data[i + 4] << 8 | data[i + 5]) * 0.01);
            point.pitch = 0;
            roll_radian = point.roll * M_PI / 180.0;
            yaw_radian = point.yaw * M_PI / 180.0;
            point.x = distance * cos(roll_radian) * sin(yaw_radian);
            point.y = distance * cos(roll_radian) * cos(yaw_radian);
            point.z = distance * sin(roll_radian);
            point.r = distance;
            point.intensity = data[i + 6];
            cloud->push_back(point);
        }
        return true;
    }

    static bool transformed(pcl::PointCloud<pcl::PointALL>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &result) {
        pcl::PointXYZ ret;
        for (const auto point: *cloud) {
            ret.x = point.x;
            ret.y = point.y;
            ret.z = point.z;
            result->push_back(ret);
        }
        return true;
    }

    static bool transformed(pcl::PointCloud<pcl::PointALL>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr &result) {
        pcl::PointXYZI ret;
        for (const auto point: *cloud) {
            ret.x = point.x;
            ret.y = point.y;
            ret.z = point.z;
            ret.intensity = point.intensity;
            result->push_back(ret);
        }
        return true;
    }

    static bool transformed(pcl::PointCloud<pcl::PointALL>::Ptr &cloud, pcl::PointCloud<pcl::PointRPRY>::Ptr &result) {
        pcl::PointRPRY ret{};
        for (const auto point: *cloud) {
            ret.r = point.r;
            ret.pitch = point.pitch;
            ret.roll = point.roll;
            ret.yaw = point.yaw;
            result->push_back(ret);
        }
        return true;
    }

    static bool transformed(pcl::PointCloud<pcl::PointALL>::Ptr &cloud, pcl::PointCloud<pcl::PointRYR>::Ptr &result) {
        pcl::PointRYR ret{};
        for (const auto point: *cloud) {
            ret.roll = point.roll;
            ret.yaw = point.yaw;
            ret.r = point.r;
            result->push_back(ret);
        }
        return true;
    }

    static bool transformed(pcl::PointCloud<pcl::PointALL>::Ptr &cloud, pcl::PointCloud<pcl::PointALL>::Ptr &result) {
        result = cloud;
        return true;
    }

    static bool checkPacket(const unsigned char *data, size_t size) {
        if (size < ZX::PACKET_HEAD_SIZE + ZX::PACKET_END_SIZE) {
            LOG(WARNING) << "Data size too small.";
            return false;
        }

        if (!(data[0] == '$' && data[1] == '&' && data[size - 2] == '&' && data[size - 1] == '$')) {
            LOG(WARNING) << "Data head or end error.";
            return false;
        }

        if ((data[2] << 8 | data[3]) < ZX::UNKNOW_TYPE || (data[2] << 8 | data[3]) > ZX::PointTYPE_MAX) {
            LOG(WARNING) << "Data type error.";
            return false;
        }
        return true;
    }

};