#pragma once

#include "communication.pb.h"
#include "vzense/device_tof3d.h"

class VzenseTof3dProtocolConversion {
public:
    static std::string ip(const Tof3dFunctionalParams &message) {
        return message.has_ip() ? message.ip() : "";
    }

    static double searchTime(const Tof3dFunctionalParams &message) {
        return message.has_search_time() ? message.search_time() : 0;
    }

    static bool open_stream(const Tof3dFunctionalParams &message) {
        return message.has_open_stream() ? message.open_stream() : false;
    }

    static VzFrame depthFrame(const Tof3dFunctionalParams &message) {
        return message.has_depthframe() ? tof3dVzFrame2VzFrame(message.depthframe()) : VzFrame();
    }

    static VzFrame tof3dVzFrame2VzFrame(const Tof3dVzFrame &tof_frame) {
        VzFrame frame;
        frame.frameIndex = tof_frame.frameindex();
        frame.frameType = (VzFrameType)tof_frame.frametype();
        frame.pixelFormat = (VzPixelFormat)tof_frame.pixelformat();
        frame.dataLen = tof_frame.datalen();
        frame.exposureTime = tof_frame.exposuretime();
        frame.depthRange = tof_frame.depthrange();
        frame.width = tof_frame.width();
        frame.height = tof_frame.height();
        frame.deviceTimestamp = tof_frame.devicetimestamp();
        frame.pFrameData = (uint8_t*)malloc(tof_frame.datalen());

        for (int i = 0; i < frame.dataLen; i += 4) {
            frame.pFrameData[i] = (tof_frame.pframedata(i/4) >> 24) & 0xff;
            frame.pFrameData[i + 1] = (tof_frame.pframedata(i/4) >> 16) & 0xff;
            frame.pFrameData[i + 2] = (tof_frame.pframedata(i/4) >> 8) & 0xff;
            frame.pFrameData[i + 3] = (tof_frame.pframedata(i/4) >> 0) & 0xff;
        }

        return frame;
    }

    static Tof3dVzFrame tof3dVzFrame2VzFrame(const VzFrame &frame) {
        Tof3dVzFrame tof_frame;
        // 图片数据转换
        if (frame.dataLen%4 == 0) {
            for (int i = 0; i < frame.dataLen; i += 4) {
                tof_frame.add_pframedata(frame.pFrameData[i] << 24 |
                                         frame.pFrameData[i + 1] << 16 |
                                         frame.pFrameData[i + 2] << 8 |
                                         frame.pFrameData[i + 3]);
            }
        } else {
            LOG(WARNING) << "tof_frame dataLen error: " << frame.dataLen;
            return tof_frame;
        }

        tof_frame.set_frameindex(frame.frameIndex);
        tof_frame.set_frametype((Tof3dVzFrameType)frame.frameType);
        tof_frame.set_pixelformat((Tof3dVzPixelFormat)frame.pixelFormat);
        tof_frame.set_datalen(frame.dataLen);
        tof_frame.set_exposuretime(frame.exposureTime);
        tof_frame.set_depthrange(frame.depthRange);
        tof_frame.set_width(frame.width);
        tof_frame.set_height(frame.height);
        tof_frame.set_devicetimestamp(frame.deviceTimestamp);
        return tof_frame;
    }

    static void pclCloud2VzList(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, std::vector<VzVector3f> &list) {
        for (auto &point: cloud->points) {
            VzVector3f vzp;
            vzp.x = point.x;
            vzp.y = point.y;
            vzp.z = point.z;
            list.emplace_back(vzp);
        }
    }
    static void VzList2pclCloud(const std::vector<VzVector3f> &list, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
        for (auto &point: list) {
            cloud->points.emplace_back(point.x, point.y, point.z);
        }
    }

    static void Tof3dCommunicatonProtocolAddCloud(Tof3dCommunicatonProtocol &message, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
        for (auto &point: cloud->points) {
            Tof3dVzVector3f vzp;
            vzp.set_x(point.x);
            vzp.set_y(point.y);
            vzp.set_z(point.z);
            message.mutable_func_params()->add_depthcloud()->CopyFrom(vzp);
        }
    }

    static Tof3dMat CvMat2Tof3dMat(const cv::Mat &mat, uint64 stamp) {
        Tof3dMat frame;
        frame.set_rows(mat.rows);
        frame.set_cols(mat.cols);
        frame.set_type(mat.type());
        frame.set_size(mat.rows * mat.cols * mat.channels());
        frame.set_stamp(stamp);
        for (int i = 0; i < frame.size(); i += 4) {
            frame.add_data((mat.data[i + 0]  << 24) |
                           (mat.data[i + 1]  << 16) |
                           (mat.data[i + 2]  << 8) |
                           (mat.data[i + 3]  << 0));
        }
        return frame;
    }

