#include "conversion.hpp"

bool Conversion::PackHexPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, unsigned char* data, unsigned int length) {
    data[0] = DEFAULT_BEGAIN;
    data[1] = length >> 4;
    data[2] = length;
    data[length - 1] = DEFAULT_END;
    for(int i = 0; i < cloud->size(); i++) {
        Float2Bytes(cloud->points[i].x, data, 3 + 3 * sizeof(float) * i);
        Float2Bytes(cloud->points[i].y, data, 3 + 3 * sizeof(float) * i + 1 * sizeof(float));
        Float2Bytes(cloud->points[i].z, data, 3 + 3 * sizeof(float) * i + 2 * sizeof(float));
    }
    return true;
}

bool Conversion::UnPackHexPointCloud(unsigned char *data, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    if(data[0] != DEFAULT_BEGAIN) {
        return false;
    }
    int size = data[1] << 8 | data[2];
    if(data[size - 1] != DEFAULT_END) {
        return false;
    }
    int i = 3;
    float number;
    pcl::PointXYZ point;
    while (data[i] != 0xFF || i < size - 1) {
        Bytes2Float(&data[i], &number);
        i += sizeof(float);
        point.x = number;
        Bytes2Float(&data[i], &number);
        i += sizeof(float);
        point.y = number;
        Bytes2Float(&data[i], &number);
        i += sizeof(float);
        point.z = number;
        cloud->points.push_back(point);
    }
    return true;
}

bool Conversion::PackStringPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string &data) {
    data += "Begain\n";
    data += "size:" + std::to_string(cloud->size()) + "\n";
    for(int i = 0; i < cloud->size(); i++) {
        data += std::to_string(cloud->points[i].x) + " ";
        data += std::to_string(cloud->points[i].y) + " ";
        data += std::to_string(cloud->points[i].z) + " ";
    }
    data += "\nend\n";
    return true;
}

bool Conversion::UnPackStringPointCloud(const std::string &data, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    std::string str_cloud = data.substr(data.find("\n", data.find("size")) + 1, data.find("end\n") - data.find("\n", data.find("size")) - 2);
    std::istringstream cloud_data;
    cloud_data.str(str_cloud);
    pcl::PointXYZ point;
    while (cloud_data >> point.x && cloud_data >> point.y && cloud_data >> point.z) {
        cloud->points.push_back(point);
    }
    return true;
}

bool Conversion::String2Hex(const std::string &cloud, unsigned char* data, unsigned int length) {
    std::string str_cloud;
    if (cloud.find("Begain") == 0 && cloud.find("end")) {
        str_cloud = cloud.substr(cloud.find("\n", cloud.find("size")) + 1, cloud.find("end\n") - cloud.find("\n", cloud.find("size")) - 2);
    } else {
        str_cloud = cloud;
    }
    std::istringstream cloud_data;
    cloud_data.str(str_cloud);
    pcl::PointXYZ point;
    int i = 0;
    while (cloud_data >> point.x && cloud_data >> point.y && cloud_data >> point.z) {
        Float2Bytes(point.x, data, i);
        i += sizeof(float);
        Float2Bytes(point.y, data, i);
        i += sizeof(float);
        Float2Bytes(point.z, data, i);
        i += sizeof(float);
    }
    return true;
}

void Conversion::Float2Bytes(float value, unsigned char *bytes, int pos)
{
    char *data = (char *)&value;
    for (int i = 0; i < 4; i++)
    {
        bytes[i + pos] = *data++;
    }
}

void Conversion::Bytes2Float(unsigned char * bytes, float* number) {
    void *data = number;
    unsigned char * buf = bytes;
    for (int i = 0; i < 4; i++){
        // data[i] = buf[i];
        *((unsigned char*)data + i) = *(buf + i);
    }
}