// Eigen
#include <Eigen/Dense>
#include <Eigen/SVD>

// OpenCV
#include <opencv2/opencv.hpp>

// ROS
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/Float64MultiArray.h>

#include <crane_msgs/CranePendulumImagePoints.h>

namespace crane_vision
{
using namespace sensor_msgs;
using namespace message_filters::sync_policies;

using ProjectionMatrix = Eigen::Matrix<double, 3, 4>;
using Vector3d = Eigen::Matrix<double, 3, 1>;
using StateVector = Eigen::Matrix<double, 6, 1>;
using TransitionMatrix = Eigen::Matrix<double, 6, 6>;
using CovarianceMatrix = Eigen::Matrix<double, 6, 6>;
using AccelerationVector = Eigen::Vector2d;

Vector3d dlt(const Eigen::Vector2d& c0, const Eigen::Vector2d& c1, const Eigen::Vector2d& c2,
             const ProjectionMatrix& P0, const ProjectionMatrix& P1, const ProjectionMatrix P2)
{
  Eigen::Matrix<double, 6, 4> X;
  X.row(0) = c0(0) * P0.row(2) - P0.row(0);
  X.row(1) = c0(1) * P0.row(2) - P0.row(1);
  X.row(2) = c1(0) * P1.row(2) - P1.row(0);
  X.row(3) = c1(1) * P1.row(2) - P1.row(1);
  X.row(4) = c2(0) * P2.row(2) - P2.row(0);
  X.row(5) = c2(1) * P2.row(2) - P2.row(1);

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(X, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::MatrixXd V = svd.matrixV();

  Eigen::Vector3d L = Eigen::Vector3d::Zero();
  double scale = V(3, 3);
  if (scale > 1e-8)
  {
    L = V.col(3).topRows(3) / scale;
  }
  return L;
}

Vector3d findLine(const Eigen::Vector2d& c01, const Eigen::Vector2d& c02, const Eigen::Vector2d& c11,
                  const Eigen::Vector2d& c12, const Eigen::Vector2d& c21, const Eigen::Vector2d& c22,
                  const ProjectionMatrix& P0, const ProjectionMatrix& P1, const ProjectionMatrix P2)
{
  Vector3d X1 = dlt(c01, c11, c21, P0, P1, P2);
  Vector3d X2 = dlt(c02, c12, c22, P0, P1, P2);
  Vector3d L = X2 - X1;
  L.normalize();
  return L;
}

StateVector fk(StateVector x, AccelerationVector u, double w, double L)
{
  double c0 = cos(x(0));
  double c1 = cos(x(1));
  double s0 = sin(x(0));
  double s1 = sin(x(1));

  StateVector x1 = StateVector::Zero();
  x1(0) = x(2);
  x1(1) = x(3);
  x1(2) = (2.0 * x(2) * x(3) * s1 - w * s0 + (u(1) * c0 / L)) / c1;
  x1(3) = -c1 * s1 * x(2) * x(2) - (u(0) * c1 + u(1) * s0 * s1) / L - w * c0 * s1;
  return x1;
}

TransitionMatrix Fk(StateVector x, AccelerationVector u, double w, double L, double dt)
{
  double c0 = cos(x(0));
  double c1 = cos(x(1));
  double s0 = sin(x(0));
  double s1 = sin(x(1));

  TransitionMatrix X = TransitionMatrix::Identity();
  X(0, 2) = dt;
  X(1, 3) = dt;
  X(2, 0) = -(dt * (w * c0 + (u(1) * s0) / L)) / c1;
  X(2, 1) = 2.0 * dt * x(2) * x(3) + (dt * s1 * (2.0 * x(2) * x(3) * s1 - w * s0 + (u(1) * c0) / L)) / (c1 * c1);
  X(2, 2) = (2.0 * dt * x(3) * s1) / c1 + 1.0;
  X(2, 3) = (2.0 * dt * x(2) * s1) / c1;
  X(3, 0) = dt * (w * s0 * s1 - (u[1] * c0 * s1) / L);
  X(3, 1) = -dt * (x(2) * x(2) * c1 * c1 - x(2) * x(2) * s1 * s1 - (u(0) * s1 - u(1) * c1 * s0 / L) + w * c0 * c1);
  X(3, 2) = -2.0 * dt * x(2) * c1 * s1;
  return X;
}

auto ekf(Vector3d Lvec, AccelerationVector uk, Eigen::MatrixXd hat_Pkm1, Eigen::VectorXd hat_thetakm1, double r,
         double dt)
{
  int D = 10;
  double g = 9.81;
  double L = r;
  AccelerationVector u = uk;
  Eigen::VectorXd x = hat_thetakm1;
  Eigen::Matrix2d R;
  R << 0.00377597, -0.00210312, -0.00210312, 0.00125147;

  CovarianceMatrix Q = CovarianceMatrix::Zero();
  Q.diagonal() << 0.00003, 0.00003, 0.0005, 0.0005, 0.0001, 0.0001;
  Eigen::Matrix<double, 2, 6> H;
  H << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;

  TransitionMatrix Fi = Fk(x, u, g / r, L, dt);

  Eigen::Vector2d zkp1;
  zkp1 << atan2(-Lvec(1), Lvec(2)), atan2(Lvec(0), sqrt(Lvec(1) * Lvec(1) + Lvec(2) * Lvec(2)));

  for (int i = 0; i < D; ++i)
  {
    x = x + fk(x, u, g / r, L) * dt / D;
  }

  Eigen::MatrixXd barP_kp1 = (Fi * hat_Pkm1 * Fi.transpose()) + Q;
  Eigen::MatrixXd K_kp1 = barP_kp1 * H.transpose() * (R + H * barP_kp1 * H.transpose()).inverse();

  Eigen::VectorXd hat_thetak = x + K_kp1 * (zkp1 - H * x);
  Eigen::MatrixXd hat_Pk = (Eigen::Matrix<double, 6, 6>::Identity() - K_kp1 * H) * barP_kp1;

  return std::make_tuple(hat_thetak, hat_Pk);
}

struct CompareArea
{
  CompareArea(const std::vector<float>& areas) : areas_(&areas)
  {
  }
  bool operator()(int a, int b) const
  {
    return (*areas_)[a] > (*areas_)[b];
  }
  const std::vector<float>* areas_;
};

class CraneVisionEKF : public nodelet::Nodelet {

  virtual void onInit();

};

class CraneVisionNodelet : public nodelet::Nodelet
{
  virtual void onInit();

  bool extractSphereCenters(const sensor_msgs::ImageConstPtr& image_msg,
                            const sensor_msgs::CameraInfoConstPtr& info_msg, const cv::Rect& roi,
                            std::vector<double>& points, bool debug, const std::string& winname);

  void imageCb(const sensor_msgs::ImageConstPtr& image0_msg, const sensor_msgs::CameraInfoConstPtr& info0_msg,
               const sensor_msgs::ImageConstPtr& image1_msg, const sensor_msgs::CameraInfoConstPtr& info1_msg,
               const sensor_msgs::ImageConstPtr& image2_msg, const sensor_msgs::CameraInfoConstPtr& info2_msg);

  boost::shared_ptr<image_transport::ImageTransport> it_;

  /// Subscriptions
  image_transport::SubscriberFilter image0_sub_, image1_sub_, image2_sub_;
  message_filters::Subscriber<CameraInfo> image0_info_sub_, image1_info_sub_, image2_info_sub_;
  typedef ExactTime<Image, CameraInfo, Image, CameraInfo, Image, CameraInfo> ExactPolicy;
  typedef ApproximateTime<Image, CameraInfo, Image, CameraInfo, Image, CameraInfo> ApproximatePolicy;
  typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
  typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
  boost::shared_ptr<ExactSync> exact_sync_;
  boost::shared_ptr<ApproximateSync> approximate_sync_;

  image_transport::CameraSubscriber camera_sub_;
  ros::Publisher pub_;

  int hmin_, hmax_, smin_, smax_, vmin_, vmax_;
  cv::Rect roi0_, roi1_, roi2_;

  Eigen::Matrix<double, 6, 6> hat_Pkm1_;
  StateVector hat_thetakm1_;
  ProjectionMatrix P0_, P1_, P2_;

  bool debug_;
};

void CraneVisionNodelet::onInit()
{
  ros::NodeHandle& nh = getNodeHandle();
  ros::NodeHandle& private_nh = getPrivateNodeHandle();
  it_.reset(new image_transport::ImageTransport(nh));

  // Synchronize inputs. Topic subscriptions happen on demand in the connection
  // callback. Optionally do approximate synchronization.
  int queue_size;
  private_nh.param("queue_size", queue_size, 5);
  bool approx;
  private_nh.param("approximate_sync", approx, false);
  if (approx)
  {
    approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size), image0_sub_, image0_info_sub_,
                                                image1_sub_, image1_info_sub_, image2_sub_, image2_info_sub_));
    approximate_sync_->registerCallback(boost::bind(&CraneVisionNodelet::imageCb, this, _1, _2, _3, _4, _5, _6));
  }
  else
  {
    exact_sync_.reset(new ExactSync(ExactPolicy(queue_size), image0_sub_, image0_info_sub_, image1_sub_,
                                    image1_info_sub_, image2_sub_, image2_info_sub_));
    exact_sync_->registerCallback(boost::bind(&CraneVisionNodelet::imageCb, this, _1, _2, _3, _4, _5, _6));
  }

  std::vector<int> roi;
  if (private_nh.getParam("roi0", roi))
  {
    roi0_ = cv::Rect(roi[0], roi[1], roi[2], roi[3]);
  }
  else
  {
    NODELET_ERROR_STREAM("Did not find any roi0 parameters!");
  }
  if (private_nh.getParam("roi1", roi))
  {
    roi1_ = cv::Rect(roi[0], roi[1], roi[2], roi[3]);
  }
  else
  {
    NODELET_ERROR_STREAM("Did not find any roi1 parameters!");
  }
  if (private_nh.getParam("roi2", roi))
  {
    roi2_ = cv::Rect(roi[0], roi[1], roi[2], roi[3]);
  }
  else
  {
    NODELET_ERROR_STREAM("Did not find any roi2 parameters!");
  }

  private_nh.param("debug", debug_, false);

  private_nh.param("hmin", hmin_, 43);
  private_nh.param("hmax", hmax_, 73);
  private_nh.param("smin", smin_, 54);
  private_nh.param("smax", smax_, 250);
  private_nh.param("vmin", vmin_, 86);
  private_nh.param("vmax", vmax_, 255);

  image0_sub_.subscribe(*it_, "/camera0/image_raw", 1);
  image0_info_sub_.subscribe(nh, "/camera0/camera_info", 1);
  image1_sub_.subscribe(*it_, "/camera1/image_raw", 1);
  image1_info_sub_.subscribe(nh, "/camera1/camera_info", 1);
  image2_sub_.subscribe(*it_, "/camera2/image_raw", 1);
  image2_info_sub_.subscribe(nh, "/camera2/camera_info", 1);

  pub_ = private_nh.advertise<crane_msgs::CranePendulumImagePoints>("points", 1);

  P0_ << 942.0, 0.0, 623.66, 0.0, 0.0, 942.0, 345.69, 0.0, 0.0, 0.0, 1.0, 0.0;
  P1_ << 941.0, 0.0, 637.21, 220005.80000000002, 0.0, 941.0, 349.9, 0.0, 0.0, 0.0, 1.0, 0.0;
  P2_ << 937.0, 0.0, 637.21, 437579.0, 0.0, 937.0, 381.54, 0.0, 0.0, 0.0, 1.0, 0.0;

  hat_thetakm1_.setZero();
  // hat_Pkm1_ << 0.0, 0.0, 0.0, 0.0, 0.04, -0.03;
}

