#ifndef NDT__TASK__HH__
#define NDT__TASK__HH__

#include <pcl/common/transforms.h>	
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <tf/transform_datatypes.h>

#include "./TaskQueue/BaseTask.h"

class LocateTask : public tq::BaseTask
{
public:
    enum StatusCode
    {
        eSucc=0,
        eFailed
    };
    LocateTask(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,
            tf::Pose initPose)
            :m_source(source),m_target(target),m_initPose(initPose),m_statu(eFailed){}
    
    ~LocateTask(){}
    bool GetResult(tf::Pose& pose)
    {
        if(m_statu==eSucc)
            pose=m_last_pose;
        return m_statu==eSucc;
    }

protected:
    StatusCode                          m_statu;
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_source;
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_target;
    tf::Pose                            m_initPose;
    tf::Pose                            m_last_pose;
};

class NdtTask:public LocateTask
{
public:
    NdtTask(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,
            tf::Pose initPose):LocateTask(source,target,initPose){}
    ~NdtTask(){}
    void Main();

};

class IcpTask:public LocateTask
{
public:
    IcpTask(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,
            tf::Pose initPose):LocateTask(source,target,initPose){}
    ~IcpTask(){}
    void Main();

};


tf::Pose EigenToTF(Eigen::Matrix4f mat);
Eigen::Matrix4f TFToEigen(tf::Pose pose);

#endif