#include <boost/foreach.hpp>

#include "rb_tracker/RBFilter.h"
#include "omip_common/OMIPUtils.h"

#include <boost/numeric/ublas/matrix.hpp>

#include <iostream>
#include <fstream>

#include <Eigen/Cholesky>

#include <pcl/filters/filter.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>

using namespace omip;

RB_id_t RBFilter::_rb_id_generator = 1;

using namespace MatrixWrapper;
using namespace BFL;

// Dimensions of each measurement (each feature location)
#define MEAS_DIM_RBF 3

// Value of the measurement noise (unused, only set at the beginning, but it will change for each concrete feature location)
#define COV_MEAS_RBF 1.0

RBFilter::RBFilter() :
    _estimation_error_threshold(-1.),
    _id(RBFilter::_rb_id_generator++),
    _velocity( 0., 0., 0., 0., 0., 0.),
    _features_database(),
    _loop_period_ns(0.),
    _min_num_supporting_feats_to_correct(5),
    _use_predicted_state_from_kh(false),
    _use_predicted_measurement_from_kh(false),
    _last_time_interval_ns(0.0)
{
    _pose = Eigen::Matrix4d::Identity();
    this->_initial_location_of_centroid = Feature::Location(0,0,0);
}

RBFilter::RBFilter(double loop_period_ns,
                   const std::vector<Eigen::Matrix4d>& trajectory,
                   const Eigen::Twistd &initial_velocity,
                   FeaturesDataBase::Ptr feats_database,
                   double estimation_error_threshold) :
    _loop_period_ns(loop_period_ns),
    _last_time_interval_ns(loop_period_ns),
    _estimation_error_threshold(estimation_error_threshold),
    _id(RBFilter::_rb_id_generator++),
    _pose(trajectory[trajectory.size()-1]),
    _velocity(initial_velocity),
    _features_database(feats_database),
    _min_num_supporting_feats_to_correct(5),
    _use_predicted_state_from_kh(false),
    _use_predicted_measurement_from_kh(false)
{
    this->_initial_location_of_centroid = Feature::Location(trajectory[0](0,3),trajectory[0](1,3),trajectory[0](2,3));
    this->_trajectory = std::vector<Eigen::Matrix4d>(trajectory.begin()+1, trajectory.end());
}

void RBFilter::Init()
{
    this->_initializeAuxiliarMatrices();
}

RBFilter::RBFilter(const RBFilter &rbm)
{
    this->_id = rbm._id;
    this->_pose = rbm._pose;
    this->_velocity = rbm._velocity;
    this->_trajectory = rbm._trajectory;
    this->_supporting_features_ids = rbm._supporting_features_ids;
    this->_features_database = rbm.getFeaturesDatabase();
    this->_predicted_measurement_pc_bh = rbm._predicted_measurement_pc_bh;
    this->_predicted_measurement_pc_vh = rbm._predicted_measurement_pc_vh;
    this->_predicted_measurement_map_bh = rbm._predicted_measurement_map_bh;
    this->_predicted_measurement_map_vh = rbm._predicted_measurement_map_vh;
    this->_predicted_pose_bh = rbm._predicted_pose_bh;
    this->_predicted_pose_vh = rbm._predicted_pose_vh;
    this->_min_num_supporting_feats_to_correct = rbm._min_num_supporting_feats_to_correct;
}

RBFilter::~RBFilter()
{
}

void RBFilter::predictState(double time_interval_ns)
{
    //Sanity check.
    if(time_interval_ns < 0)
    {
        std::cout << "Time interval negative!" << std::endl;
        this->_last_time_interval_ns = time_interval_ns;
        getchar();
    }

    // The system noise is proportional to the time elapsed between the previous and the current measurement
    Eigen::Matrix<double, 6, 6> sys_noise_COV = Eigen::Matrix<double, 6, 6>::Zero();
    sys_noise_COV(0, 0) = this->_cov_sys_acc_tx*(time_interval_ns/1e9);
    sys_noise_COV(1, 1) = this->_cov_sys_acc_ty*(time_interval_ns/1e9);
    sys_noise_COV(2, 2) = this->_cov_sys_acc_tz*(time_interval_ns/1e9);
    sys_noise_COV(3, 3) = this->_cov_sys_acc_rx*(time_interval_ns/1e9);
    sys_noise_COV(4, 4) = this->_cov_sys_acc_ry*(time_interval_ns/1e9);
    sys_noise_COV(5, 5) = this->_cov_sys_acc_rz*(time_interval_ns/1e9);

    // We update the pose with a non-linear rule:
    // The predicted pose is the belief pose pre-multiplied by the corresponding
    // delta in pose based on the belief velocity and the time elapsed
    // This is slightly different to just adding the belief pose and the
    // belief delta pose in exponential coordinates, but the difference is small
    // because the delta pose is small when the velocity is small (it is the first linear approximation)
    Eigen::Twistd belief_delta_pose_ec = (time_interval_ns/1e9)*_velocity;
    _predicted_pose_vh = belief_delta_pose_ec.exp(1e-12).toHomogeneousMatrix()*_pose;

    // We also update the covariance based on Barfoot et al.
    Eigen::Matrix<double, 6, 6> delta_pose_covariance = (time_interval_ns/1e9)*_velocity_covariance;

    // We compute the adjoint of the pose (careful, the LGSM lib assumes first rotation then translation, and we assume the opposite)
    Eigen::Twistd pose_ec;
    TransformMatrix2Twist(_pose, pose_ec);
    Eigen::Matrix<double, 6, 6> transformed_cov;
    adjointXcovXadjointT(pose_ec, delta_pose_covariance, transformed_cov);

    // We update the predicted pose_covariance adding the uncertainty about the velocity
    _predicted_pose_cov_vh = _pose_covariance +  delta_pose_covariance;

    // Finally we add the (constant) system noise
    _predicted_pose_cov_vh += sys_noise_COV;

    // The alternative forward model assumes that there was a break event (zero velocity) and therefore the pose remains constant
    // This model covers for abrupt stopping events
    this->_predicted_pose_bh = _pose;
    this->_predicted_velocity_bh = Eigen::Twistd(0,0,0,0,0,0);
    // The additive noise we use for the predicted covariance based on braking hypothesis is larger so that it is only used if it is really
    // more likely (larger and better support from input data) than the velocity based
    this->_predicted_pose_cov_bh = _pose_covariance +  delta_pose_covariance + 1.001*sys_noise_COV;
    this->_predicted_velocity_cov_bh =  sys_noise_COV;
}

// This only predicts the location of the Features that are supporting the RBFilter
// at this moment.
// This happens at the end of the iteration
void RBFilter::predictMeasurement()
{
    this->_predicted_measurement_pc_vh->points.clear();
    this->_predicted_measurement_map_vh.clear();
    this->_predicted_measurement_pc_bh->points.clear();
    this->_predicted_measurement_map_bh.clear();
    this->_predicted_measurement_pc_kh->points.clear();
    this->_predicted_measurement_map_kh.clear();

    // We need to do this outside because if we do not have any features we still want to use the predictions from kh
    if(_use_predicted_state_from_kh)
    {
        this->_use_predicted_measurement_from_kh = true;
    }

    Feature::Location predicted_location;
    FeaturePCLwc predicted_location_pcl;
    Feature::Ptr supporting_feat_ptr;
    BOOST_FOREACH(Feature::Id supporting_feat_id, this->_supporting_features_ids)
    {
        if(this->_features_database->isFeatureStored(supporting_feat_id))
        {
            supporting_feat_ptr = this->_features_database->getFeature(supporting_feat_id);

            // Predict feature location based on velocity hypothesis
            this->PredictFeatureLocation(supporting_feat_ptr,this->_predicted_pose_vh, true, predicted_location);
            LocationAndId2FeaturePCLwc(predicted_location, supporting_feat_id, predicted_location_pcl);
            this->_predicted_measurement_pc_vh->points.push_back(predicted_location_pcl);
            this->_predicted_measurement_map_vh[supporting_feat_id] = predicted_location;

            // Predict feature location based on brake hypothesis (same as last)
            this->PredictFeatureLocation(supporting_feat_ptr,this->_predicted_pose_bh, true, predicted_location);
            LocationAndId2FeaturePCLwc(predicted_location, supporting_feat_id, predicted_location_pcl);
            this->_predicted_measurement_pc_bh->points.push_back(predicted_location_pcl);
            this->_predicted_measurement_map_bh[supporting_feat_id] = predicted_location;

            if(_use_predicted_state_from_kh)
            {
                // Predict feature location based on kinematic hypothesis
                this->PredictFeatureLocation(supporting_feat_ptr,this->_predicted_pose_kh, true, predicted_location);
                LocationAndId2FeaturePCLwc(predicted_location, supporting_feat_id, predicted_location_pcl);
                this->_predicted_measurement_pc_kh->points.push_back(predicted_location_pcl);
                this->_predicted_measurement_map_kh[supporting_feat_id] = predicted_location;
                this->_use_predicted_state_from_kh = false;
            }
        }
    }
}

