/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
All rights reserved.
This software was developed in the APRIL Robotics Lab under the
direction of Edwin Olson, ebolson@umich.edu. This software may be
available under alternative licensing terms; contact the address above.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of the Regents of The University of Michigan.
*/

//./opencv_demo -f tag36h11 -p tcp://192.168.204.140:7500

#include <iostream>

#include "opencv2/opencv.hpp"
#include <fstream>
#include <json.hpp>
#include <Publisher.hpp>
#include <signal.h>
#include <getopt.h>
#include <thread>
#include <markerPoseData.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <mathUtils.h>

extern "C" {
#include "apriltag.h"
#include "tag36h11.h"
#include "tag25h9.h"
#include "tag16h5.h"
#include "tagCircle21h7.h"
#include "tagCircle49h12.h"
#include "tagCustom48h12.h"
#include "tagStandard41h12.h"
#include "tagStandard52h13.h"
#include "common/getopt.h"
#include "common/homography.h"
#include "apriltag_pose.h"
}

using namespace std;
using namespace cv;

Common::Publisher* aprilcodePublisher; //("tcp://*:8500");

std::string publisherAddress;
std::string fam;
bool publisherAddress_specified;

bool ctrl_c_pressed;
void ctrlc(int)
{
    ctrl_c_pressed = true;
}

//from rotation To Quaternion

/*
void fromRotationToQuaternion( Eigen::Quaternion<float>& q , apriltag_pose_t& a ) {

    Eigen::Vector3f traceVec;
    traceVec <<a.R->data[0] , a.R->data[4] , a.R->data[8];

    if( traceVec.sum() > 0 ) {
        double s = 0.5f / sqrtf(traceVec.sum()+ 1.0f);
        q.w() = 0.25f / s;
        q.x() = ( a.R->data[7] - a.R->data[5] ) * s;
        q.y() = ( a.R->data[2] - a.R->data[6] ) * s;
        q.z() = ( a.R->data[3] - a.R->data[1] ) * s;
    }
    else {
        if ( a.R->data[0] > a.R->data[4] && a.R->data[0] > a.R->data[8] ) {
            double s = 2.0f * sqrtf( 1.0f + a.R->data[0] - a.R->data[4] - a.R->data[8]);
            q.w() = (a.R->data[7] - a.R->data[5] ) / s;
            q.x() = 0.25f * s;
            q.y() = (a.R->data[1] + a.R->data[3] ) / s;
            q.z() = (a.R->data[2] + a.R->data[6] ) / s;
        }
        else if (a.R->data[4] > a.R->data[8]) {
            double s = 2.0f * sqrtf( 1.0f + a.R->data[4] - a.R->data[0] - a.R->data[8]);
            q.w() = (a.R->data[2] - a.R->data[6] ) / s;
            q.x() = (a.R->data[1] + a.R->data[3] ) / s;
            q.y() = 0.25f * s;
            q.z() = (a.R->data[5] + a.R->data[7] ) / s;
        } else {
            double s = 2.0f * sqrtf( 1.0f + a.R->data[8] - a.R->data[0] - a.R->data[4] );
            q.w() = (a.R->data[3] - a.R->data[1] ) / s;
            q.x() = (a.R->data[2] + a.R->data[6] ) / s;
            q.y() = (a.R->data[5] + a.R->data[7] ) / s;
            q.z() = 0.25f * s;
        }
    }

    //std::cout<<std::setprecision(4) <<" W = " << q.w() <<" , X = " << q.x() <<" , Y = " << q.y() <<" , Z = " << q.z()  << std::endl;
}

void fromQuaternionToRPY(float& roll,float& pitch,float& yaw,Eigen::Quaternion<float>& q) {
    double sqw = q.w()*q.w();
    double sqx = q.x()*q.x();
    double sqy = q.y()*q.y();
    double sqz = q.z()*q.z();
    double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
    double test = q.x()*q.y() + q.z()*q.w();
    //if (test > 0.499*unit) { // singularity at north pole
        if (test > 0.400*unit) { // singularity at north pole
            pitch = 2 * atan2(q.x(),q.w());
            yaw = M_PI/2;
            roll = 0;
        return;
    }
    //if (test < -0.499*unit) { // singularity at south pole
    if (test < -0.400*unit) {
        pitch = -2 * atan2(q.x() ,q.w());
        yaw = -M_PI/2;
        roll = 0;
        return;
    }

    pitch = atan2(2*q.y()*q.w()-2*q.x()*q.z() , sqx - sqy - sqz + sqw);
    yaw = asin(2*test/unit);
    roll = atan2(2*q.x()*q.w()-2*q.y()*q.z() , -sqx + sqy - sqz + sqw);

}

*/

bool checkInBoundaries(apriltag_detection_t* det )
{

    for(size_t i = 0 ; i < 4 ; i++)
    {
        if(    ( det->p[i][1] > 20 && det->p[i][1] < 470 )        &&  ( det->p[i][0] >20  && det->p[i][0] < 630  )   )
            continue;
        else
        {
            return false;
        }
    }
    return true;

};

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

    /*inizio opzioni apriltag Detector*/
    getopt_t *getopt = getopt_create();

    getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
    getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");
    getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");
    getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use");
    getopt_add_int(getopt, 't', "threads", "1", "Use this many CPU threads");
    getopt_add_double(getopt, 'x', "decimate", "2.0", "Decimate input image by this factor");
    getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input");
    getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
    getopt_add_string(getopt, 'p', "pubAddr", "ppubAddr", "pubAddr");

    if (!getopt_parse(getopt, argc, argv, 1) ||
        getopt_get_bool(getopt, "help")) {
        printf("Usage: %s [options]\n", argv[0]);
        getopt_do_usage(getopt);
        exit(0);
    }

    // Initialize camera
    //VideoCapture cap(0);
    VideoCapture cap(0);
    if (!cap.isOpened()) {
        cerr << "Couldn't open video capture device" << endl;
        return -1;
    }

    // Initialize tag detector with options
    apriltag_family_t *tf = NULL;
    publisherAddress = getopt_get_string(getopt, "pubAddr");
    const char *famname = getopt_get_string(getopt, "family");

    //    const char *famname = fam.c_str();
    if (!strcmp(famname, "tag36h11")) {
        tf = tag36h11_create();
        } else if (!strcmp(famname, "tag25h9")) {
            tf = tag25h9_create();
        } else if (!strcmp(famname, "tag16h5")) {
            tf = tag16h5_create();
        } else if (!strcmp(famname, "tagCircle21h7")) {
            tf = tagCircle21h7_create();
        } else if (!strcmp(famname, "tagCircle49h12")) {
            tf = tagCircle49h12_create();
        } else if (!strcmp(famname, "tagStandard41h12")) {
            tf = tagStandard41h12_create();
        } else if (!strcmp(famname, "tagStandard52h13")) {
            tf = tagStandard52h13_create();
        } else if (!strcmp(famname, "tagCustom48h12")) {
            tf = tagCustom48h12_create();
        }
        else {
            printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
            exit(-1);
    }

    apriltag_detector_t *td = apriltag_detector_create();
    apriltag_detector_add_family(td, tf);
    td->quad_decimate = getopt_get_double(getopt, "decimate");
    td->quad_sigma = getopt_get_double(getopt, "blur");
    td->nthreads = getopt_get_int(getopt, "threads");
    td->debug = getopt_get_bool(getopt, "debug");
    td->refine_edges = getopt_get_bool(getopt, "refine-edges");

    aprilcodePublisher = new Common::Publisher(publisherAddress);
    std::string out;

    Mat frame, gray;

    float x = 0 ;
    float y =0 ;
    float z  =0;
    float roll=0; float pitch = 0; float yaw=0;
    nlohmann::json j;
    markerPoseData marker,pastMarker;
    pastMarker.x = 0;
    pastMarker.y = 0;
    pastMarker.z = 0;
    pastMarker.roll = 0;
    pastMarker.pitch = 0;
    pastMarker.yaw = 0;
    pastMarker.to_json(j);

    std::string  tmp_string ;
    const char* out_cstring ;;

    while (true) {

        cap >> frame;
        cvtColor(frame, gray, COLOR_BGR2GRAY);

        // Make an image_u8_t header for the Mat data
        image_u8_t im = { .width = gray.cols,
            .height = gray.rows,
            .stride = gray.cols,
            .buf = gray.data
        };

        zarray_t *detections = apriltag_detector_detect(td, &im);

        //detections->size tells how many apriltag are detected in the image
        //cout<<"Size if zarray (zarray->size) " << detections->size << endl;

        // Draw detection outlines
        apriltag_pose_t pose;

        for (int i = 0; i < zarray_size(detections); i++) {

            apriltag_detection_t *det;

            zarray_get(detections, i, &det);

            apriltag_detection_info_t info;
            info.det = det;
            info.tagsize = 0.054;
            info.fx = 632.74999736;//683.75954508;//661.25;
            info.fy = 633.5834324;//687.37260599;//667.81;
            info.cx = 312.43271146;//264.12598696;//299.04;
            info.cy = 229.88570203;//237.96954446;//250.50;
            int flag;
            flag = 0;
            double err = estimate_tag_pose( &info,  &pose, &flag);

            if(flag)
            {
                //j["id"] = det->id;
                //pastMarker.x = x;
                //pastMarker.y = y;
               // pastMarker.z = z;
                //pastMarker.roll = roll;
               // pastMarker.pitch = pitch;
               // pastMarker.yaw = yaw;
                std::cout << "FLAG"<<std::endl;
                marker.inView = 0;
                marker.to_json(j);
                tmp_string = j.dump();
                out_cstring = tmp_string.c_str();

                aprilcodePublisher->send("MARKER", out_cstring, strlen(out_cstring) );
                continue;
            }
            x = pose.t->data[0] ;
            y = pose.t->data[1] ;
            z = pose.t->data[2] ;

            Eigen::Matrix3f R;
            R << pose.R->data[0] ,pose.R->data[1]  ,pose.R->data[2],
                pose.R->data[3] ,pose.R->data[4] ,pose.R->data[5],
                pose.R->data[6] ,pose.R->data[7], pose.R->data[8];

            fromRotationToRPYAngle(roll,pitch,yaw,R);

            std::cout<< setprecision(4) << "x = " << x <<  "y = " << y << "z = " << z << std::endl;

            //std::cerr << err <<std::endl;
            //estimate_pose_for_tag_homography( &info,  &pose);

            line(frame, Point(det->p[0][0], det->p[0][1]),
                     Point(det->p[1][0], det->p[1][1]),
                     Scalar(0, 0xff, 0), 2);
            line(frame, Point(det->p[0][0], det->p[0][1]),
                     Point(det->p[3][0], det->p[3][1]),
                     Scalar(0, 0, 0xff), 2);
            line(frame, Point(det->p[1][0], det->p[1][1]),
                     Point(det->p[2][0], det->p[2][1]),
                     Scalar(0xff, 0, 0), 2);
            line(frame, Point(det->p[2][0], det->p[2][1]),
                     Point(det->p[3][0], det->p[3][1]),
                     Scalar(0xff, 0, 0), 2);

            /*bool in_bound = checkInBoundaries(det);
            if(!in_bound)
            {
                continue;
            }*/
            stringstream ss;
            ss << det->id;
            String text = ss.str();
            int fontface = FONT_HERSHEY_SCRIPT_SIMPLEX;
            double fontscale = 1.0;
            int baseline;
            Size textsize = getTextSize(text, fontface, fontscale, 2,
                                            &baseline);
            putText(frame, text, Point(det->c[0]-textsize.width/2,
                                       det->c[1]+textsize.height/2),
                                        fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

            j["id"] = det->id;
            marker.x = x;
            marker.y = y;
            marker.z = z;
            marker.roll = roll;
            marker.pitch = pitch;
            marker.yaw = yaw;
            marker.inView = 1;

            marker.to_json(j);
            tmp_string = j.dump();
            out_cstring = tmp_string.c_str();
            aprilcodePublisher->send("MARKER", out_cstring, strlen(out_cstring) );

            pastMarker.x = x;
            pastMarker.y = y;
            pastMarker.z = z;
            pastMarker.roll = roll;
            pastMarker.pitch = pitch;
            pastMarker.yaw = yaw;
            pastMarker.inView = 1;
            pastMarker.to_json(j);

        }

        imshow("immagine" , frame);
        waitKey(10);
        apriltag_detections_destroy(detections);

         if (ctrl_c_pressed){

                std::cout << std::endl << "---------------------------------------------------------" << std::endl;
                std::cout <<              "-                   STOP REQUESTED                      -";
                std::cout << std::endl << "---------------------------------------------------------" << std::endl <<std::endl;

                break;
        }

        //float yaw   = atan2(pose.R->data[1], pose.R->data[0]  );//  atan2(R(2,1),R(1,1));
        //float pitch = atan2( pose.R->data[2]  , sqrt( pow(pose.R->data[5],2) + pow(pose.R->data[8],2)) );  //atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2)));
        //float roll  = atan2(pose.R->data[5] , pose.R->data[8] ); //atan2(R(3,2),R(3,3));

            //std::this_thread::sleep_for(std::chrono::milliseconds(2000));
       //    }
    }

    apriltag_detector_destroy(td);

    if (!strcmp(famname, "tag36h11")) {
        tag36h11_destroy(tf);
    } else if (!strcmp(famname, "tag25h9")) {
        tag25h9_destroy(tf);
    } else if (!strcmp(famname, "tag16h5")) {
        tag16h5_destroy(tf);
    } else if (!strcmp(famname, "tagCircle21h7")) {
        tagCircle21h7_destroy(tf);
    } else if (!strcmp(famname, "tagCircle49h12")) {
        tagCircle49h12_destroy(tf);
    } else if (!strcmp(famname, "tagStandard41h12")) {
        tagStandard41h12_destroy(tf);
    } else if (!strcmp(famname, "tagStandard52h13")) {
        tagStandard52h13_destroy(tf);
    } else if (!strcmp(famname, "tagCustom48h12")) {
        tagCustom48h12_destroy(tf);
    }

    getopt_destroy(getopt);

    return 0;
}


