#include <vector>
#include <iostream>
#include <iomanip>

#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>

#include "tictoc.hpp"
#include "initial_homography_estimation.hpp"

using namespace std;
using namespace Eigen;

namespace fovis
{

#define dump(var) //(cerr<<" "#var<<" =[\n"<< setprecision (12)<<var<<"];"<<endl)
Eigen::ArrayXf InitialHomographyEstimator::flattenMatrix(Eigen::MatrixXf &m)
{
  return Eigen::Map<Eigen::ArrayXf>(m.data(), m.rows() * m.cols());
}

static void
grayToEigen(const uint8_t * grayData, int width, int height, int stride,
    int downsampleFactor, Eigen::MatrixXf* result)
{
  Eigen::MatrixXf& eig_imf = *result;
  if (downsampleFactor > 0) {
    int cols = width >> downsampleFactor;
    int rows = height >> downsampleFactor;
    eig_imf = Eigen::MatrixXf::Zero(rows, cols);
    for (int y = 0; y < height; y++) {
      int ey = y >> downsampleFactor;
      for (int x = 0; x < width; x++) {
        int ex = x >> downsampleFactor;
        eig_imf(ey, ex) += grayData[y * stride + x];
      }
    }
    double pixelFactor = (1 << downsampleFactor);
    eig_imf /= pixelFactor * pixelFactor;
  }
  else {
    eig_imf.resize(height, width);
    const uint8_t* row_start = grayData;
    for(int row=0; row<height; row++) {
      for(int col=0; col<width; col++) {
        eig_imf(row, col) = row_start[col];
      }
      row_start += stride;
    }
  }
}

void InitialHomographyEstimator::setTestImage(const uint8_t * grayData, int width, int height, int stride, int downsampleFactor)
{
  grayToEigen(grayData, width, height, stride, downsampleFactor, &testImage);
}

void InitialHomographyEstimator::setTemplateImage(const uint8_t * grayData, int width, int height, int stride,
    int downsampleFactor)
{
  grayToEigen(grayData, width, height, stride, downsampleFactor, &templateImage);
  template_rows = templateImage.rows();
  template_cols = templateImage.cols();

  //compute template gradients
  Eigen::MatrixXf templateDx, templateDy;
  computeGradient(templateImage, &templateDx, &templateDy);

  templateDxRow = flattenMatrix(templateDx);
  templateDyRow = flattenMatrix(templateDy);

  //setup the utility matrices
  Eigen::MatrixXf x = VectorXf::LinSpaced(template_cols, 0, template_cols - 1).transpose().replicate(template_rows, 1);
  Eigen::MatrixXf y = VectorXf::LinSpaced(template_rows, 0, template_rows - 1).replicate(1, template_cols);
  xx = flattenMatrix(x);
  yy = flattenMatrix(y);

  templatePoints.resize(3, xx.rows());
  templatePoints.row(0) = xx;
  templatePoints.row(1) = yy;
  templatePoints.row(2).setOnes();

  xx += 1;
  yy += 1;

}

double InitialHomographyEstimator::computeError(const Eigen::MatrixXf &error)
{
  return error.norm() / sqrt((double) error.rows() * error.cols());
}

Eigen::Matrix3f InitialHomographyEstimator::track(const Eigen::Matrix3f & initH, int nIters, double * finalRMS)
{

  double minError = INFINITY;
  Eigen::Matrix3f H = initH;
  Eigen::Matrix3f bestH = H;
  int bestIter = 0;
  int lastImproved = 0;
  double lastRMS = INFINITY;

  for (int iter = 0; iter < nIters; iter++) {
    tictoc("track_iter");
    tictoc("warpPoints");
    Eigen::MatrixXf warpedHomogeneousPoints;
    warpedHomogeneousPoints = H * templatePoints;
    tictoc("warpPoints");

    tictoc("constructWarpedImage");
    warpedTestImage = constructWarpedImage(testImage, warpedHomogeneousPoints);
    tictoc("constructWarpedImage");

    errorIm = warpedTestImage - templateImage;
    Eigen::VectorXf errorRow;
    errorRow = flattenMatrix(errorIm);

    double rmsError = computeError(errorRow);

    if (rmsError < minError) {
      minError = rmsError;
      bestH = H;
      bestIter = iter;
    }

    tictoc("computeJacobian");
    tictoc("computeGradient");
    Eigen::MatrixXf warpedTestImageDx, warpedTestImageDy;
    computeGradient(warpedTestImage, &warpedTestImageDx, &warpedTestImageDy);
    tictoc("computeGradient");

    Eigen::ArrayXf warpedTestImageDxRow, warpedTestImageDyRow;
    warpedTestImageDxRow = flattenMatrix(warpedTestImageDx);
    warpedTestImageDyRow = flattenMatrix(warpedTestImageDy);

    Eigen::MatrixXf Jt = computeJacobian(templateDxRow + warpedTestImageDxRow, templateDyRow + warpedTestImageDyRow);
    tictoc("computeJacobian");

    //compute the psuedo-inverse
    tictoc("update");
    tictoc("svd_pinv");
    Eigen::JacobiSVD<MatrixXf> svd(Jt, ComputeThinU | ComputeThinV);
    Eigen::VectorXf sigma = svd.singularValues();
    Eigen::MatrixXf U = svd.matrixU();
    Eigen::MatrixXf V = svd.matrixV();
    int r = 0;
    for (r = 0; r < sigma.rows(); r++) { //singular values are in decreasing order
      if (sigma(r) < 1e-7) //TODO:better way to get the tolerance?
        break;
      else
        sigma(r) = 1.0 / sigma(r);
    }
    Eigen::MatrixXf Jt_plus;
    if (r == 0)
      Jt_plus = Eigen::MatrixXf::Zero(Jt.cols(), Jt.rows());
    else {
      Jt_plus = V.block(0, 0, V.rows(), r) * sigma.head(r).asDiagonal() * U.block(0, 0, U.rows(), r).transpose();
    }
    tictoc("svd_pinv");

    // this doesn't seem to work :-/
    //    tictoc("manual_pinv");
    //    Eigen::Matrix3f JtT_Jt = Jt.transpose() * Jt;
    //    Eigen::MatrixXf Jt_plus = JtT_Jt.inverse() * Jt;
    //    tictoc("manual_pinv");

    Eigen::VectorXf lie_d = -2 * Jt_plus * errorRow;
    tictoc("update");

    tictoc("lieToH");
    H = H * lieToH(lie_d);
    tictoc("lieToH");

    if (rmsError < lastRMS)
      lastImproved = iter;

    tictoc("track_iter");

    //        cout << iter << ") rmsError= " << rmsError << " minError = " << minError << " d.norm() =" << lie_d.norm() << endl;
    //    exit(1);
    if (lie_d.norm() < 1e-6 || (rmsError - minError > 3 && iter - bestIter > 2) || iter - bestIter > 4 || iter
        - lastImproved > 2) {
      //      printf("breaking after %d iters\n", iter);
      break;
    }
    lastRMS = rmsError;

  }
  if (finalRMS != NULL)
    *finalRMS = minError;
  return bestH;

}

void InitialHomographyEstimator::computeGradient(const Eigen::MatrixXf &image, Eigen::MatrixXf *dxp, Eigen::MatrixXf *dyp)
{
  Eigen::MatrixXf & dx = *dxp;
  Eigen::MatrixXf & dy = *dyp;
  dx = Eigen::MatrixXf::Zero(image.rows(), image.cols());
  dy = Eigen::MatrixXf::Zero(image.rows(), image.cols());

  dx.block(0, 1, dx.rows(), dx.cols() - 2) = image.block(0, 2, dx.rows(), dx.cols() - 2) - image.block(0, 0, dx.rows(),
      dx.cols() - 2);
  //handle border

  dy.block(1, 0, dy.rows() - 2, dy.cols()) = image.block(2, 0, dy.rows() - 2, dy.cols()) - image.block(0, 0, dy.rows()
      - 2, dy.cols());
  //normalize
  dx /= 2.0;
  dy /= 2.0;

  //handle borders
  dx.col(0) = image.col(1) - image.col(0);
  dx.col(image.cols() - 1) = image.col(image.cols() - 1) - image.col(image.cols() - 2);
  dy.row(0) = image.row(1) - image.row(0);
  dy.row(image.rows() - 1) = image.row(image.rows() - 1) - image.row(image.rows() - 2);

}

Eigen::MatrixXf InitialHomographyEstimator::computeJacobian(const Eigen::ArrayXf &dx, const Eigen::ArrayXf &dy) const
{
  Eigen::MatrixXf Jt(dx.rows(), 3);
  Jt.col(0) = dx;
  Jt.col(1) = dy;
  Jt.col(2) = dx * yy - dy * xx;
  return Jt;
}

Eigen::Matrix3f InitialHomographyEstimator::lieToH(const Eigen::VectorXf &lie) const
{
  //TODO: support more parameters?
  Eigen::Matrix3f M;
  M << 0, lie(2), lie(0),
      -lie(2), 0, lie(1),
       0, 0, 0;
  return M.exp();
}

Eigen::MatrixXf InitialHomographyEstimator::constructWarpedImage(const Eigen::MatrixXf &srcImage,
    const Eigen::MatrixXf &warpedPoints) const
{
  Eigen::MatrixXf warped = Eigen::MatrixXf(template_rows, template_cols);

  const double defaultValue = 128;
  //Bilinear interpolation
  for (int i = 0; i < warpedPoints.cols(); i++) {
    double val;
    Eigen::Vector2f pt = warpedPoints.col(i).head(2) / warpedPoints(2, i);
    Eigen::Vector2i fipt(floor(pt(0)), floor(pt(1)));
    Eigen::Vector2i cipt(ceil(pt(0)), ceil(pt(1)));
    if (0 <= pt(0) && pt(0) < srcImage.cols() - 1 && 0 <= pt(1) && pt(1) < srcImage.rows() - 1) {
      double x1 = pt(0) - fipt(0);
      double y1 = pt(1) - fipt(1);
      double x2 = 1 - x1;
      double y2 = 1 - y1;
      val = x2 * y2 * srcImage(fipt(1), fipt(0)) + x1 * y2 * srcImage(fipt(1), fipt(0) + 1) + x2 * y1 * srcImage(
          fipt(1) + 1, fipt(0)) + x1 * y1 * srcImage(fipt(1) + 1, fipt(0) + 1);

    }
    else if (0 <= fipt(0) && fipt(0) < srcImage.cols() && 0 <= fipt(1) && fipt(1) < srcImage.rows()) {
      val = srcImage(fipt(1), fipt(0));
    }
    else if (0 <= cipt(0) && cipt(0) < srcImage.cols() && 0 <= cipt(1) && cipt(1) < srcImage.rows()) {
      val = srcImage(cipt(1), cipt(0));
    }
    else
      val = defaultValue; //templateImage(i / template_cols, i % template_cols); //default to the same as template, so error is 0

    warped(i % template_rows, i / template_rows) = val; //Eigen is Column-major
  }
  return warped;
}

}