void RBFilter::correctState()
{

    // Resize matrices
    int num_supporting_feats = (int)this->_supporting_features_ids.size();

    if(num_supporting_feats > this->_min_num_supporting_feats_to_correct)
    {
        int meas_dimension = 3 * num_supporting_feats;

        double feature_depth = 0.;

        Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > G_t((this->_G_t_memory.data()), 6, meas_dimension);
        Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > R_inv_times_innovation((this->_R_inv_times_innovation_memory.data()), meas_dimension, 1);

        // We need to set them to zero because they are pointing to existing memory regions!
        G_t.setZero();
        R_inv_times_innovation.setZero();

        Feature::Location location_at_birth;

        int feat_idx = 0;

        Eigen::Matrix3d R_seq = Eigen::Matrix3d::Zero();
        Eigen::Matrix<double, 6, 6> P_minus_seq = _predicted_pose_covariance;

        //std::cout << _predicted_pose_covariance << std::endl;
        Eigen::Matrix<double, 6, 3> P_HT_seq = Eigen::Matrix<double, 6, 3>::Zero();
        Eigen::Matrix3d S_seq = Eigen::Matrix3d::Zero();
        Eigen::Matrix3d S_inv_seq = Eigen::Matrix3d::Zero();
        Eigen::Matrix<double, 6, 3> K_seq = Eigen::Matrix<double, 6, 3>::Zero();

        Eigen::Matrix<double, 6, 6> temp_seq1 = Eigen::Matrix<double, 6, 6>::Zero();
        Eigen::Matrix<double, 6, 6> temp_seq2 = Eigen::Matrix<double, 6, 6>::Zero();

        Eigen::Vector4d feature_innovation;
        Eigen::Vector4d location_at_birth_eig;

        Eigen::Vector4d predicted_location;
        Eigen::Vector4d current_location;

        Feature::Ptr supporting_feat_ptr;

        BOOST_FOREACH(Feature::Id supporting_feat_id, this->_supporting_features_ids)
        {
            supporting_feat_ptr = this->_features_database->getFeature(supporting_feat_id);

            if(supporting_feat_ptr->getFeatureAge() > 1)
                this->GetLocationOfFeatureAtBirthOfRB( supporting_feat_ptr, false, location_at_birth);
            LocationOfFeature2EigenVectorHomogeneous(location_at_birth, location_at_birth_eig);

            Feature::Location current_loc = supporting_feat_ptr->getLastLocation();
            LocationOfFeature2EigenVectorHomogeneous(current_loc, current_location);

            predicted_location = _predicted_pose*location_at_birth_eig;

            feature_innovation = current_location - predicted_location;

            feature_depth = supporting_feat_ptr->getLastZ();

            R_seq( 0, 0) =  std::max(this->_min_cov_meas_x, feature_depth / this->_meas_depth_factor);
            R_seq( 1, 1) =  std::max(this->_min_cov_meas_y, feature_depth / this->_meas_depth_factor);
            R_seq( 2, 2) =  std::max(this->_min_cov_meas_z, feature_depth / this->_meas_depth_factor);

            R_inv_times_innovation(3 * feat_idx ) = feature_innovation.x()/(std::max(this->_min_cov_meas_x, feature_depth / this->_meas_depth_factor));
            R_inv_times_innovation(3 * feat_idx + 1) = feature_innovation.y()/(std::max(this->_min_cov_meas_y, feature_depth / this->_meas_depth_factor));
            R_inv_times_innovation(3 * feat_idx + 2) = feature_innovation.z()/(std::max(this->_min_cov_meas_z, feature_depth / this->_meas_depth_factor));

            _D_T_p0_circle(0,4) = predicted_location[2];
            _D_T_p0_circle(0,5) = -predicted_location[1];
            _D_T_p0_circle(1,3) = -predicted_location[2];
            _D_T_p0_circle(1,5) = predicted_location[0];
            _D_T_p0_circle(2,3) = predicted_location[1];
            _D_T_p0_circle(2,4) = -predicted_location[0];

            G_t.block<6,3>(0, 3*feat_idx) = _D_T_p0_circle.transpose();

            P_HT_seq = P_minus_seq * _D_T_p0_circle.transpose();

            S_seq = _D_T_p0_circle * P_HT_seq;

            S_seq += R_seq;

            invert3x3MatrixEigen2(S_seq, S_inv_seq );

            K_seq = P_HT_seq * S_inv_seq;

            temp_seq1 = K_seq*_D_T_p0_circle;

            temp_seq2 = temp_seq1 * P_minus_seq;

            P_minus_seq -= temp_seq2;

            // copy elements
            for ( unsigned int i=0; i<P_minus_seq.rows(); i++ )
            {
                for ( unsigned int j=0; j<=i; j++ )
                {
                    P_minus_seq(j,i) = P_minus_seq(i,j);
                }
            }

            feat_idx++;
        }

        _pose_covariance = P_minus_seq;

        Eigen::Matrix<double, 6, 1> pose_update = P_minus_seq * G_t * R_inv_times_innovation;

        Eigen::Twistd pose_update_ec(pose_update[3], pose_update[4], pose_update[5], pose_update[0], pose_update[1], pose_update[2]);
        _pose = pose_update_ec.exp(1e-12).toHomogeneousMatrix()*_predicted_pose;
    }else{
        ROS_ERROR_STREAM_NAMED("RBFilter::correctState","RB" << this->_id << ": using predicted state as corrected state!");
        _pose = _predicted_pose;
        _pose_covariance = _predicted_pose_covariance;
    }

    Eigen::Matrix4d delta_pose = _pose*_trajectory.at(_trajectory.size()-1).inverse();

    Eigen::Twistd delta_pose_ec;
    TransformMatrix2Twist(delta_pose, delta_pose_ec);
    this->_velocity = delta_pose_ec/(_last_time_interval_ns/1e9);
    this->_trajectory.push_back(this->_pose);
}

bool isPredictionYBetterThanX(boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> > X, boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> > Y)
{
    // If the number of supporting features based on X is smaller than based on Y
    // then YBetterThanX = true
    if(boost::tuples::get<0>(X) < boost::tuples::get<0>(Y))
    {
        return true;
    }
    // If the number of supporting features based on X is larger than based on Y
    // then YBetterThanX = false
    else if(boost::tuples::get<0>(X) > boost::tuples::get<0>(Y))
    {
        return false;
    }
    // If the number of supporting features based on X is the same as based on Y
    // then we check the accumulated error
    else{
        // If the accumulated error based on X is larger than based on Y
        // then YBetterThanX = true
        if(boost::tuples::get<1>(X) > boost::tuples::get<1>(Y))
        {
            return true;
        }
        // If the accumulated error based on X is smaller than based on Y
        // then YBetterThanX = false
        else if(boost::tuples::get<1>(X) < boost::tuples::get<1>(Y))
        {
            return false;
        }
        // If the accumulated error based on X is the same as based on Y
        // then we check the covariances
        else
        {
            int num_larger_x_covariance = 0;
            for(int i =0; i<6; i++)
            {
                if(boost::tuples::get<2>(X)(i,i) > boost::tuples::get<2>(Y)(i,i))
                {
                    num_larger_x_covariance +=1;
                }
            }
            if(num_larger_x_covariance > 3)
            {
                return true;
            }
            else{
                return false;
            }
        }
    }
}

//TODO: Decide if I want to test ALL features or only the previously supporting features
//NOW: Only supporting features
// This function is called when there are new Features added
void RBFilter::estimateBestPredictionAndSupportingFeatures()
{
    // 1: Update the list of supporting features to only contain features that are still alive
    std::vector<Feature::Id> alive_supporting_features_ids;
    BOOST_FOREACH(Feature::Id supporting_feat_id, this->_supporting_features_ids)
    {
        if(this->_features_database->isFeatureStored(supporting_feat_id))
        {
            alive_supporting_features_ids.push_back(supporting_feat_id);
        }
    }
    this->_supporting_features_ids.swap(alive_supporting_features_ids);

    std::vector<boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> > > num_supporting_feats_and_error_and_pose_cov;

    // 2: Count the supporting features by comparing the received feature location to the predicted location based on each
    // hypothesis (velocity based, brake event based, kinematic state based)
    std::vector<Feature::Id> supporting_feats_based_on_vh;
    std::vector<Feature::Id> supporting_feats_based_on_bh;
    std::vector<Feature::Id> supporting_feats_based_on_kh;
    double error_vh = 0.0;
    double error_bh = 0.0;
    double error_kh = 0.0;
    double acc_error_vh = 0.0;
    double acc_error_bh = 0.0;
    double acc_error_kh = 0.0;
    BOOST_FOREACH(Feature::Id supporting_feat_id, this->_supporting_features_ids)
    {
        Feature::Location last_location = this->_features_database->getFeatureLastLocation(supporting_feat_id);

        Feature::Location predicted_location_vh = this->_predicted_measurement_map_vh[supporting_feat_id];
        error_vh = L2Distance(last_location, predicted_location_vh);
        if (error_vh < this->_estimation_error_threshold)
        {
            supporting_feats_based_on_vh.push_back(supporting_feat_id);
            acc_error_vh += error_vh;
        }

        Feature::Location predicted_location_bh = this->_predicted_measurement_map_bh[supporting_feat_id];
        error_bh = L2Distance(last_location, predicted_location_bh);
        if (error_bh < this->_estimation_error_threshold)
        {
            supporting_feats_based_on_bh.push_back(supporting_feat_id);
            acc_error_bh += error_bh;
        }

        // If we had a predicted state from kinematics that we used to predict measurements
        if(this->_use_predicted_measurement_from_kh)
        {
            Feature::Location predicted_location_kh = this->_predicted_measurement_map_kh[supporting_feat_id];
            error_kh = L2Distance(last_location, predicted_location_kh);
            if (error_kh < this->_estimation_error_threshold)
            {
                supporting_feats_based_on_kh.push_back(supporting_feat_id);
                acc_error_kh += error_kh;
            }
        }
    }

    num_supporting_feats_and_error_and_pose_cov.push_back(boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> >(supporting_feats_based_on_vh.size(), acc_error_vh, _predicted_pose_cov_vh));
    num_supporting_feats_and_error_and_pose_cov.push_back(boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> >(supporting_feats_based_on_bh.size(), acc_error_bh, _predicted_pose_cov_bh));

    // If we had a predicted state from kinematics that we used to predict measurements
    if(this->_use_predicted_measurement_from_kh)
    {
        num_supporting_feats_and_error_and_pose_cov.push_back(boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> >(supporting_feats_based_on_kh.size(), acc_error_kh, _predicted_pose_cov_kh));
        this->_use_predicted_measurement_from_kh = false;
    }
    // Find the best hypothesis based on the number of supporting features and the accumulated error of them
    // The criteria to select the best hypothesis is in the function ComparePredictions
    std::vector<boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> > >::iterator hyp_with_max_support_it =
            std::max_element(num_supporting_feats_and_error_and_pose_cov.begin(),
                             num_supporting_feats_and_error_and_pose_cov.end(),
                             isPredictionYBetterThanX);
    int hyp_with_max_support_idx = std::distance(num_supporting_feats_and_error_and_pose_cov.begin(), hyp_with_max_support_it);

    switch(hyp_with_max_support_idx)
    {

    case 0:
    {
        std::cout << "RB" << _id<<" Using velocity prediction" << std::endl;
        this->_predicted_pose = _predicted_pose_vh;
        this->_predicted_pose_covariance = _predicted_pose_cov_vh;
        this->_supporting_features_ids = supporting_feats_based_on_vh;
        this->_predicted_measurement = this->_predicted_measurement_pc_vh;
        this->_predicted_measurement_map = this->_predicted_measurement_map_vh;
        break;
    }

    case 1:
    {
        std::cout << "RB" << _id<<" Using brake prediction" << std::endl;
        this->_predicted_pose = _predicted_pose_bh;
        this->_predicted_pose_covariance = _predicted_pose_cov_bh;
        this->_supporting_features_ids = supporting_feats_based_on_bh;
        this->_predicted_measurement = this->_predicted_measurement_pc_bh;
        this->_predicted_measurement_map = this->_predicted_measurement_map_bh;
        break;
    }

    case 2:
    {
        ROS_ERROR_STREAM_NAMED("RBFilter.estimateBestPredictionAndSupportingFeatures",
                               "RB" << _id << " Using prediction from higher level!");
        this->_predicted_pose = _predicted_pose_kh;
        this->_predicted_pose_covariance = _predicted_pose_cov_kh;
        this->_supporting_features_ids = supporting_feats_based_on_kh;
        this->_predicted_measurement = this->_predicted_measurement_pc_kh;
        this->_predicted_measurement_map = this->_predicted_measurement_map_kh;
        break;
    }
    default:
        break;
    }
}