/*
 * DivRightHandSide.cc
 *
 *  Created on: 29.06.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/types.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/fe/fe.h>
#include <deal.II/fe/fe_update_flags.h>
#include <deal.II/fe/fe_values.h>

#include <base/DiscretizedFunction.h>
#include <forward/DivRightHandSide.h>

#include <functional>
#include <vector>

namespace wavepi {
namespace forward {
using namespace dealii;
using namespace wavepi::base;

template<int dim>
DivRightHandSide<dim>::DivRightHandSide(std::shared_ptr<DiscretizedFunction<dim>> a,
      std::shared_ptr<DiscretizedFunction<dim>> b, std::shared_ptr<DiscretizedFunction<dim>> u)
      : a(a), b(b), u(u) {
}

template<int dim>
DivRightHandSide<dim>::AssemblyScratchData::AssemblyScratchData(const FiniteElement<dim> &fe,
      const Quadrature<dim> &quad)
      : fe_values(fe, quad, update_values | update_gradients | update_quadrature_points | update_JxW_values) {
}

template<int dim>
DivRightHandSide<dim>::AssemblyScratchData::AssemblyScratchData(const AssemblyScratchData &scratch_data)
      :
            fe_values(scratch_data.fe_values.get_fe(), scratch_data.fe_values.get_quadrature(),
                  update_values | update_gradients | update_quadrature_points | update_JxW_values) {
}

template<int dim>
void DivRightHandSide<dim>::copy_local_to_global(Vector<double> &result, const AssemblyCopyData &copy_data) {
   for (unsigned int i = 0; i < copy_data.local_dof_indices.size(); ++i)
      result(copy_data.local_dof_indices[i]) += copy_data.cell_rhs(i);
}

template<int dim>
void DivRightHandSide<dim>::local_assemble(const Vector<double> &a, const Vector<double> &b, const Vector<double> &u,
      const typename DoFHandler<dim>::active_cell_iterator &cell, AssemblyScratchData &scratch_data,
      AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_rhs.reinit(dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
      for (unsigned int i = 0; i < dofs_per_cell; ++i)
         for (unsigned int ka = 0; ka < dofs_per_cell; ++ka) {
            for (unsigned int ku = 0; ku < dofs_per_cell; ++ku)
               copy_data.cell_rhs(i) -= a[copy_data.local_dof_indices[ka]]
                     * scratch_data.fe_values.shape_value(ka, q_point) * u[copy_data.local_dof_indices[ku]]
                     * scratch_data.fe_values.shape_grad(ku, q_point) * scratch_data.fe_values.shape_grad(i, q_point)
                     * scratch_data.fe_values.JxW(q_point);

            copy_data.cell_rhs(i) += b[copy_data.local_dof_indices[ka]]
                  * scratch_data.fe_values.shape_value(ka, q_point) * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.JxW(q_point);
         }
}

template<int dim>
void DivRightHandSide<dim>::create_right_hand_side(const DoFHandler<dim> &dof, const Quadrature<dim> &quad,
      Vector<double> &rhs) const {
   AssertThrow(a != nullptr, ExcZero());
   AssertThrow(b != nullptr, ExcZero());
   AssertThrow(u != nullptr, ExcZero());

   const Vector<double> &ca = a->get_function_coefficients_by_time(this->get_time());
   const Vector<double> &cb = b->get_function_coefficients_by_time(this->get_time());
   const Vector<double> &cu = u->get_function_coefficients_by_time(this->get_time());

   Assert(ca.size() == dof.n_dofs(), ExcDimensionMismatch(ca.size(), dof.n_dofs()));
   Assert(cb.size() == dof.n_dofs(), ExcDimensionMismatch(cb.size(), dof.n_dofs()));
   Assert(cu.size() == dof.n_dofs(), ExcDimensionMismatch(cu.size(), dof.n_dofs()));

   WorkStream::run(dof.begin_active(), dof.end(),
         std::bind(&DivRightHandSide<dim>::local_assemble, *this, ca, cb, cu, std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&DivRightHandSide<dim>::copy_local_to_global, *this, std::ref(rhs), std::placeholders::_1),
         AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
}

template class DivRightHandSide<1> ;
template class DivRightHandSide<2> ;
template class DivRightHandSide<3> ;

} /* namespace forward */
} /* namespace wavepi */


/** @author Ryodo Tanaka <groadpg@gmail.com> */
#include <iostream>
#include <Eigen/Core>

// Collision, Distance
#include <fcl/narrowphase/collision_object.h>
#include <fcl/narrowphase/distance.h>
// Distance Request & Result
#include <fcl/narrowphase/distance_request.h>
#include <fcl/narrowphase/distance_result.h>

// If you wonder the fcl::FCL_REAL, fcl::Transform3, and etc...//
// you should see <fcl/common/types.h>
// And you will understand it is equal to Eigen functions

Eigen::Matrix3d setRPY(const Eigen::Vector3d rot)
{
    Eigen::Matrix3d ret;
    ret = Eigen::AngleAxisd(rot.x(), Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(rot.y(), Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(rot.z(), Eigen::Vector3d::UnitZ());
    return ret;
}

int main(int argc, char* argv[]) {
    // Box
    std::shared_ptr<fcl::CollisionGeometry<double>> box_geometry1(new fcl::Box<double>(10.,2.,2.));
    // Sphere
    std::shared_ptr<fcl::CollisionGeometry<double>> box_geometry2(new fcl::Box<double>(10.,2.,2.));

    fcl::CollisionObject<double> box1(box_geometry1);
    fcl::CollisionObject<double> box2(box_geometry2);
    fcl::Vector3d trans1(0.,0.,0.);
    fcl::Vector3d rot1(0.,0.,0.);
    fcl::Vector3d trans2(0.,3.,0.);
    fcl::Vector3d rot2(0.,0.,0.);
    box1.setTranslation(trans1);
    box1.setRotation(setRPY(rot1));
    box2.setTranslation(trans2);
    box2.setRotation(setRPY(rot2));

    // Distance Request and Result
    fcl::DistanceRequest<double> request;
    fcl::DistanceResult<double> result;
    request.enable_nearest_points = true;
    request.enable_signed_distance = true;
    // request.gjk_solver_type = fcl::GJKSolverType::GST_INDEP;
    request.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD;

    // Calculate distance
    result.clear();
    double dist = fcl::distance(&box1, &box2, request, result);

    // Show results
    std::cout << dist << std::endl;
    std::cout << result.min_distance << std::endl;
    std::cout << result.nearest_points[0].transpose() << std::endl;
    std::cout << result.nearest_points[1].transpose() << std::endl;

    return 0;
}


//  Copyright 2015, 2017 Peter Dimov.
//
// 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 <boost/mp11/algorithm.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>
#include <tuple>
#include <utility>

struct X1 {};
struct X2 {};
struct X3 {};
struct X4 {};
struct X5 {};

int main()
{
    using boost::mp11::mp_list;
    using boost::mp11::mp_replace_at;
    using boost::mp11::mp_int;
    using boost::mp11::mp_true;
    using boost::mp11::mp_false;

    {
        using L = mp_list<X1, X2, X3, X4, X5>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<0>, void>, mp_list<void, X2, X3, X4, X5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<1>, void>, mp_list<X1, void, X3, X4, X5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<2>, void>, mp_list<X1, X2, void, X4, X5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<3>, void>, mp_list<X1, X2, X3, void, X5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<4>, void>, mp_list<X1, X2, X3, X4, void>>));
    }

    {
        using L = std::tuple<X1, X2, X3, X4, X5>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<0>, void>, std::tuple<void, X2, X3, X4, X5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<1>, void>, std::tuple<X1, void, X3, X4, X5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<2>, void>, std::tuple<X1, X2, void, X4, X5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<3>, void>, std::tuple<X1, X2, X3, void, X5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_int<4>, void>, std::tuple<X1, X2, X3, X4, void>>));
    }

    {
        using L = std::pair<X1, X2>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_false, void>, std::pair<void, X2>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_replace_at<L, mp_true, void>, std::pair<X1, void>>));
    }

    return boost::report_errors();
}