bool CraneVisionNodelet::extractSphereCenters(const sensor_msgs::ImageConstPtr& image_msg,
                                              const sensor_msgs::CameraInfoConstPtr& info_msg, const cv::Rect& roi,
                                              std::vector<double>& points, bool debug, const std::string& winname)
{
  using namespace std;
  using namespace cv;

  Mat bgr8_image = cv_bridge::toCvShare(image_msg, "bgr8")->image;

  Mat roi_image = bgr8_image(roi);

  Mat hsv_image;
  cvtColor(roi_image, hsv_image, CV_BGR2HSV);

  Mat thresh_image;
  inRange(hsv_image, Scalar(hmin_, smin_, vmin_), Scalar(hmax_, smax_, vmax_), thresh_image);

  using Contours = vector<vector<Point> >;
  using Hierarchy = vector<Vec4i>;
  Contours contours;
  Hierarchy hierarchy;

  /// Find contours
  findContours(thresh_image, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
  if (contours.size() > 1)
  {
    /// Get sort index based on contour area
    vector<int> sort_idx(contours.size());
    vector<float> areas(contours.size());
    for (int n = 0; n < (int)contours.size(); n++)
    {
      sort_idx[n] = n;
      areas[n] = contourArea(contours[n], false);
    }
    sort(sort_idx.begin(), sort_idx.end(), CompareArea(areas));

    /// Get the moments
    vector<Moments> mu(contours.size());
    for (int i = 0; i < contours.size(); i++)
    {
      mu[i] = moments(contours[i], false);
    }

    ///  Get the mass centers:
    vector<Point2d> mc(contours.size());
    for (int i = 0; i < contours.size(); i++)
    {
      mc[i] = Point2d(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
    }

    /// Swap to get right order
    if (mc[sort_idx[1]].y > mc[sort_idx[0]].y)
    {
      points[0] = mc[sort_idx[1]].x;
      points[1] = mc[sort_idx[1]].y;
      points[2] = mc[sort_idx[0]].x;
      points[3] = mc[sort_idx[0]].y;
    }
    else
    {
      points[0] = mc[sort_idx[0]].x;
      points[1] = mc[sort_idx[0]].y;
      points[2] = mc[sort_idx[1]].x;
      points[3] = mc[sort_idx[1]].y;
    }

    if (debug)
    {
      circle(roi_image, Point2d(points[0], points[1]), 5, Scalar(0, 0, 255), -1, 8, 0);
      circle(roi_image, Point2d(points[2], points[3]), 5, Scalar(255, 0, 0), -1, 8, 0);
      imshow(winname, roi_image);
      waitKey(30);
    }
  }
}

void CraneVisionNodelet::imageCb(const sensor_msgs::ImageConstPtr& image0_msg,
                                 const sensor_msgs::CameraInfoConstPtr& info0_msg,
                                 const sensor_msgs::ImageConstPtr& image1_msg,
                                 const sensor_msgs::CameraInfoConstPtr& info1_msg,
                                 const sensor_msgs::ImageConstPtr& image2_msg,
                                 const sensor_msgs::CameraInfoConstPtr& info2_msg)
{
  using namespace std;
  using namespace Eigen;

  vector<double> points0{ 0.0, 0.0, 0.0, 0.0 };
  vector<double> points1{ 0.0, 0.0, 0.0, 0.0 };
  vector<double> points2{ 0.0, 0.0, 0.0, 0.0 };

  extractSphereCenters(image0_msg, info0_msg, roi0_, points0, debug_, "camera0");
  extractSphereCenters(image1_msg, info1_msg, roi1_, points1, debug_, "camera1");
  extractSphereCenters(image2_msg, info2_msg, roi2_, points2, debug_, "camera2");

  Vector2d center01 = Vector2d(points0[0] + roi0_.x, points0[1] + roi0_.y);
  Vector2d center02 = Vector2d(points0[2] + roi0_.x, points0[3] + roi0_.y);
  Vector2d center11 = Vector2d(points1[0] + roi1_.x, points1[1] + roi1_.y);
  Vector2d center12 = Vector2d(points1[2] + roi1_.x, points1[3] + roi1_.x);
  Vector2d center21 = Vector2d(points2[0] + roi2_.x, points2[1] + roi2_.y);
  Vector2d center22 = Vector2d(points2[2] + roi2_.x, points2[3] + roi2_.y);

  // Vector3d Lc0 = findLine(center01, center02, center11, center12, center21, center22, P0_, P1_, P2_);

  // todo(Lars): Transform to inertial coordinatesA
  // Vector3d Lvec = Lc0;

  // std::tie(hat_thetakm1_, hat_Pkm1_) = ekf(Lvec, AccelerationVector(0.0,0.0), hat_thetakm1_, hat_Pkm1_, 1.05, 0.01);

  crane_msgs::CranePendulumImagePoints msg;
  // Reuse the synchronized time stamp
  msg.header.stamp = image0_msg->header.stamp;
  std::vector<double> data(12);
  msg.points =
      std::vector<double>{ points0[0] + roi0_.x, points0[1] + roi0_.y, points0[2] + roi0_.x, points0[3] + roi0_.y,
                           points1[0] + roi1_.y, points1[1] + roi1_.y, points1[2] + roi1_.y, points1[3] + roi1_.y,
                           points2[0] + roi2_.y, points2[1] + roi2_.y, points2[2] + roi2_.y, points2[3] + roi2_.y };
  pub_.publish(msg);
}

}  // namespace crane_vision

// Register nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(crane_vision::CraneVisionNodelet, nodelet::Nodelet)


// STL includes
#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <random>
#include <utility>
#include <thread>

// Local includes
#include "Merl.hpp"
#include <tests/Tests.hpp>
#include <tests/SH.hpp>
#include <include/Utils.hpp>
#include <include/SphericalHarmonics.hpp>
#include <include/DirectionsSampling.hpp>
#include <include/SphericalIntegration.hpp>

// GLM include
#include <glm/glm.hpp>

// Include Eigen
#include <Eigen/Core>

struct MerlProjectionThread : public std::thread {

   int order;
   std::vector<Eigen::MatrixXf> cijs;

   MerlProjectionThread(const MerlBRDF* brdf,
                        const std::vector<Vector>* ws,
                        int order, int skip, int nthread) :
      std::thread(&MerlProjectionThread::run, this, brdf, ws, skip, order, nthread) {}

   /* Pre-convolved the Zonal basis to perform smooth evaluation.
    */
   static void ApplyZonalFilter(Eigen::VectorXf& clm) {
      const auto bmax = floor(sqrt(clm.size())-1);
      for(unsigned int i=0; i<clm.size(); ++i) {
         const auto b = floor(sqrt(i));
         const auto f = exp(- pow(float(b)/float(bmax), 2));
         clm[i] *= f;
      }
   }

   void run(const MerlBRDF* brdf,
            const std::vector<Vector>* dirs,
            int skip, int order, int nthread) {

      cijs = std::vector<Eigen::MatrixXf>(6, Eigen::MatrixXf::Zero(SH::Terms(order), SH::Terms(order)));

      const int size = SH::Terms(order);
      Eigen::VectorXf ylmo(size);
      Eigen::VectorXf ylmi(size);
      for(unsigned int i=skip; i<dirs->size(); i+=nthread) {
         const Vector& wo = (*dirs)[i];

         if(skip == 0) {
            std::cout << "Progress: " << i << " / " << dirs->size() << "     \r";
            std::cout.flush();
         }

         // Skip below the horizon configuration
         if(wo.z < 0.0) continue;
         SH::FastBasis(wo, order, ylmo);

         for(unsigned int j=0; j<dirs->size(); ++j) {
            const Vector& wi = (*dirs)[j];
            // Skip below the horizon configuration
            if(wi.z < 0.0) continue;

            // Evaluate the BRDF value
            const auto rgb = brdf->value<Vector, Vector>(wi, wo);
            SH::FastBasis(wi, order, ylmi);

            // Apply filtering
            //ApplyZonalFilter(ylmo);
            //ApplyZonalFilter(ylmi);

            Eigen::MatrixXf mat = ylmo * ylmi.transpose();
#ifndef SYMMETRIZE
            mat = 0.5f*(mat + mat.transpose());
#endif
            cijs[0] += rgb[0] * mat;
            cijs[1] += rgb[1] * mat;
            cijs[2] += rgb[2] * mat;
            // Note: Here the correct weighting should be with respect to wi.z
            // but I use wo.z since it allows to reduce the ringing drastically.
#ifdef LOOKS_BETTER
            cijs[3] += rgb[0] * wo.z * mat;
            cijs[4] += rgb[1] * wo.z * mat;
            cijs[5] += rgb[2] * wo.z * mat;
#else // CORRECT
            cijs[3] += rgb[0] * wi.z * mat;
            cijs[4] += rgb[1] * wi.z * mat;
            cijs[5] += rgb[2] * wi.z * mat;
#endif
         }
      }
   }
};

int MerlProjectionMatrix(const std::string& filename,
                         int order = 15, int N = 1000) {

   // Constants
   const int size = SH::Terms(order);

   // Load the BRDF
   MerlBRDF brdf;
   if(! brdf.read_brdf(filename)) {
      std::cerr << "Failed: unable to load the MERL brdf" << std::endl;
      return 1;
   }

   const auto k = filename.rfind('.');
   std::string ofilename = filename;
   ofilename.replace(k, std::string::npos, ".mats");
   std::cout << "Will output to \"" << ofilename << "\"" << std::endl;

   // Values
   std::vector<Eigen::MatrixXf> cijs(6, Eigen::MatrixXf::Zero(size, size));
   const auto dirs = SamplingFibonacci<Vector>(N);

   const int nbthreads = std::thread::hardware_concurrency();
   std::vector<MerlProjectionThread*> threads;
   for(int k=0; k<nbthreads; ++k) {
      MerlProjectionThread* th = new MerlProjectionThread(&brdf, &dirs, order, k, nbthreads);
      threads.push_back(th);
   }

   for(MerlProjectionThread* th : threads) {
      th->join();
      cijs[0] += th->cijs[0];
      cijs[1] += th->cijs[1];
      cijs[2] += th->cijs[2];
      cijs[3] += th->cijs[3];
      cijs[4] += th->cijs[4];
      cijs[5] += th->cijs[5];
      delete th;
   }
   const float factor = 16.0*M_PI*M_PI / float(N*N);
   cijs[0] *= factor;
   cijs[1] *= factor;
   cijs[2] *= factor;
   cijs[3] *= factor;
   cijs[4] *= factor;
   cijs[5] *= factor;

   SaveMatrices(ofilename, cijs);

   // Print values
   std::string gfilename = filename;
   gfilename.replace(k, std::string::npos, ".gnuplot");
   std::ofstream file(gfilename.c_str(), std::ios_base::trunc);
   const float thetai = -0.5f*M_PI * 30.f/90.f;
   const Vector wi(sin(thetai), 0, cos(thetai));
   const auto ylmi = SH::FastBasis(wi, order);
   Vector wo;
   for(int i=0; i<90; ++i) {
      const float theta = 0.5*M_PI * i / float(90);
      wo.x = sin(theta);
      wo.y = 0;
      wo.z = cos(theta);

      // Ref
      const auto RGB = brdf.value<Vector, Vector>(wi, wo);
      const float R = RGB[0];
      const float G = RGB[1];
      const float B = RGB[2];

      // SH expansion
      const auto ylmo = SH::FastBasis(wo, order);
      const Eigen::VectorXf rlm = cijs[0] * ylmo;
      const Eigen::VectorXf glm = cijs[1] * ylmo;
      const Eigen::VectorXf blm = cijs[2] * ylmo;
      const float r = ylmi.dot(rlm);
      const float g = ylmi.dot(glm);
      const float b = ylmi.dot(blm);

      file << theta << "\t" << r << "\t" << g << "\t" << b
                    << "\t" << R << "\t" << G << "\t" << B
                    << std::endl;
   }

   int nb_fails = 0;
   return nb_fails;
}

bool parseArguments(int argc, char** argv, std::string& filename,
                    int& order, int& nb) {

   // Loop over all the different elements of the command line and search for
   // some patterns
   for(int k=0; k<argc; ++k) {
      if(argv[k] == std::string("-h") || argv[k] == std::string("--help")) {
         std::cerr << "Usage: merl2sh [options] filename.binary" << std::endl;
         return false;
      }
      if((argv[k] == std::string( "-o") || argv[k] == std::string("--order")) && k+1<argc) {
         order = std::atoi(argv[k+1]);
      }
      if((argv[k] == std::string( "-n") || argv[k] == std::string("--nb")) && k+1<argc) {
         nb = std::atoi(argv[k+1]);
      }
   }

   // The filename to convert is the last argument of the command line.
   if(argc > 1) {
      filename = argv[argc-1];
      return true;
   } else {
      return false;
   }
}

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

   int nb_fails = 0;
   std::string filename;
   int order = 3;
   int nb = 1000;
   if(! parseArguments(argc, argv, filename, order, nb)) {
      return EXIT_SUCCESS;
   }

   // Load an example
   nb_fails += MerlProjectionMatrix(argv[argc-1], order, nb);

   if(nb_fails > 0) {
      return EXIT_FAILURE;
   } else {
      return EXIT_SUCCESS;
   }
}