void RBFilter::addSupportingFeature(Feature::Id supporting_feat_id)
{
    this->_supporting_features_ids.push_back(supporting_feat_id);
}

void RBFilter::setPredictedState(const omip_msgs::RigidBodyPoseAndVelMsg hypothesis)
{
    this->_use_predicted_state_from_kh = true;

    Eigen::Twistd predicted_pose_kh_ec(hypothesis.pose_wc.twist.angular.x,
                                                   hypothesis.pose_wc.twist.angular.y,
                                                   hypothesis.pose_wc.twist.angular.z,
                                                   hypothesis.pose_wc.twist.linear.x,
                                                   hypothesis.pose_wc.twist.linear.y,
                                                   hypothesis.pose_wc.twist.linear.z);
    this->_predicted_pose_kh = predicted_pose_kh_ec.exp(1e-12).toHomogeneousMatrix();

    this->_predicted_velocity_kh = Eigen::Twistd(hypothesis.velocity_wc.twist.angular.x,
                                                 hypothesis.velocity_wc.twist.angular.y,
                                                 hypothesis.velocity_wc.twist.angular.z,
                                                 hypothesis.velocity_wc.twist.linear.x,
                                                 hypothesis.velocity_wc.twist.linear.y,
                                                 hypothesis.velocity_wc.twist.linear.z);
    for(int i =0; i<6; i++)
    {
        for(int j=0; j<6; j++)
        {
            this->_predicted_pose_cov_kh(i,j) = hypothesis.pose_wc.covariance[6*i + j];
            this->_predicted_velocity_cov_kh(i,j) = hypothesis.velocity_wc.covariance[6*i + j];
        }
    }
}

void RBFilter::integrateShapeBasedPose(const geometry_msgs::TwistWithCovariance twist_refinement, double pose_time_elapsed_ns)
{
    Eigen::Twistd shape_based_pose_ec = Eigen::Twistd(twist_refinement.twist.angular.x, twist_refinement.twist.angular.y, twist_refinement.twist.angular.z,
                                                  twist_refinement.twist.linear.x, twist_refinement.twist.linear.y, twist_refinement.twist.linear.z);

    if(pose_time_elapsed_ns < this->_loop_period_ns || _supporting_features_ids.size() < _min_num_supporting_feats_to_correct)
    {
        ROS_INFO_STREAM_NAMED("RBFilter.integrateShapeBasedPose", "Using shape-based pose (time elapsed=" << pose_time_elapsed_ns/1e9 <<
                              " s, loop_period="<< _loop_period_ns/1e9 << " s)");
        //ROS_INFO_STREAM_NAMED("RBFilter.integrateShapeBasedPose", "Pose before: " << std::endl <<  this->_pose);

        this->_pose = shape_based_pose_ec.exp(1e-12).toHomogeneousMatrix();
        //ROS_INFO_STREAM_NAMED("RBFilter.integrateShapeBasedPose", "Pose after: " << std::endl << this->_pose);

        this->_trajectory.pop_back();
        this->_trajectory.push_back(this->_pose);

        Eigen::Matrix4d delta_pose_ht = this->_trajectory[this->_trajectory.size()-1]*(this->_trajectory[this->_trajectory.size()-2].inverse());
        Eigen::Twistd delta_pose_ec;
        TransformMatrix2Twist(delta_pose_ht, delta_pose_ec);
        this->_velocity = delta_pose_ec/(this->_loop_period_ns/1e9);
    }else{
        ROS_INFO_STREAM_NAMED("RBFilter.integrateShapeBasedPose", "Shape-based pose is too old (time elapsed=" << pose_time_elapsed_ns/1e9 <<
                              " s, loop_period="<< _loop_period_ns/1e9 << " s). Try reducing the framerate");
    }
}

void RBFilter::addPredictedFeatureLocation(const Feature::Location& predicted_location_velocity,const Feature::Location& predicted_location_brake , const Feature::Id feature_id)
{
    FeaturePCLwc predicted_feature_pcl;

    LocationAndId2FeaturePCLwc(predicted_location_velocity, feature_id, predicted_feature_pcl);
    this->_predicted_measurement->points.push_back(predicted_feature_pcl);
    this->_predicted_measurement_map[feature_id] = predicted_location_velocity;

    this->_predicted_measurement_pc_vh->points.push_back(predicted_feature_pcl);
    this->_predicted_measurement_map_vh[feature_id] = predicted_location_velocity;

    this->_predicted_measurement_pc_kh->points.push_back(predicted_feature_pcl);
    this->_predicted_measurement_map_kh[feature_id] = predicted_location_brake;

    LocationAndId2FeaturePCLwc(predicted_location_brake, feature_id, predicted_feature_pcl);
    this->_predicted_measurement_pc_bh->points.push_back(predicted_feature_pcl);
    this->_predicted_measurement_map_bh[feature_id] = predicted_location_brake;
}

void RBFilter::setFeaturesDatabase(FeaturesDataBase::Ptr feats_database)
{
    this->_features_database = feats_database;
}

void RBFilter::PredictFeatureLocation(Feature::Ptr feature,
                                      const Eigen::Matrix4d& predicted_pose,
                                      bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
                                      Feature::Location& predicted_location,
                                      bool publish_locations) const
{
    Feature::Location location_at_birth_of_rb;
    this->GetLocationOfFeatureAtBirthOfRB(feature, contemporary_last_feat_loc_and_last_pose_in_trajectory, location_at_birth_of_rb);

    TransformLocation(location_at_birth_of_rb, predicted_pose, predicted_location);

    if(publish_locations)
    {
        if(this->_id != 0)
        {
            Feature::Location at_birth_location = location_at_birth_of_rb + _initial_location_of_centroid;
            boost::shared_ptr<FeaturePCLwc> at_birth_feat(new FeaturePCLwc());
            at_birth_feat->x = boost::tuples::get<0>(at_birth_location);
            at_birth_feat->y = boost::tuples::get<1>(at_birth_location);
            at_birth_feat->z = boost::tuples::get<2>(at_birth_location);
            at_birth_feat->label = feature->getId();

            _features_at_birth->points.push_back(*at_birth_feat);
        }else{
            boost::shared_ptr<FeaturePCLwc> at_birth_feat(new FeaturePCLwc());
            at_birth_feat->x = boost::tuples::get<0>(location_at_birth_of_rb);
            at_birth_feat->y = boost::tuples::get<1>(location_at_birth_of_rb);
            at_birth_feat->z = boost::tuples::get<2>(location_at_birth_of_rb);
            at_birth_feat->label = feature->getId();

            _features_at_birth->points.push_back(*at_birth_feat);
        }
        boost::shared_ptr<FeaturePCLwc> predicted_feat(new FeaturePCLwc());
        predicted_feat->x = boost::tuples::get<0>(predicted_location);
        predicted_feat->y = boost::tuples::get<1>(predicted_location);
        predicted_feat->z = boost::tuples::get<2>(predicted_location);
        predicted_feat->label = feature->getId();
        _features_predicted->points.push_back(*predicted_feat);
    }
}

