#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 "filters.h"

#include <Eigen/SVD>

namespace GPT::Filter
{
    void Contrast::apply(MatXd& img)
    {
        if (low < 0)
            low = img.minCoeff();

        if (high < 0)
            high = img.maxCoeff();

        img.array() = (img.array() - low) / (high - low); // Images are always in between 0 and 1

        // To make sure
        for (int64_t k = 0; k < img.size(); k++)
        {
            double& val = img.data()[k];
            val = std::max<double>(0, val);
            val = std::min<double>(1, val);
        }
    }

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

	void Median::apply(MatXd& img)
	{
        MatXd mat(img);

        int64_t
            radiusX = static_cast<int64_t>(0.5 * double(sizeX)),
            radiusY = static_cast<int64_t>(0.5 * double(sizeY));

        for (int64_t k = 0; k < mat.rows(); k++)
            for (int64_t l = 0; l < mat.cols(); l++)
            {
                // Getting region
                int64_t
                    xo = std::max<int64_t>(l - radiusX, 0),
                    yo = std::max<int64_t>(k - radiusY, 0),
                    xf = std::min<int64_t>(l + radiusX + 1, mat.cols()),
                    yf = std::min<int64_t>(k + radiusY + 1, mat.rows());

                int64_t size = (yf - yo) * (xf - xo);

                std::vector<double> vec;
                vec.reserve(size);

                for (int64_t y = yo; y < yf; y++)
                    for (int64_t x = xo; x < xf; x++)
                        vec.push_back(mat(y, x));

                std::sort(vec.begin(), vec.end());

                int64_t pos = static_cast<int64_t>(0.5 * double(vec.size()));
                img(k, l) = vec.at(pos);
            }
	}

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

	void CLAHE::apply(MatXd& img)
	{
        const int64_t
            nCols = img.cols(),
            nRows = img.rows(),
            tileArea = tileSizeX * tileSizeY,
            TX = static_cast<int64_t>(std::ceil(double(nCols) / double(tileSizeX))),
            TY = static_cast<int64_t>(std::ceil(double(nRows) / double(tileSizeY))),
            NT = TX * TY;

        double clipValue = clipLimit * tileArea / 256.0;

        MatXd lut = MatXd::Zero(256, NT);

        for (int64_t k = 0; k < nRows; k++)
            for (int64_t l = 0; l < nCols; l++)
            {
                int64_t
                    y = static_cast<int64_t>(k / double(tileSizeX)),
                    x = static_cast<int64_t>(l / double(tileSizeY)),
                    tid = y * TX + x;

                int64_t id = static_cast<int64_t>(255.0 * img(k, l));
                lut(id, tid)++;
            }

        // Normalization all look up tables
        for (int64_t tid = 0; tid < NT; tid++)
        {
            // To avoid contrast differences at borders, let's clip the histogram
            while (true)
            {
                double extra = 0.0;
                for (int64_t r = 0; r < 256; r++)
                    if (lut(r, tid) > clipValue)
                    {
                        extra += lut(r, tid) - clipValue;
                        lut(r, tid) = clipValue;
                    }

                if (extra < 0.00001)
                    break;

                lut.col(tid).array() += extra / 256.0;
            }

            double norm = lut.col(tid).sum();
            lut.col(tid).array() /= norm;

            for (int64_t k = 1; k < 256; k++)
                lut(k, tid) += lut(k - 1, tid);

            double bot = lut.col(tid).minCoeff();
            lut.col(tid).array() = (lut.col(tid).array() - bot) / (1.0 - bot);
        }

        // Applying clahe algorithm
        for (int64_t k = 0; k < nRows; k++)
            for (int64_t l = 0; l < nCols; l++)
            {
                // Determining tile
                int64_t x = l / tileSizeX, y = k / tileSizeY;

                double px = double(l) / tileSizeX - x,
                    py = double(k) / tileSizeY - y;

                int dx = round(px) == 1.0 ? 1 : -1,
                    dy = round(py) == 1.0 ? 1 : -1;

                // boundary conditions
                if (y == 0 && dy == -1)
                    dy = 0;

                if (y == (TY - 1) && dy == 1)
                    dy = 0;

                if (x == 0 && dx == -1)
                    dx = 0;

                if (x == (TX - 1) && dx == 1)
                    dx = 0;

                // getting distance from tile's center
                px = px > 0.5 ? px - 0.5 : 0.5 - px;
                py = py > 0.5 ? py - 0.5 : 0.5 - py;

                int64_t
                    bin = static_cast<int64_t>(255.0 * img(k, l)),
                    tid0 = (y + 0) * TX + (x + 0),
                    tid1 = (y + 0) * TX + (x + dx),
                    tid2 = (y + dy) * TX + (x + 0),
                    tid3 = (y + dy) * TX + (x + dx);

                double valx1 = (1.0 - px) * lut(bin, tid0) + px * (lut(bin, tid1));
                double valx2 = (1.0 - px) * lut(bin, tid2) + px * (lut(bin, tid3));

                img(k, l) = (1.0 - py) * valx1 + py * valx2;
            }
	}

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