#ifndef SPIRIT_SKIP_HTST

#include <engine/HTST.hpp>
#include <engine/Sparse_HTST.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <engine/Hamiltonian_Heisenberg.hpp>
#include <utility/Constants.hpp>
#include <utility/Logging.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <GenEigsSolver.h>  // Also includes <MatOp/DenseGenMatProd.h>
#include <SymEigsSolver.h>
#include <MatOp/SparseSymMatProd.h>

#include <GenEigsRealShiftSolver.h>

#include <fmt/format.h>
#include <fmt/ostream.h>

namespace C = Utility::Constants;

namespace Engine
{
    namespace Sparse_HTST
    {

        void Sparse_Get_Lowest_Eigenvector(const SpMatrixX & matrix, int nos, scalar & lowest_evalue, VectorX & lowest_evec)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "        Using Spectra to compute lowest eigenmode...");

            VectorX evalues;
            MatrixX evectors;

            int n_steps = std::max(2, nos);

            //  Create a Spectra solver
            Spectra::SparseSymMatProd<scalar> op(matrix);
            Spectra::SymEigsSolver< scalar, Spectra::SMALLEST_ALGE, Spectra::SparseSymMatProd<scalar> > matrix_spectrum(&op, 1, n_steps);

            matrix_spectrum.init();
            int nconv = matrix_spectrum.compute();

