#include "rslidar_driver.h"

// 万集雷达封装类构造函数
RslidarDevice::RslidarDevice() {
    m_rs_lidar_device_status = E_UNKNOWN;
    m_decoder_ptr_ = nullptr;
}

// 万集雷达封装类析构函数
RslidarDevice::~RslidarDevice() {
    uninit();
}

Error_manager RslidarDevice::init() {
    return SUCCESS;
}

Error_manager RslidarDevice::init(RSDriverParam &param) {
    m_lidar_params = param;
    m_lidar_params.print();
    m_lidar_driver.regPointCloudCallback(std::bind(&RslidarDevice::driverGetPointCloudFromCallerCallback, this),
                                         std::bind(&RslidarDevice::driverReturnPointCloudToCallerCallback, this,
                                                   std::placeholders::_1));
    m_lidar_driver.regExceptionCallback(std::bind(&RslidarDevice::exceptionCallback, this, std::placeholders::_1));
    m_lidar_driver.regPacketCallback(std::bind(&RslidarDevice::packetCallback, this, std::placeholders::_1));
    if (m_lidar_driver.init(param)) {
        m_decoder_ptr_ = DecoderFactory<PointCloudMsg>::createDecoder(param.lidar_type, param.decoder_param);
        m_decoder_ptr_->regCallback(&decoderexceptionCallback, &decoderCallback);
        m_decoder_ptr_->point_cloud_ = m_point_cloud_;
        m_lidar_driver.start();
    } else {
        LOG(INFO) << param.input_param.msop_port << " init falture";
        return FAILED;
    }
//    mp_cloud_handle_thread = new std::thread(std::bind(&RslidarDevice::processCloud, this));

    return SUCCESS;
}

Error_manager RslidarDevice::uninit() {
    m_lidar_driver.stop();
    return Error_code::SUCCESS;
}

Error_manager RslidarDevice::get_last_cloud(std::vector<unsigned char> &data) {
    if (m_lidar_data.empty()) {
        return FAILED;
    }

    {
        std::lock_guard<std::mutex> lck(m_cloud_mutex);
        data.swap(m_lidar_data.front());
        m_lidar_data.pop_front();
    }
    DLOG(INFO) << "M.size " << data.size() / 7 << "  " << m_lidar_data.size();

    return SUCCESS;
}

bool RslidarDevice::updateRSTransformParam(RSTransformParam &transform_param) {
    m_lidar_params.decoder_param.transform_param = transform_param;
    return true;
}

bool RslidarDevice::is_ready() {
    update_status();
    return m_rs_lidar_device_status == E_READY;
}

RslidarDevice::RSLIDAR_STATUS RslidarDevice::status() {
    update_status();
    return m_rs_lidar_device_status;
}

void RslidarDevice::update_status() {
    if (m_rs_protocol_status == E_FAULT) {
        m_rs_lidar_device_status = E_FAULT;
    } else if (m_rs_protocol_status == E_BUSY) {
        m_rs_lidar_device_status = E_BUSY;
    } else if (m_rs_protocol_status == E_READY) {
        m_rs_lidar_device_status = E_READY;
    } else {
        m_rs_lidar_device_status = E_UNKNOWN;
    }
}

std::shared_ptr<PointCloudMsg> RslidarDevice::driverGetPointCloudFromCallerCallback(void) {
    std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
    if (msg.get() != nullptr) {
        return msg;
    }
    return std::make_shared<PointCloudMsg>();
}

int RslidarDevice::id() {
    return m_lidar_params.input_param.msop_port;
}