/*===========================================================================*\

author: Matthias W. Smith
email:  mwsmith2@uw.edu
date:   2015/01/09
file:   single_fid_analyzer.cxx

notes: This program extracts the frequency of a single fid file and
      produces plots of the Fid, envelope function and fits. The analysis
      is extensive testing all available methods.

usage:

./single_fid_analyzer <fid_data> [<output_dir>]

The parameters in brackets are optional.  The default output is
data/ and the directory will be created if not present.

\*===========================================================================*/

//--- std includes ----------------------------------------------------------//
#include <fstream>
#include <iostream>

//--- other includes --------------------------------------------------------//
#include <boost/filesystem.hpp>

//--- project includes ------------------------------------------------------//
#include "fid.h"

using std::string;
using std::cout;
using std::endl;
using namespace fid;

int main(int argc, char **argv)
{
  string datafile;
  string outdir;
  string outfile;
  string figdir;

  std::ofstream out;
  out.precision(10);

  // Make sure a data file was specified and get the file handle.
  if (argc < 2) {
    cout << "Insufficient arguments: input data must be specified." << endl;
    cout << "Usage: ./single_fid_analyzer <input-file> [output-dir]" << endl;
    exit(1);
  }

  // Load the data.
  datafile = string(argv[1]);

  // Now check for an optional output directory.
  if (argc > 2){

    outdir = string(argv[2]);

  } else {

    outdir = string("data/");
  }

  // Create an output file handle.
  outfile = outdir + string("single_fid_data.csv");

  // Make a string to hold the location of the figure output directory.
  figdir = outdir + string("fig/");

  // Make certain that the directory exists by creating it.
  boost::filesystem::path dir(figdir);
  boost::filesystem::create_directories(dir);

  // Read the Fid data and create Fid object.
  Fid my_fid(datafile);

  // Open the output filestream.
  out.open(outfile);
  my_fid.WriteFreqCsv(out);

  // Make plots of the fits and residuals.
  string title("Fid Fit");
  my_fid.GetFreq("analytical");
  my_fid.SaveFreqFit(dir.string() + string("analytical_fit.png"), title);
  my_fid.SaveFreqRes(dir.string() + string("analytical_res.png"), title);

  my_fid.GetFreq("lorentzian");
  my_fid.SaveFreqFit(dir.string() + string("lorentzian_fit.png"), title);
  my_fid.SaveFreqRes(dir.string() + string("lorentzian_res.png"), title);

  my_fid.GetFreq("exponential");
  my_fid.SaveFreqFit(dir.string() + string("exponential_fit.png"), title);
  my_fid.SaveFreqRes(dir.string() + string("exponential_res.png"), title);

  my_fid.GetFreq("phase");
  my_fid.SaveTimeFit(dir.string() + string("lin_phase_fit.png"), title);
  my_fid.SaveTimeRes(dir.string() + string("lin_phase_res.png"), title);

  my_fid.GetFreq("sinusoid");
  my_fid.SaveTimeFit(dir.string() + string("sinusoid_fit.png"), title);
  my_fid.SaveTimeRes(dir.string() + string("sinusoid_res.png"), title);

  // Close the output filestream
  out.close();

  return 0;
}


#include <frovedis.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

int sum(std::string key, int a, int b) {return a + b;}

int optional_sum(std::string key, int a, boost::optional<int> b) {
  if(b) return a + *b;
  else return a;
}

BOOST_AUTO_TEST_CASE( frovedis_test )
{
  int argc = 1;
  char** argv = NULL;
  use_frovedis use(argc, argv);

  // filling sample input imap
  auto d1 = frovedis::make_dunordered_map_allocate<std::string,int>();
  auto d2 = d1;
  d1.put("apple", 10);
  d1.put("orange", 20);
  d2.put("apple", 50);
  d2.put("grape", 20);
  auto z = zip(d1,d2);
  auto d3 = z.map_values(sum);
  auto r = d3.as_dvector().sort().gather();
  //for(auto i: r) std::cout << i.first << ": " << i.second << std::endl;
  std::vector<std::pair<std::string,int>> ref1;
  ref1.push_back(std::make_pair("apple",60));
  BOOST_CHECK (r == ref1);

  auto d4 = z.leftouter_map_values(optional_sum);
  r = d4.as_dvector().sort().gather();
  //for(auto i: r) std::cout << i.first << ": " << i.second << std::endl;
  std::vector<std::pair<std::string,int>> ref2;
  ref2.push_back(std::make_pair("apple",60));
  ref2.push_back(std::make_pair("orange",20));
  BOOST_CHECK (r == ref2);
}


#ifndef _GRIDMAPGRAPHCONVERTER_HEADER__
#define _GRIDMAPGRAPHCONVERTER_HEADER__

#include <boost/smart_ptr/make_unique.hpp>
#include <boost/range/adaptor/map.hpp>

#include <cpp-utils/igraph.hpp>
#include <cpp-utils/listGraph.hpp>
#include <cpp-utils/exceptions.hpp>

#include "xyLoc.hpp"
#include "IMapGraphConverter.hpp"
#include "GridBranching.hpp"
#include "GridMap.hpp"

namespace pathfinding::maps {

/**
 * @brief compute the graph underlying a grid map (4 connected)
 *
 * The costs of each edge is obtained by averagin the endpoints of the involved edge
 *
 */
class GridMapGraphConverter: public IMapGraphConverter<GridMap, cpp_utils::graphs::IImmutableGraph<std::string, xyLoc, cost_t>> {
private:
    GridBranching branching;
public:
    GridMapGraphConverter(GridBranching branching);
    virtual ~GridMapGraphConverter();
    GridMapGraphConverter(const GridMapGraphConverter& o);
    GridMapGraphConverter(GridMapGraphConverter&& o);
    GridMapGraphConverter& operator = (const GridMapGraphConverter& o);
    GridMapGraphConverter& operator = (GridMapGraphConverter&& o);
public:
    cost_t computeCost(Direction dir, cost_t sourceCost, cost_t sinkCost) const;
    virtual std::unique_ptr<cpp_utils::graphs::IImmutableGraph<std::string,xyLoc,cost_t>> toGraph(const GridMap& map) const;
    /**
     * @brief convert the grid map into a JSON which can be read by some other tools
     *
     * the json generated is divided into 2 parts: vertices and edges.
     * vertices is a dictionary containing for each pair of x-y, the associated node id;
     * edges is a array containing all the edges, expressed as a 3 value array containing sourceid, sinkid and label.
     *
     * @param map the grid map to convert to json
     * @return std::string
     */
    template <typename G, typename V, typename E>
    std::string toJson(const cpp_utils::graphs::IImmutableGraph<G, V, E>& map) const {

        std::stringstream ss;
        ss << "{" << std::endl;

        ss << "\t\"vertices\": {" << std::endl;
        int vertexNumber = map.numberOfVertices();
        int current = 0;
        for (auto it=map.beginVertices(); it!=map.endVertices(); ++it) {
            ss << "\t\t{ \"" << it->first << "\": " << "\"" << it->second.x << ", " << it->second.y << "\"}";
            if ((current+1) < vertexNumber) {
                //we need to add a "," except for the last entry
                ss << ",";
            }
            ss << std::endl;
            current += 1;
        }
        ss << "\t}," << std::endl;
        ss << "\t\"edges\": [" << std::endl;

        int edgeNumber = map.numberOfEdges();
        current = 0;
        for (auto it=map.beginEdges(); it!=map.endEdges(); ++it) {
            ss << "\t\t{ \"source\": " << it->getSourceId() << ", \"sink\": " << it->getSinkId() << ", \"label\": " << it->getPayload() << "}";
            if ((current+1) < edgeNumber) {
                ss << ",";
            }
            ss << std::endl;
            current += 1;
        }
        ss << "\t]" << std::endl;

        ss << "}";

        return ss.str();
    }
public:
    virtual void cleanup();
};

}

#endif


//
// Copyright (c) 2015-2018 CNRS
//

#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"

#include <boost/python/overloads.hpp>

namespace pinocchio
{
  namespace python
  {

    BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMassUpdate_overload,jacobianCenterOfMass,3,4)

    BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMassNoUpdate_overload,jacobianCenterOfMass,2,3)

    static SE3::Vector3
    com_0_proxy(const Model& model,
                Data & data,
                const Eigen::VectorXd & q,
                bool computeSubtreeComs = true)
    {
      return centerOfMass(model,data,q,computeSubtreeComs);
    }

    static SE3::Vector3
    com_1_proxy(const Model& model,
                Data & data,
                const Eigen::VectorXd & q,
                const Eigen::VectorXd & v,
                bool computeSubtreeComs = true)
    {
      return centerOfMass(model,data,q,v,computeSubtreeComs);
    }

    static SE3::Vector3
    com_2_proxy(const Model & model,
                Data & data,
                const Eigen::VectorXd & q,
                const Eigen::VectorXd & v,
                const Eigen::VectorXd & a,
                bool computeSubtreeComs = true)
    {
      return centerOfMass(model,data,q,v,a,computeSubtreeComs);
    }

    static void
    com_level_proxy(const Model & model,
                    Data & data,
                    int LEVEL,
                    bool computeSubtreeComs = true)
    {
      centerOfMass(model,data,LEVEL,computeSubtreeComs);
    }

    static void
    com_default_proxy(const Model & model,
                Data & data,
                bool computeSubtreeComs = true)
    {
      centerOfMass(model,data,computeSubtreeComs);
    }

    static Data::Matrix3x
    jacobian_subtree_com_kinematics_proxy(const Model & model,
                                          Data & data,
                                          const Eigen::VectorXd & q,
                                          Model::JointIndex jointId)
    {
      Data::Matrix3x J(3,model.nv); J.setZero();
      jacobianSubtreeCenterOfMass(model, data, q, jointId, J);

      return J;
    }

    static Data::Matrix3x
    jacobian_subtree_com_proxy(const Model & model,
                               Data & data,
                               Model::JointIndex jointId)
    {
      Data::Matrix3x J(3,model.nv); J.setZero();
      jacobianSubtreeCenterOfMass(model, data, jointId, J);

      return J;
    }

    static Data::Matrix3x
    get_jacobian_subtree_com_proxy(const Model & model,
                                   Data & data,
                                   Model::JointIndex jointId)
    {
      Data::Matrix3x J(3,model.nv); J.setZero();
      getJacobianSubtreeCenterOfMass(model, data, jointId, J);

      return J;
    }

    BOOST_PYTHON_FUNCTION_OVERLOADS(com_0_overload, com_0_proxy, 3, 4)

    BOOST_PYTHON_FUNCTION_OVERLOADS(com_1_overload, com_1_proxy, 4, 5)

    BOOST_PYTHON_FUNCTION_OVERLOADS(com_2_overload, com_2_proxy, 5, 6)

    BOOST_PYTHON_FUNCTION_OVERLOADS(com_level_overload, com_level_proxy, 3, 4)

    BOOST_PYTHON_FUNCTION_OVERLOADS(com_default_overload, com_default_proxy, 2, 3)

    void exposeCOM()
    {
      using namespace Eigen;

      bp::def("computeTotalMass",
              (double (*)(const Model &))&computeTotalMass<double,0,JointCollectionDefaultTpl>,
              bp::args("Model"),
              "Compute the total mass of the model and return it.");

      bp::def("computeTotalMass",
              (double (*)(const Model &, Data &))&computeTotalMass<double,0,JointCollectionDefaultTpl>,
              bp::args("Model", "Data"),
              "Compute the total mass of the model, put it in data.mass[0] and return it.");

      bp::def("computeSubtreeMasses",
              (void (*)(const Model &, Data &))&computeSubtreeMasses<double,0,JointCollectionDefaultTpl>,
              bp::args("Model", "Data"),
              "Compute the mass of each kinematic subtree and store it in data.mass.");

      bp::def("centerOfMass",
              com_0_proxy,
              com_0_overload(
                  bp::args("Model","Data",
                           "Joint configuration q (size Model::nq)",
                           "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"
                  ),
                  "Compute the center of mass, putting the result in Data and return it."
              )[bp::return_value_policy<bp::return_by_value>()]
      );

      bp::def("centerOfMass",
              com_1_proxy,
              com_1_overload(
                  bp::args("Model","Data",
                           "Joint configuration q (size Model::nq)",
                           "Joint velocity v (size Model::nv)",
                           "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"
                  ),
                  "Computes the center of mass position and velocity by storing the result in Data."
              )[bp::return_value_policy<bp::return_by_value>()]
      );

      bp::def("centerOfMass",
              com_2_proxy,
              com_2_overload(
                  bp::args("Model","Data",
                           "Joint configuration q (size Model::nq)",
                           "Joint velocity v (size Model::nv)",
                           "Joint acceleration a (size Model::nv)",
                           "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"
                  ),
                  "Computes the center of mass position, velocity and acceleration by storing the result in Data."
              )[bp::return_value_policy<bp::return_by_value>()]
      );

      bp::def("centerOfMass",
              com_level_proxy,
              com_level_overload(
                  bp::args("Model","Data",
                           "level if = 0, computes CoM position, if = 1, also computes CoM velocity and if = 2, also computes CoM acceleration",
                           "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"
                  ),
                  "Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data and the requested level."
              )[bp::return_value_policy<bp::return_by_value>()]
      );

      bp::def("centerOfMass",
              com_default_proxy,
              com_default_overload(
                  bp::args("Model","Data",
                           "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"
                  ),
                  "Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data."
              )[bp::return_value_policy<bp::return_by_value>()]
      );

      bp::def("jacobianCenterOfMass",
              (const Data::Matrix3x & (*)(const Model &, Data &, const Eigen::MatrixBase<Eigen::VectorXd> &, bool))&jacobianCenterOfMass<double,0,JointCollectionDefaultTpl,VectorXd>,
              jacobianCenterOfMassUpdate_overload(bp::args("Model","Data",
                       "Joint configuration q (size Model::nq)",
                       "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"),
              "Computes the jacobian of the center of mass, puts the result in Data and return it.")[
              bp::return_value_policy<bp::return_by_value>()]);

      bp::def("jacobianCenterOfMass",
              (const Data::Matrix3x & (*)(const Model &, Data &, bool))&jacobianCenterOfMass<double,0,JointCollectionDefaultTpl>,
              jacobianCenterOfMassNoUpdate_overload(bp::args("Model","Data",
                       "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"),
              "Computes the jacobian of the center of mass, puts the result in Data and return it.")[
              bp::return_value_policy<bp::return_by_value>()]);

      bp::def("jacobianSubtreeCoMJacobian",jacobian_subtree_com_kinematics_proxy,
              bp::args("Model, the model of the kinematic tree",
                       "Data, the data associated to the model where the results are stored",
                       "Joint configuration q (size Model::nq)",
                       "Subtree root ID, the index of the subtree root joint."),
              "Computes the Jacobian of the CoM of the given subtree expressed in the world frame, according to the given joint configuration.");

      bp::def("jacobianSubtreeCoMJacobian",jacobian_subtree_com_proxy,
              bp::args("Model, the model of the kinematic tree",
                       "Data, the data associated to the model where the results are stored",
                       "Subtree root ID, the index of the subtree root joint."),
              "Computes the Jacobian of the CoM of the given subtree expressed in the world frame, according to the given entries in data.");

      bp::def("getJacobianSubtreeCenterOfMass",get_jacobian_subtree_com_proxy,
              bp::args("Model, the model of the kinematic tree",
                       "Data, the data associated to the model where the results are stored",
                       "Subtree root ID, the index of the subtree root joint."),
              "Get the Jacobian of the CoM of the given subtree expressed in the world frame, according to the given entries in data. It assumes that jacobianCenterOfMass has been called first.");

    }

  } // namespace python
} // namespace pinocchio


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

/**
 * @file cardinality.hpp
 *
 * @brief SAT cardinality constraints
 *
 * @author Mathias Soeken
 * @author Heinz Riener
 *
 * @since  2.2
 */

#ifndef CARDINALITY_HPP
#define CARDINALITY_HPP

#include <numeric>

#include <boost/assign/std/vector.hpp>
#include <boost/range.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>

#include <core/utils/range_utils.hpp>
#include <classical/sat/sat_solver.hpp>
#include <classical/sat/operations/logic.hpp>

using namespace boost::assign;

/**
 * Carsten Sinz's CNF encoding of Boolean cardinality constraints [1].
 * The implementation is mainly based on the Knuth's descriptions [2].
 *
 * [1] Carsten Sinz. Towards an optimal CNF encoding of boolean
 * cardinality constraints, CP, pages 827-831, 2005.
 *
 * [2] Donald E. Knuth, The Art of Computer Programming, Volume 4,
 * Pre-Fascicle 6A, page 8ff.
 */