            if (matrix_spectrum.info() == Spectra::SUCCESSFUL)
            {
                evalues = matrix_spectrum.eigenvalues().real();
                evectors = matrix_spectrum.eigenvectors().real();
            } else {
                Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "        Failed to calculate lowest eigenmode. Aborting!");
                return;
            }

            lowest_evalue = evalues[0];
            lowest_evec = evectors.col(0);
        }

        // Project vector such that it is orthogonal to all vectors in orth
        void _orth_project(VectorX & vector, const std::vector<VectorX> & orth)
        {
            for(const VectorX & cur : orth)
            {
                vector -= (vector.dot(cur)) * cur;
            }
        }

        void Sparse_Get_Lowest_Eigenvectors_VP(const SpMatrixX & matrix, scalar max_evalue, scalarfield & evalues, std::vector<VectorX> & evecs)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("    Computing eigenvalues smaller than {}", max_evalue));

            scalar tol = 1e-6;
            scalar evalue_epsilon = 1e-4;
            int n_log_step = 2500;
            scalar cur = 2 * tol;
            scalar m = 0.01;
            scalar step_size = 1e-4;
            int n_iter = 0;
            int nos = matrix.rows()/2;

            scalar sigma_shift = std::max(scalar(5.0), 2*scalar(max_evalue));

            VectorX gradient      = VectorX::Zero(2*nos);
            VectorX gradient_prev = VectorX::Zero(2*nos);
            VectorX velocity      = VectorX::Zero(2*nos);
            scalar cur_evalue_estimate;
            scalar fnorm2, ratio, proj;
            bool run = true;

            // We try to find the lowest n_values eigenvalue/vector pairs
            while(run)
            {
                VectorX x = VectorX::Random(2*nos); // Initialize solver with random normalized vector
                x.normalize();

                fnorm2 = 2*tol*tol;
                n_iter = 0;
                gradient_prev.setZero();
                velocity.setZero();

                bool search = true;
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST,     fmt::format("        Search for eigenpair"));

                while (search)
                {
                    // Compute gradient of unnormalized Rayleigh quotient
                    gradient = 2 * matrix * x;

                    cur_evalue_estimate = 0.5 * x.dot(gradient); // Update the current estimate of our evalue
                    for (int i=0; i<evecs.size(); i++)
                    {
                        gradient += 2 * (sigma_shift - evalues[i]) * (evecs[i].dot(x)) * evecs[i]; // Add the shift so that we dont land on the same eigenvalues we had before. Effectively H -> H + (sigma - lambda) * v^T v, where (lambda, v) is an eigenvalue, eigenvector pair
                    }

                    // Project the gradient orthogonally wrt to x and the previous eigenvectors
                    _orth_project(gradient, {x});

                    velocity = 0.5 * (gradient + gradient_prev) / m;
                    fnorm2 = gradient.squaredNorm();

                    proj = velocity.dot(gradient);
                    ratio = proj/fnorm2;

                    if (proj<=0)
                        velocity.setZero();
                    else
                        velocity = gradient*ratio;

                    // Update x and renormalize
                    x -= step_size * velocity + 0.5/m * step_size * gradient;
                    x.normalize();

                    // Update prev gradient
                    gradient_prev = gradient;

                    // Increment n_iter
                    n_iter++;

                    if(n_iter % n_log_step == 0)
                        Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, fmt::format("        ... Iteration {}: Evalue estimate = {}, Grad. norm = {} (> {})", n_iter, cur_evalue_estimate, std::sqrt(fnorm2), tol));

                    search = (std::sqrt(fnorm2) > tol);
                }

                // Ideally we have found one eigenvalue/vector pair now
                // We save the eigenvalue
                evecs.push_back(x);
                evalues.push_back(x.dot(matrix*x));

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST,     fmt::format("        Found an eigenpair after {} iterations", n_iter));
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST,     fmt::format("        ... Eigenvalue  = {}", evalues.back()));
                if(2*nos>=4)
                    Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, fmt::format("        ... Eigenvector = ({}, {}, {}, ..., {})", evecs.back()[0], evecs.back()[1], evecs.back()[2], evecs.back()[2*nos-1]));
                if (evalues.back() > max_evalue)
                {
                    Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, fmt::format("        No more eigenvalues < {} found. Stopping.", max_evalue));
                    run = false;
                }
            }
        }

        void Sparse_Get_Lowest_Eigenvector_VP(const SpMatrixX & matrix, int nos, const VectorX & init, scalar & lowest_evalue, VectorX & lowest_evec)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "        Minimizing Rayleigh quotient to compute lowest eigenmode...");
            auto & x = lowest_evec;
            x = init;
            x.normalize();

            scalar tol = 1e-6;
            scalar cur = 2 * tol;
            scalar m = 0.01;
            scalar step_size = 1e-4;
            int n_iter = 0;

            VectorX gradient = VectorX::Zero(2*nos);
            VectorX gradient_prev = VectorX::Zero(2*nos);
            VectorX velocity = VectorX::Zero(2*nos);

            VectorX mx; // temporary for matrix * x
            scalar proj; // temporary for gradient * x
            scalar fnorm2 = 2*tol*tol, ratio;

            while (std::sqrt(fnorm2) > tol)
            {
                // Compute gradient of Rayleigh quotient
                mx = matrix * x;
                gradient = 2 * mx;
                proj = gradient.dot(x);
                gradient -= proj * x;

                velocity = 0.5 * (gradient + gradient_prev) / m;
                fnorm2 = gradient.squaredNorm();

                proj = velocity.dot(gradient);
                ratio = proj/fnorm2;

                if (proj<=0)
                    velocity.setZero();
                else
                    velocity = gradient*ratio;

                // Update x and renormalize
                x -= step_size * velocity + 0.5/m * step_size * gradient;
                x.normalize();

                // Update prev gradient
                gradient_prev = gradient;

                // Increment n_iter
                lowest_evalue = x.dot(matrix * x);
                n_iter++;
            }

            // Compute the eigenvalue
            lowest_evalue = x.dot(matrix * x);
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("        Finished after {} iterations", n_iter));
        }

        // Note the two images should correspond to one minimum and one saddle point
        // Non-extremal images may yield incorrect Hessians and thus incorrect results
        void Calculate(Data::HTST_Info & htst_info)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "Sparse Prefactor calculation");
            bool lowest_mode_spectra = false;
            htst_info.sparse = true;
            htst_info.n_eigenmodes_keep = 0;

            const scalar epsilon = 1e-4;
            const scalar epsilon_force = 1e-8;

            auto& image_minimum = *htst_info.minimum->spins;
            auto& image_sp      = *htst_info.saddle_point->spins;

            int nos = image_minimum.size();

            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Saving NO eigenvectors.");

            vectorfield force_tmp(nos, {0,0,0});
            std::vector<std::string> block;

            // TODO
            bool is_afm = false;

            // The gradient (unprojected)
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluation of the gradient at the initial configuration...");
            vectorfield gradient_minimum(nos, {0,0,0});
            htst_info.minimum->hamiltonian->Gradient(image_minimum, gradient_minimum);

            // Check if the configuration is actually an extremum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking if initial configuration is an extremum...");
            Vectormath::set_c_a(1, gradient_minimum, force_tmp);
            Manifoldmath::project_tangential(force_tmp, image_minimum);
            scalar fmax_minimum = Vectormath::max_norm(force_tmp);
            if( fmax_minimum > epsilon_force )
            {
                Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                    "HTST: the initial configuration is not a converged minimum, its max. torque is above the threshold ({} > {})!", fmax_minimum, epsilon_force ));
                return;
            }

            // The gradient (unprojected)
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluation of the gradient at the transition configuration...");
            vectorfield gradient_sp(nos, {0,0,0});
            htst_info.saddle_point->hamiltonian->Gradient(image_sp, gradient_sp);

            // Check if the configuration is actually an extremum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking if transition configuration is an extremum...");
            Vectormath::set_c_a(1, gradient_sp, force_tmp);
            Manifoldmath::project_tangential(force_tmp, image_sp);
            scalar fmax_sp = Vectormath::max_norm(force_tmp);
            if( fmax_sp > epsilon_force )
            {
                Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                    "HTST: the transition configuration is not a converged saddle point, its max. torque is above the threshold ({} > {})!", fmax_sp, epsilon_force ));
                return;
            }

            ////////////////////////////////////////////////////////////////////////
            // Saddle point
            int n_zero_modes_sp = 0;
            scalarfield evalues_sp = scalarfield(0);
            {
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculation for the Saddle Point");

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate tangent basis ...");
                SpMatrixX tangent_basis = SpMatrixX(3*nos, 2*nos);
                Manifoldmath::sparse_tangent_basis_spherical(image_sp, tangent_basis);

                // Evaluation of the Hessian...
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate the Hessian...");
                SpMatrixX sparse_hessian_sp(3*nos, 3*nos);
                htst_info.saddle_point->hamiltonian->Sparse_Hessian(image_sp, sparse_hessian_sp);

                // Transform into geodesic Hessian
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Transform Hessian into geodesic Hessian...");
                SpMatrixX sparse_hessian_sp_geodesic_3N(3*nos, 3*nos);
                sparse_hessian_bordered_3N(image_sp, gradient_sp, sparse_hessian_sp, sparse_hessian_sp_geodesic_3N);
                SpMatrixX sparse_hessian_sp_geodesic_2N = tangent_basis.transpose() * sparse_hessian_sp_geodesic_3N * tangent_basis;

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate lowest eigenmode of the Hessian...");

                std::vector<VectorX> evecs_sp = std::vector<VectorX>(0);
                Sparse_Get_Lowest_Eigenvectors_VP(sparse_hessian_sp_geodesic_2N, epsilon, evalues_sp, evecs_sp);
                scalar lowest_evalue = evalues_sp[0];
                VectorX & lowest_evector = evecs_sp[0];

                // Check if lowest eigenvalue < 0 (else it's not a SP)
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Check if actually a saddle point...");
                if( lowest_evalue > -epsilon )
                {
                    Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                        "HTST: the transition configuration is not a saddle point, its lowest eigenvalue is above the threshold ({} > {})!", lowest_evalue, -epsilon ));
                    return;
                }
                // Check if second-lowest eigenvalue < 0 (higher-order SP)
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Check if higher order saddle point...");
                int n_negative = 0;
                for( int i=0; i < evalues_sp.size(); ++i )
                    if( evalues_sp[i] < -epsilon )
                        ++n_negative;

                if( n_negative > 1 )
                {
                    Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                        "HTST: the image you passed is a higher order saddle point (N={})!", n_negative ));
                    return;
                }

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Sparse LU Decomposition of geodesic Hessian...");
                Eigen::SparseLU<SpMatrixX, Eigen::COLAMDOrdering<int> > solver;
                solver.analyzePattern(sparse_hessian_sp_geodesic_2N);
                solver.factorize(sparse_hessian_sp_geodesic_2N);
                htst_info.det_sp = solver.logAbsDeterminant() - std::log(-lowest_evalue);

                // Perpendicular velocity
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Calculate dynamical contribution");

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate the dynamical matrix");
                SpMatrixX velocity(3*nos, 3*nos);
                Sparse_Calculate_Dynamical_Matrix(image_sp, htst_info.saddle_point->geometry->mu_s, sparse_hessian_sp_geodesic_3N, velocity);
                SpMatrixX projected_velocity = tangent_basis.transpose() * velocity * tangent_basis;

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Solving H^-1 V q_1 ...");
                VectorX x(2*nos);
                x = solver.solve(projected_velocity.transpose() * lowest_evector);
                htst_info.s = std::sqrt(lowest_evector.transpose() * projected_velocity * x );

                // Checking for zero modes at the saddle point...
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking for zero modes at the saddle point...");
                for( int i=0; i < evalues_sp.size(); ++i )
                {
                    if( std::abs( evalues_sp[i] ) <= epsilon)
                        ++n_zero_modes_sp;
                }
                // Deal with zero modes if any (calculate volume)
                htst_info.volume_sp = 1;
                if( n_zero_modes_sp > 0 )
                {
                    Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("ZERO MODES AT SADDLE POINT (N={})", n_zero_modes_sp));
                    htst_info.volume_sp = HTST::Calculate_Zero_Volume(htst_info.saddle_point);
                }
            }

            // TODO  // End saddle point
            ////////////////////////////////////////////////////////////////////////

            ////////////////////////////////////////////////////////////////////////
            // Initial state minimum
            int n_zero_modes_minimum = 0;
            scalarfield evalues_min = scalarfield(0);
            {
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculation for the Minimum");

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate tangent basis ...");
                SpMatrixX tangent_basis = SpMatrixX(3*nos, 2*nos);
                Manifoldmath::sparse_tangent_basis_spherical(image_minimum, tangent_basis);

                // Evaluation of the Hessian...
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate the Hessian...");
                SpMatrixX sparse_hessian_minimum = SpMatrixX(3*nos,3*nos);
                htst_info.minimum->hamiltonian->Sparse_Hessian(image_minimum, sparse_hessian_minimum);

                // Transform into geodesic Hessian
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Transforming Hessian into geodesic Hessian...");
                SpMatrixX sparse_hessian_geodesic_min_3N = SpMatrixX(3*nos, 3*nos);
                sparse_hessian_bordered_3N(image_minimum, gradient_minimum, sparse_hessian_minimum, sparse_hessian_geodesic_min_3N);
                SpMatrixX sparse_hessian_geodesic_min_2N = tangent_basis.transpose() * sparse_hessian_geodesic_min_3N * tangent_basis;

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Sparse LU Decomposition of geodesic Hessian...");
                Eigen::SparseLU<SpMatrixX, Eigen::COLAMDOrdering<int> > solver;
                solver.analyzePattern(sparse_hessian_geodesic_min_2N);
                solver.factorize(sparse_hessian_geodesic_min_2N);
                htst_info.det_min = solver.logAbsDeterminant();

                // Calculate modes at minimum (needed for zero-mode volume)
                std::vector<VectorX> evecs_min = std::vector<VectorX>(0);
                Sparse_Get_Lowest_Eigenvectors_VP(sparse_hessian_geodesic_min_2N, epsilon, evalues_min, evecs_min);

                // Checking for zero modes at the minimum..
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking for zero modes at the minimum ...");
                for( int i=0; i < evalues_min.size(); ++i )
                {
                    if( std::abs( evalues_min[i] ) <= epsilon)
                        ++n_zero_modes_minimum;
                    if( evalues_min[i] < 0 )
                    {
                        Log(Utility::Log_Level::Warning, Utility::Log_Sender::HTST, fmt::format("    Minimum has a negative mode with eigenvalue = {}!", evalues_min[i])); // The Question is if we should terminate the calculation here or allow to continue since often the negatives cancel sqrt(-x) * sqrt(-x) = sqrt(x^2)
                    }
                }
                // Deal with zero modes if any (calculate volume)
                htst_info.volume_min = 1;
                if( n_zero_modes_minimum > 0 )
                {
                    Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("ZERO MODES AT MINIMUM (N={})", n_zero_modes_minimum));
                    htst_info.volume_min = HTST::Calculate_Zero_Volume(htst_info.minimum);
                }
            }
            // End initial state minimum
            ////////////////////////////////////////////////////////////////////////

            ////////////////////////////////////////////////////////////////////////
            // Calculation of the prefactor...
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculating prefactor...");

            // Calculate the exponent for the temperature-dependence of the prefactor
            //      The exponent depends on the number of zero modes at the different states
            htst_info.temperature_exponent = 0.5 * (n_zero_modes_minimum - n_zero_modes_sp);

            // Calculate "me"
            htst_info.me = std::pow(2*C::Pi * C::k_B, htst_info.temperature_exponent);

            // Calculate Omega_0, i.e. the entropy contribution
            htst_info.Omega_0 = std::sqrt(std::exp(htst_info.det_min - htst_info.det_sp));

            scalar zero_mode_factor = 1;
            for (int i=0; i<n_zero_modes_minimum; i++)
                zero_mode_factor /= evalues_min[i];

            for (int i=0; i<n_zero_modes_sp; i++)
                zero_mode_factor *= evalues_sp[i+1];

            zero_mode_factor = std::sqrt(zero_mode_factor);

            htst_info.Omega_0 *= zero_mode_factor;

            // Calculate the prefactor
            htst_info.prefactor_dynamical = htst_info.me * htst_info.volume_sp / htst_info.volume_min * htst_info.s;
            htst_info.prefactor = C::g_e / (C::hbar * 1e-12) * htst_info.Omega_0 * htst_info.prefactor_dynamical / ( 2*C::Pi );

            Log.SendBlock(Utility::Log_Level::All, Utility::Log_Sender::HTST,
                {
                    "---- Prefactor calculation successful!",
                    fmt::format("exponent      = {:^20e}", htst_info.temperature_exponent),
                    fmt::format("me            = {:^20e}", htst_info.me),
                    fmt::format("m = Omega_0   = {:^20e}", htst_info.Omega_0),
                    fmt::format("s             = {:^20e}", htst_info.s),
                    fmt::format("volume_sp     = {:^20e}", htst_info.volume_sp),
                    fmt::format("volume_min    = {:^20e}", htst_info.volume_min),
                    fmt::format("log |det_min| = {:^20e}", htst_info.det_min),
                    fmt::format("log |det_sp|  = {:^20e}", htst_info.det_sp),
                    fmt::format("0-mode factor = {:^20e}", zero_mode_factor),
                    fmt::format("hbar[meV*s]   = {:^20e}", C::hbar*1e-12),
                    fmt::format("v = dynamical prefactor = {:^20e}", htst_info.prefactor_dynamical),
                    fmt::format("prefactor               = {:^20e}", htst_info.prefactor)
                }, -1, -1);
        }

        void Sparse_Calculate_Dynamical_Matrix(const vectorfield & spins, const scalarfield & mu_s, const SpMatrixX & hessian, SpMatrixX & velocity)
        {
            constexpr scalar epsilon = 1e-10;
            int nos = spins.size();

            typedef Eigen::Triplet<scalar> T;
            std::vector<T> tripletList;
            tripletList.reserve(hessian.nonZeros());

            auto levi_civita = [](int i, int j, int k)
            {
                return -0.5 * (j-i) * (k-j) * (i-k);
            };

            // We first compute the effective field temporary
            auto b_eff = vectorfield(nos, {0,0,0});
            for (int k=0; k<hessian.outerSize(); ++k)
            {
                for (SpMatrixX::InnerIterator it(hessian,k); it; ++it)
                {
                    int row = it.row(), col = it.col();
                    scalar h = it.value();

                    for(int nu=0; nu<3; nu++)
                    {
                        for(int gamma=0; gamma<3; gamma++)
                        {
                            if( (row-nu) % 3 != 0 || (col-gamma) % 3 != 0 || nu>row || gamma>col)
                                continue;
                            int i = (row-nu)/3.0;
                            int j = (col-gamma)/3.0;
                            b_eff[i][nu] += h * spins[j][gamma] / mu_s[i];
                        }
                    }
                }
            }

            // Add the contributions from the effective field
            for(int i=0; i<nos; i++)
            {
                for(int alpha=0; alpha<3; alpha++)
                {
                    for(int beta=0; beta<3; beta++)
                    {
                        for(int nu=0; nu<3; nu++)
                        {
                            scalar res = levi_civita(alpha, beta, nu) * b_eff[i][nu];
                            if(std::abs(res) > epsilon)
                                tripletList.push_back( T(3*i+alpha, 3*i+beta, res) );
                        }
                    }
                }
            }

            // Iterate over non zero entries of hessian
            for (int k=0; k<hessian.outerSize(); ++k)
            {
                for (SpMatrixX::InnerIterator it(hessian,k); it; ++it)
                {
                    int row = it.row(), col = it.col();
                    scalar h = it.value();

                    for( int mu = 0; mu < 3; mu++ )
                    {
                        for( int nu = 0; nu < 3; nu++ )
                        {
                            for( int alpha = 0; alpha < 3; alpha++ )
                            {
                                for( int beta = 0; beta < 3; beta++ )
                                {
                                    if( (row-nu) % 3 != 0 || (col-beta) % 3 != 0 || nu>row || beta>col )
                                        continue;

                                    int i = (row-nu)/3.0;
                                    int j = (col-beta)/3.0;
                                    scalar res = levi_civita(alpha, mu, nu) * spins[i][mu] * h / mu_s[i];
                                    tripletList.push_back( T(3*i+alpha, 3*j+beta, res) );
                                }
                            }
                        }
                    }
                }
            }

            velocity.setFromTriplets(tripletList.begin(), tripletList.end());
        }

        void sparse_hessian_bordered_3N(const vectorfield & image, const vectorfield & gradient, const SpMatrixX & hessian, SpMatrixX & hessian_out)
        {
            // Calculates a 3Nx3N matrix in the bordered Hessian approach and transforms it into the tangent basis,
            // making the result a 2Nx2N matrix. The bordered Hessian's Lagrange multipliers assume a local extremum.

            int nos = image.size();
            VectorX lambda(nos);
            for (int i=0; i<nos; ++i)
                lambda[i] = image[i].normalized().dot(gradient[i]);

            // Construct hessian_out
            typedef Eigen::Triplet<scalar> T;
            std::vector<T> tripletList;
            tripletList.reserve( hessian.nonZeros() + 3*nos );

            // Iterate over non zero entries of hesiian
            for (int k=0; k<hessian.outerSize(); ++k)
            {
                for (SpMatrixX::InnerIterator it(hessian,k); it; ++it)
                {
                    tripletList.push_back( T(it.row(), it.col(), it.value() ) );
                }
                int j = k % 3;
                int i = (k - j) / 3;
                tripletList.push_back( T(k,k, -lambda[i]) ); // Correction to the diagonal
            }
            hessian_out.setFromTriplets(tripletList.begin(), tripletList.end());
        }

        // NOTE WE ASSUME A SELFADJOINT MATRIX
        void Sparse_Eigen_Decomposition(const SpMatrixX & matrix, VectorX & evalues, MatrixX & evectors)
        {
            // Create a Spectra solver
            Eigen::SelfAdjointEigenSolver<SpMatrixX> matrix_solver(matrix);
            evalues = matrix_solver.eigenvalues().real();
            evectors = matrix_solver.eigenvectors().real();
        }

        void Sparse_Geodesic_Eigen_Decomposition(const vectorfield & image, const vectorfield & gradient, const SpMatrixX & hessian,
            SpMatrixX & hessian_geodesic_3N, SpMatrixX & hessian_geodesic_2N, SpMatrixX & tangent_basis, VectorX & eigenvalues, MatrixX & eigenvectors)
        {
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "---------- Sparse Geodesic Eigen Decomposition");

            int nos = image.size();

            // Calculate geodesic Hessian in 3N-representation
            hessian_geodesic_3N = SpMatrixX(3*nos, 3*nos);
            sparse_hessian_bordered_3N(image, gradient, hessian, hessian_geodesic_3N);

            // Transform into geodesic Hessian
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Transforming Hessian into geodesic Hessian...");
            hessian_geodesic_2N = SpMatrixX(2*nos, 2*nos);
            hessian_geodesic_2N = tangent_basis.transpose() * hessian_geodesic_3N * tangent_basis;

            // Calculate full eigenspectrum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Calculation of full eigenspectrum..." );

            eigenvalues = VectorX::Zero(2*nos);
            eigenvectors = MatrixX::Zero(2*nos, 2*nos);

            Sparse_Eigen_Decomposition(hessian_geodesic_2N, eigenvalues, eigenvectors);

            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "---------- Sparse Geodesic Eigen Decomposition Done");
        }

    }
}
#endif


