#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <Python.h>
#include <boost/python.hpp>
#include <numpy/arrayobject.h>
using namespace boost::python;

#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;

#include <mrpt/config.h>

#include <mrpt/vision/pnp_algos.h>
mrpt::vision::pnp::CPnP pnp_algos;

#if MRPT_HAS_OPENCV
    #include <opencv2/opencv.hpp>
    #include <opencv2/core/eigen.hpp>
    using namespace cv;
#endif

class PnPAlgos
{
public:
	PnPAlgos( int new_m );
	~PnPAlgos();
	#if MRPT_HAS_OPENCV
        int epnp_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);
        int dls_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);
        int upnp_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);
    #endif
    int p3p_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);
    int ppnp_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);
	int rpnp_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);
	int posit_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);
	int lhm_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);
	int so3_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat);

private:
	int dummy;
};

PnPAlgos::PnPAlgos( int new_m ){
	dummy = new_m;
    #if MRPT_HAS_OPENCV
    std::cout <<" Using OpenCV dependency for PnP Algorithms - EPnP, DLS-PnP, UPnP(Broken) " << std::endl << std::endl;
    #else
    std::cout << " Initializing PnP class " << std::endl << std::endl;
    #endif
}
PnPAlgos::~PnPAlgos(){
}
#if MRPT_HAS_OPENCV
    int PnPAlgos::epnp_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
        Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
        Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
        Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
        Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

        return pnp_algos.epnp(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
    }

    int PnPAlgos::dls_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
        Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
        Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
        Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
        Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

        return pnp_algos.dls(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
    }

    int PnPAlgos::upnp_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
        Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
        Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
        Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
        Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

        return pnp_algos.upnp(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
    }
#endif

int PnPAlgos::p3p_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
    Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
    Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
    Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
    Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

    return pnp_algos.p3p(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::ppnp_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
	Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
	Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
	Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
	Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

	return pnp_algos.ppnp(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::rpnp_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
	Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
	Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
	Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
	Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

	return pnp_algos.rpnp(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::posit_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
	Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
	Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
	Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
	Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

	return pnp_algos.posit(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::lhm_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
	Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
	Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
	Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
	Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

	return pnp_algos.lhm(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::so3_solve(PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic, PyObject* pose_mat){
	Map<MatrixXd> _obj_pts((double *) PyArray_DATA((PyArrayObject*)obj_pts),3,n);
	Map<MatrixXd> _img_pts((double *) PyArray_DATA((PyArrayObject*)img_pts),3,n);
	Map<MatrixXd> _pose_mat((double *) PyArray_DATA((PyArrayObject*)pose_mat),6,1);
	Map<MatrixXd> _cam_intrinsic((double *) PyArray_DATA((PyArrayObject*)cam_intrinsic),3,3);

	return pnp_algos.so3(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

void export_pnp()
{
    class_<PnPAlgos>("pnp", init<int>(args("m")))
        #if MRPT_HAS_OPENCV
            .def("epnp", &PnPAlgos::epnp_solve)
            .def("dls", &PnPAlgos::dls_solve)
            .def("upnp", &PnPAlgos::upnp_solve)
        #endif
        .def("p3p", &PnPAlgos::p3p_solve)
        .def("ppnp", &PnPAlgos::ppnp_solve)
        .def("rpnp", &PnPAlgos::rpnp_solve)
        .def("posit", &PnPAlgos::posit_solve)
        .def("lhm", &PnPAlgos::lhm_solve)
        .def("so3", &PnPAlgos::so3_solve)
    ;
}


#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/MPRealSupport>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <iomanip>
#include <iostream>

#include "gauss_hermite_quadrature.hpp"
#include "spectral/mpfr/import_std_math.hpp"

//#include <quadmath.h>

namespace boltzmann {

namespace mp = boost::multiprecision;
using namespace mpfr;

typedef mp::mpfr_float_backend<100000> mfloat_t;
typedef mp::number<mfloat_t> mpfr_float_t;

using namespace std;

void gauss_hermite_roots(std::vector<double>& roots, const int N, const int ndigits)
{
  mpreal::set_default_prec(ndigits);
  typedef mpreal mfloat_t;
  //  typedef double mfloat_t;
  typedef Eigen::Matrix<mfloat_t, Eigen::Dynamic, Eigen::Dynamic> MatrixXmp;
  MatrixXmp A(N, N);
  A.fill(0);
  for (int i = 0; i < N - 1; ++i) {
    const double beta = ::math::sqrt(1.0 * (i + 1)) / ::math::sqrt(2.0);
    A(i, i + 1) = beta;
    A(i + 1, i) = beta;
  }

  Eigen::SelfAdjointEigenSolver<MatrixXmp> eigensolver;
  // Eigen::EigenSolver<MatrixXmp> eigensolver;
  eigensolver.compute(A, Eigen::EigenvaluesOnly);

  const auto w = eigensolver.eigenvalues();

  for (int i = 0; i < N; ++i) {
    roots[i] = w(i).toDouble();
  }

  // sort
  std::sort(roots.begin(), roots.end());
}
}  // end namespace boltzmann


#include <iostream>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include<opencv2/core/eigen.hpp>
#include <chrono>
#include <sophus/se3.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>

using namespace std;
using namespace cv;
using namespace Eigen;

// 相机内参
Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

void pose_estimation_3d3d(const vector<Point3d> &pts1, const vector<Point3d> &pts2,
                          Mat &R, Mat &t);

void bundelAdjustment(const vector<Point3d> &pts1, const vector<Point3d> &pts2,
                      Mat &R, Mat &t);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p);

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

  // 读取图像
  Mat img_1 = imread("../1.png", CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread("../2.png", CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data);
  cout << "读取图像 完成！" << endl;

  // 特征点匹配
  cout << "开始特征点匹配 ......" << endl;
  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "特征点匹配 完成！ 一共找到了" << matches.size() << "组匹配点" << endl << endl;
  // for (DMatch m:matches) {
  //   cout << keypoints_1[m.queryIdx].pt.x << " " << keypoints_1[m.queryIdx].pt.y << endl;
  // }

  // 读取深度图，建立3D点
  Mat img_depth_1 = imread("../1_depth.png", CV_LOAD_IMAGE_UNCHANGED);
  Mat img_depth_2 = imread("../2_depth.png", CV_LOAD_IMAGE_UNCHANGED);

  vector<Point3d> points_1;
  vector<Point3d> points_2;

  for (DMatch m:matches){
    ushort d1 = img_depth_1.at<unsigned short>((int)keypoints_1[m.queryIdx].pt.y, (int)keypoints_1[m.queryIdx].pt.x);
    ushort d2 = img_depth_2.at<unsigned short>((int)keypoints_2[m.trainIdx].pt.y, (int)keypoints_2[m.trainIdx].pt.x);

    if (d1==0 || d2==0) continue;

    float dd2 = d2 / 5000.0;
    float dd1 = d1 / 5000.0;

    Point2d p1_2d = pixel2cam(keypoints_1[m.queryIdx].pt);
    Point2d p2_2d = pixel2cam(keypoints_2[m.trainIdx].pt);

    points_1.push_back(Point3d(p1_2d.x * dd1, p1_2d.y*dd1, dd1));
    points_2.push_back(Point3d(p2_2d.x * dd2, p2_2d.y*dd2, dd2));
  }
  cout << "valid 3d-3d pairs: " << points_1.size() << endl;

  Mat R, t;
  cout << "开始SVD求解 ......" << endl;
  pose_estimation_3d3d(points_1, points_2, R, t);
  cout << "ICP via SVD results: " << endl;
  cout << "R = " << R << endl;
  cout << "t = " << t << endl;
  cout << endl;

  cout << "开始BA求解 ......" << endl;
  bundelAdjustment(points_1, points_2, R, t);
  cout << "R = " << R << endl;
  cout << "t = " << t << endl;
  cout << endl;

  return 0;
}

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};

class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose>{
  public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point): _point(point){}

  virtual void computeError() override {
    const VertexPose *pose = static_cast<const VertexPose *>(_vertices[0]);
    _error = _measurement - pose->estimate() * _point;
  }

  virtual void linearizeOplus() override {
    const VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
    Sophus::SE3d T = pose->estimate();
    Eigen::Vector3d xyz_trans = T * _point;
    _jacobianOplusXi.block<3, 3>(0, 0) = - Eigen::Matrix3d::Identity();
    _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
  }

  bool read(istream &in) {}
  bool write(ostream &out) const {}

  protected:
  Eigen::Vector3d _point;
};

void bundelAdjustment(const vector<Point3d> &pts2, const vector<Point3d> &pts1,
                      Mat &R, Mat &t){
  typedef g2o::BlockSolverX BlockSolverType;
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;

  auto solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;
  optimizer.setAlgorithm(solver);
  optimizer.setVerbose(true);

  VertexPose *vertex_pose = new VertexPose();
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

  for(size_t i=0; i < pts1.size(); i++){
    EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x,
    pts2[i].y, pts2[i].z));
    edge->setId(i);
    edge->setVertex(0, vertex_pose);
    edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));
    edge->setInformation(Eigen::Matrix3d::Identity());
    optimizer.addEdge(edge);
  }

  optimizer.initializeOptimization();
  optimizer.optimize(10);
  cout << "T=\n" << vertex_pose->estimate().matrix() << endl;

  Eigen::Matrix3d R_ = vertex_pose->estimate().rotationMatrix();
  Eigen::Vector3d t_ = vertex_pose->estimate().translation();

  cv::eigen2cv(R_, R);
  cv::eigen2cv(t_, t);
}