namespace cirkit
{

template<class S, class C>
void atmost_one( S& solver, const C& x )
{
  for ( auto i = boost::begin( x ); i != boost::end( x ); ++i )
  {
    if ( i + 1 == boost::end( x ) ) continue;
    for ( auto j = i + 1; j != boost::end( x ); ++j )
    {
      add_clause( solver )( {-*i, -*j} );
    }
  }
}

template<class S, class C>
inline void one_hot( S& solver, const C& x )
{
  add_clause( solver )( x );
  atmost_one( solver, x );
}

template < class S >
int less_or_equal_sinz( S& solver, const clause_t& x, unsigned r, int sid, int spol = 1 )
{
  const unsigned n = x.size();

  //std::cerr << "n = " << n << " r = " << r << " sid = " << sid << '\n';
  assert( n > r );
  for ( auto j = 0u; j < n - r - 1; ++j )
  {
    for ( auto k = 0u; k < r; ++k )
    {
      // std::cerr << "(" << j << "," << k << ")" << '\n';
      //const int l1 = spol * -(sid + j * (n - r) + k);
      //const int l2 = spol * (sid + (j + 1u) * (n - r) + k);
      const int l1 = spol * -( sid + j * r + k );
      const int l2 = spol * (sid + ( j + 1u ) * r + k );
      add_clause( solver )( {l1, l2} );
    }
  }

  for ( auto j = 0u; j < (n - r); ++j )
  {
    for ( auto k = 0u; k <= r; ++k )
    {
      // std::cerr << "(" << j << "," << k << ")" << '\n';
      clause_t clause;
      clause += -x[ j + k ];
      //if ( k > 0u ) clause += spol * -(sid + j * ( n - r ) + k - 1u );
      //if ( k < r ) clause += spol * (sid + j * ( n - r ) + k );
      if ( k > 0u ) clause += spol * -(sid + j * r + k - 1u );
      if ( k < r ) clause += spol * (sid + j * r + k );
      add_clause( solver )( clause );
    }
  }

  sid += (n - r)*r/* + 1u*/;
  //std::cerr << "next sid: " << sid << '\n';
  return sid;
}

template < class S >
int greater_or_equal_sinz( S& solver, const clause_t& x, unsigned r, int sid, int spol = 1 )
{
  using boost::adaptors::transformed;

  clause_t xn;
  boost::push_back( xn, x | transformed( []( int v ) { return -v; } ) );
  return less_or_equal_sinz( solver, xn, x.size() - r, sid, spol );
}

template < class S >
int equals_sinz( S& solver, const clause_t& x, unsigned r, int sid )
{
  //std::cout << "call equals_size with x.size = " << x.size() << ", r = " << r << ", and sid = " << sid << std::endl;

  if ( r == 0u )
  {
    for ( const auto& l : x )
    {
      add_clause( solver )( {-l} );
    }
    return sid;
  }

  if ( r == x.size() )
  {
    for ( const auto& l : x )
    {
      add_clause( solver )( {l} );
    }
    return sid;
  }

  sid = less_or_equal_sinz( solver, x, r, sid, 1 );
  return greater_or_equal_sinz( solver, x, r, sid, -1 );
}

/******************************************************************************
 * Binary tree encoding from Bailleux and Boufkhad                            *
 ******************************************************************************/
template<class S>
int less_or_equal_bailleux_boufkhad_assume( S& solver, int assume, const clause_t& x, unsigned r, int sid )
{
  const auto n = x.size();                    /* number of variables */
  std::vector<unsigned> ts_count( 2 * n, 1 ); /* pre-assign t's with 1 (for the leaves), we don't use index 0 */
  std::vector<int> ts( 2 * n, 0 );
  std::vector<int> pol( 2 * n, 1 );

  auto offset = sid;
  for ( auto k = n - 1; k > 1; --k )
  {
    ts_count[k] = std::min( ts_count[k << 1] + ts_count[( k << 1 ) + 1], r );
    ts[k] = offset;
    offset += ts_count[k];
  }
  for ( auto k = n; k < 2 * n; ++k )
  {
    ts[k] = abs( x[k - n] );
    pol[k] = x[k - n] > 0 ? 1 : -1;
  }

  for ( auto k = 2u; k < n; ++k )
  {
    for ( auto i = 0u; i <= ts_count[k << 1]; ++i )
    {
      for ( auto j = 0u; j <= ts_count[( k << 1 ) + 1]; ++j )
      {
        if ( ( i + j < 1 ) || ( i + j > ts_count[k] + 1 ) ) { continue; }

        std::vector<int> clause = { -assume };
        if ( i > 0 ) { clause.push_back( -pol[k << 1] * ( ts[k << 1] + i - 1 ) ); }
        if ( j > 0 ) { clause.push_back( -pol[( k << 1 ) + 1] * ( ts[( k << 1 ) + 1] + j - 1 ) ); }
        if ( i + j <= r ) { clause.push_back( pol[k] * ( ts[k] + i + j - 1 ) ); }

        if ( !clause.empty() )
        {
          add_clause( solver )( clause );
        }
      }
    }
  }

  for ( auto i = 0; i <= static_cast<int>( ts_count[2] ); ++i )
  {
    for ( auto j = 0; j <= static_cast<int>( ts_count[3] ); ++j )
    {
      if ( i + j == static_cast<int>( r ) + 1 )
      {
        assert( i && j );
        add_clause( solver )( { -assume, -pol[2] * ( ts[2] + i - 1 ), -pol[3] * ( ts[3] + j - 1 )} );
      }
    }
  }

  return offset;
}

template<class S>
int less_or_equal_bailleux_boufkhad( S& solver, const clause_t& x, unsigned r, int sid )
{
  const auto n = x.size();                    /* number of variables */
  std::vector<unsigned> ts_count( 2 * n, 1 ); /* pre-assign t's with 1 (for the leaves), we don't use index 0 */
  std::vector<int> ts( 2 * n, 0 );
  std::vector<int> pol( 2 * n, 1 );

  auto offset = sid;
  for ( int k = n - 1; k > 1; --k )
  {
    ts_count[k] = std::min( ts_count[k << 1] + ts_count[( k << 1 ) + 1], r );
    ts[k] = offset;
    offset += ts_count[k];
  }
  for ( auto k = n; k < 2 * n; ++k )
  {
    ts[k] = abs( x[k - n] );
    pol[k] = x[k - n] > 0 ? 1 : -1;
  }

  for ( auto k = 2u; k < n; ++k )
  {
    for ( auto i = 0u; i <= ts_count[k << 1]; ++i )
    {
      for ( auto j = 0u; j <= ts_count[( k << 1 ) + 1]; ++j )
      {
        if ( ( i + j < 1 ) || ( i + j > ts_count[k] + 1 ) ) { continue; }

        std::vector<int> clause;
        if ( i > 0 ) { clause.push_back( -pol[k << 1] * ( ts[k << 1] + i - 1 ) ); }
        if ( j > 0 ) { clause.push_back( -pol[( k << 1 ) + 1] * ( ts[( k << 1 ) + 1] + j - 1 ) ); }
        if ( i + j <= r ) { clause.push_back( pol[k] * ( ts[k] + i + j - 1 ) ); }

        if ( !clause.empty() )
        {
          add_clause( solver )( clause );
        }
      }
    }
  }

  for ( auto i = 0; i <= static_cast<int>( ts_count[2] ); ++i )
  {
    for ( auto j = 0; j <= static_cast<int>( ts_count[3] ); ++j )
    {
      if ( i + j == static_cast<int>( r ) + 1 )
      {
        assert( i && j );
        add_clause( solver )( {-pol[2] * ( ts[2] + i - 1 ), -pol[3] * ( ts[3] + j - 1 )} );
      }
    }
  }

  return offset;
}

template<class S>
int greater_or_equal_bailleux_boufkhad( S& solver, const clause_t& x, unsigned r, int sid )
{
  using boost::adaptors::transformed;

  clause_t xn;
  boost::push_back( xn, x | transformed( []( int v ) { return -v; } ) );
  return less_or_equal_bailleux_boufkhad( solver, xn, x.size() - r, sid );
}

template < class S >
int equals_bailleux_boufkhad( S& solver, const clause_t& x, unsigned r, int sid )
{
  if ( r == 0u )
  {
    for ( const auto& l : x )
    {
      add_clause( solver )( {-l} );
    }
    return sid;
  }

  if ( r == x.size() )
  {
    for ( const auto& l : x )
    {
      add_clause( solver )( {l} );
    }
    return sid;
  }

  sid = less_or_equal_bailleux_boufkhad( solver, x, r, sid );
  return greater_or_equal_bailleux_boufkhad( solver, x, r, sid );
}

/******************************************************************************
 * Pairwise cardinality networks                                              *
 ******************************************************************************/

// Based on [M. Codish and M. Zazon-Ivry, LPAR, 2016, 154-172] and https://bitbucket.org/alanmi/abc/src/f78c2854aa5948fea93533c3934fb0f6a2c3c785/src/aig/gia/giaSatMap.c?at=default&fileviewer=file-view-default

// namespace detail
// {

// template<class S>
// void sat_add_half_sorter( S& solver, int a, int b, int y, int z )
// {
//   add_clause( solver )( {a, -y} );
//   add_clause( solver )( {a, -z} );
//   add_clause( solver )( {b, -y, -z} );
//   // add_clause( solver )( {-a, y} );
//   // add_clause( solver )( {-a, z} );
//   // add_clause( solver )( {-b, y, z} );
// }

// template<class S>
// void sat_pairwise_cardinality_network_add_sorter( S& solver, std::vector<int>& vvars, int i, int k, int *pnvars )
// {
//   const auto ivar1 = (*pnvars)++;
//   const auto ivar2 = (*pnvars)++;

//   sat_add_half_sorter( solver, ivar1, ivar2, vvars[i], vvars[k] );
//   vvars[i] = ivar1;
//   vvars[k] = ivar2;
// }

// template<class S>
// void sat_pairwise_cardinality_network_add_constraint_merge( S& solver, std::vector<int>& vvars, int lo, int hi, int r, int *pnvars )
// {
//   const auto step = r * 2;
//   if ( step >= hi - lo ) return;

//   sat_pairwise_cardinality_network_add_constraint_merge( solver, vvars, lo, hi - r, step, pnvars );
//   sat_pairwise_cardinality_network_add_constraint_merge( solver, vvars, lo + r, hi, step, pnvars );
//   for ( auto i = lo + r; i < hi - r; i += step )
//   {
//     sat_pairwise_cardinality_network_add_sorter( solver, vvars, i, i + r, pnvars );
//   }
// }

// template<class S>
// void sat_pairwise_cardinality_network_add_constraint_range( S& solver, std::vector<int>& vvars, int lo, int hi, int *pnvars )
// {
//   if ( hi - lo < 1 ) return;

//   const auto mid = lo + ( hi - lo ) / 2;
//   for ( auto i = lo; i <= mid; ++i )
//   {
//     sat_pairwise_cardinality_network_add_sorter( solver, vvars, i, i + ( hi - lo + 1 ) / 2, pnvars );
//   }
//   sat_pairwise_cardinality_network_add_constraint_range( solver, vvars, lo, mid, pnvars );
//   sat_pairwise_cardinality_network_add_constraint_range( solver, vvars, mid + 1, hi, pnvars );
//   sat_pairwise_cardinality_network_add_constraint_merge( solver, vvars, lo, hi, 1, pnvars );
// }

// template<class S>
// int sat_pairwise_cardinality_network_add_constraint_pairwise( S& solver, std::vector<int>& vvars )
// {
//   int nvars = vvars.size();
//   sat_pairwise_cardinality_network_add_constraint_range( solver, vvars, 0, nvars - 1, &nvars );
//   return nvars;
// }

// }

// template<class S>
// int sat_pairwise_cardinality_network2( S& solver, unsigned log_n, int sid, std::vector<int>* ovars = nullptr )
// {
//   auto nvars = 1u << log_n;
//   const auto nvars_alloc = nvars + 2u * (nvars * log_n * ( log_n - 1 ) / 4 + nvars - 1);

//   std::vector<int> vvars( nvars );
//   boost::iota( vvars, sid );

//   std::cout << "[i] original vvars: " << any_join( vvars, " " ) << std::endl;

//   const auto nvars_real = detail::sat_pairwise_cardinality_network_add_constraint_pairwise( solver, vvars );
//   assert( nvars_real == nvars_alloc );

//   if ( ovars )
//   {
//     *ovars = vvars;
//   }

//   return sid + nvars_alloc;
// }

/******************************************************************************
 * PW implementation based on paper                                           *
 ******************************************************************************/

namespace detail
{

template<class S>
void sat_pairwise_comparator( S& solver, int a, int b, int c, int d)
{
  //logic_or( solver, a, b, c );
  //logic_and( solver, a, b, d );

  add_clause( solver )( {c, -a} );
  add_clause( solver )( {c, -b} );
  add_clause( solver )( {d, -a, -b} );
}

template<class S>
void sat_pairwise_split( S& solver, const std::vector<int>& as, const std::vector<int>& bs, const std::vector<int>& cs )
{
  assert( bs.size() == cs.size() );
  assert( bs.size() << 1u == as.size() );

  for ( auto i = 0u; i < bs.size(); ++i )
  {
    sat_pairwise_comparator( solver, as[2 * i], as[2 * i + 1], bs[i], cs[i] );
  }
}

template<class S>
int sat_pairwise_merge( S& solver, const std::vector<int>& as, const std::vector<int>& bs, const std::vector<int>& cs, int sid )
{
  const auto n = as.size();

  if ( n == 1u )
  {
    equals( solver, as[0], cs[0] );
    equals( solver, bs[0], cs[1] );
    return sid;
  }

  /* fill ds and es */
  std::vector<int> ds( n ), es( n );

  ds[0u] = cs[0];
  for ( auto i = 1u; i < n; ++i ) { ds[i] = sid++; }

  for ( auto i = 0u; i < n - 1; ++i ) { es[i] = sid++; }
  es[n - 1] = cs[2 * n - 1];

  /* fill a0, b0, a1, b1 */
  std::vector<int> a0( n / 2 ), b0( n / 2 ), a1( n / 2 ), b1( n / 2 );
  for ( auto i = 0u; i < n / 2; ++i )
  {
    a0[i] = as[2 * i];
    a1[i] = as[2 * i + 1];
    b0[i] = bs[2 * i];
    b1[i] = bs[2 * i + 1];
  }

  sid = sat_pairwise_merge( solver, a0, b0, ds, sid );
  sid = sat_pairwise_merge( solver, a1, b1, es, sid );

  for ( auto i = 0u; i < n - 1; ++i )
  {
    sat_pairwise_comparator( solver, es[i], ds[i + 1], cs[2 * i + 1], cs[2 * i + 2] );
  }

  /* sort outputs */
  for ( auto i = 0u; i < cs.size() - 1; ++i )
  {
    add_clause( solver )( {cs[i], -cs[i + 1]} );
  }

  return sid;
}

template<class S>
int sat_pairwise_sort( S& solver, const std::vector<int>& as, const std::vector<int>& ds, int sid )
{
  const auto n = as.size();

  if ( n == 1u )
  {
    equals( solver, as[0], ds[0] );
    return sid;
  }

  std::vector<int> bs( n / 2 ), cs( n / 2 ), bbs( n / 2), ccs( n / 2 );

  boost::iota( bs, sid ); sid += n / 2;
  boost::iota( cs, sid ); sid += n / 2;
  boost::iota( bbs, sid ); sid += n / 2;
  boost::iota( ccs, sid ); sid += n / 2;

  sat_pairwise_split( solver, as, bs, cs );
  sid = sat_pairwise_sort( solver, bs, bbs, sid );
  sid = sat_pairwise_sort( solver, cs, ccs, sid );
  return sat_pairwise_merge( solver, bbs, ccs, ds, sid );
}

}

template<class S>
int sat_pairwise_cardinality_network( S& solver, const std::vector<int>& ivars, int sid, std::vector<int>* ovars = nullptr )
{
  std::vector<int> ds( ivars.size() );
  boost::iota( ds, sid );
  sid += ivars.size();

  if ( ovars )
  {
    *ovars = ds;
  }

  return detail::sat_pairwise_sort( solver, ivars, ds, sid );
}

template<class S>
int sat_pairwise_cardinality_network( S& solver, unsigned log_n, int sid, std::vector<int>* ovars = nullptr )
{
  const auto n = 1 << log_n;
  std::vector<int> as( n );

  boost::iota( as, sid ); sid += n;

  return sat_pairwise_cardinality_network( solver, as, sid, ovars );
}

}

