//
//  dt_util.cpp
//  Classifer_RF
//
//  Created by jimmy on 2017-02-16.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#include "dt_util.hpp"
#include <Eigen/QR>
#include <iostream>
#include <map>

using std::cout;
using std::endl;
using std::map;

namespace dt {
    template< class T>
    vector<T> randomDimension(const T dim, const T num)
    {
        assert(dim > 0);
        assert(num > 0);
        assert(num <= dim);

        vector<T> dims;
        for (T i = 0; i<dim; i++) {
            dims.push_back(i);
        }
        std::random_shuffle(dims.begin(), dims.end());
        vector<T> random_dim(dims.begin(), dims.begin() + num);
        assert(random_dim.size() > 0 && random_dim.size() <= dims.size());

        return random_dim;
    }

    template< class vectorType>
    void meanStd(const vector<vectorType> & labels, vectorType & mean, vectorType & sigma)
    {
        assert(labels.size() > 0);

        mean = vectorType::Zero(labels[0].size());

        for (int i = 0; i<labels.size(); i++) {
            mean += labels[i];
        }
        mean /= labels.size();

        sigma = vectorType::Zero(labels[0].size());
        if (labels.size() == 1) {
            return;
        }
        for (int i = 0; i<labels.size(); i++) {
            vectorType dif = labels[i] - mean;
            for (int j = 0; j<sigma.size(); j++) {
                sigma[j] += dif[j] * dif[j];
            }
        }
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] = sqrt(fabs(sigma[j])/labels.size());
        }
    }

    template<class vectorType, class intType>
    void meanStd(const vector<vectorType> & data, const vector<intType> & indices,
                 vectorType & mean, vectorType & sigma)
    {
        assert(data.size() > 0);
        assert(indices.size() > 0);

        assert(indices.size() > 0);

        mean = vectorType::Zero(data[0].size());

        for (int i = 0; i<indices.size(); i++) {
            int index = indices[i];
            assert(index >= 0 && index < data.size());
            mean += data[index];
        }
        mean /= indices.size();

        sigma = vectorType::Zero(data[0].size());
        if (indices.size() == 1) {
            return;
        }
        for (int i = 0; i<indices.size(); i++) {
            vectorType dif = data[indices[i]] - mean;
            for (int j = 0; j<sigma.size(); j++) {
                sigma[j] += dif[j] * dif[j];
            }
        }
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] = sqrt(fabs(sigma[j])/indices.size());
        }
    }

    template <class intType>
    vector<intType> balanceSamples(const vector<intType> & example_indices, const vector<intType> & labels, const int category_num)
    {
        assert(example_indices.size() <= labels.size());

        // step 1: count example numbers in each category
        vector<intType> count(category_num, 0);
        for (int i = 0; i<example_indices.size(); i++) {
            intType idx = example_indices[i];
            count[labels[idx]]++;
        }
        intType min_count = *std::min_element(count.begin(), count.end());
        assert(min_count >= 0);

        // step 2: select the first min_count example in each category
        count = vector<int>(category_num, 0);
        vector<intType> balanced_indices;
        for (int i = 0; i<example_indices.size(); i++) {
            intType idx = example_indices[i];
            intType cur_label = labels[idx];

            // skip this category
            if (count[cur_label] >= min_count) {
                continue;
            }
            else {
                balanced_indices.push_back(idx);
                count[cur_label]++;
            }
        }
        return balanced_indices;
    }

    template<class VectorType, class IntType>
    double sumOfVariance(const vector<VectorType> & labels, const vector<IntType> & indices)
    {
        if (indices.size() <= 0) {
            return 0.0;
        }
        assert(indices.size() > 0);

        VectorType mean = VectorType::Zero(labels[0].size());

        for (int i = 0; i<indices.size(); i++) {
            IntType index = indices[i];
            assert(index >= 0 && index < labels.size());
            mean += labels[index];
        }
        mean /= indices.size();

        double var = 0.0;
        for (int i = 0; i<indices.size(); i++) {
            IntType index = indices[i];
            assert(index >= 0 && index < labels.size());
            VectorType dif = labels[index] - mean;
            for (int j = 0; j<dif.size(); j++) {
                var += dif[j] * dif[j];
            }
        }
        return var;
    }

    template<class intType>
    intType mostCommon(const vector<intType> & data)
    {
        assert(data.size() > 0);
        int max_count = 0;
        int most_common = 0;
        std::map<intType, int> m;
        for (auto vi = data.begin(); vi != data.end(); vi++) {
            m[*vi]++;
            if (m[*vi] > max_count) {
                max_count = m[*vi];
                most_common = *vi;
            }
        }
        return most_common;
    }

    template <class T>
    void meanMedianError(const vector<T> & errors,
                                 T & mean,
                                 T & median)
    {
        assert(errors.size() > 0);
        const int dim = (int)errors[0].size();
        mean = T::Zero(dim);
        median = T::Zero(dim);

        vector<vector<double> > each_dim_data(dim);
        for (int i = 0; i<errors.size(); i++) {
            T err = errors[i].cwiseAbs();
            mean += err;
            for (int j = 0; j<err.size(); j++) {
                each_dim_data[j].push_back(err[j]);
            }
        }
        mean /= errors.size();

        for (int i = 0; i<each_dim_data.size(); i++) {
            std::sort(each_dim_data[i].begin(), each_dim_data[i].end());
            median[i] = each_dim_data[i][each_dim_data[i].size()/2];
        }
    }

    template vector<int> randomDimension(int dim, int num);

    template void meanStd(const vector<Eigen::VectorXd> & labels, Eigen::VectorXd & mean, Eigen::VectorXd & sigma);
    template void meanStd(const vector<Eigen::Vector3d> & labels, Eigen::Vector3d & mean, Eigen::Vector3d & sigma);

    template void meanStd(const vector<Eigen::VectorXf> & labels, const vector<int> & indices,
                          Eigen::VectorXf & mean, Eigen::VectorXf & sigma);

    template
    vector<int> balanceSamples(const vector<int> & example_indices, const vector<int> & labels, const int category_num);

    template
    double sumOfVariance(const vector<Eigen::VectorXf> & labels, const vector<int> & indices);

    template
    int mostCommon(const vector<int> & data);

    template
    void meanMedianError(const vector<Eigen::VectorXf> & errors, Eigen::VectorXf & mean, Eigen::VectorXf & median);

    template
    void meanMedianError(const vector<Eigen::VectorXd> & errors, Eigen::VectorXd & mean, Eigen::VectorXd & median);
}

vector<unsigned int> DTUtil::randomDimensions(const int dimension, const int candidate_dimension)
{
    assert(dimension > 0);
    assert(candidate_dimension > 0);
    assert(candidate_dimension <= dimension);

    vector<unsigned int> dims;
    for (unsigned int i = 0; i<dimension; i++) {
        dims.push_back(i);
    }
    std::random_shuffle(dims.begin(), dims.end());
    vector<unsigned int> random_dim(dims.begin(), dims.begin() + candidate_dimension);
    assert(random_dim.size() > 0 && random_dim.size() <= dims.size());

    return random_dim;
}

template <class T>
double DTUtil::spatialVariance(const vector<T> & labels, const vector<unsigned int> & indices)
{
    if (indices.size() <= 0) {
        return 0.0;
    }
    assert(indices.size() > 0);

    T mean = T::Zero(labels[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index];
    }
    mean /= indices.size();

    double var = 0.0;
    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        T dif = labels[index] - mean;
        for (int j = 0; j<dif.size(); j++) {
            var += dif[j] * dif[j];
        }
    }
    return var;
}

template<class T>
double DTUtil::fullVariance(const vector<T>& labels, const vector<unsigned int> & indices)
{
    assert(indices.size() > 1);
    typedef typename T::Scalar Scalar;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;

    double loss = 0.0;

    const int length = (int)labels[0].size();
    MatrixType sampled_data(indices.size(), length);
    for (unsigned i = 0; i<indices.size(); i++) {
        unsigned int index = indices[i];
        assert(index >= 0 && index < labels.size());
        sampled_data.row(i) = labels[index];
    }

    MatrixType centered = sampled_data.rowwise() - sampled_data.colwise().mean();
    MatrixType cov = (centered.adjoint() * centered) / sampled_data.rows();

    Eigen::ColPivHouseholderQR<MatrixType> qr(cov);
    if (qr.rank() == length) {
        loss = qr.logAbsDeterminant();
    }
    else {
        loss = qr.logAbsDeterminant();
        // avoid underflow
        if (std::isnan(loss) || std::isinf(loss)) {
            loss = log(0.0000001);
        }
        //printf("Warning: full variance underflow, use a small number log(0.0000001) instead. Sample number %ld\n", indices.size());
        //cout<<"covariance matrix \n"<<cov<<endl;
        //printf("logAbsDeterminant vs log(0.0000001): %lf, %lf\n", qr.logAbsDeterminant(), log(0.0000001));
    }

    return loss;
}

template <class MatrixType>
double DTUtil::sumOfVariance(const vector<MatrixType> & labels, const int row_index, const vector<unsigned int> & indices)
{
    typedef typename MatrixType::Scalar Scalar;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ScalarVector;

    if (indices.size() <= 0) {
        return 0.0;
    }
    assert(indices.size() > 0);

    ScalarVector mean = ScalarVector::Zero(labels[0].row(0).size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index].row(row_index);
    }
    mean /= indices.size();

    double var = 0.0;
    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        ScalarVector dif = labels[index].row(row_index) - mean;
        for (int j = 0; j<dif.size(); j++) {
            var += dif[j] * dif[j];
        }
    }
    return var;
}

template<class Type1, class Type2>
double DTUtil::spatialVariance(const vector<Type1> & labels, const vector<unsigned int> & indices, const vector<Type2> & wt)
{
    if (indices.size() <= 0) {
        return 0.0;
    }
    assert(indices.size() > 0);
    assert(wt.size() == labels.front().size());

    Type1 mean = Type1::Zero(labels[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index];
    }
    mean /= indices.size();

    double var = 0.0;
    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        Type1 dif = labels[index] - mean;
        for (int j = 0; j<dif.size(); j++) {
            var += dif[j] * dif[j] * fabs(double(wt[j]));
        }
    }
    return var;
}

template <class T>
void DTUtil::meanStddev(const vector<T> & labels, const vector<unsigned int> & indices, T & mean, T & sigma)
{
    assert(indices.size() > 0);

    mean = T::Zero(labels[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index];
    }
    mean /= indices.size();

    sigma = T::Zero(labels[0].size());
    if (indices.size() == 1) {
        return;
    }
    for (int i = 0; i<indices.size(); i++) {
        T dif = labels[indices[i]] - mean;
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] += dif[j] * dif[j];
        }
    }
    for (int j = 0; j<sigma.size(); j++) {
        sigma[j] = sqrt(fabs(sigma[j])/indices.size());
    }
}

template <class vectorT, class indexT>
vectorT DTUtil::mean(const vector<vectorT> & data, const vector<indexT> & indices)
{
    assert(indices.size() > 0);

    vectorT m = vectorT::Zero(data[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < data.size());
        m += data[index];
    }
    m /= indices.size();

    return m;
}

template <class T>
T DTUtil::mean(const vector<T> & data)
{
    assert(data.size() > 0);

    T m = T::Zero(data[0].size());

    for (int i = 0; i<data.size(); i++) {
        m += data[i];
    }
    m /= data.size();
    return m;
}

template <class matrixType, class vectorType>
void DTUtil::rowMeanStddev(const vector<matrixType> & labels, const vector<unsigned int> & indices, const int row_index, vectorType & mean, vectorType & sigma)
{
    assert(indices.size() > 0);

    mean = vectorType::Zero(labels[0].row(0).size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index].row(row_index);
    }
    mean /= indices.size();

    sigma = vectorType::Zero(labels[0].row(0).size());
    if (indices.size() == 1) {
        return;
    }
    for (int i = 0; i<indices.size(); i++) {
        vectorType dif = labels[indices[i]].row(row_index) - mean;
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] += dif[j] * dif[j];
        }
    }
    for (int j = 0; j<sigma.size(); j++) {
        sigma[j] = sqrt(fabs(sigma[j])/indices.size());
    }
}