#pragma once
#ifndef _RAY_TRIANGLE_
#define _RAY_TRIANGLE_

#include <Eigen/Core>
#include "ray.hpp"
#include "surface.hpp"

namespace raytracer
{

using namespace Eigen;

class Triangle : public Surface
{
public:
  // Triangle indices
  typedef Matrix<unsigned int, 3, 1> Vector3u;

  Triangle(unsigned int a, unsigned int b, unsigned int c, const Surface& owner) :
    indices_(a, b, c),
    owner_(owner);
  {
  }

  bool Intersect(const Ray& ray, HitData& data) override
  {
    // Vertices
    const Vector3f& p0 = owner.vertices_.at(indices_(0));
    const Vector3f& p1 = owner.vertices_.at(indices_(1));
    const Vector3f& p2 = owner.vertices_.at(indices_(2));

    // Calculate the edges
    Vector3f edge1 = p1 - p0;
    Vector3f edge2 = p2 - p0;

    Vector3f q = ray.distance().cross(e2);
    float alpha = edge1.dot(q);             // Determinant of matrix m

    if (alpha > eps && alpha < eps)
    {
      // data.u = 0
      // data.v = 0
      // data.time = 0
      return false;
    }
    float invAlpha = 1.0f / alpha;
    Vector3f source = ray.position() - p0;
    float u = invAlpha * source.dot(q);
    if (u < 0.0f)
    {
      // data.u = 0
      // data.v = 0
      // data.time = 0
      return false;
    }

    Vector3f r = source.cross(edge1);
    float v = invAlpha * ray.direction().dot(r);
    if (v < 0.0f)
    {
      // data.u = 0
      // data.v = 0
      // data.time = 0
      return false;
    }

    // Hit data here
    // float t = f * edge2.dot(r);
    return true;
  }

  // TODO: Calculate triangle normal here
  Vector3f normal() const override
  {
    return normal_;
  }
private:
  Vector3u indices_;
  const Surface& owner_;
  Vector3f normal_;
};

}     // end of namespace raytracer

#endif // end of _RAY_TRIANGLE_


#include "sv/node/viz.h"

#include <glog/logging.h>
#include <tf2_eigen/tf2_eigen.h>

#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

namespace sv {

using visualization_msgs::Marker;
using visualization_msgs::MarkerArray;
using Vector3d = Eigen::Vector3d;
using Matrix3d = Eigen::Matrix3d;

void MeanCovar2Marker(const Vector3d& mean,
                      Vector3d eigvals,
                      Matrix3d eigvecs,
                      Marker& marker) {
  MakeRightHanded(eigvals, eigvecs);
  const Eigen::Quaterniond quat(eigvecs);
  eigvals = eigvals.cwiseSqrt() * 2;

  marker.pose.position.x = mean.x();
  marker.pose.position.y = mean.y();
  marker.pose.position.z = mean.z();
  marker.pose.orientation.w = quat.w();
  marker.pose.orientation.x = quat.x();
  marker.pose.orientation.y = quat.y();
  marker.pose.orientation.z = quat.z();
  marker.scale.x = eigvals.x();
  marker.scale.y = eigvals.y();
  marker.scale.z = eigvals.z();
}

void Grid2Markers(const SweepGrid& grid,
                  const std_msgs::Header& header,
                  std::vector<Marker>& markers) {
  const double alpha = 0.5;
  const double eps = 1e-8;

  markers.resize(grid.total() * 2 + 1);

  Eigen::SelfAdjointEigenSolver<Matrix3d> es;
  auto& line_mk = markers.back();
  line_mk.header = header;
  line_mk.ns = "match";
  line_mk.id = 0;
  line_mk.type = Marker::LINE_LIST;
  line_mk.action = Marker::ADD;
  line_mk.color.a = 1.0;
  line_mk.color.b = 1.0;
  line_mk.points.clear();
  line_mk.points.reserve(grid.total() * 2);
  line_mk.scale.x = 0.005;
  line_mk.pose.orientation.w = 1.0;

  for (int r = 0; r < grid.rows(); ++r) {
    for (int c = 0; c < grid.cols(); ++c) {
      const auto i = grid.Px2Ind({c, r});
      const auto& match = grid.MatchAt({c, r});

      auto& pano_mk = markers.at(i);
      auto& grid_mk = markers.at(i + grid.total());

      pano_mk.header = header;
      pano_mk.ns = "pano";
      pano_mk.id = i;
      pano_mk.type = Marker::SPHERE;
      pano_mk.color.g = 1.0;

      grid_mk.header = header;
      grid_mk.ns = "grid";
      grid_mk.id = i;
      grid_mk.type = Marker::SPHERE;
      grid_mk.color.a = alpha;
      grid_mk.color.r = 1.0;

      if (match.Ok()) {
        pano_mk.action = Marker::ADD;
        pano_mk.color.a = match.scale * .9;  // use scale for alpha
        const auto pt_p = match.mc_p.mean.cast<double>().eval();
        auto pano_cov = match.mc_p.Covar().cast<double>().eval();
        // pano_cov.diagonal().array() += eps;
        es.compute(pano_cov);
        MeanCovar2Marker(pt_p, es.eigenvalues(), es.eigenvectors(), pano_mk);

        grid_mk.action = Marker::ADD;
        const auto& tf = grid.TfAt(c);
        const auto pt_g = (tf * match.mc_g.mean).cast<double>().eval();
        auto grid_cov = match.mc_g.Covar();
        const auto R = tf.rotationMatrix();
        grid_cov = R * grid_cov * R.transpose();
        grid_cov.diagonal().array() += eps;
        es.compute(grid_cov.cast<double>());
        MeanCovar2Marker(pt_g, es.eigenvalues(), es.eigenvectors(), grid_mk);

        // Line
        geometry_msgs::Point p0, p1;
        p0.x = pt_p.x();
        p0.y = pt_p.y();
        p0.z = pt_p.z();
        p1.x = pt_g.x();
        p1.y = pt_g.y();
        p1.z = pt_g.z();
        line_mk.points.push_back(p0);
        line_mk.points.push_back(p1);
      } else {
        pano_mk.action = Marker::DELETE;
        grid_mk.action = Marker::DELETE;
      }
    }
  }
}

cv::Mat ApplyCmap(const cv::Mat& input,
                  double scale,
                  int cmap,
                  uint8_t bad_color) {
  CHECK_EQ(input.channels(), 1);

  cv::Mat disp;
  input.convertTo(disp, CV_8UC1, scale * 255.0);
  cv::applyColorMap(disp, disp, cmap);

  if (input.depth() >= CV_32F) {
    disp.setTo(bad_color, cv::Mat(~(input > 0)));
  }

  return disp;
}

void Imshow(const std::string& name, const cv::Mat& mat, int flag) {
  cv::namedWindow(name, flag);
  cv::imshow(name, mat);
  cv::waitKey(1);
}
void Traj2PoseArray(const Trajectory& traj, geometry_msgs::PoseArray& parray) {
  parray.poses.resize(traj.size());
  for (int i = 0; i < traj.size(); ++i) {
    const auto& st = traj.At(i);
    auto& pose = parray.poses.at(i);
    const auto T_p_l = Sophus::SE3d(st.rot, st.pos) * traj.T_imu_lidar;
    pose.orientation = tf2::toMsg(T_p_l.unit_quaternion());
    pose.position = tf2::toMsg(T_p_l.translation());
  }
}

}  // namespace sv


///////////////////////////////////////////////////////////////////////////////
// tree_view::stage.hpp                                                      //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_TREE_VIEW_STAGE_HPP_ER_2009
#define BOOST_TREE_VIEW_STAGE_HPP_ER_2009
#include <stdexcept>
#include <boost/format.hpp>

namespace boost{
namespace tree_view{

    // This class maps a position in a tree structure to a position in a vector
    //
    // The tree structure has a root node (stage 0) with n adjacent
    // nodes (stage 1), each of which have n adjacent nodes (stage 2) etc.
    // The nodes are stored in a vector, starting with the root node,
    // followed by those in stage1, then those in stage 2 etc. (breadth first)

