//
// Created by nvidia on 6/13/18.
//

#include "SpVec.hpp"

//
// Created by nvidia on 6/11/18.
//

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>
#include <cmath>
#include "superpixel_seg.hpp"
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_matrix.hpp>

namespace cove {

using namespace cv;
using namespace std;

typedef Vec<double, SPVEC_SIZE> SpVec;

template <typename T>
void printVec(const vector<T>& v) {
    for (T a: v) {
        cout << setw(COUT_WIDTH) << a;
    }
    cout << endl;
}

SpVec multiply(const SpVec& a , const SpVec& b) {
    SpVec temp;
    for (int i = 0; i < SPVEC_SIZE; i++) {
        temp[i] = a[i] * b[i];
    }
    return temp;
}

void printVecSpVec(const vector<SpVec> &v) {
    cout << setw(COUT_WIDTH) << "idx" << setw(COUT_WIDTH) << "x" << setw(COUT_WIDTH) << "y" << setw(COUT_WIDTH) << "cL" << setw(COUT_WIDTH) << "ca" << setw(COUT_WIDTH)
         << "cb" << setw(COUT_WIDTH) << "crclr\n";
    for (int i = 0; i < v.size(); i++) {
        cout.precision(4);
        cout << setw(COUT_WIDTH) << i;
        for (int j = 0; j < SPVEC_SIZE; j++) {
            cout << setw(COUT_WIDTH) << v.at(i)[j];
        }
        cout << "\n";
    }
    cout << flush;
}

void printSpVec(const SpVec& v) {
    vector<SpVec> a;
    a.push_back(v);
    printVecSpVec(a);
}

vector<int> getTrainingLabels(InputArray _labels, int regionSize, int rows, int cols) {
    auto labels = _labels.getMat();
    Size s = labels.size();
    int radius = regionSize / 2;
    vector<int> label_vals;
    for (int ydiff = 0; ydiff < rows; ydiff++) {
        label_vals.push_back(labels.at<int>(s.height - radius - ydiff * regionSize, s.width / 2));
        for (int xdiff = 1; xdiff <= cols / 2; xdiff += 1) {
            label_vals.push_back(labels.at<int>(s.height - radius - ydiff * regionSize, s.width / 2 + xdiff * regionSize));
            label_vals.push_back(labels.at<int>(s.height - radius - ydiff * regionSize, s.width / 2 - xdiff * regionSize));
        }
    }
    return label_vals;
}

SpVec getParams(InputArray labels, InputArray _frame, int labelVal) {
    SpVec ret(0.0);
    int idx = 0;
    // mask the thing
    Mat frame = _frame.getMat();
    Mat temp;
    cvtColor(frame, temp, COLOR_BGR2HLS);
    Mat mask;
    inRange(labels, Scalar(labelVal), Scalar(labelVal), mask);

    Scalar mean, stddev;
    meanStdDev(temp, mean, stddev, mask);
    // find color
    for (int i = 0; i < 3; i++) {
        ret[idx++] = mean[i];
    }
    for (int i = 0; i < 3; i++) {
        ret[idx++] = stddev[i];
    }
    // find squareness factor
    vector<vector<Point>> contours;
    findContours(mask, contours, noArray(), RETR_LIST, CHAIN_APPROX_SIMPLE);
    if (contours.size() < 1) {
        ret[idx++] = 0;
        return ret;
    }
    double area = contourArea(contours.at(0));
//    vector<Point> hull;
//    convexHull(contours.at(0), hull);
//    double hull_area = contourArea(hull);
//    double solidity = area / hull_area;
    double p = arcLength(contours.at(0), true);
    ret[idx] = 4 * M_PI * area / (p * p);
//    ret[idx] *= solidity;
    if (isnan(ret[idx]) || isinf(ret[idx])) {
        ret[idx] = 0;
    }
    idx++;
    return ret;
}

SpVec vecAverage(const vector<SpVec>& v) {
    SpVec ret(0.0);
    for(auto vec : v) {
        ret += vec;
    }
    ret /= static_cast<int>(v.size());
    return ret;
}

SpVec vecStdDev(const vector<SpVec>& v, const SpVec& mean) {
    SpVec variance(0.0);
    for (auto vec: v) {
        SpVec t = (vec - mean);
        variance += multiply(t, t);
    }
    variance /= static_cast<int>(v.size());
    sqrt(variance, variance);
    return variance;
}

SpVec vecStdDev(const vector<SpVec>& v) {
    SpVec mean = vecAverage(v);
//    cout << "TEMPORARY MEAN" << endl;
//    printSpVec(mean);
    return vecStdDev(v, mean);
}

//            H   L   S    H  L    S    CIRC
SpVec weights(1, 0.5, 0.5, 1, 0.5, 0.5, 1.5);

double normSsd(const SpVec& v, const SpVec& mean, const SpVec& stdDev) {
    SpVec temp;
    double sum = 0;
    for (int i = 0; i < SPVEC_SIZE; i++) {
        temp[i] = (v[i] - mean[i]) / stdDev[i];
        sum += temp[i] * temp[i] * weights[i];
    }
    return sum;
}

Matx<double, 2, 3> translateImg(Mat &img, int offsetx, int offsety){
    Matx<double, 2, 3> trans_mat(1.0, 0.0, static_cast<double>(offsetx), 0.0, 1.0, static_cast<double>(offsety));
    warpAffine(img, img, trans_mat, img.size());
    return trans_mat;
}

Graph generateGraph(InputArray _labels, unsigned int max) {
    // dilate the contonours
    Mat labels;
    Graph g(max);

    _labels.getMat().convertTo(labels, CV_8UC1);

    CV_Assert(labels.channels() == 1);

    Mat tmp;
    Mat d;

    vector<pair<int,int>> offsets;
    offsets.push_back(make_pair(1, 0));
    offsets.push_back(make_pair(0, 1));
    for (auto pair: offsets) {
        labels.copyTo(tmp);
        translateImg(tmp, pair.first, pair.second);
        SHOW("translated", tmp);
        d = labels - tmp;

        int nRows = tmp.rows;
        int nCols = tmp.cols;

        if (tmp.isContinuous()) {
            CV_Assert(d.isContinuous());
            CV_Assert(labels.isContinuous());
            nCols *= nRows;
            nRows = 1;
        }

        int i, j;
        uchar *l, *dd, *pp;
        for (i = 0; i < nRows; ++i) {
            pp = tmp.ptr<uchar>(i);
            dd = d.ptr<uchar>(i);
            l = labels.ptr<uchar>(i);
            for (j = 0; j < nCols; ++j) {
                if (dd[j] == 0) continue;
                boost::add_edge(l[j], pp[j], g);
            }
        }
    }

    return g;
}

}


// #include <boost/filesystem.hpp>

#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <visualization_msgs/Marker.h>
#include <ddd_plane_extr/planes_paramConfig.h>

// Include opencv2
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

// #include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/PointCloud.h>

#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/publisher.h>
#include <pcl/ModelCoefficients.h>
#include <pcl_msgs/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/eigen.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/range_image/range_image_planar.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/extract_clusters.h>

#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/pca.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/normal_3d.h>
#include <geometry_msgs/PointStamped.h>

#include <iostream>
#include <experimental/filesystem>
#include <iterator>
#include <vector>
#include <algorithm>
#include <math.h>

using namespace cv;
using namespace std;
using namespace pcl_msgs;
// namespace fs = boost::filesystem;
namespace fs = std::experimental::filesystem;

class Planes2Depth
{
public:
    // typedef pcl::PointXYZRGB Point;
    typedef pcl::PointXYZI Point;
    typedef pcl::PointCloud<Point> PointCloud;

    /*
   * constructor
   * setup which topics are passed out (advertise) and to which topics are listend (subscribe)
   */
    Planes2Depth() : it_(nh_)
    {
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);

        // "~" means, that the node hand is opened within the private namespace (to get the "own" paraemters)
        ros::NodeHandle private_nh("~");
        // ########################################
        // This should be used without a subscriber
        // ########################################