void RBFilter::GetLocationOfFeatureAtBirthOfRB(Feature::Ptr feature,
                                               bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
                                               Feature::Location& location_at_birth) const
{
    const Feature::Trajectory& feat_traj = feature->getTrajectory();

    // Feature age counts the last step (this)
    size_t feature_age = feature->getFeatureAge();
    size_t rigid_body_age = this->_trajectory.size();

    // If the last feature location and the last pose are not contemporary (because we are at the beginning of a loop and we have added the new
    // feature locations to the database but we do not added a new trajectory because that only happens after the correction step), then we add
    // one to the rigid_body_age
    if(!contemporary_last_feat_loc_and_last_pose_in_trajectory)
    {
        rigid_body_age += 1;
    }

    // Feature is younger than RB -> Two options:
    //    - Estimate the motion between feat birthday and current frame as the transformation between RB's birthday
    //    and current "minus" the transformation between RB's birthday and feat's birthday
    //    - Move the initial location of the feature to the RB's birthday using the inverse of the transform between
    //    RB's birthday and feat's birthday. Then move this location to current
    // Using second option
    if (feature_age < rigid_body_age + 1)
    {
        Eigen::Matrix4d transform_from_rb_birth_to_feat_birth = this->_trajectory.at(rigid_body_age - feature_age);

        // This moves the feature from its initial location to where it would be when the rb was born
        Eigen::Matrix4d inverse_transform_from_rb_birth_to_feat_birth = transform_from_rb_birth_to_feat_birth.inverse();
        TransformLocation(feat_traj.at(0), inverse_transform_from_rb_birth_to_feat_birth, location_at_birth);
    }
    else
    {
        location_at_birth = feat_traj.at(feature_age - (rigid_body_age + 1));
        if(this->_id != 0)
        {
            location_at_birth = location_at_birth - _initial_location_of_centroid;
        }
    }

}

RB_id_t RBFilter::getId() const
{
    return this->_id;
}

Eigen::Matrix4d RBFilter::getPose() const
{
    return _pose;
}

Eigen::Matrix<double, 6, 6> RBFilter::getPoseCovariance() const
{
    return _pose_covariance;
}

Eigen::Twistd RBFilter::getVelocity() const
{
    return _velocity;
}

Eigen::Matrix<double, 6, 6> RBFilter::getVelocityCovariance() const
{
    return _velocity_covariance;
}

geometry_msgs::PoseWithCovariancePtr RBFilter::getPoseWithCovariance() const
{
    geometry_msgs::PoseWithCovariancePtr pose_wc_ptr = geometry_msgs::PoseWithCovariancePtr(new geometry_msgs::PoseWithCovariance());
    pose_wc_ptr->pose.position.x = _pose(0,3);
    pose_wc_ptr->pose.position.y = _pose(1,3);
    pose_wc_ptr->pose.position.z = _pose(2,3);
    Eigen::Quaterniond quaternion(_pose.block<3,3>(0,0));
    pose_wc_ptr->pose.orientation.x = quaternion.x();
    pose_wc_ptr->pose.orientation.y = quaternion.y();
    pose_wc_ptr->pose.orientation.z = quaternion.z();
    pose_wc_ptr->pose.orientation.w = quaternion.w();

    for (unsigned int i = 0; i < 6; i++)
        for (unsigned int j = 0; j < 6; j++)
            pose_wc_ptr->covariance[6 * i + j] = _pose_covariance(i, j);

    return pose_wc_ptr;
}

geometry_msgs::TwistWithCovariancePtr RBFilter::getPoseECWithCovariance() const
{
    geometry_msgs::TwistWithCovariancePtr pose_wc_ptr = geometry_msgs::TwistWithCovariancePtr(new geometry_msgs::TwistWithCovariance());
    Eigen::Twistd pose_ec;
    TransformMatrix2Twist(_pose, pose_ec);

    pose_wc_ptr->twist.linear.x = pose_ec.vx();
    pose_wc_ptr->twist.linear.y = pose_ec.vy();
    pose_wc_ptr->twist.linear.z = pose_ec.vz();
    pose_wc_ptr->twist.angular.x = pose_ec.rx();
    pose_wc_ptr->twist.angular.y = pose_ec.ry();
    pose_wc_ptr->twist.angular.z = pose_ec.rz();

    for (unsigned int i = 0; i < 6; i++)
        for (unsigned int j = 0; j < 6; j++)
            pose_wc_ptr->covariance[6 * i + j] = _pose_covariance(i, j);

    return pose_wc_ptr;
}

geometry_msgs::TwistWithCovariancePtr RBFilter::getVelocityWithCovariance() const
{
    // Error in the library. I cannot assign to rx, ry and rz if the function is const
    Eigen::Twistd velocity_copy = _velocity;
    geometry_msgs::TwistWithCovariancePtr vel_t_wc_ptr = geometry_msgs::TwistWithCovariancePtr(new geometry_msgs::TwistWithCovariance());
    vel_t_wc_ptr->twist.linear.x = _velocity.vx();
    vel_t_wc_ptr->twist.linear.y = _velocity.vy();
    vel_t_wc_ptr->twist.linear.z = _velocity.vz();
    vel_t_wc_ptr->twist.angular.x = velocity_copy.rx();
    vel_t_wc_ptr->twist.angular.y = velocity_copy.ry();
    vel_t_wc_ptr->twist.angular.z = velocity_copy.rz();

    for (unsigned int i = 0; i < 6; i++)
        for (unsigned int j = 0; j < 6; j++)
            vel_t_wc_ptr->covariance[6 * i + j] = _velocity_covariance(i, j);

    return vel_t_wc_ptr;
}

std::vector<Eigen::Matrix4d> RBFilter::getTrajectory() const
{
    return this->_trajectory;
}

FeatureCloudPCLwc RBFilter::getPredictedMeasurement(PredictionHypothesis hypothesis)
{
    FeatureCloudPCLwc predicted_measurement;
    switch(hypothesis)
    {
    case BASED_ON_VELOCITY:
        predicted_measurement = *(this->_predicted_measurement_pc_vh);
        break;
    case BASED_ON_BRAKING_EVENT:
        predicted_measurement = *(this->_predicted_measurement_pc_bh);
        break;
    default:
        ROS_ERROR_STREAM_NAMED("RBFilter::getPredictedMeasurement", "Wrong hypothesis. Value " << hypothesis);
        break;
    }
    return predicted_measurement;
}

std::vector<Feature::Id> RBFilter::getSupportingFeatures() const
{
    return this->_supporting_features_ids;
}

int RBFilter::getNumberSupportingFeatures() const
{
    return (int)this->_supporting_features_ids.size();
}

FeaturesDataBase::Ptr RBFilter::getFeaturesDatabase() const
{
    return this->_features_database;
}

void RBFilter::_initializeAuxiliarMatrices()
{
    _pose_covariance = Eigen::Matrix<double, 6, 6>::Zero();
    _velocity_covariance = Eigen::Matrix<double, 6, 6>::Zero();
    for (int i = 0; i < 6; i++)
    {
        _pose_covariance(i, i) = this->_prior_cov_pose;
        _velocity_covariance(i, i) = this->_prior_cov_vel;
    }

    this->_predicted_measurement_pc_vh = FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
    this->_predicted_measurement_pc_bh = FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
    this->_predicted_measurement_pc_kh = FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
    this->_features_at_birth= FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
    this->_features_predicted = FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
    this->_predicted_pose_vh = Eigen::Matrix4d::Identity();
    this->_predicted_pose_bh = Eigen::Matrix4d::Identity();
    this->_predicted_pose_kh = Eigen::Matrix4d::Identity();

    _G_t_memory = Eigen::MatrixXd(6, 3*_num_tracked_feats);
    _R_inv_times_innovation_memory = Eigen::VectorXd(3*_num_tracked_feats);

    _D_T_p0_circle = Eigen::Matrix<double, 3, 6>::Zero();
    _D_T_p0_circle.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
    _D_T_p0_circle.block<3,3>(0,3) = Eigen::Matrix3d::Zero();
}


