#include <ros/ros.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/TransformStamped.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <pcl_ros/transforms.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/transformation_estimation_lm.h>
#include <pcl/registration/transformation_estimation_dual_quaternion.h>

#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include "conio.h"
#include <iostream>
#include <fstream>

#include <url3Msg/url3Msg.h>
#include <Eigen/Dense>

using namespace sensor_msgs;
using namespace message_filters;
using namespace cv;
using namespace std;
using namespace Eigen;

template<typename T>
T getParam(ros::NodeHandle& n, const std::string& name, const T& defaultValue)
{
        T v;
        if (n.getParam(name, v))
        {
                ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v);
                return v;
        }
        else
                ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue);
        return defaultValue;
}

tf::Transform tfFromEigen(Eigen::Matrix4f trans)
{
  tf::Matrix3x3 btm;
  btm.setValue(trans(0, 0), trans(0, 1), trans(0, 2),
               trans(1, 0), trans(1, 1), trans(1, 2),
               trans(2, 0), trans(2, 1), trans(2, 2));
  tf::Transform ret;
  ret.setOrigin(tf::Vector3(trans(0, 3), trans(1, 3), trans(2, 3)));
  ret.setBasis(btm);
  return ret;
}
Eigen::Matrix4f EigenFromTF(tf::Transform trans)
{
  Eigen::Matrix4f out;
  tf::Quaternion quat = trans.getRotation();
  tf::Vector3 origin = trans.getOrigin();

  Eigen::Quaternionf quat_out(quat.w(), quat.x(), quat.y(), quat.z());
  Eigen::Vector3f origin_out(origin.x(), origin.y(), origin.z());

  out.topLeftCorner<3, 3>() = quat_out.toRotationMatrix();
  out.topRightCorner<3, 1>() = origin_out;
  out(3, 3) = 1;

  return out;
}

class hand_eye_calib{
	private:
		ros::NodeHandle nh;
        typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
		message_filters::Subscriber<Image> *image_sub, *depth_sub;
		typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
		Synchronizer<MySyncPolicy> *sync;
        tf::TransformBroadcaster br;

        ros::Publisher pub_image_pcl, pub_robot_pcl;

        Mat cameraM, distC, projM;
        Eigen::Matrix4f T, ef2Origin, base2EF, phy2image, ScalingM, ScalingMInv, phy2imageEarly;
        float scaleUnit;
        tf::Transform transform;
        tf::Transform base2EF_tr, ef2Origin_tr;
        PointCloud physical_points_subset_, image_points_subset_;

        bool isCalib;
        tf::TransformListener listener;
        tf::StampedTransform tr;
        double cx, cy, fx, fy, actCalErr;

        string kinectFileName, robotFileName, Path;
        void callback(const ImageConstPtr& image, const ImageConstPtr& depth);
        void calibrate(PointCloud physical_points_, PointCloud image_points_, Mat &image);
        void projectPts( PointCloud physical_points_, Mat &image, vector<Point2f> &depthBuf, vector<Point2f> pointBuf);
        void getPhyPclFromTF(PointCloud &physical_points_);
        void transform_polar_to_cart(pcl::PointXYZ pti, pcl::PointXYZ &pto);
        void transform_cart_to_polar(pcl::PointXYZ pti, pcl::PointXYZ &pto);

        string kinect_frame, robot_base_frame;
#if 0

    cv::Mat color, depth;
    cv::Mat lookupX, lookupY;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;

    bool belongs(int r, int c, vector<Point2f> points, int &pos){
    }

  void createCloud(const cv::Mat &depth, const cv::Mat &color, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, vector<Point2f> pointBuf, PointCloud &actCloud) const
  {
    const float badPoint = std::numeric_limits<float>::quiet_NaN();
    actCloud.points.clear();
    for(int i=0;i<pointBuf.size();i++){
        pcl::PointXYZ pt;
        actCloud.points.push_back(pt);
    }
    #pragma omp parallel for

    for(int r = 0; r < depth.rows; ++r)
    {
      pcl::PointXYZ *itP = &cloud->points[r * depth.cols];
      const uint16_t *itD = depth.ptr<uint16_t>(r);
      const cv::Vec3b *itC = color.ptr<cv::Vec3b>(r);
      const float y = lookupY.at<float>(0, r);
      const float *itX = lookupX.ptr<float>();

      for(size_t c = 0; c < (size_t)depth.cols; ++c, ++itP, ++itD, ++itC, ++itX)
      {
        register const float depthValue = *itD / 1000.0f;
        // Check for invalid measurements
        if(isnan(depthValue) || depthValue <= 0.001)
        {
          // not valid
          itP->x = itP->y = itP->z = badPoint;
//          itP->rgba = 0;
          continue;
        }
        itP->z = depthValue;
        itP->x = *itX * depthValue;
        itP->y = y * depthValue;
        int pos=-1;
        bool f = false;
        for(int i=0;i<pointBuf.size();i++)
            if(r==(int)pointBuf[i].y && c==(int)pointBuf[i].x){
                pos = i;
                f = true;
            }
        if(f){
            actCloud.points[pos].x = itP->x;
            actCloud.points[pos].y = itP->y;
            actCloud.points[pos].z = itP->z;
            cout<<pos<<" "<<r<<" "<<c<<" "<<depthValue<<" "<<*itX<<" "<<y<<"\t"<<itP->x<<" "<<itP->y<<" "<<itP->z<<endl;
        }
#if 0
        itP->b = itC->val[0];
        itP->g = itC->val[1];
        itP->r = itC->val[2];
        itP->a = 255;
#endif
      }
    }
  }

  void createLookup(size_t width, size_t height)
  {
    const float fx = 1.0f / cameraM.at<double>(0, 0);
    const float fy = 1.0f / cameraM.at<double>(1, 1);
    const float cx = cameraM.at<double>(0, 2);
    const float cy = cameraM.at<double>(1, 2);
    float *it;

    lookupY = cv::Mat(1, height, CV_32F);
    it = lookupY.ptr<float>();
    for(size_t r = 0; r < height; ++r, ++it)
    {
      *it = (r - cy) * fy;
    }

    lookupX = cv::Mat(1, width, CV_32F);
    it = lookupX.ptr<float>();
    for(size_t c = 0; c < width; ++c, ++it)
    {
      *it = (c - cx) * fx;
    }
  }
#endif

	public:
		hand_eye_calib();
		~hand_eye_calib();
};