// std includes
#include <vector>
#include <iostream>
#include <random> // random_device, default_random_engine, uniform_real_distribution
#include <memory> // shared_ptr
#include <cmath> // sin, cos
// thirdparty includes
#include <Eigen/Dense>
// lib includes
#include "m0sh/uniform.h"
#include "m0sh/structured_sub.h"
#include "p0l/interpolation.h"
#include "v0l/bin/file_data.h"

using TypeScalar = double;
// Space
const unsigned int DIM = 3;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
// Mesh
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMeshStructured = m0sh::Structured<TypeVector, TypeRef, TypeContainer>;
using TypeMeshStructuredSub = m0sh::StructuredSub<TypeVector, TypeRef, TypeContainer>;
using TypeMeshStructuredUniform = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
// Data
const std::size_t order = 4; // interpolation order

double f(const double x, const double y) {
    return std::cos(x) + std::sin(y);
}

int main() {
    // Init
    v0l::FileData<float> data("../data/v.vtk", 0);
    // build length
    std::vector<double> lengths(data.meta.spacing.size());
    TypeVector origin;
    for(unsigned int i = 0; i < lengths.size(); i++) {
        lengths[i] = data.meta.spacing[i] * data.meta.dimensions[i];
        origin[i] = data.meta.origin[i];
    }
    // build create mesh
    std::shared_ptr<TypeMeshStructured> sMesh = std::make_shared<TypeMeshStructuredUniform>(data.meta.dimensions, lengths, origin, TypeContainer<bool>(DIM, true));
    // interpolate
    std::cout << "interpolated value: " << p0l::lagrangeMeshCell<TypeMeshStructured, v0l::FileData, float, TypeVector, TypeRef, TypeMeshStructuredSub>(sMesh, data, TypeVector::Random() * 0.5, order + 1) << std::endl;
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/range.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <deque>
#include <iostream>
#include <list>
#include <string>
#include <type_traits>
#include <vector>

// Couldn't find the actual example used in the text,
// but the one from chapter 1 will do just fine it seems :-).
// Mostly code serrouding the dfs visitor, which is a neat idea itself,
// and a large part of chapter 3 of the boost::graph book.

using graph_type = boost::adjacency_list<boost::listS, boost::vecS, boost::directedS>;

typedef boost::graph_traits<graph_type>::vertex_descriptor vertex_t;
typedef boost::graph_traits<graph_type>::edge_descriptor edge_t;
// white means undiscovered, gray means discovered but still searching descendants, and black
// means the vertex and all of its descendants have been discovered.
enum class color_type { white,
    gray,
    black };

bool has_cycle_dfs(
    const graph_type& g,
    vertex_t u,
    color_type* color)
{
    color[u] = color_type::gray;
    boost::graph_traits<graph_type>::adjacency_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = boost::adjacent_vertices(u, g); vi != vi_end; ++vi) {
        if (color[*vi] == color_type::white) {
            if (has_cycle_dfs(g, *vi, color)) {
                return true;
            } else if (color[*vi] == color_type::gray) {
                return true;
            }
        }
    }
    color[u] = color_type::black;
    return false;
}