    void SVD::importImages(const std::vector<MatXd>& vec)
    {
        denoised.resize(vec.size());
        vImages.resize(vec.size());
        std::copy(vec.begin(), vec.end(), vImages.begin());
    }

	void SVD::run(bool &trigger)
	{
        int64_t
            maxFrames = vImages.size(),
            width = vImages[0].cols(),
            height = vImages[0].rows(),
            rows = width * height;

        // We will take the average over every rank approximated image for different slices
        std::vector<float> counter(maxFrames, 0);

        for (int64_t fr = 0; fr < maxFrames; fr++)
            denoised[fr] = MatXd::Zero(height, width);

        MatXd mat(rows, slice);
        for (int64_t fr = 0; fr <= maxFrames - slice; fr++)
        {
            for (int64_t k = 0; k < slice; k++)
                mat.col(k) = vImages[fr + k].reshaped();

            Eigen::BDCSVD<MatXd> svd(mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
            MatXd U = svd.matrixU();
            MatXd V = svd.matrixV().transpose();
            const VecXd& S = svd.singularValues();

            mat = S(0) * U.col(0) * V.row(0);

            for (int64_t k = 1; k < rank; k++)
                mat += S(k) * U.col(k) * V.row(k);

            for (int64_t k = 0; k < slice; k++)
            {
                denoised[fr + k] += mat.col(k).reshaped();
                counter[fr + k]++;
            }

            // In case we want to stop this function from outside
            if (trigger)
                return;
        }

        for (int64_t fr = 0; fr < maxFrames; fr++)
            denoised[fr] /= counter[fr];

    }

    const MatXd& SVD::getImage(int64_t frame) { return denoised[frame]; }

    void SVD::updateImages(std::vector<MatXd>& vec)
    {
        assert(vec.size() == denoised.size());
        std::copy(denoised.begin(), denoised.end(), vec.begin());
    }
}


#include <boost/test/unit_test.hpp>
#include "Werk/Math/OrderStatistics.hpp"

BOOST_AUTO_TEST_SUITE(OrderStatisticsTest)

BOOST_AUTO_TEST_CASE(TestEmpty) {
	Werk::OrderStatistics<double> s;
	BOOST_REQUIRE_EQUAL(s.count(),0);
}

BOOST_AUTO_TEST_CASE(TestBasic) {
	Werk::OrderStatistics<double> s;
	s.sample(5.0);
    s.sample(1.0);
    s.sample(3.0);
    s.sample(4.0);
    s.sample(2.0);

    BOOST_REQUIRE_EQUAL(s.count(), 5);
    BOOST_REQUIRE_EQUAL(s.min(), 1.0);
    BOOST_REQUIRE_EQUAL(s.q1(), 2.0);
    BOOST_REQUIRE_EQUAL(s.median(), 3.0);
    BOOST_REQUIRE_EQUAL(s.q3(), 4.0);
    BOOST_REQUIRE_EQUAL(s.max(), 5.0);

    s.reset();
    BOOST_REQUIRE_EQUAL(s.count(), 0);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @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_ILOGB_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ILOGB_HPP_INCLUDED

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

  /*!
    @ingroup group-ieee
    This function object Extracts the value of the unbiased exponent from
    the floating-point argument x, and returns it as a signed integer value.

    @par Note:

      - Formally, the unbiased exponent is the integral part of \f$\log_r|x|\f$
      as a signed integral value, for non-zero x, where r is
      std::numeric_limits<T>::radix and T is the floating-point type of arg.

      - In practice r = 2 for all supported platforms

      - boost::simd::ilogb differ from std::ilogb in the return type that is always the
        integer type associated to the input type and the limiting values (zero return zero
        and the result is always greater than zero)

      - for floating inputs nan and zero returns zero and +-inf return Valmax

     @par Decorators

       - std_ provides access to std::ilogb

       - pedantic_ return FP_ILOGB0 and FP_ILOGBNAN for 0 and nan respectively,
         but the return type is as in the regular call.

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

    @par Example:

      @snippet ilogb.cpp ilogb

    @par Possible output:

      @snippet ilogb.txt ilogb

  **/
  as_integer_t<Value> ilogb(Value const& x);
} }
#endif

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