hand_eye_calib::hand_eye_calib()
	:nh("~")
{
	image_sub = new message_filters::Subscriber<Image> (nh,"/kinect2/hd/image_color_rect",1);
	depth_sub = new message_filters::Subscriber<Image> (nh,"/kinect2/hd/image_depth_rect",1);

	sync = new Synchronizer<MySyncPolicy> (MySyncPolicy(10), *image_sub, *depth_sub);
	sync->registerCallback(boost::bind(&hand_eye_calib::callback, this, _1, _2));
    cameraM = Mat::zeros(3,3,CV_32F);
    string cameraFile = getParam<string> (nh, "camera_calib_file","/home/ir/kinect2WS/src/iai_kinect2/kinect2_bridge/data/507554242542/calib_color.yaml");
    FileStorage fs2(cameraFile.c_str(), FileStorage::READ);
    fs2["cameraMatrix"]>>cameraM;
    fs2["distortionCoefficients"]>>distC;
    fs2["projection"]>>projM;
    fs2.release();
    fx = cameraM.at<double>(0,0);
    fy = cameraM.at<double>(1,1);
    cx = cameraM.at<double>(0,2);
    cy = cameraM.at<double>(1,2);
#if 0
    this->color.cols = 1920;
    this->color.rows = 1080;
    this->cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
    this->cloud->header.frame_id =  "kinect2_rgb_optical_frame";
    this->cloud->height = this->color.rows;
    this->cloud->width = this->color.cols;
    this->cloud->is_dense = false;
    this->cloud->points.resize(this->cloud->height * this->cloud->width);
    createLookup(this->color.cols, this->color.rows);
#endif
//    projM = projM.inv(DECOMP_LU);
    phy2imageEarly = Eigen::Matrix4f::Identity();
    isCalib = getParam<bool> (nh,"calibrated",false);
    if(isCalib){
        ifstream fpCalib;
        fpCalib.open("/home/ir/perception_ws/src/hand_eye_calib/src/hand_eye_calib.txt");
        double temp=0.0;
        for(size_t i=0;i<4;i++)
        for(size_t j=0;j<4;j++){
            fpCalib>>temp;
            phy2imageEarly(i,j) = temp;
        }
        phy2image = phy2imageEarly;
        transform = tfFromEigen(phy2image);

    }
    cout<<phy2imageEarly<<endl;
    listener.waitForTransform("robot_base",  "end_effector", ros::Time::now(), ros::Duration(0));
    try{
        listener.lookupTransform("robot_base",  "end_effector", ros::Time(0), tr);
        //ROS_INFO_STREAM(tr.getOrigin().x());
    }
    catch (tf::TransformException ex){
//      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
    ef2Origin = Eigen::Matrix4f::Identity();
    ef2Origin(2,3) = 0.0832;
    ef2Origin(1,3) = -0.0112;
    scaleUnit =1;//00.0;//100.0;
    ScalingM = Eigen::Matrix4f::Identity();
    ScalingM *= scaleUnit;
    ScalingM(3,3) = 1.0;
    ScalingMInv = ScalingM.inverse();
    ef2Origin = ScalingM * ef2Origin * ScalingMInv;
    ef2Origin_tr = tfFromEigen(ef2Origin);

    pub_image_pcl = nh.advertise<PointCloud>("/imagepcl",10);
    pub_robot_pcl = nh.advertise<PointCloud>("/robotpcl",10);

    kinect_frame = "/kinect2_rgb_optical_frame";
    robot_base_frame = "/robot_base";
}

hand_eye_calib::~hand_eye_calib(){

}

void hand_eye_calib::callback(const ImageConstPtr& image, const ImageConstPtr& depth)
{
  // Solve all of perception here...
	//ROS_INFO_STREAM("COOL");
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), kinect_frame, robot_base_frame));
    cv_bridge::CvImagePtr cv_ptr, depth_ptr;
    try
    {
        Mat tempMat;
        cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
//        tempMat = cv_ptr->image.clone();
        depth_ptr = cv_bridge::toCvCopy(depth, sensor_msgs::image_encodings::TYPE_16UC1);

//        this->color = cv_ptr->image.clone();
//        this->depth = depth_ptr->image.clone();

#if 1
//        undistort(tempMat, cv_ptr->image, cameraM, distC);
        vector<Point2f> pointBuf;
        Size boardSize(7,5);
        Mat view = cv_ptr->image.clone();
        bool found = findChessboardCorners( view, boardSize, pointBuf, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);

        if ( found)                // If done with success,
        {
              // improve the found corners' coordinate accuracy for chessboard
            Mat viewGray;
            cvtColor(view, viewGray, COLOR_BGR2GRAY);
            cornerSubPix( viewGray, pointBuf, Size(11,11), Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));
//            drawChessboardCorners( view, boardSize, Mat(pointBuf), found );
            PointCloud image_points_, physical_points_;
            float max_depth = 0.0;
            ROS_INFO_STREAM("Detected the pattern "<<pointBuf.size());
#if 1
            int winSz = 1;
            double sc = 1.0/(double)(winSz*winSz);
            for(size_t i=0;i<pointBuf.size();i++){
                pcl::PointXYZ pt, cpt;
                pt.x = pointBuf[i].x;///(float)cameraM.at<double>(0,0);
                pt.y = pointBuf[i].y;///(float)cameraM.at<double>(1,1);
                pt.z = (static_cast<double>(depth_ptr->image.at<uint16_t>(pointBuf[i])))/1000.0;
                transform_polar_to_cart(pt,cpt);
//                cout<<i<<" "<<pt.x<<" "<<pt.y<<" "<<pt.z<<"\t"<<cpt.x<<" "<<cpt.y<<" "<<cpt.z<<endl;
                image_points_.push_back(cpt);
            }
#endif

            image_points_.header.frame_id = kinect_frame;

//            createCloud(this->depth,this->color,cloud,pointBuf,image_points_);
            pub_image_pcl.publish(image_points_);
//            pub_image_pcl.publish(image_points_);
            getPhyPclFromTF(physical_points_);
            physical_points_.header.frame_id = robot_base_frame;
            pub_robot_pcl.publish(physical_points_);

#if 1
            //ROS_INFO_STREAM(physical_points_.points.size()<<" "<<image_points_.points.size());
            if(!isCalib){
                char kb = getche();
                if(kb==' '){
                    calibrate(physical_points_, image_points_, view);
                    vector<Point2f> projected_pts;
                    projectPts(physical_points_, view, projected_pts, pointBuf);
/*
                double dist = 0.0;
                for(size_t k=0;k<pointBuf.size();k++)
                    dist += sqrt(pow(pointBuf[k].x - projected_pts[k].x,2) + pow(pointBuf[k].y - projected_pts[k].y,2));
                ROS_INFO_STREAM("Calibration error "<<dist/(double)pointBuf.size());
                if(dist/(double)pointBuf.size()<20)
                    isCalib = true;
*/
                }
                else{
                    if(kb=='d'){
                        isCalib = true;
                        ofstream fpCalib;
                        fpCalib.open("/home/ir/perception_ws/src/hand_eye_calib/src/hand_eye_calib.txt");
                        fpCalib<<phy2image;
                        fpCalib.close();
                    }
                    phy2image = phy2imageEarly;
                    vector<Point2f> projected_pts;
                    projectPts(physical_points_, view, projected_pts, pointBuf);
                }
            }
            else{
                phy2image = phy2imageEarly;
                vector<Point2f> projected_pts;
                projectPts(physical_points_, view, projected_pts, pointBuf);
            }
            if(!isCalib){
            imshow("result",view);
            waitKey(1);
            }
#endif

        }

        ROS_INFO_STREAM(endl<<phy2imageEarly);
#endif
    }
    catch (cv_bridge::Exception& e){
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
}

