#include <OGRE/OgreVector3.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>

#include <ros/console.h>
#include <rviz/ogre_helpers/axes.h>
#include <rviz/ogre_helpers/shape.h>

#include <Eigen/Dense>

#include "covariance_visual.h"

template<typename Derived>
inline bool isfinite(const Eigen::MatrixBase<Derived>& x)
{
  return ( (x - x).array() == (x - x).array()).all();
}

template<typename Derived>
inline bool isnan(const Eigen::MatrixBase<Derived>& x)
{
  return !((x.array() == x.array())).all();
}

std::pair<Eigen::Matrix3d, Eigen::Vector3d> computeEigenValuesAndVectors(const geometry_msgs::PoseWithCovariance& msg,  unsigned offset)
{
  Eigen::Matrix3d covariance   = Eigen::Matrix3d::Zero();
  Eigen::Vector3d eigenValues  = Eigen::Vector3d::Identity();
  Eigen::Matrix3d eigenVectors = Eigen::Matrix3d::Zero();

  for (size_t i = 0; i < 3; ++i)
  {
    for (size_t j = 0; j < 3; ++j)
    {
      covariance (i, j) = msg.covariance[(i + offset) * 6 + j + offset];
    }
  }

  // Compute eigen values and eigen vectors.
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covariance);

  if (eigensolver.info() == Eigen::Success)
  {
    eigenValues  = eigensolver.eigenvalues();
    eigenVectors = eigensolver.eigenvectors();
  }
  else
  {
    ROS_WARN_THROTTLE(1, "Failed to compute eigen vectors/values. Is the covariance matrix correct?");
  }

  return std::make_pair(eigenVectors, eigenValues);
}

Ogre::Quaternion computeRotation(const geometry_msgs::PoseWithCovariance& msg, std::pair<Eigen::Matrix3d, Eigen::Vector3d>& pair)
{
  Ogre::Matrix3 rotation;

  for (size_t i = 0; i < 3; ++i)
  {
    for (size_t j = 0; j < 3; ++j)
    {
      rotation[i][j] = pair.first(i, j);
    }
  }

  return Ogre::Quaternion(rotation);
}

namespace rviz_plugin_covariance
{

CovarianceVisual::CovarianceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node)
  : frame_node_(parent_node->createChildSceneNode())
  , scene_manager_(scene_manager)
  , scale_(1.0)
{
  axes_.reset(new rviz::Axes(scene_manager_, frame_node_));

  position_node_    = axes_->getSceneNode()->createChildSceneNode();
  orientation_node_ = axes_->getSceneNode()->createChildSceneNode();

  position_shape_.reset(new rviz::Shape(rviz::Shape::Sphere, scene_manager_, position_node_));
  orientation_shape_.reset(new rviz::Shape(rviz::Shape::Cone, scene_manager_, orientation_node_));

  axes_->getSceneNode()->setVisible(show_axis_);
  position_node_->setVisible(show_position_);
  orientation_node_->setVisible(use_6dof_ && show_orientation_);
}

CovarianceVisual::~CovarianceVisual()
{
  scene_manager_->destroySceneNode(orientation_node_);
  scene_manager_->destroySceneNode(position_node_);
  scene_manager_->destroySceneNode(frame_node_);
}

void CovarianceVisual::setMessage(const geometry_msgs::PoseWithCovariance& msg)
{
  // Construct pose position and orientation.
  const geometry_msgs::Point& p = msg.pose.position;
  Ogre::Vector3 position(p.x, p.y, p.z);
  Ogre::Quaternion orientation(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z);

  // Set position and orientation for axes scene node.
  if (!position.isNaN())
  {
    axes_->setPosition(position);
  }
  else
  {
    ROS_WARN_STREAM_THROTTLE(1, "Position contains NaN: " << position);
  }

  if (!orientation.isNaN())
  {
    axes_->setOrientation(orientation);
  }
  else
  {
    ROS_WARN_STREAM_THROTTLE(1, "Orientation contains NaN: " << orientation);
  }

  if (use_6dof_)
  {
    // Check for NaN in covariance
    for (size_t i = 0; i < 3; ++i)
    {
      if (isnan(msg.covariance[i]))
      {
        ROS_WARN_THROTTLE(1, "Covariance contains NaN");
        return;
      }
    }

    // Compute eigen values and vectors for both shapes.
    std::pair<Eigen::Matrix3d, Eigen::Vector3d> positionEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 0));
    std::pair<Eigen::Matrix3d, Eigen::Vector3d> orientationEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 3));

    Ogre::Quaternion positionQuaternion(computeRotation(msg, positionEigenVectorsAndValues));
    Ogre::Quaternion orientationQuaternion(computeRotation(msg, orientationEigenVectorsAndValues));

    position_node_->setOrientation(positionQuaternion);
    orientation_node_->setOrientation(orientationQuaternion);

    // Compute scaling.
    Ogre::Vector3 positionScaling(
        std::sqrt(positionEigenVectorsAndValues.second[0]),
        std::sqrt(positionEigenVectorsAndValues.second[1]),
        std::sqrt(positionEigenVectorsAndValues.second[2]));
    positionScaling *= scale_;

    Ogre::Vector3 orientationScaling(
        std::sqrt(orientationEigenVectorsAndValues.second[0]),
        std::sqrt(orientationEigenVectorsAndValues.second[1]),
        std::sqrt(orientationEigenVectorsAndValues.second[2]));
    orientationScaling *= scale_;

    // Set the scaling.
    if (!positionScaling.isNaN())
    {
      position_node_->setScale(positionScaling);
    }
    else
    {
      ROS_WARN_STREAM_THROTTLE(1, "PositionScaling contains NaN: " << positionScaling);
    }

    if (!orientationScaling.isNaN())
    {
      orientation_node_->setScale(orientationScaling);
    }
    else
    {
      ROS_WARN_STREAM_THROTTLE(1, "OrientationScaling contains NaN: " << orientationScaling);
    }

    // Debugging.
    ROS_DEBUG_STREAM_THROTTLE(1,
       "Position:\n"
       << position << "\n"
       << "Positional part 3x3 eigen values:\n"
       << positionEigenVectorsAndValues.second << "\n"
       << "Positional part 3x3 eigen vectors:\n"
       << positionEigenVectorsAndValues.first << "\n"
       << "Sphere orientation:\n"
       << positionQuaternion << "\n"
       << positionQuaternion.getYaw() << "\n"
       << "Sphere scaling:\n"
       << positionScaling << "\n"
       << "Rotational part 3x3 eigen values:\n"
       << orientationEigenVectorsAndValues.second << "\n"
       << "Rotational part 3x3 eigen vectors:\n"
       << orientationEigenVectorsAndValues.first << "\n"
       << "Cone orientation:\n"
       << orientationQuaternion << "\n"
       << orientationQuaternion.getRoll() << " "
       << orientationQuaternion.getPitch() << " "
       << orientationQuaternion.getYaw() << "\n"
       << "Cone scaling:\n"
       << orientationScaling);
  }
  else // 3DOF
  {
    // Take (x, y, th) part from the covariance matrix:
    //   x y z R P Y
    // x * *       x
    // y * *       x
    // z
    // R
    // P
    // Y x x       *
    //
    // Actually, we only take the elements marked with '*', although we could
    // also take a 3x3 matrix with the '*' and 'x' elements.
    Eigen::Matrix2d cov_xy; cov_xy << msg.covariance[0], msg.covariance[1],
                                      msg.covariance[6], msg.covariance[7];
    const double cov_yaw = msg.covariance[35];

    // Check for NaN in covariance
    if (isnan(cov_xy) || isnan(cov_yaw))
    {
      ROS_WARN_STREAM_THROTTLE(1, "Covariance contains NaN: " <<
          "C_xy = " << cov_xy << ", C_yaw = " << cov_yaw);
      return;
    }

    // Compute eigen values and vectors for xy
    Eigen::Vector2d eigen_values  = Eigen::Vector2d::Identity();
    Eigen::Matrix2d eigen_vectors = Eigen::Matrix2d::Zero();

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(cov_xy);
    if (eigensolver.info() == Eigen::Success)
    {
      eigen_values  = eigensolver.eigenvalues();
      eigen_vectors = eigensolver.eigenvectors();
    }
    else
    {
      ROS_WARN_THROTTLE(1, "Failed to compute eigen values/vectors. Is the covariance matrix correct?");
    }

    // Compute ellipsoid angle, and axes
    const double yaw = atan2(eigen_vectors(1, 0), eigen_vectors(0, 0));
    const double axis_major = sqrt(eigen_values[0]);
    const double axis_minor = sqrt(eigen_values[1]);
    const double axis_yaw   = sqrt(cov_yaw);

    // Compute the ellipsoid orientation
    Ogre::Quaternion positionQuaternion;
    Ogre::Matrix3 R;
    R.FromEulerAnglesXYZ(Ogre::Radian(0.0), Ogre::Radian(0.0), Ogre::Radian(yaw));
    positionQuaternion.FromRotationMatrix(R);

    // Set the orientation
    position_node_->setOrientation(positionQuaternion);

    // Compute the ellipsoid scale
    Ogre::Vector3 positionScaling(
        std::isnormal(axis_major) ? axis_major : 0.001,
        std::isnormal(axis_minor) ? axis_minor : 0.001,
        std::isnormal(axis_yaw  ) ? axis_yaw   : 0.001);
    positionScaling *= scale_;

    // Set the scaling
    if (!positionScaling.isNaN())
    {
      position_node_->setScale(positionScaling);
    }
    else
    {
      ROS_WARN_STREAM_THROTTLE(1, "PositionScaling contains NaN: " << positionScaling);
    }

    // Debugging
    ROS_DEBUG_STREAM_THROTTLE(1,
       "Position:\n"
       << position << "\n"
       << "C_xy:\n"
       << cov_xy << "\n"
       << "C_yaw:\n"
       << cov_yaw << "\n"
       << "axis (major, minor, yaw): (" << axis_major << ", " << axis_minor << ", " << axis_yaw << ")\n"
       << "yaw: " << yaw << "\n"
       << "Positional part 2x2 eigen values:\n"
       << eigen_values << "\n"
       << "Positional part 2x2 eigen vectors:\n"
       << eigen_vectors << "\n"
       << "Sphere orientation:\n"
       << positionQuaternion << "\n"
       << positionQuaternion.getRoll() << " "
       << positionQuaternion.getPitch() << " "
       << positionQuaternion.getYaw() << "\n"
       << "Sphere scaling:\n"
       << positionScaling);
  }
}

void CovarianceVisual::setMessage(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
  setMessage(msg->pose);
}

void CovarianceVisual::setMessage(const nav_msgs::OdometryConstPtr& msg)
{
  setMessage(msg->pose);
}

void CovarianceVisual::setFramePosition(const Ogre::Vector3& position)
{
  frame_node_->setPosition(position);
}

void CovarianceVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
{
  frame_node_->setOrientation(orientation);
}

void CovarianceVisual::setColor(float r, float g, float b, float a)
{
  position_shape_->setColor(r, g, b, a);
  orientation_shape_->setColor(r, g, b, a);
}

void CovarianceVisual::setScale(float scale)
{
  scale_ = scale;
}

void CovarianceVisual::setShowAxis(bool show_axis)
{
  show_axis_ = show_axis;

  axes_->getSceneNode()->setVisible(show_axis);
  position_node_->setVisible(show_position_);
  orientation_node_->setVisible(use_6dof_ && show_orientation_);
}

void CovarianceVisual::setShowPosition(bool show_position)
{
  show_position_ = show_position;

  position_node_->setVisible(show_position_);
}

void CovarianceVisual::setShowOrientation(bool show_orientation)
{
  show_orientation_ = show_orientation;

  orientation_node_->setVisible(use_6dof_ && show_orientation_);
}

void CovarianceVisual::setUse6DOF(bool use_6dof)
{
  use_6dof_ = use_6dof;

  orientation_node_->setVisible(use_6dof && show_orientation_);
}

} // end namespace rviz_plugin_covariance


/*
 * distributions_boost.cpp
 *
 *  Created on: 7 Sep 2018
 *      Author: admin
 */