#endif

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers

#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/otherlibs/do_opencv_includes.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/vision/chessboard_camera_calib.h>
#include <mrpt/vision/chessboard_find_corners.h>
#include <mrpt/vision/pinhole.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::config;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace std;

/* -------------------------------------------------------
				checkerBoardCameraCalibration
   ------------------------------------------------------- */
bool mrpt::vision::checkerBoardCameraCalibration(
	TCalibrationImageList& images, unsigned int check_size_x,
	unsigned int check_size_y, double check_squares_length_X_meters,
	double check_squares_length_Y_meters, CMatrixDouble33& intrinsicParams,
	std::vector<double>& distortionParams, bool normalize_image,
	double* out_MSE, bool skipDrawDetectedImgs,
	bool useScaramuzzaAlternativeDetector)
{
	// Just a wrapper for the newer version of the function which uses TCamera:
	TCamera cam;
	bool ret = checkerBoardCameraCalibration(
		images, check_size_x, check_size_y, check_squares_length_X_meters,
		check_squares_length_Y_meters, cam, normalize_image, out_MSE,
		skipDrawDetectedImgs, useScaramuzzaAlternativeDetector);

	intrinsicParams = cam.intrinsicParams;
	distortionParams = cam.getDistortionParamsAsVector();
	return ret;
}

#if MRPT_HAS_OPENCV
#include <opencv2/core/eigen.hpp>
#endif

/* -------------------------------------------------------
				checkerBoardCameraCalibration
   ------------------------------------------------------- */