void hand_eye_calib::getPhyPclFromTF(PointCloud &physical_points_){
    physical_points_.points.clear();
    try{
        listener.lookupTransform(robot_base_frame,  "end_effector", ros::Time(0), tr);
        //ROS_INFO_STREAM(tr.getOrigin().x());
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
    base2EF_tr.setOrigin(tr.getOrigin());
    base2EF_tr.setRotation(tr.getRotation());
    //cout<<tr.getOrigin().getX()<<" "<<tr.getOrigin().getY()<<" "<<tr.getOrigin().getZ()<<" "<<endl;
    base2EF = EigenFromTF(base2EF_tr);
    base2EF = ScalingM * base2EF * ScalingMInv;

//    T =  base2EF.inverse() * ef2Origin.inverse();
//    T =   ef2Origin * base2EF ;
    T =   base2EF * ef2Origin;
//    T = base2EF * ef2Origin.inverse();
//    T = base2EF.inverse() * ef2Origin;
    //cout<<"Base to End effector\n"<<T<<endl;
    for(int j=1;j<6;j++)
    for(int i=3;i>=-3;i--){
        Eigen::Vector4f Pt((float)i*0.0300*scaleUnit,0.0,(float)j*0.0300*scaleUnit,1.0);
        Eigen::Vector4f Tpt = T * Pt;
        pcl::PointXYZ pt;
        if(fabs(Tpt(3))>0.1){
            Tpt = Tpt / Tpt(3);
            pt.getVector4fMap() = Tpt;
        }
        else{
            pt.z = 0.0;
        }
#if 0
        pt.x = pt.x * 100.0;
        pt.y = pt.y * 100.0;
        pt.z = pt.z * 100.0;
#endif
        physical_points_.push_back(pt);
    }
}

void hand_eye_calib::calibrate(PointCloud physical_points_, PointCloud image_points_, Mat &image){
//    physical_points_subset_.points.clear();
//    image_points_subset_.points.clear();
//    pcl::registration::TransformationEstimationSVD < pcl::PointXYZ, pcl::PointXYZ > svd_estimator;
    pcl::registration::TransformationEstimationDualQuaternion < pcl::PointXYZ, pcl::PointXYZ > svd_estimator;
//    pcl::registration::TransformationEstimationLM < pcl::PointXYZ, pcl::PointXYZ > svd_estimator;
    vector<int> pos;
    for(int i=0;i<physical_points_.points.size();i++)
        pos.push_back(i);
#if 0
    pos.push_back(1);
    pos.push_back(34);
    pos.push_back(3);
    pos.push_back(7);
    pos.push_back(20);
    pos.push_back(7);
    pos.push_back(12);
    pos.push_back(5);
#endif
    for(int i=0;i<pos.size();i++){
        pcl::PointXYZ pt;
        pt.x = image_points_.points[pos[i]].x;
        pt.y = image_points_.points[pos[i]].y;
        pt.z = image_points_.points[pos[i]].z;
//        transform_polar_to_cart(image_points_.points[pos[i]], pt);
        if(pt.z>0 && fabs(physical_points_.points[pos[i]].z)>0.001){
            physical_points_subset_.push_back(physical_points_.points[pos[i]]);
            image_points_subset_.push_back(pt);
        }
    }
    image_points_subset_.header.frame_id = "/kinect2_rgb_optical_frame";
//    pub_image_pcl.publish(image_points_subset_);
    if(physical_points_subset_.points.size()>physical_points_.points.size()){
        ROS_INFO_STREAM("Doing calibration");
#if 0
        for(size_t i=0;i<physical_points_subset_.points.size();i++){
            cout<<physical_points_subset_.points[i].x<<" "<<physical_points_subset_.points[i].y<<" "<<physical_points_subset_.points[i].z<<"\t"<<image_points_subset_.points[i].x<<" "<<image_points_subset_.points[i].y<<" "<<image_points_subset_.points[i].z<<" "<<endl;
        }
#endif
        svd_estimator.estimateRigidTransformation(physical_points_subset_, image_points_subset_, phy2image);
        PointCloud test;
        transformPointCloud(physical_points_subset_, test, phy2image);
        double testD = 0.0;
        for(size_t ii=0;ii<physical_points_subset_.points.size();ii++)
            testD += sqrt(pow(test.points[ii].x-image_points_subset_.points[ii].x,2.0) + pow(test.points[ii].y-image_points_subset_.points[ii].y,2.0) + pow(test.points[ii].z-image_points_subset_.points[ii].z,2.0))/(double)(physical_points_subset_.points.size());
        ROS_INFO_STREAM("Calibration ERROR "<<testD);
        actCalErr = testD;

        transform = tfFromEigen(phy2image);
    }
    else
        ROS_INFO_STREAM("Number of points is "<<physical_points_subset_.points.size()<<" ; hence not doing calibration");
}

void hand_eye_calib::transform_polar_to_cart(pcl::PointXYZ pti, pcl::PointXYZ &pto){
#if 1
    fx = 1.0f / cameraM.at<double>(0, 0);
    fy = 1.0f / cameraM.at<double>(1, 1);
    pto.z = pti.z;
    pto.y = (pti.y - cy)*pto.z*fy;
    pto.x = (pti.x - cx)*pto.z*fx;
#endif
#if 0
    Mat ptIn = Mat::zeros(3,1,cameraM.type());
    Mat ptOut = ptIn.clone();
    ptIn.at<double>(0) = pti.x*pti.z;
    ptIn.at<double>(1) = pti.y*pti.z;
    ptIn.at<double>(2) = pti.z;
    Mat cameraMInv = Mat::zeros(cameraM.size(),cameraM.type());
    invert(cameraM, cameraMInv, DECOMP_LU);
    ptOut = cameraMInv * ptIn;
    cout<<pto.x<<" "<<ptOut.at<double>(0)<<" "<<pto.y<<" "<<ptOut.at<double>(1)<<" "<<pto.z<<" "<<ptOut.at<double>(2)<<" "<<endl;
    pto.x = ptOut.at<double>(0);
    pto.y = ptOut.at<double>(1);
    pto.z = ptOut.at<double>(2);
#endif
//    cout<<"From Image space to camera space:"<<pti.x<<" "<<pti.y<<" "<<pti.z<<"\t"<<pto.x<<" "<<pto.y<<" "<<pto.z<<endl;
}

void hand_eye_calib::transform_cart_to_polar(pcl::PointXYZ pti, pcl::PointXYZ &pto){
#if 0
    pto.z = pti.z;
    pto.x = pti.x*fx/pto.z + cx;
    pto.y = pti.y*fy/pto.z + cy;
#endif
#if 1
    Mat ptIn = Mat::zeros(3,1,cameraM.type());
    Mat ptOut = ptIn.clone();
    ptIn.at<double>(0) = pti.x;
    ptIn.at<double>(1) = pti.y;
    ptIn.at<double>(2) = pti.z;
    ptOut = cameraM * ptIn;
//    cout<<pto.x<<" "<<ptOut.at<double>(0)/ptOut.at<double>(2)<<" "<<pto.y<<" "<<ptOut.at<double>(1)/ptOut.at<double>(2)<<" "<<pto.z<<" "<<ptOut.at<double>(2)<<" "<<endl;
    pto.z = ptOut.at<double>(2);
    pto.x = ptOut.at<double>(0)/pto.z;
    pto.y = ptOut.at<double>(1)/pto.z;

#endif
//    cout<<"From camera space to image space:"<<pti.x<<" "<<pti.y<<" "<<pti.z<<"\t"<<pto.x<<" "<<pto.y<<" "<<pto.z<<endl;
}
void hand_eye_calib::projectPts( PointCloud physical_points_, Mat &image, vector<Point2f> &depthBuf, vector<Point2f> pointBuf){
    PointCloud trPcl, trPclEarly;//, imagePcl;
//    cout<<"Calibration stored\n"<<phy2image<<endl;
    transformPointCloud(physical_points_, trPcl, phy2image);
    transformPointCloud(physical_points_, trPclEarly, phy2imageEarly);
    vector<Point2f> earlyPts, nowPts;
    for(size_t i=0;i<trPcl.points.size();i++){
        pcl::PointXYZ pti, ptpi;
        transform_cart_to_polar(trPcl.points[i], pti);
        transform_cart_to_polar(trPclEarly.points[i], ptpi);
        Point2f pt, ptp;
        pt.x = pti.x;//*(float)cameraM.at<double>(0,0);///trPcl.points[i].z;
        pt.y = pti.y;//*(float)cameraM.at<double>(1,1);///trPcl.points[i].z;
        ptp.x = ptpi.x;//*(float)cameraM.at<double>(0,0);
        ptp.y = ptpi.y;//*(float)cameraM.at<double>(1,1);
        nowPts.push_back(pt);
        earlyPts.push_back(ptp);
//        circle(image, pt, 10, Scalar(0,0,255), 4, 8);
    }
    double dist = 0.0, distP = 0.0;
    for(size_t k=0;k<pointBuf.size();k++){
        dist += sqrt(pow(pointBuf[k].x - nowPts[k].x,2) + pow(pointBuf[k].y - nowPts[k].y,2));
        distP += sqrt(pow(pointBuf[k].x - earlyPts[k].x,2) + pow(pointBuf[k].y - earlyPts[k].y,2));
    }
    dist /= (float)pointBuf.size();
    distP /= (float)pointBuf.size();
    Size boardSize(7,5);
    ROS_INFO_STREAM("Image calibration error : "<<dist<<" "<<distP<<" "<<actCalErr);
    if(distP>dist){
        phy2imageEarly = phy2image;
        for(size_t k=0;k<pointBuf.size();k++){
            depthBuf.push_back(nowPts[k]);
//            circle(image, nowPts[k], 10, Scalar(0,0,255), 4, 8);
        }
    }
    else{
        for(size_t k=0;k<pointBuf.size();k++){
            depthBuf.push_back(earlyPts[k]);
//            circle(image, earlyPts[k], 10, Scalar(0,0,255), 4, 8);
        }
    }
    for(size_t i=0;i<depthBuf.size();i++){
        circle(image, depthBuf[i], 2+i/2, Scalar(i*5,0,255-i*5), 2, 8);
        circle(image, pointBuf[i], 2+i/2, Scalar(255-i*5,0,i*5), 2, 8);
    }

    //drawChessboardCorners( image, boardSize, Mat(depthBuf), true );
    //drawChessboardCorners( image, boardSize, Mat(pointBuf), true );
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "hand_eye_calib_node");
	hand_eye_calib VC;
	ros::spin();

	return 0;
}