template<class vectorT>
void DTUtil::quartileError(const vector<vectorT> & errors, vectorT& q1, vectorT& q2, vectorT& q3)
{
    assert(errors.size() > 0);
    const int dim = (int)errors[0].size();

    q1 = vectorT::Zero(dim);
    q2 = vectorT::Zero(dim);
    q3 = vectorT::Zero(dim);

    vector<vector<double> > each_dim_data(dim);
    for (int i = 0; i<errors.size(); i++) {
        vectorT err = errors[i].cwiseAbs();
        for (int j = 0; j<err.size(); j++) {
            each_dim_data[j].push_back(err[j]);
        }
    }

    for (int i = 0; i<each_dim_data.size(); i++) {
        std::sort(each_dim_data[i].begin(), each_dim_data[i].end());
        q1[i] = each_dim_data[i][each_dim_data[i].size()/4];
        q2[i] = each_dim_data[i][each_dim_data[i].size()/2];
        q3[i] = each_dim_data[i][each_dim_data[i].size()/4*3];
    }
}

template <class MatrixType>
void DTUtil::matrixMeanError(const vector<MatrixType> & errors, MatrixType & mean)
{
    assert(errors.size() > 0);

    const int cols  = (int)errors[0].cols();
    const int rows = (int)errors[0].rows();
    mean  = MatrixType::Zero(rows, cols);

    for (int i = 0; i<errors.size(); i++) {
        mean += errors[i];
    }
    mean /= errors.size();
}

double DTUtil::crossEntropy(const Eigen::VectorXd & prob)
{
    double entropy = 0.0;
    for (int i = 0; i<prob.size(); i++) {
        double p = prob[i];
        if (p == 0.0) {
            continue;
        }
        assert(p > 0 && p <= 1);
        entropy += - p * std::log(p);
    }
    return entropy;
}

double DTUtil::crossEntropy(const Eigen::VectorXf& prob)
{
    double entropy = 0.0;
    for (int i = 0; i<prob.size(); i++) {
        double p = prob[i];
        if (p == 0.0) {
            continue;
        }
        assert(p > 0 && p <= 1);
        entropy += - p * std::log(p);
    }
    return entropy;
}

double DTUtil::balanceLoss(const int leftNodeSize, const int rightNodeSize)
{
    double dif = leftNodeSize - rightNodeSize;
    double num = leftNodeSize + rightNodeSize;
    double loss = fabs(dif)/num;
    assert(loss >= 0);
    return loss;
}

bool
DTUtil::isSameLabel(const vector<unsigned int> & labels, const vector<unsigned int> & indices)
{
    assert(indices.size() >= 1);
    unsigned label = labels[indices[0]];
    for (int i = 1; i<indices.size(); i++) {
        if (label != labels[indices[i]]) {
            return false;
        }
    }
    return true;
}

bool DTUtil::isSameLabel(const vector<int>& labels, const vector<int>& indices)
{
    assert(indices.size() >= 1);
    int label = labels[indices[0]];
    for (int i = 1; i<indices.size(); i++) {
        if (label != labels[indices[i]]) {
            return false;
        }
    }
    return true;
}

int DTUtil::minLabelNumber(const vector<unsigned int> & labels, const vector<unsigned int> & indices,
                           const int num_category)
{
    vector<int> num(num_category, 0);
    for (int i = 0; i<indices.size(); i++) {
        int label = labels[indices[i]];
        num[label]++;
    }
    return *std::min_element(num.begin(), num.end());
}

int DTUtil::minLabelNumber(const vector<VectorXi> & labels,
                           const vector<unsigned int> & indices,
                           const int time_step,
                           const int num_category)
{
    vector<int> num(num_category, 0);
    for (int i = 0; i<indices.size(); i++) {
        int label = labels[indices[i]][time_step];
        num[label]++;
    }

    return *std::min_element(num.begin(), num.end());
}

template <class integerType>
Eigen::MatrixXd DTUtil::confusionMatrix(const vector<integerType> & preds,
                                        const vector<integerType> & labels,
                                        const int category_num,
                                        bool normalize)
{
    assert(preds.size() == labels.size());
    assert(category_num > 0);

    Eigen::MatrixXd confusion = Eigen::MatrixXd::Zero(category_num, category_num);
    for (int i = 0; i<preds.size(); i++) {
        confusion(labels[i], preds[i]) += 1.0;
    }
    if (normalize) {
        confusion = 1.0 / preds.size() * confusion;
    }
    return confusion;
}

Eigen::VectorXd DTUtil::accuracyFromConfusionMatrix(const Eigen::MatrixXd & conf)
{
    assert(conf.rows() == conf.cols());

    Eigen::VectorXd acc = Eigen::VectorXd(conf.rows() + 1, 1);
    Eigen::VectorXd row_sum = conf.rowwise().sum();
    double all_sum = conf.sum();
    double trace = conf.trace();
    for (int r = 0; r<conf.rows(); r++) {
        acc[r] = conf(r ,r)/row_sum[r];
    }
    acc[conf.rows()] = trace/all_sum;
    return acc;
}

Eigen::VectorXd DTUtil::precisionFromConfusionMatrix(const Eigen::MatrixXd & conf)
{
    assert(conf.rows() == conf.cols());

    Eigen::VectorXd precision = Eigen::VectorXd(conf.rows() + 1, 1);
    Eigen::VectorXd row_sum = conf.rowwise().sum();
    double all_sum = conf.sum();
    double trace = conf.trace();
    for (int r = 0; r<conf.rows(); r++) {
        precision[r] = conf(r ,r)/row_sum[r];
    }
    precision[conf.rows()] = trace/all_sum;
    return precision;
}

template double
DTUtil::spatialVariance(const vector<Eigen::VectorXf> & labels, const vector<unsigned int> & indices);

template double
DTUtil::fullVariance(const vector<Eigen::VectorXf>& labels, const vector<unsigned int> & indices);

template double
DTUtil::sumOfVariance(const vector<Eigen::MatrixXf> & labels, const int row_index, const vector<unsigned int> & indices);

template double
DTUtil::spatialVariance(const vector<Eigen::VectorXf> & labels, const vector<unsigned int> & indices, const vector<int> & wt);

template void
DTUtil::meanStddev(const vector<Eigen::VectorXf> & labels, const vector<unsigned int> & indices, Eigen::VectorXf & mean, Eigen::VectorXf & sigma);

template Eigen::VectorXf
DTUtil::mean(const vector<Eigen::VectorXf> & data, const vector<unsigned int> & indices);

template Eigen::VectorXf
DTUtil::mean(const vector<Eigen::VectorXf> & data, const vector<int> & indices);

template Eigen::VectorXf
DTUtil::mean(const vector<Eigen::VectorXf> & data);

template void
DTUtil::rowMeanStddev(const vector<Eigen::MatrixXf> & labels, const vector<unsigned int> & indices,
                      const int row_index, Eigen::VectorXf & mean, Eigen::VectorXf & sigma);

template
void DTUtil::quartileError(const vector<Eigen::VectorXf> & errors, Eigen::VectorXf& q1, Eigen::VectorXf& q2, Eigen::VectorXf& q3);

template void
DTUtil::matrixMeanError(const vector<Eigen::MatrixXf> & errors, Eigen::MatrixXf & mean);

template Eigen::MatrixXd
DTUtil::confusionMatrix(const vector<unsigned int> & preds,
                        const vector<unsigned int> & labels,
                        const int category_num,
                        bool normalize);

template Eigen::MatrixXd
DTUtil::confusionMatrix(const vector<int> & preds,
                        const vector<int> & labels,
                        const int category_num,
                        bool normalize);


// General ROS functionality
#include <ros/ros.h>

// Simulator services
#include <sim/Insert.h>
#include <sim/Resume.h>
#include <sim/Pause.h>

// PI
#include "pi/PIController.h"
#include "pi/global.h"
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>
// END PI

// Quadrotor services
#include <hal_quadrotor/State.h>

// Quadrotor controllers
#include <hal_quadrotor/control/Takeoff.h>
#include <hal_quadrotor/control/VelocityHeight.h>
//#include <hal_quadrotor/control/Waypoint.h>
//#include <hal_quadrotor/control/Hover.h>

#define EPS_POS			5e-1	// epsilon position
#define EPS_VEL			1e-3	// epsilon position

typedef enum { IDLE, TAKEOFF, VELHEIGHT, HOVER, WAYPOINT, OTHER } FSM;

// TODO: better way to get parameters in the callback instead of global vars
ros::ServiceClient srvTakeOff;
ros::ServiceClient srvVelocityHeight;
FSM state_FSM = IDLE;

// PI
using namespace std;
PIController pi;
// END PI

double takeoffx;
double takeoffy;
double takeoffz;
string idunit;

// compares two states
bool sameState(const hal_quadrotor::State::ConstPtr& s1, const hal_quadrotor::State &s2)
{
	double dpos = sqrt((s1->x-s2.x)*(s1->x-s2.x) + (s1->y-s2.y)*(s1->y-s2.y) + (s1->z-s2.z)*(s1->z-s2.z));
	double dvel = sqrt((s1->u-s2.u)*(s1->u-s2.u) + (s1->v-s2.v)*(s1->v-s2.v));
 	ROS_INFO("dpos: [%f %f]", dpos, dvel);
	return ((dpos < EPS_POS) && (dvel < EPS_VEL));
}

// Callback for quadrotor state
void StateCallback(const hal_quadrotor::State::ConstPtr& msg)
{
	switch (state_FSM) {
		case IDLE: {
  		ROS_INFO("IDLE, trying to take off");
			hal_quadrotor::Takeoff req_Takeoff;
			req_Takeoff.request.altitude = takeoffz;
			if(!srvTakeOff.call(req_Takeoff)){
				ROS_FATAL("NO TAKE OFF!!");
			} else {
				ROS_INFO("Taking OFF!!");
				state_FSM = TAKEOFF;
			}
		}	break;
		case TAKEOFF: {
			hal_quadrotor::State s;
			s.x = 5.; s.y = 5.; s.z = takeoffz;
			s.v = 0.; s.w = 0.;
			if (sameState(msg, s)) {
				ROS_INFO("TAKEN OFF, switch to VelocityHeight");
				state_FSM = VELHEIGHT;
			}
		} break;
		case VELHEIGHT: {
  		ROS_INFO("pos: [%f,%f,%f]", msg->x, msg->y, msg->z);
		  // rosservice call /hal/UAV0/controller/VelocityHeight
			hal_quadrotor::VelocityHeight req_VelocityHeight;
			req_VelocityHeight.request.dx = .0;
			req_VelocityHeight.request.dy = .1;
			req_VelocityHeight.request.z = takeoffz;
			req_VelocityHeight.request.yaw = 0.;
			if(!srvVelocityHeight.call(req_VelocityHeight)){
				ROS_FATAL("NO VELHEIGHT!!");
			} else {
				ROS_INFO("VelocityHeight command sent!!");
				state_FSM = OTHER;
			}
		} break;
		case OTHER: {
			// PI
			// pi controller (based on VelocityHeight)
			// convert state to pi state
			// (pi state should be the most updated joint state)
			vec s(4);
			s[0] = msg->x; s[1] = msg->y;
			s[2] = msg->u; s[3] = msg->v;
cout << "Sending state to PI controller!!" << endl;
			vec action = pi.computeControl(s);
cout << "current state " << s << endl;
cout << action << endl;
			vec sp = pi.predictState(s, action);
cout << "next state " << sp << endl;

			// convert pi control to VelocityHeight control
			hal_quadrotor::VelocityHeight req_VelocityHeight;
			req_VelocityHeight.request.dx = sp[2];
			req_VelocityHeight.request.dy = sp[3];
			req_VelocityHeight.request.z = takeoffz;
			req_VelocityHeight.request.yaw = 0.;

			if(!srvVelocityHeight.call(req_VelocityHeight)){
				ROS_FATAL("NO VELHEIGHT!!");
			} else {
				ROS_INFO("VelocityHeight command sent!!");
				state_FSM = OTHER;
			}
			// END PI
		} break;
		default: {
  		ROS_INFO("pos: [%f,%f,%f]", msg->x, msg->y, msg->z);
		} break;
	}

}