        // string saveDirPcd = "/home/funderburger/work_ws/calibration_ws/planes_extr_ws/training_data/pcd_gt/";                                  // BEWARE of that last / !!! You don't want to forget that, trust me!
        string saveDirPcd = "/home/funderburger/work_ws/calibration_ws/planes_extr_ws/catkin_ws/test_dir/pcd_combo_ir/"; // BEWARE of that last / !!! You don't want to forget that, trust me!
        // string saveDirDepth = "/home/marian/calibration_ws/monodepth-FPN/MonoDepth-FPN-PyTorch/dataset/training_data/training_data/depth_gt/"; // BEWARE of that last / !!! You don't want to forget that, trust me!
        string saveDirDepth = "/home/funderburger/work_ws/calibration_ws/planes_extr_ws/catkin_ws/test_dir/depth_gt/"; // BEWARE of that last / !!! You don't want to forget that, trust me!
        long int np = saveDirPcd.length();
        char saveDirPcdArr[np + 1];
        long int nd = saveDirDepth.length();
        char saveDirDepthArr[nd + 1];
        strcpy(saveDirPcdArr, saveDirPcd.c_str());
        strcpy(saveDirDepthArr, saveDirDepth.c_str());
        if (private_nh.getParam("pcd_folder", _param))
        {
            ROS_INFO("Got param: %s", _param.c_str());
            int i = 0;
            fs::path p(_param.c_str());
            typedef vector<fs::path> vec; // store paths,
            vec v;                        // so we can sort them later
            copy(fs::directory_iterator(p), fs::directory_iterator(), back_inserter(v));
            sort(v.begin(), v.end());
            for (vec::const_iterator it(v.begin()), it_end(v.end()); it != it_end; ++it)
            {
                // string pcd_file = *it.string();
                cout << "pcd_file:" << *it << '\n';
                if (pcl::io::loadPCDFile<pcl::PointXYZ>(*it, *cloud) == -1) //* load the file
                {
                    PCL_ERROR("Couldn't read file %s \n", it);
                }
                // cout << entry.path() << '\n';
                std::cout << "Loaded "
                          << cloud->width * cloud->height
                          << " data points"
                          << std::endl;
                string saveDirP(saveDirPcdArr);
                char numberDirPcdArr[6];
                sprintf(numberDirPcdArr, "%05d", i);
                string pcdFilename = saveDirP + numberDirPcdArr + ".pcd";
                string saveDirD(saveDirDepthArr);
                char numberDirDepthArr[6];
                sprintf(numberDirDepthArr, "%05d", i);
                string depthFilename = saveDirD + numberDirDepthArr + ".png";
                getGTPlanes(cloud, pcdFilename, depthFilename);
                i++;
            }
        }
        else
        {
            ROS_ERROR("Failed to get param 'pcd_folder' ");
        }
    }

    ~Planes2Depth() {}

    void getGTPlanes(PointCloud::Ptr inputCloud, string pcd_file_name, string depth_file_name, string ir_file)
    {

        // Set variables for the current data set,
        // because these are different depending on the camera range
        // ################################## //
        //       multiP_pico_2m_50.bag
        // ################################## //
        // _distanceThreshold = 0.0135;
        // _okDistanceThreshold = 0.005;
        // _clusterTolerance = 0.006;
        // _minClusterSize = 13000;
        // _maxClusterSize = 250000;
        // _maxIterations = 1000;
        // _remained_pointcloud = 0.2;
        //  ################################## //
        //        multiP2_pico_2m_50.bag
        //  ################################## //
        // _distanceThreshold = 0.013;
        // _okDistanceThreshold = 0.005;
        // _clusterTolerance = 0.006;
        // _minClusterSize = 11000;
        // _maxClusterSize = 250000;
        // _maxIterations = 1000;
        // _remained_pointcloud = 0.2;
        //  ################################## //
        //        multiP3_pico_2m_30.bag
        //  ################################## //
        _distanceThreshold = 0.02;
        _okDistanceThreshold = 0.009;
        _clusterTolerance = 0.006;
        _minClusterSize = 11000;
        _maxClusterSize = 250000;
        _maxIterations = 1000;
        _remained_pointcloud = 0.2;
        //  ################################## //
        //        multiP4_pico_11m_0.bag
        //  ################################## //
        // _distanceThreshold = 0.09;
        // _okDistanceThreshold = 0.007;
        // _clusterTolerance = 0.09;
        // _minClusterSize = 11000;
        // _maxClusterSize = 250000;
        // _maxIterations = 1000;
        // _remained_pointcloud = 0.15;
        //  ################################## //
        //        multiP5_pico_11m_0.bag
        //  ################################## //
        // _distanceThreshold = 0.07;
        // _okDistanceThreshold = 0.006;
        // _clusterTolerance = 0.09;
        // _minClusterSize = 12000;
        // _maxClusterSize = 250000;
        // _maxIterations = 1000;
        // _remained_pointcloud = 0.15;
        //  ################################## //
        //        multiP6_pico_9m_0.bag
        //  ################################## //
        // _distanceThreshold = 0.11;
        // _okDistanceThreshold = 0.009;
        // _clusterTolerance = 0.09;
        // _minClusterSize = 12000;
        // _maxClusterSize = 250000;
        // _maxIterations = 1000;
        // _remained_pointcloud = 0.15;
        if (!inputCloud || inputCloud->size() <= 0)
        {
            ROS_WARN("got empty or invalid pointcloud --> ignoring");
            return;
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud(*inputCloud, *cloud1);

        std::cerr << "#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#" << std::endl;
        std::cerr << "Point cloud sizeeeeeeeeeeeeeeeee: " << cloud1->size() << std::endl;

        // cleaning the point cloud of NaNs in order to further apply the Euclidean cluster extraction
        boost::shared_ptr<std::vector<int>> indices(new std::vector<int>);
        pcl::removeNaNFromPointCloud(*cloud1, *indices);
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud1);
        extract.setIndices(indices);
        extract.setNegative(false);
        extract.filter(*cloud1);

        // ///////////////////////////////////////////////////////////////////////////////////////////

        // Preparing the plane extraction
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        // Create the segmentation object
        pcl::SACSegmentation<pcl::PointXYZ> seg;
        // Optional
        seg.setOptimizeCoefficients(true);
        // Mandatory
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setMaxIterations(_maxIterations);
        seg.setDistanceThreshold(_distanceThreshold);

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_rest(new pcl::PointCloud<pcl::PointXYZ>());

        int i = 0, nr_points = (int)cloud1->size(); ////////////////// ?????????????????????????
        std::cerr << "Point cloud size: " << nr_points << std::endl;

        // Stacking all the rectified planes in here for the final reunion
        pcl::PointCloud<pcl::PointXYZ> perfectPlanesCloud;

        // While 30% of the original cloud is still there
        while (cloud1->size() > _remained_pointcloud * nr_points)
        {
            // Extract inliers
            pcl::ExtractIndices<pcl::PointXYZ> plane_extracter;

            pcl::PointCloud<pcl::PointXYZ>::Ptr plane_only(new pcl::PointCloud<pcl::PointXYZ>());
            // Extracting the plane using RANSAC
            seg.setInputCloud(cloud1);
            seg.segment(*inliers, *coefficients);

            std::vector<int> ransac_plane_indices;
            ransac_plane_indices = inliers->indices;

            if (inliers->indices.size() == 0)
            {
                PCL_ERROR("Could not estimate a planar model for the given dataset.\n");
            }

            plane_extracter.setInputCloud(cloud1);
            plane_extracter.setIndices(inliers);
            plane_extracter.setNegative(false);
            // Get the points associated with the planar surface
            plane_extracter.filter(*plane_only);
            std::cerr << "PointCloud representing the planar component: " << plane_only->width * plane_only->height << " data points." << std::endl;

            // ////////////////////////////////////////////////////////////////////////

            // Extracting indices of detected clusters for identifying the only plane
            pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
            tree->setInputCloud(plane_only);

            std::vector<pcl::PointIndices> cluster_indices;
            pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
            ec.setClusterTolerance(_clusterTolerance); // in meters
            ec.setMinClusterSize(_minClusterSize);
            ec.setMaxClusterSize(_maxClusterSize);
            ec.setSearchMethod(tree);
            ec.setInputCloud(plane_only);

            std::vector<int> POIIndices;

            ec.extract(cluster_indices);

            // here should not be more than a single cluster extracted per plane
            int j = 0;
            for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
            {
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
                for (const auto &idx : it->indices)
                {
                    cloud_cluster->push_back((*plane_only)[idx]); //*
                    POIIndices.push_back(ransac_plane_indices[idx]);
                }
                cloud_cluster->width = cloud_cluster->size();
                cloud_cluster->height = 1;
                cloud_cluster->is_dense = true;

                std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size() << " data points." << std::endl;

                j++;
            }
            i++;

            // POI and scraps
            pcl::PointCloud<pcl::PointXYZ>::Ptr scraps_cloud(new pcl::PointCloud<pcl::PointXYZ>);

            // now we need to extract the prior detected plane
            pcl::PointIndices::Ptr clusterInliersIndices(new pcl::PointIndices);
            clusterInliersIndices->indices = POIIndices;

            // Extract Inliers from the input cloud
            pcl::ExtractIndices<pcl::PointXYZ> cluster_extracter;
            cluster_extracter.setInputCloud(cloud1);
            cluster_extracter.setIndices(clusterInliersIndices);
            cluster_extracter.setNegative(false); // Keep only the inliers (i.e. the plane)
            cluster_extracter.filter(*plane_only);

            cluster_extracter.setNegative(true); // Remove the inliers (i.e. remove only the plane and keep the rest)
            cluster_extracter.filter(*scraps_cloud);
            // //////////////////////////////////////////////////////////////////////////

            // Preparing the OK plane extraction
            pcl::ModelCoefficients::Ptr okCoefficients(new pcl::ModelCoefficients);
            pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
            // Create the segmentation object
            pcl::SACSegmentation<pcl::PointXYZ> okSeg;
            // Optional
            okSeg.setOptimizeCoefficients(true);
            // Mandatory
            okSeg.setModelType(pcl::SACMODEL_PLANE);
            okSeg.setMethodType(pcl::SAC_RANSAC);
            okSeg.setMaxIterations(_maxIterations);
            okSeg.setDistanceThreshold(_okDistanceThreshold);

            okSeg.setInputCloud(plane_only);
            okSeg.segment(*inliers, *okCoefficients);

            // Project all the points from POI to a smooth (and silky) plane .....................................................................................................................#doItLikeZohan
            pcl::PointCloud<pcl::PointXYZ>::Ptr perfectPlane(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::ProjectInliers<pcl::PointXYZ> perfectProjection;
            perfectProjection.setModelType(pcl::SACMODEL_PLANE);
            perfectProjection.setInputCloud(plane_only);
            perfectProjection.setModelCoefficients(okCoefficients);
            perfectProjection.filter(*perfectPlane);

            // gathering all the rectified planes in the same place for a final concatenation
            pcl::PointCloud<pcl::PointXYZ> tempPerfectPlane;
            if (i == 0)
            {
                perfectPlanesCloud.points = perfectPlane->points;
            }
            else
            {
                tempPerfectPlane.points = perfectPlane->points;
                perfectPlanesCloud += tempPerfectPlane;
            }
            // //////////////////////////////////////////////////////

            // adding the scraps to  the original point cloud
            cloud1->points = scraps_cloud->points;

            // in case something goes wrong... (because it does, trust me, when there are too few points in some point cloud and then it gets stuck in this loop) so a maximum of 6 planes should be enough, so stop the loop
            if (i == 5)
            {
                break;
            }
        }
        // adding all the remained scraps and all the rectified planes into the same cloud
        pcl::PointCloud<pcl::PointXYZ> cloud_with_gt_planes;
        cloud_with_gt_planes.points = cloud1->points;
        cloud_with_gt_planes += perfectPlanesCloud;

        pcl::PointCloud<pcl::PointXYZ>::Ptr gt_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        gt_cloud->points = cloud_with_gt_planes.points;

        //##########################//
        // save the pcd if you want //
        //##########################//
        pcl::io::savePCDFileASCII(pcd_file_name, *gt_cloud);
        std::cerr << "Saved " << gt_cloud->size() << " data points to" << pcd_file_name << std::endl;

        // ***********************
        // **CONVERSION TO DEPTH**
        // ***********************

        cv_image = Mat(height_, width_, CV_32FC1, 0.0); //Scalar(std::numeric_limits<float>::max()));
        int minRange = 0;                               // this is the smallest distance at which the camera can measure depending on the mode (near, far, etc.)

        for (int i = 0; i < gt_cloud->points.size(); i++)
        {
            if (gt_cloud->points[i].z == gt_cloud->points[i].z)
            {
                if (gt_cloud->points[i].z != 0.0)
                {
                    z = gt_cloud->points[i].z * 1000.0;
                    u = (gt_cloud->points[i].x * 1000.0 * focalX_) / z;
                    v = (gt_cloud->points[i].y * 1000.0 * focalY_) / z;
                    pixel_pos_x = (int)(u + centerX_);
                    pixel_pos_y = (int)(v + centerY_);

                    if (pixel_pos_x > (width_ - 1))
                    {
                        pixel_pos_x = width_ - 1;
                    }
                    else if (pixel_pos_x < 0)
                    {
                        pixel_pos_x = -pixel_pos_x;
                    }
                    if (pixel_pos_y > (height_ - 1))
                    {
                        pixel_pos_y = height_ - 1;
                    }
                    else if (pixel_pos_y < 0)
                    {
                        pixel_pos_y = -pixel_pos_y;
                    }
                }
                else
                {
                    pixel_pos_x = 0;
                    pixel_pos_y = 0;
                    z = 0.0;
                }

                cv_image.at<float>(pixel_pos_y, pixel_pos_x) = z - minRange;
            }
        }

        //###########################//
        //  saving the depth images  //
        //###########################//
        cv_image.convertTo(cv_image, CV_16UC1);
        cv::imwrite(depth_file_name, cv_image);
        std::cerr << "Saved " << gt_cloud->size() << " data points to" << depth_file_name << std::endl;
    }

private:
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    ros::Subscriber sub_;
    // ros::Publisher pub_;
    // ros::Publisher pub2_;
    // ros::Publisher pub3_;
    // ros::Publisher pub4_;

    // image_transport::Publisher pub2_;
    // calibration parameters
    // double K[9] = {385.8655956930966, 0.0, 342.3593021849471,
    //                0.0, 387.13463636528166, 233.38372018194542,
    //                0.0, 0.0, 1.0};

    // EEPROM parameters
    // double K[9] = {386.804, 0.0, 341.675,
    //                0.0, 384, 238.973,
    //                0.0, 0.0, 1.0};

    // // Pico parameters
    // double K[9] = {460.585, 0.0, 334.080,
    //                 0.0, 460.268, 169.807,
    //                 0.0, 0.0, 1.0};

    // Pico parameters OK!!!!!!!!!!!!!!
    double K[9] = {460.585, 0.0, 334.081,
                   0.0, 460.268, 169.808,
                   0.0, 0.0, 1.0};

    double centerX_ = K[2];
    double centerY_ = K[5];
    double focalX_ = K[0];
    double focalY_ = K[4];
    // int height_ = 480;
    int height_ = 360; // only for pico (I guess it has its name for a reason! XD)
    int width_ = 640;
    int pixel_pos_x, pixel_pos_y;
    float z, u, v;
    Mat cv_image;
    std::vector<Point2d> imagePoints;
    dynamic_reconfigure::Server<ddd_plane_extr::planes_paramConfig> config_server_;
    float _distanceThreshold;
    float _okDistanceThreshold;
    float _clusterTolerance;
    float _planesDelay;
    int _minClusterSize;
    int _maxClusterSize;
    float _remained_pointcloud;
    int _maxIterations;
    string _param;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ddd_plane_extr");

    Planes2Depth c2d;

    return 0;
}


#include "drake/solvers/fbstab/components/dense_residual.h"

#include <cmath>
#include <iostream>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/dense_data.h"
#include "drake/solvers/fbstab/components/dense_variable.h"