bool has_cycle(const graph_type& g)
{
    std::vector<color_type> color(
        boost::num_vertices(g),
        color_type::white);

    boost::graph_traits<graph_type>::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
        if (color[*vi] == color_type::white) {
            if (has_cycle_dfs(g, *vi, &color[0])) {
                return true;
            }
        }
    }

    return false;
}

template <typename Visitor>
void dfs_v1(const graph_type& g, vertex_t u, color_type* color, Visitor vis)
{
    color[u] = color_type::gray;
    vis.discover_vertex(u, g);

    boost::graph_traits<graph_type>::out_edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ++ei) {
        // boost::target/ boost::source -> get start/end edge
        if (color[boost::target(*ei, g)] == color_type::white) {
            vis.tree_edge(*ei, g);
            dfs_v1(g, target(*ei, g), color, vis);
        } else if (color[target(*ei, g)] == color_type::gray) {
            vis.back_edge(*ei, g);
        } else {
            vis.forward_or_cross_edge(*ei, g);
        }
    }

    color[u] = color_type::black;
    vis.finish_vertex(u, g);
}

template <typename Visitor>
void generic_dfs_v1(const graph_type& g, Visitor vis)
{
    std::vector<color_type> color(num_vertices(g), color_type::white);
    boost::graph_traits<graph_type>::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
        if (color[*vi] == color_type::white) {
            dfs_v1(g, *vi, &color[0], vis);
        }
    }
}

struct default_dfs_visitor {
    template <typename V, typename G>
    void discover_vertex(V, const G&) { }

    template <typename E, typename G>
    void tree_edge(E, const G&) { }

    template <typename E, typename G>
    void back_edge(E, const G&) { }

    template <typename E, typename G>
    void forward_or_cross_edge(E, const G&) { }

    template <typename V, typename G>
    void finish_vertex(V, const G&) { }
};

struct topo_visitor : public default_dfs_visitor {
    topo_visitor(vertex_t*& order)
        : topo_order(order)
    {
    }

    void finish_vertex(vertex_t u, const graph_type&)
    {
        *--topo_order = u;
    }

    vertex_t*& topo_order;
};

void topo_sort(const graph_type& g, vertex_t* topo_order)
{
    topo_visitor vis(topo_order);
    generic_dfs_v1(g, vis);
}

struct cycle_detector : public default_dfs_visitor{
    cycle_detector(bool& cycle) : has_cycle(cycle) {}
    void back_edge (edge_t, const graph_type&)
    {
        has_cycle = true;
    }

    bool& has_cycle;
};

bool has_cycle_vis(const graph_type& g)
{
    bool has_cycle = true;
    cycle_detector vis(has_cycle);
    generic_dfs_v1(g, vis);
    return has_cycle;
}

int main()
{
    const char* tasks[] = {
        "pick up kids from school",
        "buy groceries (and snack)",
        "get cash at ATM",
        "drop off kids at soccer practice",
        "cook dinner",
        "pick up kids from soccer",
        "eat dinner"
    };

    const int n_tasks = sizeof(tasks) / sizeof(char*);

    graph_type g(n_tasks);
    add_edge(0, 3, g);
    add_edge(1, 3, g);
    add_edge(1, 4, g);
    add_edge(2, 1, g);
    add_edge(3, 5, g);
    add_edge(4, 6, g);
    add_edge(5, 6, g);

    std::cout << "cycle detection: " << (has_cycle(g) ? "true" : "false") << std::endl;
    //std::deque<int> topo_order;

    //topological_sort(g, std::front_inserter(topo_order),
    //        vertex_index_map(identity_property_map()));

    //for(const auto& [index, i] : topo_order
    //        | boost::adaptors::indexed())
    //{
    //    std::cout << tasks[i.head] << std::endl;
    //}

    return 0;
}


//  Copyright (c) 2014 Anton Bikineev
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  Computes test data for the derivatives of the
//  various bessel functions. v and x parameters are taken
//  from bessel_*_data.ipp files. Results of derivatives
//  are generated by the relations between the derivatives
//  and Bessel functions, which actual implementation
//  doesn't use. Results are printed to ~ 50 digits.
//
#include <fstream>
#include <utility>
#include <map>
#include <iterator>
#include <algorithm>

#include <boost/multiprecision/mpfr.hpp>

#include <boost/math/special_functions/bessel.hpp>

template <class T>
T bessel_j_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_j(v, x) - boost::math::cyl_bessel_j(v+1, x);
}

template <class T>
T bessel_y_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_neumann(v, x) - boost::math::cyl_neumann(v+1, x);
}

template <class T>
T bessel_i_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_i(v, x) + boost::math::cyl_bessel_i(v+1, x);
}

template <class T>
T bessel_k_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_k(v, x) - boost::math::cyl_bessel_k(v+1, x);
}

template <class T>
T sph_bessel_j_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_bessel(1, x);
   return boost::math::sph_bessel(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_bessel(itrunc(v), x);
}

template <class T>
T sph_bessel_y_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_neumann(1, x);
   return boost::math::sph_neumann(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_neumann(itrunc(v), x);
}

using FloatType = boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<200u> >;

enum class BesselFamily: char
{
   J = 0,
   Y,
   I,
   K,
   j,
   y
};

namespace
{

const unsigned kSignificand = 50u;

const std::map<BesselFamily, std::vector<std::string> > kSourceFiles = {
   {BesselFamily::J, {"bessel_j_data.ipp", "bessel_j_int_data.ipp", "bessel_j_large_data.ipp"}},
   {BesselFamily::Y, {"bessel_y01_data.ipp", "bessel_yn_data.ipp", "bessel_yv_data.ipp"}},
   {BesselFamily::I, {"bessel_i_data.ipp", "bessel_i_int_data.ipp"}},
   {BesselFamily::K, {"bessel_k_data.ipp", "bessel_k_int_data.ipp"}},
   {BesselFamily::j, {"sph_bessel_data.ipp"}},
   {BesselFamily::y, {"sph_neumann_data.ipp"}}
};

FloatType (*fp)(FloatType, FloatType) = ::bessel_j_derivative_bare;

std::string parseValue(std::string::iterator& iter)
{
   using std::isdigit;

   auto value = std::string{};

   while (!isdigit(*iter) && *iter != '-')
      ++iter;
   while (isdigit(*iter) || *iter == '.' || *iter == 'e' || *iter == '-' || *iter == '+')
   {
      value.push_back(*iter);
      ++iter;
   }
   return value;
}

void replaceResultInLine(std::string& line)
{
   using std::isdigit;

   auto iter = line.begin();

   // parse v and x values from line and convert them to FloatType
   auto v = FloatType{::parseValue(iter)};
   auto x = FloatType{::parseValue(iter)};
   auto result = fp(v, x).str(kSignificand);

   while (!isdigit(*iter) && *iter != '-')
      ++iter;
   const auto where_to_write = iter;
   while (isdigit(*iter) || *iter == '.' || *iter == 'e' || *iter == '-' || *iter == '+')
      line.erase(iter);

   line.insert(where_to_write, result.begin(), result.end());
}

void generateResultFile(const std::string& i_file, const std::string& o_file)
{
   std::ifstream in{i_file.c_str()};
   std::ofstream out{o_file.c_str()};

   auto line = std::string{};
   while (!in.eof())
   {
      std::getline(in, line);
      if (__builtin_expect(line.find("SC_") != std::string::npos, 1))
         ::replaceResultInLine(line);
      out << line << std::endl;
   }
}

void processFiles(BesselFamily family)
{
   const auto& family_files = kSourceFiles.find(family)->second;

   std::for_each(std::begin(family_files), std::end(family_files),
      [&](const std::string& src){
         auto new_file = src;

         const auto int_pos = new_file.find("int");
         const auto large_pos = new_file.find("large");
         const auto data_pos = new_file.find("data");
         const auto derivative_pos = (int_pos == std::string::npos ?
            (large_pos == std::string::npos ? data_pos : large_pos) :
            int_pos);

         new_file.insert(derivative_pos, "derivative_");

         ::generateResultFile(src, new_file);
      });
}

} // namespace

int main(int argc, char*argv [])
{
   auto functype = BesselFamily::J;
   auto letter = std::string{"J"};

   if(argc == 2)
   {
      if(std::strcmp(argv[1], "--Y") == 0)
      {
         functype = BesselFamily::Y;
         fp = ::bessel_y_derivative_bare;
         letter = "Y";
      }
      else if(std::strcmp(argv[1], "--I") == 0)
      {
         functype = BesselFamily::I;
         fp = ::bessel_i_derivative_bare;
         letter = "I";
      }
      else if(std::strcmp(argv[1], "--K") == 0)
      {
         functype = BesselFamily::K;
         fp = ::bessel_k_derivative_bare;
         letter = "K";
      }
      else if(std::strcmp(argv[1], "--j") == 0)
      {
         functype = BesselFamily::j;
         fp = ::sph_bessel_j_derivative_bare;
         letter = "j";
      }
      else if(std::strcmp(argv[1], "--y") == 0)
      {
         functype = BesselFamily::y;
         fp = ::sph_bessel_y_derivative_bare;
         letter = "y";
      }
      else
         assert(0);
   }

   ::processFiles(functype);

   return 0;
}