// Main entry point of application
int main(int argc, char **argv)
{
	// PI
	if (argc!=2) {
		cout << "requires xml_file as argument" << endl;
		exit(-1);
	}
	try {
		ifstream is(argv[1]);
		// read parameters from xml file
		using boost::property_tree::ptree;
		ptree pt;
		if (!is.is_open()) {
			cout << "file not found" << endl;
			exit(-1);
		}
		read_xml(is, pt);

//	// task parameters
//	//string ip = pt.get<string>("ip");
//	//int port = pt.get<int>("port");
//	//string taskfile = pt.get<string>("taskfile");
//	double dt = pt.get<double>("dt");
//	double R = pt.get<double>("R");
//	int dtperstep = pt.get<int>("dtperstep");
//	int H = pt.get<int>("H");
//	double nu = pt.get<double>("nu");
//	double T = pt.get<double>("T");
//	int seed = pt.get<int>("seed");
//	int N = pt.get<int>("N");
//	int units = pt.get<int>("units");
//	int dimUAVx = pt.get<int>("dimUAVx");
//	int dimUAVu = pt.get<int>("dimUAVu");
//
//	takeoffx = pt.get<double>("takeoffx");
//	takeoffy = pt.get<double>("takeoffy");
//	takeoffz = pt.get<double>("takeoffz");
//	idunit = pt.get<string>("idunit");
//
//	double dt_qrsim = 1.; // CHANGE
//	double dS = dt*dtperstep;
//	int dtperstep_qrsim = int(dS/dt_qrsim);
//	double dS_qrsim = dt_qrsim*dtperstep_qrsim;

// 	// initialize static variables
//	PIController::dt = dt;
//	PIController::units = units;
//	PIController::R = R;
//	PIController::nu = nu;
//	PIController::lambda = R*nu;
//	PIController::dtperstep = dtperstep;
//	PIController::H = H;
//	PIController::dS = dt*dtperstep;
//	PIController::stdv = sqrt(nu/dS);

		// Initial state X should be input as well (don't forget tpo convert)
		// create instance of PIController
//		pi.Init(dimUAVx, dimUAVu,	seed,	N);

		// Initial state X should be input as well (don't forget tpo convert)
		// create instance of PIController
		pi.Init(pt);//prop);

		double T = pt.get<double>("T");

	} catch (int e) {
		cout << "Exception" << e << endl;
	}

  // Initialise the ROS client
	string nodestr = string("example") + idunit;
  ros::init(argc, argv, nodestr.c_str()); //"example");

  // Create a node handle, which this C code will use to bind to the ROS server
  ros::NodeHandle n;

  // Create a client for interacting with the Simulator insert and Resume services
  ros::ServiceClient srvInsert = n.serviceClient<sim::Insert>("/simulator/Insert");
  ros::ServiceClient srvResume = n.serviceClient<sim::Resume>("/simulator/Resume");
  ros::ServiceClient srvPause  = n.serviceClient<sim::Pause> ("/simulator/Pause");

  // Subscribe to the state of the quadrotor
  string estimate = string("/hal/") + idunit + string("/Estimate");
	ros::Subscriber topState = n.subscribe(estimate.c_str(), 1000, StateCallback);

  // Create a resume message, which takes no arguments
  sim::Pause msgPause;

  // Call the client with this message
  if (!srvPause.call(msgPause))
  {
    ROS_FATAL("Failed to pause the simulator");
    return 1;
  }

  // Create a message instructing a quadrotor UAV0 to be inserted in to the world
  sim::Insert msgInsert;
  msgInsert.request.model_name = idunit;//"UAV0";
  msgInsert.request.model_type = "model://hummingbird";

  // Call the client with this message
  if (!srvInsert.call(msgInsert))
  {
    ROS_FATAL("Failed to insert a hummingbird quadrotor into the simulator");
    return 1;
  }

  // You should now see the quadrotor
  ROS_INFO("Successfully inserted the model into the world");

	sleep(2);
  // Create a resume message, which takes no arguments
  sim::Resume msgResume;

  // Call the client with this message
  if (!srvResume.call(msgResume))
  {
    ROS_FATAL("Failed to resume the simulator");
    return 1;
  }

  // rosservice call /hal/UAV0/controller/Takeoff 5
	string takeoff_cmd = string("hal/") + idunit + string("/controller/Takeoff");
	srvTakeOff = n.serviceClient<hal_quadrotor::Takeoff>(takeoff_cmd.c_str()); //"/hal/UAV0/controller/Takeoff");

	string velheight_cmd = string("hal/") + idunit + string("/controller/VelocityHeight");
	srvVelocityHeight = n.serviceClient<hal_quadrotor::VelocityHeight>(velheight_cmd.c_str()); //"/hal/UAV0/controller/VelocityHeight");

//	sleep(10);
//  // rosservice call /hal/UAV0/controller/Hover
//	hal_quadrotor::Hover req_Hover;
//	ros::ServiceClient srvHover = n.serviceClient<hal_quadrotor::Hover>("/hal/UAV0/controller/Hover");
//	if(!srvHover.call(req_Hover)){
//		ROS_FATAL("NO HOVER!!");
//		return 1;
//	} else {
//		ROS_INFO("Hovering!!");
//	}
//
//	sleep(10);
//  // rosservice call /hal/UAV0/controller/WayPoint
//	hal_quadrotor::Waypoint req_Waypoint;
//	req_Waypoint.request.x = 1.0;
//	req_Waypoint.request.y = 3.0;
//	req_Waypoint.request.z = 5.0;
//	req_Waypoint.request.yaw = 0.;
//
//	ros::ServiceClient srvWaypoint = n.serviceClient<hal_quadrotor::Waypoint>("/hal/UAV0/controller/Waypoint");
//	if(!srvWaypoint.call(req_Waypoint)){
//		ROS_FATAL("NO WAYPOINT!!");
//		return 1;
//	} else {
//		ROS_INFO("Waypointing!!");
//	}

  // Keep going until ctl+c is pressed
  ros::spin();

  // Success!
  return 0;
}


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

#include <Eigen/Core>
#include <example-robot-data/path.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/parsers/srdf.hpp>
#include <pinocchio/parsers/urdf.hpp>

#include "crocoddyl/core/activations/weighted-quadratic.hpp"
#include "crocoddyl/core/costs/cost-sum.hpp"
#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/integrator/euler.hpp"
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/residuals/control.hpp"
#include "crocoddyl/core/solvers/ddp.hpp"
#include "crocoddyl/core/utils/callbacks.hpp"
#include "crocoddyl/core/utils/timer.hpp"
#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
#include "crocoddyl/multibody/actuations/floating-base.hpp"
#include "crocoddyl/multibody/actuations/full.hpp"
#include "crocoddyl/multibody/contacts/contact-6d.hpp"
#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
#include "crocoddyl/multibody/residuals/state.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

using boost::make_shared;
using boost::shared_ptr;
using namespace crocoddyl;

int main()
{
    // because urdf is not supported with all double types.
    pinocchio::ModelTpl<double> model;
    pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf", model);
    pinocchio::srdf::loadReferenceConfigurations(model, EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf",
                                                 false);

    auto q0 = model.referenceConfigurations["half_sitting"];

    pinocchio::Data data(model);

    pinocchio::forwardKinematics(model, data, q0);
    pinocchio::updateFramePlacements(model, data);

    auto state = make_shared<StateMultibodyTpl<double>>(make_shared<pinocchio::ModelTpl<double>>(model));

    // We define an actuation model
    auto actuation = make_shared<ActuationModelFloatingBaseTpl<double>>(state);

    ContactModelMultiple contact_model(state, actuation->get_nu());

    contact_model.addContact("left_contact",
                             make_shared<ContactModel6D>(state, model.getFrameId("left_sole_link"),
                                                         pinocchio::SE3Tpl<double>::Identity(), actuation->get_nu()));

    contact_model.addContact("right_contact",
                             make_shared<ContactModel6D>(state, model.getFrameId("right_sole_link"),
                                                         pinocchio::SE3Tpl<double>::Identity(), actuation->get_nu()));

    auto x0 = state->zero();
    x0.head(q0.size()) = q0;

    Eigen::VectorXd weights(64);
    auto x_residual = make_shared<ResidualModelStateTpl<double>>(state, x0, actuation->get_nu());
    auto x_activation = make_shared<ActivationModelWeightedQuadTpl<double>>(weights);
    auto x_reg_cost = make_shared<CostModelResidualTpl<double>>(state, x_activation, x_residual);

    auto u_residual = make_shared<ResidualModelControlTpl<double>>(state, actuation->get_nu());

    // Cost for state and control
    // xResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
    // xActivation = ActivationModelWeightedQuad(
    //     np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) ** 2
    // )
    // uResidual = ResidualModelControl(state, actuation.nu)
    // xTActivation = ActivationModelWeightedQuad(
    //     np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) ** 2
    // )
    // xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
    // uRegCost = crocoddyl.CostModelResidual(state, uResidual)
    // xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual)
}

// # Create data structures
// rdata = robot.model.createData()
// state = crocoddyl.StateMultibody(robot.model)
// actuation = crocoddyl.ActuationModelFloatingBase(state)

// # Set integration time
// DT = 5e-2
// T = 60
// target = np.array([0.5, 0, 1.8])

// # Initialize reference state, target and reference CoM
// rightFoot = "right_sole_link"
// leftFoot = "left_sole_link"
// endEffector = "gripper_left_joint"
// endEffectorId = robot.model.getFrameId(endEffector)
// rightFootId = robot.model.getFrameId(rightFoot)
// leftFootId = robot.model.getFrameId(leftFoot)
// q0 = robot.model.referenceConfigurations["half_sitting"]
// x0 = np.concatenate([q0, np.zeros(robot.model.nv)])
// pinocchio.forwardKinematics(robot.model, rdata, q0)
// pinocchio.updateFramePlacements(robot.model, rdata)

// # Cost for self-collision
// maxfloat = sys.float_info.max
// xlb = np.concatenate(
//     [
//         -maxfloat * np.ones(6),  # dimension of the SE(3) manifold
//         robot.model.lowerPositionLimit[7:],
//         -maxfloat * np.ones(state.nv),
//     ]
// )
// xub = np.concatenate(
//     [
//         maxfloat * np.ones(6),  # dimension of the SE(3) manifold
//         robot.model.upperPositionLimit[7:],
//         maxfloat * np.ones(state.nv),
//     ]
// )
// bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
// xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
// xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
// limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)

// # Cost for state and control
// xResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
// xActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) ** 2
// )
// uResidual = crocoddyl.ResidualModelControl(state, actuation.nu)
// xTActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) ** 2
// )
// xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
// uRegCost = crocoddyl.CostModelResidual(state, uResidual)
// xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual)

// # Cost for target reaching
// framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
//     state, endEffectorId, pinocchio.SE3(np.eye(3), target), actuation.nu
// )
// framePlacementActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([1] * 3 + [0.0001] * 3) ** 2
// )
// goalTrackingCost = crocoddyl.CostModelResidual(
//     state, framePlacementActivation, framePlacementResidual
// )

// # Create cost model per each action model
// runningCostModel = crocoddyl.CostModelSum(state, actuation.nu)
// terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu)

// # Then let's added the running and terminal cost functions
// runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
// runningCostModel.addCost("stateReg", xRegCost, 1e-3)
// runningCostModel.addCost("ctrlReg", uRegCost, 1e-4)
// runningCostModel.addCost("limitCost", limitCost, 1e3)

// terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
// terminalCostModel.addCost("stateReg", xRegTermCost, 1e-3)
// terminalCostModel.addCost("limitCost", limitCost, 1e3)

// # Create the action model
// dmodelRunningLeft = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelLeft, runningCostModel
// )
// dmodelTerminalLeft = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelLeft, terminalCostModel
// )

// dmodelRunning = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelRight, runningCostModel
// )
// dmodelTerminal = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelRight, terminalCostModel
// )
// runningModelLeft = crocoddyl.IntegratedActionModelEuler(dmodelRunningLeft, DT)
// runningModel = crocoddyl.IntegratedActionModelEuler(dmodelRunning, DT)
// terminalModel = crocoddyl.IntegratedActionModelEuler(dmodelTerminal, 0)

// # Problem definition
// x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
// problem = crocoddyl.ShootingProblem(x0, [runningModel] * T + [runningModelLeft] * T, terminalModel)