#include <Eigen/Eigen>
#include <math.h>
#include "distributions_boost.hpp"
#include <boost/random/gamma_distribution.hpp>
#include "boost/random.hpp"
#include "boost/generator_iterator.hpp"
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_01.hpp>

Distributions_boost::Distributions_boost(unsigned int pseed):seed(pseed){
    rng=boost::mt19937(pseed);
}
Distributions_boost::~Distributions_boost(){
}

double Distributions_boost::rgamma(double shape, double scale){
    boost::random::gamma_distribution<double> myGamma(shape, scale);
    boost::random::variate_generator<boost::mt19937&, boost::random::gamma_distribution<> > rand_gamma(rng, myGamma);
    return rand_gamma();
}

double Distributions_boost::unif_rng(){
    boost::random::uniform_real_distribution<double> myU(0,1);
    boost::random::variate_generator<boost::mt19937&, boost::random::uniform_real_distribution<> > real_variate_generator(rng, myU);
    return real_variate_generator();

}

Eigen::VectorXd Distributions_boost::dirichlet_rng(Eigen::VectorXd alpha) {
    int len;
    len=alpha.size();
    Eigen::VectorXd result(len);
    for(int i=0;i<len;i++)
        result[i]=rgamma(alpha[i],(double)1.0);
    result/=result.sum();
    return result;
}

double Distributions_boost::inv_gamma_rng(double shape,double scale){
    return ((double)1.0 / rgamma(shape, 1.0/scale));
}
double Distributions_boost::gamma_rng(double shape,double scale){
    return rgamma(shape, scale);
}
double Distributions_boost::inv_gamma_rate_rng(double shape,double rate){
    return (double)1.0 / gamma_rate_rng(shape, rate);
}
double Distributions_boost::gamma_rate_rng(double shape,double rate){
    return rgamma(shape,(double)1.0/rate);
}

double Distributions_boost::inv_scaled_chisq_rng(double dof,double scale){
    return inv_gamma_rng((double)0.5*dof, (double)0.5*dof*scale);
}
double Distributions_boost::norm_rng(double mean,double sigma2){
    boost::normal_distribution<double> nd(mean, std::sqrt((double)sigma2));
    boost::variate_generator<boost::mt19937&,boost::normal_distribution<> > var_nor(rng, nd);
    return var_nor();
}

inline double runif(unsigned int seed){
    boost::mt19937 rng(seed);
    static boost::uniform_01<boost::mt19937> zeroone(rng);
    return zeroone();
}

double Distributions_boost::component_probs(double b2,Eigen::VectorXd pi){
    double sum;
    double p;
    p=runif(seed);
    sum= pi[0]*exp((-0.5*b2)/(5e-2))/sqrt(5e-2)+pi[1]*exp((-0.5*b2));
    if(p<=(pi[0]*exp((-0.5*b2)/(5e-2))/sqrt(5e-2))/sum)
        return 5e-2;
    else
        return 1;
}


#pragma once

#include <iomanip>
#include <vector>
#include <cmath>

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include "Bounds.hh"
#include "../Math/math.hh"
#include "../util/assert.hh"
#include "../util/Maybe.hh"

#include "LineSegment/linesegment.hh"

namespace bold
{
  /**
   * Line, specified using radius/distance-from-origin parameters (normal form.)
   *
   * Equations defining the line are:
   *
   *   r = x*sin(theta) + y*cos(theta)
   *
   *   x = (r - y*cos(theta))/sin(theta)
   *   y = (r - x*sin(theta))/cos(theta)
   */
  class Line
  {
  // TODO rename to NormalLine2d?
  public:
    Line(double const radius, double const theta, const ushort votes = 0)
    : d_radius(radius),
      d_theta(theta)
    {
      ASSERT(theta >= 0);
      ASSERT(theta <= M_PI);
      ASSERT(!std::isnan(theta) && !std::isnan(radius) && !std::isinf(theta) && !std::isinf(radius));
    };

    double radius() const { return d_radius; }
    double theta() const { return d_theta; }
    double thetaDegrees() const { return Math::radToDeg(d_theta); }

    double gradient() const { return tanh(d_theta); }
    double yIntersection() const { return d_radius / cos(d_theta); }

    void draw(cv::Mat& mat, cv::Scalar const& color) const
    {
      Maybe<LineSegment<int,2>> line(intersectWith(Bounds2i(Eigen::Vector2i::Zero(), Eigen::Vector2i(mat.cols, mat.rows))));

      if (line.hasValue())
      {
        Eigen::Vector2i const& p1 = line->p1();
        Eigen::Vector2i const& p2 = line->p2();
        cv::line(mat, cv::Point(p1.x(), p1.y()), cv::Point(p2.x(), p2.y()), color);
      }
    }

    template<typename T>
    Maybe<LineSegment<T,2>> intersectWith(Bounds<T,2> bounds) const
    {
      ASSERT(d_theta >= 0 && d_theta <= M_PI);

      double tsin = sin(d_theta);
      double tcos = cos(d_theta);

      int minX = bounds.min().x();
      int minY = bounds.min().y();
      int maxX = bounds.max().x();
      int maxY = bounds.max().y();

      // r = x*sin(theta) + y*cos(theta)
      // x = (r - y*cos(theta))/sin(theta)
      // y = (r - x*sin(theta))/cos(theta)

      double xWhenYMin = (d_radius-minY*tcos)/tsin;
      double yWhenXMin = (d_radius-minX*tsin)/tcos;

      double xWhenYMax = (d_radius-maxY*tcos)/tsin;
      double yWhenXMax = (d_radius-maxX*tsin)/tcos;

      std::vector<Eigen::Matrix<T,2,1>> edgeContactPoints;

      if (xWhenYMin >= minX && xWhenYMin <= maxX)
        edgeContactPoints.emplace_back(xWhenYMin, minY);

      if (xWhenYMax >= minX && xWhenYMax <= maxX)
        edgeContactPoints.emplace_back(xWhenYMax, maxY);

      if (yWhenXMin >= minY && yWhenXMin <= maxY)
        edgeContactPoints.emplace_back(minX, yWhenXMin);

      if (yWhenXMax >= minY && yWhenXMax <= maxY)
        edgeContactPoints.emplace_back(maxX, yWhenXMax);

      if (edgeContactPoints.size() == 0)
        return Maybe<LineSegment<T,2>>::empty();

      ASSERT(edgeContactPoints.size() == 2);

      if (edgeContactPoints.size() != 2)
        return Maybe<LineSegment<T,2>>::empty();

      return Maybe<LineSegment<T,2>>(LineSegment<T,2>(edgeContactPoints[0], edgeContactPoints[1]));
    }

    bool operator==(Line const& other) const
    {
      const double epsilon = 0.0000001;
      return fabs(d_radius - other.d_radius) < epsilon
          && fabs(d_theta - other.d_theta) < epsilon;
    }

    template<typename T>
    static Line fromSegment(LineSegment2<T> const& segment)
    {
      double theta = atan2(segment.p2().y() - segment.p1().y(),
                           segment.p1().x() - segment.p2().x());

      double radius = segment.p1().x() * std::sin(theta) +
        segment.p1().y() * std::cos(theta);

      while (theta < 0)
      {
        theta += M_PI;
        radius = -radius;
      }

      while (theta > M_PI)
      {
        theta -= M_PI;
        radius = -radius;
      }

      return Line(radius, theta);
    }

    friend std::ostream& operator<<(std::ostream& stream, Line const& line)
    {
      return stream << std::setprecision(13) << "Line (radius=" << line.radius() << " theta=" << line.theta() << ")";
    }

  private:
    double d_radius;
    double d_theta;
  };
}