#endif


/**
 * @file quasiinterpolation.cc
 * @brief NPDE exam TEMPLATE CODE FILE
 * @author Oliver Rietmann
 * @date 15.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include "quasiinterpolation.h"

#include <lf/base/base.h>  // nonstd::span
#include <lf/geometry/geometry.h>
#include <lf/mesh/utils/utils.h>

#include <Eigen/Core>
#include <memory>
#include <utility>

namespace QuasiInterpolation {

// Auxiliary function: computing the length of an edge
double edgeLength(const lf::mesh::Entity &edge) {
  Eigen::Matrix2d corners = lf::geometry::Corners(*(edge.Geometry()));
  return (corners.col(1) - corners.col(0)).norm();
}

// Auxiliary function: computing the length of the longest edge
double maxLength(const nonstd::span<const lf::mesh::Entity *const> &edges) {
  double length = 0.0;
  for (const lf::mesh::Entity *edge : edges) {
    length = std::max(length, edgeLength(*edge));
  }
  return length;
}

/* SAM_LISTING_BEGIN_1 */
lf::mesh::utils::CodimMeshDataSet<
    std::pair<const lf::mesh::Entity *, unsigned int>>
findKp(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // Variable for returning result
  lf::mesh::utils::CodimMeshDataSet<
      std::pair<const lf::mesh::Entity *, unsigned int>>
      KpMeshDataSet(mesh_p, 2);
  // Auxiliary array storing size of largest triangle adjacent to a node
  lf::mesh::utils::CodimMeshDataSet<double> sizeMeshDataSet(mesh_p, 2);
  // loop over all cells
  for (const lf::mesh::Entity *triangle : mesh_p->Entities(0)) {
    LF_ASSERT_MSG(triangle->RefEl() == lf::base::RefEl::kTria(),
                  "Only implemented for triangles");
    //====================
    // Your code goes here
    //====================
  }  // end of loop over triangles
  return KpMeshDataSet;
}
/* SAM_LISTING_END_1 */

}  // namespace QuasiInterpolation