//------------------------------------------------------------------------------
/*
    This file is part of calld: https://github.com/call/calld
    Copyright (c) 2012-2016 Call Labs Inc.

    Permission to use, copy, modify, and/or distribute this software for any
    purpose  with  or without fee is hereby granted, provided that the above
    copyright notice and this permission notice appear in all copies.

    THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
    WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
    MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
    ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
    WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
    ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================

#include <BeastConfig.h>
#include <call/basics/base_uint.h>
#include <call/basics/hardened_hash.h>
#include <call/beast/unit_test.h>
#include <boost/algorithm/string.hpp>

namespace call {
namespace test {

// a non-hashing Hasher that just copies the bytes.
// Used to test hash_append in base_uint
template <std::size_t Bits>
struct nonhash
{
    static constexpr std::size_t WIDTH = Bits / 8;
    std::array<std::uint8_t, WIDTH> data_;
    static beast::endian const endian = beast::endian::big;

    nonhash() = default;

    void
    operator() (void const* key, std::size_t len) noexcept
    {
        assert(len == WIDTH);
        memcpy(data_.data(), key, len);
    }

    explicit
    operator std::size_t() noexcept { return WIDTH; }
};

struct base_uint_test : beast::unit_test::suite
{
    using test96 = base_uint<96>;

    void run()
    {
        // used to verify set insertion (hashing required)
        std::unordered_set<test96, hardened_hash<>> uset;

        Blob raw { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12 };
        BEAST_EXPECT(test96::bytes == raw.size());

        test96 u { raw };
        uset.insert(u);
        BEAST_EXPECT(raw.size() == u.size());
        BEAST_EXPECT(to_string(u) == "0102030405060708090A0B0C");
        BEAST_EXPECT(*u.data() == 1);
        BEAST_EXPECT(u.signum() == 1);
        BEAST_EXPECT(!!u);
        BEAST_EXPECT(!u.isZero());
        BEAST_EXPECT(u.isNonZero());
        unsigned char t = 0;
        for (auto& d : u)
        {
            BEAST_EXPECT(d == ++t);
        }

        // Test hash_append by "hashing" with a no-op hasher (h)
        // and then extracting the bytes that were written during hashing
        // back into another base_uint (w) for comparison with the original
        nonhash<96> h;
        hash_append(h, u);
        test96 w {std::vector<std::uint8_t>(h.data_.begin(), h.data_.end())};
        BEAST_EXPECT(w == u);

        test96 v { ~u };
        uset.insert(v);
        BEAST_EXPECT(to_string(v) == "FEFDFCFBFAF9F8F7F6F5F4F3");
        BEAST_EXPECT(*v.data() == 0xfe);
        BEAST_EXPECT(v.signum() == 1);
        BEAST_EXPECT(!!v);
        BEAST_EXPECT(!v.isZero());
        BEAST_EXPECT(v.isNonZero());
        t = 0xff;
        for (auto& d : v)
        {
            BEAST_EXPECT(d == --t);
        }

        BEAST_EXPECT(compare(u, v) < 0);
        BEAST_EXPECT(compare(v, u) > 0);

        v = u;
        BEAST_EXPECT(v == u);

        test96 z { beast::zero };
        uset.insert(z);
        BEAST_EXPECT(to_string(z) == "000000000000000000000000");
        BEAST_EXPECT(*z.data() == 0);
        BEAST_EXPECT(*z.begin() == 0);
        BEAST_EXPECT(*std::prev(z.end(), 1) == 0);
        BEAST_EXPECT(z.signum() == 0);
        BEAST_EXPECT(!z);
        BEAST_EXPECT(z.isZero());
        BEAST_EXPECT(!z.isNonZero());
        for (auto& d : z)
        {
            BEAST_EXPECT(d == 0);
        }

        test96 n { z };
        n++;
        BEAST_EXPECT(n == test96(1));
        n--;
        BEAST_EXPECT(n == beast::zero);
        BEAST_EXPECT(n == z);
        n--;
        BEAST_EXPECT(to_string(n) == "FFFFFFFFFFFFFFFFFFFFFFFF");
        n = beast::zero;
        BEAST_EXPECT(n == z);

        test96 zp1 { z };
        zp1++;
        test96 zm1 { z };
        zm1--;
        test96 x { zm1 ^ zp1 };
        uset.insert(x);
        BEAST_EXPECTS(to_string(x) == "FFFFFFFFFFFFFFFFFFFFFFFE", to_string(x));

        BEAST_EXPECT(uset.size() == 4);

        // SetHex tests...
        test96 fromHex;
        BEAST_EXPECT(fromHex.SetHexExact(to_string(u)));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        // fails with extra char
        BEAST_EXPECT(! fromHex.SetHexExact("A" + to_string(u)));
        fromHex = z;

        // fails with extra char at end
        BEAST_EXPECT(! fromHex.SetHexExact(to_string(u) + "A"));
        // NOTE: the value fromHex is actually correctly parsed
        // in this case, but that is an implementation detail and
        // not guaranteed, thus we don't check the value here.
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex(to_string(u)));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        // leading space/0x allowed if not strict
        BEAST_EXPECT(fromHex.SetHex("  0x" + to_string(u)));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        // other leading chars also allowed (ignored)
        BEAST_EXPECT(fromHex.SetHex("FEFEFE" + to_string(u)));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        // invalid hex chars should fail (0 replaced with Z here)
        BEAST_EXPECT(! fromHex.SetHex(
            boost::algorithm::replace_all_copy(to_string(u), "0", "Z")));
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex(to_string(u), true));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        // strict mode fails with leading chars
        BEAST_EXPECT(! fromHex.SetHex("  0x" + to_string(u), true));
        fromHex = z;

        // SetHex ignores extra leading hexits, so the parsed value
        // is still correct for the following case (strict or non-strict)
        BEAST_EXPECT(fromHex.SetHex("DEAD" + to_string(u), true ));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex("DEAD" + to_string(u), false ));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;
    }
};

BEAST_DEFINE_TESTSUITE(base_uint, call_basics, call);

}  // namespace test
}  // namespace call


#define BOOST_TEST_MODULE graph_communicator

#include <boost/test/included/unit_test.hpp>
#include <mpl/mpl.hpp>

bool graph_communicator_test() {
  const mpl::communicator &comm_world = mpl::environment::comm_world();
  int size = comm_world.size();
  mpl::graph_communicator::edge_set es;
  for (int i = 1; i < size; ++i) {
    es.insert({0, i});
    es.insert({i, 0});
  }
  mpl::graph_communicator comm_g(comm_world, es);
  if (comm_g.neighbors_count(0) != comm_g.size() - 1)
    return false;
  auto nl0{comm_g.neighbors(0)};
  if (nl0.size() != comm_g.size() - 1)
    return false;
  auto nl1{comm_g.neighbors(1)};
  if (nl1.size() != 1)
    return false;
  return true;
}

bool graph_communicator_test2() {
  const mpl::communicator &comm_world = mpl::environment::comm_world();
  int size = comm_world.size();
  if (size == 4) {
    mpl::graph_communicator::edge_set es{{0, 1}, {0, 3}, {1, 0}, {2, 3}, {3, 0}, {3, 2}};
    mpl::graph_communicator comm_g(comm_world, es);
    if (comm_g.neighbors_count(0) != 2)
      return false;
    if (comm_g.neighbors_count(1) != 1)
      return false;
    if (comm_g.neighbors_count(2) != 1)
      return false;
    if (comm_g.neighbors_count(3) != 2)
      return false;
    if (comm_g.neighbors(0) != mpl::graph_communicator::node_list{1, 3})
      return false;
    if (comm_g.neighbors(1) != mpl::graph_communicator::node_list{0})
      return false;
    if (comm_g.neighbors(2) != mpl::graph_communicator::node_list{3})
      return false;
    if (comm_g.neighbors(3) != mpl::graph_communicator::node_list{0, 2})
      return false;
  }
  return true;
}

BOOST_AUTO_TEST_CASE(graph_communicator) {
  BOOST_TEST(graph_communicator_test());
  BOOST_TEST(graph_communicator_test2());
}


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Mesh_Boundary.h"

#include <assert.h>

#include <vector>

#pragma warning(push, 0)
#include <igl/boundary_loop.h>
#pragma warning(pop)

#include "ConversionUtilities.h"
#include "TriMesh.h"
#include "Polyline.h"

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Urho3D;

String Mesh_Boundary::iconTexture = "Textures/Icons/Mesh_Boundary.png";

Mesh_Boundary::Mesh_Boundary(Context* context) : IoComponentBase(context, 1, 1)
{
	SetName("MeshBoundary");
	SetFullName("Mesh Boundary");
	SetDescription("Compute boundary of a mesh");
	SetGroup(IoComponentGroup::MESH);
	SetSubgroup("Operators");

	inputSlots_[0]->SetName("Mesh");
	inputSlots_[0]->SetVariableName("M");
	inputSlots_[0]->SetDescription("Mesh with boundary");
	inputSlots_[0]->SetVariantType(VariantType::VAR_VARIANTMAP);
	inputSlots_[0]->SetDataAccess(DataAccess::ITEM);

	outputSlots_[0]->SetName("Boundary");
	outputSlots_[0]->SetVariableName("B");
	outputSlots_[0]->SetDescription("Boundary of mesh as polyline");
	outputSlots_[0]->SetVariantType(VariantType::VAR_VARIANTMAP);
	outputSlots_[0]->SetDataAccess(DataAccess::LIST);
}

void Mesh_Boundary::SolveInstance(
	const Vector<Variant>& inSolveInstance,
	Vector<Variant>& outSolveInstance
)
{
	assert(inSolveInstance.Size() == inputSlots_.Size());
	assert(outSolveInstance.Size() == outputSlots_.Size());

	///////////////////
	// VERIFY & EXTRACT

	// Verify input slot 0
	Variant inMesh = inSolveInstance[0];
	if (!TriMesh_Verify(inMesh)) {
		URHO3D_LOGWARNING("M must be a valid mesh.");
		outSolveInstance[0] = Variant();
		return;
	}

	///////////////////
	// COMPONENT'S WORK

	Eigen::MatrixXf V;
	Eigen::MatrixXi F;
	bool loadSuccess = IglMeshToMatrices(inMesh, V, F);

	VariantVector loopsOut;

	if (loadSuccess) {
		VariantVector vertexList = TriMesh_GetVertexList(inMesh);

		VariantVector loopsOut;
		std::vector<std::vector<int>> loops;
		igl::boundary_loop(F, loops);

		for (int j = 0; j < loops.size(); j++)
		{
			std::vector<int> loop = loops[j];
			VariantVector boundaryVertexList;
			Variant boundary_polyline = Variant();
			for (int i = 0; i < loop.size(); ++i) {
				Vector3 vec = vertexList[loop[i]].GetVector3();
				boundaryVertexList.Push(Variant(vec));
			}

			Vector3 ini_vec = vertexList[loop[0]].GetVector3();
			boundaryVertexList.Push(Variant(ini_vec));
			boundary_polyline = Polyline_Make(boundaryVertexList);

			loopsOut.Push(boundary_polyline);

		}

		outSolveInstance[0] = loopsOut;

	}
	else {
		URHO3D_LOGWARNING("Failed to convert mesh to libigl matrices.");
		SetAllOutputsNull(outSolveInstance);
		return;
	}

}


/*
 * Copyright (c) 2018-2020 Steven Varga, Toronto,ON Canada
 * Author: Varga, Steven <steven@vargaconsulting.ca>
 */