/* Boost libs/numeric/odeint/examples/openmp/lorenz_ensemble_nested.cpp

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Pascal Germroth
 Copyright 2013 Mario Mulansky

 Parallelized Lorenz ensembles using nested omp algebra

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

#include <omp.h>
#include <vector>
#include <iostream>
#include <iterator>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/openmp/openmp.hpp>
#include <boost/lexical_cast.hpp>
#include "point_type.hpp"

using namespace std;
using namespace boost::numeric::odeint;

typedef point<double, 3> point_type;
typedef vector< point_type > state_type;

const double sigma = 10.0;
const double b = 8.0 / 3.0;

struct sys_func {
    const vector<double> &R;
    sys_func( vector<double> &R ) : R(R) {}

    void operator()( const state_type &x , state_type &dxdt , double t ) const {
#       pragma omp parallel for
        for(size_t i = 0 ; i < x.size() ; i++) {
            dxdt[i][0] = -sigma * (x[i][0] - x[i][1]);
            dxdt[i][1] = R[i] * x[i][0] - x[i][1] - x[i][0] * x[i][2];
            dxdt[i][2] = -b * x[i][2] + x[i][0] * x[i][1];
        }
    }
};

int main(int argc, char **argv) {
    size_t n = 1024;
    if(argc > 1) n = boost::lexical_cast<size_t>(argv[1]);

    vector<double> R(n);
    const double Rmin = 0.1, Rmax = 50.0;
#   pragma omp parallel for
    for(size_t i = 0 ; i < n ; i++)
        R[i] = Rmin + (Rmax - Rmin) / (n - 1) * i;

    state_type state( n , point_type(10, 10, 10) );

    typedef runge_kutta4< state_type, double , state_type , double ,
                          openmp_nested_algebra<vector_space_algebra> > stepper;

    const double t_max = 10.0, dt = 0.01;

    integrate_const(
        stepper(),
        sys_func(R),
        state,
        0.0, t_max, dt
    );

    std::copy( state.begin(), state.end(), ostream_iterator<point_type>(cout, "\n") );

    return 0;
}


#pragma once

#include <armadillo>
#include <vector>
#include <tuple>

#define MAX_ITERS 100

struct KMedNode
{
	KMedNode(size_t n, size_t k) : N(n), K(k),
						medoids(K,arma::fill::zeros),
						multiplicity(K,arma::fill::zeros),
						assignments(N,arma::fill::zeros) { }
	size_t N;
	size_t K;
	arma::uvec medoids;
	arma::uvec multiplicity;
	arma::uvec assignments;
};

void
assign_to_medoids(const arma::mat& dists, KMedNode& kmn)
{
	arma::mat sub_dists = dists.rows(kmn.medoids);
	kmn.multiplicity.zeros();
	for(size_t i = 0; i < kmn.N; ++i)
	{
		arma::uvec idx = arma::find(kmn.medoids==i,1);
		if(idx.n_elem == 1)
		{
			size_t loc = idx[0];
			kmn.multiplicity[loc]++;
			kmn.assignments[i] = kmn.medoids[loc];
			continue;
		}

		size_t min_idx = sub_dists.col(i).index_min();
		kmn.assignments[i] = kmn.medoids[min_idx];
		kmn.multiplicity[min_idx]++;
	}
}

void
update_medoids(const arma::mat& dists, KMedNode& kmn)
{
	arma::uvec set;
	for(size_t k = 0; k < kmn.K; ++k)
	{
		set = arma::find(kmn.assignments == kmn.medoids[k]);
		arma::mat sub_dist = dists(set,set);
		arma::vec row_sums = arma::sum(sub_dist,1);
		size_t min_idx = row_sums.index_min();
		if(kmn.medoids[k] != set[min_idx])
			printf("Swapping %llu for %llu\n",set[min_idx],kmn.medoids[k]);
		kmn.medoids[k] = set[min_idx];
	}
}

arma::uvec
kmpp_init(const arma::mat& dists, size_t k, const arma::uvec& seeds)
{
	arma::uvec medoids(k);
	size_t start;
	printf("Initializing random center\n");
	if(seeds.n_elem > k)
		throw std::runtime_error("kmeans++ called with seed count greater than k");
	if(seeds.n_elem > 0)
	{
		for(size_t i = 0; i < seeds.n_elem; ++i)
			medoids(i) = seeds(i);
		start = seeds.n_elem;
	} else {
		// randomly initialize one value
		auto init = arma::randi<arma::uvec>(1,arma::distr_param(0,dists.n_rows-1));
		medoids(0)=init(0);
		start = 1;
	}
	for(size_t i = start; i < k; ++i)
	{
		// Dsq(x) = min_i D(x,medoid_i)^2
		// so minimize across columns to find the min value of D(x,medoids_i)
		arma::uvec assigned = medoids(arma::span(0,i-1));
		arma::vec Dsq = arma::square(arma::min(dists.cols(assigned),1));

		arma::vec cdf = arma::cumsum(Dsq/arma::accu(Dsq));
		arma::vec r = arma::randu<arma::vec>(1);
		arma::uvec loc = arma::find(cdf > r[0],1,"first");
		if(loc.is_empty())
		{
			medoids[i] = dists.n_rows-1;
		}
		else
		{
			medoids[i] = loc[0];
		}
	}

	return medoids;
}
// uses Voronoi iteration
KMedNode
select_kmedoids(const arma::mat& dists, size_t k, const arma::uvec& seeds = arma::uvec() )
{
	int N = dists.n_rows;
	KMedNode kmn(N, k);

	kmn.medoids = arma::sort(kmpp_init(dists,k,seeds));
	arma::uvec old_medoids = kmn.medoids;
	arma::uvec diff, sindex;

	bool converged = false;
	for(size_t i = 0; i < MAX_ITERS && !converged; ++i)
	{
		printf("Iteration %lu\n",i);
		assign_to_medoids(dists,kmn);
		update_medoids(dists,kmn);
		kmn.medoids = arma::sort(kmn.medoids);

		diff = arma::find(old_medoids != kmn.medoids);
		if(diff.is_empty())
			converged = true;
		else
			old_medoids = kmn.medoids;
	}

	assign_to_medoids(dists,kmn);
	return kmn;
}

std::tuple<pyarr_u,pyarr_u>
kmedoids(pyarr_d dists, size_t k, pyarr_u seeds = pyarr_u(0,nullptr))
{
	arma::arma_rng::set_seed_random();
	arma::mat d = py_to_mat(dists);
	arma::uvec seed_medoid = py_to_uvec(seeds);
	seed_medoid.print("Seed Medoids");
	KMedNode kmn = select_kmedoids(d,k, seed_medoid);
	return std::make_tuple(uvec_to_py(kmn.medoids),uvec_to_py(kmn.multiplicity));
}


#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <kv/vleq.hpp>

namespace ub = boost::numeric::ublas;
typedef kv::interval<double> itv;

int main()
{
	ub::matrix<itv> a(2, 2);
	ub::matrix<itv> b(2, 1);
	ub::matrix<itv> x(2, 1);

	a(0, 0) = 1.; a(0, 1) = 2.;
	a(1, 0) = 3.; a(1, 1) = 4.;

	b(0, 0) = 5.; b(1, 0) = 6.;

	kv::vleq(a, b, x);

	std::cout.precision(20);
	std::cout << x << "\n";
	std::cout << prod(a, x) << "\n";
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <cstdlib>
#include <iostream>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/utility/source.hpp>

namespace compute = boost::compute;

// return a random float between lo and hi
float rand_float(float lo, float hi)
{
    float x = (float) std::rand() / (float) RAND_MAX;

    return (1.0f - x) * lo + x * hi;
}

// this example demostrates a black-scholes option pricing kernel.
int main()
{
    // number of options
    const int N = 4000000;

    // black-scholes parameters
    const float risk_free_rate = 0.02f;
    const float volatility = 0.30f;

    // get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);
    std::cout << "device: " << gpu.name() << std::endl;

    // initialize option data on host
    std::vector<float> stock_price_data(N);
    std::vector<float> option_strike_data(N);
    std::vector<float> option_years_data(N);

    std::srand(5347);
    for(int i = 0; i < N; i++){
        stock_price_data[i] = rand_float(5.0f, 30.0f);
        option_strike_data[i] = rand_float(1.0f, 100.0f);
        option_years_data[i] = rand_float(0.25f, 10.0f);
    }

    // create memory buffers on the device
    compute::vector<float> call_result(N, context);
    compute::vector<float> put_result(N, context);
    compute::vector<float> stock_price(N, context);
    compute::vector<float> option_strike(N, context);
    compute::vector<float> option_years(N, context);

    // copy initial values to the device
    compute::copy_n(stock_price_data.begin(), N, stock_price.begin(), queue);
    compute::copy_n(option_strike_data.begin(), N, option_strike.begin(), queue);
    compute::copy_n(option_years_data.begin(), N, option_years.begin(), queue);

    // source code for black-scholes program
    const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        // approximation of the cumulative normal distribution function
        static float cnd(float d)
        {
            const float A1 =  0.319381530f;
            const float A2 = -0.356563782f;
            const float A3 =  1.781477937f;
            const float A4 = -1.821255978f;
            const float A5 =  1.330274429f;
            const float RSQRT2PI = 0.39894228040143267793994605993438f;

            float K = 1.0f / (1.0f + 0.2316419f * fabs(d));
            float cnd =
                RSQRT2PI * exp(-0.5f * d * d) *
                (K * (A1 + K * (A2 + K * (A3 + K * (A4 + K * A5)))));

            if(d > 0){
                cnd = 1.0f - cnd;
            }

            return cnd;
        }

        // black-scholes option pricing kernel
        __kernel void black_scholes(__global float *call_result,
                                    __global float *put_result,
                                    __global const float *stock_price,
                                    __global const float *option_strike,
                                    __global const float *option_years,
                                    float risk_free_rate,
                                    float volatility)
        {
            const uint opt = get_global_id(0);

            float S = stock_price[opt];
            float X = option_strike[opt];
            float T = option_years[opt];
            float R = risk_free_rate;
            float V = volatility;

            float sqrtT = sqrt(T);
            float d1 = (log(S / X) + (R + 0.5f * V * V) * T) / (V * sqrtT);
            float d2 = d1 - V * sqrtT;
            float CNDD1 = cnd(d1);
            float CNDD2 = cnd(d2);

            float expRT = exp(-R * T);
            call_result[opt] = S * CNDD1 - X * expRT * CNDD2;
            put_result[opt] = X * expRT * (1.0f - CNDD2) - S * (1.0f - CNDD1);
        }
    );

    // build black-scholes program
    compute::program program = compute::program::create_with_source(source, context);
    program.build();

    // setup black-scholes kernel
    compute::kernel kernel(program, "black_scholes");
    kernel.set_arg(0, call_result);
    kernel.set_arg(1, put_result);
    kernel.set_arg(2, stock_price);
    kernel.set_arg(3, option_strike);
    kernel.set_arg(4, option_years);
    kernel.set_arg(5, risk_free_rate);
    kernel.set_arg(6, volatility);

    // execute black-scholes kernel
    queue.enqueue_1d_range_kernel(kernel, 0, N, 0);

    // print out the first option's put and call prices
    float call0, put0;
    compute::copy_n(put_result.begin(), 1, &put0, queue);
    compute::copy_n(call_result.begin(), 1, &call0, queue);

    std::cout << "option 0 call price: " << call0 << std::endl;
    std::cout << "option 0 put price: " << put0 << std::endl;

    // due to the differences in the random-number generators between linux and
    // mac os x, we will get different "expected" results for this example
#ifdef __APPLE__
    double expected_call0 = 0.000249461;
    double expected_put0 = 26.2798;
#else
    double expected_call0 = 0.0999f;
    double expected_put0 = 43.0524f;
#endif

    // check option prices
    if(std::abs(call0 - expected_call0) > 1e-4 || std::abs(put0 - expected_put0) > 1e-4){
        std::cerr << "error: option prices are wrong" << std::endl;
        return -1;
    }

    return 0;
}


#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ceres/ceres.h"
#include "glog/logging.h"
#include <pangolin/pangolin.h>

using ceres::AutoDiffCostFunction;
using ceres::NumericDiffCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;
using Eigen::Isometry3d;
using Eigen::Vector3d;
using Eigen::Quaterniond;
using Eigen::Matrix3d;
using Eigen::cos;
using Eigen::sin;
using std::vector;

const double DT = 1.0 / 18;
// const Eigen::Vector3d GRAVITY{0, 0, 0};
const Eigen::Vector3d GRAVITY{0, 0, -9.8};

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
void DrawTrajectoryComparison(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>,
                              vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

struct State {
  Eigen::Vector3d pos = Eigen::Vector3d::Random(); // position
  Eigen::Vector3d vel = Eigen::Vector3d::Random(); // velocity
  Eigen::Vector3d aaxis = Eigen::Vector3d::Random(); // Lie Algebra (Angle axis)
  // Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom(); // pose Qwr

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct Measurement{
  Eigen::Matrix3d Rwr;
  Eigen::Quaterniond qwr;
  Eigen::Vector3d twr;
  Eigen::Vector3d acc;
  Eigen::Vector3d omega;

  Measurement(Eigen::Matrix3d Rwr,
              Eigen::Quaterniond qwr,
              Eigen::Vector3d twr,
              Eigen::Vector3d acc,
              Eigen::Vector3d omega)
    : Rwr(Rwr), qwr(qwr), twr(twr), acc(acc), omega(omega) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct PoseError {
  PoseError(const Eigen::Vector3d& pos_measured,
            const Eigen::Matrix3d& R_measured)
    : pos_measured_(pos_measured), R_measured_(R_measured) {}

  template <typename T>
  bool operator()(const T* const pos_hat_ptr,
                  const T* const aaxis_hat_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 6, 1> residuals;

    Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
    Eigen::Matrix<T, 3, 1> pos_delta;
    residuals.template block<3, 1>(0, 0) = pos_hat - pos_measured_.template cast<T>();

    Eigen::Matrix<T, 3, 1> axis_hat(aaxis_hat_ptr);
    Eigen::AngleAxisd aaxis_m(R_measured_);

    T angle_hat = -axis_hat.norm();
    axis_hat.normalize();
    T sin_half_hat = sin(angle_hat / T(2.0));
    T cos_half_hat = cos(angle_hat / T(2.0));

    T angle_m = T(aaxis_m.angle());
    Eigen::Matrix<T, 3, 1> axis_m = aaxis_m.axis().template cast<T>();
    T sin_half_m = sin(angle_m / T(2.0));
    T cos_half_m = cos(angle_m / T(2.0));

    T cos_half_delta = cos_half_hat * cos_half_m - sin_half_hat * sin_half_m * axis_hat.dot(axis_m);
    Eigen::Matrix<T, 3, 1> axis_delta = sin_half_hat * cos_half_m * axis_hat +
                                        cos_half_hat * sin_half_m * axis_m +
                                        sin_half_hat * sin_half_m * axis_hat.cross(axis_m);

    residuals.template block<3, 1>(3, 0) = axis_delta;
    for (int i = 0; i < 6; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& pos_measured,
                              const Eigen::Matrix3d& R_measured) {
    return new AutoDiffCostFunction<PoseError, 6, 3, 3>(
      new PoseError(pos_measured, R_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d pos_measured_;
  const Eigen::Matrix3d R_measured_;
};

struct PredictionError{
  PredictionError(const Eigen::Vector3d& acc_measured,
                  const Eigen::Vector3d& omega_measured)
    : acc_measured_(acc_measured), omega_measured_(omega_measured) {}

  template <typename T>
  bool operator()(const T* const pos_b_ptr,
                  const T* const vel_b_ptr,
                  const T* const aaxis_b_ptr,
                  const T* const pos_e_ptr,
                  const T* const vel_e_ptr,
                  const T* const aaxis_e_ptr,
                  const T* const bias_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 9, 1> residuals;

    // rot error
    Eigen::Matrix<T, 3, 1> axis_b(aaxis_b_ptr);
    Eigen::Matrix<T, 3, 1> axis_e(aaxis_e_ptr);
    Eigen::Matrix<T, 3, 1> axis_d = omega_measured_.template cast<T>() * DT;

    int choice = 1;
    if (choice == 0) {
      // Eigen::AngleAxis<T> aaxis_b(axis_b.norm(), axis_b.normalized());
      // Eigen::AngleAxis<T> aaxis_e(axis_e.norm(), axis_e.normalized());
      // Eigen::AngleAxis<T> aaxis_d(axis_d.norm(), axis_d.normalized());

      // Eigen::AngleAxis<T> aaxis_delta(aaxis_d.inverse() * aaxis_b.inverse() * aaxis_e);
      // residuals.template block<3, 1>(6, 0) = aaxis_delta.angle() * aaxis_delta.axis();
    } else if (choice == 1) {
    }
    // Reference: https://math.stackexchange.com/questions/382760/composition-of-two-axis-angle-rotations
    T angle_b = -axis_b.norm();
    axis_b.normalize();
    T sin_half_b = sin(angle_b / T(2));
    T cos_half_b = cos(angle_b / T(2));

    T angle_e = axis_e.norm();
    axis_e.normalize();
    T sin_half_e = sin(angle_e / T(2));
    T cos_half_e = cos(angle_e / T(2));

    T cos_half_be = cos_half_b * cos_half_e - sin_half_b * sin_half_e * axis_b.dot(axis_e);
    Eigen::Matrix<T, 3, 1> axis_be = sin_half_b * cos_half_e * axis_b + cos_half_b * sin_half_e * axis_e + sin_half_b * sin_half_e * axis_b.cross(axis_e);
    T sin_half_be = axis_be.norm();
    axis_be.normalize();

    T angle_d = -axis_d.norm();
    axis_d.normalize();
    T sin_half_d = sin(angle_d / T(2));
    T cos_half_d = cos(angle_d / T(2));

    T cos_half_delta = cos_half_d * cos_half_be - sin_half_d * sin_half_be * axis_d.dot(axis_be);
    Eigen::Matrix<T, 3, 1> axis_delta = sin_half_d * cos_half_be * axis_d + cos_half_d * sin_half_be * axis_be + sin_half_d * sin_half_be * axis_d.cross(axis_be);

    residuals.template block<3, 1>(6, 0) = axis_delta;

    angle_b = -angle_b;
    T sin_b = sin(angle_b);
    T cos_b = cos(angle_b);
    Eigen::Matrix<T, 3, 3> axis_b_cross = Eigen::Matrix<T, 3, 3>::Zero();
    axis_b_cross(0, 1) = -axis_b[2];
    axis_b_cross(1, 0) = axis_b[2];
    axis_b_cross(0, 2) = axis_b[1];
    axis_b_cross(2, 0) = -axis_b[1];
    axis_b_cross(1, 2) = -axis_b[0];
    axis_b_cross(2, 1) = axis_b[0];

    Eigen::Matrix<T, 3, 3> rot_b = cos_b * Eigen::Matrix<T, 3, 3>::Identity() +
                                   (T(1.0) - cos_b) * axis_b * axis_b.transpose() +
                                   sin_b * axis_b_cross;
    // vel error
    const Eigen::Matrix<T, 3, 1> bias(bias_ptr);
    const Eigen::Matrix<T, 3, 1> vel_b(vel_b_ptr);
    const Eigen::Matrix<T, 3, 1> vel_e(vel_e_ptr);
    residuals.template block<3, 1>(3, 0) = vel_b + (rot_b * (acc_measured_.template cast<T>() - bias) - GRAVITY) * DT - vel_e;

    // pos error
    const Eigen::Matrix<T, 3, 1> pos_b(pos_b_ptr);
    const Eigen::Matrix<T, 3, 1> pos_e(pos_e_ptr);
    residuals.template block<3, 1>(0, 0) = pos_b + vel_b * DT - pos_e;

    for (int i = 0; i < 9; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& acc_measured,
                              const Eigen::Vector3d& omega_measured) {
    return new AutoDiffCostFunction<PredictionError, 9, 3, 3, 3, 3, 3, 3, 3>(
      new PredictionError(acc_measured, omega_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d acc_measured_;
  const Eigen::Vector3d omega_measured_;
};

std::vector<Measurement, Eigen::aligned_allocator<Measurement>> readSensorData(std::string path) {
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> ret;

  std::ifstream csvFile;
  csvFile.open(path);

  std::string line;
  while(std::getline(csvFile, line)) {
    std::vector<double> row;
    std::cout << "line:" << line << std::endl;
    std::istringstream s(line);
    std::string field;
    while (std::getline(s, field,',')) {
      // std::cout << "field: " << field << std::endl;
      row.push_back(std::stod(field));
    }
    Eigen::Matrix3d Rwr;
    Eigen::Quaterniond qwr;
    Eigen::Vector3d twr;
    Eigen::Vector3d acc;
    Eigen::Vector3d omega;
    Rwr << row[0], row[1], row[2],
          row[4], row[5], row[6],
          row[8], row[9], row[10];
    qwr = Rwr;
    twr << row[3], row[7], row[11];
    acc << row[16], row[17], row[18];
    omega << row[19], row[20], row[21];
    std::cout << "Rwr: " << Rwr << std::endl;
    std::cout << "qwr: " << qwr.w() << " " << qwr.vec() << std::endl;
    std::cout << "twr: " << twr << std::endl;
    std::cout << "acc: " << acc << std::endl;
    std::cout << "omega: " << omega << std::endl;

    ret.push_back(Measurement(Rwr, qwr, twr, acc, omega));
  }

  return ret;
}

double abs_pos_error(const std::vector<State, Eigen::aligned_allocator<State>>& states,
                     const std::vector<State, Eigen::aligned_allocator<State>>& gt_states) {
  double err = 0.0;
  std::cout << "abs_pos_err by step: ";
  for (int i = 0; i < states.size(); i++) {
    double step_err = (states[i].pos - gt_states[i].pos).norm();
    err += step_err;
    std::cout << step_err << " ";
  }
  std::cout << std::endl;
  return err;
}

int main(int argc, char** argv) {
  if(argc < 2) {
    std::cout << "missing arg for the csv file" << std::endl;
  }

  std::string path = argv[1];
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> data = readSensorData(path);

  int cnt = data.size();
  // int cnt = 3;

  // Eigen::Vector3d bias = Eigen::Vector3d::Random();
  Eigen::Vector3d bias;
  bias << 0, 0, 0;
  std::vector<State, Eigen::aligned_allocator<State>> states(cnt);
  std::vector<State, Eigen::aligned_allocator<State>> gt_states(cnt);
  std::cout << "states size: " << states.size() << std::endl;

  for (int i = 0; i < gt_states.size(); i++) {
    gt_states[i].pos = data[i].twr;
    gt_states[i].vel = Eigen::Vector3d::Zero();
    Eigen::AngleAxisd temp(data[i].Rwr);
    gt_states[i].aaxis = temp.angle() * temp.axis();
  }

  Problem problem;

  ceres::LossFunction* loss_function = nullptr;

  for (int i = 0; i < cnt; i++) {
    ceres::CostFunction* pos_cost_function = PoseError::Create(data[i].twr, data[i].Rwr);
    problem.AddResidualBlock(pos_cost_function,
                             loss_function,
                             states[i].pos.data(),
                             states[i].aaxis.data());
    if (i > 0) {
      ceres::CostFunction* pred_cost_function = PredictionError::Create(data[i].acc, data[i].omega);
      problem.AddResidualBlock(pred_cost_function,
                               loss_function,
                               states[i - 1].pos.data(),
                               states[i - 1].vel.data(),
                               states[i - 1].aaxis.data(),
                               states[i].pos.data(),
                               states[i].vel.data(),
                               states[i].aaxis.data(),
                               bias.data());
    }
  }

  ceres::Solver::Options options;
	options.max_num_iterations = 200;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> gt_poses;
  for (int i = 0; i < cnt; i++) {
    Eigen::AngleAxisd temp(states[i].aaxis.norm(), states[i].aaxis.normalized());
    Isometry3d Twr(temp.matrix());
    Twr.pretranslate(states[i].pos / 20); // manually divided by 20 to zoom out
    poses.push_back(Twr);
  }
  for (int i = 0; i < cnt; i++) {
    Eigen::AngleAxisd temp(gt_states[i].aaxis.norm(), gt_states[i].aaxis.normalized());
    Isometry3d Twr(temp.matrix());
    Twr.pretranslate(gt_states[i].pos / 20); // manually divided by 20 to zoom out
    gt_poses.push_back(Twr);
  }

  double err = abs_pos_error(states, gt_states);
  std::cout << "absolute position error: " << err << std::endl;
  DrawTrajectoryComparison(poses, gt_poses);
  return 0;
}

void DrawTrajectoryComparison(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses,
                              vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> gt_poses) {
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(1.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < gt_poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = gt_poses[i].translation();
      Vector3d Xw = gt_poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = gt_poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = gt_poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < gt_poses.size(); i++) {
      glColor3f(0.0, 1.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = gt_poses[i], p2 = gt_poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}


#include <mona/utility.hpp>
#include <mona/line.hpp>
#include <mona/targets/window.hpp>
#include <mona/axes.hpp>
#include <mona/axes3.hpp>
#include <mona/colors.hpp>
#include <mona/grid.hpp>
#include <mona/surface_mesh.hpp>
#include <mona/camera_steady_control.hpp>

#include <armadillo>

auto fit(const arma::fvec& x, const arma::fvec& y) -> arma::fvec
{
    auto xx = x - arma::mean(x);
    auto yy = y - arma::mean(y);
    float b = arma::sum(xx % yy) / arma::sum(xx % xx);
    float a = arma::mean(y) - b * arma::mean(x);
    return a + b * x;
}

auto random(arma::fvec x, float mean, float std)
{
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> dis(mean, std);

    for(auto& e: x)
    {
        e += dis(gen);
    }

    return x;
}

auto get_dots()
{
    arma::fvec x = mona::linspace(-4, 4, 30);
    arma::fvec y = random(x, 0, 0.9);
    return mona::dots(x, y, mona::colors::tomato);
}

auto get_line()
{
    auto f = [](double x)
    {
        return std::sin(x);
    };

    arma::fvec x = mona::linspace(-4, 4, 50);
    arma::fvec y = x;
    y.transform(f);

    return mona::line(x, y, mona::colors::midnight_blue);
}

auto f1(double x, double y)
{
    return (7 * x * y) / std::exp(x*x + y*y);
};

auto f2(double x, double y)
{
    return std::pow(x, 4) + std::pow(y, 4) - std::pow(x + y, 2);
};

template <typename F>
auto get_surface(F f)
{
    arma::fvec line = mona::linspace(-2, 2, 50);
    auto [x, y] = mona::meshgrid(line, line);
    auto z = mona::apply(f, x, y);

    auto mesh = mona::surface_mesh(x, y, z);

    return mesh;
}

int main()
{
    auto win  = mona::targets::window();
    auto view = mona::grid(2, 2);
    auto cam  = mona::camera();
    auto axes_dots = mona::axes();
    auto axes_line = mona::axes();
    auto axes_surf1 = mona::axes3({-4, 4}, {-4, 4}, {-4, 4}, 5);
    axes_surf1.set_camera_control(win.get_camera_control());

    auto axes_surf2 = mona::axes3({-4, 4}, {-4, 4}, {-4, 4}, 5);
    axes_surf2.set_camera_control(mona::camera_steady_control(0.2f, 0.f));

    auto dots = get_dots();
    auto line = get_line();
    auto surf1 = get_surface(f1);
    auto surf2 = get_surface(f2);

    while (win.active())
    {
        axes_dots.submit(dots);
        axes_line.submit(line);
        axes_surf1.submit(surf1);
        axes_surf2.submit(surf2);

        view(0, 0).submit(axes_dots);
        view(0, 1).submit(axes_surf1);
        view(1, 0).submit(axes_surf2);
        view(1, 1).submit(axes_line);

        win.submit(view);
        win.draw();
    }
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015 - 2016.
// 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)
//

// fixed_point_detail_constants.hpp implements templates
// for computing fixed-point representations of the
// mathematical constants sqrt(2), pi, log(2) and e.

#ifndef FIXED_POINT_DETAIL_CONSTANTS_2015_08_16_HPP_
  #define FIXED_POINT_DETAIL_CONSTANTS_2015_08_16_HPP_

  #include <cmath>
  #include <limits>

  #include <boost/config.hpp>

  namespace boost { namespace fixed_point { namespace detail {

  template<typename NumericType>
  NumericType calculate_root_two()
  {
    // Provide a very rough initial guess of 4/3, which is about
    // 5 percent off of the actual value. Use small integers here
    // because the underlying NumericType might have limited range.
    NumericType a(NumericType(4U) / 3U);

    // Estimate the zero'th term of the iteration with [1 / (2 * result)],
    // giving [1 / (2 * (4/3))] = 3/8.
    using std::ldexp;
    NumericType vi(ldexp(NumericType(3U), -3));

    // Compute the square root of x using coupled Newton iteration.
    // We begin with an estimate of 1 binary digit of precision and
    // double the number of binary digits of precision with each iteration.

    for(std::uint_fast16_t i = UINT16_C(1); i <= std::uint_fast16_t(std::numeric_limits<NumericType>::digits); i *= UINT16_C(2))
    {
      // Perform the next iteration of vi.
      vi += vi * (-((a * vi) * 2U) + NumericType(1U));

      // Perform the next iteration of the result.
      a += (vi * (-((a) * (a)) + NumericType(2U)));
    }

    return a;
  }

  template<typename NumericType>
  NumericType calculate_pi()
  {
    using std::fabs;
    using std::ldexp;
    using std::sqrt;

    // Use a quadratically converging Gauss-AGM method to compute pi.

    NumericType val_pi;

    NumericType a (1U);
    NumericType bB(ldexp(NumericType(1U), -1));
    NumericType s (bB);
    NumericType t (ldexp(NumericType(3U), -3));

    // This loop is designed for computing a maximum of a few million
    // decimal digits of pi. The number of digits roughly doubles
    // with each iteration of the loop. After 20 iterations,
    // the precision is about 2.8 million decimal digits.
    // We are, however, not using that many digits in this
    // application --- rarely more than a few thousand at most.

    const NumericType tolerance = ldexp(NumericType(1U), -int((long(std::numeric_limits<NumericType>::digits) * 3L) / 4L));

    for(std::uint_least8_t k = UINT8_C(1); k < UINT8_C(32); ++k)
    {
      // Perform the iteration steps of the Gauss AGM.

      a      += sqrt(bB);
      a      /= 2U;
      val_pi  = (a * a);
      bB      = (val_pi - t) * 2U;

      const NumericType iterate_term((bB - val_pi) * (UINT32_C(1) << k));

      s += iterate_term;

      const bool minimum_number_of_iterations_is_complete = (k > UINT8_C(4));

      if((minimum_number_of_iterations_is_complete) && (fabs(iterate_term) <= tolerance))
      {
        break;
      }

      t = (val_pi + bB) / 4U;
    }

    val_pi += bB;
    val_pi /= s;

    return val_pi;
  }

  template<typename NumericType>
  NumericType calculate_ln_two()
  {
    using std::fabs;
    using std::ldexp;
    using std::sqrt;

    // Use a quadratically converging Gauss-AGM method for computing log(2).

    // Choose m > (N * 1.661), where N is the number of decimal digits requested.
    BOOST_CONSTEXPR_OR_CONST int m((long(std::numeric_limits<NumericType>::digits10) * 17L) / 10L);

    // Set a0 = 1.
    // Set b0 = 4 / (2^m) = 1 / 2^(m - 2).
    NumericType ak(1);

    NumericType bk(ldexp(NumericType(1), -(m - 2)));

    const NumericType tolerance = ldexp(NumericType(1U), -int((long(std::numeric_limits<NumericType>::digits) * 3L) / 4L));

    for(std::uint_least8_t k = UINT8_C(0); k < UINT8_C(32); ++k)
    {
      const NumericType a(ak);

      ak += bk;
      ak /= 2U;

      bk = sqrt(bk * a);

      const bool minimum_number_of_iterations_is_complete = (k > UINT8_C(4));

      if((minimum_number_of_iterations_is_complete) && (fabs(ak - bk) <= tolerance))
      {
        break;
      }
    }

    // The iteration is now finished. Compute ln2 = pi / [AGM(1, 4 / 2^m) * 2m].
    // Note that the result of the AGM iteration is [ak = bk = AGM(...)].
    const NumericType val_ln_two = calculate_pi<NumericType>() / (ak * (2U * m));

    return val_ln_two;
  }

  template<typename NumericType>
  NumericType calculate_e()
  {
    using std::fabs;

    NumericType term(1U);
    NumericType sum (2U);

    BOOST_CONSTEXPR_OR_CONST std::uint32_t maximum_number_of_iterations = UINT32_C(10000);

    // Perform the Taylor series expansion of Euler's constant, e = exp(1).
    for(std::uint32_t n = UINT32_C(2); n < maximum_number_of_iterations; ++n)
    {
      term /= n;

      const bool minimum_number_of_iterations_is_complete = (n > UINT32_C(4));

      if(   (minimum_number_of_iterations_is_complete)
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      sum += term;
    }

    return sum;
  }
  } } } // namespace boost::fixed_point::detail

#endif // FIXED_POINT_DETAIL_CONSTANTS_2015_08_16_HPP_


#include <Eigen/Dense>
#include <iostream>

#include <base/init.hpp>
#include "fft/shift.hpp"

using namespace std;

typedef Eigen::ArrayXXd array_t;

typedef Eigen::ArrayXd array1_t;

void test_rowwise()
{
  int n = 6;
  array1_t x = array1_t::LinSpaced(n, -n / 2, n / 2 - (n + 1) % 2);
  //  cout << x << "\n";
  array_t X(n, 10);
  X.colwise() = x;
  // --------------------------------------------------
  // test rowwise shift
  array1_t y = array1_t::LinSpaced(n, -n / 2, n / 2 - (n + 1) % 2);

  array_t Y(10, n);
  Y.rowwise() = y.transpose();

  cout << "---------- Y ----------"
       << "\n";
  cout << Y << "\n";

  cout << "---------- ifftshift(Y) ----------"
       << "\n";
  array_t iY = array_t::Zero(Y.rows(), Y.cols());
  ifftshift(iY, Y, 1);
  cout << iY << "\n\n";

  cout << "---------- fftshift(ifftshift(Y))  ----------"
       << "\n";
  array_t Y2 = array_t::Zero(Y.rows(), Y.cols());
  fftshift(Y2, iY, 1);
  cout << Y2 << "\n\n";
}

void test_colwise()
{
  int n = 6;
  array1_t x = array1_t::LinSpaced(n, -n / 2, n / 2 - (n + 1) % 2);
  cout << x << "\n";
  array_t X(n, 10);
  X.colwise() = x;
  // --------------------------------------------------
  // test colwise shift
  cout << "---------- X ----------"
       << "\n";
  cout << X << "\n";

  cout << "---------- ifftshift(X) ----------"
       << "\n";
  array_t iX = array_t::Zero(X.rows(), X.cols());
  ifftshift(iX, X, 0);
  cout << iX << "\n\n";

  cout << "---------- fftshift(ifftshift(X))  ----------"
       << "\n";
  array_t X2 = array_t::Zero(X.rows(), X.cols());
  fftshift(X2, iX, 0);
  cout << X2 << "\n\n";
}

void test2d()
{
  // test shift in both directions
  int nx = 6;
  int ny = 4;

  array_t XX(ny, nx);
  XX.setZero();
  int posx_max = nx / 2;
  int posy_max = ny / 2;

  XX.block(0, 0, posy_max, posx_max) = array_t::Ones(posy_max, posx_max);
  XX.block(posy_max, 0, ny - posy_max, nx) = 2 * array_t::Ones(ny - posy_max, nx);
  XX.block(0, posx_max, posy_max, nx - posx_max) = 3 * array_t::Ones(posy_max, nx - posx_max);
  XX.block(posy_max, posx_max, ny - posy_max, nx - posx_max) =
      4 * array_t::Ones(ny - posy_max, nx - posx_max);

  cout << "---------- XX ----------"
       << "\n";
  cout << XX << endl;

  cout << "---------- fftshift(XX) ----------"
       << "\n";
  array_t YY(ny, nx);
  YY.setZero();
  fftshift(YY, XX);
  cout << YY << "\n";

  cout << "---------- ifftshift(fftshift(XX)) ----------"
       << "\n";
  array_t XX2(ny, nx);
  XX2.setZero();
  ifftshift(XX2, YY);
  cout << XX2 << "\n";
}

void test2d_v2()
{
  // test shift in both directions
  int nx = 6;
  int ny = 4;

  array_t XX(ny, nx);
  array_t YY(ny, nx);
  XX.setZero();
  YY.setZero();
  int posx_max = nx / 2;
  int posy_max = ny / 2;

  XX = Eigen::ArrayXd::LinSpaced(nx, -posx_max, posx_max - 1).transpose().replicate(ny, 1);
  YY = Eigen::ArrayXd::LinSpaced(ny, -posy_max, posy_max - 1).replicate(1, nx);

  cout << "---------- ifftshift(fftshift(XX)) ----------"
       << "\n";
  array_t XXt(ny, nx);
  XXt.setZero();
  fftshift(XXt, XX);
  array_t XX2(ny, nx);
  ifftshift(XX2, XXt);
  {
    double diff = (XX2 - XX).abs().sum();
    cout << "diff= " << diff << endl;
  }

  cout << "---------- ifftshift(fftshift(YY)) ----------"
       << "\n";
  array_t YYt(ny, nx);
  YYt.setZero();
  fftshift(YYt, YY);
  array_t YY2(ny, nx);
  ifftshift(YY2, YYt);
  {
    double diff = (YY2 - YY).abs().sum();
    cout << "diff= " << diff << endl;
  }
}

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

  cout << "---------- test rowwise ----------"
       << "\n";
  test_rowwise();

  cout << "---------- test colwise ----------"
       << "\n";
  test_colwise();

  cout << "---------- test 2d ----------"
       << "\n";
  test2d_v2();

  return 0;
}


#include <Eigen/Eigen>
#include <iostream>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix2i m = Matrix2i::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is the transpose of m:" << endl << m.transpose() << endl;
cout << "Here is the coefficient (1,0) in the transpose of m:" << endl
     << m.transpose()(1,0) << endl;
cout << "Let us overwrite this coefficient with the value 0." << endl;
m.transpose()(1,0) = 0;
cout << "Now the matrix m is:" << endl << m << endl;

  return 0;
}


/*=============================================================================

MPHYG0022CW1: CW1, 2019: Linear Regression.

Copyright (c) University College London (UCL). All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.

See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "mphyNormalEquationSolverStrategy.h"
#include "vectorPairTypes.h"
#include <memory>
#include <iostream>
#include <math.h>
#include <Eigen/StdVector>
#include <Eigen/Dense>
#include <map>

namespace mphy {

pairdd normSolver::FitData(vecPairdd data){
	/*
	Finds optimal theta using the Normal Equation.
	theta_best = inv(X^T . X) . X^T . y, where:

	data:		a vector of pairs (x, y)
	X:			a 2xnumData matrix of (1, x0; 1, x1; 1, x2...)
	 */

	Eigen::Matrix<double, Eigen::Dynamic, 2> xdata;
	Eigen::VectorXd ydata, theta_best;

	xdata = copyXtoEigen(data);
	ydata = copyYtoEigen(data);

	theta_best = ((xdata.transpose()*xdata).inverse())*xdata.transpose()*ydata;

	return pairdd(theta_best(0), theta_best(1));
}

} // end namespace mphy