// # Creating the DDP solver for this OC problem, defining a logger
// solver = crocoddyl.SolverFDDP(problem)
// solver.setCallbacks(
//     [
//         crocoddyl.CallbackVerbose(),
//         crocoddyl.CallbackDisplay(
//             crocoddyl.GepettoDisplay(robot, 4, 4, frameNames=[rightFoot, leftFoot])
//         ),
//     ]
// )

// # Solving it with the FDDP algorithm
// xs = [x0] * (solver.problem.T + 1)
// us = solver.problem.quasiStatic([x0] * solver.problem.T)
// solver.solve(xs, us, 500, False, 0.1)

// # Visualizing the solution in gepetto-viewer
// display.displayFromSolver(solver)

// # Get final state and end effector position
// xT = solver.xs[-1]
// pinocchio.forwardKinematics(robot.model, rdata, xT[: state.nq])
// pinocchio.updateFramePlacements(robot.model, rdata)
// com = pinocchio.centerOfMass(robot.model, rdata, xT[: state.nq])
// finalPosEff = np.array(rdata.oMf[robot.model.getFrameId("gripper_left_joint")].translation.T.flat)

// print("Finally reached = ", finalPosEff)
// print("Distance between hand and target = ", np.linalg.norm(finalPosEff - target))
// print("Distance to default state = ", np.linalg.norm(x0 - np.array(xT.flat)))
// # print("XY distance to CoM reference = ", np.linalg.norm(com[:2] - comRef[:2]))


// nonfinite_facet_sstream.cpp

// 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)

// Copyright (c) 2006 Johan Rade
// Copyright (c) 2011 Paul A. Bristow

/*!
\file
\brief Examples of nonfinite with output and input facets and stringstreams.

\detail Contruct a new locale with the nonfinite_num_put and nonfinite_num_get
facets and imbue istringstream, ostringstream and stringstreams,
showing output and input (and loopback for the stringstream).

*/

#include <boost/math/special_functions/nonfinite_num_facets.hpp>
using boost::math::nonfinite_num_put;
using boost::math::nonfinite_num_get;

using boost::math::legacy;

#include <iostream>
using std::cout;
using std::endl;
#include <locale>
using std::locale;

#include <sstream>
using std::stringstream;
using std::istringstream;
using std::ostringstream;

#include <limits>
using std::numeric_limits;

#include <assert.h>

int main()
{
  //[nonfinite_facets_sstream_1
  locale old_locale;
  locale tmp_locale(old_locale, new nonfinite_num_put<char>);
  locale new_locale(tmp_locale, new nonfinite_num_get<char>);
  //] [/nonfinite_facets_sstream_1]

  // Note that to add two facets,  nonfinite_num_put and nonfinite_num_get,
  // you have to add one at a time, using a temporary locale.

  {
    ostringstream oss;
    oss.imbue(new_locale);
    double inf = numeric_limits<double>::infinity();
    oss << inf; // Write out.
    cout << "infinity output was " << oss.str() << endl;
    BOOST_ASSERT(oss.str() == "inf");
  }
  {
    istringstream iss;
    iss.str("inf");
    iss.imbue(new_locale);
    double inf;
    iss >> inf; // Read from "inf"
    cout << "Infinity input was " << iss.str() << endl;
    BOOST_ASSERT(inf == numeric_limits<double>::infinity());
  }

  {
    //[nonfinite_facets_sstream_2
    stringstream ss;
    ss.imbue(new_locale);
    double inf = numeric_limits<double>::infinity();
    ss << inf; // Write out.
    BOOST_ASSERT(ss.str() == "inf");
    double r;
    ss >> r; // Read back in.
    BOOST_ASSERT(inf == r); // Confirms that the double values really are identical.

    cout << "infinity output was " << ss.str() << endl;
    cout << "infinity input was " << r << endl;
    // But the string representation of r displayed will be the native type
    // because, when it was constructed, cout had NOT been imbued
    // with the new locale containing the nonfinite_numput facet.
    // So the cout output will be "1.#INF on MS platforms
    // and may be "inf" or other string representation on other platforms.

    //] [/nonfinite_facets_sstream_2]
  }

  {
    stringstream ss;
    ss.imbue(new_locale);

    double nan = numeric_limits<double>::quiet_NaN();
    ss << nan; // Write out.
    BOOST_ASSERT(ss.str() == "nan");

    double v;
    ss >> v; // Read back in.

    cout << "NaN output was " << ss.str() << endl;
    cout << "NaN input was " << v << endl;

    // assert(nan == v); // Always fails because NaN == NaN fails!
    // assert(nan == numeric_limits<double>::quiet_NaN()); asserts!

    // And the string representation will be the native type
    // because cout has NOT been imbued with a locale containing
    // the nonfinite_numput facet.
    // So the output will be "1.#QNAN on MS platforms
    // and may be "nan" or other string representation on other platforms.
  }

} // int main()

/*
//[nonfinite_facet_sstream_output

infinity output was inf
Infinity input was inf
infinity output was inf
infinity input was 1.#INF
NaN output was nan
NaN input was 1.#QNAN

//] [nonfinite_facet_sstream_output]
*/


#include "potgen.hpp"
#include "vector.hpp"
#include "interpolation.hpp"
#include "correlation.hpp"

#include <boost/test/unit_test.hpp>

/// \todo check that PGOptions cor_fun is valid

// helper function
static Potential generatePotential( int N,  std::size_t size, const PGOptions& opt )
{
	std::vector<std::size_t> sizes(N, size);
	return generatePotential( sizes, std::vector<double>(N, 1.0), opt);
}

BOOST_AUTO_TEST_SUITE(potgen_fn_test)

BOOST_AUTO_TEST_CASE( check_potgen_seed_determinism )
{
	PGOptions opt;
	opt.randomSeed = rand();
	opt.cor_fun = makeGaussianCorrelation(0.01);
	auto result1 = generatePotential(2, 128, opt);
	auto result2 = generatePotential(2, 128, opt);

	BOOST_CHECK_EQUAL( opt.randomSeed, result1.getSeed() );

	for(unsigned int i = 0; i < result1.getPotential().size(); ++i)
	{
		BOOST_CHECK_EQUAL( result1.getPotential()[i], result2.getPotential()[i] );
	}
}

BOOST_AUTO_TEST_CASE( check_potgen_randomization )
{
	PGOptions opt;
	opt.randomize = false;
	opt.cor_fun = makeGaussianCorrelation(0.01);
	auto result1 = generatePotential(1, 128, opt);
	// check that randomization of phases is disabled. howto?
}

BOOST_AUTO_TEST_CASE( deriv_generation )
{
	PGOptions opt;
	opt.cor_fun = makeGaussianCorrelation(0.01);
	opt.randomize = false;
	opt.maxDerivativeOrder = 2;
	auto p = generatePotential(2, 128, opt);

	// if not present, this generates segfaults
	for(MultiIndex idx(2, 0, 3); idx.valid(); ++idx)
	{
		// only check those of order <= 2!
		if( idx[0] + idx[1] <= 2 )
			BOOST_CHECK( p.hasDerivative( idx ) );
	}
}

/*! \todo check invariants:
		*	check resulting correlation function [approximate check?]
*/
BOOST_AUTO_TEST_SUITE_END()


#include <boost/graph/grid_graph.hpp>


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <Core/Algorithms/Legacy/FiniteElements/Mapping/BuildNodeLink.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/SparseRowMatrixFromMap.h>
#include <Core/Datatypes/MatrixTypeConversions.h>

#include <Core/Math/MiscMath.h>
#include <boost/scoped_array.hpp>

using namespace SCIRun;
using namespace SCIRun::Core::Geometry;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Thread;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::FiniteElements;
using namespace SCIRun::Core::Logging;

bool
BuildNodeLinkAlgo::run(MatrixHandle nodeDomain, MatrixHandle& nodeLink) const
{
  ScopedAlgorithmStatusReporter asr(this, "BuildNodeLink");

  if (!nodeDomain)
  {
    error("No matrix on input");
    return (false);
  }

  if (nodeDomain->ncols() != 1)
  {
    error("NodeDomain needs to be a column vector");
    return (false);
  }

  DenseColumnMatrixHandle nodeDomainCol = convertMatrix::toColumn(nodeDomain);

  size_type num_values = nodeDomainCol->nrows();

  double min2 = -std::numeric_limits<double>::max();
  double min = std::numeric_limits<double>::max();

  bool found_domain = false;

  LegacySparseDataContainer<double> sparseData(num_values+1, num_values, 0);
  auto rr = sparseData.rows();
  auto cc = sparseData.columns();
  size_type nnz = 0;

  if (!rr || !cc)
  {
    error("Could not allocate memory for sparse matrix");
    return (false);
  }

  for (index_type j = 0; j<num_values; j++) cc[j] = -1;

  do
  {
    found_domain = false;
    min = std::numeric_limits<double>::max();

    for (index_type j = 0; j<num_values; j++)
    {
      double val = (*nodeDomainCol)(j);
      if (IsFinite(val))
      {
        if (val < min && val > min2 )
        {
          found_domain = true;
          min = val;
        }
      }
    }

    if (found_domain)
    {
      min2 = min;
      index_type idx = -1;
      for (index_type j = 0; j<num_values; j++)
      {
        double val = (*nodeDomainCol)(j);
        if (val == min2)
        {
          if (idx < 0) idx = j;
          else { cc[j] = idx; nnz++; }
        }
      }
    }
  }
  while (found_domain);

  sparseData.allocateData(nnz);
  auto aa = sparseData.data();

  if (!aa)
  {
    error("Could not allocate memory for sparse matrix");
    return (false);
  }

  index_type k = 0;
  rr[0] = 0;
  for (index_type j = 0; j<num_values; j++)
  {
    if (cc[j] > -1 )
    {
      cc[k] = cc[j];
      aa[k] = 1.0;
      k++;
    }
    rr[j+1] = k;
  }

  SparseRowMatrix nodeLinkHalf(num_values, num_values, rr.get(), cc.get(), aa.get(), nnz);
  SparseRowMatrix nodeLinkHalfT(nodeLinkHalf.transpose());
  nodeLink.reset(new SparseRowMatrix(nodeLinkHalf + nodeLinkHalfT));
  return (true);
}

AlgorithmOutput BuildNodeLinkAlgo::run(const AlgorithmInput &) const
{
  throw "not implemented";
}


#ifndef PYTHONIC_INCLUDE_NUMPY_EXP_HPP
#define PYTHONIC_INCLUDE_NUMPY_EXP_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/exp.hpp>

namespace pythonic
{