#include <armadillo>
#include <chrono>
#include <vector>
#include <functional>
#include <h5cpp/all>

// factored out timer, note that this is not quite right as you're
// measuring 'lamda' mechanism of the compiler, OTOH is an example
// how flexible H5CPP underlying descriptors are
void timer( std::function<void()> callback ){

	using tictoc = std::chrono::high_resolution_clock;
	using us = std::chrono::microseconds;
	auto duration = std::chrono::duration_values<us>::zero();
	auto start = tictoc::now();
		callback();
	auto stop = tictoc::now();
	duration += std::chrono::duration_cast<us>(stop - start);
	std::cout << duration.count() << " [us]" << std::endl;
}

int main() {

	h5::fd_t fd = h5::create("pt.h5", H5F_ACC_TRUNC);
	h5::pt_t pt = h5::create<double>(fd, "vectors",
									h5::max_dims{H5S_UNLIMITED, 128*1024},
									h5::chunk{1, 128*1024});
	{   // VARIANT 1: vector<objects<elements>> save element wise, no penalty as you write directly to buffer

		// I see what you're doing here, while semantically correct
		// please observe that std::vector<arma::colvec> is NOT RUGGED hence is a
		// matrix with colvector bases, a common construct to 'pack' a set
		// of BLAS level 1 into a single level 2 or 3 operation
		// in any event lets have this 'your way' then below
		std::vector<arma::colvec> v(8);  // a batch of 8 chunks
		for (size_t i = 0; i < v.size(); ++i)
			v[i] = arma::randu<arma::colvec>(128*1024) + i; // let's add 'i' to color rows

		// c++ is not a bore :) let's capture 'h5::pt_t pt' by reference
		timer( [&]() -> void {
			for( size_t i = 0; i < v.size(); i++)
				for (size_t j = 0; j < v[i].n_elem; ++j) h5::append(pt, v[i][j]);
		});
	}
	{   // VARIANT 2: set of non-rugged vector is a matrix with column vec bases
		arma::mat M(8,128*1024);  // a batch of 8 chunks
		for (size_t i = 0; i < M.n_rows; ++i)
			M(i,arma::span::all) = arma::randu<arma::rowvec>(128*1024)+i;

		timer( [&]() -> void {
			arma::rowvec v(128*1024); // reserve buffer
			for( size_t i = 0; i < M.n_rows; i++){
				h5::append(pt,
				// arma::subview_row<T> should be cast to arma::rowvec: zero copy
						static_cast<arma::rowvec>( M(i,arma::span::all) ));
			}
		});
	}
	{   // VARIANT 3: append/write 'chunk size' data block directly from raw memory buffer
		arma::mat M(8,128*1024);  // a batch of 8 chunks
		for (size_t i = 0; i < M.n_rows; ++i)
			M(i,arma::span::all) = arma::randu<arma::rowvec>(128*1024) + i;
		// this is an expensive operation, will not work with huge matrices
		arma::mat T = M.t();
		// data directly written to file from passed pointer, best performance
		timer( [&]() -> void {
			for (size_t i = 0; i < T.n_cols; ++i)
				h5::append(pt, &T(0,i) );
		});
	}
	/*  DOESN't WORK FOR NOW: in the next update will add functionality to
	 *  chunk/block input data directly from object's buffer. The code is there
	 *  just not 'enabled'
	 *
	{   // VARIANT 3: set of non-rugged vector is a matrix with column vec bases
		// the buffer is 'v' variable, and h5::append knows that no copy is needed
		// instead it uses the object memory address directly instead of internal buffer

		// CAVEAT: the row/col order is not correctly handled as of 2019 spring, work in progress
		// to make 'transpose' efficient and seamless. Please check this with Julia/R/Matlab
		// if reads it in the right order
		arma::mat v(8,128*1024);  // a batch of 8 chunks
		for (size_t i = 0; i < v.n_rows; ++i)
			v(i,arma::span::all) = arma::randu<arma::rowvec>(128*1024) + 1;

		// since we buffered data acquisition h5::append is reduced to a single op
		timer( [&]() -> void {
			//h5::append(pt, v);
		});
	}
	*/
	return 0;
}