// Eigen lib tutorial -- Array class and coefficient-wise operations
// Source:
// http://eigen.tuxfamily.org/dox/group__TutorialArrayClass.html
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    // Accessing values inside an Array
    ArrayXXf m(2,2);

    // assign some value coefficient by coefficient
    m(0,0) = 1.0; m(0,1) = 2.0;
    m(1,0) = 3.0; m(1,1) = m(0,1) + m(1,0);

    // print values to standard output
    std::cout << m << '\n' << std::endl;

    // using the comma-initializer is also allowed
    m << 1.0, 3.0,
         5.0, 7.0;
    std::cout << m  << std::endl;

    // Array addition and subtraction
    ArrayXXf a(3,3);
    ArrayXXf b(3,3);
    a << 1,2,3,
         4,5,6,
         7,8,9;
    b << 1,2,3,
         1,2,3,
         1,2,3;

    // Adding two arrays
    std::cout << "a + b = " << '\n' << a + b << std::endl;

    // Subtracting a scalar from an array
    std::cout << "a - 2 = " << '\n' << a -2 << std::endl;

    // Array multiplication
    ArrayXXf c(2,2);
    ArrayXXf d(2,2);
    c << 1,2,
         3,4;
    d << 5,6,
         7,8;
    std::cout << "c * d = " << '\n' << c * d << std::endl;

    // Converting b/w array and matrix expressions
    MatrixXf u(2,2);
    MatrixXf v(2,2);
    MatrixXf result(2,2);

    u << 1,2,
         3,4;
    v << 5,6,
         7,8;

    result = u * v;
    std::cout << "-- Matrix u * v: --\n" << result << '\n' << std::endl;

    result = u.array() * v.array();
    std::cout << "-- Array u * v: --\n" << result << '\n' << std::endl;

    result = u.cwiseProduct(v);
    std::cout << "-- With cwiseProduct: --\n" << result << '\n' << std::endl;

    result = u.array() + 4;
    std::cout << "-- Array u + 4: --\n" << result << '\n' << std::endl;

    result = (u.array() + 4).matrix() * u;
    std::cout << "-- Combination 1: --" << std::endl << result << std::endl << std::endl;
    result = (u.array() * v.array()).matrix() * u;
    std::cout << "-- Combination 2: --" << std::endl << result << std::endl << std::endl;

    return 0;
}


