#ifndef RELAY_ODOM_HPP
#define RELAY_ODOM_HPP

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point32.h>

#include <tf/transform_broadcaster.h>

#include <processing_base/base_parameter.h>
#include <boost/function.hpp>
#include <boost/bind.hpp>

void callback(const geometry_msgs::Point32::ConstPtr& msg, ros::Publisher& odomPub, tf::TransformBroadcaster& odom_broadcaster, float& x, float& y, float& theta)
{
  float dleft = WHEEL_DIAMETER * PI * msg->x / TICK_PER_ROUND; // in mm
  float dright = WHEEL_DIAMETER * PI * msg->y / TICK_PER_ROUND;
  float deltaTheta = (dright - dleft) / BASE_DIAMETER;

  float dcenter = (dleft + dright) / 2.0;

  theta += deltaTheta;
  //constrain theta on [0, 2pi]
  if (theta > 2 * PI) theta -= 2 * PI;
  if (theta < 0.0) theta += 2 * PI;

  float deltaX = dcenter * cos(theta) / 1000.0; // change to meters
  float deltaY = dcenter * sin(theta) / 1000.0; // change to meters

  float vx = deltaX / (msg->z / 1000.0); // change ms to seconds (m / s)
  float vy = deltaY / (msg->z / 1000.0); // change ms to seconds (mm /s)
  float omegaTheta = deltaTheta / (msg->z / 1000.0); // rad/s

  x += deltaX;
  y += deltaY;

  ros::Time current_time = ros::Time::now();
  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta);

  geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = current_time;
  odom_trans.header.frame_id = "odom";
  odom_trans.child_frame_id = "base_link";

  odom_trans.transform.translation.x = x;
  odom_trans.transform.translation.y = y;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = odom_quat;

  odom_broadcaster.sendTransform(odom_trans);

  nav_msgs::Odometry odom;
  odom.header.stamp = current_time;
  odom.header.frame_id = "odom";
  //set the position
  odom.pose.pose.position.x = x;
  odom.pose.pose.position.y = y;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = odom_quat;
  //set the velocity
  odom.child_frame_id = "base_link";
  odom.twist.twist.linear.x = vx;
  odom.twist.twist.linear.y = vy;
  odom.twist.twist.angular.z = omegaTheta;

  odomPub.publish(odom);
}

class OdometryEncoderRelay
{
private:
  ros::NodeHandle n;
  ros::Publisher odomPub;
  ros::Subscriber encoderSub;
  tf::TransformBroadcaster odom_broadcaster;

  float x;
  float y;
  float theta;

public:
  OdometryEncoderRelay(std::string pub_name, std::string sub_name) : x(0.0f), y(0.0f), theta(0.0f)
  {
    odomPub = n.advertise<nav_msgs::Odometry>(pub_name, 50);

    boost::function<void (const geometry_msgs::Point32::ConstPtr&)> binded_callback = boost::bind(callback, _1, this->odomPub,
                                                                                this->odom_broadcaster,
                                                                                this->x,
                                                                                this->y,
                                                                                this->theta);
    encoderSub = n.subscribe<geometry_msgs::Point32> (sub_name, 100, binded_callback); // buffer 100 msgs
  }
};

#endif // end RELAY_ODOM_HPP