#ifndef YASMIC_UNDIR_SIMPLE_CSR_MATRIX_HPP
#define YASMIC_UNDIR_SIMPLE_CSR_MATRIX_HPP

/** @file undir_simple_csr_matrix.hpp
 * @author David F. Gleich
 * @copyright Stanford University, 2008
 * An exact copy of simple_csr_matrix.hpp with a different name for an
 * undirected graph type.
 */

/** History
 *  2008-09-26: Initial coding
 */

#include <yasmic/smatrix_traits.hpp>
#include <boost/iterator/counting_iterator.hpp>

namespace yasmic
{

/**
 * csr_matrix is a more user manageable compressed sparse row matrix type.
 * It is based on the same ideas as the compressed_row_matrix, but designed
 * to be less general and more specific.
 *
 * The idea is that an application will use the csr_matrix structure to
 * manage a sparse matrix and design algorithms that are NOT more
 * generally applicable.
 */
template <class IndexType, class ValueType, class NzSizeType=IndexType>
struct undir_simple_csr_matrix
{
    IndexType nrows;
    IndexType ncols;
    NzSizeType nnz;

    NzSizeType* ai;
    IndexType* aj;
    ValueType* a;

    // an extra indextype to serve as the default value of ai for
    // an empty matrix
    IndexType empty;

    undir_simple_csr_matrix()
        : nrows(0), ncols(0), nnz(0), empty(0), ai(&empty), aj(NULL), a(NULL) {}

    undir_simple_csr_matrix(IndexType nrows, IndexType ncols, NzSizeType nnz,
         NzSizeType *ai, IndexType *aj, ValueType *a)
         : nrows(nrows), ncols(ncols), nnz(nnz), ai(ai), aj(aj), a(a) {}
};

template <class IndexType, class ValueType, class NzSizeType>
struct smatrix_traits< undir_simple_csr_matrix<IndexType, ValueType, NzSizeType> >
{
    typedef IndexType index_type;
    typedef ValueType value_type;

    typedef NzSizeType nz_size_type;
    typedef NzSizeType nz_index_type;

    //typedef typename impl::csr_nonzero<IndexType, ValueType, NzSizeType> nonzero_iterator;
    //typedef impl::csr_nonzero<IndexType, ValueType, NzSizeType> nonzero_descriptor;

    //typedef typename compressed_row_matrix_type::row_nonzero_iterator row_nonzero_iterator;
    //typedef impl::csr_nonzero<IndexType, ValueType, NzSizeType> row_nonzero_descriptor;

    typedef boost::counting_iterator<IndexType> row_iterator;

    struct properties
        : public row_access_tag, public nonzero_index_tag
    {};
};

template <class IndexType, class ValueType, class NzSizeType>
IndexType nrows(const undir_simple_csr_matrix<IndexType, ValueType, NzSizeType>& m)
{ return m.nrows; }

template <class IndexType, class ValueType, class NzSizeType>
IndexType ncols(const undir_simple_csr_matrix<IndexType, ValueType, NzSizeType>& m)
{ return m.ncols; }

template <class IndexType, class ValueType, class NzSizeType>
NzSizeType nnz(const undir_simple_csr_matrix<IndexType, ValueType, NzSizeType>& m)
{ return m.nnz; }

} // namespace yasmic

#endif /* YASMIC_UNDIR_SIMPLE_CSR_MATRIX_HPP */