    // j : stage
    // n : number of branches per node
    template<unsigned j,unsigned n>
    struct stage{
        static unsigned position_first;
        static unsigned position_last;
        static unsigned number_nodes;
    };

    template<unsigned n>
    struct stage<0,n>{
        static unsigned position_first;
        static unsigned position_last;
        static unsigned number_nodes;
    };

    // Client may ignore this:
    template<typename T> struct position_first_{ static unsigned get(); };
    template<typename T> struct position_last_{ static unsigned get(); };
    template<typename T> struct number_nodes_{ static unsigned get(); };

    // Implementation //

    // Initialization
    template<unsigned n>
    unsigned stage<0,n>::position_first = 0;

    template<unsigned n>
    unsigned stage<0,n>::position_last = 1;

    template<unsigned n>
    unsigned stage<0,n>::number_nodes = 1;

    // Recursion
    template<unsigned j,unsigned n>
    unsigned stage<j,n>::position_first
        = stage<j-1,n>::position_last;

    template<unsigned j,unsigned n>
    unsigned stage<j,n>::position_last
        = stage<j,n>::position_first
            + stage<j,n>::number_nodes;

    template<unsigned j,unsigned n>
    unsigned stage<j,n>::number_nodes
        = stage<j-1,n>::number_nodes * n;

    // T = stage
    template<typename T>
    unsigned position_first_<T>::get(){ return T::position_first; }
    template<typename T>
    unsigned position_last_<T>::get(){ return T::position_last; }
    template<typename T>
    unsigned number_nodes_<T>::get(){ return T::number_nodes; }

}// tree_view
}// boost

#endif


/*
 * Point.hpp
 *
 *  Created on: Apr 4, 2012
 *      Author: david
 */

#ifndef DASP_POINT_HPP_
#define DASP_POINT_HPP_

#include "Parameters.hpp"
#include "Array.hpp"
#include <Danvil/Tools/MoreMath.h>
#include <Eigen/Dense>
#include <vector>

namespace dasp
{
	struct Point
	{
		/** image pixel coordinate of point */
		int px, py;

		/** point color */
		Eigen::Vector3f color;

		/** position [m] of world source point */
		Eigen::Vector3f position;

		/** point surface normal */
		Eigen::Vector3f normal;

		/** estimated radius [px] on the image screen of a super pixel at point depth */
		float cluster_radius_px;

		/** Invalid points are ignored during point to cluster assignment */
		bool is_valid;

		/** Depth [m] of point */
		float depth() const {
			return position[2];
		}

		/** Sets the normal and assures that it points towards the camera (=origin) */
		void setNormal(const Eigen::Vector3f& n) {
			normal = n;
			// force normal to look towards the camera
			// check if point to camera direction and normal are within 90 deg
			// enforce: normal * (cam_pos - pos) > 0
			// do not need to normalize (cam_pos - pos) as only sign is considered
			const float q = normal.dot(-position);
			if(q < 0) {
				normal *= -1.0f;
			}
			else if(q == 0) {
				// this should not happen ...
				normal = Eigen::Vector3f(0,0,-1);
			}
		}

		/** Sets normal from gradient */
		void setNormalFromGradient(const Eigen::Vector2f& g) {
			const float gx = g.x();
			const float gy = g.y();
			const float scl = Danvil::MoreMath::FastInverseSqrt(gx*gx + gy*gy + 1.0f);
			setNormal(Eigen::Vector3f(scl*gx, scl*gy, -scl));
		}

		/** Computes local depth gradient (depth[m]/distance[m]) */
		Eigen::Vector2f computeGradient() const {
			return Eigen::Vector2f(normal.x() / normal.z(), normal.y() / normal.z());
		}

		/** Computes direction of local depth gradient */
		Eigen::Vector2f computeGradientDirection() const {
			const float nx = normal.x();
			const float ny = normal.y();
			if(nx == 0.0f && ny == 0.0f) {
				return Eigen::Vector2f::Unit(0);
			}
			else {
				const float scl = Danvil::MoreMath::FastInverseSqrt(nx*nx + ny*ny);
				return scl * Eigen::Vector2f(nx, ny);
			}
		}

		/** Computes "circularity"
		  * This is |n_z| = 1/sqrt(||gradient||^2 + 1) = ea/eb = sqrt(1 - ecc*ecc)
		  */
		float computeCircularity() const {
			return std::abs(normal.z());
		}

//	public:
//		 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	};

	typedef Array<Point,unsigned int> ImagePoints;

	struct Cluster
	{
		static constexpr float cPercentage = 0.95f; //0.99f;
		static constexpr float cSigmaScale = 1.959964f; //2.575829f;

		int seed_id;

		bool is_fixed;

		Point center;

		// Eigen::Matrix3f color_covariance;

		std::vector<unsigned int> pixel_ids;

		bool isValid() const {
			return is_fixed || pixel_ids.size() > 3;
		}

		void addPixel(unsigned int index) {
			pixel_ids.push_back(index);
		}

		void removePixel(unsigned int index) {
			auto it = std::find(pixel_ids.begin(), pixel_ids.end(), index);
			if(it != pixel_ids.end()) {
				pixel_ids.erase(it);
			}
		}

//		void addPixels(const std::vector<unsigned int>& v) {
//			pixel_ids.insert(pixel_ids.begin(), v.begin(), v.end());
//		}

		// point covariance matrix
		Eigen::Matrix3f cov;
		// eigenvalues of the covariance matrix
		Eigen::Vector3f ew;
		// eigenvectors of the covariance matrix
		Eigen::Matrix3f ev;

		/** Thickness of the cluster computed using smalles eigenvalue */
		float thickness;
		/** eccentricity of the ellipse described by a and b */
		float eccentricity;
		/** flatness of the ellipsoide described by a and c */
		float flatness;
		/** actual area */
		float area;
		/** actual area / expected area defined by base radius*/
		float area_quotient;

		/** Shape fitting */
		float shape_0, shape_x, shape_y, shape_xy, shape_xx, shape_yy;

		/** Thickness of cluster computed using orthogonal distance from plane
		 * WARNING: only computed if ComputeExt is called!
		 */
		float thickness_plane;

		/** number of pixel which are within superpixel radius but are not part of the superpixel
		 * WARNING: only computed if ComputeExt is called!
		 */
		float coverage_error;

		/** actual area of the superpixel (computed from all pixels) */
		float area_actual;
		/** expected area of the superpixel (considering local geometry and thickness) */
		float area_expected;
		/** expected area using the actual base radius (computed from cluster count) (same for all clusters...) */
		float area_expected_global;

		void UpdateCenter(const ImagePoints& points, const Parameters& opt);