/*
@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/range.hpp>

#include <boost/hana/comparable/laws.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    BOOST_HANA_CONSTANT_ASSERT(Comparable::laws::check(
        list(
            range(int_<0>, int_<0>),
            range(int_<1>, int_<1>),

            range(int_<0>, int_<1>),
            range(long_<0>, long_<1>),
            range(long_<0>, int_<1>),

            range(int_<3>, int_<6>)
        )
    ));
}


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

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

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

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/buffer.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/geometries/geometries.hpp>

// For test
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

// This unit test tests boost::geometry::buffer (overload with strategies)
// More detailed tests are, per geometry type, available in buffer_<TYPE>.cpp
// (testing boost::geometry::buffer_inserter)

// SVG's are not created (they are in detailed tests)

static std::string const polygon_simplex = "POLYGON ((0 0,1 5,6 1,0 0))";
static std::string const polygon_empty = "POLYGON()";
static std::string const multi_polygon_empty = "MULTIPOLYGON()";
static std::string const multi_polygon_simplex
    = "MULTIPOLYGON(((0 1,2 5,5 3,0 1)),((1 1,5 2,5 0,1 1)))";

template
<
    typename Geometry,
    typename GeometryOut,
    typename JoinStrategy,
    typename EndStrategy,
    typename SideStrategy,
    typename PointStrategy,
    typename DistanceStrategy
>
void test_with_strategies(std::string const& caseid,
        std::string const& wkt,
        JoinStrategy const& join_strategy,
        EndStrategy const& end_strategy,
        SideStrategy const& side_strategy,
        PointStrategy const& point_strategy,
        DistanceStrategy const& distance_strategy,
        double expected_area,
        std::size_t expected_numpoints,
        double tolerance = 0.01)
{
    namespace bg = boost::geometry;
    Geometry g;
    bg::read_wkt(wkt, g);
    bg::correct(g);

    GeometryOut result;

    bg::buffer(g, result,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, point_strategy);

    BOOST_CHECK_MESSAGE
        (
            bg::num_points(result) == expected_numpoints,
            "Buffer " << caseid
            << " numpoints expected: " << expected_numpoints
            << " detected: " << bg::num_points(result)
        );

    double const area = bg::area(result);
    double const difference = area - expected_area;

    BOOST_CHECK_MESSAGE
        (
            bg::math::abs(difference) < tolerance,
            "Buffer " << caseid
            << std::setprecision(12)
            << " area expected: " << expected_area
            << " detected: " << area
        );
}

template <bool Clockwise, typename Point>
void test_all()
{
    typedef bg::model::polygon<Point, Clockwise> polygon;
    typedef bg::model::multi_polygon<polygon> multi_polygon;

    bg::strategy::buffer::join_round join(160);
    bg::strategy::buffer::end_round end(160);
    bg::strategy::buffer::point_circle circle(160);
    bg::strategy::buffer::side_straight side;

    typedef bg::strategy::buffer::distance_symmetric
    <
        typename bg::coordinate_type<Point>::type
    > distance;

    test_with_strategies<multi_polygon, multi_polygon>(
        "multi_polygon_empty", multi_polygon_empty,
        join, end, side, circle, distance(1.0),
        0.0, 0);

    // PostGIS: 34.2550669294223 216 (40 / qcircle)
    // SQL Server: 34.2550419903829 220 (default options)
    test_with_strategies<multi_polygon, multi_polygon>(
        "multi_polygon_simplex", multi_polygon_simplex,
        join, end, side, circle, distance(1.0),
        34.2551, 219);

    test_with_strategies<polygon, multi_polygon>(
        "polygon_empty", polygon_empty,
        join, end, side, circle, distance(1.0),
        0.0, 0);

    //
    // PostGIS: 35.2256914798762 164 (40 / qcircle)
    // SQL Server: 35.2252355201605 153 (default options)
    test_with_strategies<polygon, multi_polygon>(
        "polygon_simplex", polygon_simplex,
        join, end, side, circle, distance(1.0),
        35.2257, 166);
}

int test_main(int, char* [])
{
    test_all<true, bg::model::point<double, 2, bg::cs::cartesian> >();
    return 0;
}


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/chrono/duration.hpp>

#include <boost/detail/lightweight_test.hpp>
#ifdef BOOST_NO_CXX11_CONSTEXPR
#define BOOST_CONSTEXPR_ASSERT(C) BOOST_TEST(C)
#else
#include <boost/static_assert.hpp>
#define BOOST_CONSTEXPR_ASSERT(C) BOOST_STATIC_ASSERT(C)
#endif

int main()
{
  // Default construct
   {
      //BOOST_CONSTEXPR
      boost::chrono::minutes m;
      //BOOST_CONSTEXPR_ASSERT(m.count() == 0);
      (void)m;
   }

  // UNARY PLUS
  {
    boost::chrono::minutes m(3);
    boost::chrono::minutes m2 = +m;
    BOOST_TEST(m.count() == m2.count());
  }
  {
    BOOST_CONSTEXPR boost::chrono::minutes m(3);
    BOOST_CONSTEXPR boost::chrono::minutes m2(+m);
    BOOST_CONSTEXPR_ASSERT(m.count() == m2.count());
  }

  // UNARY MINUS
  {
    boost::chrono::minutes m(3);
    boost::chrono::minutes m2 = -m;
    BOOST_TEST(m2.count() == -m.count());
  }
  {
    BOOST_CONSTEXPR boost::chrono::minutes m(3);
    BOOST_CONSTEXPR boost::chrono::minutes m2 = -m;
    BOOST_CONSTEXPR_ASSERT(m2.count() == -m.count());
  }
  // PRE INCREMENT
  {
    boost::chrono::hours h(3);
    boost::chrono::hours& href = ++h;
    BOOST_TEST(&href == &h);
    BOOST_TEST(h.count() == 4);
  }
  // POST INCREMENT
  {
    boost::chrono::hours h(3);
    boost::chrono::hours h2 = h++;
    BOOST_TEST(h.count() == 4);
    BOOST_TEST(h2.count() == 3);
  }
  // PRE DECREMENT
  {
    boost::chrono::hours h(3);
    boost::chrono::hours& href = --h;
    BOOST_TEST(&href == &h);
    BOOST_TEST(h.count() == 2);
  }
  // POST DECREMENT
  {
    boost::chrono::hours h(3);
    boost::chrono::hours h2 = h--;
    BOOST_TEST(h.count() == 2);
    BOOST_TEST(h2.count() == 3);
  }
  // PLUS ASSIGN
  {
    boost::chrono::seconds s(3);
    s += boost::chrono::seconds(2);
    BOOST_TEST(s.count() == 5);
    s += boost::chrono::minutes(2);
    BOOST_TEST(s.count() == 125);
  }
  // MINUS ASSIGN
  {
    boost::chrono::seconds s(3);
    s -= boost::chrono::seconds(2);
    BOOST_TEST(s.count() == 1);
    s -= boost::chrono::minutes(2);
    BOOST_TEST(s.count() == -119);
  }
  // TIMES ASSIGN
  {
    boost::chrono::nanoseconds ns(3);
    ns *= 5;
    BOOST_TEST(ns.count() == 15);
  }
  // DIVIDE ASSIGN
  {
    boost::chrono::nanoseconds ns(15);
    ns /= 5;
    BOOST_TEST(ns.count() == 3);
  }
  // MODULUS ASSIGN duration
  {
    boost::chrono::microseconds us(11);
    boost::chrono::microseconds us2(3);
    us %= us2;
    BOOST_TEST(us.count() == 2);
    us %= boost::chrono::milliseconds(3);
    BOOST_TEST(us.count() == 2);
  }
  // MODULUS ASSIGN Rep
  {
    boost::chrono::microseconds us(11);
    us %= 3;
    BOOST_TEST(us.count() == 2);
  }
  // PLUS
  {
    boost::chrono::seconds s1(3);
    boost::chrono::seconds s2(5);
    boost::chrono::seconds r = s1 + s2;
    BOOST_TEST(r.count() == 8);
  }
  {
    BOOST_CONSTEXPR boost::chrono::seconds s1(3);
    BOOST_CONSTEXPR boost::chrono::seconds s2(5);
    BOOST_CONSTEXPR boost::chrono::seconds r = s1 + s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == 8);
  }
  {
    boost::chrono::seconds s1(3);
    boost::chrono::microseconds s2(5);
    boost::chrono::microseconds r = s1 + s2;
    BOOST_TEST(r.count() == 3000005);
  }
  {
    BOOST_CONSTEXPR boost::chrono::seconds s1(3);
    BOOST_CONSTEXPR boost::chrono::microseconds s2(5);
    BOOST_CONSTEXPR boost::chrono::microseconds r = s1 + s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == 3000005);
  }
  {
    boost::chrono::duration<int, boost::ratio<2, 3> > s1(3);
    boost::chrono::duration<int, boost::ratio<3, 5> > s2(5);
    boost::chrono::duration<int, boost::ratio<1, 15> > r = s1 + s2;
    BOOST_TEST(r.count() == 75);
  }
  {
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<2, 3> > s1(3);
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<3, 5> > s2(5);
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<1, 15> > r = s1 + s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == 75);
  }
  {
    boost::chrono::duration<int, boost::ratio<2, 3> > s1(3);
    boost::chrono::duration<double, boost::ratio<3, 5> > s2(5);
    boost::chrono::duration<double, boost::ratio<1, 15> > r = s1 + s2;
    BOOST_TEST(r.count() == 75);
  }
  {
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<2, 3> > s1(3);
    BOOST_CONSTEXPR boost::chrono::duration<double, boost::ratio<3, 5> > s2(5);
    BOOST_CONSTEXPR boost::chrono::duration<double, boost::ratio<1, 15> > r = s1 + s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == 75);
  }

  // MINUS
  {
    boost::chrono::seconds s1(3);
    boost::chrono::seconds s2(5);
    boost::chrono::seconds r = s1 - s2;
    BOOST_TEST(r.count() == -2);
  }
  {
    BOOST_CONSTEXPR boost::chrono::seconds s1(3);
    BOOST_CONSTEXPR boost::chrono::seconds s2(5);
    BOOST_CONSTEXPR boost::chrono::seconds r = s1 - s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == -2);
  }
  {
    boost::chrono::seconds s1(3);
    boost::chrono::microseconds s2(5);
    boost::chrono::microseconds r = s1 - s2;
    BOOST_TEST(r.count() == 2999995);
  }
  {
    BOOST_CONSTEXPR boost::chrono::seconds s1(3);
    BOOST_CONSTEXPR boost::chrono::microseconds s2(5);
    BOOST_CONSTEXPR boost::chrono::microseconds r = s1 - s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == 2999995);
  }
  {
    boost::chrono::duration<int, boost::ratio<2, 3> > s1(3);
    boost::chrono::duration<int, boost::ratio<3, 5> > s2(5);
    boost::chrono::duration<int, boost::ratio<1, 15> > r = s1 - s2;
    BOOST_TEST(r.count() == -15);
  }
  {
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<2, 3> > s1(3);
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<3, 5> > s2(5);
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<1, 15> > r = s1 - s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == -15);
  }
  {
    boost::chrono::duration<int, boost::ratio<2, 3> > s1(3);
    boost::chrono::duration<double, boost::ratio<3, 5> > s2(5);
    boost::chrono::duration<double, boost::ratio<1, 15> > r = s1 - s2;
    BOOST_TEST(r.count() == -15);
  }
  {
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<2, 3> > s1(3);
    BOOST_CONSTEXPR boost::chrono::duration<double, boost::ratio<3, 5> > s2(5);
    BOOST_CONSTEXPR boost::chrono::duration<double, boost::ratio<1, 15> > r = s1 - s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == -15);
  }

  // TIMES rep
  {
    boost::chrono::nanoseconds ns(3);
    boost::chrono::nanoseconds ns2 = ns * 5;
    BOOST_TEST(ns2.count() == 15);
    boost::chrono::nanoseconds ns3 = 6 * ns2;
    BOOST_TEST(ns3.count() == 90);
  }
  {
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns(3);
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns2 = ns * 5;
    BOOST_CONSTEXPR_ASSERT(ns2.count() == 15);
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns3 = 6 * ns2;
    BOOST_CONSTEXPR_ASSERT(ns3.count() == 90);
  }

  // DIVIDE duration
  {
    boost::chrono::nanoseconds ns1(15);
    boost::chrono::nanoseconds ns2(5);
    BOOST_TEST(ns1 / ns2 == 3);
  }
  {
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns1(15);
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns2(5);
    BOOST_CONSTEXPR_ASSERT(ns1 / ns2 == 3);
  }
  {
    boost::chrono::microseconds us1(15);
    boost::chrono::nanoseconds ns2(5);
    BOOST_TEST(us1 / ns2 == 3000);
  }
  {
    BOOST_CONSTEXPR boost::chrono::microseconds us1(15);
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns2(5);
    BOOST_CONSTEXPR_ASSERT(us1 / ns2 == 3000);
  }
  {
    boost::chrono::duration<int, boost::ratio<2, 3> > s1(30);
    boost::chrono::duration<int, boost::ratio<3, 5> > s2(5);
    BOOST_TEST(s1 / s2 == 6);
  }
  {
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<2, 3> > s1(30);
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<3, 5> > s2(5);
    BOOST_CONSTEXPR_ASSERT(s1 / s2 == 6);
  }
  {
    boost::chrono::duration<int, boost::ratio<2, 3> > s1(30);
    boost::chrono::duration<double, boost::ratio<3, 5> > s2(5);
    //BOOST_TEST(s1 / s2 == 20. / 3);
    BOOST_TEST(3 * s1 == 20 * s2);
  }
  {
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<2, 3> > s1(30);
    BOOST_CONSTEXPR boost::chrono::duration<double, boost::ratio<3, 5> > s2(5);
    //BOOST_CONSTEXPR_ASSERT(s1 / s2 == 20. / 3);
    BOOST_TEST(3 * s1 == 20 * s2);
  }
  // DIVIDE rep
  {
    boost::chrono::nanoseconds ns(15);
    boost::chrono::nanoseconds ns2 = ns / 5;
    BOOST_TEST(ns2.count() == 3);
  }
  {
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns(15);
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns2 = ns / 5;
    BOOST_CONSTEXPR_ASSERT(ns2.count() == 3);
  }

  // MODULUS duration

  {
    boost::chrono::nanoseconds ns1(15);
    boost::chrono::nanoseconds ns2(6);
    boost::chrono::nanoseconds r = ns1 % ns2;
    BOOST_TEST(r.count() == 3);
  }
  {
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns1(15);
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns2(6);
    BOOST_CONSTEXPR boost::chrono::nanoseconds r = ns1 % ns2;
    BOOST_CONSTEXPR_ASSERT(r.count() == 3);
  }
  {
    boost::chrono::microseconds us1(15);
    boost::chrono::nanoseconds ns2(28);
    boost::chrono::nanoseconds r = us1 % ns2;
    BOOST_TEST(r.count() == 20);
  }
  {
    BOOST_CONSTEXPR boost::chrono::microseconds us1(15);
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns2(28);
    BOOST_CONSTEXPR boost::chrono::nanoseconds r = us1 % ns2;
    BOOST_CONSTEXPR_ASSERT(r.count() == 20);
  }
  {
    boost::chrono::duration<int, boost::ratio<3, 5> > s1(6);
    boost::chrono::duration<int, boost::ratio<2, 3> > s2(3);
    boost::chrono::duration<int, boost::ratio<1, 15> > r = s1 % s2;
    BOOST_TEST(r.count() == 24);
  }
  {
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<3, 5> > s1(6);
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<2, 3> > s2(3);
    BOOST_CONSTEXPR boost::chrono::duration<int, boost::ratio<1, 15> > r = s1 % s2;
    BOOST_CONSTEXPR_ASSERT(r.count() == 24);
  }
  // MODULUS rep
  {
    boost::chrono::nanoseconds ns(15);
    boost::chrono::nanoseconds ns2 = ns % 6;
    BOOST_TEST(ns2.count() == 3);
  }
  {
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns(15);
    BOOST_CONSTEXPR boost::chrono::nanoseconds ns2 = ns % 6;
    BOOST_CONSTEXPR_ASSERT(ns2.count() == 3);
  }

  return boost::report_errors();
}


// Copyright (c) 2014-2015 The Pebblecoin developers
// Copyright (c) 2012-2013 The Cryptonote developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/foreach.hpp>

#include "cryptonote_core/cryptonote_format_utils.h"

#include "split_strategies.h"

namespace tools {
namespace detail {
//----------------------------------------------------------------------------------------------------
void digit_split_strategy::split(const std::vector<cryptonote::tx_destination_entry>& dsts,
                                 const cryptonote::tx_destination_entry& change_dst, uint64_t dust_threshold,
                                 std::vector<cryptonote::tx_destination_entry>& splitted_dsts, uint64_t& dust) const
{
  splitted_dsts.clear();
  dust = 0;

  BOOST_FOREACH(auto& de, dsts)
  {
    cryptonote::decompose_amount_into_digits(de.amount, dust_threshold,
      [&](uint64_t chunk) { splitted_dsts.push_back(cryptonote::tx_destination_entry(de.cp, chunk, de.addr)); },
      [&](uint64_t a_dust) { splitted_dsts.push_back(cryptonote::tx_destination_entry(de.cp, a_dust, de.addr)); } );
  }

  cryptonote::decompose_amount_into_digits(change_dst.amount, dust_threshold,
    [&](uint64_t chunk) { splitted_dsts.push_back(cryptonote::tx_destination_entry(change_dst.cp, chunk, change_dst.addr)); },
    [&](uint64_t a_dust) { dust = a_dust; } );
}
//----------------------------------------------------------------------------------------------------
void null_split_strategy::split(const std::vector<cryptonote::tx_destination_entry>& dsts,
                                const cryptonote::tx_destination_entry& change_dst, uint64_t dust_threshold,
                                std::vector<cryptonote::tx_destination_entry>& splitted_dsts, uint64_t& dust) const
{
  splitted_dsts = dsts;

  dust = 0;
  uint64_t change = change_dst.amount;
  if (0 < dust_threshold)
  {
    for (uint64_t order = 10; order <= 10 * dust_threshold; order *= 10)
    {
      uint64_t dust_candidate = change_dst.amount % order;
      uint64_t change_candidate = (change_dst.amount / order) * order;
      if (dust_candidate <= dust_threshold)
      {
        dust = dust_candidate;
        change = change_candidate;
      }
      else
      {
        break;
      }
    }
  }

  if (0 != change)
  {
    splitted_dsts.push_back(cryptonote::tx_destination_entry(change_dst.cp, change, change_dst.addr));
  }
}
//----------------------------------------------------------------------------------------------------
} }


#include <random>
#include <vector>

#include <Eigen/Core>
#include <gtest/gtest.h>

#include "test/utils.hpp"

#include "ldaplusplus/LDABuilder.hpp"
#include "ldaplusplus/LDA.hpp"
#include "ldaplusplus/events/ProgressEvents.hpp"

using namespace Eigen;
using namespace ldaplusplus;

// T will be available as TypeParam in TYPED_TEST functions
template <typename T>
class TestFit : public ParameterizedTest<T> {};

TYPED_TEST_CASE(TestFit, ForFloatAndDouble);

TYPED_TEST(TestFit, partial_fit) {
    // Build the corpus
    std::mt19937 rng;
    rng.seed(0);
    MatrixXi X(100, 50);
    VectorXi y(50);
    std::uniform_int_distribution<> class_generator(0, 5);
    std::exponential_distribution<> words_generator(0.1);
    for (int d=0; d<50; d++) {
        for (int w=0; w<100; w++) {
            X(w, d) = static_cast<int>(words_generator(rng));
        }
        y(d) = class_generator(rng);
    }

    LDA<TypeParam> lda = LDABuilder<TypeParam>().
            set_fast_supervised_e_step(10, 1e-2, 10).
            set_fast_supervised_m_step(10, 1e-2).
            initialize_topics_seeded(X, 10).
            initialize_eta_zeros(y.maxCoeff() + 1);

    TypeParam likelihood0, likelihood=0, py0, py;
    lda.get_event_dispatcher()->add_listener(
        [&likelihood, &py](std::shared_ptr<events::Event> event) {
            //if (event->id() == "ExpectationProgressEvent") {
            //    auto progress = std::static_pointer_cast<events::ExpectationProgressEvent<TypeParam> >(event);
            //    likelihood += progress->likelihood();
            //}

            if (event->id() == "MaximizationProgressEvent") {
                auto progress = std::static_pointer_cast<events::MaximizationProgressEvent<TypeParam> >(event);
                py = progress->likelihood();
            }
        }
    );

    lda.partial_fit(X, y);
    likelihood0 = likelihood;
    likelihood = 0;
    py0 = py;
    lda.partial_fit(X, y);

    //EXPECT_GT(likelihood, likelihood0);
    EXPECT_GT(py, py0);
}


#include <iostream>
#include <ros/ros.h>
#include <vector>
#include <math.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h> //for sensor_msgs::PointCloud2
#include <geometry_msgs/TransformStamped.h>

#include <pcl/point_types.h> // for pcl::PointXYZ
#include <pcl_conversions/pcl_conversions.h> // for pcl::fromROSMsg()

#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
// #include <pcl_ros/transforms.hpp>

#include <pcl/segmentation/sac_segmentation.h> //for SACSegmentation
#include <pcl/filters/extract_indices.h> //for ExtractIndices
#include <pcl/filters/statistical_outlier_removal.h>

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

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

#include <Eigen/Dense>

ros::Publisher org_low_frq_pub;
ros::Publisher processed_pcl_pub;

// with PointCloud2Ptr not working, but with PointCloud2::ConstPtr it works
// void sync_callback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& pcl_in, const nav_msgs::Odometry::ConstPtr &pose_in)

void sync_callback(const  sensor_msgs::PointCloud2::ConstPtr &pcl_in, const nav_msgs::Odometry::ConstPtr &pose_in)
{
  // Solve all of perception here...
  ROS_INFO("\n\nrcvd msg ... ");

  ROS_INFO_STREAM("pcl  time stamp: "<< pcl_in->header.stamp);
  ROS_INFO_STREAM("pose time stamp: "<< pose_in->header.stamp);
  // ROS_INFO_STREAM("pcl  frame     : "<< pcl_in->header.frame_id);
  // ROS_INFO_STREAM("pose position  : "<< pose_in->pose.pose.position.x <<" "<<
                                      // pose_in->pose.pose.position.y <<" "<< pose_in->pose.pose.position.z);
  // ROS_INFO_STREAM("pose orientatio: "<< pose_in->pose.pose.orientation.x <<" "<<pose_in->pose.pose.orientation.y <<" "<<
                                      // pose_in->pose.pose.orientation.z <<" "<< pose_in->pose.pose.orientation.w);

  //convert the input into a pcl::PointCloud< pcl::PointXYZ> object
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);

  // variables  for filtered/transformed/low_frq  pcl
  pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr low_frq_cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg(*pcl_in, *cloudPtr);

  //*********** outlier remover ***********
  // pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  // sor.setInputCloud (cloudPtr);
  // sor.setMeanK (50);
  // sor.setStddevMulThresh (1.0);
  // sor.filter (*filtered_cloudPtr);

  //*********** pcl to std vector ***********
  pcl::PointCloud<pcl::PointXYZI> org_pcl_vec, fr_pcl_vec;
  std::copy(cloudPtr->points.begin(), cloudPtr->points.end(),std::back_inserter(org_pcl_vec));

  ROS_INFO_STREAM("hight: "<< pcl_in->height);
  ROS_INFO_STREAM("width: "<< pcl_in->width);
  ROS_INFO_STREAM("point_step: "<< pcl_in->point_step);
  ROS_INFO_STREAM("row_step: "<< pcl_in->row_step);
  ROS_INFO_STREAM("cloudPtr [0]: "<< cloudPtr->points.size());
  ROS_INFO_STREAM("org_pcl_vec: "<< org_pcl_vec.size() );
  // ROS_INFO_STREAM("org_pcl_vec : 1st pt: "<<org_pcl_vec[0].x<<", "<< org_pcl_vec[0].y<<", "<<org_pcl_vec[0].z<<", "<<org_pcl_vec[0].intensity);

  //*********** clustering ***********
  // next step

  //*********** adding free points ***********
  const float rslujn = 0.2;
  for(int pt=0; pt<org_pcl_vec.size(); pt++){
    org_pcl_vec[pt].intensity = 1.0;
    float bm_lngth = sqrt(org_pcl_vec[pt].x*org_pcl_vec[pt].x + org_pcl_vec[pt].y*org_pcl_vec[pt].y + org_pcl_vec[pt].z*org_pcl_vec[pt].z );
    float bm_nrml[3] = {org_pcl_vec[pt].x/bm_lngth, org_pcl_vec[pt].y/bm_lngth, org_pcl_vec[pt].z/bm_lngth };
    pcl::PointXYZI fr_pt;

    if(bm_lngth < (2*rslujn)){
      fr_pt.x = bm_nrml[0]*bm_lngth/2.0;
      fr_pt.y = bm_nrml[1]*bm_lngth/2.0;
      fr_pt.z = bm_nrml[2]*bm_lngth/2.0;
      fr_pt.intensity = 0.0;
      fr_pcl_vec.push_back(fr_pt);
    }
    else{
      // fr_pt.x = bm_nrml[0]*rslujn;
      // fr_pt.y = bm_nrml[1]*rslujn;
      // fr_pt.z = bm_nrml[2]*rslujn;
      // fr_pt.intensity = 0.0;
      // fr_pcl_vec.push_back(fr_pt);

      fr_pt.x = bm_nrml[0]*(bm_lngth - rslujn);
      fr_pt.y = bm_nrml[1]*(bm_lngth - rslujn);
      fr_pt.z = bm_nrml[2]*(bm_lngth - rslujn);
      fr_pt.intensity = 0.0;
      fr_pcl_vec.push_back(fr_pt);

    }
  }
  ROS_INFO_STREAM("fr_pcl_vec: "<< fr_pcl_vec.size() );

  //*********** add fr tp org pcl_vec ***********
  // std::copy (b.begin(), b.end(), std::back_inserter(a));
  // a.insert(a.end(), b.begin(), b.end());
  org_pcl_vec.insert(org_pcl_vec.end(), fr_pcl_vec.begin(), fr_pcl_vec.end());

  ROS_INFO_STREAM("org+fr pcl_vec: "<< org_pcl_vec.size() );

    // dist = np.sqrt(x**2 + y**2 + z**2)
    // theta = np.arctan2(y, x)
    // alpha = np.arccos(z / dist)

    // x = np.array( dist * np.sin(alpha) * np.cos(theta), dtype='float32')
    // y = np.array( dist * np.sin(alpha) * np.sin(theta), dtype='float32')
    // z = np.array( dist * np.cos(alpha) , dtype='float32')

  //*********** std vector to pcl ***********
  // std::vector<pcl::PointXYZI> org_pcl_vector;
  // std::copy(org_pcl_vec.begin(), org_pcl_vec.end(),std::back_inserter(org_pcl_vector));
  // ROS_INFO_STREAM("org_pcl_vector: "<< org_pcl_vector.size());

  //*********** from velodyne to world frame ***********
  Eigen::Quaternionf rotation(pose_in->pose.pose.orientation.w, pose_in->pose.pose.orientation.x, pose_in->pose.pose.orientation.y,
                              pose_in->pose.pose.orientation.z);
  Eigen::Vector3f origin(pose_in->pose.pose.position.x, pose_in->pose.pose.position.y, pose_in->pose.pose.position.z+0.322);

  // Eigen::Quaternionf rotation(1,0,0,0);
  // Eigen::Vector3f origin(0,0,0);

  transformPointCloud(org_pcl_vec, *transformed_cloudPtr, origin, rotation);
  transformed_cloudPtr->header.frame_id = "world"; //should be after transform otherwise aha
  // transformed_cloudPtr->header.frame_id = "velodyne"; //should be after transform otherwise aha

  // ROS_INFO_STREAM("transformed_cloudPtr: "<< transformed_cloudPtr->points.size());
  // ROS_INFO_STREAM("transformed_cloudPtr: "<< transformed_cloudPtr->header.frame_id);

//*********** from pcl to ros msg ***********
  sensor_msgs::PointCloud2 pcl_msg_org_fr;
  pcl::toROSMsg(*transformed_cloudPtr, pcl_msg_org_fr);
  processed_pcl_pub.publish(pcl_msg_org_fr);

  // ROS_INFO_STREAM("hight: "<< pcl_msg_org_fr.height);
  // ROS_INFO_STREAM("width: "<< pcl_msg_org_fr.width);
  // ROS_INFO_STREAM("point_step: "<< pcl_msg_org_fr.point_step);
  // ROS_INFO_STREAM("row_step: "<< pcl_msg_org_fr.row_step);

    // sensor_msgs::PointCloud2 low_frq_pcl_msg;
    // pcl::toROSMsg(*filtered_cloudPtr, low_frq_pcl_msg);
    // org_low_frq_pub.publish(low_frq_pcl_msg);
    ros::Duration(0.3).sleep();

}

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

  ros::NodeHandle nh;

  //create publisher object
  processed_pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/pcl_in_wrld_fr", 1);
  org_low_frq_pub = nh.advertise<sensor_msgs::PointCloud2>("/org_low_frq_pcl", 1);

  // message filter time synchronization
  message_filters::Subscriber<nav_msgs::Odometry> pose_sub(nh, "/ground_truth/state", 1);
  message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(nh, "/mid/points", 1);
  message_filters::TimeSynchronizer<sensor_msgs::PointCloud2, nav_msgs::Odometry> sync(pcl_sub, pose_sub, 10);

  sync.registerCallback(boost::bind(&sync_callback, _1, _2)); // try std instead boost not working as well
  ros::spin();

  return 0;
}


/*
Copyright 2021 Dennis Rohde

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 <vector>
#include <limits>

#include <boost/chrono/include.hpp>

#include "dynamic_time_warping.hpp"

namespace Dynamic_Time_Warping {

namespace Discrete {

std::string Distance::repr() const {
    std::stringstream ss;
    ss << value;
    return ss.str();
}

Distance distance(const Curve &curve1, const Curve &curve2) {
    Distance result;
    auto start = boost::chrono::process_real_cpu_clock::now();
    std::vector<std::vector<distance_t>> a(curve1.complexity() + 1, std::vector<distance_t>(curve2.complexity() + 1, std::numeric_limits<distance_t>::infinity()));
    a[0][0] = 0;
    for (curve_size_t i = 1; i <= curve1.complexity(); ++i) {
        for (curve_size_t j = 1; j <= curve2.complexity(); ++j) {
            a[i][j] = std::sqrt(curve1[i-1].dist_sqr(curve2[j-1]));
            a[i][j] += std::min(std::min(a[i-1][j], a[i][j-1]), a[i-1][j-1]);
        }
    }
    auto value = a[curve1.complexity()][curve2.complexity()];
    auto end = boost::chrono::process_real_cpu_clock::now();
    result.time = (end-start).count() / 1000000000.0;
    result.value = value;
    return result;

}

} // end namespace Discrete

} // end namespace Dynamic Time Warping


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

#ifndef UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
#define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_

#include <autoware_planning_msgs/msg/path_point.hpp>
#include <autoware_planning_msgs/msg/path_point_with_lane_id.hpp>
#include <autoware_planning_msgs/msg/trajectory_point.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/polygon.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>

#include <boost/assign/list_of.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/segment.hpp>

#include <lanelet2_core/primitives/Polygon.h>
#include <tf2/utils.h>

#include <algorithm>
#include <vector>

// cppcheck-suppress unknownMacro
BOOST_GEOMETRY_REGISTER_POINT_3D(geometry_msgs::msg::Point, double, cs::cartesian, x, y, z)
BOOST_GEOMETRY_REGISTER_POINT_3D(
  geometry_msgs::msg::PoseWithCovarianceStamped, double, cs::cartesian, pose.pose.position.x,
  pose.pose.position.y, pose.pose.position.z)
BOOST_GEOMETRY_REGISTER_POINT_3D(
  autoware_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, pose.position.y,
  pose.position.z)
BOOST_GEOMETRY_REGISTER_POINT_3D(
  autoware_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, point.pose.position.x,
  point.pose.position.y, point.pose.position.z)
BOOST_GEOMETRY_REGISTER_POINT_3D(
  autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x,
  pose.position.y, pose.position.z)

namespace behavior_velocity_planner
{
using Point2d = boost::geometry::model::d2::point_xy<double>;
using Segment2d = boost::geometry::model::segment<Point2d>;
using LineString2d = boost::geometry::model::linestring<Point2d>;
using Polygon2d =
  boost::geometry::model::polygon<Point2d, false, false>;  // counter-clockwise, open

template <class T>
Point2d to_bg2d(const T & p)
{
  return Point2d(boost::geometry::get<0>(p), boost::geometry::get<1>(p));
}

template <class T>
LineString2d to_bg2d(const std::vector<T> & vec)
{
  LineString2d ps;
  for (const auto & p : vec) {
    ps.push_back(to_bg2d(p));
  }
  return ps;
}

inline Polygon2d linestring2polygon(const LineString2d & line_string)
{
  Polygon2d polygon;

  for (const auto & p : line_string) {
    polygon.outer().push_back(p);
  }

  return polygon;
}

inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line)
{
  Polygon2d polygon;

  polygon.outer().push_back(left_line.front());

  for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) {
    polygon.outer().push_back(*itr);
  }

  for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) {
    polygon.outer().push_back(*itr);
  }

  return polygon;
}

inline Polygon2d obj2polygon(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & shape)
{
  // rename
  const double x = pose.position.x;
  const double y = pose.position.y;
  const double h = shape.x;
  const double w = shape.y;
  const double yaw = tf2::getYaw(pose.orientation);

  // create base polygon
  Polygon2d obj_poly;
  boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of<Point2d>(h / 2.0, w / 2.0)(
    -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0);

  // rotate polygon(yaw)
  boost::geometry::strategy::transform::rotate_transformer<boost::geometry::radian, double, 2, 2>
    rotate(-yaw);  // anti-clockwise -> :clockwise rotation
  Polygon2d rotate_obj_poly;
  boost::geometry::transform(obj_poly, rotate_obj_poly, rotate);

  // translate polygon(x, y)
  boost::geometry::strategy::transform::translate_transformer<double, 2, 2> translate(x, y);
  Polygon2d translate_obj_poly;
  boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate);
  return translate_obj_poly;
}

inline double calcOverlapAreaRate(const Polygon2d & target, const Polygon2d & base)
{
  /* OverlapAreaRate: common area(target && base) / target area */

  if (boost::geometry::within(target, base)) {
    // target is within base, common area = target area
    return 1.0;
  }

  if (!boost::geometry::intersects(target, base)) {
    // target and base has not intersect area
    return 0.0;
  }

  // calculate intersect polygon
  std::vector<Polygon2d> intersects;
  boost::geometry::intersection(target, base, intersects);

  // calculate area of polygon
  double intersect_area = 0.0;
  for (const auto & intersect : intersects) {
    intersect_area += boost::geometry::area(intersect);
  }
  const double target_area = boost::geometry::area(target);
  // specification of boost1.65
  // common area is not intersect area
  const double common_area = target_area - intersect_area;

  return common_area / target_area;
}