/*
 * Copyright (c) 2016, Niklas Gürtler
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 * following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *    disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *    following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef FORMULA_AST_HH_
#define FORMULA_AST_HH_

#include <string>
#include <iostream>
#include <cstddef>
#include <utility>
#include <ios>

#include <boost/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
#include <boost/fusion/include/define_struct.hpp>
#include <boost/functional/hash.hpp>
#include <boost/preprocessor/stringize.hpp>

namespace Formula {
	struct E_Literal;
	struct E_Negation;
	struct E_And;
	struct E_Or;
	struct E_Implication;
	struct E_ExistNext;
	struct E_ExistUntil;
	struct E_ExistAlways;
	struct E_AllNext;
	struct E_AllUntil;
	struct E_AllAlways;
	struct E_Next;
	struct E_Until;
}

BOOST_FUSION_DEFINE_STRUCT(
	(Formula), E_Literal,
	(std::string::const_iterator, strBegin)
	(bool, value)
	(std::string::const_iterator, strEnd)
)

BOOST_FUSION_DEFINE_STRUCT(
	(Formula), E_Label,
	(std::string::const_iterator, strBegin)
	(std::string, name)
	(std::string::const_iterator, strEnd)
)
namespace Formula {
	using Expression = boost::variant<	E_Literal, E_Label,
										boost::recursive_wrapper<E_Negation>,
										boost::recursive_wrapper<E_And>,
										boost::recursive_wrapper<E_Or>,
										boost::recursive_wrapper<E_Implication>,
										boost::recursive_wrapper<E_ExistNext>,
										boost::recursive_wrapper<E_ExistUntil>,
										boost::recursive_wrapper<E_ExistAlways>,
										boost::recursive_wrapper<E_AllNext>,
										boost::recursive_wrapper<E_AllUntil>,
										boost::recursive_wrapper<E_AllAlways>,
										boost::recursive_wrapper<E_Next>,
										boost::recursive_wrapper<E_Until>
									>;
	template <typename T>
	struct ExpTraits;

	extern int Formula_Xalloc;
}

#define MCHECK_DEFINE_UNARY(NAME,C,L)	BOOST_FUSION_DEFINE_STRUCT((Formula), NAME, (std::string::const_iterator, strBegin)(Formula::Expression, exp)(std::string::const_iterator, strEnd)) \
										namespace Formula { \
											std::size_t hash_value (const NAME& a); \
											bool operator == (const NAME& a, const NAME& b); \
											std::ostream& operator<< (std::ostream& os, const NAME& l); \
											template <> struct ExpTraits<NAME> { static constexpr bool unary = true; static constexpr bool ltl = L; static constexpr bool ctl = C; static const char* name; }; \
										}

#define MCHECK_IMPL_UNARY(NAME,PM,PL)	std::size_t Formula::hash_value (const Formula::NAME& a) { return boost::hash<Formula::Expression> {} (a.exp); } \
										bool Formula::operator == (const Formula::NAME& a, const Formula::NAME& b) { return a.exp == b.exp; } \
										const char* Formula::ExpTraits<Formula::NAME>::name = BOOST_PP_STRINGIZE(NAME); \
										std::ostream& Formula::operator<< (std::ostream& os, const Formula::NAME& a) { long f = os.iword (Formula_Xalloc); if (f == 0) os << ExpTraits<NAME>::name << " {" << a.exp << '}'; else if (f == 1) os << PM ; else os << PL ; return os; }

#define MCHECK_DEFINE_BINARY(NAME,C,L)	BOOST_FUSION_DEFINE_STRUCT((Formula), NAME, (std::string::const_iterator, strBegin)(Formula::Expression, lhs)(Formula::Expression, rhs)(std::string::const_iterator, strEnd)) \
										namespace Formula { \
											std::size_t hash_value (const NAME& a); \
											bool operator == (const NAME& a, const NAME& b); \
											std::ostream& operator<< (std::ostream& os, const NAME& l); \
											template <> struct ExpTraits<NAME> { static constexpr bool binary = true; static constexpr bool ltl = L; static constexpr bool ctl = C; static const char* name; }; \
										}

#define MCHECK_IMPL_BINARY(NAME,PM,PL)	std::size_t Formula::hash_value (const Formula::NAME& a) { std::size_t s = 0; boost::hash_combine (s, a.lhs); boost::hash_combine (s, a.rhs); return s; } \
										bool Formula::operator == (const Formula::NAME& a, const Formula::NAME& b) { return a.lhs == b.lhs && a.rhs == b.rhs; } \
										const char* Formula::ExpTraits<Formula::NAME>::name = BOOST_PP_STRINGIZE(NAME); \
										std::ostream& Formula::operator<< (std::ostream& os, const Formula::NAME& a) { long f = os.iword (Formula_Xalloc); if (f == 0) os << ExpTraits<NAME>::name << " {" << a.lhs << ", " << a.rhs << '}'; else if (f == 1) os << PM ; else os << PL ; return os; }

MCHECK_DEFINE_UNARY(E_Negation, true, true)
MCHECK_DEFINE_UNARY(E_ExistNext, true, false)
MCHECK_DEFINE_UNARY(E_ExistAlways, true, false)
MCHECK_DEFINE_UNARY(E_AllNext, true, false)
MCHECK_DEFINE_UNARY(E_AllAlways, true, false)
MCHECK_DEFINE_UNARY(E_Next, false, true)

MCHECK_DEFINE_BINARY(E_And, true, true)
MCHECK_DEFINE_BINARY(E_Or, true, true)
MCHECK_DEFINE_BINARY(E_Implication, true, true)
MCHECK_DEFINE_BINARY(E_ExistUntil, true, false)
MCHECK_DEFINE_BINARY(E_AllUntil, true, false)
MCHECK_DEFINE_BINARY(E_Until, false, true)

namespace Formula {
	bool operator == (const E_Label& a, const E_Label& b);
	bool operator == (const E_Literal& a, const E_Literal& b);
	std::ostream& operator<< (std::ostream& os, const E_Literal& l);
	std::ostream& operator<< (std::ostream& os, const E_Label& l);
    std::size_t hash_value (const E_Label& a);
    std::size_t hash_value (const E_Literal& a);

	std::ios_base& mathprint (std::ios_base& os);
	std::ios_base& cxxprint (std::ios_base& os);
	std::ios_base& latexprint (std::ios_base& os);

    class ExpRef {
    	public:
    		using Target = const Expression;

    		inline ExpRef () : exp {} {}
    		inline ExpRef (Target* e) : exp (e) {}
    		inline ExpRef (const ExpRef&) = default;
    		inline ExpRef (ExpRef&&) = default;
    		inline ExpRef& operator = (const ExpRef&) = default;
    		inline ExpRef& operator = (ExpRef&&) = default;

			inline const Target* operator -> () const { return exp; }
			inline const Target& operator * () const { return *exp; }

    	private:
			const Target* exp;
    };
    bool operator == (const ExpRef& a, const ExpRef& b);
    std::size_t hash_value (const ExpRef& e);
}

#endif /* FORMULA_AST_HH_ */


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Ordering.cpp
 * @author  Richard Roberts
 * @author  Andrew Melim
 * @date    Sep 2, 2010
 */

#include <vector>
#include <limits>

#include <boost/format.hpp>

#include <gtsam/inference/Ordering.h>
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>

#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
#ifdef GTSAM_USE_SYSTEM_METIS
#include <metis.h>
#else
#include <gtsam/3rdparty/metis/include/metis.h>
#endif
#endif