  namespace numpy
  {
#define NUMPY_NARY_FUNC_NAME exp
#define NUMPY_NARY_FUNC_SYM boost::simd::exp
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


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

#include <boost/hana/ext/boost/mpl/integral_c.hpp>

#include <boost/hana/constant/laws.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/list/instance.hpp>

#include <boost/mpl/integral_c.hpp>
using namespace boost::hana;

template <typename T>
void test() {
    namespace mpl = boost::mpl;
    BOOST_HANA_CONSTANT_ASSERT(Constant::laws::check(
        list(
            mpl::integral_c<T, 0>{},
            mpl::integral_c<T, 1>{},
            mpl::integral_c<T, 2>{},
            mpl::integral_c<T, 3>{}
        )
    ));
}

int main() {
    test<int>();
    test<long>();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2015-2020 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2015, 2017, 2019.
// Modifications copyright (c) 2015-2019 Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIRECTION_CODE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIRECTION_CODE_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/arithmetic/infinite_line_functions.hpp>
#include <boost/geometry/algorithms/detail/make/make.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

#include <boost/mpl/assert.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename CSTag>
struct direction_code_impl
{
    BOOST_MPL_ASSERT_MSG((false), NOT_IMPLEMENTED_FOR_THIS_CS, (CSTag));
};

template <>
struct direction_code_impl<cartesian_tag>
{
    template <typename Point1, typename Point2>
    static inline int apply(Point1 const& segment_a, Point1 const& segment_b,
                            Point2 const& point)
    {
        typedef typename geometry::select_coordinate_type
            <
                Point1, Point2
            >::type calc_t;

        typedef model::infinite_line<calc_t> line_type;

        // Situation and construction of perpendicular line
        //
        //     P1     a--------------->b   P2
        //                             |
        //                             |
        //                             v
        //
        // P1 is located right of the (directional) perpendicular line
        // and therefore gets a negative side_value, and returns -1.
        // P2 is to the left of the perpendicular line and returns 1.
        // If the specified point is located on top of b, it returns 0.

        line_type const line
            = detail::make::make_perpendicular_line<calc_t>(segment_a,
                segment_b, segment_b);

        if (arithmetic::is_degenerate(line))
        {
            return 0;
        }

        calc_t const sv = arithmetic::side_value(line, point);
        return sv == 0 ? 0 : sv > 0 ? 1 : -1;
    }
};

template <>
struct direction_code_impl<spherical_equatorial_tag>
{
    template <typename Point1, typename Point2>
    static inline int apply(Point1 const& segment_a, Point1 const& segment_b,
                            Point2 const& p)
    {
        typedef typename coordinate_type<Point1>::type coord1_t;
        typedef typename coordinate_type<Point2>::type coord2_t;
        typedef typename cs_angular_units<Point1>::type units_t;
        typedef typename cs_angular_units<Point2>::type units2_t;
        BOOST_MPL_ASSERT_MSG((boost::is_same<units_t, units2_t>::value),
                             NOT_IMPLEMENTED_FOR_DIFFERENT_UNITS,
                             (units_t, units2_t));

        typedef typename geometry::select_coordinate_type <Point1, Point2>::type calc_t;
        typedef math::detail::constants_on_spheroid<coord1_t, units_t> constants1;
        typedef math::detail::constants_on_spheroid<coord2_t, units_t> constants2;
        static coord1_t const pi_half1 = constants1::max_latitude();
        static coord2_t const pi_half2 = constants2::max_latitude();
        static calc_t const c0 = 0;

        coord1_t const a0 = geometry::get<0>(segment_a);
        coord1_t const a1 = geometry::get<1>(segment_a);
        coord1_t const b0 = geometry::get<0>(segment_b);
        coord1_t const b1 = geometry::get<1>(segment_b);
        coord2_t const p0 = geometry::get<0>(p);
        coord2_t const p1 = geometry::get<1>(p);

        if ( (math::equals(b0, a0) && math::equals(b1, a1))
          || (math::equals(b0, p0) && math::equals(b1, p1)) )
        {
            return 0;
        }

        bool const is_a_pole = math::equals(pi_half1, math::abs(a1));
        bool const is_b_pole = math::equals(pi_half1, math::abs(b1));
        bool const is_p_pole = math::equals(pi_half2, math::abs(p1));

        if ( is_b_pole && ((is_a_pole && math::sign(b1) == math::sign(a1))
                        || (is_p_pole && math::sign(b1) == math::sign(p1))) )
        {
            return 0;
        }

        // NOTE: as opposed to the implementation for cartesian CS
        // here point b is the origin

        calc_t const dlon1 = math::longitude_distance_signed<units_t, calc_t>(b0, a0);
        calc_t const dlon2 = math::longitude_distance_signed<units_t, calc_t>(b0, p0);

        bool is_antilon1 = false, is_antilon2 = false;
        calc_t const dlat1 = latitude_distance_signed<units_t, calc_t>(b1, a1, dlon1, is_antilon1);
        calc_t const dlat2 = latitude_distance_signed<units_t, calc_t>(b1, p1, dlon2, is_antilon2);

        calc_t mx = is_a_pole || is_b_pole || is_p_pole ?
                    c0 :
                    (std::min)(is_antilon1 ? c0 : math::abs(dlon1),
                               is_antilon2 ? c0 : math::abs(dlon2));
        calc_t my = (std::min)(math::abs(dlat1),
                               math::abs(dlat2));

        int s1 = 0, s2 = 0;
        if (mx >= my)
        {
            s1 = dlon1 > 0 ? 1 : -1;
            s2 = dlon2 > 0 ? 1 : -1;
        }
        else
        {
            s1 = dlat1 > 0 ? 1 : -1;
            s2 = dlat2 > 0 ? 1 : -1;
        }

        return s1 == s2 ? -1 : 1;
    }

    template <typename Units, typename T>
    static inline T latitude_distance_signed(T const& lat1, T const& lat2, T const& lon_ds, bool & is_antilon)
    {
        typedef math::detail::constants_on_spheroid<T, Units> constants;
        static T const pi = constants::half_period();
        static T const c0 = 0;

        T res = lat2 - lat1;

        is_antilon = math::equals(math::abs(lon_ds), pi);
        if (is_antilon)
        {
            res = lat2 + lat1;
            if (res >= c0)
                res = pi - res;
            else
                res = -pi - res;
        }

        return res;
    }
};

template <>
struct direction_code_impl<spherical_polar_tag>
{
    template <typename Point1, typename Point2>
    static inline int apply(Point1 segment_a, Point1 segment_b,
                            Point2 p)
    {
        typedef math::detail::constants_on_spheroid
            <
                typename coordinate_type<Point1>::type,
                typename cs_angular_units<Point1>::type
            > constants1;
        typedef math::detail::constants_on_spheroid
            <
                typename coordinate_type<Point2>::type,
                typename cs_angular_units<Point2>::type
            > constants2;

        geometry::set<1>(segment_a,
            constants1::max_latitude() - geometry::get<1>(segment_a));
        geometry::set<1>(segment_b,
            constants1::max_latitude() - geometry::get<1>(segment_b));
        geometry::set<1>(p,
            constants2::max_latitude() - geometry::get<1>(p));

        return direction_code_impl
                <
                    spherical_equatorial_tag
                >::apply(segment_a, segment_b, p);
    }
};

// if spherical_tag is passed then pick cs_tag based on Point1 type
// with spherical_equatorial_tag as the default
template <>
struct direction_code_impl<spherical_tag>
{
    template <typename Point1, typename Point2>
    static inline int apply(Point1 segment_a, Point1 segment_b,
                            Point2 p)
    {
        return direction_code_impl
            <
                typename boost::mpl::if_c
                    <
                        boost::is_same
                            <
                                typename geometry::cs_tag<Point1>::type,
                                spherical_polar_tag
                            >::value,
                        spherical_polar_tag,
                        spherical_equatorial_tag
                    >::type
            >::apply(segment_a, segment_b, p);
    }
};

template <>
struct direction_code_impl<geographic_tag>
    : direction_code_impl<spherical_equatorial_tag>
{};

// Gives sense of direction for point p, collinear w.r.t. segment (a,b)
// Returns -1 if p goes backward w.r.t (a,b), so goes from b in direction of a
// Returns 1 if p goes forward, so extends (a,b)
// Returns 0 if p is equal with b, or if (a,b) is degenerate
// Note that it does not do any collinearity test, that should be done before
template <typename CSTag, typename Point1, typename Point2>
inline int direction_code(Point1 const& segment_a, Point1 const& segment_b,
                          Point2 const& p)
{
    return direction_code_impl<CSTag>::apply(segment_a, segment_b, p);
}

} // namespace detail
#endif //DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIRECTION_CODE_HPP


/**
 * \file      kinetics-observer.hpp
 * \author    Mehdi Benallegue
 * \date      2018
 * \brief     Unified Kinetics estimator
 *
 * \details
 *
 *
 */

#ifndef KINETICSOBSERVER_HPP
#define KINETICSOBSERVER_HPP

#include <map>
#include <set>

#include <boost/utility.hpp>

#include <state-observation/api.h>
#include <state-observation/dynamical-system/dynamical-system-functor-base.hpp>
#include <state-observation/noise/noise-base.hpp>
#include <state-observation/observer/extended-kalman-filter.hpp>
#include <state-observation/sensors-simulation/accelerometer-gyrometer.hpp>
#include <state-observation/tools/definitions.hpp>
#include <state-observation/tools/rigid-body-kinematics.hpp>
#include <state-observation/tools/state-vector-arithmetics.hpp>

namespace stateObservation
{

/**
 * \class  KineticsObserver
 * \brief
 *
 */

class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunctorBase, protected StateVectorArithmetics
{
public:
  typedef kine::Kinematics Kinematics;
  typedef kine::Orientation Orientation;

  /// The constructor.
  ///  \li maxContacts : maximum number of contacts,
  ///  \li maxNumberOfIMU : the maximumnumber of IMUs
  KineticsObserver(unsigned maxContacts = 4, unsigned maxNumberOfIMU = 1);

  /// virtual destructor
  virtual ~KineticsObserver();

  /// Gets the state size
  Index getStateSize() const;

  /// Gets the measurement size
  Index getMeasurementSize() const;

  /// ///////////////////////////////////////////////////////////
  /// Getting, setting the current time and running the estimation
  /// //////////////////////////////////////////////////////////

  /// gets the sampling time
  double getSamplingTime() const;

  /// sets the sampling time
  void setSamplingTime(double);

  /// this function triggers the estimation itself
  Vector update();

  /// ////////////////////////////////
  /// Getting and setting the state
  /// ////////////////////////////////

  /// Gets an estimation of the state in the form of a state vector $\hat{x_{k+1}}$
  Vector getStateVector() const;

  /// Gets the internal sample time of the state vector
  stateObservation::TimeIndex getStateVectorSampleTime() const;

  /// Get the Kinematics of the observed frams
  Kinematics getKinematics() const;

  /// Get the kinematics of a given frame
  Kinematics getKinematicsOf(const Kinematics & localKinematics) const;
  Kinematics getKinematicsOf(const Kinematics & localKinematics);
  Kinematics getKinematicsOf(Kinematics & localKinematics) const;
  Kinematics getKinematicsOf(Kinematics & localKinematics);

  /// get the contact force provided by the estimator
  /// which is different from a contact sensor measurement
  Vector6 getContactWrench(int contactNbr) const;
  Kinematics getContactPosition(int contactNbr) const;

  /// gets the external unmodeled forces
  Vector6 getUnmodeledWrench() const;

  /// This function allows to estimate the acceleration
  /// it returns a Kinemactis
  Kinematics estimateAccelerations();

  /// Sets a value for the kinematics part of the state
  /// if resetForces is set to true the forces are set to zero
  ///
  void setStateKinematics(const Kinematics &, bool resetContactWrenches = true, bool resetCovariance = true);

  /// Allows to initializa the value of the gyro bias of the IMU
  /// corresponding to the numberOfIMU
  /// reset Covariance allows to reinitialize the gyro bias
  void setGyroBias(const Vector3 &, unsigned numberOfIMU = 1, bool resetCovariance = true);

  /// if only force or torque is available, set the unavailable value to zero
  void setStateUnmodeledWrench(const Vector6 &, bool resetCovariance = true);

  /// Sets a value of the state x_k provided from another source
  /// can be used for initialization of the estimator
  void setStateVector(const Vector &, bool resetCovariance = true);

  /// Getters for the indexes of the state Vector
  inline unsigned kineIndex() const;
  inline unsigned posIndex() const;
  inline unsigned oriIndex() const;
  inline unsigned linVelIndex() const;
  inline unsigned angVelIndex() const;
  inline unsigned gyroBiasIndex(unsigned IMUNumber) const;
  inline unsigned unmodeledWrenchIndex() const;
  inline unsigned unmodeledForceIndex() const;
  inline unsigned unmodeledTorqueIndex() const;
  inline unsigned contactsIndex() const;
  inline unsigned contactIndex(unsigned contactNbr) const;
  inline unsigned contactKineIndex(unsigned contactNbr) const;
  inline unsigned contactPosIndex(unsigned contactNbr) const;
  inline unsigned contactOriIndex(unsigned contactNbr) const;
  inline unsigned contactForceIndex(unsigned contactNbr) const;
  inline unsigned contactTorqueIndex(unsigned contactNbr) const;
  inline unsigned contactWrenchIndex(unsigned contactNbr) const;