/*
 * assessment.cpp
 *
 *  Created on: Mar 15, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

//libraries
#include <Eigen/Eigen>

//local
#include "almost_equal.tpp"
#include "typedefs.hpp"

namespace eig = Eigen;
namespace math {

bool almost_equal(float a, float b) {
	return almost_equal_absolute(a, b);
}

bool almost_equal(double a, double b) {
	return almost_equal_absolute(a, b);
}

bool almost_equal(float a, float b, float tolerance) {
	return almost_equal_absolute(a, b, tolerance);
}

bool almost_equal(float a, float b, double tolerance) {
	return almost_equal_absolute(a, b, static_cast<float>(tolerance));
}

bool almost_equal(double a, double b, double tolerance) {
	return almost_equal_absolute(a, b, tolerance);
}

template bool almost_equal<float, float>(math::Vector2f a, math::Vector2f b, float tolerance);  // @formatter: off
template bool almost_equal<float, float>(math::Vector3f a, math::Vector3f b, float tolerance);
template bool almost_equal<float, float>(math::Matrix2f a, math::Matrix2f b, float tolerance);
template bool almost_equal<float, double>(math::Vector2f a, math::Vector2f b, double tolerance);
template bool almost_equal<float, double>(math::Vector3f a, math::Vector3f b, double tolerance);
template bool almost_equal<float, double>(math::Matrix2f a, math::Matrix2f b, double tolerance);

template bool almost_equal<float, float>(eig::MatrixXf matrix_a, eig::MatrixXf matrix_b, float tolerance);
template bool almost_equal<float, double>(eig::MatrixXf matrix_a, eig::MatrixXf matrix_b, double tolerance);
template bool almost_equal<double, double>(eig::MatrixXd matrix_a, eig::MatrixXd matrix_b, double tolerance);
template bool almost_equal_verbose<float, float>(eig::MatrixXf matrix_a, eig::MatrixXf matrix_b, float tolerance);
template bool almost_equal_verbose<float, double>(eig::MatrixXf matrix_a, eig::MatrixXf matrix_b, double tolerance);
template bool almost_equal_verbose<double, double>(eig::MatrixXd matrix_a, eig::MatrixXd matrix_b, double tolerance);

template bool almost_equal<math::Vector2f, float>(math::MatrixXv2f matrix_a, math::MatrixXv2f matrix_b,float tolerance);
template bool almost_equal<math::Vector2f, double>(math::MatrixXv2f matrix_a,math::MatrixXv2f matrix_b, double tolerance);
template bool almost_equal_verbose<math::Vector2f, float>(math::MatrixXv2f matrix_a, math::MatrixXv2f matrix_b,float tolerance);
template bool almost_equal_verbose<math::Vector2f, double>(math::MatrixXv2f matrix_a,math::MatrixXv2f matrix_b, double tolerance);

template bool almost_equal<math::Matrix2f, float>(math::MatrixXm2f matrix_a, math::MatrixXm2f matrix_b,float tolerance);
template bool almost_equal<math::Matrix2f, double>(math::MatrixXm2f matrix_a, math::MatrixXm2f matrix_b,double tolerance);
template bool almost_equal_verbose<math::Matrix2f, float>(math::MatrixXm2f matrix_a, math::MatrixXm2f matrix_b,float tolerance);
template bool almost_equal_verbose<math::Matrix2f, double>(math::MatrixXm2f matrix_a, math::MatrixXm2f matrix_b,double tolerance);

template bool almost_equal<float, float>(math::Tensor3f tensor_a, math::Tensor3f tensor_b, float tolerance);
template bool almost_equal<float, double>(math::Tensor3f tensor_a, math::Tensor3f tensor_b, double tolerance);
template bool almost_equal_verbose<float, float>(math::Tensor3f tensor_a, math::Tensor3f tensor_b, float tolerance);
template bool almost_equal_verbose<float, double>(math::Tensor3f tensor_a, math::Tensor3f tensor_b, double tolerance);

template bool almost_equal<math::Vector3f, float>(math::Tensor3v3f tensor_a, math::Tensor3v3f tensor_b, float tolerance);
template bool almost_equal<math::Vector3f, double>(math::Tensor3v3f tensor_a, math::Tensor3v3f tensor_b, double tolerance);
template bool almost_equal_verbose<math::Vector3f, float>(math::Tensor3v3f tensor_a, math::Tensor3v3f tensor_b, float tolerance);
template bool almost_equal_verbose<math::Vector3f, double>(math::Tensor3v3f tensor_a, math::Tensor3v3f tensor_b, double tolerance);

template bool almost_equal<math::Matrix3f, float>(math::Tensor3m3f tensor_a, math::Tensor3m3f tensor_b, float tolerance);
template bool almost_equal<math::Matrix3f, double>(math::Tensor3m3f tensor_a, math::Tensor3m3f tensor_b, double tolerance);
template bool almost_equal_verbose<math::Matrix3f, float>(math::Tensor3m3f tensor_a, math::Tensor3m3f tensor_b, float tolerance);
template bool almost_equal_verbose<math::Matrix3f, double>(math::Tensor3m3f tensor_a, math::Tensor3m3f tensor_b, double tolerance);  // @formatter: on

}//namespace math


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/functions/IncompleteGammaQFunction.h"

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

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    IncompleteGammaQFunction::IncompleteGammaQFunction(double alpha) :
        mAlpha(alpha) {
    }

    IncompleteGammaQFunction::IncompleteGammaQFunction(const
        IncompleteGammaQFunction& other) :
        mAlpha(other.mAlpha) {
    }

    IncompleteGammaQFunction& IncompleteGammaQFunction::operator = (const
        IncompleteGammaQFunction& other) {
      if (this != &other) {
        mAlpha = other.mAlpha;
      }
      return *this;
    }

    IncompleteGammaQFunction::~IncompleteGammaQFunction() {
    }

/******************************************************************************/
/* Stream operations                                                          */
/******************************************************************************/

    void IncompleteGammaQFunction::read(std::istream& stream) {
    }

    void IncompleteGammaQFunction::write(std::ostream& stream) const {
      stream << "alpha: " << mAlpha;
    }

    void IncompleteGammaQFunction::read(std::ifstream& stream) {
    }

    void IncompleteGammaQFunction::write(std::ofstream& stream) const {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    double IncompleteGammaQFunction::getValue(const VariableType& argument)
        const {
      return boost::math::gamma_q(mAlpha, argument);
    }

    double IncompleteGammaQFunction::getAlpha() const {
      return mAlpha;
    }

    void IncompleteGammaQFunction::setAlpha(double alpha) {
      mAlpha = alpha;
    }

  }
}