inline std::vector<Segment2d> makeSegments(const LineString2d & ls)
{
  std::vector<Segment2d> segments;
  for (size_t i = 0; i < ls.size(); ++i) {
    segments.emplace_back(ls.at(i), ls.at(i + 1));
  }
  return segments;
}

inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon)
{
  geometry_msgs::msg::Polygon polygon_msg;
  geometry_msgs::msg::Point32 point_msg;
  for (const auto & p : polygon.outer()) {
    point_msg.x = p.x();
    point_msg.y = p.y();
    polygon_msg.points.push_back(point_msg);
  }
  return polygon_msg;
}

inline Polygon2d toBoostPoly(const geometry_msgs::msg::Polygon & polygon)
{
  Polygon2d boost_poly;
  for (const auto & point : polygon.points) {
    const Point2d point2d(point.x, point.y);
    boost_poly.outer().push_back(point2d);
  }
  return boost_poly;
}

inline Polygon2d toBoostPoly(const lanelet::BasicPolygon2d & polygon)
{
  Polygon2d boost_poly;
  for (const auto & vec : polygon) {
    const Point2d point2d(vec.x(), vec.y());
    boost_poly.outer().push_back(point2d);
  }

  return boost_poly;
}
}  // namespace behavior_velocity_planner

#endif  // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_LAPACK_GENERAL_GEMSV_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_LAPACK_GENERAL_GEMSV_HPP_INCLUDED