  /// Getters for the indexes of the state Vector
  inline unsigned kineIndexTangent() const;
  inline unsigned posIndexTangent() const;
  inline unsigned oriIndexTangent() const;
  inline unsigned linVelIndexTangent() const;
  inline unsigned angVelIndexTangent() const;
  inline unsigned gyroBiasIndexTangent(unsigned IMUNumber) const;
  inline unsigned unmodeledWrenchIndexTangent() const;
  inline unsigned unmodeledForceIndexTangent() const;
  inline unsigned unmodeledTorqueIndexTangent() const;
  inline unsigned contactsIndexTangent() const;
  inline unsigned contactIndexTangent(unsigned contactNbr) const;
  inline unsigned contactKineIndexTangent(unsigned contactNbr) const;
  inline unsigned contactPosIndexTangent(unsigned contactNbr) const;
  inline unsigned contactOriIndexTangent(unsigned contactNbr) const;
  inline unsigned contactForceIndexTangent(unsigned contactNbr) const;
  inline unsigned contactTorqueIndexTangent(unsigned contactNbr) const;
  inline unsigned contactWrenchIndexTangent(unsigned contactNbr) const;

  /// /////////////////////////////////////////////////////
  /// Setting and getting the state of the estimation
  /// ////////////////////////////////////////////////////
  void setWithUnmodeledWrench(bool b = true);

  /// Sets if the update() function estimates also the accelerations
  void setWithAccelerationEstimation(bool b = true);

  void setWithGyroBias(bool b = true);

  /// ///////////////////////////////////////////////
  /// Getting and setting input data and measurements
  /// /////////////////////////////////////////////

  /// sets the measurement of the IMU (gyrometer, accelerometer, kinematics and number of the IMU)
  /// accelero and gyrometer are the measurement
  /// localKine gets the kinematics of the IMU expressed in the observed frame
  /// the best is to provide the position, the orientation,
  /// the angular and linear velocities and the linear acceleration
  /// Nevertheless if velocities or accelerations are not available they will be
  /// automatically computed through finite differences
  /// the acceleroCov and the gyroCov are the covariance matrices of the sensors
  int setIMU(const Vector3 & accelero, const Vector3 & gyrometer, const Kinematics & localKine, int num = -1);
  int setIMU(const Vector3 & accelero,
             const Vector3 & gyrometer,
             const Matrix3 & acceleroCov,
             const Matrix3 gyroCov,
             const Kinematics & localKine,
             int num = -1);

  void setIMUDefaultCovarianceMatrix(const Matrix3 & acceleroCov, const Matrix3 & gyroCov);

  ////////// Contact stters, these are MANDATORY for every contact at every iteration ///
  /// if the contact is equipped with wrench sensor call setContactWrenchSensor
  /// otherwise calls etContactWithoutSensor

  /// sets the measurements of a force/torque sensor of the contact numbered contactNumber
  /// wrenchMeasurement is the measurment vector composed with 3D forces and 3D torques
  /// localKine sets the kinematics of the contact expressed in the observed frame
  /// the best is to provide the position , the orientation,
  /// the angular and the linear velocities.

  void setContactWrenchSensor(const Vector6 & wrenchMeasurement, const Kinematics & localKine, unsigned contactNumber);
  void setContactWrenchSensor(const Vector6 & wrenchMeasurement,
                              const Matrix6 & wrenchCovMatrix,
                              const Kinematics & localKine,
                              unsigned contactNumber);

  void setContactWrenchSensorDefaultCovarianceMatrix(const Matrix6 & wrenchSensorCovMat);

  /// sets the the kinematics of the contact expressed in the observed frame
  /// the best is to provide the position, the orientation, the angular and the linear velocities.
  /// otherwise they will be automatically computed
  void setContactWithNoSensor(const Kinematics & localKine, unsigned contactNumber);

  /// Set a measurement of the pose. The input is the Measured kinematics
  /// namely position and orientation.
  void setAbsolutePoseSensor(const Kinematics &);
  void setAbsolutePoseSensor(const Kinematics &, const Matrix6 & CovarianceMatrix);

  /// TODO
  // void setVelocityGuess(const Kinematics)

  void setAbsolutePoseSensorDefaultCovarianceMatrix(const Matrix6 &);

  ///////////////////////////////////////////////
  /// Setting inputs to the dynamical system
  //////////////////////////////////////////////////
  /// Add known external forces and moments which are not due to contact
  /// they must be expressed in the same frame as the kinematic root
  void setAdditionalWrench(const Vector3 & force, const Vector3 & torque);

  /// sets the mass of the robot
  void setMass(double);

  /// Sets the 3x3 inertia matrix expressed in the local frame and  optionally
  ///  its time derivative (computed with finite differences otherwise)
  /// the Vector6 version is a vector containing the diagonal and the three non
  /// diagonal values concatenated
  /// it is highly recommended to update these values at every iteration
  void setInertiaMatrix(const Matrix3 & I, const Matrix3 & I_dot);
  void setInertiaMatrix(const Matrix3 & I);
  void setInertiaMatrix(const Vector6 & I, const Vector6 & I_dot);
  void setInertiaMatrix(const Vector6 & I);

  /// Sets the center of mass position expressed in the local frame
  /// and optionally its first and second order time derivarives
  /// computed through finite differences otherwise.
  /// it is highly recommended to update these values at every iteration
  void setCenterOfMass(const Vector3 & com, const Vector3 & com_dot, const Vector3 & com_dot_dot);
  void setCenterOfMass(const Vector3 & com, const Vector3 & com_dot);
  void setCenterOfMass(const Vector3 & com);

  /// Sets the angular momentum expressed in the local frame
  /// and optionally its time derivarive
  /// computed through finite differences otherwise.
  /// it is highly recommended to update these values at every iteration
  void setAngularMomentum(const Vector3 & sigma, const Vector3 & sigma_dot);
  void setAngularMomentum(const Vector3 & sigma);

  ///////////////////////////////
  /// Contact management
  ///////////////////////////////
  /// Set a new contact
  /// -pose is the initial guess on the position of the contact. Only position
  /// and orientation are enough
  /// -initialCovarianceMatrix is the covariance matrix expressing the
  ///  uncertainty of the initial guess (if no initial guess is available
  ///  give a rough position with a high initial covariance matrix)
  /// -processCovarianceMatrix is the covariance matrix expressing the
  ///  rate at which the contact slides (set to zero for no sliding)
  /// -linear, angular stiffness and damping set the flexibility model
  ///  of the contact
  /// set contactNumber to -1 in order to set the number automatically
  /// returns the number of this contact
  int addContact(const Kinematics & pose,
                 const Matrix12 & initialCovarianceMatrix,
                 const Matrix12 & processCovarianceMatrix,
                 const Matrix3 & linearStiffness,
                 const Matrix3 & linearDamping,
                 const Matrix3 & angularStiffness,
                 const Matrix3 & angularDamping,
                 int contactNumber = -1);
  /// version with default stiffness and damping
  /// use when the contact parameters are known
  int addContact(const Kinematics & pose,
                 const Matrix12 & initialCovarianceMatrix,
                 const Matrix12 & processCovarianceMatrix,
                 int contactNumber = -1);
  /// version when the contact position is perfectly known
  int addContact(const Kinematics & pose,
                 const Matrix3 & linearStiffness,
                 const Matrix3 & linearDamping,
                 const Matrix3 & angularStiffness,
                 const Matrix3 & angularDamping,
                 int contactNumber = -1);
  /// version when the position is perfectly known but not the stiffness and damping
  int addContact(const Kinematics & pose, int contactNumber = -1);

  void removeContact(int contactnbr);

  void clearContacts();

  Index getNumberOfContacts() const;

  std::vector<int> getListOfContacts() const;

  /// Sets the covariance matrix of the flexibility Guess
  void setStateCovariance(const Matrix & P);

  void setKinematicsStateCovariance(const Matrix &);
  void setKinematicsInitCovarianceDefault(const Matrix &);
  void setKinematicsProcessCovariance(const Matrix &);

  void setGyroBiasStateCovariance(const Matrix3 & covMat, unsigned imuNumber);
  void setGyroBiasInitCovarianceDefault(const Matrix3 & covMat);
  void setGyroBiasProcessCovariance(const Matrix3 & covMat, unsigned imuNumber);

  void setUnmodeledWrenchStateCovMat(const Matrix6 & currentCovMat);
  void setUnmodeledWrenchIniCovMatDefault(const Matrix6 & initCovMat);
  void setUnmodeledWrenchProcessCovMat(const Matrix6 & processCovMat);

  void setContactStateCovMat(int contactNbr, const Matrix12 & contactCovMat);
  void setContactInitCovMatDefault(const Matrix12 & contactCovMat);
  void setContactProcessCovMat(int contactNbr, const Matrix12 & contactCovMat);

  /// Gets the covariance matrix of the flexibility
  Matrix getStateCovariance() const;

  /// Sets/gets the covariance matrices for the process noises
  /// \li Q process noise
  void setProcessNoiseCovariance(const Matrix & Q);

  /// gets the measurement vector
  Vector getMeasurementVector();

  /// Gets a const reference on the extended Kalman filter
  const ExtendedKalmanFilter & getEKF() const;

  /// Gets a reference on the extended Kalman filter
  /// modifying this object may lead to instabilities
  ExtendedKalmanFilter & getEKF();

  /// Resets the covariance matrices to their original values
  void resetStateCovarianceMat();
  void resetStateKinematicsCovMat();
  void resetStateGyroBiasCovMat(unsigned i);
  void resetStateUnmodeledWrenchCovMat();
  void resetStateContactsCovMat();
  void resetStateContactCovMat(unsigned contactNbr);

  void resetProcessCovarianceMat();
  void resetProcessKinematicsCovMat();
  void resetProcessGyroBiasCovMat(unsigned i);
  void resetProcessUnmodeledWrenchCovMat();
  void resetProcessContactsCovMat();
  void resetProcessContactCovMat(unsigned contactNbr);

  /// Reset the default values for the covariance matrix
  void resetSensorsDefaultCovMat();

  /// to reset all the sensor inputs and provided contact positions but keeps the contacts
  void resetInputs();

protected:
  struct Sensor
  {
    Sensor(int signalSize) : size(signalSize), time(0) {}
    virtual ~Sensor() {}
    int measIndex;
    int size;
    TimeIndex time;

    inline Vector extractFromVector(const Vector & v)
    {
      return v.segment(size, measIndex);
    }
  };

  struct IMU : public Sensor
  {
    virtual ~IMU() {}
    IMU() : Sensor(sizeIMUSignal) {}
    Kinematics kinematics;
    Vector6 acceleroGyro;
    Matrix3 covMatrixAccelero;
    Matrix3 covMatrixGyro;

    static int currentNumber;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  typedef std::vector<IMU, Eigen::aligned_allocator<IMU>> VectorIMU;
  typedef VectorIMU::iterator VectorIMUIterator;
  typedef VectorIMU::const_iterator VectorIMUConstIterator;

  struct Contact : public Sensor
  {
    Contact() : Sensor(sizeWrench), isSet(false), withRealSensor(false), stateIndex(-1), stateIndexTangent(-1) {}
    virtual ~Contact() {}

    Kinematics absPose;
    Vector6 wrench;
    CheckedMatrix6 sensorCovMatrix;

    Matrix3 linearStiffness;
    Matrix3 linearDamping;
    Matrix3 angularStiffness;
    Matrix3 angularDamping;

    bool isSet;
    bool withRealSensor;
    int stateIndex;
    int stateIndexTangent;

    Kinematics localKine; /// describes the kinematics of the contact point in the local frame
    static const Kinematics::Flags::Byte localKineFlags = /// flags for the components of the kinematics
        Kinematics::Flags::position | Kinematics::Flags::orientation | Kinematics::Flags::linVel
        | Kinematics::Flags::angVel;

    static int numberOfRealSensors;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  typedef std::vector<Contact, Eigen::aligned_allocator<Contact>> VectorContact;
  typedef VectorContact::iterator VectorContactIterator;
  typedef VectorContact::const_iterator VectorContactConstIterator;

  struct AbsolutePoseSensor : public Sensor
  {
    AbsolutePoseSensor() : Sensor(sizePose) {}

    Kinematics pose;
    CheckedMatrix6 covMatrix;
  };

protected:
  ///////////// DYNAMICAL SYSTEM IMPLEMENTATION
  virtual Vector stateDynamics(const Vector & x, const Vector & u, TimeIndex k);

  virtual Vector measureDynamics(const Vector & x, const Vector & u, TimeIndex k);

  void addUnmodeledAndContactWrench_(const Vector & stateVector, Vector3 & force, Vector3 & torque);