/**
 * @file
 * @brief NPDE homework NonConformingCrouzeixRaviartFiniteElements code
 * @author Anian Ruoss
 * @date   18.03.2019
 * @copyright Developed at ETH Zurich
 */

#include <gtest/gtest.h>
#include <lf/base/base.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/test_utils/test_meshes.h>

#include <Eigen/Core>

#include "../crfespace.h"
#include "../crl2errordirichletbvp.h"

using namespace NonConformingCrouzeixRaviartFiniteElements;

TEST(CRReferenceFiniteElement, RefEl) {
  CRReferenceFiniteElement cr_ref_el;
  EXPECT_TRUE(cr_ref_el.RefEl() == lf::base::RefEl::kTria());
}

TEST(CRReferenceFiniteElement, Degree) {
  CRReferenceFiniteElement cr_ref_el;
  EXPECT_EQ(cr_ref_el.Degree(), 1);
}

TEST(CRReferenceFiniteElement, NumRefShapeFunctions) {
  CRReferenceFiniteElement cr_ref_el;
  EXPECT_EQ(cr_ref_el.NumRefShapeFunctions(), 3);
  EXPECT_EQ(cr_ref_el.NumRefShapeFunctions(0), 0);
  EXPECT_EQ(cr_ref_el.NumRefShapeFunctions(1), 1);
  EXPECT_EQ(cr_ref_el.NumRefShapeFunctions(2), 0);
  EXPECT_EQ(cr_ref_el.NumRefShapeFunctions(1, 0), 1);
  EXPECT_EQ(cr_ref_el.NumRefShapeFunctions(1, 1), 1);
  EXPECT_EQ(cr_ref_el.NumRefShapeFunctions(1, 2), 1);
}