namespace drake {
namespace solvers {
namespace fbstab {

DenseResidual::DenseResidual(int nz, int nv) {
  nz_ = nz;
  nv_ = nv;
  z_.resize(nz_);
  v_.resize(nv_);
}

void DenseResidual::Negate() {
  z_ *= -1.0;
  v_ *= -1.0;
}

void DenseResidual::NaturalResidual(const DenseVariable& x) {
  const DenseData* const data = x.data();
  // rz = H*z + f + A'*v
  // Calls are arranged to avoid creating temporaries.
  z_.noalias() = data->H() * x.z();
  z_.noalias() += data->f();
  z_.noalias() += data->A().transpose() * x.v();

  // rv = min(y,v)
  v_.noalias() = x.y().cwiseMin(x.v());

  znorm_ = z_.norm();
  vnorm_ = v_.norm();
}

void DenseResidual::PenalizedNaturalResidual(const DenseVariable& x) {
  NaturalResidual(x);
  for (int i = 0; i < nv_; i++) {
    v_(i) = alpha_ * v()(i) +
            (1.0 - alpha_) * max(0.0, x.y()(i)) * max(0.0, x.v()(i));
  }
  znorm_ = z_.norm();
  vnorm_ = v_.norm();
}

void DenseResidual::InnerResidual(const DenseVariable& x,
                                  const DenseVariable& xbar, double sigma) {
  const DenseData* const data = x.data();
  if (xbar.data() != data) {
    throw std::runtime_error(
        "In DenseResidual::InnerResidual: x and xbar have mismatched problem "
        "data.");
  }
  if (x.num_variables() != xbar.num_variables() ||
      x.num_constraints() != xbar.num_constraints()) {
    throw std::runtime_error("Size mismatch in DenseResidual::InnerResidual.");
  }
  if (sigma <= 0) {
    throw std::runtime_error(
        "In DenseResidual::InnerResidual: sigma must be positive.");
  }
  // rz = Hz + f + A'v + sigma(z - zbar)
  // Calls are arranged so as to avoid creating temporaries.
  z_.noalias() = data->H() * x.z();
  z_.noalias() += data->f();
  z_.noalias() += data->A().transpose() * x.v();
  z_.noalias() += sigma * (x.z() - xbar.z());

  // v_ = phi(ys,v), ys = y + sigma(x.v - xbar.v)
  for (int i = 0; i < nv_; i++) {
    const double ys = x.y()(i) + sigma * (x.v()(i) - xbar.v()(i));
    v_(i) = pfb(ys, x.v()(i), alpha_);
  }

  znorm_ = z_.norm();
  vnorm_ = v_.norm();
}

void DenseResidual::Fill(double a) {
  z_.setConstant(a);
  v_.setConstant(a);
}

double DenseResidual::Norm() const {
  return sqrt(znorm_ * znorm_ + vnorm_ * vnorm_);
}

double DenseResidual::Merit() const {
  const double temp = Norm();
  return 0.5 * temp * temp;
}

double DenseResidual::max(double a, double b) { return a > b ? a : b; }

double DenseResidual::min(double a, double b) { return a < b ? a : b; }

double DenseResidual::pfb(double a, double b, double alpha) {
  const double fb = a + b - sqrt(a * a + b * b);
  return alpha * fb + (1.0 - alpha) * max(0, a) * max(0, b);  // NOLINT
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


#ifndef STAN_MATH_REV_SCAL_FUN_BOOST_FPCLASSIFY_HPP
#define STAN_MATH_REV_SCAL_FUN_BOOST_FPCLASSIFY_HPP

#include <stan/math/rev/meta.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <stan/math/rev/core.hpp>

namespace boost {

namespace math {

/**
 * Categorizes the given stan::math::var value.
 *
 * Categorizes the stan::math::var value, v, into the following categories:
 * zero, subnormal, normal, infinite, or NAN.
 *
 * @param v Variable to classify.
 * @return One of FP_ZERO, FP_NORMAL,
 *   FP_FINITE, FP_INFINITE, FP_NAN,
 *   or FP_SUBZERO, specifying the category of v.
 */
template <>
inline int fpclassify(const stan::math::var& v) {
  return (boost::math::fpclassify)(v.val());
}

}  // namespace math
}  // namespace boost
#endif


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef MATHUTILS_HPP
#define MATHUTILS_HPP

#include <algorithm>
#include <bitset>
#include <cmath>
#include <fstream>
#include <iterator>
#include <iomanip>
#include <limits>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "cnpy.hpp"
#include "SplineFunction.hpp"
#include "Symmetry.hpp"

/*! \fn inline int parity(std::bitset<nBits> bitrep)
 *  \param[in] bitrep a bitset
 *  \tparam nBits lenght of the input bitset
 *
 *  Calculate the parity of the bitset as defined by:
 *     bitrep[0] XOR bitrep[1] XOR ... XOR bitrep[nBits-1]
 */
template <size_t nBits>
inline int parity(std::bitset<nBits> bitrep)
{
    int parity = 0;
    for (size_t i = 0; i < bitrep.size(); ++i) {
        parity ^= bitrep[i];
    }
    return parity;
}

/*! \fn inline double parity(unsigned int i)
 *  \param[in] i an integer, usually an index for an irrep or a symmetry operation
 *
 * Returns parity of input integer.
 * The parity is defined as the result of using XOR on the bitrep
 * of the given integer. For example:
 *   2 -> 010 -> 0^1^0 = 1 -> -1.0
 *   6 -> 110 -> 1^1^0 = 0 ->  1.0
 *
 * It can also be interpreted as the action of a given operation on the
 * Cartesian axes:
 *      zyx         Parity
 *   0  000    E      1.0
 *   1  001   Oyz    -1.0
 *   2  010   Oxz    -1.0
 *   3  011   C2z     1.0
 *   4  100   Oxy    -1.0
 *   5  101   C2y     1.0
 *   6  110   C2x     1.0
 *   7  111    i     -1.0
 */
inline double parity(unsigned int i)
{
    // Use a ternary if construct. If the bitset is odd return -1.0 Return +1.0 otherwise.
    return (parity(std::bitset<3>(i)) ? -1.0 : 1.0);
}

/*! \fn inline bool isZero(double value, double threshold)
 *  \param[in] value     the value to be checked
 *  \param[in] threshold the threshold
 *
 *  Returns true if value is less or equal to threshold
 */
inline bool isZero(double value, double threshold)
{
    return (std::abs(value) <= threshold);
}

/*! \fn inline bool numericalZero(double value)
 *  \param[in] value the value to be checked
 *
 *  Returns true if value is less than 1.0e-14
 */
inline bool numericalZero(double value)
{
    return (isZero(value, 1.0e-14));
}

/*! \fn inline void symmetryBlocking(Eigen::MatrixXd & matrix, int cavitySize, int ntsirr, int nr_irrep)
 *  \param[out] matrix the matrix to be block-diagonalized
 *  \param[in]  cavitySize the size of the cavity (size of the matrix)
 *  \param[in]  ntsirr     the size of the irreducible portion of the cavity (size of the blocks)
 *  \param[in]  nr_irrep   the number of irreducible representations (number of blocks)
 */
inline void symmetryBlocking(Eigen::MatrixXd & matrix, PCMSolverIndex cavitySize, PCMSolverIndex ntsirr,
                             int nr_irrep)
{
    // This function implements the simmetry-blocking of the PCM
    // matrix due to point group symmetry as reported in:
    // L. Frediani, R. Cammi, C. S. Pomelli, J. Tomasi and K. Ruud, J. Comput.Chem. 25, 375 (2003)
    // u is the character table for the group (t in the paper)
    Eigen::MatrixXd u = Eigen::MatrixXd::Zero(nr_irrep, nr_irrep);
    for (int i = 0; i < nr_irrep; ++i) {
        for (int j = 0; j < nr_irrep; ++j) {
            u(i, j) = parity(i&j);
        }
    }
    // Naming of indices:
    //     a, b, c, d   run over the total size of the cavity (N)
    //     i, j, k, l   run over the number of irreps (n)
    //     p, q, r, s   run over the irreducible size of the cavity (N/n)
    // Instead of forming U (T in the paper) and then perform the dense
    // multiplication, we multiply block-by-block using just the u matrix.
    //      matrix = U * matrix * Ut; U * Ut = Ut * U = id
    // First half-transformation, i.e. first_half = matrix * Ut
    Eigen::MatrixXd first_half = Eigen::MatrixXd::Zero(cavitySize, cavitySize);
    for (int i = 0; i < nr_irrep; ++i) {
        int ioff = i * ntsirr;
        for (int k = 0; k < nr_irrep; ++k) {
            int koff = k * ntsirr;
            for (int j = 0; j < nr_irrep; ++j) {
                int joff = j * ntsirr;
                double ujk = u(j, k) / nr_irrep;
                for (int p = 0; p < ntsirr; ++p) {
                    int a = ioff + p;
                    for (int q = 0; q < ntsirr; ++q) {
                        int b = joff + q;
                        int c = koff + q;
                        first_half(a, c) += matrix(a, b) * ujk;
                    }
                }
            }
        }
    }
    // Second half-transformation, i.e. matrix = U * first_half
    matrix.setZero(cavitySize, cavitySize);
    for (int i = 0; i < nr_irrep; ++i) {
        int ioff = i * ntsirr;
        for (int k = 0; k < nr_irrep; ++k) {
            int koff = k * ntsirr;
            for (int j = 0; j < nr_irrep; ++j) {
                int joff = j * ntsirr;
                double uij = u(i, j);
                for (int p = 0; p < ntsirr; ++p) {
                    int a = ioff + p;
                    int b = joff + p;
                    for (int q = 0; q < ntsirr; ++q) {
                        int c = koff + q;
                        matrix(a, c) += uij * first_half(b, c);
                    }
                }
            }
        }
    }
    // Traverse the matrix and discard numerical zeros
    for (PCMSolverIndex a = 0; a < cavitySize; ++a) {
        for (PCMSolverIndex b = 0; b < cavitySize; ++b) {
            if (numericalZero(matrix(a, b))) {
                matrix(a, b) = 0.0;
            }
        }
    }
}

/*! \fn inline void symmetryPacking(std::vector<Eigen::MatrixXd> & blockedMatrix, const Eigen::MatrixXd & fullMatrix, int nrBlocks, int dimBlock)
 *  \param[out] blockedMatrix the result of packing fullMatrix
 *  \param[in]  fullMatrix the matrix to be packed
 *  \param[in]  dimBlock the dimension of the square blocks
 *  \param[in]  nrBlocks the number of square blocks
 */
inline void symmetryPacking(std::vector<Eigen::MatrixXd> & blockedMatrix,
                            const Eigen::MatrixXd & fullMatrix, int dimBlock, int nrBlocks)
{
    // This function packs the square block diagonal fullMatrix with nrBlocks of dimension dimBlock
    // into a std::vector<Eigen::MatrixXd> containing nrBlocks square matrices of dimenion dimBlock.
    int j = 0;
    for (int i = 0; i < nrBlocks; ++i) {
        blockedMatrix.push_back(fullMatrix.block(j, j, dimBlock, dimBlock));
        j += dimBlock;
    }
}

/*! \fn inline void hermitivitize(Eigen::MatrixBase<Derived> & obj_)
 *  \param[out] obj_ the Eigen object to be hermitivitized
 *  \tparam     Derived the numeric type of obj_ elements
 *
 *  Given obj_ returns 0.5 * (obj_ + obj_^dagger)
 *  \note We check if a matrix or vector was given, since in the latter
 *  case we only want the complex conjugation operation to happen.
 */
template <typename Derived>
inline void hermitivitize(Eigen::MatrixBase<Derived> & obj_)
{
  // We need to use adjoint().eval() to avoid aliasing issues, see:
  // http://eigen.tuxfamily.org/dox/group__TopicAliasing.html
  // The adjoint is evaluated explicitly into an intermediate.
  obj_ = 0.5 * (obj_ + obj_.adjoint().eval());
}

/*! \fn inline void eulerRotation(Eigen::Matrix3d & R_, const Eigen::Vector3d & eulerAngles_)
 *  \brief Build rotation matrix between two reference frames given the Euler angles.
 *  \param[out] R_ the rotation matrix
 *  \param[in]  eulerAngles_ the Euler angles, in degrees, describing the rotation
 *
 *  We assume the convention \f$ R = Z_3 X_2 Z_1 \f$ for the ordering of the extrinsic
 *  elemental rotations (see http://en.wikipedia.org/wiki/Euler_angles)
 *  The Euler angles are given in the order \f$ \phi, \theta, \psi \f$.
 *  If we write \f$ c_i, s_i \,\, i = 1, 3 \f$ for their cosines and sines the rotation
 *  matrix will be:
 *  \f[
 *  	R = \begin{pmatrix}
 *  	      c_1c_3 - s_1c_2s_3 & -s_1c_3 - c_1c_2s_3 &  s_2s_3 \\
 *  	      c_1s_3 + s_1c_2c_3 & -s_1s_3 + c_1c_2c_3 & -s_2c_3 \\
 *  	      s_1s_2             & c_1s_2              &  c_2
 *  	    \end{pmatrix}
 *  \f]
 *  Eigen's geometry module is used to calculate the rotation matrix
 */
inline void eulerRotation(Eigen::Matrix3d & R_, const Eigen::Vector3d & eulerAngles_)
{
    double to_radians = M_PI / 180.0;
    double phi   = eulerAngles_(0) * to_radians;
    double theta = eulerAngles_(1) * to_radians;
    double psi   = eulerAngles_(2) * to_radians;
    R_ = Eigen::AngleAxis<double>(psi,   Eigen::Vector3d::UnitZ())
         * Eigen::AngleAxis<double>(theta, Eigen::Vector3d::UnitX())
         * Eigen::AngleAxis<double>(phi,   Eigen::Vector3d::UnitZ());
}

/*! \brief Return value of function defined on grid at an arbitrary point
 *  \param[in] point     where the function has to be evaluated
 *  \param[in] grid      holds points on grid where function is known
 *  \param[in] function  holds known function values
 *
 *  This function finds the nearest values for the given point
 *  and performs a linear interpolation.
 *  \warning This function assumes that grid has already been sorted!
 */
inline double linearInterpolation(const double point, const std::vector<double> & grid,
        const std::vector<double> & function)
{
    // Find nearest points on grid to the arbitrary point given
    size_t index = std::distance(grid.begin(), std::lower_bound(grid.begin(), grid.end(), point)) - 1;

    // Parameters for the interpolating line
    double y_1 = function[index], y_0 = function[index-1];
    double x_1 = grid[index], x_0 = grid[index-1];
    double m = (y_1 - y_0) / (x_1 - x_0);

    return (m * (point - x_0) + y_0);
}

/*! \brief Return value of function defined on grid at an arbitrary point
 *  \param[in] point     where the function has to be evaluated
 *  \param[in] grid      holds points on grid where function is known
 *  \param[in] function  holds known function values
 *
 *  This function finds the nearest values for the given point
 *  and performs a cubic spline interpolation.
 *  \warning This function assumes that grid has already been sorted!
 */
inline double splineInterpolation(const double point, const std::vector<double> & grid,
        const std::vector<double> & function)
{
    // Find nearest points on grid to the arbitrary point given
    size_t index = std::distance(grid.begin(), std::lower_bound(grid.begin(), grid.end(), point)) - 1;

    // Parameters for the interpolating spline
    Eigen::Vector3d x, y;
    x << grid[index-1], grid[index], grid[index+1];
    y << function[index-1], function[index], function[index+1];
    SplineFunction s(x, y);

    return s(point);
}

/*! \brief Prints Eigen object (matrix or vector) to file
 *  \param[in] matrix Eigen object
 *  \param[in] fname  name of the file
 *  \tparam Derived template parameters of the MatrixBase object
 *
 *  \note This is for debugging only, the format is in fact rather ugly.
 *      Row index     Column index      Matrix entry
 *         0               0              0.0000
 */
template <typename Derived>
inline void print_eigen_matrix(const Eigen::MatrixBase<Derived> & matrix, const std::string & fname)
{
  std::ofstream fout;
  fout.open(fname.c_str());
  fout << " Row index " << '\t' << " Column index " << '\t' << " Matrix entry " << std::endl;
  int rows = matrix.rows();
  int cols = matrix.cols();
  for (int i = 0; i < rows; ++i) {
    for (int j = 0; j < cols; ++j) {
      fout << i << '\t' << j << '\t' << matrix(i, j) << std::endl;
    }
  }
  fout.close();
}

namespace cnpy
{
  /*!
   *  \addtogroup custom
   *  @{
   */
  /*! Custom overloads for cnpy load and save functions */
  namespace custom
  {
    /*! \brief Save Eigen object to NumPy array file
     *  \param fname name of the NumPy array file
     *  \param obj Eigen object to be saved, either a matrix or a vector
     *  \tparam Scalar the data type of the matrix to be returned. Default is double
     *  \tparam Rows number of rows in the Eigen object. Default is dynamic
     e  \tparam Cols number of columns in the Eigen object. Default is dynamic
     */
    template <typename Scalar, int Rows, int Cols>
    inline void npy_save(const std::string & fname, const Eigen::Matrix<Scalar, Rows, Cols> & obj)
    {
      unsigned int rows = static_cast<unsigned int>(obj.rows());
      unsigned int cols = static_cast<unsigned int>(obj.cols());
      const unsigned int shape[] = {rows, cols};
      cnpy::npy_save(fname, obj.data(), shape, 2, "w", obj.IsRowMajor);
    }

    /*! \brief Save Eigen object to a compressed NumPy file
     *  \param fname name of the compressed NumPy file
     *  \param name tag for the given object in the compressed NumPy file
     *  \param obj  Eigen object to be saved, either a matrix or a vector
     *  \param overwrite if file exists, overwrite. Appends by default.
     *  \tparam Scalar the data type of the matrix to be returned. Default is double
     *  \tparam Rows number of rows in the Eigen object. Default is dynamic
     *  \tparam Cols number of columns in the Eigen object. Default is dynamic
     */
    template <typename Scalar, int Rows, int Cols>
    inline void npz_save(const std::string & fname, const std::string & name,
        const Eigen::Matrix<Scalar, Rows, Cols> & obj,
        bool overwrite = false)
    {
      unsigned int rows = static_cast<unsigned int>(obj.rows());
      unsigned int cols = static_cast<unsigned int>(obj.cols());
      const unsigned int shape[] = {rows, cols};
      std::string mode = overwrite ? "w" : "a";
      cnpy::npz_save(fname, name, obj.data(), shape, 2, mode, obj.IsRowMajor);
    }

    /*! \brief Load NpyArray object into Eigen object
     *  \param npy_array the NpyArray object
     *  \tparam Scalar the data type of the matrix to be returned. Default is double
     *  \return An Eigen object (matrix or vector) with the data
     *
     *  \todo Extend to read in also data in row-major (C) storage order
     *  \warning We check that the rank of the object read is not more than 2
     *  Eigen cannot handle general tensors.
     */
    template <typename Scalar>
    inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> npy_to_eigen(const NpyArray & npy_array)
    {
      if (npy_array.shape.size() > 2) PCMSOLVER_ERROR("Only vectors and matrices can be read into Eigen objects.", BOOST_CURRENT_FUNCTION);
      return Eigen::Map<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> >(reinterpret_cast<Scalar *>(npy_array.data), npy_array.shape[0], npy_array.shape[1]);
    }

    /*! \brief Load NumPy array file into Eigen object
     *  \param fname name of the NumPy array file
     *  \tparam Scalar the data type of the matrix to be returned. Default is double
     *  \return An Eigen object (matrix or vector) with the data
     *
     *  \todo Extend to read in also data in row-major (C) storage order
     */
    template <typename Scalar>
    inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> npy_load(const std::string & fname)
    {
      return npy_to_eigen<Scalar>(cnpy::npy_load(fname));
    }
  } // namespace custom
  /*! @} */
} // namespace cnpy

#endif // MATHUTILS_HPP


///////////////////////////////////////////////////////////////////////////////
#include <string.h>
#include <iostream>
#include <vector>
#include <cassert>
#include <thread>
#include <mutex>
#include <dirent.h>
#include <string>
#include <stdlib.h>
#include <sys/stat.h>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <DGtal/base/Common.h>
#include <DGtal/io/boards/Board2D.h>
#include <DGtal/helpers/StdDefs.h>
#include <DGtal/io/readers/PGMReader.h>
#include "DGtal/images/imagesSetsUtils/SetFromImage.h"
#include "DGtal/images/ImageContainerBySTLVector.h"
#include "imageCharacterization.h"
///////////////////////////////////////////////////////////////////////////////

using namespace std;
using namespace DGtal;
using namespace DGtal::Z2i; //We'll only consider Z² digital space on
                //32bit integers

int main(int argc, char *argv[]) {
    if(argc != 3) {
        cerr << "Syntax: " << argv[0] << " <image 1> <image 2>" << endl ;
        exit(EXIT_FAILURE) ;
    }

    ImageCharacterization img1(argv[1]) ;
    ImageCharacterization img2(argv[2]) ;
    vector<double> normalization = { // generated with the file descriptors.info
        0.897024,
        1.00415,
        1.54932,
        2.16182,
        0.941636,
        0.327149,
        0.181375,
        0.203551,
        0.0035972,
        0.0128021
    } ;
    cout << img1.distance(img2, normalization) << endl ;
    return 0;
}


#include <gtest/gtest.h>

#include <random>

#include <Eigen/Core>

#include "rtff/buffer/audio_buffer.h"
#include "rtff/buffer/overlap_ring_buffer.h"
#include "rtff/buffer/ring_buffer.h"

TEST(Buffer, AudioBuffer) {
  // Test convertion split channel to interleaved and interleaved to split
  // channel
  using namespace rtff;
  const auto frame_number = 44100;
  const auto channel_count = 2;

  AudioBuffer buffer_from_split(frame_number, channel_count);
  AudioBuffer buffer_from_interleaved(frame_number, channel_count);

  Eigen::VectorXf left_channel = Eigen::VectorXf::Random(frame_number);
  Eigen::VectorXf right_channel = Eigen::VectorXf::Random(frame_number);
  Eigen::MatrixXf interleaved(channel_count, frame_number);
  interleaved.row(0) = left_channel;
  interleaved.row(1) = right_channel;

  // write data
  std::copy(left_channel.data(), left_channel.data() + frame_number,
            buffer_from_split.data(0));
  std::copy(right_channel.data(), right_channel.data() + frame_number,
            buffer_from_split.data(1));
  buffer_from_interleaved.fromInterleaved(interleaved.data());

  // read
  Eigen::VectorXf read_left = Eigen::Map<Eigen::VectorXf>(
      buffer_from_interleaved.data(0), frame_number);
  Eigen::VectorXf read_right = Eigen::Map<Eigen::VectorXf>(
      buffer_from_interleaved.data(1), frame_number);
  Eigen::MatrixXf read_interleaved(channel_count, frame_number);
  buffer_from_split.toInterleaved(read_interleaved.data());

  // make sure it is the same
  ASSERT_EQ(read_left, left_channel);
  ASSERT_EQ(read_right, right_channel);
  ASSERT_EQ(interleaved, read_interleaved);
}

TEST(Buffer, OverlapRingBuffer) {
  using namespace rtff;

  const auto frame_number = 44100;
  Eigen::VectorXf data = Eigen::VectorXf::Random(frame_number);

  const auto write_size = 512;  // write one block at a time
  const auto read_size = 2048;  // read fft_size data
  const auto step_size = 1024;  // overlap
  auto current_frame = 0;
  auto successful_read_count = 0;

  Eigen::VectorXf output_data(read_size);
  OverlapRingBuffer buffer(read_size, step_size);

  // write first 3 parts
  // => we shouldn't be able to read as we don't have enough data
  buffer.Write(data.segment(current_frame, write_size).data(), write_size);
  ASSERT_FALSE(buffer.Read(output_data.data()));
  current_frame += write_size;
  buffer.Write(data.segment(current_frame, write_size).data(), write_size);
  ASSERT_FALSE(buffer.Read(output_data.data()));
  current_frame += write_size;
  buffer.Write(data.segment(current_frame, write_size).data(), write_size);
  ASSERT_FALSE(buffer.Read(output_data.data()));
  current_frame += write_size;
  // write 4th part
  // => we have enough data. outut should be the first read_size frames of the
  // input signal
  buffer.Write(data.segment(current_frame, write_size).data(), write_size);
  ASSERT_TRUE(buffer.Read(output_data.data()));
  ASSERT_EQ(output_data, data.segment(0, read_size));
  current_frame += write_size;
  successful_read_count++;
  // write 5th part
  // can't read as we don't have enough data
  buffer.Write(data.segment(current_frame, write_size).data(), write_size);
  ASSERT_FALSE(buffer.Read(output_data.data()));
  current_frame += write_size;
  // write 6th part
  // => read should work and read data should be the input skipped by step_size
  buffer.Write(data.segment(current_frame, write_size).data(), write_size);
  ASSERT_TRUE(buffer.Read(output_data.data()));
  ASSERT_EQ(output_data, data.segment(step_size, read_size));
  current_frame += write_size;
  successful_read_count++;

  // keep on doing this until the end of the signal
  while (current_frame < frame_number - read_size) {
    // can't read the first time
    buffer.Write(data.segment(current_frame, write_size).data(), write_size);
    ASSERT_FALSE(buffer.Read(output_data.data()));
    current_frame += write_size;

    // can read the second
    buffer.Write(data.segment(current_frame, write_size).data(), write_size);
    ASSERT_TRUE(buffer.Read(output_data.data()));
    ASSERT_EQ(output_data,
              data.segment(successful_read_count * step_size, read_size));
    current_frame += write_size;
    successful_read_count++;
  }
}

TEST(Buffer, MultichannelOverlapRingBuffer) {
  using namespace rtff;

  const auto frame_number = 44100;
  const auto channel_number = 2;
  Eigen::MatrixXf data = Eigen::MatrixXf::Random(channel_number, frame_number);

  const auto write_size = 512;  // write one block at a time
  const auto read_size = 2048;  // read fft_size data
  const auto step_size = 1024;  // overlap
  auto current_frame = 0;
  auto successful_read_count = 0;

  Eigen::MatrixXf output_data(channel_number, read_size);
  Eigen::MatrixXf input_data(channel_number, write_size);
  MultichannelOverlapRingBuffer buffer(read_size, step_size, channel_number);

  AudioBuffer input_buffer(write_size, channel_number);
  AudioBuffer output_buffer(read_size, channel_number);

  // write first 2 can't read
  input_data = data.block(0, current_frame, channel_number, write_size);
  input_buffer.fromInterleaved(input_data.data());
  buffer.Write(input_buffer, input_buffer.frame_count());
  ASSERT_FALSE(buffer.Read(&output_buffer));
  current_frame += write_size;
  input_data = data.block(0, current_frame, channel_number, write_size);
  input_buffer.fromInterleaved(input_data.data());
  buffer.Write(input_buffer, input_buffer.frame_count());
  ASSERT_FALSE(buffer.Read(&output_buffer));
  current_frame += write_size;

  // then each time we add 2 we can read 1
  while (current_frame < frame_number - read_size) {
    input_data = data.block(0, current_frame, channel_number, write_size);
    input_buffer.fromInterleaved(input_data.data());
    buffer.Write(input_buffer, input_buffer.frame_count());
    ASSERT_FALSE(buffer.Read(&output_buffer));
    current_frame += write_size;

    input_data = data.block(0, current_frame, channel_number, write_size);
    input_buffer.fromInterleaved(input_data.data());
    buffer.Write(input_buffer, input_buffer.frame_count());
    ASSERT_TRUE(buffer.Read(&output_buffer));
    output_buffer.toInterleaved(output_data.data());
    ASSERT_EQ(output_data, data.block(0, successful_read_count * step_size,
                                      channel_number, read_size));

    current_frame += write_size;
    successful_read_count++;
  }
}

TEST(Buffer, OverlapRingBufferRandomWriteSize) {
  using namespace rtff;

  std::random_device
      rd;  // Will be used to obtain a seed for the random number engine
  std::mt19937 gen(rd());  // Standard mersenne_twister_engine seeded with rd()

  const auto frame_number = 44100;
  Eigen::VectorXf data = Eigen::VectorXf::Random(frame_number);

  const auto max_write_size = 512;
  const auto read_size = 2048;  // read fft_size data
  const auto step_size = 1024;  // overlap
  auto current_frame = 0;
  auto successful_read_count = 0;

  Eigen::VectorXf output_data(read_size);
  OverlapRingBuffer buffer(read_size, step_size);

  std::uniform_int_distribution<> dis(1, max_write_size);
  auto written_size = 0;
  while (written_size < frame_number) {
    auto write_size = dis(gen);
    buffer.Write(data.segment(current_frame, write_size).data(), write_size);

    written_size += write_size;
    if (written_size < read_size) {
      ASSERT_FALSE(buffer.Read(output_data.data()));
    } else if (written_size - successful_read_count >= read_size) {
      ASSERT_TRUE(buffer.Read(output_data.data()));
      successful_read_count += step_size;
    } else {
      ASSERT_FALSE(buffer.Read(output_data.data()));
    }
  }
}

TEST(Buffer, RingBuffer) {
  using namespace rtff;

  const auto frame_number = 44100;

  RingBuffer buffer(2048 * 8);

  Eigen::VectorXf data = Eigen::VectorXf::Random(frame_number);
  Eigen::VectorXf output_data(frame_number);

  auto written_size = 0;
  buffer.Write(data.data(), 256);
  written_size += 256;
  // we can't read more data than what we input
  ASSERT_FALSE(buffer.Read(output_data.data(), written_size + 1));
  // we can read less
  ASSERT_TRUE(buffer.Read(output_data.data(), written_size - 20));
  // but we won't have any left
  ASSERT_FALSE(buffer.Read(output_data.data(), written_size));
  ASSERT_FALSE(buffer.Read(output_data.data(), 21));
  ASSERT_TRUE(buffer.Read(output_data.data(), 20));

  // we can also write in several times
  buffer.Write(data.data(), 256);
  ASSERT_FALSE(buffer.Read(output_data.data(), 512));
  buffer.Write(data.data(), 256);
  ASSERT_TRUE(buffer.Read(output_data.data(), 512));
}

TEST(Buffer, MultichannelRingBuffer) {
  using namespace rtff;

  const auto channel_number = 2;

  AudioBuffer input_buffer(2048, channel_number);
  AudioBuffer output_buffer(2048, channel_number);

  MultichannelRingBuffer buffer(2048 * 8, 2);

  auto written_size = 0;
  buffer.Write(input_buffer, 256);
  written_size += 256;
  // we can't read more data than what we input
  ASSERT_FALSE(buffer.Read(&output_buffer, written_size + 1));
  // we can read less
  ASSERT_TRUE(buffer.Read(&output_buffer, written_size - 20));
  // but we won't have any left
  ASSERT_FALSE(buffer.Read(&output_buffer, written_size));
  ASSERT_FALSE(buffer.Read(&output_buffer, 21));
  ASSERT_TRUE(buffer.Read(&output_buffer, 20));

  // we can also write in several times
  buffer.Write(input_buffer, 256);
  ASSERT_FALSE(buffer.Read(&output_buffer, 512));
  buffer.Write(input_buffer, 256);
  ASSERT_TRUE(buffer.Read(&output_buffer, 512));
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, New York University, Max Planck Gesellschaft,
//                          University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_KKT_HPP_
#define CROCODDYL_CORE_SOLVERS_KKT_HPP_

#include <Eigen/Dense>
#include <Eigen/Cholesky>

#include "crocoddyl/core/solver-base.hpp"

namespace crocoddyl {

class SolverKKT : public SolverAbstract {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  explicit SolverKKT(boost::shared_ptr<ShootingProblem> problem);
  virtual ~SolverKKT();

  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
                     const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t& maxiter = 100,
                     const bool& is_feasible = false, const double& regInit = 1e-9);
  virtual void computeDirection(const bool& recalc = true);
  virtual double tryStep(const double& steplength = 1);
  virtual double stoppingCriteria();
  virtual const Eigen::Vector2d& expectedImprovement();

  const Eigen::MatrixXd& get_kkt() const;
  const Eigen::VectorXd& get_kktref() const;
  const Eigen::VectorXd& get_primaldual() const;
  const std::vector<Eigen::VectorXd>& get_dxs() const;
  const std::vector<Eigen::VectorXd>& get_dus() const;
  const std::vector<Eigen::VectorXd>& get_lambdas() const;
  const std::size_t& get_nx() const;
  const std::size_t& get_ndx() const;
  const std::size_t& get_nu() const;

 protected:
  double regfactor_;
  double regmin_;
  double regmax_;
  double cost_try_;
  std::vector<Eigen::VectorXd> xs_try_;
  std::vector<Eigen::VectorXd> us_try_;

 private:
  std::size_t nx_;
  std::size_t ndx_;
  std::size_t nu_;
  std::vector<Eigen::VectorXd> dxs_;
  std::vector<Eigen::VectorXd> dus_;
  std::vector<Eigen::VectorXd> lambdas_;
  double calc();
  void computePrimalDual();
  void increaseRegularization();
  void decreaseRegularization();
  void allocateData();

  // allocate data
  Eigen::MatrixXd kkt_;
  Eigen::VectorXd kktref_;
  Eigen::VectorXd primaldual_;
  Eigen::VectorXd primal_;
  Eigen::VectorXd dual_;
  std::vector<double> alphas_;
  double th_grad_;
  bool was_feasible_;
  Eigen::VectorXd kkt_primal_;
  Eigen::VectorXd dF;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_KKT_HPP_


#include <opengv/optimization_tools/objective_function_tools/GlobalPnPInfiniteNormFunctionInfo.hpp>
#include <Eigen/Dense>
#include <opengv/Indices.hpp>
#include <iostream>

GlobalPnPInfiniteNormFunctionInfo::GlobalPnPInfiniteNormFunctionInfo(const opengv::absolute_pose::AbsoluteAdapterBase & adapter, const opengv::rotation_t & rotation, const opengv::translation_t &translation){
  Eigen::MatrixXd M   = Eigen::MatrixXd::Zero(3,5);
  Eigen::Matrix3d Vi  = Eigen::MatrixXd::Zero(3,3);
  Eigen::Matrix3d id  = Eigen::MatrixXd::Identity(3,3);
  opengv::Indices indices(adapter.getNumberCorrespondences());
  int total_points = (int) indices.size();
  //std::cout << "Total points: " << total_points << std::endl;
  Eigen::MatrixXd vi     = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd xi     = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd ci     = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd result = Eigen::MatrixXd::Zero(1,1);

  Eigen::MatrixXd C_all = Eigen::MatrixXd::Zero(3, total_points);
  Eigen::MatrixXd V_all = Eigen::MatrixXd::Zero(3, total_points);
  Eigen::MatrixXd X_all = Eigen::MatrixXd::Zero(3, total_points);
  Eigen::MatrixXd wi    = Eigen::MatrixXd::Zero(3,1);

  Eigen::MatrixXd coefs_gradients = Eigen::MatrixXd::Zero(9,13);
  Eigen::MatrixXd Qi              = Eigen::MatrixXd::Zero(3,3);
  Eigen::MatrixXd Mr              = Eigen::MatrixXd::Zero(3,9);
  for( int i = 0; i < total_points; i++ )
  {
    vi = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    xi = adapter.getPoint(indices[i]);
    ci = adapter.getCamOffset(indices[i]);
    C_all.block<3,1>(0,i)  = adapter.getCamOffset(indices[i]);
    X_all.block<3,1>(0,i)  = adapter.getPoint(indices[i]);
    V_all.block<3,1>(0,i)  = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);

    Vi =  vi * vi.transpose()  / ((vi.transpose() * vi)(0,0));
    Qi = (id - Vi).transpose() * (id - Vi);
    M.block<3,3>(0,0) = (id - Vi).transpose() * (id - Vi);
    M.block<3,1>(0,3) = xi;
    M.block<3,1>(0,4) = ci;
    //Coefficients for obj function calculation and the translation
    //gradient are obtained and stored
    coefficients.push_back(M);

    //Initialize list of objective functions
    wi = rotation * xi + translation - ci;
    //std::cout << "wi: " << std::endl << wi << std::endl;
    result = wi.transpose() *  Qi * wi;
    function[i] = result(0,0);

    //Create the matrix used to compute the
    Mr.block<3,3>(0,0) = xi(0,0) * Eigen::MatrixXd::Identity(3,3);
    Mr.block<3,3>(0,3) = xi(1,0) * Eigen::MatrixXd::Identity(3,3);
    Mr.block<3,3>(0,6) = xi(2,0) * Eigen::MatrixXd::Identity(3,3);
    coefs_gradients.block<9,9>(0,0) = Mr.transpose() * Qi * Mr;
    coefs_gradients.block<9,3>(0,9) = 2 * Mr.transpose() * Qi;
    coefs_gradients.block<9,1>(0,12) = -2 * Mr.transpose() * Qi * ci;
    matrix_gradients.push_back(coefs_gradients);
  }

  /*std::cout << "ci"   << std::endl << C_all       << std::endl;
  std::cout << "xi"   << std::endl << X_all       << std::endl;
  std::cout << "vi"   << std::endl << V_all       << std::endl;
  std::cout << "R_: " << std::endl << rotation    << std::endl;
  std::cout << "t_: " << std::endl << translation << std::endl;
  for(int i = 0; i < coefficients.size(); ++i){
    std::cout << "Matrix n: " << i << std::endl << coefficients[i] << std::endl;
  }
  for(int i = 0; i < coefficients.size(); ++i){
    std::cout << "Matrix grad n: " << i << std::endl << matrix_gradients[i] << std::endl;
  }
  for(auto it = function.cbegin(); it != function.cend(); ++it){
    std::cout << it->first << " " << it->second << "\n";
    }*/

}

GlobalPnPInfiniteNormFunctionInfo::~GlobalPnPInfiniteNormFunctionInfo(){};

double GlobalPnPInfiniteNormFunctionInfo::objective_function_value(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  Eigen::MatrixXd wi = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd Qi = Eigen::MatrixXd::Zero(3,3);
  for(std::map<int,double>::iterator it=function.begin(); it != function.end(); ++it){
    wi = rotation * coefficients[it->first].block<3,1>(0,3) + translation - coefficients[it->first].block<3,1>(0,4);
    Qi = coefficients[it->first].block<3,3>(0,0);
    it->second = (wi.transpose() * Qi * wi)(0,0);
  }

  using pair_type = decltype(function)::value_type;
  auto pr = std::max_element(
    std::begin(function), std::end(function),
    [] (const pair_type & p1, const pair_type & p2) {
        return p1.second < p2.second;
    }
			     );
  //for(auto it = function.cbegin(); it != function.cend(); ++it){
  //  std::cout << it->first << " " << it->second << "\n";
  //}

  min_key = pr->first;
  //std::cout << "MIN KEY: " << min_key << std::endl;
  return (pr->second);
}

opengv::rotation_t GlobalPnPInfiniteNormFunctionInfo::rotation_gradient(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  objective_function_value(rotation, translation);
  Eigen::MatrixXd Mr  = matrix_gradients[min_key].block<9,9>(0,0);
  Eigen::MatrixXd Mrt = matrix_gradients[min_key].block<9,3>(0,9);
  Eigen::MatrixXd vr  = matrix_gradients[min_key].block<9,1>(0,12);

  const double * p = &rotation(0);
  Map<const Matrix<double,1,9> > r(p, 1, 9);
  Eigen::MatrixXd grad_r = (2 * Mr * r.transpose()) + Mrt * translation + vr;
  double * ptr = &grad_r(0);
  Map<Matrix<double, 3,3> > m(ptr, 3, 3);
  return m;
}

opengv::translation_t GlobalPnPInfiniteNormFunctionInfo::translation_gradient(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  objective_function_value(rotation, translation);
  Eigen::MatrixXd Qi = coefficients[min_key].block<3,3>(0,0);
  Eigen::MatrixXd xi = coefficients[min_key].block<3,1>(0,3);
  Eigen::MatrixXd ci = coefficients[min_key].block<3,1>(0,4);
  Eigen::MatrixXd grad_t = 2 * Qi * (rotation * xi + translation - ci);
  return grad_t;
}


#ifndef BOOST_QVM_MATH_HPP_INCLUDED
#define BOOST_QVM_MATH_HPP_INCLUDED

/// Copyright (c) 2008-2021 Emil Dotchevski and Reverge Studios, Inc.

/// Distributed under the Boost Software License, Version 1.0. (See accompanying
/// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <math.h>
#include <boost/qvm/inline.hpp>

namespace boost { namespace qvm {

template <class T> T acos( T );
template <class T> T asin( T );
template <class T> T atan( T );
template <class T> T atan2( T, T );
template <class T> T cos( T );
template <class T> T sin( T );
template <class T> T tan( T );
template <class T> T cosh( T );
template <class T> T sinh( T );
template <class T> T tanh( T );
template <class T> T exp( T );
template <class T> T log( T );
template <class T> T log10( T );
template <class T> T mod( T , T );
template <class T> T pow( T, T );
template <class T> T sqrt( T );
template <class T> T ceil( T );
template <class T> T abs( T );
template <class T> T floor( T );
template <class T> T mod( T, T );
template <class T> T ldexp( T, int );
template <class T> T sign( T );

template <> BOOST_QVM_INLINE_TRIVIAL float acos<float>( float x ) { return ::acosf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float asin<float>( float x ) { return ::asinf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float atan<float>( float x ) { return ::atanf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float atan2<float>( float x, float y ) { return ::atan2f(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL float cos<float>( float x ) { return ::cosf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float sin<float>( float x ) { return ::sinf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float tan<float>( float x ) { return ::tanf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float cosh<float>( float x ) { return ::coshf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float sinh<float>( float x ) { return ::sinhf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float tanh<float>( float x ) { return ::tanhf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float exp<float>( float x ) { return ::expf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float log<float>( float x ) { return ::logf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float log10<float>( float x ) { return ::log10f(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float mod<float>( float x, float y ) { return ::fmodf(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL float pow<float>( float x, float y ) { return ::powf(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL float sqrt<float>( float x ) { return ::sqrtf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float ceil<float>( float x ) { return ::ceilf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float abs<float>( float x ) { return ::fabsf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float floor<float>( float x ) { return ::floorf(x); }
template <> BOOST_QVM_INLINE_TRIVIAL float ldexp<float>( float x, int y ) { return ::ldexpf(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL float sign<float>( float x ) { return x<0 ? -1.f : +1.f; }

template <> BOOST_QVM_INLINE_TRIVIAL double acos<double>( double x ) { return ::acos(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double asin<double>( double x ) { return ::asin(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double atan<double>( double x ) { return ::atan(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double atan2<double>( double x, double y ) { return ::atan2(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL double cos<double>( double x ) { return ::cos(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double sin<double>( double x ) { return ::sin(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double tan<double>( double x ) { return ::tan(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double cosh<double>( double x ) { return ::cosh(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double sinh<double>( double x ) { return ::sinh(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double tanh<double>( double x ) { return ::tanh(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double exp<double>( double x ) { return ::exp(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double log<double>( double x ) { return ::log(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double log10<double>( double x ) { return ::log10(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double mod<double>( double x, double y ) { return ::fmod(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL double pow<double>( double x, double y ) { return ::pow(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL double sqrt<double>( double x ) { return ::sqrt(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double ceil<double>( double x ) { return ::ceil(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double abs<double>( double x ) { return ::fabs(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double floor<double>( double x ) { return ::floor(x); }
template <> BOOST_QVM_INLINE_TRIVIAL double ldexp<double>( double x, int y ) { return ::ldexp(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL double sign<double>( double x ) { return x<0 ? -1.0 : +1.0; }

template <> BOOST_QVM_INLINE_TRIVIAL long double acos<long double>( long double x ) { return ::acosl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double asin<long double>( long double x ) { return ::asinl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double atan<long double>( long double x ) { return ::atanl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double atan2<long double>( long double x, long double y ) { return ::atan2l(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL long double cos<long double>( long double x ) { return ::cosl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double sin<long double>( long double x ) { return ::sinl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double tan<long double>( long double x ) { return ::tanl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double cosh<long double>( long double x ) { return ::coshl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double sinh<long double>( long double x ) { return ::sinhl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double tanh<long double>( long double x ) { return ::tanhl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double exp<long double>( long double x ) { return ::expl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double log<long double>( long double x ) { return ::logl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double log10<long double>( long double x ) { return ::log10l(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double mod<long double>( long double x, long double y ) { return ::fmodl(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL long double pow<long double>( long double x, long double y ) { return ::powl(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL long double sqrt<long double>( long double x ) { return ::sqrtl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double ceil<long double>( long double x ) { return ::ceill(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double abs<long double>( long double x ) { return ::fabsl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double floor<long double>( long double x ) { return ::floorl(x); }
template <> BOOST_QVM_INLINE_TRIVIAL long double ldexp<long double>( long double x, int y ) { return ::ldexpl(x,y); }
template <> BOOST_QVM_INLINE_TRIVIAL long double sign<long double>( long double x ) { return x<0 ? -1.0 : +1.0; }

} }

#endif


#ifndef MJOLNIR_TEST_UTIL_CHECK_POTENTIAL_HPP
#define MJOLNIR_TEST_UTIL_CHECK_POTENTIAL_HPP

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

namespace mjolnir
{
namespace test
{

// This checks force applied to each particle and the numerical difference of
// the corresponding energy

template<typename Potential>
void check_potential(const Potential& pot,
        const typename Potential::real_type x_min,
        const typename Potential::real_type x_max,
        const typename Potential::real_type tol,
        const typename Potential::real_type h,
        const std::size_t N)
{
    using real_type = typename Potential::real_type;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=1; i<N; ++i)
    {
        const real_type x = x_min + i * dx;
        const real_type pot1 = pot.potential(x + h);
        const real_type pot2 = pot.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);

        const real_type deri = pot.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(tol));
    }
}

template<typename Potential>
void check_potential(const Potential& pot,
        const typename Potential::parameter_type& para,
        const typename Potential::real_type x_min,
        const typename Potential::real_type x_max,
        const typename Potential::real_type tol,
        const typename Potential::real_type h,
        const std::size_t N)
{
    using real_type = typename Potential::real_type;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=1; i<N; ++i)
    {
        const real_type x = x_min + i * dx;
        const real_type pot1 = pot.potential(x + h, para);
        const real_type pot2 = pot.potential(x - h, para);
        const real_type dpot = (pot1 - pot2) / (2 * h);

        const real_type deri = pot.derivative(x, para);

        BOOST_TEST_MESSAGE("x = " << x << ", V(x+h) = " << pot1 << ", V(x-h) = " << pot2
                           << ", dV/h = " << dpot << ", dV/dx = " << deri);
        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(tol));
    }
}

} // test
} // mjolnir
#endif// MJOLNIR_TEST_UTIL_CHECK_POTENTIAL_HPP


#ifndef NETWORKS_HPP
#define NETWORKS_HPP

#include <iostream>
#include <vector>
// For External Library
#include <torch/torch.h>
#include <boost/program_options.hpp>

// Define Namespace
namespace nn = torch::nn;
namespace po = boost::program_options;

// Function Prototype
void weights_init(nn::Module &m);
void Convolution(nn::Sequential &sq, const size_t in_nc, const size_t out_nc, const size_t ksize, const size_t stride, const size_t pad, const bool BN, const bool LReLU, const bool bias=false);

// -------------------------------------------------
// struct{YOLOv2Impl}(nn::Module)
// -------------------------------------------------
struct YOLOv2Impl : nn::Module{
private:
    nn::Sequential stage1, stage2a, stage2b, stage3;
public:
    YOLOv2Impl(){}
    YOLOv2Impl(po::variables_map &vm);
    torch::Tensor forward(torch::Tensor x);
};

// -------------------------------------------------
// struct{FloorAvgPool2dImpl}(nn::Module)
// -------------------------------------------------
struct FloorAvgPool2dImpl : nn::Module{
private:
    std::vector<long int> multiple;
public:
    FloorAvgPool2dImpl(){}
    FloorAvgPool2dImpl(std::vector<long int> multiple_);
    torch::Tensor forward(torch::Tensor x);
    void pretty_print(std::ostream& stream) const override;
};

// -------------------------------------------------
// struct{ReorganizeImpl}(nn::Module)
// -------------------------------------------------
struct ReorganizeImpl : nn::Module{
private:
    long int stride;
public:
    ReorganizeImpl(){}
    ReorganizeImpl(long int stride_);
    torch::Tensor forward(torch::Tensor x);
    void pretty_print(std::ostream& stream) const override;
};

TORCH_MODULE(YOLOv2);
TORCH_MODULE(FloorAvgPool2d);
TORCH_MODULE(Reorganize);

#endif


/* Copyright (C) 2012-2018 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */

#include <NTL/BasicThreadPool.h>
#include "NumbTh.h"
#include "powerful.h"
#include "fhe_stats.h"
#include "ArgMap.h"

#include <algorithm>
#include <cmath>
#include <string>
#include <cstdio>

NTL_CLIENT

#ifdef NTL_THREADS
typedef atomic<long> ATOMIC_LONG;
#else
typedef long ATOMIC_LONG;
#endif

static string v_values_name = "";

static void
dump_v_values()
{
  const vector<double> *v_values = fetch_saved_values("v_values");
  if (v_values && v_values_name != "") {
    // write v_values to a file

    cerr << "writing v_values to " << v_values_name << "\n";

    ofstream F;
    F.open(v_values_name.c_str());
    for (long i: range(v_values->size()))
      F << (*v_values)[i] << "\n";
  }
}

static void
anderson_darling(const vector<double>& X, double& AD, double& p_val)
{
  long N = X.size();

  if (N < 2) {
    AD = 0;
    p_val = 1;
    return;
  }

  vector<double> Y(X);
  sort(Y.begin(), Y.end());

  // compute the sample mean
  double SM = 0;
  for (long i: range(N)) SM += Y[i];
  SM /= N;

  // compute the sample variance
  double SV = 0;
  for (long i: range(N)) SV += (Y[i]-SM)*(Y[i]-SM);
  SV /= (N-1);

  // replace Y[i] by CDF of Y[i]
  for (long i: range(N))
    Y[i] = 0.5*(1 + erf((Y[i]-SM)/sqrt(2*SV)));

  double S = 0;
  for (long i: range(N)) {
    S += (2*i+1)*(log(Y[i]) + log1p(-Y[N-1-i]));
  }
  AD = -N - S/N;

  AD *= (1 + 0.75/N + 2.25/N/N);
  // This adjustment and the p-values below come from:
  // R.B. D'Augostino and M.A. Stephens, Eds., 1986,
  // Goodness-of-Fit Techniques, Marcel Dekker.

  p_val;
  if (AD >= 0.6)
    p_val = exp(1.2937 - 5.709*(AD)+ 0.0186*fsquare(AD));
  else if (AD > 0.34)
    p_val = exp(0.9177 - 4.279*(AD) - 1.38*fsquare(AD));
  else if (AD > 0.2)
    p_val = 1 - exp(-8.318 + 42.796*(AD)- 59.938*fsquare(AD));
  else
    p_val = 1 - exp(-13.436 + 101.14*(AD)- 223.73*fsquare(AD));
}

static void
print_anderson_darling()
{
  const vector<double> *v_values = fetch_saved_values("v_values");
  if (v_values) {
    double AD, p_val;
    anderson_darling(*v_values, AD, p_val);
    printf("AD=%6.4f, p_val=%8.6f", AD, p_val);
  }
}

static void
print_sigma_info()
{
  const vector<double> *v_values = fetch_saved_values("v_values");
  if (v_values) {
    double max_sigma = 0;
    for (double val: *v_values) {
      if (fabs(val) > max_sigma) max_sigma = fabs(val);
    }
    double max_sigma_prob = 1 - exp(double(v_values->size())*log(erf(max_sigma/sqrt(2))));
    // max_sigma_prob is the probability of seeing max_sigma this big,
    // assuming independent gaussians
    printf("max_sigma=%6.4f, max_sigma_prob=%8.6f\n", max_sigma, max_sigma_prob);
  }
}

int main(int argc, char *argv[])
{
  ArgMap amap;

  long t=120;
  long nthreads=1;
  long seed=0;
  Vec<long> mvec;
  long m = 0;
  long iter;
  long nfacs = 0;

  amap.arg("t", t, "Hamming weight of recryption secret key");
  amap.arg("nthreads", nthreads, "number of threads");
  amap.arg("seed", seed, "random number seed");
  amap.arg("mvec", mvec);
  amap.arg("m", m);
  amap.arg("nfacs", nfacs);
  amap.arg("iter", iter);
  amap.arg("v_values", v_values_name);

  amap.parse(argc, argv);

  if (seed)
    SetSeed(ZZ(seed));

  SetNumThreads(nthreads);

  fhe_stats = true;

  const long N = (1L << 20) + 1;

  if (mvec.length() == 0) {
    if (m == 0) {
      if (nfacs == 0) nfacs = RandomBnd(5)+1;
      vector<long> mmvec;
      do {
	m = RandomBnd(15000) + 25000;
	if (m % 2 == 0) m++;
        pp_factorize(mmvec, m);
      } while (mmvec.size() != nfacs);
      convert(mvec, mmvec);
    }
    else {
      vector<long> mmvec;
      pp_factorize(mmvec, m);
      convert(mvec, mmvec);
    }
  }
  else {
    m = 1;
    for (long fac: mvec) m *= fac;
  }

  long phim = phi_N(m);

  ZZX phimX_ZZX = Cyclotomic(m);

  long k = mvec.length();
  double mrat = double(phim)/double(m);
  double sigma = double(N) * sqrt( mrat * double(t) * double(1L << k)  / 3.0 ) * 0.5;

  ATOMIC_LONG counter(iter);

  zz_p::FFTInit(0);
  long p = zz_p::modulus();

  PowerfulTranslationIndexes ind(mvec);
  PowerfulConversion pConv(ind);

  zz_pX phimX = conv<zz_pX>(phimX_ZZX);
  zz_pXModulus PhimX(phimX);

  zz_pContext context;
  context.save();

  NTL_EXEC_INDEX(nthreads, index)

  context.restore();

  HyperCube<zz_p> pwrfl(pConv.getShortSig());
  Vec<zz_p>& pwrfl_data = pwrfl.getData();

  zz_pX poly1, poly2;

  while (--counter >= 0) {
    // initialize poly1 to a polynomial mod X^m-1 with t non-zero
    // +/- 1 coeffs
    poly1.rep.SetLength(m);
    for (long i: range(m)) poly1.rep[i] = 0;
    for (long i: range(t)) {
      long idx;
      do {
	idx = RandomBnd(m);
      } while (poly1.rep[idx] != 0);
      if (RandomBnd(2))
        poly1.rep[idx] = 1;
      else
        poly1.rep[idx] = -1;
    }
    poly1.normalize();
    rem(poly1, poly1, PhimX);

    // initialize pwrfl1 to random mod N (balanced)
    for (zz_p& data: pwrfl_data) {
      long val = RandomBnd(N);
      data = balRem(val, N);
    }

    pConv.powerfulToPoly(poly2, pwrfl);

    MulMod(poly2, poly1, poly2, PhimX);

    pConv.polyToPowerful(pwrfl, poly2);

    double max_pwrfl = 0;
    for (long i: range(phim)) {
       long max_pwrfl0 = rep(pwrfl_data[i]);
       max_pwrfl0 = balRem(max_pwrfl0, p);
       double max_pwrfl1 = max_pwrfl0;
       double std_devs = fabs(max_pwrfl1)/sigma;
       if (std_devs > max_pwrfl) max_pwrfl = std_devs;
    }

    FHE_STATS_SAVE("max_pwrfl_saved", max_pwrfl);

    long ran_pwrfl0 = rep(pwrfl_data[RandomBnd(phim)]);
    ran_pwrfl0 = balRem(ran_pwrfl0, p);
    double ran_pwrfl = ran_pwrfl0;
    double std_devs = fabs(ran_pwrfl)/sigma;

    // update various indicator variables
    FHE_STATS_UPDATE("sigma_1_0", double(std_devs <= 1.0)); // 0.683
    FHE_STATS_UPDATE("sigma_2_0", double(std_devs <= 2.0)); // 0.954
    FHE_STATS_UPDATE("sigma_3_0", double(std_devs <= 3.0)); // 0.997, 1 in 370

    // compute sample variance, and scale by the variance we expect
    FHE_STATS_UPDATE("var_calc", fsquare(ran_pwrfl)/fsquare(sigma));

    // save the scaled value for application of other tests
    FHE_STATS_SAVE("v_values", ran_pwrfl/sigma);
  }

  NTL_EXEC_INDEX_END

  const vector<double>& max_pwrfl_saved = *fetch_saved_values("max_pwrfl_saved");
  double max_pwrfl = 0;
  for (double one_pwrfl: max_pwrfl_saved) {
    if (one_pwrfl > max_pwrfl) max_pwrfl = one_pwrfl;
  }

  double max_pwrfl_prob = 1 - exp(double(iter)*double(phim)*log(erf(max_pwrfl/sqrt(2))));
  // max_pwrfl_prob is the probability of seeing max_pwrfl this big,
  // assuming independent gaussians

  double max_pwrfl_prob0, max_pwrfl_prob1;
  // max_pwrfl_prob1 is the probability of seeing max_pwrfl this big,
  // using union bound

  max_pwrfl_prob0 = double(phim)*erfc(max_pwrfl/sqrt(2));
  if (max_pwrfl_prob0 > 1)
    max_pwrfl_prob1 = 1;
  else
    max_pwrfl_prob1 = 1 - exp(double(iter)*log(1-max_pwrfl_prob0));

  cerr << "\n";

  print_stats(cout);
  print_anderson_darling();
  cout << ", mvec=" << mvec << "\n";
  print_sigma_info();
  printf("max_pwrfl=%6.4f, max_pwrfl_prob=%8.6f, max_pwrfl_prob1=%8.6f\n", max_pwrfl, max_pwrfl_prob, max_pwrfl_prob1);
  dump_v_values();

  return 0;
}


#define BOOST_TEST_MODULE pcraster geo circular_neighbourhood
#include <boost/test/unit_test.hpp>
#include "geo_circularneighbourhood.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace geo;

  {
    CircularNeighbourhood neighbourhood(1.0);
    BOOST_CHECK(neighbourhood.cell(0,0) == 0.0);
    BOOST_CHECK(neighbourhood.cell(0,1) == 1.0);
    BOOST_CHECK(neighbourhood.cell(0,2) == 0.0);
    BOOST_CHECK(neighbourhood.cell(1,0) == 1.0);
    BOOST_CHECK(neighbourhood.cell(1,1) == 1.0);
    BOOST_CHECK(neighbourhood.cell(1,2) == 1.0);
    BOOST_CHECK(neighbourhood.cell(2,0) == 0.0);
    BOOST_CHECK(neighbourhood.cell(2,1) == 1.0);
    BOOST_CHECK(neighbourhood.cell(2,2) == 0.0);
  }
}


#include <stdint.h>
#include <cstdlib>

#include <iostream>
#include <memory>
#include <random>
#include <string>
#include <vector>

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdeprecated-register"
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/stuff/sampler.h>
#pragma clang diagnostic pop

#include <pcg_random.hpp>
#include <cpp_mpl.hpp>

#include "sampler.hpp"
#include "types.hpp"


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

//[building_footprint_rectangle_gilviewer_parameters
#include "rjmcmc-wx/wx_parameter.hpp"
typedef parameters< wx_parameter > param;
#include "building_footprint_rectangle_parameters_inc.hpp"
//]

//[building_footprint_rectangle_gilviewer_core_include
#include "building_footprint_rectangle.hpp"
//]

//[building_footprint_rectangle_gilviewer_gilviewer_and_resources
#include <GilViewer/gui/panel_viewer.hpp>
#include <GilViewer/gui/layer_control.hpp>
#include <GilViewer/gui/panel_manager.hpp>
#include <GilViewer/plugins/plugin_base.hpp>
#include <GilViewer/layers/image_layer.hpp>
#include <GilViewer/layers/vector_layer_ghost.hpp>
#include <GilViewer/io/gilviewer_file_io.hpp>
#include <GilViewer/io/gilviewer_io_factory.hpp>
//]

//[building_footprint_rectangle_gilviewer_visitors
#include "rjmcmc/simulated_annealing/visitor/composite_visitor.hpp"
#include "rjmcmc/simulated_annealing/visitor/any_visitor.hpp"
#include "rjmcmc-wx/all.hpp"
//]

#include "rjmcmc/rjmcmc/sampler/any_sampler.hpp"
#include "rjmcmc/simulated_annealing/salamon_initial_schedule.hpp"

//[building_footprint_rectangle_gilviewer_specific
#include <boost/thread.hpp>
#include <boost/filesystem/operations.hpp>
#include <wx/app.h>
#include <wx/gdicmn.h>
#include <vector>
#include <wx/aui/framemanager.h>
//]

#include <boost/filesystem.hpp>

//[building_footprint_rectangle_gilviewer_app_declaration
class building_footprint_rectangle_gilviewer : public wx_plugin_base, public simulated_annealing::Controler
{
    //]

private:
    typedef rjmcmc::mt19937_generator Engine;
    typedef rjmcmc::any_sampler<Engine,configuration> any_sampler;
    typedef simulated_annealing::any_composite_visitor<configuration,any_sampler> any_composite_visitor;

    //[building_footprint_rectangle_gilviewer_app_oninit
    bool OnInit()
    {
        if(m_confg_visitor) return true;
        /*< Parameters processing >*/
        m_param = new param;
        initialize_parameters(m_param);

        /*< Visitors initialization >*/
        panel_viewer *panel = panel_manager::instance()->panels_list()[0];
        m_confg_visitor = new simulated_annealing::wx::configuration_visitor(panel);
        m_param_visitor = new simulated_annealing::wx::parameters_visitor(panel,m_param,m_parent);
        m_chart_visitor = new simulated_annealing::wx::chart_visitor(m_parent);
        m_contr_visitor = new simulated_annealing::wx::controler_visitor(this,m_parent);
        // m_visitor.push_back( m_log_visitor);
        m_visitor.push_back( *m_confg_visitor);
        m_visitor.push_back( *m_param_visitor);
        m_visitor.push_back( *m_chart_visitor);
        m_visitor.push_back( *m_contr_visitor);

        m_contr_visitor->add_pane(m_manager);
        m_manager->Update();
        return true;
    }
    //]

public:

    std::string submenu_name() { return "librjmcmc_unused ???"; }
    std::string menuentry_name() { return "librjmcmc"; }

    virtual void show(wxCommandEvent&) {
        OnInit();
        param_visibility(true);
        chart_visibility(true);
    }

    void load_image(const std::string& file)
    {
        if(file=="") return;
        layer_control *lc = m_confg_visitor->panelviewer()->layercontrol();
        layer_control::const_iterator it  = lc->begin();
        layer_control::const_iterator end = lc->end();
        for(;it!=end;++it)
        {
            using namespace boost::filesystem;
            if((*it)->filename()==path(system_complete(file)).string())
            {
                return; // it;
            }
        }
        // return
        panel_viewer *panel = m_confg_visitor->panelviewer();
        boost::shared_ptr<vector_layer_ghost> ghost = panel->vectorlayerghost();
        std::string extension(boost::filesystem::extension(file));
        try
        {
            boost::shared_ptr<gilviewer_file_io> file_io = m_factory->create_object(extension.substr(1,extension.size()-1));
            layer::ptrLayerType  ilayer = file_io->load(file);
            if(!ilayer) throw std::exception();
            lc->add_layer( ilayer );
            if(ghost)
            {
                ghost->reset();
                ghost->transform() = ilayer->transform();
            }
        }
        catch (const std::exception &e)
        {
            using namespace std;
            GILVIEWER_LOG_EXCEPTION("Read error: " + file);
            return;
        }
    }

    //[building_footprint_rectangle_gilviewer_go
    virtual void process()
    {
        OnInit();
        release();
        update_values(m_param);

        Iso_rectangle_2 bbox = get_bbox(m_param);
        std::string  dsm_file = m_param->get<boost::filesystem::path>("dsm"  ).string();
        std::string mask_file = m_param->get<boost::filesystem::path>("mask" ).string();
        clip_bbox(bbox,dsm_file );

        gradient_functor gf(m_param->get<double>("sigmaD"));
        oriented_gradient_image grad_image(dsm_file,  bbox, gf);
        m_grad = grad_image.img();

        load_image(dsm_file );
        load_image(mask_file);

        set_bbox(m_param,bbox);
        wxPoint p0(wxCoord(bbox.min().x()),wxCoord(bbox.min().y()));
        wxPoint p1(wxCoord(bbox.max().x()),wxCoord(bbox.max().y()));
        m_confg_visitor->set_bbox(wxRect(p0,p1));

        init_visitor        (m_param,m_visitor);
        create_configuration(m_param,grad_image,m_config);
        //<-
        //    estimate_initial_temperature(m_param,100,*m_config);
        //->
        create_sampler      (m_param,m_sampler);
        create_schedule     (m_param,m_schedule);
        create_end_test     (m_param,m_end_test);

        //    std::cout << "Salamon initial schedule : " << salamon_initial_schedule(m_sampler->density(),*m_config,1000) << std::endl;
        //    m_config->clear();

        any_sampler *sampler = new any_sampler(*m_sampler);
        Engine& e = rjmcmc::random();

        m_thread = new boost::thread(
                simulated_annealing::optimize<Engine,configuration,any_sampler,schedule,end_test,any_composite_visitor>,
                e, boost::ref(*m_config), boost::ref(*sampler),
                boost::ref(*m_schedule),   boost::ref(*m_end_test),
                boost::ref(m_visitor) );
    }
    //]

    //[building_footprint_rectangle_gilviewer_controller_overload
    virtual void go  () { process(); }
    virtual void stop() { m_end_test->stop(); }

    virtual void param_visibility(bool b) { m_param_visitor->Show(b); }
    virtual void chart_visibility(bool b) { m_chart_visitor->Show(b); }
    virtual bool param_visibility() const { return m_param_visitor->IsShown(); }
    virtual bool chart_visibility() const { return m_chart_visitor->IsShown(); }
    //]

    //[building_footprint_rectangle_gilviewer_cons_des
    building_footprint_rectangle_gilviewer() :
            m_config(NULL), m_sampler(NULL), m_schedule(NULL), m_end_test(NULL),
            m_visitor(), m_thread(NULL),
            m_confg_visitor(NULL), m_param_visitor(NULL), m_chart_visitor(NULL)
    {
    }

    virtual ~building_footprint_rectangle_gilviewer() {
        release();
        if(m_param) delete m_param;
    }
    //]

    virtual bool Register(gilviewer_io_factory *factory)
    {
        m_factory = factory;
		return true;
    }

private:
    //[building_footprint_rectangle_gilviewer_release
    void release()
    {
        if(m_end_test){ m_end_test->stop(); }
        if(m_thread)  { m_thread->join(); delete m_thread; m_thread=NULL; }
        if(m_end_test){ delete m_end_test;m_end_test=NULL; }
        if(m_config)  { delete m_config;  m_config  =NULL; }
        if(m_schedule){ delete m_schedule;m_schedule=NULL; }
        if(m_sampler) { delete m_sampler; m_sampler =NULL; }
    }
    //]

    //[building_footprint_rectangle_gilviewer_members
    configuration *m_config;
    sampler       *m_sampler;
    schedule      *m_schedule;
    end_test      *m_end_test;
    any_composite_visitor  m_visitor;
    boost::thread *m_thread;
    param         *m_param;

    gilviewer_io_factory *m_factory;

    simulated_annealing::wx::controler_visitor     *m_contr_visitor;
    simulated_annealing::wx::configuration_visitor *m_confg_visitor;
    simulated_annealing::wx::parameters_visitor    *m_param_visitor;
    simulated_annealing::wx::chart_visitor         *m_chart_visitor;
    simulated_annealing::wx::log_visitor            m_log_visitor;
    boost::shared_ptr<gradient_image_t>     m_grad;
};
//]

//[building_footprint_rectangle_gilviewer_implement
IMPLEMENT_PLUGIN(building_footprint_rectangle_gilviewer);
//]


#include <CompositeCalendar.h>
#include <LondonCalendar.h>
#include <NewYorkFedCalendar.h>
#include <SydneyCalendar.h>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>

using namespace qff_a;
using boost::gregorian::date;

int main() {
  const SydneyCalendar syd{};
  const auto comp =
      CompositeCalendar(SydneyCalendar(), NewYorkFedCalendar(), LondonCalendar());
  const auto comp2 = comp.Clone();

  const date bank_holiday(2019, 8, 5);
  std::cout << "Bank Holiday is a holiday in Sydney: "
            << syd.IsHoliday(bank_holiday) << '\n';
  std::cout << "Bank Holiday is a business day in Sydney: "
            << syd.IsBusinessDay(bank_holiday) << '\n';
  std::cout << "Bank Holiday is a holiday in Composite Calendar: "
            << comp.IsHoliday(bank_holiday) << '\n';
  std::cout << "Bank Holiday is a business day in Composite Calendar: "
            << comp.IsBusinessDay(bank_holiday) << '\n';
  std::cout << "Bank Holiday is a holiday in Composite Calendar (Clone): "
            << comp2->IsHoliday(bank_holiday) << '\n';
  std::cout << "Bank Holiday is a business day in Composite Calendar (Clone): "
            << comp2->IsBusinessDay(bank_holiday) << '\n';
  return 0;
}


/*!
 * @file
 * Forward declares the @ref Field typeclass.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_FWD_FIELD_HPP
#define BOOST_MPL11_FWD_FIELD_HPP

#include <boost/mpl11/fwd/bool.hpp>

namespace boost { namespace mpl11 {
    /*!
     * @ingroup typeclasses
     * @defgroup Field Field
     *
     * `Ring` whose non-zero elements form a `Group` under multiplication.
     *
     *
     * ### Methods
     * `recip` and `quot`
     *
     * ### Minimal complete definition
     * Either `recip` or `quot`.
     *
     *
     * @todo
     * Test the default implementations.
     *
     * @{
     */
    template <typename Left, typename Right = Left, typename = true_>
    struct Field;

    //! Multiplication of `x` by the multiplicative inverse of `y`.
    template <typename x, typename y>
    struct quot;

    //! Multiplicative inverse of `x`.
    template <typename x>
    struct recip;
    //! @}
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_FWD_FIELD_HPP


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Core>

double summation(const Eigen::MatrixXd& X, const Eigen::MatrixXd& Y) {
    double s = 0;
    for (size_t i = 0; i < X.rows(); ++i) {
        for (size_t j = 0; j < Y.rows(); ++j) {
            if (0 < X(i) * Y(j)) {
                s += std::log10((X(i) + Y(j)) * (X(i) + Y(j))) + std::sqrt(X(i) * Y(j));
            }
        }
    }
    return s;
}

PYBIND11_MODULE(cpp, m) {
    m.def("summation", &summation);
}