/*
 * point_kinematics.hpp
 *
 * Created on: Oct 24, 2018 23:05
 * Description: (reduced) point kinematics model, used for lattice generation
 *
 * Reference:
 *  [1] M. McNaughton and C. Urmson and J. M. Dolan and J. W. Lee. 2011.
 *    “Motion Planning for Autonomous Driving with a Conformal Spatiotemporal
 * Lattice.” In 2011 IEEE International Conference on Robotics and Automation,
 * 4889–95.
 *
 * Copyright (c) 2018 Ruixiang Du (rdu)
 */

#ifndef POINT_KINEMATICS_HPP
#define POINT_KINEMATICS_HPP

#include <iostream>

#include <eigen3/Eigen/Dense>

#include <boost/numeric/odeint.hpp>

#include "state_lattice/details/motion_state.hpp"

namespace robosw {
class PointKinematics {
 public:
  struct Param {
    Param(double _p0 = 0, double _p1 = 0, double _p2 = 0, double _p3 = 0,
          double _sf = 0)
        : p0(_p0), p1(_p1), p2(_p2), p3(_p3), sf(_sf) {}

    double p0;
    double p1;
    double p2;
    double p3;
    double sf;

    friend std::ostream &operator<<(std::ostream &os, const Param &p) {
      os << "(p0,p1,p2,p3,sf): " << p.p0 << " , " << p.p1 << " , " << p.p2
         << " , " << p.p3 << " , " << p.sf;
      return os;
    }
  };

  using state_type = std::vector<double>;

  /////////////////////////////////////////////////////

 public:
  PointKinematics() = default;
  PointKinematics(double a, double b, double c, double d)
      : a_(a), b_(b), c_(c), d_(d){};

  // calculate intermediate parameters
  void SetParameters(const Param &p);

  // propagate system model
  MotionState Propagate(const MotionState &init, const Param &p,
                        double ds = 0.1);
  StatePMatrix PropagateP(const StatePMatrix &init, const Param &p,
                          double ds = 0.1);

  // The following functions could be called externally ONLY when a_,b_,c_,d_
  // have been set properly by
  //  (1) construct the model with: PointKinematics(double a, double b, double
  //  c, double d) (2) default construct and then use SetParameters() to set
  //  parameters
  inline MotionState Propagate(const MotionState &init, double sf, double ds) {
    {
      double sf_squared = sf * sf;

      // theta_p and kappa_p could be calculated analytically
      double theta_p = a_ * sf + b_ * sf_squared / 2.0 +
                       c_ * sf_squared * sf / 3.0 +
                       d_ * sf_squared * sf_squared / 4.0;
      double kappa_p = a_ + b_ * sf + c_ * sf_squared + d_ * sf_squared * sf;

      // calculate x_p, y_p numerically
      double s = 0;
      state_type x = {init.x, init.y};
      while (s <= sf) {
        // integrator_(*this, x, s, ds);
        boost::numeric::odeint::integrate_const(
            boost::numeric::odeint::runge_kutta4<state_type>(), *this, x, s,
            s + ds, ds / 10.0);
      }

      return MotionState(x[0], x[1], theta_p, kappa_p);
    }
  }

  std::vector<MotionState> GenerateTrajectoryPoints(const MotionState &init,
                                                    double sf, double step,
                                                    double ds = 0.1);

  // Shall not be called by user, used for propagation by RK4 integrator class
  void operator()(const state_type &x, state_type &xd, const double s);

 private:
  double a_;
  double b_;
  double c_;
  double d_;

  inline void CalculateIntermediateParams(const Param &p);
};
}  // namespace robosw

#endif /* POINT_KINEMATICS_HPP */


#include <state_estimation/filters/ekf.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

void EKF::myPredict(const Eigen::VectorXd& u, double dt) {
    system_model_->update(filter_state_.x, u, dt);

    // Update the state and covariance
    filter_state_.x = system_model_->g();
    filter_state_.covariance =
        system_model_->G() * filter_state_.covariance * system_model_->G().transpose() +
        system_model_->P() * system_model_->Rp() * system_model_->P().transpose() +
        system_model_->V() * system_model_->Rc() * system_model_->V().transpose();

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "EKF predicition update:" << std::endl
              << "g=" << printMatrix(system_model_->g()) << std::endl
              << "G=" << std::endl
              << printMatrix(system_model_->G()) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void EKF::myCorrect(const Eigen::VectorXd& z, measurement_models::NonlinearMeasurementModel* model,
                    double dt) {
    // Update our measurement model
    model->update(filter_state_.x, dt);

    // Compute the Kalman gain
    const Eigen::MatrixXd cov_H_T = filter_state_.covariance * model->H().transpose();
    const Eigen::MatrixXd K = cov_H_T * (model->H() * cov_H_T + model->covariance()).inverse();

    // Update the state and covariance with the measurement
    const Eigen::MatrixXd I =
        Eigen::MatrixXd::Identity(filter_state_.x.rows(), filter_state_.x.rows());
    const Eigen::VectorXd dx = K * model->subtractVectors(z, model->h());
    filter_state_.x = system_model_->addVectors(filter_state_.x, dx);
    filter_state_.covariance = (I - K * model->H()) * filter_state_.covariance;

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "EKF measurement update:" << std::endl
              << "h=" << printMatrix(model->h()) << std::endl
              << "H=" << std::endl
              << printMatrix(model->H()) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "Innovation=" << printMatrix(dx) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation


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

 * 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    Rot3.cpp
 * @brief   Rotation, common code between Rotation matrix and Quaternion
 * @author  Alireza Fathi
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @author  Richard Roberts
 */

#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>

using namespace std;

namespace gtsam {

static const Matrix3 I3 = Matrix3::Identity();

/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
  gtsam::print((Matrix)matrix(), s);
}

/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
  return rodriguez((Vector)w.vector(),theta);
}

/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Unit3& w, double theta) {
  return rodriguez(w.point3(),theta);
}

/* ************************************************************************* */
Rot3 Rot3::Random(boost::mt19937 & rng) {
  // TODO allow any engine without including all of boost :-(
  Unit3 w = Unit3::Random(rng);
  boost::uniform_real<double> randomAngle(-M_PI,M_PI);
  double angle = randomAngle(rng);
  return rodriguez(w,angle);
}

/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
  double t = w.norm();
  if (t < 1e-10) return Rot3();
  return rodriguez(w/t, t);
}

/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
  return equal_with_abs_tol(matrix(), R.matrix(), tol);
}

/* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
  Unit3 q = Unit3(rotate(p.point3(Hp)));
  if (Hp)
    (*Hp) = q.basis().transpose() * matrix() * (*Hp);
  if (HR)
    (*HR) = -q.basis().transpose() * matrix() * p.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
  Unit3 q = Unit3(unrotate(p.point3(Hp)));
  if (Hp)
    (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
  if (HR)
    (*HR) = q.basis().transpose() * q.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::operator*(const Unit3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  Point3 q(transpose()*p.vector()); // q = Rt*p
  if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
  if (H2) *H2 = transpose();
  return q;
}

/* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpL(const Vector3& v) {
  if(zero(v)) return eye(3);
  Matrix x = skewSymmetric(v);
  Matrix x2 = x*x;
  double theta = v.norm(), vi = theta/2.0;
  double s1 = sin(vi)/vi;
  double s2 = (theta - sin(theta))/(theta*theta*theta);
  Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
  return res;
}

/* ************************************************************************* */
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpInvL(const Vector3& v) {
  if(zero(v)) return eye(3);
  Matrix x = skewSymmetric(v);
  Matrix x2 = x*x;
  double theta = v.norm(), vi = theta/2.0;
  double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
  Matrix res = eye(3) + 0.5*x - s2*x2;
  return res;
}

/* ************************************************************************* */
Point3 Rot3::column(int index) const{
  if(index == 3)
    return r3();
  else if(index == 2)
    return r2();
  else if(index == 1)
    return r1(); // default returns r1
  else
    throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}

/* ************************************************************************* */
Vector3 Rot3::xyz() const {
  Matrix I;Vector3 q;
  boost::tie(I,q)=RQ(matrix());
  return q;
}

/* ************************************************************************* */
Vector3 Rot3::ypr() const {
  Vector3 q = xyz();
  return Vector3(q(2),q(1),q(0));
}

/* ************************************************************************* */
Vector3 Rot3::rpy() const {
  return xyz();
}

/* ************************************************************************* */
Vector Rot3::quaternion() const {
  Quaternion q = toQuaternion();
  Vector v(4);
  v(0) = q.w();
  v(1) = q.x();
  v(2) = q.y();
  v(3) = q.z();
  return v;
}

/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x)    {
  // x is the axis-angle representation (exponential coordinates) for a rotation
  double normx = norm_2(x); // rotation angle
  Matrix3 Jr;
  if (normx < 10e-8){
    Jr = Matrix3::Identity();
  }
  else{
    const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
    Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
        ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
  }
  return Jr;
}

/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x)    {
  // x is the axis-angle representation (exponential coordinates) for a rotation
  double normx = norm_2(x); // rotation angle
  Matrix3 Jrinv;

  if (normx < 10e-8){
    Jrinv = Matrix3::Identity();
  }
  else{
    const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
    Jrinv = Matrix3::Identity() +
        0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx))   ) * X * X;
  }
  return Jrinv;
}

/* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) {

  double x = -atan2(-A(2, 1), A(2, 2));
  Rot3 Qx = Rot3::Rx(-x);
  Matrix3 B = A * Qx.matrix();

  double y = -atan2(B(2, 0), B(2, 2));
  Rot3 Qy = Rot3::Ry(-y);
  Matrix3 C = B * Qy.matrix();

  double z = -atan2(-C(1, 0), C(1, 1));
  Rot3 Qz = Rot3::Rz(-z);
  Matrix3 R = C * Qz.matrix();

  Vector xyz = Vector3(x, y, z);
  return make_pair(R, xyz);
}

/* ************************************************************************* */
ostream &operator<<(ostream &os, const Rot3& R) {
  os << "\n";
  os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
  os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
  os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
  return os;
}