void RslidarDevice::driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg) {
    // 先将之前队列的首个给释放回调函数
    free_cloud_queue.push(stuffed_cloud_queue.pop());

    // 再存入一个
    stuffed_cloud_queue.push(msg);
    DLOG(INFO) << "Get points form driver, point size: " << msg->points.size();

    // 转换
    if (!msg->points.empty()) {
        DLOG(INFO) << m_decoder_ptr_->point_cloud_->points.begin()->x << " " << m_decoder_ptr_->point_cloud_->points.begin()->y;
        Packet packet;
        int buf_size = msg->points.size();
        float x, y, z, x_y, r, roll, yaw;

//        Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
//        // 1，定义平移
//        transform_2.translation() << m_lidar_params.decoder_param.transform_param.x, m_lidar_params.decoder_param.transform_param.y, m_lidar_params.decoder_param.transform_param.z;
//
//        // The same rotation matrix as before; theta radians around Z axis
//        // 2，定义然后Z旋转
//        transform_2.rotate(Eigen::AngleAxisf(m_lidar_params.decoder_param.transform_param.roll, Eigen::Vector3f::UnitX()));  // UnitZ 即z轴
//        transform_2.rotate(Eigen::AngleAxisf(m_lidar_params.decoder_param.transform_param.pitch, Eigen::Vector3f::UnitY()));  // UnitZ 即z轴
//        transform_2.rotate(Eigen::AngleAxisf(m_lidar_params.decoder_param.transform_param.yaw, Eigen::Vector3f::UnitZ()));  // UnitZ 即z轴
//        // Executing the transformation
//        // 定义存放变换后的点云对象。
//        pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZI>());
//        for (auto &point: msg->points) {
//            pcl::PointXYZI p;
//            p.x = point.x;
//            p.x = point.y;
//            p.x = point.z;
//            p.intensity = point.intensity;
//            transformed_cloud->points.emplace_back(p);
//        }
//
//        // You can either apply transform_1 or transform_2; they are the same
//        pcl::transformPointCloud(*transformed_cloud, *transformed_cloud, transform_2);
//
//        WriteTxtCloud(ETC_PATH"/etc/transformed_cloud.txt", transformed_cloud);
//        for (auto &point: transformed_cloud->points) {
//            x = point.x * 100;
//            y = point.y * 100;
//            z = point.z * 100;
//            r = sqrt(x * x + y * y + z * z);
//            x_y = sqrt(x * x + y * y);
//            roll = atan2(z, x_y);
//            yaw = atan2(x, y);
//            roll = (roll * 18000) / 3.1415926535;
//            yaw = (yaw * 18000) / 3.1415926535;
//
//            if (roll < 0) {
//                roll += 36000;
//            }
//            if (roll > 36000) {
//                roll -= 36000;
//            }
//            if (yaw < 0) {
//                yaw += 36000;
//            }
//            if (yaw > 36000) {
//                yaw -= 36000;
//            }
//            r = r * 4;
//
//            m_tmp_data.emplace_back((unsigned short) r >> 8);
//            m_tmp_data.emplace_back((unsigned short) r);
//            m_tmp_data.emplace_back((unsigned short) roll >> 8);
//            m_tmp_data.emplace_back((unsigned short) roll);
//            m_tmp_data.emplace_back((unsigned short) yaw >> 8);
//            m_tmp_data.emplace_back((unsigned short) yaw);
//            m_tmp_data.emplace_back(point.intensity);
//        }
        for (int i = 0; i < buf_size; i++) {
            // roll旋转
            double angle = (m_lidar_params.decoder_param.transform_param.roll * M_PI) / 180.0f;
            y = msg->points[i].y * std::cos(angle) - msg->points[i].z * std::sin(angle);
            z = msg->points[i].y * std::sin(angle) + msg->points[i].z * std::cos(angle);
            msg->points[i].y = y;
            msg->points[i].z = z;
            // pitch旋转
            angle = (m_lidar_params.decoder_param.transform_param.pitch * M_PI) / 180.0f;
            x = msg->points[i].x * std::cos(angle) + msg->points[i].z * std::sin(angle);
            z = msg->points[i].z * std::cos(angle) - msg->points[i].x * std::sin(angle);
            msg->points[i].x = x;
            msg->points[i].z = z;
            // yaw旋转
            angle = (m_lidar_params.decoder_param.transform_param.yaw * M_PI) / 180.0f;
            x = msg->points[i].x * std::cos(angle) - msg->points[i].y * std::sin(angle);
            y = msg->points[i].x * std::sin(angle) + msg->points[i].y * std::cos(angle);
            msg->points[i].x = x;
            msg->points[i].y = y;
            x = x * 100.0f + m_lidar_params.decoder_param.transform_param.x ;
            y = y * 100.0f + m_lidar_params.decoder_param.transform_param.y;
            z = z * 100.0f + m_lidar_params.decoder_param.transform_param.z;
            r = sqrt(x * x + y * y + z * z);
            x_y = sqrt(x * x + y * y);
            roll = atan2(z, x_y);
            yaw = atan2(x, y);
            roll = (roll * 18000) / 3.1415926535;
            yaw = (yaw * 18000) / 3.1415926535;

            if (roll < 0) {
                roll += 36000;
            }
            if (roll > 36000) {
                roll -= 36000;
            }
            if (yaw < 0) {
                yaw += 36000;
            }
            if (yaw > 36000) {
                yaw -= 36000;
            }
            r = r * 4;

            m_tmp_data.emplace_back((unsigned short) r >> 8);
            m_tmp_data.emplace_back((unsigned short) r);
            m_tmp_data.emplace_back((unsigned short) roll >> 8);
            m_tmp_data.emplace_back((unsigned short) roll);
            m_tmp_data.emplace_back((unsigned short) yaw >> 8);
            m_tmp_data.emplace_back((unsigned short) yaw);
            m_tmp_data.emplace_back(msg->points[i].intensity);
        }

//        std::chrono::duration<double> diff = std::chrono::steady_clock::now() - m_capture_time;
//        if (diff.count() * 1000 > 10) {
//                int tmp_size = m_tmp_data.size() / 7;
//                for (int i = tmp_size; i < 2880 * 10; i++) {
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                }
            m_capture_time = std::chrono::steady_clock::now();
            std::lock_guard<std::mutex> lck(m_cloud_mutex);
            DLOG(INFO) << "save cloud data size: " << m_tmp_data.size() / 7;
            m_lidar_data.emplace_back(m_tmp_data);
            m_tmp_data.clear();
            if (m_lidar_data.size() > 100) {
                m_lidar_data.pop_front();
            }
//        }
    }
}