		void ComputeExt(const ImagePoints& points, const Parameters& opt);

	};

}

#endif


#define BOOST_TEST_MODULE blas

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/all.h++>
#include <mla/vector/all.h++>

#include <mla/operations/level2/gemv.h++>

using Scalar = float;

typedef boost::mpl::list<
	mla::matrix::DenseRowMajor<Scalar>,
	mla::matrix::Diagonal<Scalar>,
	mla::matrix::SparseDOK<Scalar>,
	mla::matrix::SparseCRS<Scalar>
> matrix_type_list;

using MatrixType = mla::matrix::SparseCRS<Scalar>;

typedef boost::mpl::list<
	mla::vector::Dense<Scalar>,
	mla::vector::SparseCS<Scalar>
> vector_type_list;

BOOST_AUTO_TEST_SUITE(test_boost_level2)

BOOST_AUTO_TEST_CASE_TEMPLATE( blas_level2_test_gemv_zero, VectorType, vector_type_list )
{
	using namespace mla;

	Scalar alpha = 1.0f;

	//MatrixType A(3,3);
	size_t length = 3;
	mla::matrix::DenseRowMajor<Scalar> A( length, length);

	A(0,0) = 1.0f;
	A(1,1) = 1.0f;
	A(2,2) = 1.0f;

	VectorType x(length);
	x.setValue(0, 1.0f);
	x.setValue(1, 1.0f);
	x.setValue(2, 1.0f);

	Scalar beta = 1.0f;

	VectorType y(length);
	y.setValue(0, 1.0f);
	y.setValue(1, 1.0f);
	y.setValue(2, 1.0f);

	gemv(alpha, A, x, beta, y);

	for(size_t i = 0; i < length; i++)
	{
		BOOST_CHECK_CLOSE( y.getValue(i), 2.0f, 1.0e-5 );
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <boost/algorithm/clamp.hpp>
#include <boost/algorithm/minmax.hpp>

#include <ros/ros.h>
#include <eigen_conversions/eigen_msg.h>
#include <eigen3/Eigen/Dense>
#include <control_velpid/pid_controllerv2.h>

double quaternion_to_euler(double x, double y, double z, double w)
{
	double t0, t1, t2, t3, t4;
	double X,Y,Z;
	t0 = 2.0*(w*x + y*z);
	t1 = 1.0 - 2.0*(x*x + y*y);
	X = atan(t0/t1)*180/M_PI;

	t2 = 2*(w*y - z*x);
	if (t2 > 1)
		t2 = 1;
	else if (t2 <-1)
		t2 = -1;
	else
		t2 = t2;
	Y = asin(t2)*180/M_PI;

	t3 = 2.0*(w*z + x*y);
	t4 = 1.0 - 2*(y*y + z*z);
	Z = atan(t3/t4)*180/M_PI;

	return X, Y, Z;
}

/*
double compute_linvel_effort_v2(geometry_msgs::PoseStamped goal, geometry_msgs::PoseStamped current, ros::Time last_time){
	// Local machine time
	_dt = ros::Time::now() - last_time;

	tf::pointMsgToEigen(goal.pose.position, _goal);
	tf::pointMsgToEigen(current.pose.position, _current);

	// Estimate error in world (Initial) Frame
	distance_error = _goal - _current;
	distance_integral = distance_integral + distance_error * _dt.toSec();
	distance_derivative = (distance_error - previous_distance_error)/_dt.toSec();

	// Store
	previous_distance_error = distance_error;

	//vel_world = Kp.dot(distance_error) + Ki.dot(distance_integral) + Kd.dot(distance_derivative);
	vel_world(0) = Kp(0)*distance_error(0) + Ki(0)*distance_integral(0) + Kd(0)*distance_derivative(0);
	vel_world(1) = Kp(1)*distance_error(1) + Ki(1)*distance_integral(1) + Kd(1)*distance_derivative(1);
	vel_world(2) = Kp(2)*distance_error(2) + Ki(2)*distance_integral(2) + Kd(2)*distance_derivative(2);

	roll, pitch, yaw = quaternion_to_euler(current.pose.orientation.x, current.pose.orientation.y, current.pose.orientation.z, current.pose.orientation.w);
	yaw_rate_world = (pose_goal(3) - yaw)*yaw_gain;

	Rbv1 << cos(yaw), sin(yaw), 0;
	Rbv2 << -sin(yaw), cos(yaw), 0;
	Rbv3 << 0, 0, 1;

	vel_body(0) = Rbv1(0)*vel_world(0) + Rbv1(1)*vel_world(1) + Rbv1(2)*vel_world(2);
	vel_body(1) = Rbv2(0)*vel_world(0) + Rbv2(1)*vel_world(1) + Rbv2(2)*vel_world(2);
	vel_body(2) = Rbv3(0)*vel_world(0) + Rbv3(1)*vel_world(1) + Rbv3(2)*vel_world(2);

	return (vel_body(0), vel_body(1), vel_body(2), yaw_rate_world);
}
*/

double compute_linvel_effort_v2(geometry_msgs::PoseStamped goal, geometry_msgs::PoseStamped current, ros::Time last_time){
	// Local machine time
	_dt = ros::Time::now() - last_time;

	tf::pointMsgToEigen(goal.pose.position, _goal);
	tf::pointMsgToEigen(current.pose.position, _current);

	// Estimate error in world (Initial) Frame
	distance_error = _goal - _current;
	distance_integral = distance_integral + distance_error * _dt.toSec();
	distance_derivative = (distance_error - previous_distance_error)/_dt.toSec();

	// Store
	previous_distance_error = distance_error;

	//vel_world = Kp.dot(distance_error) + Ki.dot(distance_integral) + Kd.dot(distance_derivative);
	vel_world(0) = Kp(0)*distance_error(0) + Ki(0)*distance_integral(0) + Kd(0)*distance_derivative(0);
	vel_world(1) = Kp(1)*distance_error(1) + Ki(1)*distance_integral(1) + Kd(1)*distance_derivative(1);
	vel_world(2) = Kp(2)*distance_error(2) + Ki(2)*distance_integral(2) + Kd(2)*distance_derivative(2);

	//roll, pitch, yaw = quaternion_to_euler(current.pose.orientation.x, current.pose.orientation.y, current.pose.orientation.z, current.pose.orientation.w);
	//yaw_rate_world = (pose_goal(3) - yaw)*yaw_gain;

	//return (vel_world(0), vel_world(1), vel_world(2), yaw_rate_world);
	return (vel_world(0), vel_world(1), vel_world(2));
}


#include <algorithm>
#include <bitset>
#include <cassert>
#include <cmath>
#include <iostream>
#include <set>
#include <vector>

#include <future>
#include <thread>

#include <Eigen/Dense>

#include <gmpxx.h>

#include "Expression.h"

//int THREADS_NUM = std::thread::hardware_concurrency();
constexpr int THREADS_NUM = 4;

typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> MatrixXq;

static std::ostream& operator<<(std::ostream & stream, mpq_class const & rop) {
  stream << rop.get_str();
  return stream;
}

Expression Expression::NumericSimplify(Indices const & indices, bool print_matrix) const {
  assert(!this->IsZero());

  std::bitset<64> bs(0);
  bs[2 * indices.size()] = true;
  size_t number_of_combinations = bs.to_ulong();

  auto thread_function = [expression=*this,noc=number_of_combinations,indices=indices] (size_t thread_counter, auto coefficient, auto sum) mutable -> void {
    std::set<size_t> coefficient_set;
    std::set<ScalarSum> sum_set;
    for (size_t counter = thread_counter * noc / THREADS_NUM; counter < (thread_counter + 1) * noc / THREADS_NUM; ++counter) {
      std::vector<size_t> numbers (indices.size());
      std::bitset<64> binary_representation(counter);
      for (size_t digit_counter = 0; digit_counter < indices.size(); ++digit_counter) {
        unsigned int binary_digit_a = static_cast<unsigned int>(binary_representation[2 * digit_counter + 1]);
        unsigned int binary_digit_b = static_cast<unsigned int>(binary_representation[2 * digit_counter]);
        *(numbers.rbegin() + digit_counter) = 2 * binary_digit_a + binary_digit_b;
      }

      ScalarSum sum_tmp = expression.EvaluateIndices(indices, numbers);
      if (!sum_tmp.IsZero()) {
        coefficient_set.merge(sum_tmp.CoefficientSet());
        sum_set.insert(sum_tmp);
      }
    }
    coefficient.set_value(coefficient_set);
    sum.set_value(sum_set);
  };

  std::vector<std::thread> t (THREADS_NUM);

  std::vector<std::promise<std::set<size_t>>> coefficients_promises (THREADS_NUM);
  std::vector<std::promise<std::set<ScalarSum>>> sums_promises (THREADS_NUM);

  std::vector<std::future<std::set<size_t>>> coefficients_futures (THREADS_NUM);
  std::vector<std::future<std::set<ScalarSum>>> sums_futures (THREADS_NUM);

  for (int thread_counter = 0; thread_counter < THREADS_NUM; ++thread_counter) {
    coefficients_futures[thread_counter] = coefficients_promises[thread_counter].get_future();
    sums_futures[thread_counter] = sums_promises[thread_counter].get_future();
    std::cout << "Launching thread " << thread_counter << std::endl;
    t[thread_counter] = std::thread(thread_function, thread_counter, std::move(coefficients_promises[thread_counter]),
                                                                     std::move(sums_promises[thread_counter]));
  }

  for (int thread_counter = 0; thread_counter < THREADS_NUM; ++thread_counter) {
    t[thread_counter].join();
    std::cout << "Joined thread " << thread_counter << std::endl;
  }

  std::set<ScalarSum> sum_set;
  std::set<size_t> coefficient_set;

  for (int thread_counter = 0; thread_counter < THREADS_NUM; ++thread_counter) {
    coefficient_set.merge(coefficients_futures.at(thread_counter).get());
    sum_set.merge(sums_futures.at(thread_counter).get());
  }

  std::map<size_t, size_t> coefficient_map;
  std::for_each(coefficient_set.begin(), coefficient_set.end(), [n=0,&coefficient_map](auto a) mutable { coefficient_map[a] = n++; });

  std::cout << "number of different (not necessarily linear independent) equations : " << sum_set.size() << std::endl;
  std::cout << "number of coefficients (e_.) in these equations                    : " << coefficient_map.size() << std::endl;
  std::cout << "Thus the problem of finding linear dependencies is equivalent to the problem of finding the null space for a " << sum_set.size() << " by " << coefficient_map.size() << " matrix." << std::endl;

  std::vector<std::vector<Rational>> matrix;
  std::for_each(sum_set.begin(), sum_set.end(), [&coefficient_map, &matrix](auto & a) {
    matrix.push_back(a.CoefficientVector(coefficient_map));
    });

  MatrixXq mq(matrix.size(), coefficient_map.size());

  for (size_t row_counter = 0; row_counter < matrix.size(); ++row_counter) {
    for (size_t column_counter = 0; column_counter < coefficient_map.size(); ++column_counter) {
      Fraction frac = matrix[row_counter][column_counter].get_fraction();
      mq(row_counter, column_counter) = mpq_class(frac.first, frac.second);
    }
  }

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  std::cout << "the rank of the matrix is : " << lu_decompq.rank() << std::endl;
  std::cout << "null space basis: " << std::endl;

  MatrixXq kq = lu_decompq.kernel();

  if (print_matrix) {
    std::cout << kq << std::endl;
  }

  std::set<size_t> coeff_removed;

  for (int column_counter = 0; column_counter < kq.cols(); ++column_counter) {
    for (int row_counter = kq.rows() - 1; row_counter >= 0; --row_counter) {
      if (kq(row_counter, column_counter) == 0 ) {
        continue;
      } else if (std::find(coeff_removed.begin(), coeff_removed.end(), row_counter) != coeff_removed.end()) {
        continue;
      } else {
        coeff_removed.insert(row_counter);
        break;
      }
    }
  }

  std::map<size_t, size_t> coefficient_rmap;
  std::for_each(coefficient_map.begin(), coefficient_map.end(), [&coefficient_rmap](auto a) { coefficient_rmap.insert(std::make_pair(a.second, a.first)); });

  Expression ret (*this);
  std::for_each(coeff_removed.begin(), coeff_removed.end(), [&ret, &coefficient_rmap] (auto a) { ret.EliminateVariable(coefficient_rmap.at(a)); });
  ret.CanonicalisePrefactors();
  ret.RedefineScalars();

  return Expression(ret);
}


//           Copyright Matthew Pulver 2018 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE_1_0.txt or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

// THIS IS NOT MEANT TO BE COMPILED - ONLY FOR INCLUSION IN DOCUMENTATION.

#include <boost/math/differentiation/autodiff.hpp>

namespace boost { namespace math { namespace differentiation {

// Type for variables and constants.
template<typename RealType, size_t Order, size_t... Orders>
using autodiff_fvar = typename detail::nest_fvar<RealType,Order,Orders...>::type;

// Function returning a variable of differentiation.
template<typename RealType, size_t Order, size_t... Orders>
autodiff_fvar<RealType,Order,Orders...> make_fvar(const RealType& ca);

// Type of combined autodiff types.
template<typename RealType, typename... RealTypes>
using promote = typename detail::promote_args_n<RealType,RealTypes...>::type;

namespace detail {

// Single autodiff variable. Independent variables are created by nesting.
template<typename RealType, size_t Order>
class fvar
{
  public:

    // Query return value of function to get the derivatives.
    template<typename... Orders>
    get_type_at<RealType, sizeof...(Orders)-1> derivative(Orders... orders) const;

    // All of the arithmetic and comparison operators are overloaded.
    template<typename RealType2, size_t Order2>
    fvar& operator+=(const fvar<RealType2,Order2>&);

    fvar& operator+=(const root_type&);

    // ...
};

// Standard math functions are overloaded and called via argument-dependent lookup (ADL).
template<typename RealType, size_t Order>
fvar<RealType,Order> floor(const fvar<RealType,Order>&);

template<typename RealType, size_t Order>
fvar<RealType,Order> exp(const fvar<RealType,Order>&);

// ...

} // namespace detail

} } } // namespace boost::math::differentiation
/**/


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     tilt_calibration.cpp
 * \author   Collin Johnson
 *
 * Definition of calibrate_laser_pitch.
 */

#include "calibration/laser/tilt_calibration.h"
#include "math/regression.h"
#include <boost/range/iterator_range.hpp>
#include <iostream>

namespace vulcan
{
namespace calibration
{

using Measurements = std::vector<Point<double>>;

tilt_calibration_results_t calibrate_laser_tilt(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                                std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                                int lineStartIndex,
                                                int lineEndIndex,
                                                double pitchStepSize,
                                                double maxPitch,
                                                double rollStepSize,
                                                double maxRoll);
Measurements extract_line_measurements(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                       std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                       int lineStartIndex,
                                       int lineEndIndex);
Point<double> apply_tilt(const Point<double>& point, double pitch, double roll);

tilt_calibration_results_t calibrate_laser_pitch(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                                 std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                                 int lineStartIndex,
                                                 int lineEndIndex,
                                                 double minStepSize,
                                                 double maxPitch)
{
    return calibrate_laser_tilt(beginLaser, endLaser, lineStartIndex, lineEndIndex, minStepSize, maxPitch, 0.0, 0.0);
}

tilt_calibration_results_t calibrate_laser_roll(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                                std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                                int lineStartIndex,
                                                int lineEndIndex,
                                                double minStepSize,
                                                double maxRoll)
{
    return calibrate_laser_tilt(beginLaser, endLaser, lineStartIndex, lineEndIndex, 0.0, 0.0, minStepSize, maxRoll);
}

tilt_calibration_results_t calibrate_laser_tilt(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                                std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                                int lineStartIndex,
                                                int lineEndIndex,
                                                double pitchStepSize,
                                                double maxPitch,
                                                double rollStepSize,
                                                double maxRoll)
{
    assert(lineEndIndex > lineStartIndex + 5);
    assert(endLaser > beginLaser);

    std::cout << "Beginning tilt calibration for " << std::distance(beginLaser, endLaser) << " laser scans using "
              << "indices " << lineStartIndex << "->" << lineEndIndex << " for fitting the line.\n";

    tilt_calibration_results_t results;

    double minError = std::numeric_limits<double>::max();

    Measurements measurements = extract_line_measurements(beginLaser, endLaser, lineStartIndex, lineEndIndex);
    Measurements pitchMeasurements = measurements;

    auto pitchLine = math::total_least_squares(measurements.begin(), measurements.end());

    for (double pitch = 0.0, roll = 0.0; pitch >= -maxPitch && roll >= -maxRoll;
         pitch -= pitchStepSize, roll -= rollStepSize) {
        std::transform(measurements.begin(),
                       measurements.end(),
                       pitchMeasurements.begin(),
                       [pitch, roll](Point<double> point) {
                           return apply_tilt(point, pitch, roll);
                       });

        double error = 0;

        for (auto point : pitchMeasurements) {
            error += distance_to_line(point, pitchLine);
        }

        tilt_t tilt(pitch, roll);
        error /= pitchMeasurements.size();
        results.tiltErrors.emplace_back(tilt, error);

        std::cout << "Pitch: " << pitch << " Roll:" << roll << " Line: " << pitchLine << " Error:" << error << '\n';

        if (error < minError) {
            results.bestTilt = tilt;
            minError = error;
        }
    }

    std::cout << "Finished tilt calibration: Pitch:" << results.bestTilt.pitch << " Roll:" << results.bestTilt.roll
              << '\n';

    return results;
}

Measurements extract_line_measurements(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                       std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                       int lineStartIndex,
                                       int lineEndIndex)
{
    Measurements measurements;
    measurements.reserve(std::distance(beginLaser, endLaser) * (lineEndIndex - lineStartIndex));
    cartesian_laser_scan_t cartesian;

    for (auto& scan : boost::make_iterator_range(beginLaser, endLaser)) {
        polar_scan_to_cartesian_scan(scan, cartesian);

        std::copy(cartesian.scanPoints.begin() + lineStartIndex,
                  cartesian.scanPoints.begin() + lineEndIndex,
                  std::back_inserter(measurements));
    }

    std::cout << "INFO: tilt_calibration: Using " << measurements.size() << " measurements for fitting.\n";

    return measurements;
}

Point<double> apply_tilt(const Point<double>& point, double pitch, double roll)
{
    return Point<double>(point.x * std::cos(pitch), point.y * std::cos(roll));
}

}   // namespace calibration
}   // namespace vulcan


/**
 * \file PanFilter.cpp
 */

#include <ATK/Tools/PanFilter.h>

#include <cmath>
#include <complex>
#include <cstdint>

#include <boost/math/constants/constants.hpp>

#include <ATK/Core/TypeTraits.h>

namespace ATK
{
  template<typename DataType_>
  PanFilter<DataType_>::PanFilter(gsl::index nb_channels)
  :Parent(nb_channels, 2 * nb_channels)
  {
  }

  template<typename DataType_>
  void PanFilter<DataType_>::set_pan_law(PAN_LAWS law)
  {
    this->law = law;
  }

  template<typename DataType_>
  typename PanFilter<DataType_>::PAN_LAWS PanFilter<DataType_>::get_pan_law() const
  {
    return law;
  }

  template<typename DataType_>
  void PanFilter<DataType_>::set_pan(double pan)
  {
    if(pan < -1 || pan > 1)
    {
      throw std::out_of_range("Pan must be a value between -1 and 1");
    }
    this->pan = pan;
  }

  template<typename DataType_>
  double PanFilter<DataType_>::get_pan() const
  {
    return pan;
  }

  template<typename DataType_>
  void PanFilter<DataType_>::process_impl(gsl::index size) const
  {
    double left_coeff = 1;
    double right_coeff = 1;

    switch(law)
    {
    case PAN_LAWS::SINCOS_0_CENTER:
      left_coeff = std::sqrt(2) * std::cos((pan + 1) / 4 * boost::math::constants::pi<double>());
      right_coeff = std::sqrt(2) * std::sin((pan + 1) / 4 * boost::math::constants::pi<double>());
      break;
    case PAN_LAWS::SINCOS_3_CENTER:
      left_coeff = std::cos((pan + 1) / 4 * boost::math::constants::pi<double>());
      right_coeff = std::sin((pan + 1) / 4 * boost::math::constants::pi<double>());
      break;
    case PAN_LAWS::SQUARE_0_CENTER:
      left_coeff = std::sqrt(2) * std::sqrt((1 - pan) / 2);
      right_coeff = std::sqrt(2) * std::sqrt((1 + pan) / 2);
      break;
    case PAN_LAWS::SQUARE_3_CENTER:
      left_coeff = std::sqrt((1 - pan) / 2);
      right_coeff = std::sqrt((1 + pan) / 2);
      break;
    case PAN_LAWS::LINEAR_TAPER:
      left_coeff = (1 - pan) / 2;
      right_coeff = (1 + pan) / 2;
      break;
    case PAN_LAWS::BALANCE:
      left_coeff = pan < 0 ? 1 : 1 - pan;
      right_coeff = pan > 0 ? 1 : 1 + pan;
      break;
    }

    assert(2 * nb_input_ports == nb_output_ports);

    for (gsl::index channel = 0; channel < nb_input_ports; ++channel)
    {
      const DataType* ATK_RESTRICT input = converted_inputs[channel];
      DataType* ATK_RESTRICT output0 = outputs[2 * channel];
      DataType* ATK_RESTRICT output1 = outputs[2 * channel + 1];
      for(gsl::index i = 0; i < size; ++i)
      {
        output0[i] = static_cast<DataType>(static_cast<typename TypeTraits<DataType>::Scalar>(left_coeff) * input[i]);
        output1[i] = static_cast<DataType>(static_cast<typename TypeTraits<DataType>::Scalar>(right_coeff) * input[i]);
      }
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class PanFilter<std::int16_t>;
  template class PanFilter<std::int32_t>;
  template class PanFilter<std::int64_t>;
  template class PanFilter<float>;
  template class PanFilter<std::complex<float>>;
  template class PanFilter<std::complex<double>>;
#endif
  template class PanFilter<double>;
}


/* Copyright (C) 5/23/18 Julian Stobbe - All Rights Reserved
 * You may use, distribute and modify this code under the
 * terms of the MIT license.
 *
 * You should have received a copy of the MIT license with
 * this file.
 */

#ifndef VALUATION_NETWORK_SIM_HPP
#define VALUATION_NETWORK_SIM_HPP

#define USE_ACTUAL_CONN 0

#include <type_traits>
#include <cmath>
#include <cstdlib>
#include <type_traits>
#include <string>
#include <unordered_map>
#include <limits>
#include <random>
#include <vector>

#include "trng/chi_square_dist.hpp"

#include "Utils.hpp"

#ifdef USE_MPI

#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>

#endif

#include "Config.hpp"
#include "StudentT.hpp"
#include "MVarNormal.hpp"
#include "Sampler.hpp"
#include "StatAcc.hpp"
#include "BlackScholesNetwork.hpp"
#include "RndGraphGen.hpp"

typedef typename std::conditional<USE_EIGEN_ACC, Eigen::MatrixXd, double>::type AccType;
typedef typename std::map<int, std::unordered_map<std::string, Eigen::MatrixXd>> ResultType;

struct SimulationParameters
{
    const long iterations, N_networks;
    NetworkType net_t;
};

constexpr int deg_of_freedom = 8;
//const std::string io_deg_str("In/Out degree distribution");

class NetwSim {
    friend class Py_ER_Net;
private:

#ifdef USE_MPI
    const boost::mpi::communicator local;
    const boost::mpi::communicator world;
    const bool isGenerator;
#endif

    trng::yarn2 gen_u;
    trng::uniform01_dist<> u_dist;
    Student_t_dist t_dist;
    Multivariate_Normal_Dist mvndist;
    //std::vector<double> dbg_weights;
    double last_weight;

    NetworkType net_t;
    int N;
    bool initialized;
    double T;              // maturity
    double r;              // interest
    double p;
    double val;
    double S0scalar;
    double sigmaScalar;
    double default_prob_scale;
    int setM;
    const double tmp[2][2] = {{1, 0},
                              {0, 1}};
    long Num_Samples;
    long Num_Networks;

    BlackScholesNetwork* bsn;
    Eigen::MatrixXd iSigma;
    Eigen::VectorXd sigma;
    Eigen::VectorXd Z;                 // Multivariate normal, used to generate lognormal assets
    Eigen::VectorXd var_h;
    Eigen::VectorXd S0;
    Eigen::VectorXd debt;
    Eigen::MatrixXd io_deg_dist;
    Eigen::MatrixXd avg_rc_sums;
    std::pair<double, double> avg_io_deg;

    std::map<int, std::unordered_map<std::string, Eigen::MatrixXd> > results;
    double connectivity;

    // last result, returned by observe
    void reset_network();

    Eigen::MatrixXd in_out_degree(Eigen::MatrixXd* M);

    void init_2DFixed_BS(const double vs01, const double vs10, const double vr01, const double vr10)
    {
        connectivity = 1;
        Eigen::MatrixXd M = Eigen::MatrixXd::Zero(2, 4);
        Utils::fixed_2d(&M, vs01, vs10, vr01, vr10);
        io_deg_dist = Utils::in_out_degree(&M);
        avg_io_deg = Utils::avg_io_deg(&M);
        avg_rc_sums = Utils::avg_row_col_sums(&M);
        bsn->re_init(M, S0, debt, sigma);
    }

    template <typename F>
    void init_BS(F gen_function) {
        if (val < 0 || val >= 1) throw std::logic_error("Row sum is not in [0,1)");
        if (p < 0 || p > 1) throw std::logic_error("p is not a probability");
        connectivity = N * p;
        Eigen::MatrixXd M = Eigen::MatrixXd::Zero(N, 2 * N);
        gen_function(&M, gen_u, p, val, setM);
        //Utils::gen_fixed_degree(&M, gen_u, p, val, which_to_set);
        io_deg_dist += Utils::in_out_degree(&M);
        avg_io_deg = Utils::avg_io_deg(&M);
        avg_rc_sums += Utils::avg_row_col_sums(&M);
        bsn->re_init(M, S0, debt, sigma);
    }

//TODO: config struct
public:
    /*!
     * @brief               (re-)initializes network to given parameters
     * @param N             Size of network
     * @param p             Probability of cross holding
     * @param val           total value in/being held by other firms
     * @param which_to_set  Flag to disable connections between parts of the network. Can be 0/1/2. 2: cross debt is 0, 1: cross equity is 0, 0: none is 0
     * @TODO: config struct
     */
    void init_network(const int N_, const double p_, const double val_, const int which_to_set, const double T_,\
        const double r_, const double S0_, const double sigma_, const double default_prob_scale_, const NetworkType net_t_);

    void init_2D_network(BSParameters& bs_params, const double vs01, const double vs10, const double vr01, const double vr10);

    virtual ~NetwSim(){
        if(bsn != nullptr)
            delete bsn;
    }

    /*!
     * @brief               Constructs the Black Scholes Model using random cross holdings.
     * @param local         local MPI communicator (between producers/consumers only)
     * @param world         global MPI communicator
     * @param isGenerator   Flag for generator/consumer ranks
     */
#ifdef USE_MPI
    NetwSim(const boost::mpi::communicator local, const boost::mpi::communicator world, const bool isGenerator):
            local(local), world(world), isGenerator(isGenerator), Z_dist(&tmp[0][0], &tmp[1][1]), chi_dist(deg_of_freedom), t_dist(deg_of_freedom)
#else
    NetwSim():
            Z_dist(&tmp[0][0], &tmp[1][1]), chi_dist(deg_of_freedom), t_dist(deg_of_freedom), initialized(false)
#endif
    {
        bsn = nullptr;
        iSigma = Eigen::MatrixXd::Zero(1,1);
        Z = Eigen::VectorXd::Zero(1,1);
        var_h = Eigen::VectorXd::Zero(1,1);
    }

    /*!
     * @brief               Constructs the Black Scholes Model using random cross holdings.
     * @param local         local MPI communicator (between producers/consumers only)
     * @param world         global MPI communicator
     * @param isGenerator   Flag for generator/consumer ranks
     * @param N             Size of network
     * @param p             Probability of connection between firms
     * @param val
     * @param which_to_set  Flag to disable connections between parts of the network. Can be 0/1/2. 2: cross debt is 0, 1: cross equity is 0, 0: none is 0
     * @param T             maturity
     * @param r             interest rate
     */
#ifdef USE_MPI
    NetwSim(const boost::mpi::communicator local, const boost::mpi::communicator world, const bool isGenerator,
               long N, double p, double val, int which_to_set, const double T, const double r, const double S0, const NetworkType net_t_) :
            local(local), world(world), isGenerator(isGenerator),
#else
    NetwSim(long N_, double p_, double val, int which_to_set, const double T_, const double r_, const double S0_, const double sigma_, const double default_scale_, const NetworkType net_t_) :
#endif
            val(val), T(T_), r(r_), S0scalar(S0_), sigmaScalar(sigma_), default_prob_scale(default_scale_)\
        , Z_dist(&tmp[0][0], &tmp[1][1]), chi_dist(deg_of_freedom), t_dist(deg_of_freedom), net_t(net_t_)
    {
        gen_u.seed();
        bsn = nullptr;
        init_network(N_, p_, val, which_to_set, T_, r_, S0_, sigma_, default_scale_, net_t_);
    }

    inline ResultType run_valuation(const SimulationParameters sim_params)
    {
        return run_valuation(sim_params.iterations, sim_params.N_networks);
    }

    /*!
     * @brief       Runs a series of example simulations
     * @param N_in  Size of network
     */
    ResultType run_valuation(const long N_Samples = 2000, const long N_networks = 100, const bool fix_degree = false);

    /*!
     * @brief   Draws a random number from a multivariate lognormal distribution
     * @return  Random sample from a multivariate lognormal distribution
     */
    const Eigen::MatrixXd draw_from_dist();

    const Eigen::MatrixXd transformZ(const Eigen::Ref<const Eigen::MatrixXd>& Z) const;

    double get_weight();

    /*!
     * @brief       Runs a single simulation of the Black Scholes model to find the fix point valuation.
     * @param St_in Initial asset value
     * @return      Valuation of firms at maturity T
     */
    auto run(const Eigen::Ref<const Eigen::VectorXd>& St_in)//Eigen::VectorXd St)
    {
        //Eigen::VectorXd St = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(St_in.data(), St_in.size());
        bsn->set_St(St_in);
        bsn->run_valuation(1000);
    }

    /*!
     * @brief   Compute \f$\Delta\f$ using the covariance matrix of the normal distribution
     * @return  \f$\Delta\f$
     */
    const Eigen::MatrixXd delta_v2();

    /*!
     * @brief   Computes the sum over all elements of the cross holdings matrix
     * @return  \f$\sum_{ij} M_{ij}\f$
     */
    std::vector<double> sumM() {
        std::vector<double> res{(bsn->get_M()).sum()};
        return res;
    }

    Eigen::MatrixXd get_M()
    {
        return bsn->get_M();
    }

    auto test_out()
    {
        auto v_o = bsn->get_valuation();
        auto s_o = bsn->get_solvent();
        std::cout << "output after sample: " << std::endl;
        LOG(INFO) << "Valuation: \n" << v_o;
        LOG(INFO) << "solvent: \n" << s_o;
        LOG(INFO) << "St: \n" << bsn->get_assets();
        LOG(INFO) << "debt: \n" << bsn->get_debt();
        LOG(INFO) << "M: \n" << bsn->get_M();
        std::cout << "------" << std::endl;
        Eigen::MatrixXd out = Eigen::MatrixXd::Constant(1,1,0);
        return out;
    }

private:

    trng::yarn2 gen_z;
    trng::yarn2 gen_chi;
    trng::chi_square_dist<double> chi_dist;
    trng::correlated_normal_dist<> Z_dist;

public:
    Eigen::MatrixXd get_io_deg_dist() const
    {
        return io_deg_dist;
    }

    Eigen::MatrixXd get_avg_row_col_sums() const
    {
        return avg_rc_sums;
    }

    void set_weight();
    //}

};

#endif //VALUATION_NETWORK_SIM_HPP


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

 * GTSAM Copyright 2010-2019, 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    SO4.cpp
 * @brief   4*4 matrix representation of SO(4)
 * @author  Frank Dellaert
 * @author  Luca Carlone
 */

#include <gtsam/base/concepts.h>
#include <gtsam/base/timing.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/geometry/Unit3.h>

#include <Eigen/Eigenvalues>
#include <boost/random.hpp>

#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>

using namespace std;

namespace gtsam {

// /* *************************************************************************
// */ static Vector3 randomOmega(boost::mt19937 &rng) {
//   static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
//   return Unit3::Random(rng).unitVector() * randomAngle(rng);
// }

// /* *************************************************************************
// */
// // Create random SO(4) element using direct product of lie algebras.
// SO4 SO4::Random(boost::mt19937 &rng) {
//   Vector6 delta;
//   delta << randomOmega(rng), randomOmega(rng);
//   return SO4::Expmap(delta);
// }

//******************************************************************************
template <>
GTSAM_EXPORT
Matrix4 SO4::Hat(const Vector6& xi) {
  // skew symmetric matrix X = xi^
  // Unlike Luca, makes upper-left the SO(3) subgroup.
  Matrix4 Y = Z_4x4;
  Y(0, 1) = -xi(5);
  Y(0, 2) = +xi(4);
  Y(1, 2) = -xi(3);
  Y(0, 3) = -xi(2);
  Y(1, 3) = +xi(1);
  Y(2, 3) = -xi(0);
  return Y - Y.transpose();
}

//******************************************************************************
template <>
GTSAM_EXPORT
Vector6 SO4::Vee(const Matrix4& X) {
  Vector6 xi;
  xi(5) = -X(0, 1);
  xi(4) = +X(0, 2);
  xi(3) = -X(1, 2);
  xi(2) = -X(0, 3);
  xi(1) = +X(1, 3);
  xi(0) = -X(2, 3);
  return xi;
}

//******************************************************************************
/* Exponential map, porting MATLAB implementation by Luca, which follows
 * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
 * Ramona-Andreaa Rohan */
template <>
GTSAM_EXPORT
SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
  using namespace std;
  if (H) throw std::runtime_error("SO4::Expmap Jacobian");

  // skew symmetric matrix X = xi^
  const Matrix4 X = Hat(xi);

  // do eigen-decomposition
  auto eig = Eigen::EigenSolver<Matrix4>(X);
  Eigen::Vector4cd e = eig.eigenvalues();
  using std::abs;
  sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
    return abs(a.imag()) > abs(b.imag());
  });

  // Get a and b from eigenvalues +/i ai and +/- bi
  double a = e[0].imag(), b = e[2].imag();
  if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
    throw runtime_error("SO4::Expmap: wrong eigenvalues.");
  }

  // Build expX = exp(xi^)
  Matrix4 expX;
  using std::cos;
  using std::sin;
  const auto X2 = X * X;
  const auto X3 = X2 * X;
  double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
  if (a != 0 && b == 0) {
    double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
    return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
  } else if (a == b && b != 0) {
    double sin_a = sin(a), cos_a = cos(a);
    double c0 = (a * sin_a + 2 * cos_a) / 2,
           c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
           c3 = (sin_a - a * cos_a) / (2 * a3);
    return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
  } else if (a != b) {
    double sin_a = sin(a), cos_a = cos(a);
    double sin_b = sin(b), cos_b = cos(b);
    double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
           c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
           c2 = (cos_a - cos_b) / (b2 - a2),
           c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
    return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
  } else {
    return SO4();
  }
}

//******************************************************************************
// local vectorize
static SO4::VectorN2 vec4(const Matrix4& Q) {
  return Eigen::Map<const SO4::VectorN2>(Q.data());
}

// so<4> generators
static std::vector<Matrix4, Eigen::aligned_allocator<Matrix4> > G4(
    {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
     SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
     SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});

// vectorized generators
static const Eigen::Matrix<double, 16, 6> P4 =
    (Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
     vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
        .finished();

//******************************************************************************
template <>
GTSAM_EXPORT
Matrix6 SO4::AdjointMap() const {
  // Elaborate way of calculating the AdjointMap
  // TODO(frank): find a closed form solution. In SO(3) is just R :-/
  const Matrix4& Q = matrix_;
  const Matrix4 Qt = Q.transpose();
  Matrix6 A;
  for (size_t i = 0; i < 6; i++) {
    // Calculate column i of linear map for coeffcient of Gi
    A.col(i) = SO4::Vee(Q * G4[i] * Qt);
  }
  return A;
}

//******************************************************************************
template <>
GTSAM_EXPORT
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
  const Matrix& Q = matrix_;
  if (H) {
    // As Luca calculated, this is (I4 \oplus Q) * P4
    *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
        Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
  }
  return gtsam::vec4(Q);
}

///******************************************************************************
template <>
GTSAM_EXPORT
SO4 SO4::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
  gttic(SO4_Retract);
  const Matrix4 X = Hat(xi / 2);
  return SO4((I_4x4 + X) * (I_4x4 - X).inverse());
}

//******************************************************************************
template <>
GTSAM_EXPORT
Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) {
  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
  const Matrix4& R = Q.matrix();
  const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse();
  return -2 * Vee(X);
}