#include <geometry.h>
#include <tiny_math_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(raycast_obb)
{
  using std::sqrt;

  typedef tiny::MathTypes<double>   MT;
  typedef MT::vector3_type          V;
  typedef MT::real_type             T;
  typedef MT::value_traits          VT;
  typedef MT::quaternion_type       Q;

  V const center   = V::make(0.0, 0.0, 0.0);
  V const half_ext = V::make(1.0, 1.0, 1.0);
  Q const q        = Q::identity();

  geometry::OBB<MT> obb = geometry::make_obb<MT>(center, q, half_ext);

  // Hit straigth on
  {
    V const r      = V::make( 0.0, 0.0, 1.0);
    V const p      = V::make( 0.0, 0.0,-3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 2.0, 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Hit straigth on corner
  {
    V const r      = V::make( 0.0, 0.0, 1.0);
    V const p      = V::make( -1.0, -1.0,-3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 2.0, 0.01);
    BOOST_CHECK_CLOSE( q(0), -1.0, 0.01);
    BOOST_CHECK_CLOSE( q(1), -1.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Hit straigth on edge
  {
    V const r      = V::make( 0.0, 0.0, 1.0);
    V const p      = V::make( 0.0, -1.0,-3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 2.0, 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1), -1.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Aligned ray no hitting
  {
    V const r      = V::make( 0.0, 0.0, 1.0);
    V const p      = V::make( -2.0, 0.0,-3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( !hit );

    BOOST_CHECK_CLOSE( length, VT::infinity(), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }

  // Obligue ray no hitting
  {
    V const r      = V::make( 1.0, 1.0, 1.0);
    V const p      = V::make( 0.0, 0.0,-10.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( !hit );

    BOOST_CHECK_CLOSE( length, VT::infinity(), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }

  // Obligue ray central hit
  {
    V const r      = V::make( 1.0,  1.0,  1.0);
    V const p      = V::make(-1.0, -1.0, -2.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 1.732050807568877, 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Obligue ray corner hit
  {
    V const r      = V::make(  1.0,  1.0,  1.0);
    V const p      = V::make( -1.0,  -1.0, -2.9999);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 2*1.732050807568877, 0.1);
    BOOST_CHECK_CLOSE( q(0),  1.0, 0.1);
    BOOST_CHECK_CLOSE( q(1),  1.0, 0.1);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.1);
  }

  // Obligue ray edge hit
  {
    V const r      = V::make( 1.0,  1.0,  1.0);
    V const p      = V::make( 0.0, -1.0, -2.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 1.732050807568877, 0.01);
    BOOST_CHECK_CLOSE( q(0),  1.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

}

BOOST_AUTO_TEST_CASE(raycast_strange_size_obb)
{
  using std::sqrt;

  typedef tiny::MathTypes<double>   MT;
  typedef MT::vector3_type          V;
  typedef MT::real_type             T;
  typedef MT::value_traits          VT;
  typedef MT::quaternion_type       Q;

  V const center   = V::make(0.0, 0.0, 0.0);
  V const half_ext = V::make(1.0, 2.0, 3.0);
  //  Q const q        = Q::Rx(VT::pi_half() );
  Q const q        = Q::identity();

  geometry::OBB<MT> obb = geometry::make_obb<MT>(center, q, half_ext);

  {
    V const r      = V::make(  2.0,  3.0, -1.0);
    V const p      = V::make( -2.0, -3.0,  4.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, tiny::norm(r), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2),  3.0, 0.01);
  }
}

BOOST_AUTO_TEST_CASE(raycast_rotated_obb)
{
  using std::sqrt;

  typedef tiny::MathTypes<double>   MT;
  typedef MT::vector3_type          V;
  typedef MT::real_type             T;
  typedef MT::value_traits          VT;
  typedef MT::quaternion_type       Q;

  V const center   = V::make(0.0, 0.0, 0.0);
  V const half_ext = V::make(1.0, 2.0, 3.0);
  Q const q        = Q::Rx(VT::pi_half() );

  geometry::OBB<MT> obb = geometry::make_obb<MT>(center, q, half_ext);

  {
    V const r      = V::make(  2.0,   1.0,  3.0);
    V const p      = V::make( -2.0,  -4.0, -3.0 );

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_obb(ray, obb, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, tiny::norm(r), 0.01);
    BOOST_CHECK_SMALL( q(0),   0.01);
    BOOST_CHECK_CLOSE( q(1),  -3.0, 0.01);
    BOOST_CHECK_SMALL( q(2),   0.01);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <mlpack/core.hpp>
#include <mlpack/methods/amf/amf.hpp>
#include <mlpack/methods/amf/update_rules/svd_incomplete_incremental_learning.hpp>
#include <mlpack/methods/amf/update_rules/svd_complete_incremental_learning.hpp>
#include <mlpack/methods/amf/init_rules/random_init.hpp>
#include <mlpack/methods/amf/termination_policies/incomplete_incremental_termination.hpp>
#include <mlpack/methods/amf/termination_policies/complete_incremental_termination.hpp>
#include <mlpack/methods/amf/termination_policies/simple_tolerance_termination.hpp>
#include <mlpack/methods/amf/termination_policies/validation_RMSE_termination.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(SVDIncrementalTest);

using namespace std;
using namespace mlpack;
using namespace mlpack::amf;
using namespace arma;

/**
 * Test for convergence of incomplete incremenal learning
 */
BOOST_AUTO_TEST_CASE(SVDIncompleteIncrementalConvergenceTest)
{
  sp_mat data;
  data.sprandn(1000, 1000, 0.2);

  SVDIncompleteIncrementalLearning svd(0.01);
  IncompleteIncrementalTermination<SimpleToleranceTermination<sp_mat> > iit;

  AMF<IncompleteIncrementalTermination<SimpleToleranceTermination<sp_mat> >,
      RandomInitialization,
      SVDIncompleteIncrementalLearning> amf(iit, RandomInitialization(), svd);

  mat m1,m2;
  amf.Apply(data, 2, m1, m2);

  BOOST_REQUIRE_NE(amf.TerminationPolicy().Iteration(),
                   amf.TerminationPolicy().MaxIterations());
}

/**
 * Test for convergence of complete incremenal learning
 */
BOOST_AUTO_TEST_CASE(SVDCompleteIncrementalConvergenceTest)
{
  sp_mat data;
  data.sprandn(1000, 1000, 0.2);

  SVDCompleteIncrementalLearning<sp_mat> svd(0.01);
  CompleteIncrementalTermination<SimpleToleranceTermination<sp_mat> > iit;

  AMF<CompleteIncrementalTermination<SimpleToleranceTermination<sp_mat> >,
      RandomInitialization,
      SVDCompleteIncrementalLearning<sp_mat> > amf(iit,
                                                   RandomInitialization(),
                                                   svd);
  mat m1,m2;
  amf.Apply(data, 2, m1, m2);

  BOOST_REQUIRE_NE(amf.TerminationPolicy().Iteration(),
                   amf.TerminationPolicy().MaxIterations());
}

//! This is used to ensure we start from the same initial point.
class SpecificRandomInitialization
{
 public:
  SpecificRandomInitialization(const size_t n, const size_t r, const size_t m) :
      W(arma::randu<arma::mat>(n, r)),
      H(arma::randu<arma::mat>(r, m)) { }

  template<typename MatType>
  inline void Initialize(const MatType& /* V */,
                         const size_t /* r */,
                         arma::mat& W,
                         arma::mat& H)
  {
    W = this->W;
    H = this->H;
  }

 private:
  arma::mat W;
  arma::mat H;
};

BOOST_AUTO_TEST_CASE(SVDIncompleteIncrementalRegularizationTest)
{
  mat dataset;
  data::Load("GroupLens100k.csv", dataset);

  // Generate list of locations for batch insert constructor for sparse
  // matrices.
  arma::umat locations(2, dataset.n_cols);
  arma::vec values(dataset.n_cols);
  for (size_t i = 0; i < dataset.n_cols; ++i)
  {
    // We have to transpose it because items are rows, and users are columns.
    locations(0, i) = ((arma::uword) dataset(0, i));
    locations(1, i) = ((arma::uword) dataset(1, i));
    values(i) = dataset(2, i);
  }

  // Find maximum user and item IDs.
  const size_t maxUserID = (size_t) max(locations.row(0)) + 1;
  const size_t maxItemID = (size_t) max(locations.row(1)) + 1;

  // Fill sparse matrix.
  sp_mat cleanedData = arma::sp_mat(locations, values, maxUserID, maxItemID);
  sp_mat cleanedData2 = cleanedData;

  SpecificRandomInitialization sri(cleanedData.n_rows, 2, cleanedData.n_cols);

  ValidationRMSETermination<sp_mat> vrt(cleanedData, 2000);
  AMF<IncompleteIncrementalTermination<ValidationRMSETermination<sp_mat> >,
      SpecificRandomInitialization,
      SVDIncompleteIncrementalLearning> amf1(vrt, sri,
      SVDIncompleteIncrementalLearning(0.001, 0, 0));

  mat m1, m2;
  double regularRMSE = amf1.Apply(cleanedData, 2, m1, m2);

  ValidationRMSETermination<sp_mat> vrt2(cleanedData2, 2000);
  AMF<IncompleteIncrementalTermination<ValidationRMSETermination<sp_mat> >,
      SpecificRandomInitialization,
      SVDIncompleteIncrementalLearning> amf2(vrt2, sri,
      SVDIncompleteIncrementalLearning(0.001, 0.01, 0.01));

  mat m3, m4;
  double regularizedRMSE = amf2.Apply(cleanedData2, 2, m3, m4);

  BOOST_REQUIRE_LT(regularizedRMSE, regularRMSE + 0.075);
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * 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.
 */

#include <string>
#include <ros/ros.h>

#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>

#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/jntarrayvel.hpp>

#include <Eigen/Dense>
#include <kdl_conversions/kdl_msg.h>

class DebugEvaluateJointStates
{
    ros::NodeHandle nh_;
    ros::Subscriber jointstate_sub_;
    ros::Publisher manipulability_pub_;
    ros::Publisher twist_current_pub_;

    std::string chain_base_link_;
    std::string chain_tip_link_;

    KDL::Chain chain_;
    KDL::ChainFkSolverVel_recursive* p_fksolver_vel_;
    KDL::ChainJntToJacSolver* p_jnt2jac_;

public:
    int init()
    {
        if (!nh_.getParam("chain_base_link", this->chain_base_link_))
        {
            ROS_ERROR("Failed to get parameter \"chain_base_link\".");
            return -1;
        }

        if (!nh_.getParam("chain_tip_link", this->chain_tip_link_))
        {
            ROS_ERROR("Failed to get parameter \"chain_tip_link\".");
            return -2;
        }

        /// parse robot_description and generate KDL chains
        KDL::Tree my_tree;
        if (!kdl_parser::treeFromParam("/robot_description", my_tree))
        {
            ROS_ERROR("Failed to construct kdl tree");
            return -3;
        }

        my_tree.getChain(this->chain_base_link_, this->chain_tip_link_, chain_);
        if (chain_.getNrOfJoints() == 0)
        {
            ROS_ERROR("Failed to initialize kinematic chain");
            return -4;
        }

        p_fksolver_vel_ = new KDL::ChainFkSolverVel_recursive(chain_);
        p_jnt2jac_ = new KDL::ChainJntToJacSolver(chain_);

        /// initialize ROS interfaces
        jointstate_sub_ = nh_.subscribe("joint_states", 1, &DebugEvaluateJointStates::jointstateCallback, this);
        manipulability_pub_ = nh_.advertise<std_msgs::Float64> ("debug/manipulability", 1);
        twist_current_pub_ = nh_.advertise<geometry_msgs::Twist> ("debug/twist_current", 1);

        return 0;
    }

    void jointstateCallback(const sensor_msgs::JointState::ConstPtr& msg)
    {
        KDL::JntArray q = KDL::JntArray(chain_.getNrOfJoints());
        KDL::JntArray q_dot = KDL::JntArray(chain_.getNrOfJoints());

        for (unsigned int i = 0; i < msg->name.size(); i++)
        {
            q(i) = msg->position[i];
            q_dot(i) = msg->velocity[i];
        }

        /// compute current twist
        KDL::FrameVel FrameVel;
        KDL::JntArrayVel jntArrayVel = KDL::JntArrayVel(q, q_dot);
        if (p_fksolver_vel_->JntToCart(jntArrayVel, FrameVel, -1) >= 0)
        {
            geometry_msgs::Twist twist_msg;
            tf::twistKDLToMsg(FrameVel.GetTwist(), twist_msg);
            twist_current_pub_.publish(twist_msg);
        }

        /// compute manipulability
        KDL::Jacobian jac(chain_.getNrOfJoints());
        p_jnt2jac_->JntToJac(q, jac);
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> prod = jac.data * jac.data.transpose();
        double d = prod.determinant();
        double kappa = std::sqrt(std::abs(d));
        std_msgs::Float64 manipulability_msg;
        manipulability_msg.data = kappa;
        manipulability_pub_.publish(manipulability_msg);
    }
};

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

    DebugEvaluateJointStates dejs;
    if (dejs.init() != 0)
    {
        ROS_ERROR("Failed to initialize DebugEvaluateJointStates.");
        return -1;
    }

    ros::spin();
}


#include <complex>
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <eigenpc.h>

using namespace Eigen;

template <typename T, int CompressionType>
class ICFArgs{

    typedef typename NaturalOrdering<int>::PermutationType PermutationType;
    typedef typename PermutationType::StorageIndex StorageIndex;
    //
    typedef SparseMatrix<T,ColMajor,StorageIndex>          FactorType;
    typedef SparseMatrix<T,CompressionType,StorageIndex>   CompressedSpMat;
    typedef Matrix<T, Dynamic, 1>                          VectorType;

    IncompleteCholesky<T, Lower, NaturalOrdering<int> > ichol;
    FactorType Lfactor;

  public:
    ICFArgs(void *vals, int *innerIndices, int *outerIndexPtr, const int n, const int nnz) : ichol( Map<CompressedSpMat >(n,n,nnz,outerIndexPtr,innerIndices,static_cast<T*>(vals))){
       std::cout << "Creating ICC preconditioner.." << std::endl;
       Lfactor = ichol.matrixL();// The lower part stored in CSC
       return;
    }

    inline void	apply(T *out, T *in){
       Map<VectorType, Unaligned> out_(out, Lfactor.rows());
       Map<const VectorType, Unaligned> in_(in,Lfactor.cols());

       if(out == in) return; //nothing to do

       out_ = Lfactor.template triangularView<Lower>().solve(in_);
       Lfactor.adjoint().template triangularView<Upper>().solveInPlace(out_);

       return;
    }

    ~ICFArgs() {}
};

template <typename T, int CompressType>
void createPreconditioner(void *vals, int *innerIndices, int *outerIndexPtr, PrecondDescr &descr) {
  if(descr.pctype == PRECOND_ICC) {
    descr.pc_args = static_cast<void*>( new ICFArgs<T, CompressType >(vals, innerIndices, outerIndexPtr, descr.rows, descr.nnz) );
  } else {
    printf("Precond type is not supported.\n"), exit(-1);
  }

  return;
}

template <typename T>
void createPreconditioner(void *vals, int *innerIndices, int *outerIndexPtr, PrecondDescr &descr) {
  if(descr.ctype == CRS_FORMAT){
    createPreconditioner<T, RowMajor>(vals, innerIndices, outerIndexPtr, descr);
  } else if(descr.ctype == CCS_FORMAT) {
    createPreconditioner<T, ColMajor>(vals, innerIndices, outerIndexPtr, descr);
  } else {
    printf("Compression type is not supported."), exit(-1);
  }
  return;
}

//Create preconditioner for a given problem matrix.
//we accept maptrix in CRS or CCS formats
void CreatePreconditioner(void *vals, int *innerIndices, int *outerIndexPtr, PrecondDescr *descr) {
  if (descr->rows != descr->cols) printf("Error: incorrect matrix dimensions (rows %d, cols %d)\n", descr->rows, descr->cols), exit(-1);

  std::cout << "Number of nnz " << descr->nnz << " problem size is : " << descr->rows << std::endl;
  std::cout << "Precision : " << descr->precision << " Complex : " << descr->is_complex << std::endl;

  if(descr->precision == FLOAT_SINGLE){
    if(descr->is_complex) {
      createPreconditioner<std::complex<float>>(vals, innerIndices, outerIndexPtr, *descr);
    } else {
      createPreconditioner<float>(vals, innerIndices, outerIndexPtr, *descr);
    }
  } else {
    if(descr->is_complex) {
      createPreconditioner<std::complex<double>>(vals, innerIndices, outerIndexPtr, *descr);
    } else {
      createPreconditioner<double>(vals, innerIndices, outerIndexPtr, *descr);
    }
  }

  descr->is_init = 1;

  return;
}

template <typename T, int CompressType>
void DestroyPreconditioner(PrecondDescr &descr){
  if(descr.pctype == PRECOND_ICC) {
    typedef ICFArgs<T, CompressType > Preconditioner;
    delete static_cast<Preconditioner*> (descr.pc_args);
  } else {
    printf("Precond type is not supported.\n"), exit(-1);
  }

  return;
}

template <typename T>
void DestroyPreconditioner(PrecondDescr &descr){
  if(descr.ctype == CRS_FORMAT){
    DestroyPreconditioner<T, RowMajor>(descr);
  } else if(descr.ctype == CCS_FORMAT) {
    DestroyPreconditioner<T, ColMajor>(descr);
  } else {
    printf("Cannot destroy preconditioner with unsupported compression type."), exit(-1);
  }
  return;
}

//Create preconditioner:
void DestroyPreconditioner(PrecondDescr *descr){
  if (descr->is_init != 1) printf("Preconditioner was not initialized\n"), exit(-1);

  if(descr->precision == FLOAT_SINGLE){
    if(descr->is_complex) {
      DestroyPreconditioner<std::complex<float>>(*descr);
    } else {
      DestroyPreconditioner<float>(*descr);
    }
  } else {
    if(descr->is_complex) {
      DestroyPreconditioner<std::complex<double>>(*descr);
    } else {
      DestroyPreconditioner<double>(*descr);
    }
  }
  descr->pc_args = nullptr;
  descr->is_init = 0;

  return;
}

	template <typename T, int CompressType>
	void applyPreconditioner(void *out, void *in, PrecondDescr &descr)
  {
		 if(descr.pctype == PRECOND_ICC) {
			 typedef ICFArgs<T, CompressType > Preconditioner;
			 (static_cast<Preconditioner*> (descr.pc_args))->apply(static_cast<T*>(out), static_cast<T*>(in));
		 } else {
			 printf("Precond type is not supported.\n"), exit(-1);
		 }

		 return;
  }

	template <typename T>
	void applyPreconditioner(void *out, void *in, PrecondDescr &descr)
  {
		 if(descr.ctype == CRS_FORMAT){
		   applyPreconditioner<T, RowMajor>(out, in, descr);
		 } else if(descr.ctype == CCS_FORMAT) {
		   applyPreconditioner<T, ColMajor>(out, in, descr);
		 } else {
		   printf("Cannot destroy preconditioner with unsupported compression type."), exit(-1);
		 }
		 return;
  }

  //Create preconditioner:
  void ApplyPreconditioner(void *out, void *in, PrecondDescr *descr){
		 if(descr->precision == FLOAT_SINGLE){
			 if(descr->is_complex) {
				 applyPreconditioner<std::complex<float>>(out, in, *descr);
			 } else {
				 applyPreconditioner<float>(out, in, *descr);
			 }
		 } else {
			 if(descr->is_complex) {
				 applyPreconditioner<std::complex<double>>(out, in, *descr);
			 } else {
				 applyPreconditioner<double>(out, in, *descr);
			 }
		 }
     return;
  }


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_EXPX2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_EXPX2_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/constant/expx2c1.hpp>
#include <boost/simd/detail/constant/expx2c2.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/function/sqr.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/is_equal.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( expx2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const A0& a0) const
      {
        A0 x =  bs::abs(a0);
        // Represent x as an exact multiple of 1/32 plus a residual.
        A0 m = Expx2c1<A0>()*bs::floor(fma(Expx2c2<A0>(), x, bs::Half<A0>()));
        A0 f =  x-m;
        // x**2 = m**2 + 2mf + f**2
        A0 u = bs::sqr(m);
        A0 u1 = fma(m+m,f,sqr(f));
        // u is exact, u1 is small.
        A0 r = bs::if_else(bs::is_greater(u+u1, bs::Maxlog<A0>()),
                            bs::Inf<A0>(),
                            bs::exp(u)*bs::exp(u1));
        #ifndef BOOST_SIMD_NO_INFINITIES
        r =  bs::if_else(is_equal(x, Inf<A0>()), x, r);
        #endif
        return r;
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF( expx2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const A0& a0,  const A0 & s) const
      {
        A0 sgn =  signnz(s);
        A0 x =  a0*sgn;
        // Represent x as an exact multiple of 1/32 plus a residual.
        A0 m = Expx2c1<A0>()*bs::floor(fma(Expx2c2<A0>(), x, bs::Half<A0>()));
        A0 f =  x-m;
        // x**2 = m**2 + 2mf + f**2
        A0 u = sgn*bs::sqr(m);
        A0 u1 = sgn*fma(m+m,f,sqr(f));
        // u is exact, u1 is small.
        A0 r = bs::if_else(bs::is_greater(u+u1, bs::Maxlog<A0>()),
                            bs::Inf<A0>(),
                            bs::exp(u)*bs::exp(u1));
        #ifndef BOOST_SIMD_NO_INFINITIES
        r =  bs::if_else(is_equal(x, Inf<A0>()), x, r);
        #endif
        return r;
      }
   };

} } }

#endif


#ifndef TRACE_RAYTRACE_
#define TRACE_RAYTRACE_
#include <headers/setup.hpp>
#include <headers/geometry.hpp>
#include <boost/iterator/indirect_iterator.hpp>

namespace raytrace {

    using geometry::Point;
    using geometry::Vector;
    using geometry::Sphere;
    using geometry::Line;
    using geometry::SphPoint;

    struct ExpGeometry
    {
        static constexpr float rho = 1000.0;                           //mm
        static constexpr float d = 60.0;                               //mm
        static constexpr float l1 = 195.0;                             //mm
        static constexpr float l2 = 185.0;                             //mm
        static constexpr float L = 60.0;                               //mm
    };

    class Surface
    {
        private:
            setup::Spline delta_;
            setup::Spline gamma_;                                       //Permettivity = 1 - delta + i * gamma
            double RMSHeight_;                                          //RMS Height [nm]
            double CorrLength_;                                         //Correlation length [um]
            double alpha_;                                              //alpha parameter in PSD ABC model

            double k(double wl) const {return 2 * Constants::pi / wl; }
            double mu0(double th0, double corl, double wl) const {return corl * 1e3 * pow(sin(th0), 2) / (2 * wl); }
            std::complex<double> muc(double corl, double wl) const {return corl * 1e3 * (1.0 - permettivity(wl)) / (2 * wl); }
            double F(double tau, double alpha) const {return 2.0 / sqrt(Constants::pi) * tgamma(alpha + 0.5) / tgamma(alpha) / pow(1 + tau * tau, alpha + 0.5); }
        public:
            Surface(double RMSHeight = Constants::RMSHeight,
            double CorrLength = Constants::CorrLength, double alpha = Constants::alpha, const std::string & str = "Al2O3.txt");
            std::complex<double> permettivity (double wl = Constants::WL) const;
            double Rf (double th0, double wl = Constants::WL) const;
            double TIS (double th0, double rmsh, double corl, double alpha, double wl = Constants::WL) const;
            double Indicatrix1D (double th, double th0, double wl = Constants::WL) const;
            double Indicatrix2D (double th, double phi, double th0, double wl = Constants::WL) const;
            double PSD1D (double p) const noexcept;                              //PSD 1D ([micrometer ^ -1]) [micrometer ^ 3]
            double PSD2D (double p1, double p2) const noexcept;                  //PSD 2D ([micrometer ^ -1]) [micrometer ^ 4]
            double CritAng(double wl = Constants::WL) const {return sqrt(1.0 - real(permettivity(wl))); }
            double RMSHeight() const noexcept {return RMSHeight_; }
            double & RMSHeight() noexcept {return RMSHeight_; }
            double CorrLength() const noexcept {return CorrLength_; }
            double & CorrLength() noexcept {return CorrLength_; }
            double alpha() const noexcept {return alpha_; }
    };

    class ExpSetup
    {
        private:
            double wl_, inc_ang_, src_dist_, det_dist_, src_l_;
            Surface surf_;
            std::vector<Sphere> sphs_;
        public:
            ExpSetup (double inc_ang = 0.0, double RMSHeight = Constants::RMSHeight, double CorrLength = Constants::CorrLength) :
            surf_(Surface(RMSHeight, CorrLength)), sphs_(std::vector<Sphere>{Sphere(0.0, 0.0, ExpGeometry::rho, ExpGeometry::rho, ExpGeometry::d, Sphere::LOWER)}), inc_ang_(inc_ang),
            wl_(Constants::WL), src_dist_(ExpGeometry::l1), det_dist_(ExpGeometry::l2), src_l_(ExpGeometry::L) {}

            const Sphere & substrate() const noexcept {return sphs_.front(); }
            Sphere & substrate() noexcept {return sphs_.front(); }
            const std::vector<Sphere> & spheres() const noexcept {return sphs_; }
            const Surface & surface() const noexcept {return surf_; }
            Surface & surface() noexcept {return surf_; }
            const double & IncAngle() const noexcept {return inc_ang_; }
            double & IncAngle() noexcept {return inc_ang_; }
            const double wavelength() const noexcept {return wl_; }
            const double source_distance() const noexcept {return src_dist_; }
            const double source_length() const noexcept {return src_l_; }
            const double detector_distance() const noexcept {return det_dist_; }
            void add_sphere(const Sphere & sph) noexcept {sphs_.emplace_back(sph); }
            void add_sphere(double x, double y, double height, double radius) noexcept;
            void add_sphere(const std::vector<Sphere> & sphs) noexcept {sphs_.insert(sphs_.end(), sphs.cbegin(), sphs.cend()); }
            void add_sphere(const std::vector<double> & x, const std::vector<double> & y, const std::vector<double> & height, const std::vector<double> & radius);
            void reset_sphere() noexcept {if(sphs_.size() > 1) sphs_.erase(sphs_.cbegin() + 1,sphs_.cend()); }
    };

    class Beam : public Line
    {
        private:
            static constexpr double intersect_limit = 1e-8;
        public:
            using Line::Line;
            double Delta (const Sphere & sph) const noexcept;
            std::vector<SphPoint> Intersect(const Sphere & sph) const;
            std::vector<SphPoint> Intersect(const std::vector<Sphere> & sphs) const;
            bool is_intersect (const Sphere & sph) const noexcept;
            bool is_intersect (const std::vector<Sphere> & sphs) const noexcept;
            double IncAng (const SphPoint & spt) const;
            Vector SpecVec (const SphPoint & spt) const;
            virtual Vector ScatVec(const SphPoint & pt, const Surface & surf, double wl) const = 0;
    };

    class Beam2D : public Beam
    {
        public:
            using Beam::Beam;
            virtual Vector ScatVec(const SphPoint & spt, const Surface & surf, double wl) const override;
    };

    class SphBeam2D : public Beam2D
    {
        public:
            using Beam2D::Beam2D;
            SphBeam2D (const Sphere & sph, double inc_ang, double src_dist);
            SphBeam2D (const ExpSetup & setup) : SphBeam2D(setup.substrate(), setup.IncAngle(), setup.source_distance()) {}
    };

    class PlaneBeam2D : public Beam2D
    {
        public:
            using Beam2D::Beam2D;
            PlaneBeam2D (const Sphere & sph, double inc_ang, double src_dist);
            PlaneBeam2D (const ExpSetup & setup) : PlaneBeam2D(setup.substrate(), setup.IncAngle(), setup.source_distance()) {}
    };

    class Beam3D : public Beam
    {
        public:
            using Beam::Beam;
            virtual Vector ScatVec(const SphPoint & spt, const Surface & surf, double wl) const override;
    };

    class SphBeam3D : public Beam3D
    {
        public:
            using Beam3D::Beam3D;
            SphBeam3D(const Sphere & sph, double inc_ang, double src_dist, double src_len);
            SphBeam3D(const ExpSetup & setup) : SphBeam3D(setup.substrate(), setup.IncAngle(), setup.source_distance(), setup.source_length()) {}
    };

    class PlaneBeam3D : public Beam3D
    {
        public:
            using Beam3D::Beam3D;
            PlaneBeam3D(const Sphere & sph, double inc_ang, double src_dist);
            PlaneBeam3D(const ExpSetup & setup) : PlaneBeam3D(setup.substrate(), setup.IncAngle(), setup.source_distance()) {}
    };

    class TestBeam3D : public Beam3D
    {
        public:
            using Beam3D::Beam3D;
            TestBeam3D(const Sphere & sph, double h, double src_dist);
            TestBeam3D(const ExpSetup & setup, double h) : TestBeam3D(setup.substrate(), h, setup.source_distance()) {}
    };

    struct TracePoint
    {
        Point det_point;
        bool is_scattered;
        bool is_transmitted;
        TracePoint (const Point & pt, bool is_tr, bool is_sc) : det_point(pt), is_transmitted(is_tr), is_scattered(is_sc) {}
    };

    class Trace
    {
        private:
            std::vector<std::unique_ptr<Beam>> trace_;
            bool is_transmitted_ {};
            bool is_scattered_ {};
        public:
            using iterator = boost::indirect_iterator<std::vector<std::unique_ptr<Beam>>::iterator>;
            using const_iterator = boost::indirect_iterator<std::vector<std::unique_ptr<Beam>>::const_iterator>;
            iterator begin() {return iterator(trace_.begin()); }
            iterator end() {return iterator(trace_.end()); }
            const_iterator begin() const {return const_iterator(trace_.begin()); }
            const_iterator end() const {return const_iterator(trace_.end()); }

            template <typename T, typename Object>
            Trace (T && line, const Surface & surf, const Object & sphs, double wl)
            {
                trace_.emplace_back(std::make_unique<std::decay_t<T>>(std::forward<T>(line)));
                is_transmitted_ = trace_.back()->is_intersect(sphs);
                if (is_transmitted_)
                {
                    double sw = setup::dist(setup::gen);
                    auto spts = trace_.back()->Intersect(sphs);
                    assert(spts.size() != 0);
                    auto next_spt_ = *std::min_element(spts.cbegin(), spts.cend(), [](const SphPoint & a, const SphPoint & b) {return a.point().y() < b.point().y(); });
                    is_transmitted_ = sw < surf.Rf(trace_.back()->IncAng(next_spt_), wl);
                    while (is_transmitted_)
                    {
                        if (sw < surf.TIS(trace_.back()->IncAng(next_spt_), surf.RMSHeight(), surf.CorrLength(), surf.alpha(), wl))
                        {
                            trace_.emplace_back(std::make_unique<std::decay_t<T>>(next_spt_.point(), trace_.back()->ScatVec(next_spt_, surf, wl)));
                            is_scattered_ = true;
                        }
                        else
                            trace_.emplace_back(std::make_unique<std::decay_t<T>>(next_spt_.point(), trace_.back()->SpecVec(next_spt_)));
                        spts = trace_.back()->Intersect(sphs);
                        if (spts.size() == 0)
                            break;
                        next_spt_ = *std::min_element(spts.cbegin(), spts.cend(), [](const SphPoint & a, const SphPoint & b) {return a.point().y() < b.point().y(); });
                        sw = setup::dist(setup::gen);
                        is_transmitted_ = sw < surf.Rf(trace_.back()->IncAng(next_spt_), wl);
                    }
                }
            }

            template <typename T>
            Trace (T && line, const ExpSetup & setup) : Trace(std::forward<T>(line), setup.surface(), setup.spheres(), setup.wavelength()) {}

            TracePoint det_res(double det_dist) const noexcept;
            bool is_transmitted() const noexcept {return is_transmitted_; }
            bool is_scattered() const noexcept {return is_scattered_; }
            int size() const noexcept {return trace_.size(); }
    };

    template <typename T>
    Trace make_trace(const ExpSetup & setup)
    {
        return Trace (T(setup), setup);
    }

}

#endif


#include <Eigen/Geometry>
#include <Eigen/LU>
#include <iostream>
#include <numeric>
#include <sps/plane.hpp>
#include <vector>

// This implementation is general but slow because it needs to solve linear systems. It is possible to implement this
// function much faster for specific plane types.
Eigen::VectorXd sps::AbstractPlane::CalcParameters(const Eigen::Vector2d& coords) const
{
    const Eigen::Vector2d              g_c(+0.0, +0.0);
    const std::vector<Eigen::Vector2d> g = {
        {-1.0, -1.0},
        {+1.0, -1.0},
        {+1.0, +1.0},
        {-1.0, +1.0},
    };

    const auto vertex_indices = [&]() -> std::pair<unsigned, unsigned> {
        if (coords(0) >= coords(1) && coords(0) <= -coords(1))
        {
            return {0, 1};
        }
        if (coords(0) >= coords(1) && coords(0) >= -coords(1))
        {
            return {1, 2};
        }
        if (coords(0) <= coords(1) && coords(0) >= -coords(1))
        {
            return {2, 3};
        }
        if (coords(0) <= coords(1) && coords(0) <= -coords(1))
        {
            return {3, 0};
        }

        assert(false);
        return {0, 0};
    }();

    // Prepare to calculate the barycentric coordinates
    const Eigen::Matrix3d R = [&]() {
        Eigen::Matrix3d R = Eigen::Matrix3d::Ones();

        R.block<2, 1>(0, 0) = g_c;
        R.block<2, 1>(0, 1) = g[std::get<0>(vertex_indices)];
        R.block<2, 1>(0, 2) = g[std::get<1>(vertex_indices)];

        return R;
    }();
    const Eigen::Vector3d r(coords(0), coords(1), 1.0);

    // Find the barycentric coordinates
    const Eigen::Vector3d barycentric_coords = Eigen::FullPivLU<Eigen::Matrix3d>(R).solve(r);

    return barycentric_coords(0) * m_center + barycentric_coords(1) * m_vertices[std::get<0>(vertex_indices)] +
           barycentric_coords(2) * m_vertices[std::get<1>(vertex_indices)];
}

Eigen::VectorXd sps::AbstractPlane::CalcGridParameters(const GridCellIndex&              grid_cell,
                                                       const unsigned int                num_candidates,
                                                       const double                      inter_level_scale,
                                                       const std::vector<GridCellIndex>& prev_grid_cells) const
{
    assert(num_candidates % 2 == 1);

    const int    grid_radius         = (num_candidates - 1) / 2;
    const double grid_to_coord_scale = (1.0 / static_cast<double>(grid_radius));

    // Find the coordinates in the top-level grid coordinate system
    auto zoom_transform = Eigen::Affine2d::Identity();
    for (auto& prev_grid_cell : prev_grid_cells)
    {
        const auto prev_unscaled_grid_coords = Eigen::Vector2d(static_cast<double>(std::get<0>(prev_grid_cell)),
                                                               static_cast<double>(std::get<1>(prev_grid_cell)));
        const auto center                    = grid_to_coord_scale * prev_unscaled_grid_coords;
        zoom_transform =
            zoom_transform * Eigen::Translation2d(center) * Eigen::UniformScaling<double>(inter_level_scale);
    }
    const auto unscaled_grid_coords =
        Eigen::Vector2d(static_cast<double>(std::get<0>(grid_cell)), static_cast<double>(std::get<1>(grid_cell)));
    const Eigen::Vector2d finest_level_coords   = grid_to_coord_scale * unscaled_grid_coords;
    const Eigen::Vector2d coarsest_level_coords = zoom_transform * finest_level_coords;

    return CalcParameters(coarsest_level_coords);
}

double sps::AbstractPlane::CalcArea() const
{
    const auto triangle_area_evaluator =
        [](const Eigen::VectorXd& v_0, const Eigen::VectorXd& v_1, const Eigen::VectorXd& v_2) {
            const Eigen::VectorXd r_0      = v_1 - v_0;
            const Eigen::VectorXd r_1      = v_2 - v_0;
            const double          dot_prod = r_0.dot(r_1);

            return 0.5 * std::sqrt(r_0.squaredNorm() * r_1.squaredNorm() - dot_prod * dot_prod);
        };

    std::array<double, 4> areas;

    areas[0] = triangle_area_evaluator(m_center, m_vertices[0], m_vertices[1]);
    areas[1] = triangle_area_evaluator(m_center, m_vertices[1], m_vertices[2]);
    areas[2] = triangle_area_evaluator(m_center, m_vertices[2], m_vertices[3]);
    areas[3] = triangle_area_evaluator(m_center, m_vertices[3], m_vertices[0]);

    return std::accumulate(areas.begin(), areas.end(), 0.0);
}


// Standar dlibraries
#include <vector>

// Simulator description format (SDF)
#include <sdf/sdf.hh>

// Boost includes
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>

// For converting Gazebo <-> ECEF coordinates
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>

// Required for the maths functions
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>

// A common time and position system
#include <gpstk/CommonTime.hpp>
#include <gpstk/Position.hpp>
#include <gpstk/WGS84Ellipsoid.hpp>

// Class for storing "broadcast-type" ephemerides
#include <gpstk/SP3EphemerisStore.hpp>
#include <gpstk/GPSEphemerisStore.hpp>
#include <gpstk/GloEphemerisStore.hpp>

// Tropospheric modelling
#include <gpstk/TropModel.hpp>

// Classes for handling RINEX satellite navigation parameters (ephemerides)
#include <gpstk/Rinex3NavHeader.hpp>
#include <gpstk/Rinex3NavData.hpp>
#include <gpstk/Rinex3NavStream.hpp>

// Classes for handling RINEX files with IONEX data
#include <gpstk/IonexStore.hpp>
#include <gpstk/IonexData.hpp>
#include <gpstk/IonexHeader.hpp>
#include <gpstk/IonexStream.hpp>

// Class to store satellite precise navigation data
#include <gpstk/SP3Data.hpp>
#include <gpstk/SP3Header.hpp>
#include <gpstk/SP3Stream.hpp>

// ROS includes
#include <ros/ros.h>

// Custom messages
#include "environment.pb.h"
#include "meteorological.pb.h"
#include "satellites.pb.h"
#include "noise.pb.h"

// Required for noise distributions
#include "../../noise/NoiseFactory.h"

// Approx frequencies used to claculate ionospheric perturbation
#define SPEED_OF_LIGHT 	299792458.0

namespace gazebo
{
	// Convenience declarations
	typedef const boost::shared_ptr<const msgs::Meteorological> MeteorologicalPtr;
	typedef const boost::shared_ptr<const msgs::Environment> 	EnvironmentPtr;
	typedef const boost::shared_ptr<const msgs::Noise> 			NoisePtr;

	class Satellites : public WorldPlugin
	{

	private:

		// BASIC PARAMETERS ////////////////////////////////////////////////////

		double                              rate;

		// MDATA STORAGE MECHANISMS ////////////////////////////////////////////

		// UTC time systems
		gpstk::CommonTime 					currentTimeUTC, currentTimeGPS, currentTimeGLO;
		gpstk::CivilTime 					civilTime;

	    // For storing GPS and Glonass ephemerides
	    gpstk::GPSEphemerisStore    		gpsEphemerides;
	    gpstk::GloEphemerisStore    		gloEphemerides;
	    gpstk::SP3EphemerisStore    		sp3Ephemerides;
	    gpstk::IonexStore           		tecStore;

 		// Goad and Goodman (1974) troppspheric model
 		gpstk::WGS84Ellipsoid           	wgs84;
	    gpstk::GGTropModel                 	tropModel;
		gpstk::Triple 						ionoModel;

	    // Locally store temperature, pressure and humidity (troposhperic model)
	    Noise* 								nGPSeph[gpstk::MAX_PRN];
	    Noise* 								nGPSclk[gpstk::MAX_PRN];
	    Noise* 								nGPStro[gpstk::MAX_PRN];
	    Noise* 								nGPSion[gpstk::MAX_PRN];
	    Noise* 								nGLOeph[gpstk::MAX_PRN];
	    Noise* 								nGLOclk[gpstk::MAX_PRN];
	    Noise* 								nGLOtro[gpstk::MAX_PRN];
	    Noise*								nGLOion[gpstk::MAX_PRN];

	    // Required for messaging
	    physics::WorldPtr 					worldPtr;
		transport::NodePtr 					nodePtr;
		transport::PublisherPtr 			pubPtr;
		transport::SubscriberPtr 			subPtrMet, subPtrEnv, subPtrNoi;

		// Required for ROS interaction
		ros::Timer 							timer;
		ros::NodeHandle 					rosNode;

      	// Message containing information
      	msgs::Satellites 					sat;
      	msgs::Meteorological                met;
      	msgs::Environment  		            env;

      	// Meterological info
      	bool								rcvMet, rcvEnv;

	    //  Called to update the world information
	    void Update(const ros::TimerEvent& event)
	    {
	    	// We can onyl proceed if we know the UTC time and current weather
	    	if (!rcvMet || !rcvEnv)
	    		return;

			// CALCULATE THE CURRENT TIME /////////////////////////////////////////////////////

			// Simulated seconds since UTC start point
			double t = worldPtr->GetSimTime().Double();

            // Create a common time variable from the UTC info in the SDF data
            civilTime = gpstk::CivilTime(
            	env.year(),
            	env.month(),
            	env.day(),
            	env.hour(),
            	env.minute(),
            	env.second(),
            	gpstk::TimeSystem::UTC
            );

			// Calculate UTC time
			currentTimeUTC = civilTime.convertToCommonTime();
			currentTimeUTC.addSeconds(t);
			currentTimeUTC.setTimeSystem(gpstk::TimeSystem::UTC);

			// Calculate UTC time
			currentTimeGPS = civilTime.convertToCommonTime();
			currentTimeGPS.addSeconds(t +
                gpstk::TimeSystem::Correction(
                    gpstk::TimeSystem::UTC, gpstk::TimeSystem::GPS,
                    civilTime.year, civilTime.month, civilTime.day
                )
            );
			currentTimeGPS.setTimeSystem(gpstk::TimeSystem::GPS);

			// Calculate UTC time
			currentTimeGLO = civilTime.convertToCommonTime();
			currentTimeGLO.addSeconds(t +
                gpstk::TimeSystem::Correction(
                    gpstk::TimeSystem::UTC, gpstk::TimeSystem::GLO,
                    civilTime.year, civilTime.month, civilTime.day
                )
            );
			currentTimeGLO.setTimeSystem(gpstk::TimeSystem::GLO);

			// Get the epoch from the time subsystem
			double epoch;
			currentTimeUTC.get(epoch);
			sat.set_epoch(epoch);

			// CALCULATE THE CURRENT POSITION /////////////////////////////////////////////////

			// Use geographiclib for projection
			GeographicLib::Geocentric wgs84_ecef(
				GeographicLib::Constants::WGS84_a(),
				GeographicLib::Constants::WGS84_f()
			);
			GeographicLib::LocalCartesian wgs84_enu(
				worldPtr->GetSphericalCoordinates()->GetLatitudeReference().Degree(),
				worldPtr->GetSphericalCoordinates()->GetLongitudeReference().Degree(),
				worldPtr->GetSphericalCoordinates()->GetElevationReference(),
				wgs84_ecef
			);

			// Find the latitude, longitude and height for the origin
			double lat, lon, h;
			wgs84_enu.Reverse(0, 0, 0, lat, lon, h);

			// USE gpstk for projection
			gpstk::Position originPosGeodetic    = gpstk::Position(
				lat,
				lon,
				h,
				gpstk::Position::Geodetic,
				&wgs84
			);
			gpstk::Position originPosGeocentric  = originPosGeodetic.transformTo(gpstk::Position::Geocentric);
			gpstk::Position originPosECEF        = originPosGeodetic.asECEF();

			// TROPOSHPERIC DELAYS //////////////////////////////////////////////////////////////

			tropModel.setWeather(met.temperature() - 273.15, met.pressure(), met.humidity());

			// IONOSPHERIC DELAYS ///////////////////////////////////////////////////////////////

			gpstk::Triple tec;
			try
			{
				tec = tecStore.getIonexValue(currentTimeGPS, originPosGeocentric, 1);
			}
			catch (const gpstk::Exception& e)
			{
				ROS_WARN("Problem setting ionospheric data: %s", e.what().c_str());
			}

			// TIME EPOCH //////////////////////////////////////////////////////////////////////

			// Clear all existing satellites
			sat.clear_svs();

			// GPS EPHEMERIDES /////////////////////////////////////////////////////////////////

			// Iterate over possible satellite identifiers
			for (int prn = 1; prn <= gpstk::MAX_PRN; ++prn)
			{
				try
				{
					// Don't look for invalid satellites
					if (sp3Ephemerides.isPresent(gpstk::SatID(prn,gpstk::SatID::systemGPS)))
					{
						// Get the ephemeris
						gpstk::Xvt eph = sp3Ephemerides.getXvt(gpstk::SatID(prn,gpstk::SatID::systemGPS),currentTimeGPS);

						// Elevation of the current satellite with respect to HOME position
						gpstk::Triple satellitePos = eph.getPos();

						// Save to the message
						msgs::Vehicle* gps = sat.add_svs();
						gps->set_sys(gpstk::SatID::systemGPS);
						gps->set_prn(prn);

						// Write the position
						gps->mutable_pos()->set_x(satellitePos[0]);
						gps->mutable_pos()->set_y(satellitePos[1]);
						gps->mutable_pos()->set_z(satellitePos[2]);

						// Default = zero error
						gps->mutable_err_pos()->set_x(0);
						gps->mutable_err_pos()->set_y(0);
						gps->mutable_err_pos()->set_z(0);
						gps->set_err_clk(0);
						gps->set_err_tro(0);
						gps->set_err_ion(0);

						// Try and get the broadcast ephemeride
						try
						{
							// Only add noise if its needed
							if (!NoiseFactory::GetEnabled())
								continue;

							// Get the broadcast ephemeris
							gpstk::Xvt ephb = gpsEphemerides.getXvt(
								gpstk::SatID(prn,gpstk::SatID::systemGPS),
								currentTimeGPS
							);

							// Elevation of the current satellite with respect to HOME position
							gpstk::Triple err = ephb.getPos() - satellitePos;

							// Set the ephemeris error
							gps->mutable_err_pos()->set_x(err[0]);
							gps->mutable_err_pos()->set_y(err[1]);
							gps->mutable_err_pos()->set_z(err[2]);

							// Write the clock bias and relativity correction
							gps->set_err_clk(
								SPEED_OF_LIGHT * (ephb.getClockBias() - eph.getClockBias())
							);

							// Set the tropospheic delay, based on the simulation location
							gps->set_err_tro(
								tropModel.correction(
									originPosECEF,                     	 // Current receiver position
									satellitePos,                        // Current satellite position
									currentTimeGPS                       // Time of observation
								)
							);

							// GET L1 ionosphere delay
							gps->set_err_ion(
								tecStore.getIonoL1(
									originPosECEF.elvAngle(satellitePos), // Elevation of the satellite
									tec[0],                               // Total electron count
									"NONE"                                // Mapping function
								)
							);
						}
						catch (const gpstk::Exception& e)
						{
							ROS_DEBUG("GPS-B: %s", e.what().c_str());

							// Draw a new ephemeride error
							math::Vector3 	err_pos = nGPSeph[prn-1]->DrawVector(t);
							double       	err_clk = nGPSclk[prn-1]->DrawScalar(t);
							double       	err_ion = nGPSion[prn-1]->DrawScalar(t);
							double       	err_tro = nGPStro[prn-1]->DrawScalar(t);

							// Set the ephemeris error
							gps->mutable_err_pos()->set_x(err_pos.x);
							gps->mutable_err_pos()->set_y(err_pos.y);
							gps->mutable_err_pos()->set_z(err_pos.z);
							gps->set_err_clk(err_clk);
							gps->set_err_tro(err_tro);
							gps->set_err_ion(err_ion);

						}
					}
				}
				catch (const gpstk::Exception& e)
				{
				  ROS_DEBUG("GPS-F: %s", e.what().c_str());
				}
			}

			// GLO EPHEMERIDES /////////////////////////////////////////////////////////////////

			for (int prn = 1; prn <= gpstk::MAX_PRN; ++prn)
			{
				try
				{
				  	// Don't look for invalid satellites
				  	if (sp3Ephemerides.isPresent(gpstk::SatID(prn,gpstk::SatID::systemGlonass)))
				  	{
					    // Get the ephemeris (not that SP3 are in GPST)
					    gpstk::Xvt eph = sp3Ephemerides.getXvt(gpstk::SatID(prn,gpstk::SatID::systemGlonass),currentTimeGPS);

					    // Elevation of the current satellite with respect to HOME position
					    gpstk::Triple satellitePos = eph.getPos();

						// Save to the message
						msgs::Vehicle* glo = sat.add_svs();
						glo->set_sys(gpstk::SatID::systemGlonass);
						glo->set_prn(prn);

						// Write the position
						glo->mutable_pos()->set_x(satellitePos[0]);
						glo->mutable_pos()->set_y(satellitePos[1]);
						glo->mutable_pos()->set_z(satellitePos[2]);

						// Default = zero error
						glo->mutable_err_pos()->set_x(0);
						glo->mutable_err_pos()->set_y(0);
						glo->mutable_err_pos()->set_z(0);
						glo->set_err_clk(0);
						glo->set_err_tro(0);
						glo->set_err_ion(0);

						// Try and get the broadcast ephemeride
						try
						{
							// Only add noise if its needed
							if (!NoiseFactory::GetEnabled())
								continue;

							// Get the broadcast ephemeris
							gpstk::Xvt ephb = gloEphemerides.getXvt(
								gpstk::SatID(prn,gpstk::SatID::systemGlonass),
								currentTimeGLO
							);

							// Elevation of the current satellite with respect to HOME position
							gpstk::Triple err = ephb.getPos() - satellitePos;

							// Set the ephemeris error
							glo->mutable_err_pos()->set_x(err[0]);
							glo->mutable_err_pos()->set_y(err[1]);
							glo->mutable_err_pos()->set_z(err[2]);

							// Write the clock bias and relativity correction
							glo->set_err_clk(
								SPEED_OF_LIGHT * (ephb.getClockBias()-eph.getClockBias())
							);

							// Set the tropospheic delay, based on the simulation location
							glo->set_err_tro(
								tropModel.correction(
									originPosECEF,                       // Current receiver position
									satellitePos,                        // Current satellite position
									currentTimeGPS                       // Time of observation
								)
							);

							// GET L1 ionosphere delay
							glo->set_err_ion(
								tecStore.getIonoL1(
									originPosECEF.elvAngle(satellitePos), // Elevation of the satellite
									tec[0],                               // Total electron count
									"NONE"                                // Mapping function
								)
							);
						}
						catch (const gpstk::Exception& e)
						{
							ROS_DEBUG("GLO-B: %s", e.what().c_str());

							// Draw a new ephemeride error
							math::Vector3	err_pos = nGLOeph[prn-1]->DrawVector(t);
							double       	err_clk = nGLOclk[prn-1]->DrawScalar(t);
							double       	err_ion = nGLOion[prn-1]->DrawScalar(t);
							double       	err_tro = nGLOtro[prn-1]->DrawScalar(t);

							// Set the ephemeris error
							glo->mutable_err_pos()->set_x(err_pos.x);
							glo->mutable_err_pos()->set_y(err_pos.y);
							glo->mutable_err_pos()->set_z(err_pos.z);
							glo->set_err_clk(err_clk);
							glo->set_err_tro(err_tro);
							glo->set_err_ion(err_ion);
						}
					}
				}
				catch(const gpstk::Exception& e)
				{
				  ROS_DEBUG("GLO-F: %s", e.what().c_str());
				}
			}

			// Publish wind information to all subscribers
			ROS_DEBUG("Sending satellite data");
			pubPtr->Publish(sat);
	    }

	    // CALLBACKS FOR MESSAGES //////////////////////////////////////////////////

		// This will be called whenever a new meteorlogical topic is posted
		void ReceiveEnvironment(EnvironmentPtr& msg)
		{
			// Use copy constructor to indicate
			env = *msg;

			// Received an environment update message
			rcvEnv = true;
		}

		// This will be called whenever a new meteorlogical topic is posted
		void ReceiveMeteorological(MeteorologicalPtr& msg)
		{
			// Use copy constructor to indicate
			met = *msg;

			// Received an environment update message
			rcvMet = true;
		}

		// This will be called whenever a new meteorlogical topic is posted
		void ReceiveNoise(NoisePtr& inmsg)
		{
			NoiseFactory::SetEnabled(inmsg->enable());
		}

	public:

		// Constructor
		Satellites() : rosNode(ros::NodeHandle("satellites")),
			rate(1.0), rcvMet(false), rcvEnv(false)
		{
			// Make sure that ROS actually started, or there will be some issues...
			if (!ros::isInitialized())
			    ROS_FATAL("A ROS node has not been initialized");

			// Initialise a noise factory
			NoiseFactory::Init();

		}

		// Destructor
		~Satellites()
		{
			// Clear the noise factory
			NoiseFactory::Destroy();
		}

        // All sensors must be configured using the current world information
        void Load(physics::WorldPtr world, sdf::ElementPtr root)
        {
			// Save the world pointer
			worldPtr = world;

			// Basic parameters
			root->GetElement("rate")->GetValue()->Get(rate);

		    // Create a noise distribution for each satellite
		    for (int prn = 1; prn <= gpstk::MAX_PRN; ++prn)
		    {
				nGPSeph[prn-1] = NoiseFactory::Create(root->GetElement("errors")->GetElement("gpseph"));
				nGPSclk[prn-1] = NoiseFactory::Create(root->GetElement("errors")->GetElement("gpsclk"));
				nGPStro[prn-1] = NoiseFactory::Create(root->GetElement("errors")->GetElement("gpstro"));
				nGPSion[prn-1] = NoiseFactory::Create(root->GetElement("errors")->GetElement("gpsion"));
				nGLOeph[prn-1] = NoiseFactory::Create(root->GetElement("errors")->GetElement("gloeph"));
				nGLOclk[prn-1] = NoiseFactory::Create(root->GetElement("errors")->GetElement("gloclk"));
				nGLOtro[prn-1] = NoiseFactory::Create(root->GetElement("errors")->GetElement("glotro"));
				nGLOion[prn-1] = NoiseFactory::Create(root->GetElement("errors")->GetElement("gloion"));
			}

			// PROCESS THE RINEX AND SP3 FILES FOR DATA ///////////////////////////

			// Will be useful for parsing the sdf
			sdf::ElementPtr el;

			// So that we don't use bad satellite positions as the truth
			sp3Ephemerides.rejectBadPositions(true);
			sp3Ephemerides.rejectBadClocks(true);

			// Extract any
			try
			{
				el = root->GetElement("ephemerides")->GetElement("final")->GetElement("gps")->GetFirstElement();
				do
				{
					// Obtain the URI
					std::string uri;
					el->GetValue()->Get(uri);

					// Open and store final ephemerides (Note that all times are GPS)
					sp3Ephemerides.loadSP3File(
						common::SystemPaths::Instance()->FindFileURI(uri).c_str());
				}
				while(el = el->GetNextElement());
			}
			catch (const std::exception& e)
			{
				ROS_DEBUG("Could not obtain GPS final ephemerides: %s",e.what());
			}

			try
			{
				el = root->GetElement("ephemerides")->GetElement("final")->GetElement("glo")->GetFirstElement();
				do
				{
					// Obtain the URI
					std::string uri;
					el->GetValue()->Get(uri);

					// Open and store final ephemerides
					sp3Ephemerides.loadSP3File(
						common::SystemPaths::Instance()->FindFileURI(uri).c_str());
				}
				while(el = el->GetNextElement());
			}
			catch (const std::exception& e)
			{
				ROS_DEBUG("Could not obtain final Glonass ephemerides: %s",e.what());
			}

			// OPEN AND STORE GPS AND GLONASS BROADCAST EPHEMERIDES /////////////////////////////////////////////

			try
			{
				// This is not how I would have chosen to setup the API...
				el = root->GetElement("ephemerides")->GetElement("broadcast")->GetElement("gps")->GetFirstElement();
				do
				{
					// Obtain the URI
					std::string uri;
					el->GetValue()->Get(uri);

					// Read nav file and store unique list of ephemerides
					gpstk::Rinex3NavStream gps_rnffs(
						common::SystemPaths::Instance()->FindFileURI(uri).c_str());
					gpstk::Rinex3NavData   gps_rne;
					gpstk::Rinex3NavHeader gps_hdr;

					// Let's read the header (may be skipped)
					gps_rnffs >> gps_hdr;

					// Storing the ephemeris in "bcstore"
					while (gps_rnffs >> gps_rne)
					   gpsEphemerides.addEphemeris(gps_rne);

				}
				while(el = el->GetNextElement());
			}
			catch (const std::exception& e)
			{
				ROS_WARN("Could not obtain broadcast GPS ephemerides: %s",e.what());
			}

			try
			{
				// This is not how I would have chosen to setup the API...
				el = root->GetElement("ephemerides")->GetElement("broadcast")->GetElement("glo")->GetFirstElement();
				do
				{
					// Obtain the URI
					std::string uri;
					el->GetValue()->Get(uri);

					// Read nav file and store unique list of ephemerides
					gpstk::Rinex3NavStream glo_rnffs(common::SystemPaths::Instance()->FindFileURI(uri).c_str());
					gpstk::Rinex3NavData   glo_rne;
					gpstk::Rinex3NavHeader glo_hdr;

					// Let's read the header (may be skipped)
					glo_rnffs >> glo_hdr;

					// Storing the ephemeris in "bcstore"
					while (glo_rnffs >> glo_rne)
					   gloEphemerides.addEphemeris(glo_rne);

				}
				while(el = el->GetNextElement());
			}
			catch (const std::exception& e)
			{
				ROS_DEBUG("Could not obtain any broadcast Glonass ephemerides: %s",e.what());
			}

			// OPEN AND STORE IONOSPHERERE INFORMATION /////////////////////////////////////////////

			try
			{
				// This is not how I would have chosen to setup the API...
				el = root->GetElement("ionosphere")->GetFirstElement();
				do
				{
					// Obtain the URI
					std::string uri;
					el->GetValue()->Get(uri);

					// Open meteorological data file
					gpstk::IonexStream is(common::SystemPaths::Instance()->FindFileURI(uri).c_str());
					gpstk::IonexHeader ih;
					gpstk::IonexData   id;

					// Let's read the header (may be skipped)
					is >> ih;

					// Storing the ephemeris in "bcstore"
					while (is >> id)
					{
						id.time.setTimeSystem(gpstk::TimeSystem::GPS);
						tecStore.addMap(id);
					}
				}
				while (el = el->GetNextElement());
			}
			catch (const std::exception& e)
			{
				ROS_DEBUG("Could not obtain ionex data: %s",e.what());
			}

			// SETUP MESSAGING //////////////////////////////////////////////////////////////////

			// Create and initialize a new Gazebo transport node
			nodePtr = transport::NodePtr(new transport::Node());
			nodePtr->Init(worldPtr->GetName());

			// Broadcast messages on the /global/satellites topic
			pubPtr = nodePtr->Advertise<msgs::Satellites>("~/satellites");

			// Subscribe to meteorological updates
			subPtrMet = nodePtr->Subscribe("~/meteorological",
				&Satellites::ReceiveMeteorological, this);

			// Subscribe to meteorological updates
			subPtrEnv = nodePtr->Subscribe("~/environment",
				&Satellites::ReceiveEnvironment, this);

			// Subscribe to meteorological updates
			subPtrNoi = nodePtr->Subscribe("~/noise",
				&Satellites::ReceiveNoise, this);

			// ROS timer respects gazebo
			if (rate > 0)
			{
				timer = rosNode.createTimer(
					ros::Duration(1.0/rate),
					&Satellites::Update,
					this,
			        false,                                      // Oneshot
			        true                                        // Autostart
				);
			}

			// Reset module
			Reset();
        }

        // All sensors must be resettable
        void Reset()
        {
			rcvMet = false;
			rcvEnv = false;
        }

	};

	// Resgister the plugin
	GZ_REGISTER_WORLD_PLUGIN(Satellites);
}


/**
 * \file Test program for checking registration quality. Reads two
 * point clouds and a transformation, and computes a measure of
 * whether they are aligned or not.
 *
 * \author Martin Magnusson
 * \date   Created 2014-10-14
 */

#include "pcl/io/pcd_io.h"
#include "pcl/point_cloud.h"
#include <Eigen/Geometry> // for eulerAngles
#include <iostream>
#include <ndt_registration/ndt_matcher_d2d.h>
#include <ndt_registration/ndt_matcher_p2d.h>
#include <string>
//#include <boost/program_options.hpp>
//#include <cstdlib>

using namespace std;
using namespace perception_oru;

int main( int argc, char** argv )
{
  if (argc < 8+1)
  {
    std::cerr << "Usage: " << argv[0] << " <pc1> <pc2> <3xtranslation> <3xrotation>\n";
    return -1;
  }
  int i = 1;
  std::string file_fixed  (argv[i++]);
  std::string file_offset (argv[i++]);

  double tx (atof(argv[i++]));
  double ty (atof(argv[i++]));
  double tz (atof(argv[i++]));
  double rx (atof(argv[i++]));
  double ry (atof(argv[i++]));
  double rz (atof(argv[i++]));

  Eigen::Transform<double,3,Eigen::Affine> T;
  T = Eigen::Translation3d( tx, ty, tz );
   //Eigen::AngleAxisd( 0.0, Eigen::Vector3d( 1.0, 0.0, 0.0 ) );
  //  Eigen::eulerAngles( rx, ry, rz );
    //  T.setIdentity();

  pcl::PointCloud<pcl::PointXYZ> cloud_fixed;
  if (pcl::io::loadPCDFile (file_fixed, cloud_fixed) == -1)
  {
    cerr << "Was not able to open file \"" << file_fixed << "\".\n";
    return 1;
  }
  pcl::PointCloud<pcl::PointXYZ> cloud_offset;
  if (pcl::io::loadPCDFile (file_offset, cloud_offset) == -1)
  {
    cerr << "Was not able to open file \"" << file_offset << "\".\n";
    return 1;
  }

  perception_oru::transformPointCloudInPlace( T, cloud_offset );

  std::vector<double> resolutions;
  resolutions.push_back( 1 );
  NDTMatcherP2D matcherP2D(resolutions);
  NDTMatcherD2D matcherD2D(false, false, resolutions);

  Eigen::Matrix<double,6,6> covP2D;
  Eigen::MatrixXd covD2D(6,6);
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6> > Sol ;
  //Eigen::SelfAdjointEigenSolver<Eigen::Matrix6d > Sol ();
  Eigen::Matrix<double,6,1> evals;

  // Check the alignment before registration
  //matcherP2D.check( cloud_fixed, cloud_offset, T );
  matcherP2D.covariance( cloud_fixed, cloud_offset, T, covP2D );
  cout << "Cov(P2D)\n" << covP2D << "\n";
  Sol.compute( covP2D );
  evals = Sol.eigenvalues().real();
  cout << "Cov(P2D) eigenvalues "
       << evals[0] << " "
       << evals[1] << " "
       << evals[2] << " "
       << evals[3] << " "
       << evals[4] << " "
       << evals[5] << " "
       << "\n";
  matcherD2D.covariance( cloud_fixed, cloud_offset, T, covD2D );
  Sol.compute( covD2D );
  evals = Sol.eigenvalues().real();
  cout << "Cov(D2D) eigenvalues "
       << evals[0] << " "
       << evals[1] << " "
       << evals[2] << " "
       << evals[3] << " "
       << evals[4] << " "
       << evals[5] << " "
       << "\n";

  // (optionally) perform registration
  matcherP2D.ITR_MAX = 0;
  matcherP2D.subsample_size = 0.4;
  bool converged = matcherP2D.match( cloud_fixed, cloud_offset, T );
  if (not converged)
    cerr << "warning: matcher terminated before convergence\n";

  cout << "Result T: " << T.matrix() << "\n";

  // Now check the alignment
  //matcherP2D.check( cloud_fixed, cloud_offset, T );
  matcherP2D.covariance( cloud_fixed, cloud_offset, T, covP2D );
  cout << "Cov(P2D)\n" << covP2D << "\n";
  Sol.compute(covP2D);
  evals = Sol.eigenvalues().real();
  cout << "Cov(P2D) eigenvalues "
       << evals[0] << " "
       << evals[1] << " "
       << evals[2] << " "
       << evals[3] << " "
       << evals[4] << " "
       << evals[5] << " "
       << "\n";
  matcherD2D.covariance( cloud_fixed, cloud_offset, T, covD2D );
  Sol.compute( covD2D );
  evals = Sol.eigenvalues().real();
  cout << "Cov(D2D) eigenvalues "
       << evals[0] << " "
       << evals[1] << " "
       << evals[2] << " "
       << evals[3] << " "
       << evals[4] << " "
       << evals[5] << " "
       << "\n";

  // pcl::io::savePCDFile ("test_pcd_fixed.pcd", cloud_fixed);
  // pcl::io::savePCDFile ("test_pcd_trans.pcd", cloud_trans);

  return 0;
}


//////////////////////////////////////////////////////////////////////////////////
// survival::models::exponential::scalar::function::log_unnormalized_pdf.hpp    //
//                                                                              //
//  Copyright 2009 Erwann Rogard. 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)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_FUNCTION_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_FUNCTION_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <stdexcept>
#include <string>
#include <boost/mpl/identity.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#include <boost/fusion/sequence/intrinsic/value_at_key.hpp>
#include <boost/statistics/detail/distribution/model/key/covariate.hpp>
#include <boost/statistics/detail/distribution/model/key/response.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/model.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/detail/lpdf_rt.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{

    template<typename T,typename D,typename U>
    T
    log_unnormalized_pdf(
        const exponential_model<T,D>& m,
        const U& unit
    ){

        typedef exponential_model<T, D> m_;
        typedef typename distribution::meta::value<m_>::type val_;

        typedef std::string str_;

        val_ log_rate  = m.log_rate(
            boost::fusion::at_key<distribution::model::tag::covariate>(unit)
        );

        return exponential_model_::lpdf_rt(
            log_rate,
            boost::fusion::at_key<distribution::model::tag::response>(unit)
        );
    }

}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


#include <iostream>
#include <cstring>
#include <string>
#include <random>

#include <Eigen/Dense>

#include "sim_node_network.hpp"
#include "sim_node.hpp"

const char* DATA_FILE_ARG = "data";
const char* NUM_NODES_ARG = "nodes";
const char* SENSING_RANGE_ARG = "sense_range";
const char* COMMUNICATION_RANGE_ARG = "comm_range";
const char* FIELD_X_ARG = "x";
const char* FIELD_Y_ARG = "y";
const char* FIELD_CELLS_ARG = "cells";

const int SEED = 1;

class Configurations{
 public:
  std::string data_file_name;
  int number_of_sensor_nodes;
  double sensing_range;
  double communication_range;
  double field_x_size;
  double field_y_size;
  int field_cells;

  Configurations() {
    data_file_name = "data_files_should_have_names";
    number_of_sensor_nodes = 10;
    sensing_range = 1.6;
    communication_range = 1.5;
    field_x_size = 4.0;
    field_y_size = 4.0;
    field_cells = 25;
  };
};

Configurations process_command_line_arguments(int pArgc, char** pArguments);

SimNode::CoordinatePair createRandomLocation(
  std::default_random_engine& p_generator,
  std::normal_distribution<double>& p_x_distribution,
  std::normal_distribution<double>& p_y_distribution);

int main(int num_arguments, char** arguments) {
  puts("Parsing arguments...");
  Configurations configurations = process_command_line_arguments(
    num_arguments,
    arguments
  );

  puts("Preparing random number generator...");
  std::default_random_engine random_engine(SEED);
  std::normal_distribution<double> x_location_distribution(
    0.0,
    configurations.field_x_size / 2.0
  );
  std::normal_distribution<double> y_location_distribution(
    0.0,
    configurations.field_y_size / 2.0);
  double sample = x_location_distribution(random_engine);

  puts("Generating target field...");
  std::vector<SimNode::Target> target_field;
  target_field.push_back({50.0, {0.0, 0.0}});

  puts("Generating sensor node network...");
  SimNodeNetwork sensorNodeNetwork(SimNodeNetwork::MaxDegreeConsensus, 0.001);
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    SimNode::CoordinatePair location = createRandomLocation(
      random_engine,
      x_location_distribution,
      y_location_distribution
    );

std::cout << location.x << ' ' << location.y << std::endl;

    sensorNodeNetwork.AddNode(
      configurations.sensing_range,
      configurations.communication_range,
      location
    );
  }

  puts("Performing consensus filter...");
  std::vector<SimNodeNetwork::EstimateLog> logs =
    sensorNodeNetwork.BuildMapOfField(target_field);
  // record initial readings for output

  // output results

  return 0;
}

Configurations
process_command_line_arguments(int pArgc, char** pArguments) {
  Configurations configurations;

  for (int i = 1; i < pArgc; ++i) {
    char* arg_value_pair = pArguments[i];
    std::string argument = strtok(arg_value_pair, "-=");
    char* value = strtok(NULL, "=");

    if (argument == DATA_FILE_ARG) {
      configurations.data_file_name = value;
      printf(
        "Data File: %s\n",
        configurations.data_file_name.c_str()
      );
    } else if (argument == NUM_NODES_ARG) {
      configurations.number_of_sensor_nodes = atoi(value);
      printf(
        "Number of senor nodes: %i\n",
        configurations.number_of_sensor_nodes
      );
    } else if (argument == SENSING_RANGE_ARG) {
      configurations.sensing_range = strtod(value,NULL);
      printf(
        "Node sensing range: %f\n",
        configurations.sensing_range
      );
    } else if (argument == COMMUNICATION_RANGE_ARG) {
      configurations.communication_range = strtod(value, NULL);
      printf(
        "Node communication range: %f\n",
        configurations.communication_range
      );
    } else if (argument == FIELD_X_ARG) {
      configurations.field_x_size = strtod(value, NULL);
      printf(
        "Field X size: %f\n",
        configurations.field_x_size
      );
    } else if (argument == FIELD_Y_ARG) {
      configurations.field_y_size = strtod(value, NULL);
      printf(
        "Field Y size: %f\n",
        configurations.field_y_size
      );
    } else if (argument == FIELD_CELLS_ARG) {
      configurations.field_cells = atoi(value);
      printf(
        "Cells per side of the field: %i\n",
        configurations.field_cells
      );
    }else {
      printf(
        "%s is an unrecognized argument. Program terminating.\n",
        argument.c_str()
      );
      throw std::exception();
    }
  }

  return configurations;
}

SimNode::CoordinatePair
createRandomLocation(
  std::default_random_engine& p_generator,
  std::normal_distribution<double>& p_x_distribution,
  std::normal_distribution<double>& p_y_distribution) {

  return {p_x_distribution(p_generator), p_y_distribution(p_generator)};
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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)

//! \file
//!\brief Tests basic mixed-math fixed_point negatable.

#define BOOST_TEST_MODULE test_negatable_basic_mixed_math
#define BOOST_LIB_DIAGNOSTIC

#include <algorithm>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>

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

typedef boost::fixed_point::negatable<12,  -8> fixed_point_type_more_range;
typedef boost::fixed_point::negatable<10, -10> fixed_point_type_more_resol;

BOOST_AUTO_TEST_CASE(test_negatable_basic_mixed_math)
{
  {
    const fixed_point_type_more_range a(4.25F);
    const fixed_point_type_more_resol b(2.5F);

    const auto c = a + b;

    const decltype(c) control(6.75F);

    BOOST_CHECK_EQUAL(c, control);
  }

  {
    const fixed_point_type_more_resol a(4.25F);
    const fixed_point_type_more_range b(2.5F);

    const auto c = a + b;

    const decltype(c) control(6.75F);

    BOOST_CHECK_EQUAL(c, control);
  }

  {
    const fixed_point_type_more_range a(4.25F);
    const fixed_point_type_more_resol b(2.5F);

    const auto c = a - b;

    const decltype(c) control(1.75F);

    BOOST_CHECK_EQUAL(c, control);
  }

  {
    const fixed_point_type_more_resol a(4.25F);
    const fixed_point_type_more_range b(2.5F);

    const auto c = a - b;

    const decltype(c) control(1.75F);

    BOOST_CHECK_EQUAL(c, control);
  }

  {
    const fixed_point_type_more_range a(4.25F);
    const fixed_point_type_more_resol b(2.5F);

    const auto c = a * b;

    const decltype(c) control(10.625F);

    BOOST_CHECK_EQUAL(c, control);
  }

  {
    const fixed_point_type_more_resol a(4.25F);
    const fixed_point_type_more_range b(2.5F);

    const auto c = a * b;

    const decltype(c) control(10.625F);

    BOOST_CHECK_EQUAL(c, control);
  }

  {
    const fixed_point_type_more_range a(4.5F);
    const fixed_point_type_more_resol b(2.25F);

    const auto c = a / b;

    const decltype(c) control(2.0F);

    BOOST_CHECK_EQUAL(c, control);
  }

  {
    const fixed_point_type_more_resol a(4.5F);
    const fixed_point_type_more_range b(2.25F);

    const auto c = a / b;

    const decltype(c) control(2.0F);

    BOOST_CHECK_EQUAL(c, control);
  }
}


#pragma once

#include <Eigen/Core>
#include <Eigen/LU>
#include <cppad/cppad.hpp>

// copy of eigen_mat_inv from CppAD to make the reverse pass codegen compatible (not assume double scalar)

namespace tds {
template <class Base>
class atomic_eigen_mat_inv : public CppAD::atomic_base<Base> {
 public:
  // -----------------------------------------------------------
  // type of elements during calculation of derivatives
  typedef Base scalar;
  // type of elements during taping
  typedef CppAD::AD<scalar> ad_scalar;
  // type of matrix during calculation of derivatives
  typedef Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
      matrix;
  // type of matrix during taping
  typedef Eigen::Matrix<ad_scalar, Eigen::Dynamic, Eigen::Dynamic,
                        Eigen::RowMajor>
      ad_matrix;
  /* %$$
  $subhead Constructor$$
  $srccode%cpp% */
  // constructor
  atomic_eigen_mat_inv(void)
      : CppAD::atomic_base<Base>("atom_eigen_mat_inv",
                                 CppAD::atomic_base<Base>::set_sparsity_enum) {}
  /* %$$
  $subhead op$$
  $srccode%cpp% */
  // use atomic operation to invert an AD matrix
  ad_matrix op(const ad_matrix& arg) {
    size_t nr = size_t(arg.rows());
    size_t ny = nr * nr;
    size_t nx = 1 + ny;
    assert(nr == size_t(arg.cols()));
    // -------------------------------------------------------------------
    // packed version of arg
    CPPAD_TESTVECTOR(ad_scalar) packed_arg(nx);
    packed_arg[0] = ad_scalar(nr);
    for (size_t i = 0; i < ny; i++) packed_arg[1 + i] = arg.data()[i];
    // -------------------------------------------------------------------
    // packed version of result = arg^{-1}.
    // This is an atomic_base function call that CppAD uses to
    // store the atomic operation on the tape.
    CPPAD_TESTVECTOR(ad_scalar) packed_result(ny);
    (*this)(packed_arg, packed_result);
    // -------------------------------------------------------------------
    // unpack result matrix
    ad_matrix result(nr, nr);
    for (size_t i = 0; i < ny; i++) result.data()[i] = packed_result[i];
    return result;
  }
  /* %$$
$head Private$$

$subhead Variables$$
$srccode%cpp% */
 private:
  // -------------------------------------------------------------
  // one forward mode vector of matrices for argument and result
  CppAD::vector<matrix> f_arg_, f_result_;
  // one reverse mode vector of matrices for argument and result
  CppAD::vector<matrix> r_arg_, r_result_;
  // -------------------------------------------------------------
  /* %$$
  $subhead forward$$
  $srccode%cpp% */
  // forward mode routine called by CppAD
  virtual bool forward(
      // lowest order Taylor coefficient we are evaluating
      size_t p,
      // highest order Taylor coefficient we are evaluating
      size_t q,
      // which components of x are variables
      const CppAD::vector<bool>& vx,
      // which components of y are variables
      CppAD::vector<bool>& vy,
      // tx [ j * (q+1) + k ] is x_j^k
      const CppAD::vector<scalar>& tx,
      // ty [ i * (q+1) + k ] is y_i^k
      CppAD::vector<scalar>& ty) {
    size_t n_order = q + 1;
    size_t nr = size_t(CppAD::Integer(tx[0 * n_order + 0]));
    size_t ny = nr * nr;
#ifndef NDEBUG
    size_t nx = 1 + ny;
#endif
    assert(vx.size() == 0 || nx == vx.size());
    assert(vx.size() == 0 || ny == vy.size());
    assert(nx * n_order == tx.size());
    assert(ny * n_order == ty.size());
    //
    // -------------------------------------------------------------------
    // make sure f_arg_ and f_result_ are large enough
    assert(f_arg_.size() == f_result_.size());
    if (f_arg_.size() < n_order) {
      f_arg_.resize(n_order);
      f_result_.resize(n_order);
      //
      for (size_t k = 0; k < n_order; k++) {
        f_arg_[k].resize(long(nr), long(nr));
        f_result_[k].resize(long(nr), long(nr));
      }
    }
    // -------------------------------------------------------------------
    // unpack tx into f_arg_
    for (size_t k = 0; k < n_order; k++) {  // unpack arg values for this order
      for (size_t i = 0; i < ny; i++)
        f_arg_[k].data()[i] = tx[(1 + i) * n_order + k];
    }
    // -------------------------------------------------------------------
    // result for each order
    // (we could avoid recalculting f_result_[k] for k=0,...,p-1)
    //
    f_result_[0] = f_arg_[0].inverse();
    for (size_t k = 1; k < n_order; k++) {  // initialize sum
      matrix f_sum = matrix::Zero(long(nr), long(nr));
      // compute sum
      for (size_t ell = 1; ell <= k; ell++)
        f_sum -= f_arg_[ell] * f_result_[k - ell];
      // result_[k] = arg_[0]^{-1} * sum_
      f_result_[k] = f_result_[0] * f_sum;
    }
    // -------------------------------------------------------------------
    // pack result_ into ty
    for (size_t k = 0; k < n_order; k++) {
      for (size_t i = 0; i < ny; i++)
        ty[i * n_order + k] = f_result_[k].data()[i];
    }
    // -------------------------------------------------------------------
    // check if we are computing vy
    if (vx.size() == 0) return true;
    // ------------------------------------------------------------------
    // This is a very dumb algorithm that over estimates which
    // elements of the inverse are variables (which is not efficient).
    bool var = false;
    for (size_t i = 0; i < ny; i++) var |= vx[1 + i];
    for (size_t i = 0; i < ny; i++) vy[i] = var;
    return true;
  }
  /* %$$
  $subhead reverse$$
  $srccode%cpp% */
  // reverse mode routine called by CppAD
  virtual bool reverse(
      // highest order Taylor coefficient that we are computing derivative of
      size_t q,
      // forward mode Taylor coefficients for x variables
      const CppAD::vector<Base>& tx,
      // forward mode Taylor coefficients for y variables
      const CppAD::vector<Base>& ty,
      // upon return, derivative of G[ F[ {x_j^k} ] ] w.r.t {x_j^k}
      CppAD::vector<Base>& px,
      // derivative of G[ {y_i^k} ] w.r.t. {y_i^k}
      const CppAD::vector<Base>& py) {
    size_t n_order = q + 1;
    size_t nr = size_t(CppAD::Integer(tx[0 * n_order + 0]));
    size_t ny = nr * nr;
#ifndef NDEBUG
    size_t nx = 1 + ny;
#endif
    //
    assert(nx * n_order == tx.size());
    assert(ny * n_order == ty.size());
    assert(px.size() == tx.size());
    assert(py.size() == ty.size());
    // -------------------------------------------------------------------
    // make sure f_arg_ is large enough
    assert(f_arg_.size() == f_result_.size());
    // must have previous run forward with order >= n_order
    assert(f_arg_.size() >= n_order);
    // -------------------------------------------------------------------
    // make sure r_arg_, r_result_ are large enough
    assert(r_arg_.size() == r_result_.size());
    if (r_arg_.size() < n_order) {
      r_arg_.resize(n_order);
      r_result_.resize(n_order);
      //
      for (size_t k = 0; k < n_order; k++) {
        r_arg_[k].resize(long(nr), long(nr));
        r_result_[k].resize(long(nr), long(nr));
      }
    }
    // -------------------------------------------------------------------
    // unpack tx into f_arg_
    for (size_t k = 0; k < n_order; k++) {  // unpack arg values for this order
      for (size_t i = 0; i < ny; i++)
        f_arg_[k].data()[i] = tx[(1 + i) * n_order + k];
    }
    // -------------------------------------------------------------------
    // unpack py into r_result_
    for (size_t k = 0; k < n_order; k++) {
      for (size_t i = 0; i < ny; i++)
        r_result_[k].data()[i] = py[i * n_order + k];
    }
    // -------------------------------------------------------------------
    // initialize r_arg_ as zero
    for (size_t k = 0; k < n_order; k++)
      r_arg_[k] = matrix::Zero(long(nr), long(nr));
    // -------------------------------------------------------------------
    // matrix reverse mode calculation
    //
    for (size_t k1 = n_order; k1 > 1; k1--) {
      size_t k = k1 - 1;
      // bar{R}_0 = bar{R}_0 + bar{R}_k (A_0 R_k)^T
      r_result_[0] +=
          r_result_[k] * f_result_[k].transpose() * f_arg_[0].transpose();
      //
      for (size_t ell = 1; ell <= k;
           ell++) {  // bar{A}_l = bar{A}_l - R_0^T bar{R}_k R_{k-l}^T
        r_arg_[ell] -= f_result_[0].transpose() * r_result_[k] *
                       f_result_[k - ell].transpose();
        // bar{R}_{k-l} = bar{R}_{k-1} - (R_0 A_l)^T bar{R}_k
        r_result_[k - ell] -=
            f_arg_[ell].transpose() * f_result_[0].transpose() * r_result_[k];
      }
    }
    r_arg_[0] -=
        f_result_[0].transpose() * r_result_[0] * f_result_[0].transpose();
    // -------------------------------------------------------------------
    // pack r_arg into px
    for (size_t k = 0; k < n_order; k++) {
      for (size_t i = 0; i < ny; i++)
        px[(1 + i) * n_order + k] = r_arg_[k].data()[i];
    }
    //
    return true;
  }
};
}  // namespace tds


#include "graph.hpp"
#include "algorithms.hpp"
#include "printing.hpp"

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_ALTERNATIVE_INIT_API
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

using namespace graph;

BOOST_AUTO_TEST_CASE(algorithms_symmetric) {
	using Graph = list::Graph<NoProperty, NoProperty>;

	Graph myGraph{{"1", "2"}, {"3", "4"}, {"5", "6"}}, expected{{"2", "1"}, {"4", "3"}, {"6", "5"}};

	BOOST_CHECK(graph::symmetric(myGraph) == expected);
}

BOOST_AUTO_TEST_CASE(algorithms_undirected) {
	using Graph = list::Graph<NoProperty, NoProperty>;

	Graph myGraph{{"1", "2"}, {"3", "4"}, {"5", "6"}},
	        expected{{"1", "2"}, {"2", "1"}, {"3", "4"}, {"4", "3"}, {"5", "6"}, {"6", "5"}};

	BOOST_CHECK(graph::undirected(myGraph) == expected);
}

BOOST_AUTO_TEST_CASE(algorithms_strongly_connected_component) {
	using Graph = list::Graph<NoProperty, NoProperty>;
	using ConstNode = Graph::ConstNode_t;

	const Graph myGraph{{"0", "1"},
	                    {"0", "5"},
	                    {"1", "2"},
	                    {"2", "5"},
	                    {"3", "1"},
	                    {"3", "2"},
	                    {"4", "2"},
	                    {"5", "1"},
	                    {"5", "3"},
	                    {"5", "4"}};
	std::set<ConstNode> expectedFor0{myGraph["0"]},
	        expectedForOthers{myGraph["1"], myGraph["2"], myGraph["3"], myGraph["4"], myGraph["5"]};

	BOOST_CHECK(graph::stronglyConnectedComponent(myGraph, myGraph["0"]) == expectedFor0);
	BOOST_CHECK(graph::stronglyConnectedComponent(myGraph, myGraph["1"]) == expectedForOthers);
	BOOST_CHECK(graph::stronglyConnectedComponent(myGraph, myGraph["2"]) == expectedForOthers);
	BOOST_CHECK(graph::stronglyConnectedComponent(myGraph, myGraph["5"]) == expectedForOthers);
}

BOOST_AUTO_TEST_CASE(algorithms_connected_component) {
	using Graph = list::Graph<NoProperty, NoProperty>;
	using ConstNode = Graph::ConstNode_t;

	const Graph myGraph{
		    {"0", "1"}, {"1", "2"}, {"2", "0"}, {"3", "4"}, {"4", "3"}, {"5", "6"}, {"7", "7"}};
	std::set<ConstNode> expectedFor0{myGraph["0"], myGraph["1"], myGraph["2"]},
	        expectedFor3{myGraph["3"], myGraph["4"]}, expectedFor5{myGraph["5"], myGraph["6"]},
	        expectedFor7{myGraph["7"]};

	BOOST_CHECK(graph::connectedComponent(myGraph, myGraph["0"]) == expectedFor0);
	BOOST_CHECK(graph::connectedComponent(myGraph, myGraph["1"]) == expectedFor0);
	BOOST_CHECK(graph::connectedComponent(myGraph, myGraph["2"]) == expectedFor0);
	BOOST_CHECK(graph::connectedComponent(myGraph, myGraph["3"]) == expectedFor3);
	BOOST_CHECK(graph::connectedComponent(myGraph, myGraph["4"]) == expectedFor3);
	BOOST_CHECK(graph::connectedComponent(myGraph, myGraph["5"]) == expectedFor5);
	BOOST_CHECK(graph::connectedComponent(myGraph, myGraph["6"]) == expectedFor5);
	BOOST_CHECK(graph::connectedComponent(myGraph, myGraph["7"]) == expectedFor7);
}

BOOST_AUTO_TEST_CASE(algorithms_minimum_spanning_tree) {
	using Graph = list::Graph<NoProperty, WeightedProperty>;

	Graph myGraph;
	for(std::string i : {"0", "1", "2", "3", "4"}) {
		myGraph.addNode(i);
	}

	myGraph.connect(myGraph["0"], myGraph["1"], {9});
	myGraph.connect(myGraph["0"], myGraph["2"], {75});

	myGraph.connect(myGraph["1"], myGraph["2"], {95});
	myGraph.connect(myGraph["1"], myGraph["3"], {19});
	myGraph.connect(myGraph["1"], myGraph["4"], {42});

	myGraph.connect(myGraph["2"], myGraph["3"], {51});

	myGraph.connect(myGraph["3"], myGraph["4"], {31});

	Graph myUndirectedGraph = graph::undirected(myGraph);

	Graph directedExpected;
	for(std::string i : {"0", "1", "2", "3", "4"}) {
		myGraph.addNode(i);
	}

	directedExpected.connect(directedExpected["0"], directedExpected["1"], {9});
	directedExpected.connect(directedExpected["1"], directedExpected["3"], {19});
	directedExpected.connect(directedExpected["3"], directedExpected["2"], {51});
	directedExpected.connect(directedExpected["3"], directedExpected["4"], {31});

	Graph expected = graph::undirected(directedExpected);

	BOOST_CHECK(graph::minimumSpanningTree(myUndirectedGraph, myUndirectedGraph["0"]) == expected);

	Graph result = graph::minimumSpanningTree(myUndirectedGraph, myUndirectedGraph["0"]);
}


/**
 * @file base.hpp
 * @author Vahid Bastani
 *
 * Base class for resampling method
 *
 * J. D. Hol, T. B. Schon and F. Gustafsson, "On Resampling Algorithms for
 * Particle Filters," Nonlinear Statistical Signal Processing Workshop, 2006
 * IEEE, Cambridge, UK, 2006, pp. 79-82
 */
#ifndef SSMPACK_FILTER_RESAMPLER_BASE
#define SSMPACK_FILTER_RESAMPLER_BASE

#include <armadillo>

namespace ssmkit {
namespace filter {
namespace resampler {

/** Base resampling class
 */
template<class T>
class BaseResampler;

template <template <class> class Method, class Criterion>
class BaseResampler<Method<Criterion>> {
  protected:
  Criterion criterion_;

  public:
   BaseResampler(Criterion criterion) : criterion_(std::move(criterion)) {}

   template <class Particles, class Weights>
   void operator()(Particles &pars, Weights &w) {
     // return if resampling criterion is false
     if (!criterion_(w))
       return;

     // generate ordered numbers
     auto u = static_cast<Method<Criterion> *>(this)
                  ->generateOrderedNumbers(w.n_rows);

     auto ws = arma::cumsum(w);
     auto u_it = u.begin();

     Particles old_pars = pars;

     pars.each_col([&u_it, &old_pars,
                    &ws](arma::Col<typename Particles::elem_type> &col) {
       col = old_pars.col(
           static_cast<arma::uvec>(arma::find(ws > *u_it++, 1, "first"))(0));
     });

     w.fill(1.0 / w.n_rows);
   }
};

} // namespace resampler
} // namespace filter
} // namespace ssmkit
#endif //SSMPACK_FILTER_RESAMPLER_IDENTITY


//
// Created by blackgeorge on 3/6/19.
//

#include <iostream>
#include <vector>
#include <boost/range/combine.hpp>
#include <boost/tuple/tuple.hpp>

#include "adj_list.h"

/*
 * Implementation of Vertex class methods
 */

void Vertex::print_adj() const {
    Vertex v;
    int w;
    std::cout << "Neighbors of " << n << ": \n";
    for (auto& i : adj) {
        v = i.first;
        w = i.second;
        std::cout << "(" << v << ", " << w << ") ";
    }
    std::cout << '\n';
}

void Vertex::add_edge(const Vertex& v, int w) {
    std::pair<Vertex, int> edge {v, w};
    adj.push_back(edge);
}

void Vertex::add_edge_u(Vertex& v, int w) {
    std::pair<Vertex, int> edge {v, w};
    adj.push_back(edge);
    v.add_edge(*this, w);
}

std::ostream& operator<<(std::ostream& os, const Vertex& u)
{
    return os << u.name();
}

/*
 * Implementation of Graph class methods
 */

void Graph::print_vertices() const {
    for (auto& i : vv) i.print_adj();
}

// TODO:
/*
void reverse_graph(Graph& g) {
    Graph rev_g;
    for (auto& u : g.vertices())
        rev_g.add_vertex(Vertex(u.name()));
}

void mult_to_undir(Graph& g) {
    auto vert_num = g.vertices().size();
    int A[vert_num];
    for (auto& u : g.vertices()) {
        for (auto i = 0; i < u.adjacents().size(); i++) {
            if (u.name() == u.adjacents()[i].first.name())
                u.adjacents().erase(u.adjacents().begin() + i);
        }
    }
}
*/

BFS_Vertex::BFS_Vertex(char c) : Vertex(c) {
    col = Color::white;
    dist = 0;
    pred = '0';
}

void BFS_Vertex::print_adj() const {
    BFS_Vertex v;
    int w;
    std::cout << "Neighbors of " << n << ": \n";
    for (auto& i : adj) {
        v = i.first;
        w = i.second;
        std::cout << "(" << v << ", " << w << ") ";
    }
    std::cout << '\n';
}

std::ostream& operator<<(std::ostream& os, const BFS_Vertex& u)
{
    os << u.name() << ", ";
    switch (u.color()) {
        case Color::white:
            os << "white";
            break;
        case Color::grey:
            os << "GREY";
            break;
        case Color::black:
            os << "BLACK";
            break;
        default:
            os << "Something's wrong";
    }
    return os << ", " << u.distance() << ", " << u.predecessor();
}


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

#include <Eigen/LU>
#include <queue>
#include <vector>

typedef Eigen::SparseMatrix<float> eigen_colmajor;
typedef Eigen::SparseMatrix<float, Eigen::RowMajor> eigen_rowmajor;
typedef std::size_t size_t;
typedef std::pair<size_t, float> score;
typedef std::pair<int, size_t> node;
typedef std::pair<node, size_t> chunk;

float combine(float x, float y, std::string combiner);
float transform(float x, std::string post_processor);

/*! Class to store and perform realtime inference in xcb mode
    for pecos xlinear model.
*/
class ModelChain {
 private:
  int size;  // meant to store the number of levels in the model chain
  std::vector<eigen_colmajor> weights;  // vector of weight matrices for routing
  std::vector<eigen_colmajor> clusters;  // vector of cluster matrices
  size_t num_labels;                     // total number of labels
  std::vector<eigen_colmajor>
      regression_weights;  // vector of regression weights

 public:
  // returns number of levels in the model chain
  int get_size() { return this->size; }
  // add elements one level at the time.
  void add_elements(eigen_colmajor weight, eigen_colmajor cluster,
                    eigen_colmajor regression_weight);
  // get the labels that are connected to the given codes for a particular level
  // in the model chain
  std::vector<score> get_labels(std::vector<score>& codes,
                                eigen_colmajor& cluster_mat);
  // get a random label in the subtree of the current node
  size_t get_random_arm(node& current_node);
  // get top scores with exploration enabled from score_list
  std::vector<score> get_top_scores(
      std::vector<std::pair<float, size_t>>& score_list, int topk, int level,
      int num_explore, float multiplier, bool explore_in_routing,
      std::string explore_strategy, float alpha);
  // get top labels given input vector and the level of the replace model
  std::vector<score> get_top_labels(
      eigen_rowmajor& in_vector, int level, std::vector<score>& labels,
      int topk, int num_explore, float multiplier, std::string post_processor,
      std::string combiner, bool explore_in_routing,
      std::string explore_strategy, std::vector<node>& rejected_nodes,
      std::vector<chunk>& selected_chunks, float alpha);
  // perform beam search given an input vector
  std::pair<std::vector<score>, std::vector<chunk>> beam_search(
      eigen_rowmajor& in_vector, int beam_size, int topk, int num_explore,
      float multiplier, std::string post_processor, std::string combiner,
      bool explore_in_routing, std::string explore_strategy, float alpha);
  // sample according to falcon distribution given a score_list
  score get_sample(std::vector<std::pair<float, size_t>>& score_list,
                   float multiplier, int level, int& num_exploit,
                   std::string explore_strategy, float alpha);
};