#ifdef SPARSE
#include "fast_mass_springs_precomputation_sparse.h"
#include "fast_mass_springs_step_sparse.h"
#else
#include "fast_mass_springs_precomputation_dense.h"
#include "fast_mass_springs_step_dense.h"
#endif
#include "read_json.h"
#include <igl/get_seconds.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Sparse>

int main(int argc, char * argv[])
{
  Eigen::MatrixXd V,VT;
  Eigen::MatrixXi F,FT,E;
  double k;
  Eigen::VectorXd m;
  Eigen::VectorXi b;
  Eigen::MatrixXd bc;
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> R,G,B;
  if(!read_json(argv[1],V,F,E,k,m,b,bc,VT,FT,R,G,B))
  {
    std::cerr<<"Error: Failed to load "<<argv[1]<<std::endl;
    return EXIT_FAILURE;
  }

  const double delta_t = 1.0/30.0;
  Eigen::VectorXd r;
#ifdef SPARSE
  Eigen::SparseMatrix<double> M,A,C;
  Eigen::SimplicialLLT <Eigen::SparseMatrix<double > > prefactorization;
  if(!fast_mass_springs_precomputation_sparse(
    V,E,k,m,b,delta_t,r,M,A,C,prefactorization))
  {
    std::cerr<<"precomputation failed."<<std::endl;
  }
#else
  Eigen::MatrixXd M,A,C;
  Eigen::LLT<Eigen::MatrixXd> prefactorization;
  if(!fast_mass_springs_precomputation_dense(
    V,E,k,m,b,delta_t,r,M,A,C,prefactorization))
  {
    std::cerr<<"precomputation failed."<<std::endl;
  }
#endif

  // Point colors
  Eigen::MatrixXd CM = Eigen::MatrixXd::Zero(V.rows(),V.cols());
  const Eigen::RowVector3d silver(0.8,0.8,0.8);
  for(int i = 0;i<b.rows();i++) { CM.row(b(i)) = silver; }

  int graph_id,mesh_id;
  igl::opengl::glfw::Viewer viewer;
  std::cout<<R"(
  [space]  (turn off animation and) step forward one time step
  A,a  toggle animation
  L,l  show/hide edges (springs) and verties (point masses)
  R,r  reset to rest configuration
  W,w  toggle horizontal wind
)";

  // Initial conditions
  const Eigen::MatrixXd U0 = V;
  const Eigen::MatrixXd Udot0 = Eigen::MatrixXd::Zero(V.rows(),V.cols());

  Eigen::MatrixXd U = U0;
  Eigen::MatrixXd Uprev = U0 - delta_t*Udot0;

  const auto update = [&]()
  {
    viewer.data_list[mesh_id].set_vertices(U);
    viewer.data_list[mesh_id].compute_normals();
    viewer.data_list[graph_id].set_points(U,CM);
    viewer.data_list[graph_id].set_edges(U,E,Eigen::RowVector3d(0,0,0));
  };
  bool first = true;
  bool wind = false;
  const double bbd = (V.colwise().maxCoeff()-V.colwise().minCoeff()).norm();
  const auto step = [&]()
  {
    Eigen::MatrixXd fext = Eigen::MatrixXd::Zero(V.rows(),V.cols());
    // gravity
    fext.col(1).setConstant(-9.8);
    if(wind)
    {
      for(int i = 0;i<V.rows();i++)
      {
        const double x = U(i,0)/bbd;
        const double phi = x+igl::get_seconds();
        fext(i,0) = 3.0+3.0*0.5*(1.0+cos(5.*phi));
        fext(i,2) = 2.0*0.5*(sin(phi));
        fext(i,1) += 1.0*(1.0+cos(0.3+phi));
      }
    }
    const Eigen::MatrixXd U_stash = U;
#ifdef SPARSE
    fast_mass_springs_step_sparse(
        V,E,k,b,delta_t,fext,
        r,M,A,C,prefactorization,
        Uprev,U_stash,
        U);
#else
    fast_mass_springs_step_dense(
        V,E,k,b,delta_t,fext,
        r,M,A,C,prefactorization,
        Uprev,U_stash,
        U);
#endif
    Uprev = U_stash;
    update();
  };
  viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer &)->bool
  {
    if(viewer.core().is_animating)
    {
      if(!first)
      {
        step();
      }
      first = false;
    }
    return false;
  };

  viewer.callback_key_pressed = [&](
    igl::opengl::glfw::Viewer & v,
    unsigned char key,
    int /*modifier*/
    )->bool
  {
    switch(key)
    {
      default:
        return false;
      case ' ':
        viewer.core().is_animating = false;
        step();
        return true;
      case 'L':
      case 'l':
        viewer.data_list[0].is_visible ^= 1;
        return true;
      case 'W':
      case 'w':
        wind ^= 1;
        return true;
      case 'R':
      case 'r':
        first = true;
        U = U0;
        Uprev = U0 - delta_t*Udot0;
        update();
        return true;
    }
  };

  graph_id = viewer.selected_data_index;
  viewer.data_list[graph_id].set_points(V,CM);
  viewer.data_list[graph_id].set_edges(V,E,Eigen::RowVector3d(0,0,0));
  viewer.append_mesh();
  mesh_id = viewer.selected_data_index;

  viewer.data_list[mesh_id].set_mesh(V,F);
  viewer.data_list[mesh_id].set_face_based(true);
  if(R.size() > 0 && VT.rows()>0 && FT.rows() >0)
  {
    viewer.data_list[mesh_id].set_uv(VT,FT);
    viewer.data_list[mesh_id].set_texture(R,G,B);
    viewer.data_list[mesh_id].set_colors(Eigen::RowVector3d(1,1,1));
    viewer.data_list[mesh_id].show_texture = true;
    viewer.data_list[mesh_id].set_face_based(false);
  }
  viewer.data_list[mesh_id].show_lines = false;
  viewer.core().is_animating = false;

  viewer.launch_init(true,false);
  viewer.data().meshgl.init();
  // Ooof. All this to turn on double-sided lighting...
  igl::opengl::destroy_shader_program(viewer.data().meshgl.shader_mesh);
  {
    std::string mesh_vertex_shader_string =
R"(#version 150
  uniform mat4 view;
  uniform mat4 proj;
  uniform mat4 normal_matrix;
  in vec3 position;
  in vec3 normal;
  out vec3 position_eye;
  out vec3 normal_eye;
  in vec4 Ka;
  in vec4 Kd;
  in vec4 Ks;
  in vec2 texcoord;
  out vec2 texcoordi;
  out vec4 Kai;
  out vec4 Kdi;
  out vec4 Ksi;

  void main()
  {
    position_eye = vec3 (view * vec4 (position, 1.0));
    normal_eye = vec3 (normal_matrix * vec4 (normal, 0.0));
    normal_eye = normalize(normal_eye);
    gl_Position = proj * vec4 (position_eye, 1.0); //proj * view * vec4(position, 1.0);"
    Kai = Ka;
    Kdi = Kd;
    Ksi = Ks;
    texcoordi = texcoord;
  }
)";

    std::string mesh_fragment_shader_string =