//******************************************************************************
GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
  const Matrix4& R = Q.matrix();
  const Matrix3 M = R.topLeftCorner<3, 3>();
  if (H) {
    const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
                  q = R.topRightCorner<3, 1>();
    *H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2,  //
        Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1,   //
        q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
  }
  return M;
}

//******************************************************************************
GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
  const Matrix4& R = Q.matrix();
  const Matrix43 M = R.leftCols<3>();
  if (H) {
    const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
    *H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2,  //
        Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1,   //
        q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
  }
  return M;
}

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

}  // end namespace gtsam


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

  Program:   Small Body Geophysical Analysis
  Module:    SBGATMassProperties.hpp

  Class derived from VTK's vtkPolyDataAlgorithm by Benjamin Bercovici

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/**
\file SBGATMassProperties.hpp
\class  SBGATMassProperties
\author Benjamin Bercovici
\author Jay McMahon
\brief  Computes volume, area, shape index, center of mass,
inertia tensor and principal axes of a polyhedral mesh of constant density
\details Computes the volume, the surface area, and the
normalized shape index, center of mass and inertia tensor of a topologically-closed, constant-density polyhedron.
This class will always use results expressed in `meters` as their distance unit (e.g center-of-mass coordinates in meters, volume in m^3,...) . Unit consistency is enforced through the use of the SetScaleMeters()
and SetScaleKiloMeters() method.

See "Inertia of Any Polyhedron" by Anthony R. Dobrovolskis, Icarus 124, 698–704 (1996) Article No. 0243
for further details.  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
\copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATMassProperties_h
#define SBGATMassProperties_h

#include <vtkFiltersCoreModule.h> // For export macro
#include <vtkPolyDataAlgorithm.h>
#include <armadillo>
#include <SBGATFilter.hpp>

class VTKFILTERSCORE_EXPORT SBGATMassProperties : public SBGATFilter{
public:
  /**
   * Constructs with initial values of zero.
   */
  static SBGATMassProperties *New();

  vtkTypeMacro(SBGATMassProperties,vtkPolyDataAlgorithm);
  void PrintSelf(std::ostream& os, vtkIndent indent) override;
  void PrintHeader(std::ostream& os, vtkIndent indent) override;
  void PrintTrailer(std::ostream& os, vtkIndent indent) override;

  /**
   * Compute and return the volume (m^3)
   */
  double GetVolume() const { return this->Volume;}

  /**
   * Compute and return the projected volume.
   * Typically you should compare this volume to the value returned by GetVolume
   * if you get an error (GetVolume()-GetVolumeProjected())*10000 that is greater
   * than GetVolume() this should identify a problem:
   * * Either the polydata is not closed
   * * Or the polydata contains triangle that are flipped
   */
  double GetVolumeProjected() const { return this->VolumeProjected;}

  /**
   * Compute and return the volume projected on to each axis aligned plane.
   */
  double GetVolumeX() { return this->VolumeX;}
  double GetVolumeY() { return this->VolumeY;}
  double GetVolumeZ() { return this->VolumeZ;}

  /**
   * Compute and return the weighting factors for the maximum unit
   * normal component (MUNC).
   */
  double GetKx() const { return this->Kx;}
  double GetKy() const { return this->Ky;}
  double GetKz() const { return this->Kz;}

  /**
   * Compute and return the area in m^2
   */
  double GetSurfaceArea()  const{ return this->SurfaceArea;
  }

  /**
   * Compute and return the min cell area in m^2
   */
  double GetMinCellArea()  const{ return this->MinCellArea;
  }

  /**
   * Compute and return the max cell area in m^2
   */
  double GetMaxCellArea()  const{ return this->MaxCellArea;
  }

  /**
  Checks whether the polydata is topologically closed or open
  If closed, the sum of the oriented surface area should be equal to zero
  */
  bool CheckClosed() const{ return this -> IsClosed;}

  /**
   * Compute and return the normalized shape index. This characterizes the
   * deviation of the shape of an object from a sphere. A sphere's NSI
   * is one. This number is always >= 1.0.
   */
  double GetNormalizedShapeIndex() const
  { return this->NormalizedShapeIndex;
  }

  /**
  * Compute and return the coordinates of the center of mass (m)
  * evaluated in the frame of origin assuming a constant density distribution
  * across the shape
  */
  const arma::vec::fixed<3> & GetCenterOfMass() const {
   return this -> center_of_mass;
 }

  /**
  * Compute and return the coordinates of the center of mass (m)
  * evaluated in the frame of origin assuming a constant density distribution
  * across the shape
  */
 void GetCenterOfMass(double * com) const{

  com[0] = this -> center_of_mass(0);
  com[1] = this -> center_of_mass(1);
  com[2] = this -> center_of_mass(2);
}

  /**
  * Compute and return the dimensionless inertia tensor
  * evaluated in the frame of origin assuming a constant density distribution
  * across the shape. The normalization applied to the inertia tensor is I_norm = I / (mass * r_avg ^ 2) where r_avg = cbrt(3/4*Volume/pi)
  */