  void computeAccelerations_(Kinematics & stateKine,
                             const Vector3 & totalForceLocal,
                             const Vector3 & totalMomentLocal,
                             Vector3 & linAcc,
                             Vector3 & angAcc);

  /// the kinematics is not const to allow more optimized non const operators to work
  void computeContactForces_(VectorContactIterator i,
                             Kinematics & stateKine,
                             Kinematics & contactPose,
                             Vector3 & Force,
                             Vector3 torque);

  /// Sets a noise which disturbs the state dynamics
  virtual void setProcessNoise(NoiseBase *);

  /// Removes the process noise
  virtual void resetProcessNoise();
  /// Gets the process noise
  virtual NoiseBase * getProcessNoise() const;

  /// Sets a noise which disturbs the measurements
  virtual void setMeasurementNoise(NoiseBase *);
  /// Removes the measurement noise
  virtual void resetMeasurementNoise();
  /// Gets a pointer on the measurement noise
  virtual NoiseBase * getMeasurementNoise() const;

  /// Gets the input size
  virtual Index getInputSize() const;

public:
  virtual void stateSum(const Vector & stateVector, const Vector & tangentVector, Vector & sum);
  inline Vector stateSum(const Vector & stateVector, const Vector & tangentVector);

  virtual void stateDifference(const Vector & stateVector1, const Vector & stateVector2, Vector & difference);
  inline Vector stateDifference(const Vector & stateVector1, const Vector & stateVector2);

  virtual void measurementDifference(const Vector & measureVector1, const Vector & measureVector2, Vector & difference);
  ///////////////////////////////////////

  ////////////////////////////
  virtual void setFiniteDifferenceStep(const Vector & dx);
  virtual void useFiniteDifferencesJacobians(bool b = true);

protected:
  Vector stateNaNCorrection_();

  /// updates stateKine_ from the stateVector
  void updateKine_();

protected:
  unsigned maxContacts_;
  unsigned maxImuNumber_;

  AbsolutePoseSensor absPoseSensor_;
  VectorContact contacts_;
  VectorIMU imuSensors_;

  Index stateSize_;
  Index stateTangentSize_;
  Index measurementSize_;
  Index measurementTangentSize_;

  Kinematics stateKinematics_;

  Vector stateVector_;
  Vector stateVectorDx_;
  Vector oldStateVector_;

  Vector3 additionalForce_;
  Vector3 additionalTorque_;

  Vector measurementVector_;
  Matrix measurementCovMatrix_;

  stateObservation::ExtendedKalmanFilter ekf_;
  bool finiteDifferencesJacobians_;
  bool withGyroBias_;
  bool withUnmodeledWrench_;
  bool withAccelerationEstimation_;

  IndexedVector3 com_, comd_, comdd_;
  IndexedVector3 sigma_, sigmad_;
  IndexedMatrix3 I_, Id_;

  TimeIndex k_est_;
  TimeIndex k_data_;

  double mass_;

  double dt_;

  NoiseBase * processNoise_;
  NoiseBase * measurementNoise_;

  /// function to call before adding any measurement
  /// detects if there is a new estimation beginning and then
  /// calls the reset of the iteration
  void startNewIteration_();

  virtual Matrix computeAMatrix_();
  virtual Matrix computeCMatrix_();

  /// Getters for the indexes of the state Vector using private types
  inline unsigned contactIndex(VectorContactConstIterator i) const;
  inline unsigned contactKineIndex(VectorContactConstIterator i) const;
  inline unsigned contactPosIndex(VectorContactConstIterator i) const;
  inline unsigned contactOriIndex(VectorContactConstIterator i) const;
  inline unsigned contactForceIndex(VectorContactConstIterator i) const;
  inline unsigned contactTorqueIndex(VectorContactConstIterator i) const;
  inline unsigned contactWrenchIndex(VectorContactConstIterator i) const;

  /// Getters for the indexes of the state Vector using private types
  inline unsigned contactIndexTangent(VectorContactConstIterator i) const;
  inline unsigned contactKineIndexTangent(VectorContactConstIterator i) const;
  inline unsigned contactPosIndexTangent(VectorContactConstIterator i) const;
  inline unsigned contactOriIndexTangent(VectorContactConstIterator i) const;
  inline unsigned contactForceIndexTangent(VectorContactConstIterator i) const;
  inline unsigned contactTorqueIndexTangent(VectorContactConstIterator i) const;
  inline unsigned contactWrenchIndexTangent(VectorContactConstIterator i) const;

private:
public:
  ///////////SIZE OF VECTORS
  static const unsigned sizeAcceleroSignal = 3;
  static const unsigned sizeGyroSignal = 3;
  static const unsigned sizeIMUSignal = sizeAcceleroSignal + sizeGyroSignal;

  static const unsigned sizePos = 3;
  static const unsigned sizeOri = 4;
  static const unsigned sizeOriTangent = 3;
  static const unsigned sizeLinVel = sizePos;
  static const unsigned sizeAngVel = sizeOriTangent;
  static const unsigned sizeGyroBias = sizeGyroSignal;

  static const unsigned sizeForce = 3;
  static const unsigned sizeTorque = 3;

  static const unsigned sizeWrench = sizeForce + sizeTorque;

  static const unsigned sizeStateKine = sizePos + sizeOri + sizeLinVel + sizeAngVel;
  static const unsigned sizeStateBase = sizeStateKine + sizeForce + sizeTorque;
  static const unsigned sizeStateKineTangent = sizePos + sizeOriTangent + sizeLinVel + sizeAngVel;
  static const unsigned sizeStateTangentBase = sizeStateKineTangent + sizeForce + sizeTorque;

  static const unsigned sizePose = sizePos + sizeOri;
  static const unsigned sizePoseTangent = sizePos + sizeOriTangent;

  static const unsigned sizeContactKine = sizePose;
  static const unsigned sizeContactKineTangent = sizePoseTangent;

  static const unsigned sizeContact = sizeContactKine + sizeWrench;
  static const unsigned sizeContactTangent = sizeContactKineTangent + sizeWrench;

  static const Kinematics::Flags::Byte flagsStateKine = Kinematics::Flags::position | Kinematics::Flags::orientation
                                                        | Kinematics::Flags::linVel | Kinematics::Flags::angVel;

  static const Kinematics::Flags::Byte flagsContactKine = Kinematics::Flags::position | Kinematics::Flags::orientation;

  static const Kinematics::Flags::Byte flagsPoseKine = Kinematics::Flags::position | Kinematics::Flags::orientation;

  static const Kinematics::Flags::Byte flagsIMUKine = Kinematics::Flags::position | Kinematics::Flags::orientation
                                                      | Kinematics::Flags::linVel | Kinematics::Flags::angVel
                                                      | Kinematics::Flags::linAcc;

  ////////////DEFAULT VALUES //////
  static const double defaultMass;

  static const double statePoseInitVarianceDefault;
  static const double stateOriInitVarianceDefault;
  static const double stateLinVelInitVarianceDefault;
  static const double stateAngVelInitVarianceDefault;
  static const double gyroBiasInitVarianceDefault;
  static const double unmodeledWrenchInitVarianceDefault;
  static const double contactForceInitVarianceDefault;
  static const double contactTorqueInitVarianceDefault;

  static const double statePoseProcessVarianceDefault;
  static const double stateOriProcessVarianceDefault;
  static const double stateLinVelProcessVarianceDefault;
  static const double stateAngVelProcessVarianceDefault;
  static const double gyroBiasProcessVarianceDefault;
  static const double unmodeledWrenchProcessVarianceDefault;
  static const double contactPositionProcessVarianceDefault;
  static const double contactOrientationProcessVarianceDefault;
  static const double contactForceProcessVarianceDefault;
  static const double contactTorqueProcessVarianceDefault;

  static const double acceleroVarianceDefault;
  static const double gyroVarianceDefault;
  static const double forceSensorVarianceDefault;
  static const double torqueSensorVarianceDefault;
  static const double positionSensorVarianceDefault;
  static const double orientationSensorVarianceDefault;

  static const double linearStiffnessDefault;
  static const double angularStiffnessDefault;
  static const double linearDampingDefault;
  static const double angularDampingDefault;

  ////////////
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:
  /// Default Stiffness and damping
  Matrix3 linearStiffnessMatDefault_;
  Matrix3 angularStiffnessMatDefault_;
  Matrix3 linearDampingMatDefault_;
  Matrix3 angularDampingMatDefault_;

  ////////////Sensor Covariance mnatrices
  Matrix3 acceleroCovMatDefault_;
  Matrix3 gyroCovMatDefault_;
  Matrix6 contactWrenchSensorCovMatDefault_;
  Matrix6 absPoseSensorCovMatDefault_;

  Matrix3 statePosInitCovMat_;
  Matrix3 stateOriInitCovMat_;
  Matrix3 stateLinVelInitCovMat_;
  Matrix3 stateAngVelInitCovMat_;
  Matrix3 gyroBiasInitCovMat_;
  Matrix6 unmodeledWrenchInitCovMat_;
  Matrix12 contactInitCovMat_;

  Matrix3 statePosProcessCovMat_;
  Matrix3 stateOriProcessCovMat_;
  Matrix3 stateLinVelProcessCovMat_;
  Matrix3 stateAngVelProcessCovMat_;
  Matrix3 gyroBiasProcessCovMat_;
  Matrix6 unmodeledWrenchProcessCovMat_;
  Matrix3 contactPositionProcessCovMat_;
  Matrix3 contactOrientationProcessCovMat_;
  Matrix3 contactForceProcessCovMat_;
  Matrix3 contactTorqueProcessCovMat_;
  Matrix12 contactProcessCovMat_;

  Matrix12 stateKinematicsInitCovMat_;
  Matrix12 stateKinematicsProcessCovMat_;

  /// default derivation steps
  static const double defaultdx;

  /// a structure to optimize computations
  struct Opt
  {
    Opt() : kine(kine1), ori(kine.orientation), ori1(kine1.orientation), ori2(kine2.orientation) {}

    Kinematics kine1, kine2;
    Kinematics & kine;
    Orientation & ori;
    Orientation & ori1;
    Orientation & ori2;
  } opt_;
};

#include <state-observation/dynamics-estimators/kinetics-observer.hxx>

} // namespace stateObservation

#endif /// KINETICSOBSERVER_HPP


#include "OccupancyGrid/cvmat_serialization.h"
#include "OccupancyGrid/occgrid.hpp"
#include <cmath>
#include <opencv2/opencv.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/format.hpp>
#include "OccupancyGrid/loadData.h"
#include <boost/program_options.hpp>

namespace bfs = boost::filesystem;
namespace po = boost::program_options;
using namespace std;
using namespace gtsam;

/// Override class to override is_occupied function so that it can copy the
/// ground truth map everytime a laser crosses a cell.
template<typename real_t, typename int_t>
class OccupancyGrid2DGT : public OccupancyGrid2D<real_t, int_t> {
  public:
    cv::Mat_<uint8_t> gt_;
    using OccupancyGrid2D<real_t, int_t>::og_;
    using OccupancyGrid2D<real_t, int_t>::FREE;

    OccupancyGrid2DGT(real_t min_x, real_t min_y, real_t cell_size_x, real_t
        cell_size_y, int_t ncells_x, int_t ncells_y) :
      OccupancyGrid2D<real_t, int_t>(min_x, min_y, cell_size_x, cell_size_y,
          ncells_x, ncells_y),
      gt_(ncells_x, ncells_y)
    {
    };

    virtual bool is_occupied(int_t i, int_t j) {
        uint8_t* row = og_.ptr(i);
        uint8_t val = row[j];
        gt_(i, j) = val;
        return (val != FREE);
    }
};

void cvArrow(cv::Mat& vis,
    int x, int y, int u, int v, cv::Scalar line_color, int line_thickness = 1)
{
  cv::Point2i p,q;
  q.x = x;
  q.y = y;
  p.x = x + u;
  p.y = y + v;

  cv::line(vis, p, q, line_color, line_thickness, CV_AA, 0);

  const double pi = boost::math::constants::pi<double>();
  double angle = atan2((double) p.y-q.y,(double) p.x-q.x);
  double arrow_length = sqrt(u*u + v*v);
  q.x = (int) (p.x - arrow_length / 3 * cos(angle + (pi / 6)));
  q.y = (int) (p.y - arrow_length / 3 * sin(angle + (pi / 6)));
  cv::line(vis, p, q, line_color, line_thickness, CV_AA, 0);

  q.x = (int) (p.x - arrow_length / 3 * cos(angle - (pi / 6)));
  q.y = (int) (p.y - arrow_length / 3 * sin(angle - (pi / 6)));
  cv::line(vis, p, q, line_color, line_thickness, CV_AA, 0);
}

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