/* ************************************************************************* */

} // namespace gtsam


// Petter Strandmark 2012.

#include <cstdio>
#include <iomanip>
#include <iostream>
#include <limits>
#include <memory>
#include <random>
#include <stdexcept>

// GNU 4.8.1 define _X on Cygwin.
// This breaks Eigen.
// http://eigen.tuxfamily.org/bz/process_bug.cgi
#ifdef _X
#undef _X
#endif
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include <spii/spii.h>
#include <spii/solver.h>

namespace spii {

void NewtonSolver::solve(const Function& function,
                         SolverResults* results) const
{
	double global_start_time = wall_time();

	// Random number engine for random pertubation.
	std::mt19937 prng(unsigned(1));
	std::uniform_real_distribution<double> uniform11(-1.0, 1.0);
	auto rand11 = std::bind(uniform11, prng);

	// Dimension of problem.
	size_t n = function.get_number_of_scalars();

	if (n == 0) {
		results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
		return;
	}

	// Determine whether to use sparse representation
	// and matrix factorization.
	bool use_sparsity;
	if (this->sparsity_mode == SparsityMode::DENSE) {
		use_sparsity = false;
	}
	else if (this->sparsity_mode == SparsityMode::SPARSE) {
		use_sparsity = true;
	}
	else {
		if (n <= 50) {
			use_sparsity = false;
		}
		else {
			use_sparsity = true;
		}
	}

	auto factorization_method = this->factorization_method;
	if (use_sparsity && this->factorization_method == FactorizationMethod::MESCHACH) {
		if (this->log_function) {
			this->log_function("Can not use the Meschach library for sparse problems. Switching to iterative factorization.");
		}
		factorization_method = FactorizationMethod::ITERATIVE;
	}

	// Current point, gradient and Hessian.
	double fval   = std::numeric_limits<double>::quiet_NaN();;
	double fprev  = std::numeric_limits<double>::quiet_NaN();
	double normg0 = std::numeric_limits<double>::quiet_NaN();
	double normg  = std::numeric_limits<double>::quiet_NaN();
	double normdx = std::numeric_limits<double>::quiet_NaN();

	Eigen::VectorXd x, g;
	Eigen::MatrixXd H;
	Eigen::SparseMatrix<double> sparse_H;
	if (use_sparsity) {
		// Create sparsity pattern for H.
		function.create_sparse_hessian(&sparse_H);
		if (this->log_function) {
			double nnz = double(sparse_H.nonZeros()) / double(n * n);
			char str[1024];
			std::sprintf(str, "H is %dx%d with %d (%.5f%%) non-zeroes.",
				sparse_H.rows(), sparse_H.cols(), sparse_H.nonZeros(), 100.0 * nnz);
			this->log_function(str);
		}
	}

	// Copy the user state to the current point.
	function.copy_user_to_global(&x);
	Eigen::VectorXd x2(n);

	// p will store the search direction.
	Eigen::VectorXd p(function.get_number_of_scalars());

	// Dense and sparse Cholesky factorizers.
	typedef Eigen::LLT<Eigen::MatrixXd> LLT;
	typedef Eigen::SimplicialLLT<Eigen::SparseMatrix<double> > SparseLLT;
	std::unique_ptr<LLT> factorization;
	std::unique_ptr<SparseLLT> sparse_factorization;
	if (!use_sparsity) {
		factorization.reset(new LLT(n));
	}
	else {
		sparse_factorization.reset(new SparseLLT);
		// The sparsity pattern of H is always the same. Therefore, it is enough
		// to analyze it once.
		sparse_factorization->analyzePattern(sparse_H);
	}

	FactorizationCache factorization_cache((int)n);
	CheckExitConditionsCache exit_condition_cache;

	//
	// START MAIN ITERATION
	//
	results->startup_time   += wall_time() - global_start_time;
	results->exit_condition = SolverResults::INTERNAL_ERROR;
	int iter = 0;
	while (true) {

		int log_interval = 1;
		if (iter > 30) {
			log_interval = 10;
		}
		if (iter > 200) {
			log_interval = 100;
		}
		if (iter > 2000) {
			log_interval = 1000;
		}

		//
		// Evaluate function and derivatives.
		//
		double start_time = wall_time();
		if (use_sparsity) {
			fval = function.evaluate(x, &g, &sparse_H);
		}
		else {
			fval = function.evaluate(x, &g, &H);
		}

		normg = std::max(g.maxCoeff(), -g.minCoeff());
		if (iter == 0) {
			normg0 = normg;
		}

		// Check for NaN.
		if (normg != normg) {
			results->exit_condition = SolverResults::FUNCTION_NAN;
			break;
		}

		results->function_evaluation_time += wall_time() - start_time;

		//
		// Test stopping criteriea
		//
		start_time = wall_time();
		if (this->check_exit_conditions(fval, fprev, normg,
			                            normg0, x.norm(), normdx,
			                            true, &exit_condition_cache, results)) {
			break;
		}
		if (iter >= this->maximum_iterations) {
			results->exit_condition = SolverResults::NO_CONVERGENCE;
			break;
		}
		if (this->callback_function) {
			CallbackInformation information;
			information.objective_value = fval;
			information.x = &x;
			information.g = &g;
			if (use_sparsity) {
				information.H_sparse = &sparse_H;
			}
			else {
				information.H_dense = &H;
			}

			if (!callback_function(information)) {
				results->exit_condition = SolverResults::USER_ABORT;
				break;
			}
		}
		results->stopping_criteria_time += wall_time() - start_time;

		int factorizations = 0;
		double tau = 0;
		double mindiag = 0;
		Eigen::VectorXd dH;
		if (use_sparsity) {
			dH = sparse_H.diagonal();
		}
		else {
			dH = H.diagonal();
		}
		mindiag = dH.minCoeff();

		if (factorization_method == FactorizationMethod::ITERATIVE) {
			//
			// Attempt repeated Cholesky factorization until the Hessian
			// becomes positive semidefinite.
			//
			//start_time = wall_time();
			double beta = 1.0;

			if (mindiag > 0) {
				tau = 0;
			}
			else {
				tau = -mindiag + beta;
			}
			while (true) {
				// Add tau*I to the Hessian.
				if (tau > 0) {
					for (size_t i = 0; i < n; ++i) {
						if (use_sparsity) {
							int ii = static_cast<int>(i);
							sparse_H.coeffRef(ii, ii) = dH(i) + tau;
						}
						else {
							H(i, i) = dH(i) + tau;
						}
					}
				}
				// Attempt Cholesky factorization.
				bool success;
				if (use_sparsity) {
					sparse_factorization->factorize(sparse_H);
					success = sparse_factorization->info() == Eigen::Success;
				}
				else {
					factorization->compute(H);
					success = factorization->info() == Eigen::Success;
				}
				factorizations++;
				// Check for success.
				if (success) {
					break;
				}
				tau = std::max(2*tau, beta);

				spii_assert(factorizations <= 100,
				            "Solver::solve: factorization failed.");
			}

			results->matrix_factorization_time += wall_time() - start_time;

			//
			// Solve linear system to obtain search direction.
			//
			start_time = wall_time();

			if (use_sparsity) {
				p = sparse_factorization->solve(-g);
			}
			else {
				p = factorization->solve(-g);
			}

			results->linear_solver_time += wall_time() - start_time;
		}
		else if (factorization_method == FactorizationMethod::MESCHACH) {
			spii_assert(!use_sparsity);

			// Performs a BKP block diagonal factorization, modifies it, and
			// solvers the linear system.
			this->BKP_dense(H, g, factorization_cache, &p, results);
			factorizations = 1;
		}
		else if (factorization_method == FactorizationMethod::SYM_ILDL) {
			factorizations = 1;
			if (use_sparsity) {
				this->BKP_sym_ildl(sparse_H, g, &p, results);
			}
			else {
				this->BKP_sym_ildl(H, g, &p, results);
			}
		}
		else {
			throw std::runtime_error("Unknown factorization method.");
		}

		//
		// Perform line search.
		//
		start_time = wall_time();
		double start_alpha = 1.0;
		double alpha = this->perform_linesearch(function, x, fval, g, p, &x2,
		                                        start_alpha);

		if (alpha <= 1e-15) {

			// Attempt a simple steepest descent instead.
			p = -g;
			alpha = this->perform_linesearch(function, x, fval, g, p, &x2,
			                                 1.0);
			if (alpha <= 0) {

				if (this->log_function) {
					this->log_function("Steepest descent step failed. Numerical problems?");
				}

				// This happens in really rare cases with numerical problems or
				// incorrectly defined objective functions. In the latter case,
				// there is not much to do. In the former case, randomly perturbing
				// x has been effective.
				for (size_t i = 0; i < n; ++i) {
					x[i] = x[i] + 1e-6 * rand11() * x[i];
				}

				alpha = 0;
			}
		}

		// Record length of this step.
		normdx = alpha * p.norm();
		// Update current point.
		x = x + alpha * p;

		results->backtracking_time += wall_time() - start_time;

		//
		// Log the results of this iteration.
		//
		start_time = wall_time();

		if (this->log_function && iter % log_interval == 0) {
			if (use_sparsity) {
				if (iter == 0) {
					this->log_function("Itr        f        max|g_i|   alpha    fac    tau   min(H_ii)");
				}
				this->log_function(
					to_string(std::setw(4), iter) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(6), std::setw(10), fval) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), normg) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), alpha) + " " +
					to_string(std::setw(3), factorizations) + "   " +
					to_string(std::scientific, std::setprecision(1), tau) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(2), mindiag)
					);
			}
			else {
				double detH = H.determinant();
				double normH = H.norm();

				if (iter == 0) {
					this->log_function("Itr        f        max|g_i|   ||H||     det(H)    min(H_ii)  alpha    fac");
				}

				this->log_function(
					to_string(std::setw(4), iter) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(6), std::setw(10), fval) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), normg) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), normH) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(3), std::setw(10), detH) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(2), mindiag) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), alpha) + " " +
					to_string(std::setw(3), factorizations)
					);
			}
		}
		results->log_time += wall_time() - start_time;

		fprev = fval;
		iter++;
	}

	function.copy_global_to_user(x);
	results->total_time += wall_time() - global_start_time;

	if (this->log_function) {
		char str[1024];
		std::sprintf(str, " end %+10.6e %.3e", fval, normg);
		this->log_function(str);
	}
}

}  // namespace spii