arma::mat::fixed<3,3> GetNormalizedInertiaTensor() const {
 return this -> inertia_tensor;
}

  /**
  * Compute and return the dimensionless inertia tensor
  * evaluated in the frame of origin assuming a constant density distribution
  * across the shape. The normalization applied to the inertia tensor is I_norm = I / (rho) where rho is the density
  */
arma::mat::fixed<3,3> GetUnitDensityInertiaTensor() const{
 return unit_density_inertia_tensor;
}

  /**
  * Compute and return the dcm orienting the principal axes of the small body relative to
  the body coordinates frame. That is, denoting P the principal frame and B the frame in which the
  coordinates of the body are currently expressed, this method returns [PB]
  @return [PB] direction cosine matrix
  */
arma::mat::fixed<3,3> GetPrincipalAxes() const{
 return this -> principal_axes;
}

  /**
  Computes and returns the principal dimensions (m) of the ellipsoid associated with the inertia tensor
  tensor, sorted from the longest (smallest inertia) to shortest (largest inertia)
  @return principal dimensions associated with inertia tensor (m)
  */
arma::vec::fixed<3> GetPrincipalDimensions() const {
 return this -> principal_dimensions;
}

  /**
  * Compute and return the normalized inertia moments assuming uniform density distribution
  * across the shape, sorted from the smallest inertia to the largest.
  * The normalization applied to the inertia tensor is I_norm = I / (mass * r_avg ^ 2) where r_avg = cbrt(3/4*Volume/pi)
  */