bool mrpt::vision::checkerBoardCameraCalibration(
	TCalibrationImageList& images, unsigned int check_size_x,
	unsigned int check_size_y, double check_squares_length_X_meters,
	double check_squares_length_Y_meters, mrpt::img::TCamera& out_camera_params,
	bool normalize_image, double* out_MSE, bool skipDrawDetectedImgs,
	bool useScaramuzzaAlternativeDetector)
{
	MRPT_UNUSED_PARAM(skipDrawDetectedImgs);
#if MRPT_HAS_OPENCV
	try
	{
		ASSERT_(check_size_x > 2);
		ASSERT_(check_size_y > 2);
		ASSERT_(check_squares_length_X_meters > 0);
		ASSERT_(check_squares_length_Y_meters > 0);

		if (images.size() < 1)
		{
			std::cout << "ERROR: No input images." << std::endl;
			return false;
		}

		const unsigned CORNERS_COUNT = check_size_x * check_size_y;
		const CvSize check_size = cvSize(check_size_x, check_size_y);

		// Fill the pattern of expected pattern points only once out of the
		// loop:
		vector<cv::Point3f> pattern_obj_points(CORNERS_COUNT);
		{
			unsigned int y, k;
			for (y = 0, k = 0; y < check_size_y; y++)
			{
				for (unsigned int x = 0; x < check_size_x; x++, k++)
				{
					pattern_obj_points[k].x = -check_squares_length_X_meters *
											  x;  // The "-" is for convenience,
					// so the camera poses appear
					// with Z>0
					pattern_obj_points[k].y = check_squares_length_Y_meters * y;
					pattern_obj_points[k].z = 0;
				}
			}
		}

		// First: Assure all images are loaded:
		// -------------------------------------------
		TCalibrationImageList::iterator it;
		for (it = images.begin(); it != images.end(); ++it)
		{
			TImageCalibData& dat = it->second;

			dat.projectedPoints_distorted.clear();  // Clear reprojected points.
			dat.projectedPoints_undistorted.clear();

			// Skip if images are marked as "externalStorage":
			if (!dat.img_original.isExternallyStored() &&
				!mrpt::system::extractFileExtension(it->first).empty())
			{
				if (!dat.img_original.loadFromFile(it->first))
					THROW_EXCEPTION_FMT(
						"Error reading image: %s", it->first.c_str());

				dat.img_checkboard = dat.img_original;
				dat.img_rectified = dat.img_original;
			}
		}

		// For each image, find checkerboard corners:
		// -----------------------------------------------
		vector<vector<cv::Point3f>>
			objectPoints;  // final container for detected stuff
		vector<vector<cv::Point2f>>
			imagePoints;  // final container for detected stuff

		unsigned int valid_detected_imgs = 0;
		vector<string> pointsIdx2imageFile;
		cv::Size imgSize(0, 0);

		unsigned int i;
		for (i = 0, it = images.begin(); it != images.end(); it++, i++)
		{
			TImageCalibData& dat = it->second;

			// Make grayscale version:
			const CImage img_gray(
				dat.img_original, FAST_REF_OR_CONVERT_TO_GRAY);

			if (!i)
			{
				imgSize = cv::Size(img_gray.getWidth(), img_gray.getHeight());
				out_camera_params.ncols = imgSize.width;
				out_camera_params.nrows = imgSize.height;
			}
			else
			{
				if (imgSize.height != (int)img_gray.getHeight() ||
					imgSize.width != (int)img_gray.getWidth())
				{
					std::cout << "ERROR: All the images must have the same size"
							  << std::endl;
					return false;
				}
			}

			// Try with expanded versions of the image if it fails to detect the
			// checkerboard:
			unsigned corners_count;
			bool corners_found = false;

			corners_count = CORNERS_COUNT;

			vector<cv::Point2f> this_img_pts(
				CORNERS_COUNT);  // Temporary buffer for points, to be added if
			// the points pass the checks.

			dat.detected_corners.clear();

			// Do detection (this includes the "refine corners" with
			// cvFindCornerSubPix):
			vector<TPixelCoordf> detectedCoords;
			corners_found = mrpt::vision::findChessboardCorners(
				img_gray, detectedCoords, check_size_x, check_size_y,
				normalize_image,  // normalize_image
				useScaramuzzaAlternativeDetector);

			corners_count = detectedCoords.size();

			// Copy the data into the overall array of coords:
			ASSERT_(detectedCoords.size() <= CORNERS_COUNT);
			for (size_t p = 0; p < detectedCoords.size(); p++)
			{
				this_img_pts[p].x = detectedCoords[p].x;
				this_img_pts[p].y = detectedCoords[p].y;
			}

			if (corners_found && corners_count != CORNERS_COUNT)
				corners_found = false;

			cout << format(
				"Img %s: %s\n",
				mrpt::system::extractFileName(it->first).c_str(),
				corners_found ? "DETECTED" : "NOT DETECTED");

			if (corners_found)
			{
				// save the corners in the data structure:
				int x, y;
				unsigned int k;
				for (y = 0, k = 0; y < check_size.height; y++)
					for (x = 0; x < check_size.width; x++, k++)
						dat.detected_corners.emplace_back(
							this_img_pts[k].x, this_img_pts[k].y);

				// Draw the checkerboard in the corresponding image:
				// ----------------------------------------------------
				if (!dat.img_original.isExternallyStored())
				{
					const int r = 4;
					CvPoint prev_pt = cvPoint(0, 0);
					const int line_max = 8;
					CvScalar line_colors[8];

					line_colors[0] = CV_RGB(255, 0, 0);
					line_colors[1] = CV_RGB(255, 128, 0);
					line_colors[2] = CV_RGB(255, 128, 0);
					line_colors[3] = CV_RGB(200, 200, 0);
					line_colors[4] = CV_RGB(0, 255, 0);
					line_colors[5] = CV_RGB(0, 200, 200);
					line_colors[6] = CV_RGB(0, 0, 255);
					line_colors[7] = CV_RGB(255, 0, 255);

					// Checkboad as color image:
					dat.img_original.colorImage(dat.img_checkboard);

					cv::Mat rgb_img =
						dat.img_checkboard.asCvMat<cv::Mat>(SHALLOW_COPY);

					for (y = 0, k = 0; y < check_size.height; y++)
					{
						cv::Scalar color = line_colors[y % line_max];
						for (x = 0; x < check_size.width; x++, k++)
						{
							cv::Point pt{cvRound(this_img_pts[k].x),
										 cvRound(this_img_pts[k].y)};

							if (k != 0) cv::line(rgb_img, prev_pt, pt, color);

							cv::line(
								rgb_img, cv::Point(pt.x - r, pt.y - r),
								cv::Point(pt.x + r, pt.y + r), color);
							cv::line(
								rgb_img, cv::Point(pt.x - r, pt.y + r),
								cv::Point(pt.x + r, pt.y - r), color);
							cv::circle(rgb_img, pt, r + 1, color);
							prev_pt = pt;
						}
					}
				}

				// Accept this image as good:
				pointsIdx2imageFile.push_back(it->first);
				imagePoints.push_back(this_img_pts);
				objectPoints.push_back(pattern_obj_points);

				valid_detected_imgs++;
			}

		}  // end find corners

		std::cout << valid_detected_imgs << " valid images." << std::endl;
		if (!valid_detected_imgs)
		{
			std::cout << "ERROR: No valid images. Perhaps the checkerboard "
						 "size is incorrect?"
					  << std::endl;
			return false;
		}

		// ---------------------------------------------
		// Calculate the camera parameters
		// ---------------------------------------------
		// Calibrate camera
		cv::Mat cameraMatrix, distCoeffs(1, 5, CV_64F, cv::Scalar::all(0));
		vector<cv::Mat> rvecs, tvecs;

		const double cv_calib_err = cv::calibrateCamera(
			objectPoints, imagePoints, imgSize, cameraMatrix, distCoeffs, rvecs,
			tvecs, 0 /*flags*/);

		// Load matrix:
		{
			Eigen::Matrix3d M;
			cv::cv2eigen(cameraMatrix, M);
			out_camera_params.intrinsicParams = M;
		}

		out_camera_params.dist.fill(0);
		for (int k = 0; k < 5; k++)
			out_camera_params.dist[k] = distCoeffs.ptr<double>()[k];

		// Load camera poses:
		for (i = 0; i < valid_detected_imgs; i++)
		{
			CMatrixDouble44 HM;
			HM.setZero();
			HM(3, 3) = 1;

			{
				// Convert rotation vectors -> rot matrices:
				cv::Mat cv_rot;
				cv::Rodrigues(rvecs[i], cv_rot);

				Eigen::Matrix3d rot;
				cv::cv2eigen(cv_rot, rot);
				HM.block<3, 3>(0, 0) = rot;
			}

			{
				Eigen::Matrix<double, 3, 1> trans;
				cv::cv2eigen(tvecs[i], trans);
				HM.block<3, 1>(0, 3) = trans;
			}

			CPose3D p = CPose3D(0, 0, 0) - CPose3D(HM);

			images[pointsIdx2imageFile[i]].reconstructed_camera_pose = p;

			std::cout << "Img: "
					  << mrpt::system::extractFileName(pointsIdx2imageFile[i])
					  << ": " << p << std::endl;
		}

		{
			CConfigFileMemory cfg;
			out_camera_params.saveToConfigFile("CAMERA_PARAMS", cfg);
			std::cout << cfg.getContent() << std::endl;
		}

		// ----------------------------------------
		// Undistort images:
		// ----------------------------------------
		for (it = images.begin(); it != images.end(); ++it)
		{
			TImageCalibData& dat = it->second;
			if (!dat.img_original.isExternallyStored())
				dat.img_original.undistort(
					dat.img_rectified, out_camera_params);
		}  // end undistort

		// -----------------------------------------------
		// Reproject points to measure the fit sqr error
		// -----------------------------------------------
		double sqrErr = 0;

		for (i = 0; i < valid_detected_imgs; i++)
		{
			TImageCalibData& dat = images[pointsIdx2imageFile[i]];
			if (dat.detected_corners.size() != CORNERS_COUNT) continue;

			// Reproject all the points into pixel coordinates:
			// -----------------------------------------------------
			vector<TPoint3D> lstPatternPoints(
				CORNERS_COUNT);  // Points as seen from the camera:
			for (unsigned int p = 0; p < CORNERS_COUNT; p++)
				lstPatternPoints[p] = TPoint3D(
					pattern_obj_points[p].x, pattern_obj_points[p].y,
					pattern_obj_points[p].z);

			vector<TPixelCoordf>& projectedPoints =
				dat.projectedPoints_undistorted;
			vector<TPixelCoordf>& projectedPoints_distorted =
				dat.projectedPoints_distorted;

			vision::pinhole::projectPoints_no_distortion(
				lstPatternPoints,  // Input points
				dat.reconstructed_camera_pose,
				out_camera_params.intrinsicParams,  // calib matrix
				projectedPoints  // Output points in pixels
			);

			vision::pinhole::projectPoints_with_distortion(
				lstPatternPoints,  // Input points
				dat.reconstructed_camera_pose,
				out_camera_params.intrinsicParams,  // calib matrix
				out_camera_params.getDistortionParamsAsVector(),
				projectedPoints_distorted  // Output points in pixels
			);

			ASSERT_(projectedPoints.size() == CORNERS_COUNT);
			ASSERT_(projectedPoints_distorted.size() == CORNERS_COUNT);

			for (unsigned int p = 0; p < CORNERS_COUNT; p++)
			{
				const double px = projectedPoints[p].x;
				const double py = projectedPoints[p].y;

				const double px_d = projectedPoints_distorted[p].x;
				const double py_d = projectedPoints_distorted[p].y;

				// Only draw if the img is NOT external:
				if (!dat.img_original.isExternallyStored())
				{
					if (px >= 0 && px < imgSize.width && py >= 0 &&
						py < imgSize.height)
						cv::circle(
							dat.img_rectified.asCvMatRef(), cv::Point(px, py),
							4, CV_RGB(0, 0, 255));
				}

				// Accumulate error:
				sqrErr +=
					square(px_d - dat.detected_corners[p].x) +
					square(py_d - dat.detected_corners[p].y);  // Error relative
				// to the
				// original
				// (distorted)
				// image.
			}
		}

		if (valid_detected_imgs)
		{
			sqrErr /= CORNERS_COUNT * valid_detected_imgs;
			std::cout << "Average err. of reprojection: " << sqrt(sqrErr)
					  << " pixels (OpenCV error=" << cv_calib_err << ")\n";
		}
		if (out_MSE) *out_MSE = sqrt(sqrErr);

		return true;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return false;
	}
#else
	THROW_EXCEPTION("Function not available: MRPT was compiled without OpenCV");
#endif
}


#ifndef IGNITION_GAZEBO_SYSTEMS_OMNIDRIVE_HH_
#define IGNITION_GAZEBO_SYSTEMS_OMNIDRIVE_HH_

#include <memory>

#include <mutex>
#include <string>

#include <Eigen/Eigen>

#include <ignition/gazebo/System.hh>

#include <ignition/common/Profiler.hh>
#include <ignition/math/Angle.hh>
#include <ignition/math/Quaternion.hh>
#include <ignition/math/SpeedLimiter.hh>
#include <ignition/transport/Node.hh>

#include "ignition/gazebo/Link.hh"
#include "ignition/gazebo/Model.hh"
#include "ignition/gazebo/Util.hh"

#include "OmniDriveOdometry.hh"

/** @brief Velocity command. **/
struct Commands
{
  /** @brief Linear velocity in X-axis. **/
  double lin_x;

  /** @brief Linear velocity in Y-axis. **/
  double lin_y;

  /** @brief Angular velocity in Z-axis. **/
  double ang_z;

  Commands() : lin_x(0.0), lin_y(0.0), ang_z(0.0) {}
};

namespace ignition
{
namespace gazebo
{
// Inline bracket to help doxygen filtering.
inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
namespace omni_drive
{

  class OmniDrivePrivate
  {
    public:
      /**
       * @brief Callback for velocity subscription.
       *
       * @param _msg Velocity message.
       */
      void OnCmdVel(const ignition::msgs::Twist &_msg);

      /**
       * @brief Callback for enable/disable subscription.
       *
       * @param _msg Boolean message.
       */
      void OnEnable(const ignition::msgs::Boolean &_msg);

      /**
       * @brief Update odometry and publish an odometry message.
       *
       * @param _info System update information.
       * @param _ecm The EntityComponentManager of the given simulation instante.
       */
      void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info,
                          const ignition::gazebo::EntityComponentManager &_ecm);

      /**
       * @brief Update linear and angular velocities.
       *
       * @param _info System update information.
       * @param _ecm  The EntityComponentManager of the given simulation instance.
       */
      void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info,
                          const ignition::gazebo::EntityComponentManager &_ecm);

      /** @brief Ignition communication node **/
      transport::Node node;

      /** @brief Entity of the robot reference link **/
      Entity referenceFrame;

      /** @brief Entity of the front left joint **/
      Entity frontLeftJoint;

      /** @brief Entity of the rear left joint **/
      Entity rearLeftJoint;

      /** @brief Entity of the front right joint **/
      Entity frontRightJoint;

      /** @brief Entity of the rear right joint **/
      Entity rearRightJoint;

      /** @brief Name of the robot reference link **/
      std::string referenceFrameName;

      /** @brief Name of the front left joint **/
      std::string frontLeftJointName;

      /** @brief Name of the rear left joint **/
      std::string rearLeftJointName;

      /** @brief Name of the front right joint **/
      std::string frontRightJointName;

      /** @brief Name of the rear right joint **/
      std::string rearRightJointName;

      /** @brief Calculated speed of front left joint **/
      double frontLeftJointSpeed{0};

      /** @brief Calculated speed of rear left joint **/
      double rearLeftJointSpeed{0};

      /** @brief Calculated speed of front right joint **/
      double frontRightJointSpeed{0};

      /** @brief Calculated speed of rear right joint **/
      double rearRightJointSpeed{0};

      /** @brief Wheel radius **/
      double wheelRadius{0.2};