R"(#version 150
  uniform mat4 view;
  uniform mat4 proj;
  uniform vec4 fixed_color;
  in vec3 position_eye;
  in vec3 normal_eye;
  uniform vec3 light_position_eye;
  vec3 Ls = vec3 (1, 1, 1);
  vec3 Ld = vec3 (1, 1, 1);
  vec3 La = vec3 (1, 1, 1);
  in vec4 Ksi;
  in vec4 Kdi;
  in vec4 Kai;
  in vec2 texcoordi;
  uniform sampler2D tex;
  uniform float specular_exponent;
  uniform float lighting_factor;
  uniform float texture_factor;
  out vec4 outColor;
  void main()
  {
    vec3 Ia = La * vec3(Kai);    // ambient intensity

    vec3 vector_to_light_eye = light_position_eye - position_eye;
    vec3 direction_to_light_eye = normalize (vector_to_light_eye);
    float dot_prod = dot (direction_to_light_eye, normalize(normal_eye));
    float clamped_dot_prod = abs(dot_prod);
    vec3 Id = Ld * vec3(Kdi) * clamped_dot_prod;    // Diffuse intensity

    vec3 reflection_eye = reflect (-direction_to_light_eye, normalize(normal_eye));
    vec3 surface_to_viewer_eye = normalize (-position_eye);
    float dot_prod_specular = dot (reflection_eye, surface_to_viewer_eye);
    dot_prod_specular = float(abs(dot_prod)==dot_prod) * max (dot_prod_specular, 0.0);
    float specular_factor = pow (dot_prod_specular, specular_exponent);
    vec3 Kfi = 0.5*vec3(Ksi);
    vec3 Lf = Ls;
    float fresnel_exponent = 2*specular_exponent;
    float fresnel_factor = 0;
    {
      float NE = max( 0., dot( normalize(normal_eye), surface_to_viewer_eye));
      fresnel_factor = pow (max(sqrt(1. - NE*NE),0.0), fresnel_exponent);
    }
    vec3 Is = Ls * vec3(Ksi) * specular_factor;    // specular intensity
    vec3 If = Lf * vec3(Kfi) * fresnel_factor;     // fresnel intensity
    vec4 color = vec4(lighting_factor * (If + Is + Id) + Ia + (1.0-lighting_factor) * vec3(Kdi),(Kai.a+Ksi.a+Kdi.a)/3);
    outColor = mix(vec4(1,1,1,1), texture(tex, texcoordi), texture_factor) * color;
    if (fixed_color != vec4(0.0)) outColor = fixed_color;
  }
)";

    igl::opengl::create_shader_program(
      mesh_vertex_shader_string,
      mesh_fragment_shader_string,
      {},
      viewer.data().meshgl.shader_mesh);
  }
  viewer.launch_rendering(true);
  viewer.launch_shut();
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testGaussianISAM.cpp
 * @brief   Unit tests for GaussianISAM
 * @author  Michael Kaess
 */

#include <tests/smallExample.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/geometry/Rot2.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

using namespace std;
using namespace gtsam;
using namespace example;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests

static double sigmax1 = 0.786153, /*sigmax2 = 1.0/1.47292,*/ sigmax3 = 0.671512, sigmax4 =
    0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1;

static const double tol = 1e-4;

/* ************************************************************************* *
 Bayes tree for smoother with "natural" ordering:
C1 x6 x7
C2   x5 : x6
C3     x4 : x5
C4       x3 : x4
C5         x2 : x3
C6           x1 : x2
**************************************************************************** */
TEST( GaussianBayesTree, linear_smoother_shortcuts )
{
  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7);

  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal();

  // Create the Bayes tree
  LONGS_EQUAL(6, (long)bayesTree.size());

  // Check the conditional P(Root|Root)
  GaussianBayesNet empty;
  GaussianBayesTree::sharedClique R = bayesTree.roots().front();
  GaussianBayesNet actual1 = R->shortcut(R);
  EXPECT(assert_equal(empty,actual1,tol));

  // Check the conditional P(C2|Root)
  GaussianBayesTree::sharedClique C2 = bayesTree[X(5)];
  GaussianBayesNet actual2 = C2->shortcut(R);
  EXPECT(assert_equal(empty,actual2,tol));

  // Check the conditional P(C3|Root)
  double sigma3 = 0.61808;
  Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished();
  GaussianBayesNet expected3;
  expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3);
  GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
  GaussianBayesNet actual3 = C3->shortcut(R);
  EXPECT(assert_equal(expected3,actual3,tol));

  // Check the conditional P(C4|Root)
  double sigma4 = 0.661968;
  Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished();
  GaussianBayesNet expected4;
  expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4);
  GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
  GaussianBayesNet actual4 = C4->shortcut(R);
  EXPECT(assert_equal(expected4,actual4,tol));
}