arma::vec::fixed<3> GetNormalizedInertiaMoments() const {
 return normalized_principal_moments;
}

  /**
  * Compute and return the inertia moments assuming uniform unit density distribution
  * across the shape, sorted from the smallest inertia to the largest.
  */
arma::vec::fixed<3> GetUnitDensityInertiaMoments() const {
 return unit_density_principal_moments;
}

  /**
  Return the average radius of the shape (that is, the radius of a sphere occupying the same volume) (m)
  */
double GetAverageRadius() const {
 return this -> r_avg;
}

    /**
    Computes the mass properties of the provided shape and saves the results to a JSON file
    @param shape pointer to considered shape
    @param path savepath (ex: "mass_properties.json")
    */
static void ComputeAndSaveMassProperties(vtkSmartPointer<vtkPolyData> shape,std::string path);

  /**
  Save the computed mass properties to a JSON file
  @param path savepath (ex: "mass_properties.json")

  */
void SaveMassProperties(std::string path) const ;

/**
  Return signed contribution to total volume of tetrahedron subtended by facet
  f (m^3)
  @param f facet index
  @return signed volume of tetrahedron subtended by facet
  */
double GetDeltaV(const int & f) const;

  /**
  Return coordinates of the tetrahedron's center-of-mass (m)
  @param f facet index
  @return coordinates of tetrahedron
  */