  // parse arguments ///////////////////////////////////////////
  // Declare the supported options.
  po::options_description desc("Visualize data");
  desc.add_options()
    ("help", "produce help message")
    ("width", po::value<double>()->required(), "Width ")
    ("height", po::value<double>()->required(), "Height ")
    ("resolution", po::value<double>()->required(), "Resolution ")
    ("dir", po::value<std::string>()->default_value("."), "Data directory")
    ("erode", po::value<bool>()->default_value(true), "Erode the image before resizing")
;

  po::positional_options_description pos;
  pos.add("width", 1);
  pos.add("height", 1);
  pos.add("resolution", 1);
  pos.add("dir", 1);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm);
  po::notify(vm);

  double width = vm["width"].as<double>();
  double height = vm["height"].as<double>();
  double resolution = vm["resolution"].as<double>();
  std::string datadirectory = vm["dir"].as<std::string>();
  bool do_erode = vm["erode"].as<bool>();
  // end of parse arguments ////////////////////////////////////
  cv::Mat laser_pose, laser_ranges, scan_angles, laser_reflectance;
  loadMat(laser_pose, datadirectory + "/laser_pose_all.bin");
  loadMat(laser_ranges, datadirectory + "/laser_range_all.bin");
  loadMat(scan_angles, datadirectory + "/scan_angles_all.bin");
  loadMat(laser_reflectance, datadirectory + "/laser_reflectance_all.bin");

  std::string floorplanfile = datadirectory + "/floorplan.png";
  cv::Mat floorplan = cv::imread(floorplanfile, cv::IMREAD_GRAYSCALE);

  cv::Vec2d original_size(width, height);
  cv::Vec2i gridsize;
  cv::Vec2d cellsize(resolution, resolution);
  //cv::divide(original_size, gridsize, cellsize); // cellsize = in meters per pixel
  cv::divide(original_size, cellsize, gridsize);
  if (do_erode) {
    int kernel_rows = ceil(floorplan.cols / gridsize(0));
    int kernel_cols = ceil(floorplan.rows / gridsize(1));
    // Thickens the map walls before resizing
    cv::erode(floorplan, floorplan, cv::Mat::ones(kernel_rows, kernel_cols, CV_8U));
  }

  cv::resize(floorplan, floorplan, cv::Size(gridsize(0), gridsize(1)),
      0, 0, cv::INTER_LINEAR);

  double origin_x = 0, origin_y = 0;
  double size_x = width, size_y = height;

    shiftPoses(laser_pose, scan_angles, laser_ranges, laser_reflectance,
        size_x, size_y, origin_x, origin_y);
    std::cout << "Laser spans (" << size_x << ", " << size_y << ")\n";
    std::cout << "Laser count: " << laser_ranges.rows * laser_ranges.cols << "\n";
    cv::Vec2d size_bitmap(size_x, size_y);
    cv::Vec2i size_bitmap_px(size_x / cellsize(0), size_y / cellsize(1));
    cv::Vec2i origin_px(origin_x / cellsize(0), origin_y / cellsize(1));
    cv::Vec2d min_pt(- size_bitmap(0)/2, - size_bitmap(1)/2);
    cv::Vec2i ncells;
    cv::divide(min_pt, cellsize, ncells, -2); // ncells = number of cells in map including margins

    OccupancyGrid2DGT<double, int> map(
        min_pt(0),
        min_pt(1),
        cellsize(0),
        cellsize(1),
        ncells(0),
        ncells(1));
    map.og_ = cv::Scalar(map.OCCUPIED);

    cv::Vec2i min_floorplan, max_floorplan;
    cv::addWeighted(origin_px, 1, size_bitmap_px, -0.5, 0, min_floorplan);
    cv::addWeighted(origin_px, 1, size_bitmap_px, 0.5, 0, max_floorplan);
    //cv::transpose(floorplan, floorplan);
    cv::Mat fp_roi = floorplan(
        cv::Range(std::max(0, floorplan.rows / 2 - max_floorplan(1)),
                  std::min(floorplan.rows / 2 - min_floorplan(1), floorplan.rows)),
        cv::Range(std::max(0, floorplan.cols / 2 + min_floorplan(0)),
                  std::min(floorplan.cols / 2 + max_floorplan(0), floorplan.cols))
        );

    cv::Vec2i fp_roi_size(fp_roi.cols, fp_roi.rows);
    cv::Vec2i min_bitmap;
    cv::addWeighted(ncells, 0.5, fp_roi_size, -0.5, 0, min_bitmap);
    cv::Vec2i max_bitmap;
    cv::add(min_bitmap, fp_roi_size, max_bitmap);
    printf("Map size (%d, %d)\n", ncells(0), ncells(1));
    printf("(%d, %d) <-> (%d, %d)\n", min_bitmap(0), min_bitmap(1), max_bitmap(0), max_bitmap(1));
    cv::Mat map_roi = map.og_(cv::Range(min_bitmap(0), max_bitmap(0)),
      cv::Range(min_bitmap(1), max_bitmap(1)));

    cv::imshow("fp_roi", fp_roi);
    cv::transpose(fp_roi, fp_roi);
    cv::flip(fp_roi, fp_roi, 1);
    fp_roi.copyTo(map_roi);

    for (cv::MatIterator_<uint8_t> it = map.gt_.begin();
        it != map.gt_.end();
        it++) {
        *it = 127;
    }

    double MAX_RANGE = 8.0;

    std::vector<double> ranges(scan_angles.cols, MAX_RANGE);
    int r;
    boost::format in_fmter(datadirectory + "/inputstream/%d.png");
    bfs::create_directory(datadirectory + "/inputstream");
    boost::format gt_fmter(datadirectory + "/groundtruth/%d.png");
    bfs::create_directory(datadirectory + "/groundtruth");

    // trajectory map
    cv::Mat trajectory;
    cv::cvtColor(map.og_, trajectory, cv::COLOR_GRAY2BGR);
    //const double pi = boost::math::constants::pi<double>();
    for (r = 0; r < laser_pose.rows; r++) {
        double* pose = laser_pose.ptr<double>(r);
        double* angles = scan_angles.ptr<double>(r);
        double robot_angle = pose[2];
        for (int c = 0; c < scan_angles.cols; c++) {
            double total_angle = robot_angle + angles[c];

            cv::Vec2d final_pos;
            bool reflectance;
            ranges[c] = map.ray_trace(pose[0], pose[1], total_angle, MAX_RANGE, final_pos, reflectance);
        }
        cv::Vec2d position(pose[0], pose[1]);
        // draw input
        cv::Mat visin;
        cv::cvtColor(map.og_, visin, cv::COLOR_GRAY2BGR);
        map.draw_lasers(visin, position, robot_angle, angles,
            laser_ranges.ptr<double>(r),
            laser_reflectance.ptr<uint8_t>(r),
            scan_angles.cols,
            CV_RGB(0, 255, 0));

        cv::transpose(visin, visin);
        cv::flip(visin, visin, 0);
        cv::imshow("c", visin);
        cv::imwrite((in_fmter % r).str(), visin);

        // draw trajectory
        double arrow_len = .5;
        if ( r % (int)(arrow_len / cellsize(0)) == 0) {
          double u = arrow_len * cos(robot_angle);
          double v = arrow_len * sin(robot_angle);
          cv::Point2i pt = map.xy2rc(cv::Vec2d(pose[0], pose[1]));
          cv::Point2i pt2 = map.xy2rc(cv::Vec2d(pose[0] + u, pose[1] + v));
          cvArrow(trajectory,
              pt.x, pt.y,
              pt2.x - pt.x, pt2.y - pt.y,
              cv::Scalar(255, 0, 0));
          cv::Mat trajvis;
          cv::transpose(trajectory, trajvis);
          cv::flip(trajvis, trajvis, 0);
          cv::imshow("e", trajvis);
        }

        // draw ground truth
        cv::Mat vis;
        cv::cvtColor(map.gt_, vis, cv::COLOR_GRAY2BGR);
        map.draw_lasers(vis, position, robot_angle, angles, &ranges[0],
            laser_reflectance.ptr<uint8_t>(r),
            scan_angles.cols, CV_RGB(0, 255, 0));
        cv::transpose(vis, vis);
        cv::flip(vis, vis, 0);
        cv::imshow("d", vis);
        cv::imwrite((gt_fmter % r).str(), vis);
        cv::waitKey(1);
    }
    cv::transpose(trajectory, trajectory);
    cv::flip(trajectory, trajectory, 0);
    cv::imwrite(datadirectory + "/trajectory.png", trajectory);

    cv::Mat visgt;
    cv::cvtColor(map.gt_, visgt, cv::COLOR_GRAY2BGR);
    for (r = 0; r < laser_pose.rows; r++) {
        double* pose = laser_pose.ptr<double>(r);
        double* angles = scan_angles.ptr<double>(r);
        double robot_angle = pose[2];
        for (int c = 0; c < scan_angles.cols; c++) {
            double total_angle = robot_angle + angles[c];

            cv::Vec2d final_pos;
            bool reflectance;
            ranges[c] = map.ray_trace(pose[0], pose[1], total_angle, MAX_RANGE, final_pos, reflectance);
        }
        cv::Vec2d position(pose[0], pose[1]);
        // draw trajectory
        double arrow_len = .5;
        if ( r % (int)(arrow_len * 10/ cellsize(0)) == 0) {
          double u = arrow_len * cos(robot_angle);
          double v = arrow_len * sin(robot_angle);
          cv::Point2i pt = map.xy2rc(cv::Vec2d(pose[0], pose[1]));
          cv::Point2i pt2 = map.xy2rc(cv::Vec2d(pose[0] + u, pose[1] + v));
          cvArrow(visgt,
              pt.x, pt.y,
              pt2.x - pt.x, pt2.y - pt.y,
              cv::Scalar(255, 0, 0));
          cv::imshow("d", visgt);
          cv::waitKey(33);
        }
    }
    std::stringstream ss;
    ss << datadirectory << "/gt-final.png";
    cv::transpose(visgt, visgt);
    cv::flip(visgt, visgt, 0);
    cv::imwrite(ss.str(), visgt);
}


// Boost.Polygon library polygon_rectangle_test.cpp file

//          Copyright Andrii Sydorchuk 2014.
// 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)

// See http://www.boost.org for updates, documentation, and revision history.

#define BOOST_TEST_MODULE POLYGON_RECTANGLE_TEST
#include <boost/mpl/list.hpp>
#include <boost/test/test_case_template.hpp>

#include "boost/polygon/rectangle_concept.hpp"
#include "boost/polygon/rectangle_data.hpp"
#include "boost/polygon/rectangle_traits.hpp"
using namespace boost::polygon;

typedef boost::mpl::list<int> test_types;

template <typename interval_type>
void CHECK_INTERVAL_EQUAL(const interval_type& i1, const interval_type& i2) {
  BOOST_CHECK_EQUAL(get(i1, LOW), get(i2, LOW));
  BOOST_CHECK_EQUAL(get(i1, HIGH), get(i2, HIGH));
}

template <typename rectangle_type>
void CHECK_RECT_EQUAL(const rectangle_type& r1, const rectangle_type& r2) {
  CHECK_INTERVAL_EQUAL(horizontal(r1), horizontal(r2));
  CHECK_INTERVAL_EQUAL(vertical(r1), vertical(r2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(rectangle_concept_test1, T, test_types) {
  typedef rectangle_data<T> rectangle_type;

  rectangle_type rectangle1 = construct<rectangle_type>(-1, -1, 1, 1);
  scale_up(rectangle1, 2);
  CHECK_RECT_EQUAL(construct<rectangle_type>(-2, -2, 2, 2), rectangle1);

  scale_down(rectangle1, 2);
  CHECK_RECT_EQUAL(construct<rectangle_type>(-1, -1, 1, 1), rectangle1);
}