/* ************************************************************************* *
 Bayes tree for smoother with "nested dissection" ordering:

   Node[x1] P(x1 | x2)
   Node[x3] P(x3 | x2 x4)
   Node[x5] P(x5 | x4 x6)
   Node[x7] P(x7 | x6)
   Node[x2] P(x2 | x4)
   Node[x6] P(x6 | x4)
   Node[x4] P(x4)

 becomes

   C1     x5 x6 x4
   C2      x3 x2 : x4
   C3        x1 : x2
   C4      x7 : x6

************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_marginals )
{
  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7);

  // Create the Bayes tree
  Ordering ordering;
  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);

  VectorValues actualSolution = bayesTree.optimize();
  VectorValues expectedSolution = VectorValues::Zero(actualSolution);
  EXPECT(assert_equal(expectedSolution,actualSolution,tol));

  LONGS_EQUAL(4, (long)bayesTree.size());

  double tol=1e-5;

  // Check marginal on x1
  JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
  JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
  Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
  Matrix actualCovarianceX1;
  GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
  actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
  EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
  EXPECT(assert_equal(expected1,actual1,tol));

  // Check marginal on x2
  double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
  JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
  JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
  EXPECT(assert_equal(expected2,actual2,tol));

  // Check marginal on x3
  JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
  JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
  EXPECT(assert_equal(expected3,actual3,tol));

  // Check marginal on x4
  JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
  JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
  EXPECT(assert_equal(expected4,actual4,tol));

  // Check marginal on x7 (should be equal to x1)
  JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
  JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
  EXPECT(assert_equal(expected7,actual7,tol));
}

/* ************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_shortcuts )
{
  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7);

  // Create the Bayes tree
  Ordering ordering;
  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);

  // Check the conditional P(Root|Root)
  GaussianBayesNet empty;
  GaussianBayesTree::sharedClique R = bayesTree.roots().front();
  GaussianBayesNet actual1 = R->shortcut(R);
  EXPECT(assert_equal(empty,actual1,tol));

  // Check the conditional P(C2|Root)
  GaussianBayesTree::sharedClique C2 = bayesTree[X(3)];
  GaussianBayesNet actual2 = C2->shortcut(R);
  EXPECT(assert_equal(empty,actual2,tol));

  // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
  /** TODO: Note for multifrontal conditional:
   * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional()
   * We don't know yet how to take it out.
   */
//  GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
//  p_x2_x4->print("Conditional p_x2_x4: ");
//  GaussianBayesNet expected3(p_x2_x4);
//  GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
//  GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
//  EXPECT(assert_equal(expected3,actual3,tol));
}

///* ************************************************************************* */
//TEST( BayesTree, balanced_smoother_clique_marginals )
//{
//  // Create smoother with 7 nodes
//  Ordering ordering;
//  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
//  GaussianFactorGraph smoother = createSmoother(7, ordering).first;
//
//  // Create the Bayes tree
//  GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
//  GaussianISAM bayesTree(chordalBayesNet);
//
//  // Check the clique marginal P(C3)
//  double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
//  GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt);
//  push_front(expected,ordering[X(1)], Z_2x1, eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
//  GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]];
//  GaussianFactorGraph marginal = C3->marginal(R);
//  GaussianVariableIndex varIndex(marginal);
//  Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
//  Permutation toFrontInverse(*toFront.inverse());
//  varIndex.permute(toFront);
//  for(const GaussianFactor::shared_ptr& factor: marginal) {
//    factor->permuteWithInverse(toFrontInverse); }
//  GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
//  actual.permuteWithInverse(toFront);
//  EXPECT(assert_equal(expected,actual,tol));
//}

/* ************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_joint )
{
  // Create smoother with 7 nodes
  Ordering ordering;
  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
  GaussianFactorGraph smoother = createSmoother(7);

  // Create the Bayes tree, expected to look like:
  //   x5 x6 x4
  //     x3 x2 : x4
  //       x1 : x2
  //     x7 : x6
  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);

  // Conditional density elements reused by both tests
  const Matrix I = I_2x2, A = -0.00429185*I;

  // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
  GaussianBayesNet expected1 = list_of
    // Why does the sign get flipped on the prior?
    (GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7))
    (GaussianConditional(X(7), Z_2x1, -1*I/sigmax7));
  GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7));
  EXPECT(assert_equal(expected1, actual1, tol));

  //  // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
  //  GaussianBayesNet expected2;
  //  GaussianConditional::shared_ptr
  //      parent2(new GaussianConditional(X(1), Z_2x1, -1*I/sigmax1, ones(2)));
  //    expected2.push_front(parent2);
  //  push_front(expected2,X(7), Z_2x1, I/sigmax1, X(1), A/sigmax1, sigma);
  //  GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1));
  //  EXPECT(assert_equal(expected2,actual2,tol));

  // Check the joint density P(x1,x4), i.e. with a root variable
  double sig14 = 0.784465;
  Matrix A14 = -0.0769231*I;
  GaussianBayesNet expected3 = list_of
    (GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14))
    (GaussianConditional(X(4), Z_2x1, I/sigmax4));
  GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4));
  EXPECT(assert_equal(expected3,actual3,tol));

  //  // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
  //  GaussianBayesNet expected4;
  //  GaussianConditional::shared_ptr
  //      parent4(new GaussianConditional(X(1), Z_2x1, -1.0*I/sigmax1, ones(2)));
  //    expected4.push_front(parent4);
  //  double sig41 = 0.668096;
  //  Matrix A41 = -0.055794*I;
  //  push_front(expected4,X(4), Z_2x1, I/sig41, X(1), A41/sig41, sigma);
  //  GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1));
  //  EXPECT(assert_equal(expected4,actual4,tol));
}

/* ************************************************************************* */
TEST(GaussianBayesTree, shortcut_overlapping_separator)
{
  // Test computing shortcuts when the separator overlaps.  This previously
  // would have highlighted a problem where information was duplicated.

  // Create factor graph:
  // f(1,2,5)
  // f(3,4,5)
  // f(5,6)
  // f(6,7)
  GaussianFactorGraph fg;
  noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
  fg.add(1, (Matrix(1, 1) <<  1.0).finished(), 3, (Matrix(1, 1) <<  2.0).finished(), 5, (Matrix(1, 1) <<  3.0).finished(), (Vector(1) << 4.0).finished(), model);
  fg.add(1, (Matrix(1, 1) <<  5.0).finished(), (Vector(1) << 6.0).finished(), model);
  fg.add(2, (Matrix(1, 1) <<  7.0).finished(), 4, (Matrix(1, 1) <<  8.0).finished(), 5, (Matrix(1, 1) <<  9.0).finished(), (Vector(1) << 10.0).finished(), model);
  fg.add(2, (Matrix(1, 1) <<  11.0).finished(), (Vector(1) << 12.0).finished(), model);
  fg.add(5, (Matrix(1, 1) <<  13.0).finished(), 6, (Matrix(1, 1) <<  14.0).finished(), (Vector(1) << 15.0).finished(), model);
  fg.add(6, (Matrix(1, 1) <<  17.0).finished(), 7, (Matrix(1, 1) <<  18.0).finished(), (Vector(1) << 19.0).finished(), model);
  fg.add(7, (Matrix(1, 1) <<  20.0).finished(), (Vector(1) << 21.0).finished(), model);

  // Eliminate into BayesTree
  // c(6,7)
  // c(5|6)
  //   c(1,2|5)
  //   c(3,4|5)
  Ordering ordering(fg.keys());
  GaussianBayesTree bt = *fg.eliminateMultifrontal(ordering); // eliminate in increasing key order, fg.keys() is sorted.

  GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR);

  Matrix expectedJointJ = (Matrix(2,3) <<
    5, 0, 6,
    0, -11, -12
    ).finished();
  Matrix actualJointJ = joint.augmentedJacobian();

  EXPECT(assert_equal(expectedJointJ, actualJointJ));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


//(XTX)-1XTY
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <string>
#include <vector>
#include <core/engine.hpp>
#include "io/input/hdfs_line_inputformat.hpp"
#include "boost/tokenizer.hpp"
typedef std::vector<double> fvec;
typedef std::vector<std::vector<double>> matrix;

class PIObject {
   public:
    typedef int KeyT;
    int key;

    explicit PIObject(KeyT key) { this->key = key; }

    const int& id() const { return key; }
};

std::string vec_to_str(fvec v) {
    std::string str("");
    for (auto& x : v) {
        str += std::to_string(x);
        str += " ";
    }
    return str;
}

fvec& operator+= (fvec& va, const fvec& vb) {

    int n = va.size();
    for (int i=0; i < n; i++) va[i] += vb[i];
    return va;
}
matrix& operator+= (matrix& ma, const matrix& mb) {

    int m = ma.size();
    int n = ma[0].size();
    for (int i=0; i < m; i++)
      for (int j=0; j< n; j++)
        ma[i][j] += mb[i][j];
    return ma;
}

namespace ublas = boost::numeric::ublas;
/* Matrix inversion routine.
   Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T>
bool InvertMatrix (const ublas::matrix<T>& input, ublas::matrix<T>& inverse) {

 typedef ublas::permutation_matrix<std::size_t> pmatrix;
 // create a working copy of the input
 ublas::matrix<T> A(input);
 // create a permutation matrix for the LU-factorization
 pmatrix pm(A.size1());
 // perform LU-factorization
 int res = lu_factorize(A,pm);
       if( res != 0 ) return false;
 // create identity matrix of "inverse"
 inverse.assign(ublas::identity_matrix<T>(A.size1()));
 // backsubstitute to get the inverse
 lu_substitute(A, pm, inverse);
 return true;
}

bool MatrixInversion(const matrix& input, matrix& output){
  int n=input.size();
  ublas::matrix<double> input2 (n,n);
  ublas::matrix<double> output2 (n,n);
  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      input2(i,j)=input[i][j];
    }
  }

  InvertMatrix(input2,output2);

  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      output[i][j]=output2(i,j);
    }
  }
}

void MatrixVectormultiplication(const matrix& A,const fvec& B,fvec& output){
  int m=A.size();
  int n=A[0].size();
  for(int i=0;i<m;i++){
    for(int j=0;j<n;j++){
      output[i]+=A[i][j]*B[j];
    }
  }

}

void linear_regression() {
  husky::io::HDFSLineInputFormat infmt;
  infmt.set_input(husky::Context::get_param("input"));
  husky::ObjList<PIObject> pi_list;
  std::vector<fvec> X;
  std::vector<double> y;

  auto parse_wc = [&](boost::string_ref& chunk) {
      if (chunk.size() == 0)
          return;
      fvec here;
      boost::char_separator<char> sep(" \t");
      boost::tokenizer<boost::char_separator<char>> tok(chunk, sep);

      for (auto& w : tok) {
          here.push_back(std::stod(w));
      }
      y.push_back(here.back());
      here.pop_back();
      // The intercept term
      here.push_back(1.0);
      X.push_back(here);

  };

  husky::load(infmt, parse_wc);

  std::cout << "helloworld" << std::endl;

  auto& ch1 =husky::ChannelFactory::create_push_combined_channel<matrix, husky::SumCombiner<matrix>>(pi_list, pi_list);
  auto& ch2 =husky::ChannelFactory::create_push_combined_channel<fvec, husky::SumCombiner<fvec>>(pi_list, pi_list);
  std::cout << "helloworld" << std::endl;
  int dimension=0;
  if (X.size()>0)
    dimension=X[0].size();
  std::cout << "dimension: "<<dimension<<" numberofexamples"<<X.size() << std::endl;
  matrix A(dimension, fvec(dimension));
  for(int i=0;i<X.size();i++){
    fvec now=X[i];
    for(int a=0;a<dimension;a++){
      for(int b=0;b<dimension;b++){
        A[a][b]+=now[a]*now[b];
      }
    }

  }
  std::cout << "helloworld2" << std::endl;
  if(dimension!=0)
    ch1.push(A,0);
  std::cout << "helloworld3" << std::endl;
  fvec B(dimension);
  for(int i=0;i<X.size();i++){
    fvec now=X[i];
    for(int a=0;a<dimension;a++){
        B[a]+=now[a]*y[i];
    }
  }
  if(dimension!=0)
    ch2.push(B,0);

  ch1.flush();
  ch2.flush();
  list_execute(pi_list, [&ch1,&ch2](PIObject& obj) {
      matrix XTX = ch1.get(obj);

      fvec XTy=ch2.get(obj);

      int dimension=XTX.size();
      matrix IA(dimension,fvec(dimension));
      MatrixInversion(XTX,IA);
      fvec output(dimension,0.0);
      MatrixVectormultiplication(IA,XTy,output);
      husky::base::log_msg(vec_to_str(output));
  });

}

int main(int argc, char** argv) {
    std::vector<std::string> args;
    args.push_back("hdfs_namenode");
    args.push_back("hdfs_namenode_port");
    args.push_back("input");

    if (husky::init_with_args(argc, argv, args)) {
        husky::run_job(linear_regression);
        return 0;
    }
    return 1;
}


#include <Eigen/Dense>
#include <iostream>
#include <vector>

template <class Matrix>
void kron(const Matrix & A, const Matrix & B, Matrix & C)
{
	/*int n=A.cols();
	int m=A.rows();*/
	C= Matrix(A.rows()*B.rows(),A.cols()*B.cols());
	for (int i=0;i<A.rows();++i){
		for (int j=0;j<A.cols();++j){
			C.block(i*B.rows(),j*B.cols(),B.rows(),B.cols())=A(i,j)*B;
		}
	}
	// TODO
}