arma::vec::fixed<3> GetDeltaCM(const int & f) const ;

  /**
  Return the unit-density tetrahedron's inertia tensor divided by tetrahedron's signed volume (m^2)
  @param f facet index
  @return tetrahedron's inertia tensor divided by tetrahedron's signed volume
  */
arma::mat::fixed<3,3> GetDeltaIOverDeltaV(const int & f) const ;

/**
Return the parametrization of the the unit-density tetrahedron's inertia tensor
@param f facet index
@return parametrization of the tetrahedron's inertia tensor
*/
arma::vec::fixed<6> GetDeltaIf(const int & f) const;

protected:
  SBGATMassProperties();
  ~SBGATMassProperties() override;

  int RequestData(vtkInformation* request,
    vtkInformationVector** inputVector,
    vtkInformationVector* outputVector) override;

  arma::vec::fixed<3> center_of_mass;
  arma::mat::fixed<3,3> inertia_tensor;
  arma::mat::fixed<3,3> principal_axes;

  arma::vec::fixed<3> normalized_principal_moments;
  arma::vec::fixed<3> unit_density_principal_moments;
  arma::mat::fixed<3,3> unit_density_inertia_tensor;
  arma::vec::fixed<3> principal_dimensions;

  double  SurfaceArea;
  double  MinCellArea;
  double  MaxCellArea;
  double  Volume;
  double  VolumeProjected;
  double  VolumeX;
  double  VolumeY;
  double  VolumeZ;
  double  Kx;
  double  Ky;
  double  Kz;
  double  NormalizedShapeIndex;
  double r_avg;
  bool IsClosed;

private:
  SBGATMassProperties(const SBGATMassProperties&) = delete;
  void operator=(const SBGATMassProperties&) = delete;
};

#endif


//  (C) Copyright John Maddock 2007.
//  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)

#include "mp_t.hpp"
#include <boost/math/special_functions/expint.hpp>
#include <boost/math/constants/constants.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>

using namespace boost::math::tools;

struct expint_data_generator
{
   mp_t operator()(mp_t a, mp_t b)
   {
      unsigned n = boost::math::tools::real_cast<unsigned>(a);
      std::cout << n << "  " << b << "  ";
      mp_t result = boost::math::expint(n, b);
      std::cout << result << std::endl;
      return result;
   }
};

int main()
{
   boost::math::expint(1, 0.06227754056453704833984375);
   std::cout << boost::math::expint(1, mp_t(0.5)) << std::endl;

   parameter_info<mp_t> arg1, arg2;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the expint function:\n"
      "  expint(a, b)\n\n";

   bool cont;
   std::string line;

   do{
      get_user_parameter_info(arg1, "a");
      get_user_parameter_info(arg2, "b");
      data.insert(expint_data_generator(), arg1, arg2);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=expint_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "expint_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, "expint_data");

   return 0;
}


// ----------------------------------------------------------------------------
// FILENAME: timetest.cpp
//
// DESCRIPTION:
//    This file contians the function that used for save the running time of
//    computing polynomial real root isolation
//
// AUTHOR: Xinlong Yi
//
// ----------------------------------------------------------------------------

#include "budan.h"
#include "poly.h"
#include "range.h"
#include "vincent.h"
#include <boost/numeric/interval/utility_fwd.hpp>
#include <chrono>
#include <time.h>

static const int kTESTDEGREE = 7;
static const int digit = 4; // number of digit after point
static const int digit_control = std::pow(10, digit); // controler of digit

static const double max_root = 10000;
// static const double max_root = std::pow(2, kTESTDEGREE);

/**
 * Get random double in range min to max
 */
double rand_double(double min, double max) {
  double f = (double)rand() / RAND_MAX;
  f = min + f * (max - min);
  f = std::ceil(f * digit_control) / digit_control;
  return f;
}

int main() {
  srand(time(NULL));

  // Get random polynomial
  double *coeffs = new double[kTESTDEGREE];

  double budan_total = 0, vincent_total = 0;

  for (int i = 0; i < 1000; i++) {

    for (size_t i = 0; i <= kTESTDEGREE; i++) {
      coeffs[i] = rand_double(-max_root, max_root);
    }

    Poly<kTESTDEGREE + 1> tt(coeffs, kTESTDEGREE);

    std::cout << tt << std::endl;

    // save roots
    Range *roots = new Range[kTESTDEGREE];

    // Budan
    auto budan_start = std::chrono::high_resolution_clock::now();
    BudanRootIsolate(coeffs, kTESTDEGREE, roots);
    auto budan_end = std::chrono::high_resolution_clock::now();

    // Vincent
    auto vincent_start = std::chrono::high_resolution_clock::now();
    VincentRootIsolate(coeffs, kTESTDEGREE, roots);
    auto vincent_end = std::chrono::high_resolution_clock::now();

    //     Time
    auto budan_duration = std::chrono::duration_cast<std::chrono::microseconds>(
        budan_end - budan_start);
    auto vincent_duration =
        std::chrono::duration_cast<std::chrono::microseconds>(vincent_end -
                                                              vincent_start);

    budan_total += budan_duration.count();
    vincent_total += vincent_duration.count();
  }

  std::cout << "Budan Theorem takes " << budan_total / 1000.0 << " us for "
            << kTESTDEGREE - 1 << " degree" << std::endl;

  std::cout << "Continued Fraction takes " << vincent_total / 1000.0
            << " us for " << kTESTDEGREE - 1 << " degree" << std::endl;

  return 0;
}