using namespace std;

namespace gtsam {

/* ************************************************************************* */
FastMap<Key, size_t> Ordering::invert() const {
  FastMap<Key, size_t> inverted;
  for (size_t pos = 0; pos < this->size(); ++pos)
    inverted.insert(make_pair((*this)[pos], pos));
  return inverted;
}

/* ************************************************************************* */
Ordering Ordering::Colamd(const VariableIndex& variableIndex) {
  // Call constrained version with all groups set to zero
  vector<int> dummy_groups(variableIndex.size(), 0);
  return Ordering::ColamdConstrained(variableIndex, dummy_groups);
}

/* ************************************************************************* */
Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
    std::vector<int>& cmember) {
  gttic(Ordering_COLAMDConstrained);

  gttic(Prepare);
  const size_t nVars = variableIndex.size();
  if (nVars == 0)
  {
    return Ordering();
  }

  if (nVars == 1)
  {
    return Ordering(KeyVector(1, variableIndex.begin()->first));
  }

  const size_t nEntries = variableIndex.nEntries(), nFactors =
      variableIndex.nFactors();
  // Convert to compressed column major format colamd wants it in (== MATLAB format!)
  const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors,
      (int) nVars); /* colamd arg 3: size of the array A */
  vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
  vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */

  // Fill in input data for COLAMD
  p[0] = 0;
  int count = 0;
  KeyVector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
  size_t index = 0;
  for (auto key_factors: variableIndex) {
    // Arrange factor indices into COLAMD format
    const FactorIndices& column = key_factors.second;
    for(size_t factorIndex: column) {
      A[count++] = (int) factorIndex; // copy sparse column
    }
    p[index + 1] = count;  // column j (base 1) goes from A[j-1] to A[j]-1
    // Store key in array and increment index
    keys[index] = key_factors.first;
    ++index;
  }

  assert((size_t)count == variableIndex.nEntries());

  //double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */
  double knobs[CCOLAMD_KNOBS];
  ccolamd_set_defaults(knobs);
  knobs[CCOLAMD_DENSE_ROW] = -1;
  knobs[CCOLAMD_DENSE_COL] = -1;

  int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */

  gttoc(Prepare);

  // call colamd, result will be in p
  /* returns (1) if successful, (0) otherwise*/
  if (nVars > 0) {
    gttic(ccolamd);
    int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0],
        knobs, stats, &cmember[0]);
    if (rv != 1)
      throw runtime_error(
          (boost::format("ccolamd failed with return value %1%") % rv).str());
  }

  //  ccolamd_report(stats);

  // Convert elimination ordering in p to an ordering
  gttic(Fill_Ordering);
  Ordering result;
  result.resize(nVars);
  for (size_t j = 0; j < nVars; ++j)
    result[j] = keys[p[j]];
  gttoc(Fill_Ordering);

  return result;
}

/* ************************************************************************* */
Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
    const KeyVector& constrainLast, bool forceOrder) {
  gttic(Ordering_COLAMDConstrainedLast);

  size_t n = variableIndex.size();
  std::vector<int> cmember(n, 0);

  // Build a mapping to look up sorted Key indices by Key
  // TODO(frank): think of a way to not build this
  FastMap<Key, size_t> keyIndices;
  size_t j = 0;
  for (auto key_factors: variableIndex)
    keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));

  // If at least some variables are not constrained to be last, constrain the
  // ones that should be constrained.
  int group = (constrainLast.size() != n ? 1 : 0);
  for (Key key: constrainLast) {
    cmember[keyIndices.at(key)] = group;
    if (forceOrder)
      ++group;
  }

  return Ordering::ColamdConstrained(variableIndex, cmember);
}

/* ************************************************************************* */
Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
    const KeyVector& constrainFirst, bool forceOrder) {
  gttic(Ordering_COLAMDConstrainedFirst);

  const int none = -1;
  size_t n = variableIndex.size();
  std::vector<int> cmember(n, none);

  // Build a mapping to look up sorted Key indices by Key
  FastMap<Key, size_t> keyIndices;
  size_t j = 0;
  for (auto key_factors: variableIndex)
    keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));

  // If at least some variables are not constrained to be last, constrain the
  // ones that should be constrained.
  int group = 0;
  for (Key key: constrainFirst) {
    cmember[keyIndices.at(key)] = group;
    if (forceOrder)
      ++group;
  }

  if (!forceOrder && !constrainFirst.empty())
    ++group;
  for(int& c: cmember)
    if (c == none)
      c = group;

  return Ordering::ColamdConstrained(variableIndex, cmember);
}

/* ************************************************************************* */
Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
    const FastMap<Key, int>& groups) {
  gttic(Ordering_COLAMDConstrained);
  size_t n = variableIndex.size();
  std::vector<int> cmember(n, 0);

  // Build a mapping to look up sorted Key indices by Key
  FastMap<Key, size_t> keyIndices;
  size_t j = 0;
  for (auto key_factors: variableIndex)
    keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));

  // Assign groups
  typedef FastMap<Key, int>::value_type key_group;
  for(const key_group& p: groups) {
    // FIXME: check that no groups are skipped
    cmember[keyIndices.at(p.first)] = p.second;
  }

  return Ordering::ColamdConstrained(variableIndex, cmember);
}

/* ************************************************************************* */
Ordering Ordering::Metis(const MetisIndex& met) {
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
  gttic(Ordering_METIS);

  idx_t size = met.nValues();
  if (size == 0)
  {
    return Ordering();
  }

  if (size == 1)
  {
    return Ordering(KeyVector(1, met.intToKey(0)));
  }

  vector<idx_t> xadj = met.xadj();
  vector<idx_t> adj = met.adj();
  vector<idx_t> perm, iperm;

  for (idx_t i = 0; i < size; i++) {
    perm.push_back(0);
    iperm.push_back(0);
  }

  int outputError;

  outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0],
      &iperm[0]);
  Ordering result;

  if (outputError != METIS_OK) {
    std::cout << "METIS failed during Nested Dissection ordering!\n";
    return result;
  }

  result.resize(size);
  for (size_t j = 0; j < (size_t) size; ++j) {
    // We have to add the minKey value back to obtain the original key in the Values
    result[j] = met.intToKey(perm[j]);
  }
  return result;
#else
  throw runtime_error("GTSAM was built without support for Metis-based "
                      "nested dissection");
#endif
}

/* ************************************************************************* */
void Ordering::print(const std::string& str,
    const KeyFormatter& keyFormatter) const {
  cout << str;
  // Print ordering in index order
  // Print the ordering with varsPerLine ordering entries printed on each line,
  // for compactness.
  static const size_t varsPerLine = 10;
  bool endedOnNewline = false;
  for (size_t i = 0; i < size(); ++i) {
    if (i % varsPerLine == 0)
      cout << "Position " << i << ": ";
    if (i % varsPerLine != 0)
      cout << ", ";
    cout << keyFormatter(at(i));
    if (i % varsPerLine == varsPerLine - 1) {
      cout << "\n";
      endedOnNewline = true;
    } else {
      endedOnNewline = false;
    }
  }
  if (!endedOnNewline)
    cout << "\n";
  cout.flush();
}

/* ************************************************************************* */
bool Ordering::equals(const Ordering& other, double tol) const {
  return (*this) == other;
}

}