void RslidarDevice::exceptionCallback(const Error &code) {

}

void RslidarDevice::packetCallback(const Packet &pkt) {
    return;
    if (!pkt.is_difop && m_timestamp != pkt.timestamp) {
        bool ret = false;
        m_decoder_ptr_->processMsopPkt(pkt.buf_.data(), pkt.buf_.size());
        if (m_decoder_ptr_->point_cloud_->points.size() > 0) {
            DLOG(INFO) << m_decoder_ptr_->point_cloud_->points.begin()->x << " " << m_decoder_ptr_->point_cloud_->points.begin()->y;
            Packet packet;
            int buf_size = m_decoder_ptr_->point_cloud_->points.size();
            float x, y, z, x_y, r, roll, yaw;

            for (int i = 0; i < buf_size; i++) {
                // yaw旋转
                double angle = (m_lidar_params.decoder_param.transform_param.yaw * M_PI) / 180.0f;
                x = m_decoder_ptr_->point_cloud_->points[i].x * std::cos(angle) - m_decoder_ptr_->point_cloud_->points[i].y * std::sin(angle);
                y = m_decoder_ptr_->point_cloud_->points[i].x * std::sin(angle) + m_decoder_ptr_->point_cloud_->points[i].y * std::cos(angle);
                // roll旋转
                angle = (m_lidar_params.decoder_param.transform_param.roll * M_PI) / 180.0f;
                m_decoder_ptr_->point_cloud_->points[i].y = y;
                y = m_decoder_ptr_->point_cloud_->points[i].y * std::cos(angle) - m_decoder_ptr_->point_cloud_->points[i].z * std::sin(angle);
                z = m_decoder_ptr_->point_cloud_->points[i].y * std::sin(angle) + m_decoder_ptr_->point_cloud_->points[i].z * std::cos(angle);
                // pitch旋转
                angle = (m_lidar_params.decoder_param.transform_param.pitch * M_PI) / 180.0f;
                m_decoder_ptr_->point_cloud_->points[i].x = x;
                m_decoder_ptr_->point_cloud_->points[i].z = z;
                x = m_decoder_ptr_->point_cloud_->points[i].x * std::cos(angle) - m_decoder_ptr_->point_cloud_->points[i].z * std::sin(angle);
                z = m_decoder_ptr_->point_cloud_->points[i].x * std::sin(angle) + m_decoder_ptr_->point_cloud_->points[i].z * std::cos(angle);
                x = x * 100.0f + m_lidar_params.decoder_param.transform_param.x ;
                y = y * 100.0f + m_lidar_params.decoder_param.transform_param.y;
                z = z * 100.0f + m_lidar_params.decoder_param.transform_param.z;
                r = sqrt(x * x + y * y + z * z);
                x_y = sqrt(x * x + y * y);
                roll = atan2(z, x_y);
                yaw = atan2(x, y);
                roll = (roll * 18000) / 3.1415926535;
                yaw = (yaw * 18000) / 3.1415926535;

                if (roll < 0) {
                    roll += 36000;
                }
                if (roll > 36000) {
                    roll -= 36000;
                }
                if (yaw < 0) {
                    yaw += 36000;
                }
                if (yaw > 36000) {
                    yaw -= 36000;
                }
                r = r * 4;

                m_tmp_data.emplace_back((unsigned short) r >> 8);
                m_tmp_data.emplace_back((unsigned short) r);
                m_tmp_data.emplace_back((unsigned short) roll >> 8);
                m_tmp_data.emplace_back((unsigned short) roll);
                m_tmp_data.emplace_back((unsigned short) yaw >> 8);
                m_tmp_data.emplace_back((unsigned short) yaw);
                m_tmp_data.emplace_back(m_decoder_ptr_->point_cloud_->points[i].intensity);
            }

            m_decoder_ptr_->point_cloud_->points.clear();
            std::chrono::duration<double> diff = std::chrono::steady_clock::now() - m_capture_time;
            if (diff.count() * 1000 > 10) {
//                int tmp_size = m_tmp_data.size() / 7;
//                for (int i = tmp_size; i < 2880 * 10; i++) {
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                    m_tmp_data.emplace_back(0);
//                }
                m_capture_time = std::chrono::steady_clock::now();
                std::lock_guard<std::mutex> lck(m_cloud_mutex);
                m_lidar_data.emplace_back(m_tmp_data);
                m_tmp_data.clear();
                if (m_lidar_data.size() > 100) {
                    DLOG(INFO) << "pop data";
                    m_lidar_data.pop_front();
                }
            }
        }
    } else {
        m_decoder_ptr_->processDifopPkt(pkt.buf_.data(), pkt.buf_.size());
    }
}