    static cv::Mat Tof3dMat2CvMat(const Tof3dMat &tof_mat) {
        cv::Mat mat(tof_mat.rows(), tof_mat.cols(), tof_mat.type());
        for (int i = 0; i < tof_mat.size(); i += 4) {
            mat.data[i] = (tof_mat.data(i/4) >> 24) & 0xff;
            mat.data[i + 1] = (tof_mat.data(i/4) >> 16) & 0xff;
            mat.data[i + 2] = (tof_mat.data(i/4) >> 8) & 0xff;
            mat.data[i + 3] = (tof_mat.data(i/4) >> 0) & 0xff;
        }
        return mat;
    }

    static Tof3dCommunicatonProtocol runFunc(Tof3dCommunicatonProtocol &protocol) {
        Tof3dCommunicatonProtocol protocol_resp;
        protocol_resp.set_func(protocol.func());
        protocol_resp.mutable_func_params()->set_ip(protocol.func_params().ip());
        Error_manager ret;
        VzFrame irFrame;
        VzFrame depthFrame;
        cv::Mat depthMat(480, 640, CV_16UC1);
        cv::Mat irMat(480, 640, CV_8UC1);
        pcl::PointCloud<pcl::PointXYZ>::Ptr depthCloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr irCloud(new pcl::PointCloud<pcl::PointXYZ>);
        auto t = std::chrono::steady_clock::now();
        std::chrono::duration<double> cost{};
        switch (protocol.func()) {
            case SearchDevice:
                ret = DeviceTof3D::iter()->SearchDevice(searchTime(protocol.func_params()));
                break;
            case ConnectDevice:
                ret = DeviceTof3D::iter()->ConnectDevice(ip(protocol.func_params()), open_stream(protocol.func_params()));
                break;
            case ConnectAllEtcDevices:
                ret = DeviceTof3D::iter()->ConnectAllEtcDevices(open_stream(protocol.func_params()));
                break;
            case ConnectAllSearchDevices:
                ret = DeviceTof3D::iter()->ConnectAllDevices(open_stream(protocol.func_params()));
                break;
            case DisConnectDevice:
                ret = DeviceTof3D::iter()->DisConnectDevice(ip(protocol.func_params()));
                break;
            case DisConnectAllEtcDevices:
                ret = DeviceTof3D::iter()->DisConnectAllEtcDevices();
                break;
            case DisConnectAllSearchDevices:
                ret = DeviceTof3D::iter()->DisConnectAllDevices();
                break;
            case getDepthFrame:
                ret = DeviceTof3D::iter()->getDepthFrame(ip(protocol.func_params()), depthFrame);
                if (ret == SUCCESS) {
                    protocol_resp.mutable_func_params()->mutable_depthframe()->CopyFrom(tof3dVzFrame2VzFrame(depthFrame));
                }
                break;
            case getIrFrame:
                DeviceTof3D::iter()->getIrFrame(ip(protocol.func_params()), irFrame);
                if (ret == SUCCESS) {
                    protocol_resp.mutable_func_params()->mutable_irframe()->CopyFrom(tof3dVzFrame2VzFrame(irFrame));
                }
                break;
            case getDepthAndIrPicture:
                DeviceTof3D::iter()->getDepthAndIrPicture(ip(protocol.func_params()), depthFrame, irFrame);
                if (ret == SUCCESS) {
                    protocol_resp.mutable_func_params()->mutable_depthframe()->CopyFrom(tof3dVzFrame2VzFrame(irFrame));
                    protocol_resp.mutable_func_params()->mutable_irframe()->CopyFrom(tof3dVzFrame2VzFrame(irFrame));
                }
                break;
            case getDepthPointCloud:
                DeviceTof3D::iter()->getDepthPointCloud(ip(protocol.func_params()), depthCloud);
                Tof3dCommunicatonProtocolAddCloud(protocol_resp, depthCloud);
                break;
            case DepthFrame2PointCloud:
                depthFrame = tof3dVzFrame2VzFrame(protocol.func_params().depthframe());
                DeviceTof3D::iter()->DepthFrame2PointCloud(ip(protocol.func_params()), depthFrame, depthCloud);
                Tof3dCommunicatonProtocolAddCloud(protocol_resp, depthCloud);
                break;
            case Frame2Mat:
                ret = DeviceTof3D::iter()->getDepthAndIrPicture(ip(protocol.func_params()), depthFrame, irFrame);
                if (ret == SUCCESS) {
                    DeviceTof3D::iter()->Frame2Mat(depthFrame, depthMat);
                    DeviceTof3D::iter()->Frame2Mat(irFrame, irMat);
                    protocol_resp.mutable_func_params()->mutable_depthmat()->CopyFrom(CvMat2Tof3dMat(depthMat, depthFrame.deviceTimestamp));
                    protocol_resp.mutable_func_params()->mutable_irmat()->CopyFrom(CvMat2Tof3dMat(irMat, irFrame.deviceTimestamp));
                }
                break;
            default:
                break;
        }
        protocol_resp.mutable_func_params()->set_error_code(ret.get_error_code());
        protocol_resp.mutable_func_params()->set_error_level(ret.get_error_level());
        protocol_resp.mutable_func_params()->set_error_string(ret.get_error_description());
        if (ret != SUCCESS) {
            LOG(WARNING) << protocol_resp.DebugString();
        }
        return protocol_resp;
    }
private:

};