// 注意pts2和pts1顺序
void pose_estimation_3d3d(const vector<Point3d> &pts2, const vector<Point3d> &pts1,
                          Mat &R, Mat &t){

  assert(pts1.size() == pts2.size());
  int N = pts1.size();

  Point3d p1, p2;
  for (int i=0; i<N; i++){
    p1 += pts1[i];
    p2 += pts2[i];
  }

  p1 = p1 / N;
  p2 = p2 / N;

  vector<Point3d> q1(N), q2(N);
  for (int i=0; i<N; i++){
    q1[i] = pts1[i] - p1;
    q2[i] = pts2[i] - p2;
  }

  Matrix3d W = Matrix3d::Zero();
  for (int i=0; i<N; i++){
    Vector3d q1_eig(q1[i].x, q1[i].y, q1[i].z);
    Vector3d q2_eig(q2[i].x, q2[i].y, q2[i].z);
    W += q1_eig * q2_eig.transpose();
  }
  cout << "W = " << W << endl;

  Eigen::JacobiSVD<Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  cout << "U=" << U << endl;
  cout << "V=" << V << endl;

  Eigen::Matrix3d R_eig = U * V.transpose();
  if (R_eig.determinant() < 0){
    R_eig = -R_eig;
  }

  Vector3d p1_eig(p1.x, p1.y, p1.z);
  Vector3d p2_eig(p2.x, p2.y, p2.z);
  Eigen::Vector3d t_eig = p1_eig - R_eig*p2_eig;

  cv::eigen2cv(R_eig, R);
  cv::eigen2cv(t_eig, t);
}

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches){

    Mat descriptors_1, descriptors_2;
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");

    // --第一步：检测Oriented Fast角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);
    cout << "--第一步完成：检测Oriented Fast角点位置" << endl;

    // --第二步：根据角点位置计算BRIEF描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);
    cout << "--第二步完成：根据角点位置计算BRIEF描述子" << endl;

    // -- 第三步：对两幅图像中的BRIEF描述子进行匹配，使用Hamming距离
    vector<DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);
    cout << "--第三步完成：对两幅图像中的BRIEF描述子进行匹配，使用Hamming距离" << endl;

    // --第四步：匹配点对 筛选
    auto min_max = minmax_element(match.begin(), match.end(),
                [] (const DMatch &m1, const DMatch &m2) {return m1.distance < m2.distance;});
    double min_dist = min_max.first->distance;
    double max_dist = min_max.second->distance;

    for (int i=0; i<descriptors_1.rows; i++){
        if(match[i].distance <= max(2*min_dist, 30.0)){
            matches.push_back(match[i]);
        }
    }
    cout << "--第四步完成：匹配点对 筛选" << endl;
}

Point2d pixel2cam(const Point2d &p) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}


#include <CGAL/minkowski_sum_2.h>
#include <CGAL/Polygon_vertical_decomposition_2.h>
#include <CGAL/Polygon_triangulation_decomposition_2.h>
#include <CGAL/Boolean_set_operations_2.h>
#include <CGAL/Small_side_angle_bisector_decomposition_2.h>

#include "read_polygon.h"

#include <string.h>
#include <list>
#include <boost/timer.hpp>

typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
typedef CGAL::Polygon_2<Kernel> Polygon_2;
typedef CGAL::Polygon_with_holes_2<Kernel> Polygon_with_holes_2;

bool are_equal(const Polygon_with_holes_2& ph1,
               const Polygon_with_holes_2& ph2)
{
  std::list<Polygon_with_holes_2> sym_diff;
  CGAL::symmetric_difference(ph1, ph2, std::back_inserter(sym_diff));
  return sym_diff.empty();
}

typedef enum {
  REDUCED_CONVOLUTION,
  VERTICAL_DECOMPOSITION,
  TRIANGULATION_DECOMPOSITION,
  VERTICAL_AND_ANGLE_BISECTOR_DECOMPOSITION,
  TRIANGULATION_AND_ANGLE_BISECTOR_DECOMPOSITION,
  OPTIMAL_DECOMPOSITION,
} Strategy;

static const char* strategy_names[] = {
  "reduced convolution",
  "vertical decomposition",
  "constrained triangulation decomposition",
  "vertical and angle bisector decomposition",
  "constrained triangulation and angle bisector decomposition",
  "optimal decomposition"
};

Polygon_with_holes_2 compute_minkowski_sum_2(Polygon_with_holes_2& p,
                                             Polygon_with_holes_2& q,
                                             Strategy strategy)
{
  switch (strategy) {
   case REDUCED_CONVOLUTION:
     return CGAL::minkowski_sum_by_reduced_convolution_2(p, q);

   case VERTICAL_DECOMPOSITION:
    {
     CGAL::Polygon_vertical_decomposition_2<Kernel> decomp;
     return CGAL::minkowski_sum_2(p, q, decomp);
    }

   case TRIANGULATION_DECOMPOSITION:
    {
     CGAL::Polygon_triangulation_decomposition_2<Kernel> decomp;
     return CGAL::minkowski_sum_2(p, q, decomp);
    }

   case VERTICAL_AND_ANGLE_BISECTOR_DECOMPOSITION:
    {
     typedef CGAL::Small_side_angle_bisector_decomposition_2<Kernel>
                                                No_holes_decomposition;
     typedef CGAL::Polygon_vertical_decomposition_2<Kernel>
                                                With_holes_decomposition;

     if (0 == p.number_of_holes()) {
       const Polygon_2& pnh = p.outer_boundary();
       No_holes_decomposition decomp_no_holes;
       if  (0 == q.number_of_holes()) {
         const Polygon_2& qnh = q.outer_boundary();
         return CGAL::minkowski_sum_2(pnh, qnh, decomp_no_holes, decomp_no_holes);
       }

       With_holes_decomposition decomp_with_holes;
       return CGAL::minkowski_sum_2(pnh, q, decomp_no_holes, decomp_with_holes);
     }

     With_holes_decomposition decomp_with_holes;
     if (0 == q.number_of_holes()) {
       const Polygon_2& qnh = q.outer_boundary();
       No_holes_decomposition decomp_no_holes;
       return CGAL::minkowski_sum_2(p, qnh, decomp_with_holes, decomp_no_holes);
     }

     return CGAL::minkowski_sum_2(p, q, decomp_with_holes, decomp_with_holes);
    }

   case TRIANGULATION_AND_ANGLE_BISECTOR_DECOMPOSITION:
    {
     typedef CGAL::Small_side_angle_bisector_decomposition_2<Kernel>
                                                No_holes_decomposition;
     typedef CGAL::Polygon_triangulation_decomposition_2<Kernel>
                                                With_holes_decomposition;
     if (0 == p.number_of_holes()) {
       const Polygon_2& pnh = p.outer_boundary();
       No_holes_decomposition decomp_no_holes;
       if (0 == q.number_of_holes()) {
         const Polygon_2& qnh = q.outer_boundary();
         return CGAL::minkowski_sum_2(pnh, qnh, decomp_no_holes, decomp_no_holes);
       }

       With_holes_decomposition decomp_with_holes;
       return CGAL::minkowski_sum_2(pnh, q, decomp_no_holes, decomp_with_holes);
     }

     With_holes_decomposition decomp_with_holes;
     if (0 == q.number_of_holes()) {
       const Polygon_2& qnh = q.outer_boundary();
       No_holes_decomposition decomp_no_holes;
       return CGAL::minkowski_sum_2(p, qnh, decomp_with_holes, decomp_no_holes);
     }

     return CGAL::minkowski_sum_2(p, q, decomp_with_holes, decomp_with_holes);
    }

   case OPTIMAL_DECOMPOSITION:
    {
     CGAL::Small_side_angle_bisector_decomposition_2<Kernel> decomp_no_holes;
     CGAL::Polygon_triangulation_decomposition_2<Kernel> decomp_with_holes;
     return CGAL::minkowski_sum_by_decomposition_2(p, q,
                                                   decomp_no_holes,
                                                   decomp_with_holes);
    }

   default:
    std::cerr << "Invalid strategy" << std::endl;
    return Polygon_with_holes_2();
  }
}