TEST(CRReferenceFiniteElement, EvalReferenceShapeFunctions) {
  CRReferenceFiniteElement cr_ref_el;

  Eigen::MatrixXd ref_coords(2, 3);
  ref_coords << 0, 1, 2, 3, 4, 5;

  Eigen::MatrixXd ref_fun_evals(3, 3);
  ref_fun_evals << -5, -7, -9, 5, 9, 13, 1, -1, -3;

  EXPECT_EQ(cr_ref_el.EvalReferenceShapeFunctions(ref_coords), ref_fun_evals);
}

TEST(CRReferenceFiniteElement, GradientsReferenceShapeFunctions) {
  CRReferenceFiniteElement cr_ref_el;

  Eigen::MatrixXd ref_coords(2, 3);
  ref_coords << 0, 1, 2, 3, 4, 5;

  Eigen::MatrixXd ref_fun_grads(3, 6);
  ref_fun_grads << 0, -2, 0, -2, 0, -2, 2, 2, 2, 2, 2, 2, -2, 0, -2, 0, -2, 0;

  EXPECT_EQ(cr_ref_el.GradientsReferenceShapeFunctions(ref_coords),
            ref_fun_grads);
}

TEST(CRReferenceFiniteElement, EvaluationNodes) {
  CRReferenceFiniteElement cr_ref_el;

  Eigen::MatrixXd eval_nodes(2, 3);
  eval_nodes << .5, .5, 0, 0, .5, .5;

  EXPECT_EQ(cr_ref_el.EvaluationNodes(), eval_nodes);
}

TEST(CRReferenceFiniteElement, NumEvaluationNodes) {
  CRReferenceFiniteElement cr_ref_el;
  EXPECT_EQ(cr_ref_el.NumEvaluationNodes(), 3);
}

TEST(CRReferenceFiniteElement, NodalValuesToDofs) {
  CRReferenceFiniteElement cr_ref_el;

  Eigen::MatrixXd nodvals(1, 3);
  nodvals << 0, 1, 2;

  EXPECT_EQ(cr_ref_el.NodalValuesToDofs(nodvals), nodvals);
}

TEST(CRFeSpace, Constructor) {
  std::shared_ptr<lf::mesh::Mesh> mesh_ptr =
      lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  CRFeSpace fe_space(mesh_ptr);

  EXPECT_EQ(fe_space.ShapeFunctionLayout(lf::base::RefEl::kSegment()), nullptr);
  EXPECT_EQ(typeid(*(fe_space.ShapeFunctionLayout(lf::base::RefEl::kTria()))),
            typeid(CRReferenceFiniteElement));
  EXPECT_EQ(fe_space.ShapeFunctionLayout(lf::base::RefEl::kQuad()), nullptr);
}

TEST(NonConformingCrouzeixRaviartFiniteElements,
     L2errorCRDiscretizationDirichletBVP) {
  std::vector<double> l2_errors = {0.0227969, 0.00579489, 0.0014546923,
                                   0.000364046};

  // Loop over meshes
  for (int i = 1; i <= 4; ++i) {
    std::string mesh_file = CURRENT_SOURCE_DIR "/../../meshes/refined_square" +
                            std::to_string(i) + ".msh";

    EXPECT_FLOAT_EQ(L2errorCRDiscretizationDirichletBVP(mesh_file),
                    l2_errors[i - 1]);
  }
}


#include <unordered_set>
#include <chrono>
#include <random>
#include <iostream>
#include <stack>
#include <complex>
#include <utility>
#include <vector>
#include <cmath>
#include <initializer_list>
#include <sstream>
#include <fstream>
#include <unordered_map>
#include <memory>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>


#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
}


#include "project/LinearRegression.hpp"
#include "project/AutoregressiveModel.hpp"
#include "project/ExponentialSmoothing.hpp"
#include <armadillo>
#include <iostream>