      /** @brief Distance between right and left wheels **/
      double wheelRightLeftSeparation{1.0};

      /** @brief Distance between front and rear wheels **/
      double wheelFrontRearSeparation{1.0};

      /** @brief Kinematics Matrix used for calculate wheel joint speeds **/
      Eigen::MatrixXd kinematicsMatrix;

      /** @brief Model interface **/
      Model model{kNullEntity};

      /** @brief The model's canonical link **/
      Link canonicalLink{kNullEntity};

      /** \brief Update period calculated from <odom__publish_frequency>. **/
      std::chrono::steady_clock::duration odomPubPeriod{0};

      /** \brief Last sim time odom was published. **/
      std::chrono::steady_clock::duration lastOdomPubTime{0};

      /** \brief Diff drive odometry. **/
      math::OmniDriveOdometry odom;

      /** \brief Omni drive odometry message publisher. **/
      transport::Node::Publisher odomPub;

      /** @brief Omni drive tf message publisher **/
      transport::Node::Publisher tfPub;

      /** @brief Linear velocity limiter **/
      std::unique_ptr<ignition::math::SpeedLimiter> limiterLin;

      /** @brief Angular velocity limiter **/
      std::unique_ptr<ignition::math::SpeedLimiter> limiterAng;

      /** @brief Previous control command **/
      Commands last0Cmd;

      /** @brief Previous control command to last0Cmd **/
      Commands last1Cmd;

      /** @brief Last target velocity requested **/
      msgs::Twist targetVel;

      /** @brief Target linear velocity in X-axis **/
      double targetLinVelX;

      /** @brief Target linear velocity in Y-axis **/
      double targetLinVelY;

      /** @brief Target angular velocity in Z-axis **/
      double targetAngVelZ;

      /** @brief Enable/disable state of the controller **/
      bool enabled;

      /** @brief A muterx to protect the target velocity command **/
      std::mutex mutex;

      /** @brief frame_id from sdf. **/
      std::string sdfFrameId;

      /** @brief child_frame_id from sdf. **/
      std::string sdfChildFrameId;

  };

  class OmniDrive
      : public System,
        public ISystemConfigure,
        public ISystemPreUpdate,
        public ISystemPostUpdate
  {
    public:
      /** @brief Constructor **/
      OmniDrive();

      /** @brief Destructor **/
      ~OmniDrive() override = default;

      /** Documentation inherited **/
      void Configure(const Entity &_entity,
                     const std::shared_ptr<const sdf::Element> &_sdf,
                     EntityComponentManager &_ecm,
                     EventManager &_eventMgr) override;

      /** Documentation inherited **/
      void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
                     ignition::gazebo::EntityComponentManager &_ecm) override;

      /** Documentation inherited **/
      void PostUpdate(const UpdateInfo &_info,
                      const EntityComponentManager &_ecm) override;

    private:
      /** Private data pointer **/
      std::unique_ptr<OmniDrivePrivate> dataPtr;
  };

} // namespace omni_drive

} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
} // namespace gazebo
} // namespace ignition

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/include/constants/true_false.hpp>
#include <boost/simd/sdk/simd/logical.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include "../constant.hpp"

NT2_TEST_CASE_TPL( true_false, BOOST_SIMD_TYPES )
{
  using boost::simd::logical;

  NT2_CHECK_CONSTANT(False, logical<T>(false) , logical<T> );
  NT2_CHECK_CONSTANT(True , logical<T>(true)  , logical<T> );
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/python/overloads.hpp>

#include <mmtbx/scaling/twinning.h>

namespace mmtbx { namespace scaling {

namespace{

  struct twin_r_wrapper
  {
    typedef twinning::twin_r<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("twin_r", no_init)
        .def(init<
             scitbx::af::const_ref< cctbx::miller::index<> >  const&,
             scitbx::af::const_ref< double > const&,
             cctbx::sgtbx::space_group const&,
             bool const&,
             scitbx::mat3<double> const& >
             ((arg("miller_indices"),
               arg("intensity"),
               arg("space_group"),
               arg("anomalous_flag"),
               arg("symop") )))
        .def("r_abs_value", &w_t::r_abs_value)
        .def("r_sq_value", &w_t::r_sq_value)
        .def("r_abs_pair", &w_t::r_abs_pair)
        .def("r_sq_pair", &w_t::r_abs_pair)
        .def("correlation", &w_t::correlation)
        ;
    }

  };

  struct l_test_wrapper
  {
    typedef twinning::l_test<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("l_test", no_init)
        .def(init<
             scitbx::af::const_ref< cctbx::miller::index<> >  const&,
             scitbx::af::const_ref< double > const&,
             cctbx::sgtbx::space_group const&,
             bool const&,
             long const&,
             long const&,
             long const&,
             std::size_t const& >
             ((arg("miller_indices"),
               arg("intensity"),
               arg("space_group"),
               arg("anomalous_flag"),
               arg("parity_h"),
               arg("parity_k"),
               arg("parity_l"),
               arg("max_delta_h") )))
        .def("mean_l", &w_t::mean_l)
        .def("mean_l2", &w_t::mean_l2)
        .def("ml_alpha", &w_t::ml_alpha)
        .def("cumul", &w_t::cumul)
        ;
    }

  };

  struct detwin_wrapper
  {
    typedef twinning::detwin<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("detwin", no_init)
        .def(init<
             scitbx::af::const_ref< cctbx::miller::index<> >  const&,
             scitbx::af::const_ref< double > const&,
             scitbx::af::const_ref< double > const&,
             cctbx::sgtbx::space_group const&,
             bool const&,
             scitbx::mat3<double> const&
             >
             ((arg("miller_indices"),
               arg("intensity"),
               arg("sigma"),
               arg("space_group"),
               arg("anomalous_flag"),
               arg("twin_law") )))
        .def("detwin_with_alpha", &w_t::detwin_with_alpha)
        .def("detwinned_i", &w_t::detwinned_i)
        .def("detwinned_sigi", &w_t::detwinned_sigi)
        .def("detwinned_hkl", &w_t::detwinned_hkl)
        .def("completeness", &w_t::completeness)
        .def("location", &w_t::location)
        ;
    }

  };

  struct h_test_wrapper
  {
    typedef twinning::h_test<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("h_test", no_init)
        .def(init<
             scitbx::af::const_ref< cctbx::miller::index<> >  const&,
             scitbx::af::const_ref< double > const&,
             scitbx::af::const_ref< double > const&,
             cctbx::sgtbx::space_group const&,
             bool const&,
             scitbx::mat3<double> const&,
             double const&
             >
             ((arg("miller_indices"),
               arg("intensity"),
               arg("sigma"),
               arg("space_group"),
               arg("anomalous_flag"),
               arg("twin_law"),
               arg("fraction")) ))
        .def("h_array", &w_t::h_array)
        .def("h_values", &w_t::h_values)
        .def("h_cumul_obs", &w_t::h_cumul_obs)
        .def("h_cumul_fit", &w_t::h_cumul_fit)
        .def("distance", &w_t::distance)
        .def("alpha", &w_t::alpha)
        .def("mean_h", &w_t::mean_h)
        .def("mean_h2", &w_t::mean_h2)
        ;
    }

  };

  struct ml_murray_rust_wrapper
  {
    typedef twinning::ml_murray_rust<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("ml_murray_rust", no_init)
        .def(init<
             scitbx::af::const_ref<double> const&,
             scitbx::af::const_ref<double> const&,
             scitbx::af::const_ref< cctbx::miller::index<> > const&,
             cctbx::sgtbx::space_group const&,
             bool const&,
             scitbx::mat3<double> const&,
             long const&
             > (( arg("z"),
                  arg("sig_z"),
                  arg("indices"),
                  arg("space_group"),
                  arg("anomalous_flag"),
                  arg("twin_law"),
                  arg("n_hermite"))
                ))
        .def("num_int", &w_t::num_int,
             (( arg("ito1"),
                arg("sito1"),
                arg("ito2"),
                arg("sito2"),
                arg("low_sig"),
                arg("high_sig"),
                arg("twin_fraction"),
                arg("n")
                ))
            )
        .def("p_raw", &w_t::p_raw)
        .def("log_p_given_t", &w_t::log_p_given_t)
        .def("fast_log_p_given_t", &w_t::fast_log_p_given_t)

        ;

    }

  };

  struct ml_twin_with_ncs
  {
    typedef twinning::ml_twin_with_ncs<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("ml_twin_with_ncs", no_init)
        .def(init<
             scitbx::af::const_ref<double> const&,
             scitbx::af::const_ref<double> const&,
             scitbx::af::const_ref< cctbx::miller::index<> > const&,
             scitbx::af::const_ref< std::size_t > const&,
             cctbx::sgtbx::space_group const&,
             bool const&,
             scitbx::mat3<double> const&,
             cctbx::uctbx::unit_cell const&,
             long const&
             > (( arg("z"),
                  arg("sig_z"),
                  arg("indices"),
                  arg("bins"),
                  arg("space_group"),
                  arg("anomalous_flag"),
                  arg("twin_law"),
                  arg("unit_cell"),
                  arg("n_quadrature"))
                ))
        .def("p_raw", &w_t::p_raw)
        .def("num_int", &w_t::num_int )
        .def("p_tot_given_t_and_coeff", &w_t::p_tot_given_t_and_coeff)
        ;

    }

  };

  struct very_quick_erf_wrapper
  {
    typedef twinning::very_quick_erf<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("very_quick_erf", no_init)
        .def(init<double const& > ((arg("step_size"))))
        .def("erf", &w_t::erf)
        .def("loop_for_timings", &w_t::loop_for_timings, (
          arg("number_of_iterations"), arg("optimized")))
        ;
    }
  };

  struct quick_ei0_wrapper
  {
    typedef twinning::quick_ei0<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("quick_ei0", no_init)
        .def(init<int const& > ((arg("n_points"))))
        .def("ei0", &w_t::ei0)
        .def("loop_for_timings", &w_t::loop_for_timings, (
          arg("number_of_iterations"), arg("optimized")))
        ;
    }
  };

}  // namespace <anonymous>

namespace boost_python {

  void wrap_twinning()
  {
    twin_r_wrapper::wrap();
    l_test_wrapper::wrap();
    detwin_wrapper::wrap();
    h_test_wrapper::wrap();
    ml_murray_rust_wrapper::wrap();
    ml_twin_with_ncs::wrap();
    very_quick_erf_wrapper::wrap();
    quick_ei0_wrapper::wrap();
  }

}}}