int main(int argc, char* argv[])
{
  if (argc < 2) {
    std::cerr << "Usage: " << argv[0] << " [-method flag] [polygon files]..."
              << std::endl;
    std::cerr << "For the method flag, use a subset of the letters 'rfsohg'."
              << std::endl;
    std::cerr << "The program will compute the Minkowski sum of the first "
              << "and second polygon, of the third and fourth, and so on."
              << std::endl;
    return 1;
  }

  Polygon_with_holes_2 p, q;
  boost::timer timer;

  std::list<Strategy> strategies;

  int i(1);
  while (i < argc) {
    if (argv[i][0] == '-') {
      strategies.clear();
      for (std::size_t j = 1; j < strlen(argv[i]); ++j) {
        switch (argv[i][j]) {
         case 'r': strategies.push_back(REDUCED_CONVOLUTION); break;
         case 'v': strategies.push_back(VERTICAL_DECOMPOSITION); break;
         case 't': strategies.push_back(TRIANGULATION_DECOMPOSITION); break;
         case 'w':
          strategies.push_back(VERTICAL_AND_ANGLE_BISECTOR_DECOMPOSITION);
          break;

         case 'u':
          strategies.push_back(TRIANGULATION_AND_ANGLE_BISECTOR_DECOMPOSITION);
          break;

         case 'd': strategies.push_back(OPTIMAL_DECOMPOSITION); break;
         default:
          std::cerr << "Unknown flag '" << argv[i][j] << "'" << std::endl;
          return -1;
        }
      }
      ++i;
      continue;
    }

    std::cout << "Testing " << argv[i] << " + " << argv[i+1] << std::endl;
    if (!read_polygon(argv[i], p)) return -1;
    if (!read_polygon(argv[i+1], q)) return -1;

    bool compare = false;
    Polygon_with_holes_2 reference;
    std::list<Strategy>::iterator it;
    for (it = strategies.begin(); it != strategies.end(); ++it) {
      std::cout << "Using " << strategy_names[*it] << ": ";
      timer.restart();
      Polygon_with_holes_2 result = compute_minkowski_sum_2(p, q, *it);
      double secs = timer.elapsed();
      std::cout << secs << " s " << std::flush;

      if (compare) {
        if (are_equal(reference, result)) std::cout << "(OK)";
        else {
          std::cout << "(ERROR: different result)";
          return 1;
        }
      }
      else {
        compare = true;
        reference = result;

        std::size_t n = result.outer_boundary().size();
        Polygon_with_holes_2::Hole_const_iterator it = result.holes_begin();
        while(it != result.holes_end()) n += (*it++).size();

        std::cout << std::endl << "Result has " << n << " vertices and "
                  << result.number_of_holes() << " holes.";
      }
      std::cout << std::endl;
    }

    std::cout << std::endl;
    i += 2;
  }

  return 0;
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//% Code implementing the paper "Accelerated Quadratic Proxy for Geometric Optimization", SIGGRAPH 2016.
//% Disclaimer: The code is provided as-is for academic use only and without any guarantees.
//%             Please contact the author to report any bugs.
//% Written by Shahar Kovalsky (http://www.wisdom.weizmann.ac.il/~shaharko/)
//%            Meirav Galun (http://www.wisdom.weizmann.ac.il/~/meirav/)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

#include "mex.h"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "mexHelpers.cpp"

using namespace Eigen;

void helperFcuntionalIsoDist2x2(VectorXd &pA, const VectorXd &areas, int dim, double& val, bool& flips)
{
	int block_size = dim*dim;
	int num_blocks = pA.size() / block_size;
	Map<Matrix2d> currA(pA.data(), dim, dim);
	Matrix2d currA_inv;

	// project
	val = 0;
	flips = 0;
	for (int ii = 0; ii < num_blocks; ii++)
	{
		// get current block
		new (&currA) Map<MatrixXd>(pA.data() + ii*block_size, dim, dim);
		// check inverse
		flips = flips || (currA.determinant() < 0);
		// compute inverse
		currA_inv = currA.inverse();
		val = val + areas(ii) * (currA.squaredNorm() + currA_inv.squaredNorm());
		// compute Tx_grad
		currA = 2 * areas(ii) * (currA - currA_inv.transpose()*currA_inv*currA_inv.transpose());
	}
}

void helperFcuntionalIsoDist3x3(VectorXd &pA, const VectorXd &areas, int dim, double& val, bool& flips)
{
	int block_size = dim*dim;
	int num_blocks = pA.size() / block_size;
	Map<Matrix3d> currA(pA.data(), dim, dim);
	Matrix3d currA_inv;

	// project
	val = 0;
	flips = 0;
	for (int ii = 0; ii < num_blocks; ii++)
	{
		// get current block
		new (&currA) Map<MatrixXd>(pA.data() + ii*block_size, dim, dim);
		// check inverse
		flips = flips || (currA.determinant() < 0);
		// compute inverse
		currA_inv = currA.inverse();
		val = val + areas(ii) * (currA.squaredNorm() + currA_inv.squaredNorm());
		// compute Tx_grad
		currA = 2 * areas(ii) * (currA - currA_inv.transpose()*currA_inv*currA_inv.transpose());
	}
}

void mexFunction(int nlhs, mxArray *plhs[],
	int nrhs, const mxArray*prhs[])
{
	// assign input
	int A_rows = mxGetM(prhs[0]); // # rows of A
	int A_cols = mxGetN(prhs[0]); // # cols of A
	int areas_rows = mxGetM(prhs[1]); // # rows of A
	int areas_cols = mxGetN(prhs[1]); // # cols of A
	double *dim;
	double val;
	bool flips;
	const Map<VectorXd> A(mxGetPr(prhs[0]), A_rows, A_cols);
	const Map<VectorXd> areas(mxGetPr(prhs[1]), areas_rows, areas_cols);
	dim = mxGetPr(prhs[2]);

	if (A_cols!=1)
		mexErrMsgIdAndTxt("MATLAB:wrong_input", "first argument must be a column vector");
	if (areas_cols != 1)
		mexErrMsgIdAndTxt("MATLAB:wrong_input", "second argument must be a column vector");

	// copy
	VectorXd pA(A_rows);
	pA = A;

	// compute
	if (*dim == 2)
		helperFcuntionalIsoDist2x2(pA, areas, *dim, val, flips);
	else if (*dim == 3)
		helperFcuntionalIsoDist3x3(pA, areas, *dim, val, flips);
	else
		mexErrMsgIdAndTxt("MATLAB:wrong_dimension", "dim must be either 2 or 3");

	// output
	plhs[0] = mxCreateDoubleScalar(val); // functional value
	mapDenseMatrixToMex(pA, &(plhs[1])); // return Tx_grad
	plhs[2] = mxCreateLogicalScalar(flips); // were there any flips
}


/*
 *
 * Independent implementation of
 *     Successive Convexification for 6-DoF Mars Rocket Powered Landing
 *     with Free-Final-Time (Michael Szmuk, Behcet Acikmese)
 *
 * https://arxiv.org/abs/1802.03827
 *
 */

#include "active_model.hpp"
#include "EcosWrapper.hpp"
#include "MosekWrapper.hpp"
#include "Discretization.hpp"
#include "SuccessiveConvexificationSOCP.hpp"
#include "timing.hpp"

#include <iostream>
#include <array>
#include <cmath>
#include <ctime>
#include <fstream>
#include <sstream>
#include <iomanip>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>

using std::array;
using std::cout;
using std::endl;
using std::ofstream;
using std::ostringstream;
using std::setw;
using std::setfill;

string get_output_path() {
    return "../output/" + Model::get_name() + "/";
}

void clear_output_path() {
    string command = "rm -r " + get_output_path();
    assert(system(command.c_str()) == 0);
}

void make_output_path() {
    string command = "mkdir -p " + get_output_path();
    assert(system(command.c_str()) == 0);
}

int main() {
    clear_output_path();
    make_output_path();
    Model model;

    double weight_trust_region_sigma = 1e-3;
    double weight_trust_region_xu = 1e-3;
    double weight_virtual_control = 1e3;

    double nu_tol = 1e-3;
    double delta_tol = 1e-3;

    const size_t n_states = Model::n_states;
    const size_t n_inputs = Model::n_inputs;

    Eigen::Matrix<double, n_states, K> X;
    Eigen::Matrix<double, n_inputs, K> U;

    model.initialize(X, U);

    double sigma = model.total_time_guess();

    array<Model::StateMatrix,   (K-1)> A_bar;
    array<Model::ControlMatrix, (K-1)> B_bar;
    array<Model::ControlMatrix, (K-1)> C_bar;
    array<Model::StateVector,   (K-1)> Sigma_bar;
    array<Model::StateVector,   (K-1)> z_bar;

    optimization_problem::SecondOrderConeProgram socp = build_successive_convexification_SOCP (
        model, weight_trust_region_sigma, weight_trust_region_xu, weight_virtual_control, X, U, sigma, A_bar, B_bar, C_bar, Sigma_bar, z_bar );

    // Cache indices for performance
    const size_t sigma_index = socp.get_tensor_variable_index("sigma", {});
    size_t X_indices[n_states][K];
    size_t U_indices[n_inputs][K];
    for (size_t k = 0; k < K; k++) {
        for (size_t i = 0; i < n_states; ++i) X_indices[i][k] = socp.get_tensor_variable_index("X",{i,k});
        for (size_t i = 0; i < n_inputs; ++i) U_indices[i][k] = socp.get_tensor_variable_index("U",{i,k});
    }

    EcosWrapper solver(socp);
//    MosekWrapper solver(socp);

    const size_t iterations = 40;
    for(size_t it = 0; it < iterations; it++) {

        weight_trust_region_xu *= 1.2;

        const double timer_total = tic();
        double timer = tic();
        calculate_discretization ( model, sigma, X, U, A_bar, B_bar, C_bar, Sigma_bar, z_bar );
        cout << "Time, discretization: " << toc(timer) << " ms" << endl;

        // Write problem to file
        timer = tic();
        string file_name_prefix;
        {
            ostringstream file_name_prefix_ss;
            file_name_prefix_ss << get_output_path() << "iteration"
            << setfill('0') << setw(3) << it << "_";
            file_name_prefix = file_name_prefix_ss.str();
        }

        {
            ofstream f(file_name_prefix + "problem.txt");
            socp.print_problem(f);
        }
        cout << "Time, problem file: " << toc(timer) << " ms" << endl;

        timer = tic();
        solver.solve_problem();
        cout << "Time, solver: " << toc(timer) << " ms" << endl;

//        timer = tic();
//        if(!socp.feasibility_check(solver.get_solution_vector())) {
//            cout << "ERROR: Solver produced an invalid solution." << endl;
//            return EXIT_FAILURE;
//        }
//        cout << "Time, solution check: " << toc(timer) << " ms" << endl;

        // Read solution
        for (size_t k = 0; k < K; k++) {
            for (size_t i = 0; i < n_states; ++i) X(i,k) = solver.get_solution_value(X_indices[i][k]);
            for (size_t i = 0; i < n_inputs; ++i) U(i,k) = solver.get_solution_value(U_indices[i][k]);
        }
        sigma = solver.get_solution_value(sigma_index);

        // Write solution to files
        timer = tic();
        {
            ofstream f(file_name_prefix + "X.txt");
            f << X;
        }
        {
            ofstream f(file_name_prefix + "U.txt");
            f << U;
        }
        cout << "Time, solution files: " << toc(timer) << " ms" << endl;

        cout << "sigma   " << sigma << endl;
        cout << "norm2_nu   " << solver.get_solution_value("norm2_nu", {}) << endl;
        cout << "Delta_sigma   " << solver.get_solution_value("Delta_sigma", {}) << endl;
        cout << "norm2_Delta   " << solver.get_solution_value("norm2_Delta", {}) << endl;
        cout << "Time, total: " << toc(timer_total) << " ms" << endl;
        cout << "==========================================================" << endl;

        if (solver.get_solution_value("norm2_Delta", {}) < delta_tol
           && solver.get_solution_value("norm2_nu", {}) < nu_tol){
            cout << "Converged after " << it << " iterations.";
            break;
        }
    }
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/four_point_focal_length.h"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <glog/logging.h>
#include <vector>

#include "theia/alignment/alignment.h"
#include "theia/sfm/pose/four_point_focal_length_helper.h"

namespace theia {
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {
void GetRigidTransform(const Matrix<double, 3, 4>& points1,
                       const Matrix<double, 3, 4>& points2,
                       const bool left_handed_coordinates,
                       Eigen::Matrix3d* rotation,
                       Vector3d* translation) {
  // Move the centroid to th origin.
  const Vector3d mean_points1 = points1.rowwise().mean();
  const Vector3d mean_points2 = points2.rowwise().mean();

  const Matrix<double, 3, 4> points1_shifted = points1.colwise() - mean_points1;
  const Matrix<double, 3, 4> points2_shifted = points2.colwise() - mean_points2;

  // Normalize to unit size.
  const Matrix<double, 3, 4> points1_normalized =
      points1_shifted.colwise().normalized();
  const Matrix<double, 3, 4> points2_normalized =
      points2_shifted.colwise().normalized();

  // Compute the necessary rotation from the difference in points.
  Matrix3d rotation_diff = points2_normalized * points1_normalized.transpose();
  Eigen::JacobiSVD<Matrix3d> svd(rotation_diff,
                                 Eigen::ComputeFullU | Eigen::ComputeFullV);

  Matrix3d s = Matrix3d::Zero();
  s(0, 0) = svd.singularValues()(0) < 0 ? -1.0 : 1.0;
  s(1, 1) = svd.singularValues()(1) < 0 ? -1.0 : 1.0;
  const double sign =
      (svd.matrixU() * svd.matrixV().transpose()).determinant() < 0 ? -1.0
                                                                    : 1.0;

  if (left_handed_coordinates) {
    s(2, 2) = -sign;
  } else {
    s(2, 2) = sign;
  }
  *rotation = svd.matrixU() * s * svd.matrixV().transpose();
  *translation = - *rotation * mean_points1 + mean_points2;
}

}  // namespace

int FourPointPoseAndFocalLength(
    const std::vector<Eigen::Vector2d>& feature_vectors,
    const std::vector<Eigen::Vector3d>& world_points_vector,
    std::vector<Eigen::Matrix<double, 3, 4> >* projection_matrices) {
  Eigen::Map<const Matrix<double, 2, 4> > features(feature_vectors[0].data());
  Eigen::Map<const Matrix<double, 3, 4> > world_points(world_points_vector[0]
                                                           .data());

  // Normalize the points such that the mean = 0, variance = sqrt(2.0).
  const Vector3d mean_world_point = world_points.rowwise().mean();
  Eigen::Matrix<double, 3, 4> world_point_normalized =
      world_points.colwise() - mean_world_point;
  const double world_point_variance =
      world_point_normalized.colwise().norm().mean();
  world_point_normalized /= world_point_variance;

  // Scale 2D data so variance = sqrt(2.0).
  const double features_variance = features.colwise().norm().mean();
  Eigen::Matrix<double, 2, 4> features_normalized =
      features / features_variance;

  // Precompute monomials.
  const double glab = (world_point_normalized.col(0) -
                       world_point_normalized.col(1)).squaredNorm();
  const double glac = (world_point_normalized.col(0) -
                       world_point_normalized.col(2)).squaredNorm();
  const double glad = (world_point_normalized.col(0) -
                       world_point_normalized.col(3)).squaredNorm();
  const double glbc = (world_point_normalized.col(1) -
                       world_point_normalized.col(2)).squaredNorm();
  const double glbd = (world_point_normalized.col(1) -
                       world_point_normalized.col(3)).squaredNorm();
  const double glcd = (world_point_normalized.col(2) -
                       world_point_normalized.col(3)).squaredNorm();

  if (glab * glac * glad * glbc * glbd * glcd < 1e-15) {
    return -1;
  }

  // Call the helper function.
  std::vector<double> focal_length;
  std::vector<Vector3d> depths;

  FourPointFocalLengthHelper(glab, glac, glad, glbc, glbd, glcd,
                             features_normalized, &focal_length, &depths);

  if (focal_length.size() == 0) {
    return -1;
  }

  // Get the rotation and translation.
  for (int i = 0; i < focal_length.size(); i++) {
    // Create world points in camera coordinate system.
    Matrix<double, 3, 4> adjusted_world_points;
    adjusted_world_points.block<2, 4>(0, 0) = features_normalized;
    adjusted_world_points.row(2).setConstant(focal_length[i]);
    adjusted_world_points.col(1) *= depths[i].x();
    adjusted_world_points.col(2) *= depths[i].y();
    adjusted_world_points.col(3) *= depths[i].z();

    // Fix the scale.
    Matrix<double, 6, 1> d;
    d(0) = sqrt(glab / (adjusted_world_points.col(0) -
                        adjusted_world_points.col(1)).squaredNorm());
    d(1) = sqrt(glac / (adjusted_world_points.col(0) -
                        adjusted_world_points.col(2)).squaredNorm());
    d(2) = sqrt(glad / (adjusted_world_points.col(0) -
                        adjusted_world_points.col(3)).squaredNorm());
    d(3) = sqrt(glbc / (adjusted_world_points.col(1) -
                        adjusted_world_points.col(2)).squaredNorm());
    d(4) = sqrt(glbd / (adjusted_world_points.col(1) -
                        adjusted_world_points.col(3)).squaredNorm());
    d(5) = sqrt(glcd / (adjusted_world_points.col(2) -
                        adjusted_world_points.col(3)).squaredNorm());

    const double gta = d.mean();

    adjusted_world_points *= gta;

    // Get the transformation by aligning the points.
    Matrix3d rotation;
    Vector3d translation;
    GetRigidTransform(world_point_normalized, adjusted_world_points, false,
                      &rotation, &translation);

    translation =
        world_point_variance * translation - rotation * mean_world_point;

    focal_length[i] *= features_variance;

    Matrix<double, 3, 4> transformation_matrix;
    transformation_matrix.block<3, 3>(0, 0) = rotation;
    transformation_matrix.col(3) = translation;
    Matrix3d camera_matrix =
        Eigen::DiagonalMatrix<double, 3>(focal_length[i], focal_length[i], 1.0);
    projection_matrices->push_back(camera_matrix * transformation_matrix);
  }
  return projection_matrices->size();
}

}  // namespace theia


////////////////////////////////////////////////////////////////////////////////
// MetricFittingEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Sheet material energy that is a function of the **deformation gradient**.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  05/30/2019 17:15:18
////////////////////////////////////////////////////////////////////////////////
#ifndef METRICFITTINGENERGY_HH
#define METRICFITTINGENERGY_HH

#include <Eigen/Dense>
#include <MeshFEM/EnergyDensities/Tensor.hh>

struct MetricFittingEnergy {
    using M2d = Eigen::Matrix2d;

    M2d targetMetric;

    void setMatrix(Eigen::Ref<const M2d> C) {
        m_C = C;
    }

    double energy() const {
        return 0.5 * (m_C - targetMetric).squaredNorm();
    }

    M2d denergy() const { return m_C - targetMetric; }

    auto delta_denergy(Eigen::Ref<const M2d> dC) const { return dC; } // 4th order identity tensor

    M2d currMetric() const {
        return m_C;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    M2d m_C, m_diff;
};

#endif /* end of include guard: METRICFITTINGENERGY_HH */


/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Solving 2D Poisson + Drift Diffusion semiconductor eqns for a solar cell using
%                      Scharfetter-Gummel discretization
%
%                         Written by Timofey Golubev
%
%     This includes the 2D poisson equation and 2D continuity/drift-diffusion
%     equations using Scharfetter-Gummel discretization. The Poisson equation
%     is solved first, and the solution of potential is used to calculate the
%     Bernoulli functions and solve the continuity eqn's.
%
%   Boundary conditions for Poisson equation are:
%
%     -a fixed voltage at (x,0) and (x, Nz) defined by V_bottomBC
%      and V_topBC which are defining the  electrodes
%
%    -insulating boundary conditions: V(0,z) = V(1,z) and
%     V(0,N+1) = V(1,N) (N is the last INTERIOR mesh point).
%     so the potential at the boundary is assumed to be the same as just inside
%     the boundary. Gradient of potential normal to these boundaries is 0.
%
%   Matrix equations are AV*V = bV, Ap*p = bp, and An*n = bn where AV, Ap, and An are sparse matrices
%   (generated using spdiag), for the Poisson and continuity equations.
%   V is the solution for electric potential, p is the solution for hole
%   density, n is solution for electron density
%   bV is the rhs of Poisson eqn which contains the charge densities and boundary conditions
%   bp is the rhs of hole continuity eqn which contains net generation rate
%   and BCs
%
%     The code as is will calculate data for a JV curve
%     as well as carrier densities, current densities, and electric field
%     distributions of a generic solar cell made of an active layer and electrodes.
%     More equations for carrier recombination can be easily added.f
%
%     Photogeneration rate will be inputed from gen_rate.inp file
%     (i.e. the output of an optical model can be used) or an analytic expression
%     for photogeneration rate can be added to photogeneration.cpp. Generation rate file
%     should contain num_cell-2 number of entries in a single column, corresponding to
%     the the generation rate at each mesh point (except the endpoints).
%
%     The code can also be applied to non-illuminated devices by
%     setting photogeneration rate to 0.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include <iostream>
#include <vector>
#include <iomanip>
#include <algorithm>   //allows to use fill and min
#include <fstream>
#include <chrono>
#include <string>
#include <time.h>
#include <fstream>
#include <string>

#include <Eigen/Sparse>
#include <Eigen/Dense>
#include<Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCholesky>
#include<Eigen/SparseQR>
#include <Eigen/OrderingMethods>
#include<Eigen/SparseLU>
#include <unsupported/Eigen/CXX11/Tensor>  //allows for 3D matrices (Tensors)

#include "constants.h"        //these contain physics constants only
#include "parameters.h"
#include "poisson.h"
#include "continuity_p.h"
#include "continuity_n.h"
#include "recombination.h"
#include "photogeneration.h"
#include "Utilities.h"

int main()
{
    std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();  //start clock timer
    Parameters params;    //params is struct storing all parameters
    params.Initialize();  //reads parameters from file

    const int num_cell = params.num_cell;   //create a local num_cell so don't have to type params.num_cell everywhere

    const int num_V = static_cast<int>(floor((params.Va_max-params.Va_min)/params.increment))+1;  //floor returns double, explicitely cast to int
    params.tolerance_eq = 100.*params.tolerance_i;
    const int N = params.num_cell -1;
    const int num_rows = N*N*N;  //number of rows in the solution vectors (V, n, p)
    //NOTE: num_rows is the same as num_elements

    std::ofstream JV;
    JV.open("JV.txt");  //note: file will be created inside the build directory

    //-------------------------------------------------------------------------------------------------------
    //Initialize other vectors
    //Will use indicies for n and p... starting from 1 --> since is more natural--> corresponds to 1st node inside the device...
    //NOTE: ALL THESE INCLUDE THE INTERIOR ELEMENTS ONLY
    std::vector<double> n(num_rows+ 1), p(num_rows+ 1), oldp(num_rows+ 1), newp(num_rows+ 1), oldn(num_rows+ 1), newn(num_rows+ 1);
    std::vector<double> oldV(num_rows+ 1), newV(num_rows+ 1), V(num_rows+ 1);

    //create matrices to hold the V, n, and p values (including those at the boundaries) according to the (x,z) coordinates.
    //allows to write formulas in terms of coordinates
    Eigen::VectorXd soln_Xd(num_rows);  //vector for storing solutions to the  sparse solver (indexed from 0, so only num_rows size)

    //For the following, only need gen rate on insides, so N+1 size is enough
    std::vector<double> Un(num_rows+1); //will store generation rate as vector, for easy use in rhs
    std::vector<double> Up = Un;
    Eigen::Tensor<double, 3> R_Langevin(N+1,N+1,N+1), PhotogenRate(N+1,N+1,N+1);
    Eigen::Tensor<double, 3> J_total_Z(num_cell+1, num_cell+1, num_cell+1), J_total_X(num_cell+1, num_cell+1, num_cell+1), J_total_Y(num_cell+1, num_cell+1, num_cell+1);                  //matrices for spacially dependent current

    Eigen::SparseMatrix<double> input; //for feeding input matrix into BiCGSTAB, b/c it crashes if try to call get matrix from the solve call.

    //std::cout << Eigen::nbThreads( ) << std::endl;  //displays the # of threads that will be used by Eigen--> mine displays 8, but doesn't seem like it's using 8.

//------------------------------------------------------------------------------------
    //Construct objects
    Poisson poisson(params);
    Recombo recombo(params);
    Continuity_p continuity_p(params);  //note this also sets up the constant top and bottom electrode BC's
    Continuity_n continuity_n(params);  //note this also sets up the constant top and bottom electrode BC's
    Photogeneration photogen(params, params.Photogen_scaling, params.GenRateFileName);
    Utilities utils;
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::UpLoType::Lower, Eigen::AMDOrdering<int>> SCholesky; //Note using NaturalOrdering is much much slower

    Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> SQR;
    Eigen::SparseLU<Eigen::SparseMatrix<double> >  poisson_LU, cont_n_LU, cont_p_LU;
    Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IncompleteLUT<double>> BiCGStab_solver;  //BiCGStab solver object

    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::UpLoType::Lower|Eigen::UpLoType::Upper > cg;

//--------------------------------------------------------------------------------------------
    //Define boundary conditions and initial conditions. Note: electrodes are at the top and bottom.
    double Va = 0;
    poisson.set_V_bottomBC(params, Va);
    poisson.set_V_topBC(params, Va);

    //Initial conditions
    //std::vector<double> diff;
    //for (int x = 0; x <= num_cell; x++)
       //diff[x] = (poisson.get_V_topBC()[x] - poisson.get_V_bottomBC()[x])/num_cell;    //note, the difference can be different at different x values..., diff is in Z directiont

    //for now assume diff is constant everywhere...
    double diff = (poisson.get_V_topBC(0,0) - poisson.get_V_bottomBC(0,0))/num_cell;  //this is  calculated correctly

    int index = 0;
    for (int k = 1; k <= N; k++) {
        index++;
        V[index] = poisson.get_V_bottomBC(0,0) + diff*k;   //for now just  use 1 pt on bottom BC, since is uniform anyway
        for (int i = 2; i <= N*N; i++) {//  %elements along the x and y directions assumed to have same V
            index++;
            V[index] = V[index-1];
        }
    }

    //side BCs, insulating BC's
    poisson.set_V_leftBC_X(V);
    poisson.set_V_rightBC_X(V);
    poisson.set_V_leftBC_Y(V);
    poisson.set_V_rightBC_Y(V);

    //Fill n and p with initial conditions (need for error calculation)
    double min_dense = continuity_n.get_n_bottomBC(1,1) < continuity_p.get_p_topBC(1,1) ? continuity_n.get_n_bottomBC(1,1):continuity_p.get_p_topBC(1,1);  //this should be same as std::min  fnc which doesn't work for some reason
    //double min_dense = std::min (continuity_n.get_n_bottomBC(1,1), continuity_p.get_p_topBC(1,1));  //Note: I defined the get fnc to take as arguments the i,j values... //Note: the bc's along bottom and top are currently uniform, so index, doesn't really matter.
    for (int i = 1; i<= num_rows; i++) {
        n[i] = min_dense;
        p[i] = min_dense;
    }

    //Convert the n and p to n_matrix and p_matrix
    continuity_n.to_matrix(n);
    continuity_p.to_matrix(p);

    poisson.setup_matrix();  //outside of loop since matrix never changes

    //////////////////////MAIN LOOP////////////////////////////////////////////////////////////////////////////////////////////////////////

    int iter, not_cnv_cnt, Va_cnt;
    bool not_converged;
    double error_np, old_error;  //this stores max value of the error and the value of max error from previous iteration
    std::vector<double> error_np_vector(num_rows+1);  //note: since n and p solutions are in vector form, can use vector form here also

    for (Va_cnt = 0; Va_cnt <= num_V +1; Va_cnt++) {  //+1 b/c 1st Va is the equil run
        not_converged = false;
        not_cnv_cnt = 0;
        if (params.tolerance > 1e-5)
            std::cerr<<"ERROR: Tolerance has been increased to > 1e-5" <<std::endl;

        if (Va_cnt==0) {
            params.use_tolerance_eq();  //relaxed tolerance for equil. run
            params.use_w_eq();
            Va = 0;
        }
        else {
            Va = params.Va_min+params.increment*(Va_cnt-1);
        }
        if (Va_cnt == 1) {
            params.use_tolerance_i();  //reset tolerance back
            params.use_w_i();
            PhotogenRate = photogen.getPhotogenRate();    //otherwise PhotogenRate is pre-initialized to 0 in this main.cpp when declared
        }
        std::cout << "Va = " << Va <<std::endl;

        //Reset top and bottom BCs (outside of loop b/c don't change iter to iter)
        poisson.set_V_bottomBC(params, Va);
        poisson.set_V_topBC(params, Va);

        //-----------------------------------------------------------
        error_np = 1.0;
        iter = 0;

        while (error_np > params.tolerance) {
            //std::cout << "Va " << Va <<std::endl;

            //-----------------Solve Poisson Equation------------------------------------------------------------------
            poisson.set_rhs(n, p);  //this finds netcharge and sets rhs
            //std::cout << poisson.get_sp_matrix() << std::endl;
            oldV = V;

            if (iter == 0) { //INSTEAD OF HAVING IF here, can move these 2 lines, outside of the loop
                poisson_LU.analyzePattern(poisson.get_sp_matrix());  //by doing only on first iter, since pattern never changes, save a bit cpu
                poisson_LU.factorize(poisson.get_sp_matrix());
            }
            soln_Xd = poisson_LU.solve(poisson.get_rhs());

/*
            if (iter == 0) {  //This is slower than  LU
                SCholesky.analyzePattern(poisson.get_sp_matrix());
                SCholesky.factorize(poisson.get_sp_matrix());         //since numerical values of Poisson matrix don't change for 1 set of BC's, can factorize, just on 1st iter
            }
            soln_Xd = SCholesky.solve(poisson.get_rhs());
            */
            //std::cout << soln_Xd << std::endl;
             //std::cout << "Poisson solver error " << poisson.get_sp_matrix() * soln_Xd - poisson.get_rhs() << std::endl;

            //RECALL, I am starting my V vector from index of 1, corresponds to interior pts...
            for (int i = 1; i<=num_rows; i++) {
                newV[i] = soln_Xd(i-1);   //fill VectorXd  rhs of the equation
            }

            //Mix old and new solutions for V
            if (iter > 0)
                V  = utils.linear_mix(params, newV, oldV);
            else
                V = newV;

            //update side BC's and V_matrix
            poisson.set_V_leftBC_X(V);
            poisson.set_V_rightBC_X(V);
            poisson.set_V_leftBC_Y(V);
            poisson.set_V_rightBC_Y(V);
            poisson.to_matrix(V);

            //------------------------------Calculate Net Generation Rate----------------------------------------------------------

            //R_Langevin = recombo.ComputeR_Langevin(params,n,p);
            //FOR NOW CAN USE 0 FOR R_Langevin

            if (Va_cnt > 0) {
                for (int i = 1; i <= num_rows; i++) {
                    Un[i] = params.Photogen_scaling;  //This is what was used in Matlab version for testing.   photogen.getPhotogenRate()(i,j); //- R_Langevin(i,j);
                }
                Up = Un;
            }

            //--------------------------------Solve equations for n and p------------------------------------------------------------

            continuity_n.setup_eqn(poisson.get_V_matrix(), Un, n);
            oldn = n;

            //std::chrono::high_resolution_clock::time_point start2 = std::chrono::high_resolution_clock::now();  //start clock timer

            if (iter == 0 ) //can move this outside of the loop, instead of using if here...
                cont_n_LU.analyzePattern(continuity_n.get_sp_matrix());  //by doing only on first iter, since pattern never changes, save a bit cpu
            cont_n_LU.factorize(continuity_n.get_sp_matrix());  //need to do on each iter, b/c matrix elements change
            soln_Xd = cont_n_LU.solve(continuity_n.get_rhs());

            //std::chrono::high_resolution_clock::time_point finish2 = std::chrono::high_resolution_clock::now();
            //std::chrono::duration<double> time2 = std::chrono::duration_cast<std::chrono::duration<double>>(finish2-start2);
            //std::cout << "CPU time = " << time2.count() << std::endl;

            //std::cout << "solver error " << continuity_n.get_sp_matrix() * soln_Xd - continuity_n.get_rhs() << std::endl;
/*
            input = continuity_n.get_sp_matrix();
            if (iter == 0)
                BiCGStab_solver.analyzePattern(input);
            BiCGStab_solver.factorize(input);  //this computes preconditioner, if use along with analyzePattern (for 1st iter)
            //BiCGStab_solver.compute(input);  //this computes the preconditioner.
            soln_Xd = BiCGStab_solver.solve(continuity_n.get_rhs());
            //std::cout << soln_Xd << std::endl;
            */

            //std::cout << "#iterations:     " << solver.iterations() << std::endl;
            //std::cout << "estimated error: " << BiCGStab_solver.error()      << std::endl;

            //save results back into n std::vector. RECALL, I am starting my V vector from index of 1, corresponds to interior pts...
            for (int i = 1; i<=num_rows; i++) {
                newn[i] = soln_Xd(i-1);   //fill VectorXd  rhs of the equation
            }

            //-------------------------------------------------------
            continuity_p.setup_eqn(poisson.get_V_matrix(), Up, p);
            //std::cout << continuity_p.get_sp_matrix() << std::endl;   //Note: get rhs, returns an Eigen VectorXd
            oldp = p;
/*
            input = continuity_p.get_sp_matrix();
            if (iter == 0)
                BiCGStab_solver.analyzePattern(input);
            BiCGStab_solver.factorize(input);  //this computes preconditioner, if use along with analyzePattern (for 1st iter)
            //BiCGStab_solver.compute(input);  //this computes the preconditioner..compute(input);
            soln_Xd = BiCGStab_solver.solve(continuity_p.get_rhs());
*/

            if (iter == 0 )
                cont_p_LU.analyzePattern(continuity_p.get_sp_matrix());
            cont_p_LU.factorize(continuity_p.get_sp_matrix());
            soln_Xd = cont_p_LU.solve(continuity_p.get_rhs());

            //save results back into n std::vector. RECALL, I am starting my V vector from index of 1, corresponds to interior pts...
            for (int i = 1; i<=num_rows; i++) {
                newp[i] = soln_Xd(i-1);   //fill VectorXd  rhs of the equation
            }

            //------------------------------------------------

            //if get negative p's or n's set them = 0
            for (int i = 1; i <= num_rows; i++) {
                if (newp[i] < 0.0) newp[i] = 0;
                if (newn[i] < 0.0) newn[i] = 0;
            }

            //calculate the error
            old_error = error_np;

            //THIS CAN BE MOVED TO A FUNCTION IN UTILS
            for (int i = 1; i <= num_rows; i++) {
                if (newp[i]!=0 && newn[i] !=0) {
                    error_np_vector[i] = (abs(newp[i]-oldp[i]) + abs(newn[i]-oldn[i]))/abs(oldp[i]+oldn[i]);
                }
            }
            error_np = *std::max_element(error_np_vector.begin()+1,error_np_vector.end());  //+1 b/c we are not using the 0th element
            std::fill(error_np_vector.begin(), error_np_vector.end(),0.0);  //refill with 0's so have fresh one for next iter

            //auto decrease w if not converging
            if (error_np >= old_error)
                not_cnv_cnt = not_cnv_cnt+1;
            if (not_cnv_cnt > 2000) {
                params.reduce_w();
                params.relax_tolerance();
                not_cnv_cnt = 0;
            }

            p = utils.linear_mix(params, newp, oldp);
            n = utils.linear_mix(params, newn, oldn);

            //Apply side continuity equation  BC's
            //WE ARE UPDATING BC'S here b/c we need them for setting up the n and p matrices below
            //Bc's are also updated when setup continuity eqn.
            continuity_n.set_n_leftBC_X(n);  //this sets both x and y left BC's
            continuity_n.set_n_rightBC_X(n);
            continuity_n.set_n_leftBC_Y(n);  //this sets both x and y left BC's
            continuity_n.set_n_rightBC_Y(n);

            continuity_p.set_p_leftBC_X(p);
            continuity_p.set_p_rightBC_X(p);
            continuity_p.set_p_leftBC_Y(p);
            continuity_p.set_p_rightBC_Y(p);
            //note: top and bottom BC's don't need to be changed for now, since assumed to be constant... (they are set when initialize continuity objects)

            //Convert the n and p to n_matrix and p_matrix
            continuity_n.to_matrix(n);
            continuity_p.to_matrix(p);

            //std::cout << error_np << std::endl;
            //std::cout << "weighting factor = " << params.w << std::endl << std::endl;

            iter = iter+1;
        }

        //-------------------Calculate Currents using Scharfetter-Gummel definition--------------------------

        continuity_n.calculate_currents();
        continuity_p.calculate_currents();

        J_total_Z = continuity_p.get_Jp_Z() + continuity_n.get_Jn_Z();
        J_total_X = continuity_p.get_Jp_X() + continuity_n.get_Jn_X();
        J_total_Y = continuity_p.get_Jp_Y() + continuity_n.get_Jn_Y();

        //---------------------Write to file----------------------------------------------------------------
        utils.write_details(params, Va, poisson.get_V_matrix(), p, n, J_total_Z, Un);
        if(Va_cnt >0) utils.write_JV(params, JV, iter, Va, J_total_Z);

    }//end of main loop

    JV.close();

    std::chrono::high_resolution_clock::time_point finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> time = std::chrono::duration_cast<std::chrono::duration<double>>(finish-start);
    std::cout << "CPU time = " << time.count() << std::endl;

    return 0;
}


//
// Created by Hamza El-Kebir on 6/6/21.
//

#ifndef LODESTAR_TESTAUXFUNCTIONS_HPP
#define LODESTAR_TESTAUXFUNCTIONS_HPP

#include <Eigen/Dense>

// A(x, u)
Eigen::Matrix<double, 2, 2> jacStates(const double x, const double y, const double u, const double t) {
    Eigen::Matrix<double, 2, 2> mat;
    mat <<  y+u+2.0*x, x,
            0.0, 2.0*y;

    return mat;
}
// B(x, u)
Eigen::Matrix<double, 2, 1> jacInputs(const double x, const double y, const double u, const double t) {
    Eigen::Matrix<double, 2, 1> mat;
    mat <<  x+2e-01,
            2.0;

    return mat;
}

#endif //LODESTAR_TESTAUXFUNCTIONS_HPP


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

#include "geometrycentral/surface/halfedge_mesh.h"
#include "geometrycentral/surface/heat_method_distance.h"
#include "geometrycentral/surface/halfedge_factories.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/surface_centers.h"
#include "geometrycentral/surface/vector_heat_method.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

#include <sstream>
#include <chrono>

using namespace geometrycentral;
using namespace geometrycentral::surface;
namespace py = pybind11;

// Geometry-central data
std::unique_ptr<HalfedgeMesh> mesh;
std::unique_ptr<VertexPositionGeometry> geometry;

// Algorithm parameters for Vector Heat method
float tCoef = 1.0;
std::unique_ptr<VectorHeatMethodSolver> solver;

// HELPER FUNCTIONS ------------------------------------------------------------

// Loads a mesh from a NumPy array
std::tuple<std::unique_ptr<HalfedgeMesh>, std::unique_ptr<VertexPositionGeometry>>
loadMesh_np(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces) {

  // Set vertex positions
  std::vector<Vector3> vertexPositions(pos.rows());
  for (size_t i = 0; i < pos.rows(); i++) {
    vertexPositions[i][0] = pos(i, 0);
    vertexPositions[i][1] = pos(i, 1);
    vertexPositions[i][2] = pos(i, 2);
  }

  // Get face list
  std::vector<std::vector<size_t>> faceIndices(faces.rows());
  for (size_t i = 0; i < faces.rows(); i++) {
    faceIndices[i] = {faces(i, 0), faces(i, 1), faces(i, 2)};
  }

  return makeHalfedgeAndGeometry(faceIndices, vertexPositions);
}

// Precompute parallel transport and logarithmic map for a given neighborhood.
Eigen::MatrixXd precomputeHarmonic(Vertex& sourceV, Eigen::Matrix<size_t, Eigen::Dynamic, 1> targetVs,
                                  Eigen::Matrix<size_t, Eigen::Dynamic, 1>& sample_points) {
  if (solver == nullptr) {
    solver.reset(new VectorHeatMethodSolver(*geometry, tCoef));
  }

  // Coordinate systems are aligned to smoothed principal curvature directions
  Vector2 sourcePrincipalCurvature = geometry->vertexPrincipalCurvatureDirections[sourceV].normalize();

  // To compute parallel transport from point i (targetV) to point j (sourceV),
  // we transport the x-axis (the principal curvature direction) from sourceV to targetV.

  // First, set the source vectors to the principal curvature directions.
  std::vector<std::tuple<SurfacePoint, Vector2>> points;
  points.emplace_back(sourceV, sourcePrincipalCurvature);

  // Then, compute parallel transport of source vectors.
  VertexData<Vector2> connection = solver->transportTangentVectors(points);

  // And compute the logarithmic map from point i to j
  VertexData<Vector2> logmap = solver->computeLogMap(sourceV);

  // Store the results in an Eigen matrix, which can be accessed as a NumPy array.
  Eigen::MatrixXd res(targetVs.rows(), 4);

  // For every target point
  for (size_t i = 0; i < targetVs.rows(); i++) {
    size_t v = sample_points(targetVs(i));

    // The original logarithmic map is computed with a coordinate system aligned to the first edge.
    // To align the logarithmic map to the principal curvature directions,
    // we rotate the logmap by the principal curvature direction at the source point.
    Vector2 targetCoords = logmap[v] / sourcePrincipalCurvature;

    // Likewise for parallel transport, but we rotate by the principal curvature direction at the target point.
    Vector2 targetPrincipalCurvature = geometry->vertexPrincipalCurvatureDirections[v].normalize();
    Vector2 targetConnection = connection[v] / targetPrincipalCurvature;

    // Store the parallel transport (connection) and logarithmic map.
    res(i, 0) = targetConnection.x;
    res(i, 1) = targetConnection.y;
    res(i, 2) = targetCoords.x;
    res(i, 3) = targetCoords.y;
  }

  return res;
}

// PRECOMPUTATION for HSN ------------------------------------------------------------

// Precomputes the logarithmic map and parallel transport, given a mesh.
// The mesh should be provides as a NumPy array of vertex positions and a NumPy array of face indices.
// Additionally, one should provide a NumPy array of edge indices (source, target),
// a NumPy array with the degree of every source vertex, and indices of sampled points to return values for.
Eigen::MatrixXd precompute(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces,
        Eigen::Matrix<size_t, Eigen::Dynamic, 2>& edge_index, Eigen::Matrix<size_t, Eigen::Dynamic, 1> degree,
        Eigen::Matrix<size_t, Eigen::Dynamic, 1>& sample_points) {

  // Load mesh
  std::tie(mesh, geometry) = loadMesh_np(pos, faces);

  geometry->requireVertexIndices();
  geometry->requireVertexLumpedMassMatrix();
  geometry->requireVertexPrincipalCurvatureDirections();

  // Setup solver for Vector Heat Method.
  solver.reset(new VectorHeatMethodSolver(*geometry, tCoef));

  // Store the results in an Eigen matrix, which can be accessed as a NumPy array.
  Eigen::MatrixXd res(edge_index.rows(), 4);
  size_t index = 0;
  // For each sampled point:
  for (size_t row = 0; row < sample_points.rows(); row++) {
    Vertex v = mesh->vertex(sample_points(row));

    // Compute parallel transport and logarithmic map for neighborhood.
    res.block(index, 0, degree(row), 4) = precomputeHarmonic(v, edge_index.block(index, 1, degree(row), 1), sample_points);
    index += degree(row);
  }

  geometry->unrequireVertexPrincipalCurvatureDirections();
  geometry->unrequireVertexLumpedMassMatrix();
  geometry->unrequireVertexIndices();
  return res;
}

// Computes the vertex lumped mass matrix for each sampled vertex,
// automatically adding the weights from nearest geodesic neighbors.
Eigen::MatrixXd weights(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces,
        Eigen::Matrix<size_t, Eigen::Dynamic, 1>& sample_points, Eigen::Matrix<size_t, Eigen::Dynamic, 1>& labels) {

  // Load mesh
  std::tie(mesh, geometry) = loadMesh_np(pos, faces);

  geometry->requireVertexIndices();
  geometry->requireVertexLumpedMassMatrix();

  // We use short-time heat diffusion to retrieve geodesic nearest neighbors.
  VectorHeatMethodSolver vhmSolver(*geometry, 0.0001);

  // Set up indices of sampled points to diffuse.
  std::vector<std::tuple<SurfacePoint, double>> points;
  for (size_t row = 0; row < sample_points.rows(); row++) {
    points.emplace_back(SurfacePoint(mesh->vertex(sample_points(row))), labels(row));
  }

  // Solve heat diffusion.
  VertexData<double> scalarExtension = vhmSolver.extendScalar(points);

  // Store the results in an Eigen matrix, which can be accessed as a NumPy array.
  Eigen::MatrixXd res = Eigen::MatrixXd::Zero(sample_points.rows(), 1);
  // For each vertex:
  for (size_t row = 0; row < pos.rows(); row++) {
    size_t to_idx = std::lround(scalarExtension[mesh->vertex(row)]);

    // Clamp nearest neighbor index from heat diffusion to range [0, n_vertices]
    if (to_idx >= sample_points.rows()) {
      to_idx = sample_points.rows() - 1;
    } else if (to_idx < 0) {
      to_idx = 0;
    }

    // Add vertex lumped mass to nearest sampled vertex.
    res(to_idx) += geometry->vertexLumpedMassMatrix.coeff(row, row);
  }

  geometry->unrequireVertexLumpedMassMatrix();
  geometry->unrequireVertexIndices();

  return res;
}

// UTILITIES for HSN ------------------------------------------------------------

// Compute the surface area of a mesh.
double surface_area(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces) {

  // Load mesh
  std::tie(mesh, geometry) = loadMesh_np(pos, faces);

  float surfaceArea = 0.0f;
  for (Face f : mesh->faces()) {
    surfaceArea += geometry->faceArea(f);
  }

  return surfaceArea;
}

// Compute geodesic nearest neighbors
Eigen::MatrixXd nearest(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces,
        Eigen::Matrix<size_t, Eigen::Dynamic, 1>& selected_points, Eigen::Matrix<size_t, Eigen::Dynamic, 1>& labels) {

  // Load mesh
  std::tie(mesh, geometry) = loadMesh_np(pos, faces);

  geometry->requireVertexIndices();

  // We use short-time heat diffusion to retrieve geodesic nearest neighbors.
  VectorHeatMethodSolver vhmSolver(*geometry, 0.0001);

  // Set up indices of sampled points to diffuse.
  std::vector<std::tuple<SurfacePoint, double>> points;
  for (size_t row = 0; row < selected_points.rows(); row++) {
    points.emplace_back(SurfacePoint(mesh->vertex(selected_points(row))), labels(row));
  }

  // Solve heat diffusion
  VertexData<double> scalarExtension = vhmSolver.extendScalar(points);

  // Store the results in an Eigen matrix, which can be accessed as a NumPy array.
  Eigen::MatrixXd res(pos.rows(), 1);
  for (size_t row = 0; row < pos.rows(); row++) {
    res(row) = scalarExtension[mesh->vertex(row)];
  }

  geometry->unrequireVertexIndices();

  return res;
}

PYBIND11_MODULE(vectorheat, m) {
    m.doc() = R"pbdoc(
        Harmonic Surface Networks precomputation module.
        -----------------------

        .. currentmodule:: precomputation

        .. autosummary::
           :toctree: _generate

           add
           precompute
           diameter
    )pbdoc";

    m.def("precompute", &precompute, py::return_value_policy::copy, R"pbdoc(
        Precompute parallel transport and logarithmic map for meshes given by pos, face, edges and degree.
    )pbdoc");

    m.def("surface_area", &surface_area, py::return_value_policy::copy, R"pbdoc(
        Computes surface area of the given mesh.
    )pbdoc");

    m.def("weights", &weights, py::return_value_policy::copy, R"pbdoc(
        Computes vertex lumped mass matrix for sampled points.
    )pbdoc");

    m.def("nearest", &nearest, py::return_value_policy::copy, R"pbdoc(
        Returns a mapping from all vertices to the nearest sampled points.
    )pbdoc");

#ifdef VERSION_INFO
    m.attr("__version__") = VERSION_INFO;
#else
    m.attr("__version__") = "dev";
#endif
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic cosine: \f$(e^{x}+e^{-x})/2\f$.

    @see sinh, tanh, sech, csch, sinhcosh

    @par Header <boost/simd/function/cosh.hpp>

    @par Example:

      @snippet cosh.cpp cosh

    @par Possible output:

      @snippet cosh.txt cosh
  **/
  IEEEValue cosh(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/cosh.hpp>
#include <boost/simd/function/simd/cosh.hpp>

#endif


#pragma once
#include "utils.hpp"

#include <Eigen/Dense>
#include <cmath>

namespace khmot {

const int STATE_SIZE = 6;
const int OBSERVATION_SIZE = 3;
const double EPSILON = 1e-9;
const double PI = M_PI;
constexpr double TAU = 2 * M_PI;

// clang-format off
// TODO: noise covariance matrix should be configurable
const auto defaultNoiseCov =
    (Eigen::MatrixXd(STATE_SIZE, STATE_SIZE) << 0.5, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.5, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, .05, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.1, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.0, .01)
        .finished();
const auto defaultObsMatrix = // observe x, y, yaw
    (Eigen::MatrixXd(STATE_SIZE, STATE_SIZE) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        .finished();
// clang-format on

using State = Eigen::Matrix<double, STATE_SIZE, 1>;
using Covariance = Eigen::Matrix<double, STATE_SIZE, STATE_SIZE>;

enum StateMembers {
  StateMemberX = 0,
  StateMemberY,
  StateMemberYaw,
  StateMemberVx,
  StateMemberVy,
  StateMemberVyaw
};

struct KalmanObservation {
  State state = Eigen::VectorXd::Zero(STATE_SIZE);
  Covariance covariance =
      Eigen::MatrixXd::Identity(STATE_SIZE, STATE_SIZE) * EPSILON;
  double timestamp = 0.0;
};

class Kalman {
 public:
  Kalman(bool omnidirectional = true,
         Eigen::MatrixXd noiseCov = defaultNoiseCov);
  const Covariance& covariance() const { return P_; };
  const State& state() const { return state_; };

  void correct(KalmanObservation obs);
  void predict(const double timestamp);
  double lastObsTime() const { return lastObsTime_; };
  void reset();
  void wrapYaw();

 private:
  bool initialized_;
  bool omnidirectional_;  // Restrict motion sideways for non-omnidirectional
                          // robots
  double lastPredTime_;
  double lastObsTime_;
  Eigen::MatrixXd H_;  // KalmanObservation matrix
  Eigen::MatrixXd F_;  // State transition matrix (system dynamics)
  Eigen::MatrixXd Q_;  // Process noise covariance matrix
  State state_;        // Estimated state vector
  Covariance P_;       // Estimated error covariance matrix
};

double clampRotation(double rotation);
void preprocessObs(KalmanObservation& obs);

}  // namespace khmot