int main() {

    arma::arma_rng::set_seed_random();
    arma::vec X = arma::randu(10, 1);
    arma::vec y = X.col(0) + 2 * X.col(0) + 2;

    ExponentialSmoothing expsmth = ExponentialSmoothing(y, 0.2);
    expsmth.fit();
    std::cout<<expsmth.getWeigths()<<std::endl;

    // AutoregressiveModel linreg = AutoregressiveModel(X, y, 3);
    // linreg.fit(true);

    // std::cout<<linreg.forecast(3)<<std::endl;
    // std::cout<<linreg.getCoef()<<std::endl;

    return 0;
}


/*
 *
 * 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;
}


#include "pressure.hpp"

#include "geometry/projection.hpp"

#include <Eigen/Geometry>

namespace neon::mechanics::solid
{
std::pair<index_view, vector> pressure::external_force(std::int64_t const element,
                                                       double const load_factor) const
{
    auto const node_view = node_indices(Eigen::all, element);

    matrix3x const& X = coordinates->initial_configuration(node_view);

    auto const pressure = interpolate_prescribed_load(load_factor);

    // Perform the computation of the external load vector
    matrix f_ext = -pressure
                   * sf->quadrature().integrate(matrix::Zero(X.cols(), 3).eval(),
                                                [&](auto const& femval, auto) -> matrix {
                                                    auto const& [N, dN] = femval;

                                                    matrix32 const jacobian = X * dN;

                                                    auto const j = jacobian_determinant(jacobian);

                                                    vector3 dx_dxi = jacobian.col(0);
                                                    vector3 dx_deta = jacobian.col(1);

                                                    vector3 normal = dx_dxi.cross(dx_deta).normalized();

                                                    return N * normal.transpose() * j;
                                                });

    // Map the matrix back to a vector for the assembly operator
    return {dof_indices(Eigen::all, element), Eigen::Map<matrix>(f_ext.data(), X.cols() * 3, 1)};
}
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2018 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Use, modification and distribution is 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)

#define BOOST_GEOMETRY_NORMALIZE_LATITUDE

#include <sstream>

#include "test_formula.hpp"
#include "direct_meridian_cases.hpp"

#include <boost/geometry/srs/srs.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/meridian_direct.hpp>

template <typename Result, typename Expected, typename Reference>
void check_meridian_direct(Result& result,
                           Expected const& expected,
                           Reference& reference,
                           double reference_error)
{
    boost::geometry::math::normalize_spheroidal_coordinates
        <
            boost::geometry::radian,
            double
        >(result.lon2, result.lat2);

    boost::geometry::math::normalize_spheroidal_coordinates
        <
            boost::geometry::radian,
            double
        >(reference.lon2, reference.lat2);

    std::stringstream ss;
    ss << "(" << result.lon2 * bg::math::r2d<double>()
       << " " << result.lat2 * bg::math::r2d<double>() << ")";

    check_one("lon:" + ss.str(), result.lon2, expected.lon, reference.lon2,
              reference_error);
    check_one("lat:" + ss.str(), result.lat2, expected.lat, reference.lat2,
              reference_error);
    check_one("rev_az:" + ss.str(), result.reverse_azimuth,
              result.reverse_azimuth, reference.reverse_azimuth, reference_error);
    check_one("red len:" + ss.str(), result.reduced_length, result.reduced_length,
              reference.reduced_length, 0.01);
    check_one("geo scale:" + ss.str(), result.geodesic_scale, result.geodesic_scale,
              reference.geodesic_scale, 0.01);

}

void test_all(expected_results const& results)
{
    double const d2r = bg::math::d2r<double>();

    double lon1_rad = results.p1.lon * d2r;
    double lat1_rad = results.p1.lat * d2r;
    coordinates expected_point;
    expected_point.lon = results.p2.lon * d2r;
    expected_point.lat = results.p2.lat * d2r;
    double distance = results.distance;
    bool direction = results.direction;

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    bg::formula::result_direct<double> vincenty_result;
    bg::formula::result_direct<double> meridian_result;

    typedef bg::formula::vincenty_direct<double, true, true, true, true> vi_t;
    double vincenty_azimuth = direction ? 0.0 : bg::math::pi<double>();
    vincenty_result = vi_t::apply(lon1_rad, lat1_rad, distance, vincenty_azimuth, spheroid);

    {
        typedef bg::formula::meridian_direct<double, true, true, true, true, 1> eli;
        meridian_result = eli::apply(lon1_rad, lat1_rad, distance, direction, spheroid);
        check_meridian_direct(meridian_result, expected_point, vincenty_result, 0.001);
    }
    {
        typedef bg::formula::meridian_direct<double, true, true, true, true, 2> eli;
        meridian_result = eli::apply(lon1_rad, lat1_rad, distance, direction, spheroid);
        check_meridian_direct(meridian_result, expected_point, vincenty_result, 0.00001);
    }
    {
        typedef bg::formula::meridian_direct<double, true, true, true, true, 3> eli;
        meridian_result = eli::apply(lon1_rad, lat1_rad, distance, direction, spheroid);
        check_meridian_direct(meridian_result, expected_point, vincenty_result, 0.00000001);
    }
    {
        typedef bg::formula::meridian_direct<double, true, true, true, true, 4> eli;
        meridian_result = eli::apply(lon1_rad, lat1_rad, distance, direction, spheroid);
        check_meridian_direct(meridian_result, expected_point, vincenty_result, 0.00000001);
    }
    {
        typedef bg::formula::meridian_direct<double, true, true, true, true, 5> eli;
        meridian_result = eli::apply(lon1_rad, lat1_rad, distance, direction, spheroid);
        check_meridian_direct(meridian_result, expected_point, vincenty_result, 0.00000000001);
    }
}

int test_main(int, char*[])
{
    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    return 0;
}


// See LICENSE for license details.

#include "tetra.hpp"

#define CGAL_DISABLE_ROUNDING_MATH_CHECK
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_3.h>
#include <boost/iterator/transform_iterator.hpp>

namespace tetra
{

namespace __detail {

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_3<K>      Triangulation;
typedef Triangulation::Point          Point;
typedef Triangulation::Locate_type    Locate_type;

static Point vec2point(const Vec3d& v)
{
    return Point(v[0], v[1], v[2]);
}

struct _Octagon_Impl
{
    _Octagon_Impl(const std::array<Vec3d, 8> &vertices);
    const Triangulation T;
};

_Octagon_Impl::_Octagon_Impl(const std::array<Vec3d, 8> &vertices)
    : T(boost::make_transform_iterator(vertices.begin(), vec2point),
        boost::make_transform_iterator(vertices.end(), vec2point)) {}

static bool contains(const _Octagon_Impl &oi, const Vec3d &v)
{
    Point p = vec2point(v);
    Locate_type lt;
    int li, lj;
    oi.T.locate(p, lt, li, lj);
    // Also accepts corners, edges of the polygon
    return lt <= Triangulation::CELL;
}

} // namespace __detail

// These are declared here because _Octagon_Impl is an imcomplete type in the header.
Octagon::Octagon() = default;
Octagon::~Octagon() = default;
Octagon::Octagon(Octagon&& o) = default;

Octagon::Octagon(const std::array<Vec3d, 8> &vertices)
    : oi( std::make_unique<__detail::_Octagon_Impl>(vertices)) {}

bool Octagon::contains(const Vec3d &p) const
{
    if (!oi)
        throw std::runtime_error("contains() on empty octagon");
    return __detail::contains(*oi, p);
}

bool Octagon::contains(double x, double y, double z) const
{
    return contains({{x, y, z}});
}

void Octagon::operator=(Octagon o)
{
    swap(*this, o);
}

void swap(Octagon& a, Octagon& b) {
    std::swap(a.oi, b.oi);
}

} // namespace tetra


//
// 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