#include <nt2/linalg/functions/gemsv.hpp>
#include <nt2/sdk/memory/container.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/linalg/details/utility/workspace.hpp>

#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/width.hpp>

#include <nt2/sdk/meta/as_real.hpp>
#include <boost/dispatch/meta/downgrade.hpp>

extern "C"
{
  void NT2_F77NAME(zcgesv)( const nt2_la_int* n    , const nt2_la_int* nrhs
                         , nt2_la_complex* a       , const nt2_la_int* lda
                         , nt2_la_int* ipiv        , const nt2_la_complex* b
                         , const nt2_la_int* ldb   , nt2_la_complex* x
                         , const nt2_la_int* ldx   , nt2_la_complex* work
                         , nt2_la_complex* swork   , double* rwork
                         , nt2_la_int* iter        , nt2_la_int* info
                         );

  void NT2_F77NAME(dsgesv)( const nt2_la_int* n, const nt2_la_int* nrhs
                         , double* a           , const nt2_la_int* lda
                         , nt2_la_int* ipiv
                         , const double* b     , const nt2_la_int* ldb
                         , double* x           , const nt2_la_int* ldx
                         , double* work        , float* swork
                         , nt2_la_int* iter    , nt2_la_int* info
                         );
}

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( gemsv_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)
                            , ((container_< nt2::tag::table_, double_<A0>, S0 >)) // A
                              ((container_< nt2::tag::table_, double_<A1>, S1 >)) // B
                              ((container_< nt2::tag::table_, double_<A2>, S2 >)) // X
                            )
  {
     typedef nt2_la_int result_type;
     typedef typename A0::value_type v_t;
     typedef typename boost::dispatch::meta::downgrade<v_t>::type f_t;

     BOOST_FORCEINLINE result_type operator()(A0& a0, A1 const& a1, A2& a2) const
     {
        details::workspace<typename A2::value_type> w;
        nt2_la_int  n  = std::min(nt2::height(a0),nt2::width(a0));
        nt2_la_int  lda = n;
        nt2_la_int  nhrs = nt2::width(a1);
        nt2_la_int  ldb = a1.leading_size();
        nt2_la_int iter,info;

        nt2::memory::container<tag::table_, f_t, nt2::_2D> swork(nt2::of_size(n*(n+nhrs),1));
        nt2::memory::container<tag::table_, nt2_la_int, nt2::_2D> ipiv(nt2::of_size(n,1));
        w.resize_main(n*nhrs);

        NT2_F77NAME(dsgesv)( &n, &nhrs, a0.data(), &lda, ipiv.data() , a1.data()
                           , &ldb, a2.data(), &ldb , w.main(), swork.data()
                           , &iter, &info
                           );
        return iter;
     }
  };

  BOOST_DISPATCH_IMPLEMENT  ( gemsv_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)
                            , ((container_< nt2::tag::table_, complex_<double_<A0> >, S0 >)) // A
                              ((container_< nt2::tag::table_, complex_<double_<A1> >, S1 >)) // B
                              ((container_< nt2::tag::table_, complex_<double_<A2> >, S2 >)) // X
                            )
  {
     typedef nt2_la_int result_type;
     typedef typename A0::value_type v_t;
     typedef typename boost::dispatch::meta::downgrade<v_t>::type f_t;
     typedef typename nt2::meta::as_real<f_t>::type   r_t;
     typedef typename nt2::meta::as_real<v_t>::type   d_t;

     BOOST_FORCEINLINE result_type operator()(A0& a0, A1 const& a1, A2& a2) const
     {
        details::workspace<typename A2::value_type> w;
        nt2_la_int  n  = std::min(nt2::height(a0),nt2::width(a0));
        nt2_la_int  lda = n;
        nt2_la_int  nhrs = nt2::width(a1);
        nt2_la_int  ldb = a1.leading_size();
        nt2_la_int iter,info;

        nt2::memory::container<tag::table_, f_t, nt2::_2D> swork(nt2::of_size(n*(n+nhrs),1));
        nt2::memory::container<tag::table_, d_t, nt2::_2D> rwork(nt2::of_size(n,1));
        nt2::memory::container<tag::table_, nt2_la_int, nt2::_2D> ipiv(nt2::of_size(n,1));
        w.resize_main(n*nhrs);

        NT2_F77NAME(zcgesv)( &n, &nhrs, a0.data(), &lda, ipiv.data() , a1.data()
                           , &ldb, a2.data(), &ldb , w.main(), swork.data()
                           , rwork.data(), &iter, &info
                           );
        return iter;
     }
  };

} }

#endif