#pragma once

#include <cstddef>
#include <atomic>

#include <sophus/se3.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Dense>

#include "id/id_generator.h"


namespace Dobot {


class Frame {
public:
    using PointType = pcl::PointXYZINormal;
    using CloudType = pcl::PointCloud<PointType>;

    using Ptr = std::shared_ptr<Frame>;

    Frame() {
        id_ = next_id<Frame>();
    }
    Frame(const CloudType::Ptr& cloud, const Eigen::Matrix4d& pose)
        : cloud_(cloud), pose_(pose) {}

    size_t getId() const { return id_; }
    void setId(size_t id) { id_ = id; }

    // 获取点云
    CloudType::Ptr getCloud() const { return cloud_; }
    void setCloud(const CloudType::Ptr& cloud) { cloud_ = cloud; }

    Eigen::Matrix4d getPose() const { return pose_.matrix(); }
    Eigen::Vector3d getTranslatioin() const {return pose_.translation();}
    Eigen::Matrix3d getRotation() const {return pose_.rotationMatrix();}
    void setPose(const Eigen::Matrix4d& pose) { pose_ = Sophus::SE3<double>(pose); }
    void setPose(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation) {
        pose_.setRotationMatrix(rotation);
        pose_.translation() = translation;
    }


    Eigen::Matrix4d getWorldPose() const { return pose_in_world_; }
    void setWorldPose(const Eigen::Matrix4d& pose) { pose_in_world_ = pose; }
    void setWorldPose(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation) {
        pose_in_world_.block<3, 3>(0, 0) = rotation;
        pose_in_world_.block<3, 1>(0, 3) = translation;
    }

    // 应用变换到点云
    void transformCloud(const Eigen::Matrix4d& T) {
        for (auto& pt : cloud_->points) {
            Eigen::Vector4d p(pt.x, pt.y, pt.z, 1.0);
            p = T * p;
            pt.x = p.x();
            pt.y = p.y();
            pt.z = p.z();
        }
        // pose_ = T * pose_;
    }

private:
    size_t id_;

    CloudType::Ptr cloud_{new CloudType()};
    // Eigen::Matrix4d pose_{Eigen::Matrix4d::Identity()};
    Sophus::SE3<double> pose_;

    Eigen::Matrix4d pose_in_world_{Eigen::Matrix4d::Identity()};


};

}