//####### Test module for Quantum Synchrotron tables ###########################

//Define Module name
 #define BOOST_TEST_MODULE "phys/quantum_sync/tables"

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include <vector>
#include <algorithm>
#include <array>

#include "quantum_sync_engine_tables.hpp"

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-3;
const double double_small = 1e-20;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-2;
const float float_small = 1e-10;

using namespace picsar::multi_physics::phys::quantum_sync;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

template <typename T>
T constexpr small()
{
    if(std::is_same<T,float>::value)
        return float_small;
    else
        return double_small;
}

const double chi_min = 0.001;
const double chi_max = 1000;
const int how_many = 500;
const int how_many_frac = 500;
const double frac_min = 1e-12;

template <typename RealType, typename VectorType>
auto get_table()
{
    const auto params =
        dndt_lookup_table_params<RealType>{
            static_cast<RealType>(chi_min),
            static_cast<RealType>(chi_max), how_many};

    return dndt_lookup_table<RealType, VectorType>{params};
}

template <typename RealType, typename VectorType>
auto get_em_table()
{
    const auto params =
        photon_emission_lookup_table_params<RealType>{
            static_cast<RealType>(chi_min),
            static_cast<RealType>(chi_max),
            static_cast<RealType>(frac_min),
            how_many, how_many_frac};

    return photon_emission_lookup_table<RealType, VectorType>{params};
}

// ------------- Tests --------------

template <typename RealType, typename VectorType>
void check_dndt_table()
{
    auto table = get_table<RealType, VectorType>();
    BOOST_CHECK_EQUAL(table.is_init(),false);

    VectorType coords = table.get_all_coordinates();

    BOOST_CHECK_EQUAL(coords.size(),how_many);

    const RealType log_chi_min = log(chi_min);
    const RealType log_chi_max = log(chi_max);

     for (int i = 0 ; i < static_cast<int>(coords.size()); ++i){
         auto res = coords[i];
         auto expected = static_cast<RealType>(
             exp(log_chi_min + i*(log_chi_max-log_chi_min)/(how_many-1)));
         BOOST_CHECK_SMALL((res-expected)/expected, tolerance<RealType>());
     }

    auto vals = VectorType{coords};

    const RealType alpha = 3.0;

    std::transform(coords.begin(), coords.end(), vals.begin(),
        [=](RealType x){return alpha*x;});

    bool result = table.set_all_vals(vals);
    BOOST_CHECK_EQUAL(result,true);
    BOOST_CHECK_EQUAL(table.is_init(),true);

    auto table_2 = get_table<RealType, VectorType>();
    BOOST_CHECK_EQUAL(table_2 == table, false);
    BOOST_CHECK_EQUAL(table == table, true);
    BOOST_CHECK_EQUAL(table_2 == table_2, true);

    const RealType xo0 = chi_min*0.1;
    const RealType xo1 = chi_max*10;
    const RealType x0 = chi_min;
    const RealType x1 = (chi_max+chi_min)*0.5642 + chi_min;
    const RealType x2 = chi_max;

    const RealType ye_ext_o0 = alpha*chi_min;
    const RealType ye_ext_o1 = alpha*chi_max;
    const RealType ye0 = alpha*x0;
    const RealType ye1 = alpha*x1;
    const RealType ye2 = alpha*x2;

    const auto xxs = std::array<RealType, 5>
        {xo0, x0, x1, x2, xo1};

    const auto exp_ext = std::array<RealType, 5>
        {ye_ext_o0, ye0, ye1, ye2, ye_ext_o1};

    const auto is_out = std::array<bool, 5>
        {true, false, false, false, true};

    for(int i = 0 ; i < static_cast<int>(xxs.size()) ; ++i){
        const RealType res = table.interp(xxs[i]);
        bool flag_out = false;
        const RealType res2 = table.interp(xxs[i], &flag_out);
        BOOST_CHECK_EQUAL(flag_out, is_out[i]);
        BOOST_CHECK_EQUAL(res, res2);

        const RealType expect = exp_ext[i];

        if(i != 0)
            BOOST_CHECK_SMALL((res-expect)/expect, tolerance<RealType>());
        else
            BOOST_CHECK_SMALL((res-expect), tolerance<RealType>());
    }

    const auto table_view = table.get_view();

    for(int i = 0 ; i < static_cast<int>(xxs.size()) ; ++i){
        BOOST_CHECK_EQUAL(table_view.interp(xxs[i]), table.interp(xxs[i]));
    }
}

// ***Test Quantum Synchrotron dndt table
BOOST_AUTO_TEST_CASE( picsar_breit_wheeler_dndt_table)
{
    check_dndt_table<double, std::vector<double>>();
    check_dndt_table<float, std::vector<float>>();
}

template <typename RealType, typename VectorType>
void check_dndt_table_serialization()
{
    const auto params =
        dndt_lookup_table_params<RealType>{
            static_cast<RealType>(0.1),
            static_cast<RealType>(10.0), 3};

    auto table = dndt_lookup_table<
        RealType, VectorType>{params, {1.,2.,3.}};

    auto raw_data = table.serialize();
    auto new_table = dndt_lookup_table<
        RealType, VectorType>{raw_data};

    BOOST_CHECK_EQUAL(new_table.is_init(), true);
    BOOST_CHECK_EQUAL(new_table == table, true);
}

// ***Test Quantum Synchrotron dndt table serialization
BOOST_AUTO_TEST_CASE( picsar_quantum_sync_dndt_table_serialization)
{
    check_dndt_table_serialization<float, std::vector<float>>();
}

template <typename RealType, typename VectorType>
void check_photon_emission_table()
{
    auto table = get_em_table<RealType, VectorType>();
    BOOST_CHECK_EQUAL(table.is_init(),false);

    auto coords = table.get_all_coordinates();

    BOOST_CHECK_EQUAL(coords.size(),how_many*how_many_frac);

    const RealType log_chi_min = log(chi_min);
    const RealType log_chi_max = log(chi_max);
    const RealType log_frac_min = log(frac_min);

     for (int i = 0 ; i < how_many*how_many_frac; ++i){
         auto res_1 = coords[i][0];
         auto res_2 = coords[i][1];
         const auto ii = i/how_many_frac;
         const auto jj = i%how_many_frac;
         auto expected_1 = static_cast<RealType>(
             exp(log_chi_min +ii*(log_chi_max-log_chi_min)/(how_many-1)));
        auto expected_2 = static_cast<RealType>(
             expected_1*exp(log_frac_min +jj*(0.0-log_frac_min)/(how_many_frac-1)));

         BOOST_CHECK_SMALL((res_1-expected_1)/expected_1, tolerance<RealType>());
         if(expected_2 != static_cast<RealType>(0.0))
            BOOST_CHECK_SMALL((res_2-expected_2)/expected_2, tolerance<RealType>());
        else
            BOOST_CHECK_SMALL((res_2-expected_2), tolerance<RealType>());
     }

    auto vals = VectorType(coords.size());

    auto functor = [=](std::array<RealType,2> x){
        return static_cast<RealType>(1.0*pow(x[1]/x[0], 4.0 + log10(x[0])));};

    auto inverse_functor = [=](std::array<RealType,2> x){
            return static_cast<RealType>(1.0*pow((1-x[1]), 1.0/(4.0 + log10(x[0]))));};

    std::transform(coords.begin(), coords.end(), vals.begin(),functor);

    bool result = table.set_all_vals(vals);
    BOOST_CHECK_EQUAL(result,true);
    BOOST_CHECK_EQUAL(table.is_init(),true);

    auto table_2 = get_em_table<RealType, VectorType>();
    BOOST_CHECK_EQUAL(table_2 == table, false);
    BOOST_CHECK_EQUAL(table == table, true);
    BOOST_CHECK_EQUAL(table_2 == table_2, true);

    const RealType xo0 = chi_min*0.1;
    const RealType xo1 = chi_max*10;
    const RealType x0 = chi_min;
    const RealType x1 = (chi_max+chi_min)*0.5642 + chi_min;
    const RealType x2 = chi_max;

    const auto xxs = std::array<RealType, 5>
        {xo0, x0, x1, x2, xo1};
    const auto rrs = std::array<RealType, 11>
            {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 0.99};

    for (const auto xx : xxs){
        for (const auto rr : rrs){
            auto res = table.interp(xx, rr);
            auto rxx = xx;
            if(rxx < chi_min) rxx = chi_min;
            if(rxx > chi_max) rxx = chi_max;
            auto expected = inverse_functor(std::array<RealType,2>{rxx, rr})*xx;
            if(expected < small<RealType>())
                BOOST_CHECK_SMALL((res-expected)/expected, tolerance<RealType>());
            else
                BOOST_CHECK_SMALL((res-expected), tolerance<RealType>());

        }
    }

    const auto table_view = table.get_view();

    for(auto xx : xxs){
        for (auto r : rrs)
            BOOST_CHECK_EQUAL(table_view.interp(xx,r ), table.interp(xx, r));
    }
}

// ***Test Quantum Synchrotron photon emission table
BOOST_AUTO_TEST_CASE( picsar_quantum_sync_photon_emission_table)
{
    check_photon_emission_table<double, std::vector<double>>();
    check_photon_emission_table<float, std::vector<float>>();
}

template <typename RealType, typename VectorType>
void check_photon_emission_table_serialization()
{
    const auto params =
        photon_emission_lookup_table_params<RealType>{
            static_cast<RealType>(0.1),static_cast<RealType>(10.0),
            static_cast<RealType>(1e-5), 3, 3};

    auto table = photon_emission_lookup_table<
        RealType, VectorType>{params, {1.,2.,3.,4.,5.,6.,7.,8.,9.}};

    auto raw_data = table.serialize();
    auto new_table =
        photon_emission_lookup_table<RealType, VectorType>{raw_data};

    BOOST_CHECK_EQUAL(new_table.is_init(), true);
    BOOST_CHECK_EQUAL(new_table == table, true);
}

// ***Test Quantum Synchrotron photon emission table serialization
BOOST_AUTO_TEST_CASE( picsar_quantum_sync_photon_emission_table_serialization)
{
    check_photon_emission_table_serialization<double, std::vector<double>>();
    check_photon_emission_table_serialization<float, std::vector<float>>();
}