#include <fstream>
#include<boost/shared_ptr.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/partition_2.h>
#include <CGAL/Partition_traits_2.h>
#include<CGAL/create_straight_skeleton_2.h>
#include<CGAL/create_offset_polygons_2.h>
#include <CGAL/linear_least_squares_fitting_2.h>
#include <CGAL/extremal_polygon_2.h>
#include <CGAL/minkowski_sum_2.h>
#include <boost/config.hpp>
#include <boost/version.hpp>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif

// Qt headers
#include <QtGui>
#include <QString>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsLineItem>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/PolygonGraphicsItem.h>
#include <CGAL/Qt/PolygonWithHolesGraphicsItem.h>
#include <CGAL/Qt/LineGraphicsItem.h>

// the two base classes
#include "ui_Polygon_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Segment_2 Segment_2;
typedef K::Line_2 Line_2;

typedef CGAL::Polygon_2<K,std::list< Point_2 > > Polygon2; // it must be a list for the partition
typedef CGAL::Polygon_with_holes_2<K,std::list< Point_2 > > Polygon_with_holes_2;

typedef CGAL::Straight_skeleton_2<K> Ss ;

typedef boost::shared_ptr<Ss> SsPtr ;

typedef boost::shared_ptr<Polygon2> PolygonPtr ;

typedef std::vector<PolygonPtr> PolygonPtr_vector ;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Polygon_2
{
  Q_OBJECT

private:

  enum PartitionAlgorithm {YMonotone, ApproximateConvex, OptimalConvex} ;

  CGAL::Qt::Converter<K> convert;
  Polygon2 poly, kgon;
  Polygon_with_holes_2 selfmink;
  QGraphicsScene scene;

  CGAL::Qt::PolygonGraphicsItem<Polygon2> * pgi;

  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;

  CGAL::Qt::PolygonWithHolesGraphicsItem<Polygon_with_holes_2> * minkgi;

  std::list<Polygon2> partitionPolygons;
  std::list<CGAL::Qt::PolygonGraphicsItem<Polygon2>* >  partitionGraphicsItems;
  std::list<QGraphicsLineItem* >  skeletonGraphicsItems;
  std::list<QGraphicsLineItem* >  offsetGraphicsItems;
  CGAL::Qt::LineGraphicsItem<K>* lgi;

  CGAL::Qt::PolygonGraphicsItem<Polygon2> * kgongi;

public:
  MainWindow();

public Q_SLOTS:

  void processInput(CGAL::Object o);

  void on_actionClear_triggered();

  void on_actionLoadPolygon_triggered();
  void on_actionSavePolygon_triggered();

  void on_actionSelfMinkowskiSum_triggered();
  void on_actionRecenter_triggered();
  void on_actionMaximumAreaKGon_triggered();
  void on_actionInnerSkeleton_triggered();
  void on_actionOuterOffset_triggered();
  void on_actionLinearLeastSquaresFitting_triggered();
  void on_actionLinearLeastSquaresFittingOfSegments_triggered();
  void on_actionCreateInputPolygon_toggled(bool);

  void on_actionYMonotonePartition_triggered();
  void on_actionApproximateConvexPartition_triggered();
  void on_actionOptimalConvexPartition_triggered();
  void partition(PartitionAlgorithm);

  void clearPartition();
  void clearMinkowski();
  void clearSkeleton();
  void clearOffset();
  void clear();

  virtual void open(QString);
Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  minkgi = nullptr;
  // Add a GraphicItem for the Polygon2
  pgi = new CGAL::Qt::PolygonGraphicsItem<Polygon2>(&poly);

  QObject::connect(this, SIGNAL(changed()),
                   pgi, SLOT(modelChanged()));

  scene.addItem(pgi);

  kgongi =  new CGAL::Qt::PolygonGraphicsItem<Polygon2>(&kgon);
  kgongi->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  kgongi->hide();
  scene.addItem(kgongi);

  lgi = new CGAL::Qt::LineGraphicsItem<K>();
  lgi->setPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  lgi->hide();
  scene.addItem(lgi);
  assert(lgi->scene() == &scene);
  // Setup input handlers. They get events before the scene gets them
  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 0, true);

  this->actionCreateInputPolygon->setChecked(true);
  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
                   this, SLOT(processInput(CGAL::Object)));

  //
  // Manual handling of actions
  //
  QObject::connect(this->actionQuit, SIGNAL(triggered()),
                   this, SLOT(close()));

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Uncomment the following line to get antialiasing by default.
//   actionUse_Antialiasing->setChecked(true);

  // Turn the vertical axis upside down
  this->graphicsView->scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Polygon_2.html");
  this->addAboutCGAL();
  this->setupExportSVG(action_Export_SVG, graphicsView);

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
          this, SLOT(open(QString)));
}

void
MainWindow::processInput(CGAL::Object o)
{
  this->actionCreateInputPolygon->setChecked(false);
  std::list<Point_2> points;
  if(CGAL::assign(points, o)){
    if((points.size() == 1)&& poly.size()>0){

    } else {
      poly.clear();
      if(points.front() == points.back()){
        points.pop_back();
      }
      poly.insert(poly.vertices_begin(), points.begin(), points.end());
    }
    Q_EMIT( changed());
  }
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionClear_triggered()
{
  poly.clear();
  clear();
  this->actionCreateInputPolygon->setChecked(true);
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadPolygon_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Polygon File"),
                                                  ".",
                                                  tr( "Polyline files (*.polygons.cgal);;"
                                                      "WSL files (*.wsl);;"
                                                    #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                      "WKT files (*.wkt *.WKT);;"
                                                    #endif
                                                      "All file (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  this->actionCreateInputPolygon->setChecked(false);
  std::ifstream ifs(qPrintable(fileName));
  poly.clear();
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    CGAL::Polygon_with_holes_2<K> P;
    CGAL::read_polygon_WKT(ifs, P);
    poly = Polygon2(P.outer_boundary().begin(),
                    P.outer_boundary().end());
#endif
  }
  else
  {
    ifs >> poly;
  }
  clear();

  this->addToRecentFiles(fileName);
  Q_EMIT( changed());
}

void
MainWindow::on_actionSavePolygon_triggered()
{
  QString fileName = QFileDialog::getSaveFileName(this,
                                                  tr("Save Polygon"),
                                                  ".",
                                                  tr( "Polyline files (*.polygons.cgal);;"
                                                    #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                      "WKT files (*.wkt *.WKT);;"
                                                    #endif
                                                      "All file (*)"));
  if(! fileName.isEmpty()){
    std::ofstream ofs(qPrintable(fileName));
    if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
    {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      CGAL::Polygon_2<K> P(poly.begin(),
                           poly.end());
      CGAL::Polygon_with_holes_2<K> Pwh(P);
      CGAL::write_polygon_WKT(ofs, Pwh);
#endif
    }
    else
      ofs << poly;
  }
}

void
MainWindow::on_actionCreateInputPolygon_toggled(bool checked)
{
  poly.clear();
  clear();
  if(checked){
    scene.installEventFilter(pi);
  } else {
    scene.removeEventFilter(pi);
  }
  Q_EMIT( changed());
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(pgi->boundingRect());
  this->graphicsView->fitInView(pgi->boundingRect(), Qt::KeepAspectRatio);
}

void
MainWindow::on_actionSelfMinkowskiSum_triggered()
{
  if(poly.size()>0){
    if(! poly.is_simple()){
      return;
    }

    selfmink = minkowski_sum_2 (poly, poly);

    minkgi = new CGAL::Qt::PolygonWithHolesGraphicsItem<Polygon_with_holes_2>(&selfmink);

    minkgi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
    scene.addItem(minkgi);
    minkgi->setEdgesPen(QPen(Qt::darkRed, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  }
}

void
MainWindow::on_actionInnerSkeleton_triggered()
{
  if(poly.size()>0){
    if(! poly.is_simple()){
      return;
    }
    clear();
    if(! poly.is_counterclockwise_oriented()){
      poly.reverse_orientation();
    }
    SsPtr iss = CGAL::create_interior_straight_skeleton_2(poly.vertices_begin(), poly.vertices_end());

    CGAL::Straight_skeleton_2<K> const& ss = *iss;

    typedef Ss::Vertex_const_handle     Vertex_const_handle ;
    typedef Ss::Halfedge_const_handle   Halfedge_const_handle ;
    typedef Ss::Halfedge_const_iterator Halfedge_const_iterator ;

    Halfedge_const_handle null_halfedge ;
    Vertex_const_handle   null_vertex ;

    for ( Halfedge_const_iterator i = ss.halfedges_begin(); i != ss.halfedges_end(); ++i )
      {
        if ( i->is_bisector() ){
          Segment_2 s(i->opposite()->vertex()->point(), i->vertex()->point());
          skeletonGraphicsItems.push_back(new QGraphicsLineItem(convert(s)));
          scene.addItem(skeletonGraphicsItems.back());
          skeletonGraphicsItems.back()->setPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
        }
      }

  }

}

void
MainWindow::on_actionOuterOffset_triggered()
{
  if(poly.size()>0 && poly.is_simple())
  {
    clear();

    if(! poly.is_counterclockwise_oriented())
      poly.reverse_orientation();

    double w = poly.bbox().xmax() - poly.bbox().xmin() ;
    double h = poly.bbox().ymax() - poly.bbox().ymin() ;
    double s = (std::max)(w,h);
    double def_offset = s * 0.05 ;

    bool ok;
    const double off = QInputDialog::getDouble( this
                                              , tr("Offset distance")
                                              , tr("Offset distance:")
                                              , def_offset
                                              , 0.0
                                              , s
                                              , 2
                                              , &ok
                                              );
    if ( ok )
    {
      PolygonPtr_vector lContours = create_exterior_skeleton_and_offset_polygons_2(off,poly) ;

      PolygonPtr_vector::const_iterator frame ;

      double max_area = 0.0 ;

      for( PolygonPtr_vector::const_iterator cit = lContours.begin(); cit != lContours.end(); ++ cit )
      {
        double area = (*cit)->area();
        if ( area > max_area )
        {
          max_area = area ;
          frame = cit ;
        }
      }

      for( PolygonPtr_vector::const_iterator cit = lContours.begin(); cit != lContours.end(); ++ cit )
      {
        if ( cit != frame )
        {
          Polygon2 const& lContour = **cit ;
          lContour.area();
          for ( Polygon2::Edge_const_iterator eit = lContour.edges_begin(); eit != lContour.edges_end(); ++ eit )
          {
            Segment_2 s(eit->source(), eit->target());
            offsetGraphicsItems.push_back(new QGraphicsLineItem(convert(s)));
            scene.addItem(offsetGraphicsItems.back());
            offsetGraphicsItems.back()->setPen(QPen(Qt::red, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
          }
        }
      }
    }
  }
}

void
MainWindow::on_actionMaximumAreaKGon_triggered()
{
  if( (poly.size()>2) && poly.is_convex()){
    clear();

    kgon.clear();
    std::vector<Point_2> points(poly.vertices_begin(),
                                poly.vertices_end());
    CGAL::maximum_area_inscribed_k_gon_2(points.begin(),
                                         points.end(),
                                         3,
                                         std::back_inserter(kgon));

    kgongi->modelChanged();
    kgongi->show();
  } else {
    std::cout << "The polygon must be convex" << std::endl;
  }
}

void
MainWindow::on_actionLinearLeastSquaresFitting_triggered()
{
  if(poly.size()>2){
    clear();

    Line_2 line;
    CGAL::linear_least_squares_fitting_2(poly.vertices_begin(),
                                         poly.vertices_end(),
                                         line,
                                         CGAL::Dimension_tag<0>());

    lgi->setLine(line);
    lgi->show();
  }
}

void
MainWindow::on_actionLinearLeastSquaresFittingOfSegments_triggered()
{
  if(poly.size()>2){
    clear();

    Line_2 line;
    CGAL::linear_least_squares_fitting_2(poly.edges_begin(),
                                         poly.edges_end(),
                                         line,
                                         CGAL::Dimension_tag<1>());

    lgi->setLine(line);
    lgi->show();
  }
}

void
MainWindow::on_actionYMonotonePartition_triggered()
{
  partition(YMonotone);
}

void
MainWindow::on_actionOptimalConvexPartition_triggered()
{
  partition(OptimalConvex);
}

void
MainWindow::on_actionApproximateConvexPartition_triggered()
{
  partition(ApproximateConvex);
}

void
MainWindow::partition(PartitionAlgorithm pa)
{
  if(poly.size()>0){
    clear();
    if(! poly.is_counterclockwise_oriented()){
      poly.reverse_orientation();
    }
    switch (pa) {
    case YMonotone :
      CGAL::y_monotone_partition_2(poly.vertices_begin(), poly.vertices_end(), std::back_inserter(partitionPolygons));
      break;
    case ApproximateConvex:
      CGAL::approx_convex_partition_2(poly.vertices_begin(), poly.vertices_end(), std::back_inserter(partitionPolygons));
      break;
    default:
      CGAL::optimal_convex_partition_2(poly.vertices_begin(), poly.vertices_end(), std::back_inserter(partitionPolygons));
      break;
    }
    for(std::list<Polygon2>::iterator it = partitionPolygons.begin();
        it != partitionPolygons.end();
        ++it){
      partitionGraphicsItems.push_back(new CGAL::Qt::PolygonGraphicsItem<Polygon2>(&(*it)));
      scene.addItem(partitionGraphicsItems.back());
      partitionGraphicsItems.back()->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
    }
  }
}

void
MainWindow::clearPartition()
{
  partitionPolygons.clear();
  for(std::list<CGAL::Qt::PolygonGraphicsItem<Polygon2>* >::iterator it = partitionGraphicsItems.begin();
      it != partitionGraphicsItems.end();
      ++it){
    scene.removeItem(*it);
  }
  partitionGraphicsItems.clear();
}

void
MainWindow::clearMinkowski()
{
  if(minkgi != nullptr){
    scene.removeItem(minkgi);
    delete minkgi;
    minkgi = nullptr;
  }
}

void
MainWindow::clearSkeleton()
{ for(std::list<QGraphicsLineItem* >::iterator it = skeletonGraphicsItems.begin();
      it != skeletonGraphicsItems.end();
      ++it){
    scene.removeItem(*it);
  }
  skeletonGraphicsItems.clear();
}

void
MainWindow::clearOffset()
{ for(std::list<QGraphicsLineItem* >::iterator it = offsetGraphicsItems.begin();
      it != offsetGraphicsItems.end();
      ++it){
    scene.removeItem(*it);
  }
  offsetGraphicsItems.clear();
}

void
MainWindow::clear()
{
  clearPartition();
  clearMinkowski();
  clearSkeleton();
  clearOffset();
  lgi->hide();
  kgongi->hide();
}

#include "Polygon_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Polygon_2 demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;
  Q_INIT_RESOURCE(Polygon_2);

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}


#include <StatSim/Math.hpp>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

#include <cmath>
#include <numbers>

namespace StatSim {
	double BinomialPMF(int tryCount, int occurCount, double probability) {
		const auto numberOfCases = [](int n, int r) {
			if (n - r < r) {
				r = n - r;
			}

			boost::multiprecision::cpp_int result = 1;
			for (int i = 0; i < r; ++i) {
				result *= n - i;
			}
			for (int i = 0; i < r; ++i) {
				result /= i + 1;
			}
			return result;
		}(tryCount, occurCount);

		const boost::multiprecision::cpp_bin_float_100 probabilityEx(probability);
		const auto occurProbability = boost::multiprecision::pow(probabilityEx, occurCount);
		const auto notOccurProbability = boost::multiprecision::pow(1 - probabilityEx, tryCount - occurCount);

		const auto result = static_cast<boost::multiprecision::cpp_bin_float_100>(numberOfCases) * occurProbability * notOccurProbability;
		return static_cast<double>(result);
	}
	double NormalCDF(double value, double mean, double standardDeviation) {
		return 0.5 * std::erfc((mean - value) / standardDeviation / std::numbers::sqrt2_v<double>);
	}
}


#include<iostream>
#include <math.h>
#include <vector>
#include <random>
#include <iomanip>
#include <Eigen/Core>
#include <algorithm>
using namespace std;
//高斯密度函数
float gussian_fx(float x){
    float weight=1/(sqrt(2*M_PI));
    float exow=-pow(x,2)/2;
    return (weight*exp(exow));
};
//求X<x 的概率
float guassian_pb(float x, float u,float sigma2){
    float detx=0.01;
    float pb=0;
    float MINX=-5;
    while (MINX<=(x-u)/sqrt(sigma2)){
        MINX+=detx;
        pb+=detx*gussian_fx(MINX);
    };
    return pb;
};
float guassian_pb_from_data(vector<float >data,float x){
    static int i=0,j=0;
    while(i<data.size()){
        if(data[i]<=x){i++;j++;}
        else{i++;};
    }
    return float(j)/data.size();
}
//最大似然估计，接受参数为向量，返回pair first为均值，second为方差
pair<float,float> MaxLikelihood(vector<float > &xi){
    float hatu=0; float hatsigma2=0;
    for (int i = 0; i <xi.size() ; ++i) {
        hatu+=xi[i];
    }
    for (int j = 0; j <xi.size() ; ++j) {
        hatsigma2+=pow(xi[j]-(hatu/xi.size()),2);
    }
    pair<float ,float > us={hatu/xi.size(),hatsigma2/xi.size()};
    return us;
};

//生成正态分布，mu均值，sigma2方差，默认个数500
vector<float >NormalGenerating(float mu,float sigma2,int length=500){
    random_device rd;
    std::mt19937 gen(rd());
    uniform_real_distribution<float > ud(0.0,1.0);
    int i=0;
    vector<float > seed;
    while(i<length) {
        float x = ud(gen);
        float y = ud(gen);
        float temp;
        if (i % 2) {
            temp =cos(2 * M_PI * x) * sqrt(-2 * log(1 - y));
        }
        else{
            temp=sin(2*M_PI*x)*sqrt(-2*log(1-y));
        }
        float ns=temp*sqrt(sigma2)+mu;
        i++;
        seed.push_back(ns);
    }
    return seed;
}
//生成正态分布，mu均值，sigma2方差，默认个数500
vector<float >NormalGenerating2(float mu,float sigma2,int length=500){
    random_device rd;
    std::mt19937 gen(rd());
    normal_distribution<float > normal(mu,sqrt(sigma2));
    vector<float >seed;
    while(length--){
        seed.push_back(normal(gen));
    }
    return seed;
}

//需要显示初始化
struct MultiNormal{
    int length=500;
    float u=0;
    float sigma2=0;
    vector<float> Gussiandata;
    MultiNormal(vector<float> data){
        u=MaxLikelihood(data).first;
        sigma2=MaxLikelihood(data).second;
        Gussiandata=data;
    }
};
//混合高斯分布，将多个正态分布相加
vector<float>MultiNormalAdding(vector <MultiNormal> &kargs){
    vector<float > reciver(500,0);
    int k=kargs.size();
    for (int loc = 0; loc <500 ; ++loc) {
        int j=0;
        while(j<k) {
            reciver[loc]+=kargs[j].Gussiandata[loc];
            j++;
        }
    }
    return reciver;
}
//混合高斯分布，将多个正态分布混合
vector<float>MultiNormalMerging(vector <MultiNormal> &kargs){
    int k=kargs.size();
    int Alllength=0;
    int i=0;
    while(i<k){
        Alllength+=kargs[i].Gussiandata.size();
        i++;
    }
    vector<float > reciver(Alllength,0);
    int j=0;
    int det=0;
    while(j<k){
        for (int l = 0; l <kargs[j].Gussiandata.size() ; ++l) {
            int pos=j-1;
            if(pos==-1)pos=0;
            reciver[l+j*kargs[pos].Gussiandata.size()]=kargs[j].Gussiandata[l];
        }
        j++;
    }
    return reciver;
}

//生成统计数据
vector<vector<float >> CalNormalData(vector<float > &multgassian,int quration,int GMM=2){
    vector<float > temp=multgassian;
    sort(temp.begin(),temp.end());
    float mindata=temp.front();
    float maxdata=temp.back();
    float middle=(mindata+maxdata)/2;
    cout<<middle<<endl;
    //cout<<temp.size()<<endl;
    //for(auto &item:temp)cout<<item<<endl;
    int qu=quration;//分区数
    float step=(maxdata-mindata)/qu;//分区数;
    vector<vector<float >> SortedHistData;
    int i=0;
    int pos=0;
    while(i<qu){
        vector<float> tempdata;
        while(mindata+i*step<=temp[pos]&&temp[pos]<=mindata+(i+1)*step){
            tempdata.push_back(temp[pos]);
            pos++;
        }
        if(tempdata.size()==0) tempdata.push_back(0);//防止该分区为空
        SortedHistData.push_back(tempdata);
        tempdata.clear();
        i++;
    }
    Eigen::MatrixXf QKI(quration,GMM);
    Eigen::MatrixXf SITA(GMM,2);//同列0表示mu 1表示sigma2
    SITA<<middle,middle,1,1;
    vector<float> cof;
    for (int i = 0; i <quration ; ++i) {
        for (int j = 0; j <GMM ; ++j) { //混合个数
            QKI(i,j)=guassian_pb(SortedHistData[i].back(),SITA(j,0),SITA(j,1))
                     -guassian_pb(SortedHistData[i].front(),SITA(j,0),SITA(j,1));
        }
    }
    for (int k = 0; k <quration ; ++k) {
        float temp=0;
        for (int l = 0; l <GMM ; ++l) {
            temp+=QKI(k,l);

        }
        cof.push_back(temp);
    }
    for (int m = 0; m <quration ; ++m) {
        for (int n = 0; n <GMM ; ++n) {
            if(!cof[m])QKI(m,n)=0;
            else QKI(m,n)=QKI(m,n)/cof[m];
        }

    }
    //cout<<QKI<<endl;

    return SortedHistData;

}

int main(){
    vector<float > seed1=NormalGenerating(0,2);
    vector<float > seed2=NormalGenerating2(10,2);
    //vector<float > seed3=NormalGenerating2(15,2);
    MultiNormal No1(seed1);
    MultiNormal No2(seed2);
   // MultiNormal No3(seed3);
    vector<MultiNormal> Multiseeds;
    Multiseeds.push_back(No1);
    Multiseeds.push_back(No2);
    //Multiseeds.push_back(No3);
    vector<float > merge=MultiNormalMerging(Multiseeds);
    //for(auto &item:merge)cout<<item<<endl;
    //cout<<"the mixed guassian mean is u="<<MaxLikelihood(merge).first<<endl<<"the mixed guassian variance is simga="<<MaxLikelihood(merge).second<<endl;
//    for(auto &item:CalNormalData(merge,20)){
//        for(auto &j:item){
//            cout<<j<<"  "<<endl;
//        }
//        cout<<endl;
//    }
    CalNormalData(merge,20);
    return 0;
}


#include <iostream>

#include <boost/multiprecision/gmp.hpp>

#include "headers/euler.hpp"

const int EXPANSIONS_BEGIN = 1;
const int EXPANSIONS_NUM = 1000;

typedef boost::multiprecision::mpz_int big_int;
typedef euler::fraction<big_int> fraction;

fraction expandSqrt(int n, int depth = 0) {
  if (depth == n) {
    return fraction(1, 2);
  }

  fraction cd = expandSqrt(n, depth + 1);

  big_int a = cd.denom + (depth ? 0 : cd.numer);
  big_int b = depth ? (2 * cd.denom + cd.numer) : cd.denom;

  return fraction(a, b);
}

int main(int argc, char** argv) {
  int termsWithHigherDigitCountNumers = 0;

  for (int i = EXPANSIONS_BEGIN; i <= EXPANSIONS_NUM; i++) {
    fraction expandedTerm = expandSqrt(i);

    if (euler::countDigits(expandedTerm.numer) > euler::countDigits(expandedTerm.denom)) {
      termsWithHigherDigitCountNumers++;
    }
  }

  std::cout << "Total expansions between " << EXPANSIONS_BEGIN << " and " << EXPANSIONS_NUM
            << " of sqrt(2) with higher counts of digits in numerator than denominator: "
            << termsWithHigherDigitCountNumers << std::endl;

  return 0;
}


#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace cv;

string file_1 = "./LK1.png";  // first image
string file_2 = "./LK2.png";  // second image

class OpticalFlowTracker{
public:
    OpticalFlowTracker(const Mat &img1_,
                       const Mat &img2_,
                       const vector<KeyPoint> &kp1_,
                       vector<KeyPoint> &kp2_,
                       vector<bool> &success_,
                       bool inverse_ = true,
                       bool has_initial_ = false):
                       img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
                       has_initial(has_initial_) {}

    void calculateOpticalFlow(const Range &range);

private:
    const Mat &img1;
    const Mat &img2;
    const vector<KeyPoint> &kp1;
    vector<KeyPoint> &kp2;
    vector<bool> &success;
    bool inverse = true;
    bool has_initial = false;
};

void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse = false
);

void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse = false, bool has_initial = false);

inline float GetPixelValue(const Mat &img, double x, double y){
    if (x < 0) x = 0;
    else if (x > img.cols - 1) x = img.cols - 1;
    if (y < 0) y = 0;
    else if (y > img.rows - 1) y = img.rows - 1;

    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    float pixelValue = (1-xx)*(1-yy)*data[0] + (xx)*(1-yy)*data[1]
                        + (1-xx)*(yy)*data[img.step] + (xx)*(yy)*data[img.step+1];
    return pixelValue;
}

int main(int argc, char **argv){
    cv::Mat img_1 = cv::imread(file_1, 0);
    cv::Mat img_2 = cv::imread(file_2, 0);

    // 从img1中提取特征点，使用GFTT特征
    vector<KeyPoint> kp_1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20);
    detector->detect(img_1, kp_1);

    // OpenCV LK光流
    vector<Point2f> pts_1, pts_2;
    for (auto &kp:kp_1){
        pts_1.push_back(kp.pt);
    }
    vector<uchar> status;
    vector<float> error;
    cv::calcOpticalFlowPyrLK(img_1, img_2, pts_1, pts_2, status, error);
    // 画图：By OpenCV LK光流
    Mat img_2_CV;
    cv::cvtColor(img_2,img_2_CV, CV_GRAY2BGR);
    for (int i=0; i<pts_2.size(); i++){
        if (status[i]){
            cv::circle(img_2_CV, pts_2[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img_2_CV, pts_1[i], pts_2[i], cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("By OpenCV", img_2_CV);

    // 单层LK光流
    vector<KeyPoint> kp_2_single;
    vector<bool> success_single;
    OpticalFlowSingleLevel(img_1, img_2, kp_1, kp_2_single, success_single);
    // 画图：单层LK光流
    Mat img_2_single;
    cv::cvtColor(img_2,img_2_single, CV_GRAY2BGR);
    for (int i=0; i<kp_2_single.size(); i++){
        if (success_single[i]){
            cv::circle(img_2_single, kp_2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img_2_single, kp_1[i].pt, kp_2_single[i].pt, cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("By single", img_2_single);

    // 反向单层LK光流
    vector<KeyPoint> kp_2_single_inverse;
    vector<bool> success_single_inverse;
    OpticalFlowSingleLevel(img_1, img_2, kp_1, kp_2_single_inverse, success_single_inverse, true);
    // 画图：单层LK光流
    Mat img_2_single_inverse;
    cv::cvtColor(img_2,img_2_single_inverse, CV_GRAY2BGR);
    for (int i=0; i<kp_2_single_inverse.size(); i++){
        if (success_single_inverse[i]){
            cv::circle(img_2_single_inverse, kp_2_single_inverse[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img_2_single_inverse, kp_1[i].pt, kp_2_single_inverse[i].pt, cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("By single_inverse", img_2_single_inverse);

    // 多层LK光流
    vector<KeyPoint> kp_2_multi;
    vector<bool> success_multi;
    OpticalFlowMultiLevel(img_1, img_2, kp_1, kp_2_multi, success_multi, true);
    // OpticalFLowMultiLevel(img_1, img_2, kp_1, kp_2_multi, success_multi, true);
    // 画图：多层LK光流
    Mat img_2_multi;
    cv::cvtColor(img_2,img_2_multi, CV_GRAY2BGR);
    for (int i=0; i<kp_2_multi.size(); i++){
        if (success_multi[i]){
            cv::circle(img_2_multi, kp_2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img_2_multi, kp_1[i].pt, kp_2_multi[i].pt, cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("By multi", img_2_multi);

    cv::waitKey(0);
    return 0;
}

void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse, bool has_initial){

    kp2.resize(kp1.size());
    success.resize(kp1.size());
    OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
    parallel_for_(Range(0, kp1.size()),
        std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
}

void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse) {

    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    vector<Mat> pyr1, pyr2;
    for (int i = 0; i < pyramids; i++){
        if(i==0){
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        }
        else{
            Mat img1_ptr, img2_ptr;
            cv::resize(pyr1[i-1], img1_ptr, Size(), pyramid_scale, pyramid_scale);
            cv::resize(pyr2[i-1], img2_ptr, Size(), pyramid_scale, pyramid_scale);
            pyr1.push_back(img1_ptr);
            pyr2.push_back(img2_ptr);
        }
    }

    vector<KeyPoint> kp1_ptr, kp2_ptr;
    for (auto &kp: kp1){
        auto kp_top = kp;
        kp_top.pt *= (pow(pyramid_scale, pyramids - 1));
        kp1_ptr.push_back(kp_top);
        kp2_ptr.push_back(kp_top);
    }

    for(int level = pyramids - 1; level >= 0; level--){
        success.clear();
        OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_ptr, kp2_ptr, success, inverse, true);

        if (level > 0){
            for (auto &kp:kp1_ptr){
                kp.pt /= pyramid_scale;
            }
            for (auto &kp:kp2_ptr){
                kp.pt /= pyramid_scale;
            }
        }
    }

    for(auto &kp: kp2_ptr){
        kp2.push_back(kp);
    }
}

// void OpticalFlowTracker::calculateOpticalFlow(const Range &range){
//     int half_patch_size = 4;
//     int iterations = 10;
//     for(size_t i = range.start; i < range.end; i++){
//         auto kp = kp1[i];
//         double dx = 0, dy = 0;
//         if (has_initial){
//             dx = kp2[i].pt.x - kp.pt.x;
//             dy = kp2[i].pt.y - kp.pt.y;
//         }

//         double cost = 0, lastCost = 0;
//         bool succ = true;

//         // Gauss-Newton iterations
//         Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
//         Eigen::Vector2d b = Eigen::Vector2d::Zero();
//         Eigen::Vector2d J;

//         for (int iter = 0; iter < iterations; iter++){

//             cost = 0;
//             if (inverse == false){
//                 H = Eigen::Matrix2d::Zero();
//                 b = Eigen::Vector2d::Zero();
//             }
//             else{
//                 b = Eigen::Vector2d::Zero();
//             }

//             for (int x = -half_patch_size; x < half_patch_size; x++){
//                 for (int y = -half_patch_size; y < half_patch_size; y++) {
//                     double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
//                                    GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
//                     if (inverse == false) {
//                         J = -1.0 * Eigen::Vector2d(
//                             0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
//                                    GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
//                             0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
//                                    GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
//                         );
//                     } else if (iter == 0) {
//                         // in inverse mode, J keeps same for all iterations
//                         // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
//                         J = -1.0 * Eigen::Vector2d(
//                             0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
//                                    GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
//                             0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
//                                    GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
//                         );
//                     }
//                     // compute H, b and set cost;
//                     b += -error * J;
//                     cost += error * error;
//                     if (inverse == false || iter == 0) {
//                         // also update H
//                         H += J * J.transpose();
//                     }
//                 }
//             }

//             Eigen::Vector2d update = H.ldlt().solve(b);

//             if (std::isnan(update[0])) {
//                 // sometimes occurred when we have a black or white patch and H is irreversible
//                 cout << "update is nan" << endl;
//                 succ = false;
//                 break;
//             }

//             if (iter > 0 && cost > lastCost) {
//                 break;
//             }

//             // update dx, dy
//             dx += update[0];
//             dy += update[1];
//             lastCost = cost;
//             succ = true;

//             if (update.norm() < 1e-2) {
//                 // converge
//                 break;
//             }
//         }

//         success[i] = succ;

//         // set kp2
//         kp2[i].pt = kp.pt + Point2f(dx, dy);
//     }
// }

void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {
    // parameters
    int half_patch_size = 4;
    int iterations = 10;
    for (size_t i = range.start; i < range.end; i++) {
        auto kp = kp1[i];
        double dx = 0, dy = 0; // dx,dy need to be estimated
        if (has_initial) {
            dx = kp2[i].pt.x - kp.pt.x;
            dy = kp2[i].pt.y - kp.pt.y;
        }

        double cost = 0, lastCost = 0;
        bool succ = true; // indicate if this point succeeded

        // Gauss-Newton iterations
        Eigen::Matrix2d H = Eigen::Matrix2d::Zero();    // hessian
        Eigen::Vector2d b = Eigen::Vector2d::Zero();    // bias
        Eigen::Vector2d J;  // jacobian
        for (int iter = 0; iter < iterations; iter++) {
            if (inverse == false) {
                H = Eigen::Matrix2d::Zero();
                b = Eigen::Vector2d::Zero();
            } else {
                // only reset b
                b = Eigen::Vector2d::Zero();
            }

            cost = 0;

            // compute cost and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++) {
                    double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
                                   GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
                    if (inverse == false) {
                        J = -1.0 * Eigen::Vector2d(
                            0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
                                   GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
                            0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
                                   GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
                        );
                    } else if (iter == 0) {
                        // in inverse mode, J keeps same for all iterations
                        // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
                        J = -1.0 * Eigen::Vector2d(
                            0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
                                   GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
                            0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
                                   GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
                        );
                    }
                    // compute H, b and set cost;
                    b += -error * J;
                    cost += error * error;
                    if (inverse == false || iter == 0) {
                        // also update H
                        H += J * J.transpose();
                    }
                }

            // compute update
            Eigen::Vector2d update = H.ldlt().solve(b);

            if (std::isnan(update[0])) {
                // sometimes occurred when we have a black or white patch and H is irreversible
                cout << "update is nan" << endl;
                succ = false;
                break;
            }

            if (iter > 0 && cost > lastCost) {
                break;
            }

            // update dx, dy
            dx += update[0];
            dy += update[1];
            lastCost = cost;
            succ = true;

            if (update.norm() < 1e-2) {
                // converge
                break;
            }
        }

        success[i] = succ;

        // set kp2
        kp2[i].pt = kp.pt + Point2f(dx, dy);
    }
}


/**
 * @file
 * @brief  NPDE UnstableBVP. Solution of source-free heat equation and
 * computation of H1 seminorms on different triangular meshes and refinement
 * levels
 * @author Julien Gacon, Ralf Hiptmair, Amélie Loher
 * @date   March 2019
 * @copyright MIT License
 */
#include "unstablebvp.h"
// General includes
#include <fstream>
#include <iomanip>
#include <memory>
// Math includes
#include <cmath>
// Eigen
#include <Eigen/Core>
// Lehrfempp
#include <lf/mesh/mesh.h>
#include <lf/refinement/refinement.h>

int main() {
  // Define the number of refinement levels we want for our mesh
  const int reflevels = 7;

  // Do the computations for all three possible mesh locations:
  // Above x2 = 0 (top), intersecting it (center), or below it (bottom)
  // Store the data in a Eigen Matrix for nice display later on
  Eigen::MatrixXd h1_seminorms(reflevels + 1, 3), h1_diffs(reflevels + 1, 3);

  // 0 for top (-> first column), 1 for center, 2 for bottom
  int type_idx = 0;
  for (auto mesh_type : {"top", "center", "bottom"}) {
    // Get a hierachy of refined meshes
    std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
        UnstableBVP::createMeshHierarchy(reflevels, mesh_type);
    lf::refinement::MeshHierarchy &multi_mesh{*multi_mesh_p};

    // Number of levels
    const int L = multi_mesh.NumLevels();

    // Computing the difference u_L - u_k, k=0...L-1 is hard, since we would
    // the functions are defined on different meshes.
    // To avoid this caveat we can approximate using the triangle inequality:
    // abs(norm(u_L) - norm(u_k)) <= norm(u_L - u_k)
    // This should also display the right convergence properties.

    // H1 seminorm of most refined solution (level L-1)
    double h1_uL =
        UnstableBVP::solveTemperatureDistribution(multi_mesh.getMesh(L - 1));

    // Store H1 seminorms and the difference of the seminorms to uL
    h1_seminorms(reflevels, type_idx) = h1_uL;
    h1_diffs(reflevels, type_idx) = 0;

    // Compute H1 seminorm for all levels 0..L-2
    for (int level = 0; level < L - 1; ++level) {
      // Get the mesh pointer
      std::shared_ptr<const lf::mesh::Mesh> mesh_p = multi_mesh.getMesh(level);

      // Get the seminorm
      double h1 = UnstableBVP::solveTemperatureDistribution(mesh_p);

      // Store
      h1_seminorms(level, type_idx) = h1;
      h1_diffs(level, type_idx) = std::abs(h1_uL - h1);
    }
    ++type_idx;
  }

  // Print to terminal
  std::cout << std::left << std::setfill('-');
  std::cout << std::setw(39) << "#  -- Above x2 = 0 " << std::setw(30)
            << "|-- Intersecting x2 = 0 " << std::setw(30)
            << "|-- Below x2 = 0 "
            << "\n"
            << std::right << std::setfill(' ');
  std::cout << "#" << std::setw(8) << "Level" << std::setw(10) << "|u_k|"
            << std::setw(20) << "||u_L| - |u_k||" << std::setw(10) << "|u_k|"
            << std::setw(20) << "||u_L| - |u_k||" << std::setw(10) << "|u_k|"
            << std::setw(20) << "||u_L| - |u_k||"
            << "\n";

  for (int l = 0; l < reflevels; ++l) {
    std::cout << std::setw(9) << l << std::setw(10) << h1_seminorms(l, 0)
              << std::setw(20) << h1_diffs(l, 0) << std::setw(10)
              << h1_seminorms(l, 1) << std::setw(20) << h1_diffs(l, 1)
              << std::setw(10) << h1_seminorms(l, 2) << std::setw(20)
              << h1_diffs(l, 2) << "\n";
  }
  std::cout << std::setw(9) << reflevels << std::setw(10)
            << h1_seminorms(reflevels, 0) << std::setw(20) << 0 << std::setw(10)
            << h1_seminorms(reflevels, 1) << std::setw(20) << 0 << std::setw(10)
            << h1_seminorms(reflevels, 2) << std::setw(20) << 0 << "\n";

  return 0;
}


#include <iostream>
#include <cmath>
#include <vector>
#include <chrono>
#include <memory>
#include <random>
#include <boost/range/irange.hpp>
#include <Eigen/Dense>
#include "GPs.h"

// Include CppOptLib files
#include "./include/cppoptlib/meta.h"
#include "./include/cppoptlib/boundedproblem.h"
#include "./include/cppoptlib/solver/lbfgsbsolver.h"

// Retrieve aliases from GP namescope
using Matrix = GP::Matrix;
using Vector = GP::Vector;

// Compute pairwise distance between lists of points
void GP::pdist(Matrix & Dv, Matrix & X1, Matrix & X2)
{
  auto n = static_cast<int>(X1.rows());
  int k = 0;
  auto entryCount = static_cast<int>( (n*(n-1))/2);
  Dv.resize(entryCount, 1);
  for ( auto i : boost::irange(0,n-1) )
    {
      for ( auto j : boost::irange(i+1,n) )
          Dv(k++) = (static_cast<Vector>(X1.row(i) - X2.row(j))).squaredNorm();
    }
}

// Re-assemble pairwise distances into a dense matrix
void GP::squareForm(Matrix & D, Matrix & Dv, int n, double diagVal)
{

  D.resize(n,n);

  int k = 0;
  for ( auto i : boost::irange(0,n-1) )
    {
      for ( auto j : boost::irange(i+1,n) )
        D(i,j) = D(j,i) = Dv(k++,0);
    }

  D.diagonal() = diagVal * Eigen::MatrixXd::Ones(n,1);
}

// Compute covariance matrix (and gradients) from a vector of squared pairwise distances Dv
std::vector<Matrix> GP::RBF::computeCov(Matrix & K, Matrix & Dv, Vector & params, double jitter, bool evalGrad)
{
  auto n = static_cast<int>(K.rows());
  int lengthIndex;
  if ( params.size() > paramCount )
    {
      Kv.noalias() = ( (-0.5 / std::pow(params(1),2)) * Dv ).array().exp().matrix();
      squareForm(K, Kv, n, 1.0 + params(0) + jitter);
      lengthIndex = 1;
    }
  else
    {
      Kv.noalias() = ( (-0.5 / std::pow(params(0),2)) * Dv ).array().exp().matrix();
      squareForm(K, Kv, n, 1.0 + noiseLevel + jitter);
      lengthIndex = 0;
    }

  // Compute gradient list if "evalGrad=true"
  std::vector<Matrix> gradList;
  if ( evalGrad )
    {
      dK_iv.noalias() = 1/std::pow(params(lengthIndex),2) * ( Dv.array() * Kv.array() ).matrix();
      squareForm(dK_i, dK_iv, n); // Note: diagVal = 0.0
      gradList.push_back(dK_i);
    }

  return gradList;

};

// Compute cross covariance between two input vectors using kernel parameters params
void GP::RBF::computeCrossCov(Matrix & K, Matrix & X1, Matrix & X2, Vector & params)
{
  // Get prediction count
  auto m = static_cast<int>(X2.rows());

   // Define lambda function to create unary operator (by clamping kernelParams argument)
  auto lambda = [=,&params](double d)->double { return evalDistKernel(d, params, 0); };
  for ( auto j : boost::irange(0,m) )
    {
      K.col(j) = ((X1.rowwise() - X2.row(j)).rowwise().squaredNorm()).unaryExpr(lambda);
    }

};

// Precompute distance matrix to avoid repeated calculations during optimization procedure
void GP::GaussianProcess::computeDistMat()
{
  pdist(distMatrix, obsX, obsX);
}

// Evaluate NLML for specified kernel hyperparameters p
double GP::GaussianProcess::evalNLML(const Vector & p, Vector & g, bool evalGrad)
{
  // Get matrix input observation count
  auto n = static_cast<int>(obsX.rows());

  // ASSUME OPTIMIZATION OVER LOG VALUES
  auto params = static_cast<Vector>(p);
  params = params.array().exp().matrix();

  // Compute covariance matrix and store Cholesky factor
  K.resize(n,n);
  //time start = high_resolution_clock::now();
  auto gradList = (*kernel).computeCov(K, distMatrix, params, jitter, evalGrad);
  //time end = high_resolution_clock::now();
  //time_computecov += getTime(start, end);

  //start = high_resolution_clock::now();
  cholesky = K.llt();
  //end = high_resolution_clock::now();
  //time_cholesky_llt += getTime(start, end);

  // Store alpha for DNLML calculation
  _alpha.noalias() = cholesky.solve(obsY);

  // Compute NLML value
  //start = high_resolution_clock::now();
  double NLML_value = 0.5*(obsY.transpose()*_alpha)(0);
  NLML_value += static_cast<Matrix>(cholesky.matrixL()).diagonal().array().log().sum();
  NLML_value += 0.5*n*std::log(2*PI);
  //end = high_resolution_clock::now();
  //time_NLML += getTime(start, end);

  if ( evalGrad )
    {

      //
      // Precompute the multiplicative term in derivative expressions
      //
      // [ THIS APPEARS TO BE A COMPUTATIONAL BOTTLE-NECK ]
      //

      //start = high_resolution_clock::now();
      // Direct evaluation of inverse matrix
      term.noalias() = cholesky.solve(Matrix::Identity(n,n)) - _alpha*_alpha.transpose();

      /*
      //
      //          ---  PARALLEL IMPLEMENTATION  ---
      //
      //  [ Typically much slower; possibly not thread-safe... ]
      //
      term.resize(n,n);
      //Matrix ei = Eigen::MatrixXd::Zero(n,1);
      int i = 0;
      //#pragma omp parallel for private(i) shared(cholesky, term, n)
#pragma omp parallel for private(i) shared(cholesky, _alpha, term, n)
      for ( i=0 ; i<n ; i++ )
        {
          Matrix ei = Eigen::MatrixXd::Zero(n,1);
          ei(i) = 1.0;
          //term.col(i) = cholesky.solve(ei);
          term.col(i) = cholesky.solve(ei);
          term.col(i) -= _alpha(i)*_alpha;
        }
      */

      //term.noalias() -= _alpha*_alpha.transpose();
      //end = high_resolution_clock::now();
      //time_term += getTime(start, end);

      //start = high_resolution_clock::now();
      // Compute gradient for noise term if 'fixedNoise=false'
      int index = 0;
      if (!fixedNoise)
        {
          // Specify gradient of white noise kernel
          g(index++) = 0.5 * (term * params(0)).trace() ;
        }

      // Compute gradients with respect to kernel hyperparameters
      for (auto dK_i = gradList.begin(); dK_i != gradList.end(); ++dK_i)
        {
          // Compute trace of full matrix
          g(index++) = 0.5 * (term * (*dK_i) ).trace() ;
        }
      //end = high_resolution_clock::now();
      //time_grad += getTime(start, end);

    }

  return NLML_value;

}

// Define simplified interface for evaluating NLML without gradient calculation
double GP::GaussianProcess::evalNLML(const Vector & p)
{
  Vector nullGrad(0);
  return evalNLML(p,nullGrad,false);
}

// Define function for uniform sampling
Matrix GP::sampleUnif(double a, double b, int N, int dim)
{
  //return (b-a)*(Eigen::MatrixXd::Random(N,1) * 0.5 + 0.5*Eigen::MatrixXd::Ones(N,1)) + a*Eigen::MatrixXd::Ones(N,1);
  return (b-a)*(Eigen::MatrixXd::Random(N,dim) * 0.5 + 0.5*Eigen::MatrixXd::Ones(N,dim) ) + a*Eigen::MatrixXd::Ones(N,dim);
}

// Define function for uniform sampling [Vectors]
Vector GP::sampleUnifVector(Vector lbs, Vector ubs)
{
  auto n = static_cast<int>(lbs.rows());

  Vector sampleVector = ((ubs-lbs).array()*( 0.5*Eigen::MatrixXd::Random(n,1) + 0.5*Eigen::MatrixXd::Ones(n,1) ).array()).matrix() + (lbs.array()*Eigen::MatrixXd::Ones(n,1).array()).matrix();

  return sampleVector;
}

// Define function for sampling from standard normal distribution
Matrix GP::sampleNormal(int N)
{
  // Note: Boost random is currently throwing deprecated header warnings...
  //boost::random::mt19937 rng;
  //boost::random::normal_distribution<> normalDist;
  std::default_random_engine rng;
  std::normal_distribution<double> normalDist(0.0,1.0);
  Matrix sampleVals(N,1);
  for ( auto i : boost::irange(0,N) )
    sampleVals(i) = normalDist(rng);
  return sampleVals;
}

// Generate equally spaced points on an interval or square region
Matrix GP::linspace(double a, double b, int N, int dim)
{

  Matrix linspaceVals;
  if ( dim == 1 )
    {
      linspaceVals.resize(N,1);
      linspaceVals = Eigen::Array<double, Eigen::Dynamic, 1>::LinSpaced(N, a, b);
    }
  else if ( dim == 2 )
    {
      linspaceVals.resize(N*N,2);
      Matrix linspaceVals1D = Eigen::Array<double, Eigen::Dynamic, 1>::LinSpaced(N, a, b);
      int k = 0;
      for ( auto i : boost::irange(0,N) )
        {
          for ( auto j : boost::irange(0,N) )
            {
              linspaceVals(k,0) = linspaceVals1D(i);
              linspaceVals(k,1) = linspaceVals1D(j);
              k++;
            }
        }
    }
  else
      std::cout << "[*] GP::linspace has not been implemented for dim > 2\n";

  return linspaceVals;
}

// Define utility function for formatting hyperparameter bounds
void GP::GaussianProcess::parseBounds(Vector & lbs, Vector & ubs, int augParamCount)
{
  lbs.resize(augParamCount);
  ubs.resize(augParamCount);

  double defaultLowerBound = 0.00001;
  double defaultUpperBound = 10.0;

  if ( fixedBounds )
    {
      // Check if bounds for noise parameter were provided
      if ( lowerBounds.size() < augParamCount )
        {
          // Set noise bounds to defaults
          lbs(0) = std::log( defaultLowerBound );
          ubs(0) = std::log( defaultUpperBound );

          // Convert specified bounds to log-scale
          for ( auto bi : boost::irange(1,augParamCount) )
            {
              lbs(bi) = std::log(lowerBounds(bi-1));
              ubs(bi) = std::log(upperBounds(bi-1));
            }
        }
      else
        {
          // Convert specified bounds to log-scale
          lbs = (lowerBounds.array().log()).matrix();
          ubs = (upperBounds.array().log()).matrix();
        }
    }
  else
    {
      // Set noise and hyperparameter bounds to defaults
      lbs = ( defaultLowerBound * Eigen::MatrixXd::Ones(augParamCount,1) ).array().log().matrix();
      ubs = ( defaultUpperBound * Eigen::MatrixXd::Ones(augParamCount,1) ).array().log().matrix();
    }

  /*
  std::cout << "Bounds:\n";
  std::cout << lbs.array().exp().matrix().transpose() << std::endl;
  std::cout << ubs.array().exp().matrix().transpose() << std::endl;
  std::cout << "\nLog Bounds:\n";
  std::cout << lbs.transpose() << std::endl;
  std::cout << ubs.transpose() << std::endl << std::endl;
  */

}

// Fit model hyperparameters
void GP::GaussianProcess::fitModel()
{

  // Get combined parameter/noise vector size
  paramCount = (*kernel).getParamCount();
  augParamCount = (fixedNoise) ? static_cast<int>(paramCount) : static_cast<int>(paramCount) + 1 ;

  // Pass noise level to kernel when 'fixedNoise=true'
  if ( fixedNoise )
    (*kernel).setNoise(noiseLevel);

  // Precompute distance matrix
  computeDistMat();

  // Declare vector for storing gradient calculations
  Vector g(augParamCount);

  // Initialize gradient vector size
  cppOptLibgrad.resize(augParamCount);

  // Convert hyperparameter bounds to log-scale
  Vector lbs, ubs;
  parseBounds(lbs, ubs, augParamCount);

  Vector optParams = Eigen::MatrixXd::Zero(augParamCount,1);

  this->setLowerBound(lbs);
  this->setUpperBound(ubs);
  cppoptlib::LbfgsbSolver<GaussianProcess> solver;

  // Specify stopping criteria
  cppoptlib::Criteria<double> crit = cppoptlib::Criteria<double>::defaults();
  //crit.iterations = 5000;
  //crit.gradNorm = 10.0;   //!< Minimum norm of gradient vector
  //crit.xDelta = 0;      //!< Minimum change in parameter vector
  //crit.fDelta = 0;      //!< Minimum change in cost function
  //crit.condition = 0;
  solver.setStopCriteria(crit);

  solver.minimize(*this, optParams);

  // ASSUME OPTIMIZATION OVER LOG VALUES
  optParams = optParams.array().exp().matrix();

  //start = high_resolution_clock::now();
  ///* [ This is included in the SciKit Learn model.fit() call as well ]

  // Recompute covariance and Cholesky factor
  auto nullGradList = (*kernel).computeCov(K, distMatrix, optParams);
  cholesky = K.llt();
  _alpha.noalias() = cholesky.solve(obsY);

  //*/
  //end = high_resolution_clock::now();
  //time_final_chol += getTime(start, end);

  // Assign tuned parameters to model
  if (!fixedNoise)
    {
      noiseLevel = static_cast<double>((optParams.head(1))[0]);
      optParams = static_cast<Vector>(optParams.tail(augParamCount - 1));
    }

  (*kernel).setParams(optParams);

  // DISPLAY TIMING INFORMATION
  /*
  std::cout << "\n\n Time Diagnostics |\n";
  std::cout << "------------------\n";
  std::cout << "computeCov():\t  " << time_computecov  << std::endl;
  std::cout << "cholesky.llt():\t  " << time_cholesky_llt  << std::endl;
  std::cout << "NLML:\t  \t  " << time_NLML  << std::endl;
  std::cout << "Grad term:\t  " << time_term  << std::endl;
  std::cout << "Gradient:\t  " << time_grad  << std::endl;
  std::cout << "\n" << std::endl;
  std::cout << "paramSearch:\t  " << time_paramsearch  << std::endl;
  std::cout << "Minimize:\t  " << time_minimize  << std::endl;
  std::cout << "Final Cholesky:\t  " << time_final_chol  << std::endl;
  */

};

// Compute predicted values
void GP::GaussianProcess::predict()
{
  // Get matrix input observation count
  auto n = static_cast<int>(obsX.rows());
  auto m = static_cast<int>(predX.rows());

  // Get optimized kernel hyperparameters
  Vector params = (*kernel).getParams();

  // Compute cross covariance for test points
  Matrix kstar;
  kstar.resize(n,m);
  (*kernel).computeCrossCov(kstar, obsX, predX, params);

  // Compute covariance matrix for test points
  Matrix kstarmat;
  kstarmat.resize(m,m);
  (*kernel).computeCrossCov(kstarmat, predX, predX, params);

  // Possible redundant calculations; should simplify...
  Matrix cholMat(cholesky.matrixL());
  //Matrix alpha = cholesky.solve(obsY);
  Matrix v = kstar;
  cholMat.triangularView<Eigen::Lower>().solveInPlace(v);

  // Set predictive means/variances and compute negative log marginal likelihood
  //predMean = kstar.transpose() * alpha;
  predMean = kstar.transpose() * _alpha;
  predCov = kstarmat - v.transpose() * v;

}

// Draw sample paths from posterior distribution
Matrix GP::GaussianProcess::getSamples(int count)
{
  // Get number of target points
  auto n = static_cast<int>(predX.rows());

  // Construct simple random generator engine from a time-based seed
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::normal_distribution<double> normal (0.0,1.0);

  // Assign i.i.d. random normal values to uVals
  Matrix uVals(n,count);
  for (auto i : boost::irange(0,n))
    {
      for (auto j : boost::irange(0,count))
          uVals(i,j) = normal(generator);
    }

  // Compute Cholesky factor L
  Matrix L = ( predCov + (noiseLevel+jitter)*Matrix::Identity(static_cast<int>(predCov.cols()), static_cast<int>(predCov.cols())) ).llt().matrixL();

  // Draw samples using the formula:  y = m + L*u
  Matrix samples = predMean.replicate(1,count) + L*uVals;

  return samples;
}

// Evaluate NLML [public interface]
double GP::GaussianProcess::computeNLML(const Vector & p, double noise)
{
  // Compute log-hyperparameters
  Vector logparams(augParamCount);

  if ( !fixedNoise )
    {
      logparams(0) = std::log(noise);
      for ( auto i : boost::irange(1,augParamCount) )
        logparams(i) = std::log(p(i-1));
    }
  else
    {
      logparams = logparams.array().log().matrix();
    }

  // Evaluate NLML using log-hyperparameters
  return evalNLML(logparams);
}

// Evaluate NLML with default noise level [public interface]
double GP::GaussianProcess::computeNLML(const Vector & p)
{
  return computeNLML(p, noiseLevel);
}

// Evaluate NLML with default noise level [public interface]
double GP::GaussianProcess::computeNLML()
{
  auto params = (*kernel).getParams();
  return computeNLML(params, noiseLevel);
}

// Define function for retrieving time from chrono
float GP::getTime(std::chrono::high_resolution_clock::time_point start, std::chrono::high_resolution_clock::time_point end)
{
  return static_cast<float>(std::chrono::duration_cast<std::chrono::microseconds>( end - start ).count() / 1000000.0);
};

// Define kernel function for RBF
double GP::RBF::evalKernel(Matrix & x, Matrix & y, Vector & params, int n)
{
  switch (n)
    {
    case 0: return std::exp( -(x-y).squaredNorm() / (2.0*std::pow(params(0),2)));
    case 1: return (x-y).squaredNorm() / std::pow(params(0),3) * std::exp( -(x-y).squaredNorm() / (2.0*std::pow(params(0),2)));
    default: std::cout << "\n[*] UNDEFINED DERIVATIVE\n"; return 0.0;
    }
};

// Define distance kernel function for RBF
//
// REVISION:  Optimize w.r.t. theta = log(l) for stability  ==>   / l^2  instead of  / l^3
//
double GP::RBF::evalDistKernel(double d, Vector & params, int n)
{
  switch (n)
    {
    case 0: return std::exp( -d / (2.0*std::pow(params(0),2)));
    case 1: return d / std::pow(params(0),2) * std::exp( -d / (2.0*std::pow(params(0),2)));
    default: std::cout << "\n[*] UNDEFINED DERIVATIVE\n"; return 0.0;
    }
};

/*
// POTENTIAL PARALLEL IMPLEMENTATION OF SQUARE FORM; SPEED-UP APPEARS NEGLIGIBLE
// Re-assemble pairwise distances into a dense matrix
void GP::squareFormParallel(Matrix & D, Matrix & Dv, int n, double diagVal)
{

  D.resize(n,n);
  int i;
  int j;
#pragma omp parallel for private(i,j) shared(D,Dv,n)
  for ( i = 0 ; i<n-1; i++ )
    {
      for ( j = i+1 ; j<n ; j++ )
        {
          D(i,j) = D(j,i) = Dv( static_cast<int>(i*n - (i*(i+1))/2 + j - i -1) ,0);
        }
    }
  D.diagonal() = diagVal * Eigen::MatrixXd::Ones(n,1);
}
*/

//
//   ORIGINAL MINIMIZATION IMPLEMENTATION USING RASMUSSEN'S CODE
//

//
//    NOTE:
//
//    Remember to include "utils/minimize.h" and derive the
//   'GaussianProcess' class from the 'GradientObj' class:
//
//    i.e.  class GaussianProcess : public minimize::GradientObj
//

/*
void minimize(...)
{

  //
  // Specify the parameters for the minimization algorithm
  //

  //   HIGH ACCURACY SETTINGS   //
  // max of MAX function evaluations per line search
  //int MAX = 30;
  // max number of line searches = length
  //int length = 20;
  // don't reevaluate within INT of the limit of the current bracket
  //double INT = 0.00001;
  // SIG is a constant controlling the Wolfe-Powell conditions
  //double SIG = 0.9;
  // extrapolate maximum EXT times the current step-size
  //double EXT = 5.0;

  //  EFFICIENT SETTINGS  //

  int MAX = 15;
  int length = 10;
  double INT = 0.00001;
  double SIG = 0.9;
  double EXT = 5.0;

  // Define number of exploratory NLML evaluations for specifying
  // a reasonable initial value for the optimization algorithm
  int initParamSearchCount = 30;

  // Define restart count for optimizer
  int restartCount = 0;

  // Convert hyperparameter bounds to log-scale
  Vector lbs, ubs;
  parseBounds(lbs, ubs, augParamCount);

  // Declare variables to store optimization loop results
  double currentVal;
  double optVal = 1e9;
  Vector theta(augParamCount);
  Vector optParams(augParamCount);

  //time start = high_resolution_clock::now();
  // First explore hyperparameter space to get a reasonable initializer for optimization
  for ( auto i : boost::irange(0,initParamSearchCount) )
    {
      if ( i == 0 )
          theta = Eigen::MatrixXd::Zero(augParamCount,1);
      else
          theta = sampleUnifVector(lbs, ubs);

      // Compute current NLML and store parameters if optimal
      currentVal = evalNLML(theta);
      if ( currentVal < optVal )
        {
          optVal = currentVal;
          optParams = theta;
        }
      //std::cout << "Theta Search:  " << theta.transpose() << "  [ NLML = " << currentVal << " ]"<< std::endl;
    }
  //time end = high_resolution_clock::now();
  //time_paramsearch += getTime(start, end);

  // NOTE: THIS NEEDS TO BE RE-WRITTEN TO USE THE PRELIMINARY PARAMETER SEARCH RESULTS
  // Evaluate optimizer with various different initializations
  for ( auto i : boost::irange(0,restartCount) )
    {
      // Avoid compiler warning for unused variable
      //(void)i;

      if ( i == 0 )
        {
          // Set initial guess (should make this user specifiable...)
          theta = Eigen::MatrixXd::Zero(augParamCount,1);
        }
      else
        {
          // Sample initial hyperparameter vector
          theta = sampleUnifVector(lbs, ubs);
        }

      // Optimize hyperparameters
      minimize::cg_minimize(theta, this, g, length, SIG, EXT, INT, MAX);

      // Compute current NLML and store parameters if optimal
      currentVal = evalNLML(theta);
      if ( currentVal < optVal )
        {
          optVal = currentVal;
          optParams = theta;
        }
    }

  // Perform one last optimization starting from best parameters so far
  if ( ( initParamSearchCount == 0 ) && ( restartCount == 0 ) )
    optParams = sampleUnifVector(lbs, ubs);
  //std::cout << "\n[*] FINAL - Initial Values (log):  " << optParams.transpose() << std::endl;
  //std::cout << "[*] FINAL - Initial Values (std):  " << optParams.transpose().array().exp().matrix() << std::endl;
  //start = high_resolution_clock::now();
  minimize::cg_minimize(optParams, this, g, length, SIG, EXT, INT, MAX);
  //end = high_resolution_clock::now();
  //time_minimize += getTime(start, end);

}
*/


#include <iostream>
#include <random>
#include <Eigen/Geometry>

#include "GeometryDerivatives.h"
#include "MeshConnectivity.h"

Eigen::Matrix3d crossMatrix(Eigen::Vector3d v)
{
    Eigen::Matrix3d ret;
    ret << 0, -v[2], v[1],
        v[2], 0, -v[0],
        -v[1], v[0], 0;
    return ret;
}

Eigen::Matrix2d adjugate(Eigen::Matrix2d M)
{
    Eigen::Matrix2d ret;
    ret << M(1, 1), -M(0, 1), -M(1, 0), M(0, 0);
    return ret;
}

double angle(const Eigen::Vector3d& v, const Eigen::Vector3d& w, const Eigen::Vector3d& axis,
    Eigen::Matrix<double, 1, 9>* derivative, // v, w, a
    Eigen::Matrix<double, 9, 9>* hessian
)
{
    double theta = 2.0 * atan2((v.cross(w).dot(axis) / axis.norm()), v.dot(w) + v.norm() * w.norm());

    if (derivative)
    {
        derivative->segment<3>(0) = -axis.cross(v) / v.squaredNorm() / axis.norm();
        derivative->segment<3>(3) = axis.cross(w) / w.squaredNorm() / axis.norm();
        derivative->segment<3>(6).setZero();
    }
    if (hessian)
    {
        hessian->setZero();
        hessian->block<3, 3>(0, 0) += 2.0 * (axis.cross(v)) * v.transpose() / v.squaredNorm() / v.squaredNorm() / axis.norm();
        hessian->block<3, 3>(3, 3) += -2.0 * (axis.cross(w)) * w.transpose() / w.squaredNorm() / w.squaredNorm() / axis.norm();
        hessian->block<3, 3>(0, 0) += -crossMatrix(axis) / v.squaredNorm() / axis.norm();
        hessian->block<3, 3>(3, 3) += crossMatrix(axis) / w.squaredNorm() / axis.norm();

        Eigen::Matrix3d dahat = (Eigen::Matrix3d::Identity() / axis.norm() - axis * axis.transpose() / axis.norm() / axis.norm() / axis.norm());

        hessian->block<3, 3>(0, 6) += crossMatrix(v) * dahat / v.squaredNorm();
        hessian->block<3, 3>(3, 6) += -crossMatrix(w) * dahat / w.squaredNorm();
    }

    return theta;
}

Eigen::Vector3d faceNormal(const MeshConnectivity &mesh,
    const Eigen::MatrixXd &curPos,
    int face, int startidx,
    Eigen::Matrix<double, 3, 9> *derivative,
    std::vector<Eigen::Matrix<double, 9, 9> > *hessian)
{
    if (derivative)
        derivative->setZero();

    if (hessian)
    {
        hessian->resize(3);
        for (int i = 0; i < 3; i++) (*hessian)[i].setZero();
    }

    int v0 = startidx % 3;
    int v1 = (startidx + 1) % 3;
    int v2 = (startidx + 2) % 3;
    Eigen::Vector3d qi0 = curPos.row(mesh.faceVertex(face, v0)).transpose();
    Eigen::Vector3d qi1 = curPos.row(mesh.faceVertex(face, v1)).transpose();
    Eigen::Vector3d qi2 = curPos.row(mesh.faceVertex(face, v2)).transpose();
    Eigen::Vector3d n = (qi1 - qi0).cross(qi2 - qi0);

    if (derivative)
    {
        derivative->block<3, 3>(0, 0) += crossMatrix(qi2 - qi1);
        derivative->block<3, 3>(0, 3) += crossMatrix(qi0 - qi2);
        derivative->block<3, 3>(0, 6) += crossMatrix(qi1 - qi0);
    }

    if (hessian)
    {
        for (int j = 0; j < 3; j++)
        {
            Eigen::Vector3d ej(0, 0, 0);
            ej[j] = 1.0;
            Eigen::Matrix3d ejc = crossMatrix(ej);
            (*hessian)[j].block<3, 3>(0, 3) -= ejc;
            (*hessian)[j].block<3, 3>(0, 6) += ejc;
            (*hessian)[j].block<3, 3>(3, 6) -= ejc;
            (*hessian)[j].block<3, 3>(3, 0) += ejc;
            (*hessian)[j].block<3, 3>(6, 0) -= ejc;
            (*hessian)[j].block<3, 3>(6, 3) += ejc;
        }
    }

    return n;
}

double triangleAltitude(const MeshConnectivity &mesh,
    const Eigen::MatrixXd &curPos,
    int face,
    int edgeidx,
    Eigen::Matrix<double, 1, 9> *derivative,
    Eigen::Matrix<double, 9, 9> *hessian)
{
    if (derivative)
        derivative->setZero();
    if (hessian)
        hessian->setZero();

    Eigen::Matrix<double, 3, 9> nderiv;
    std::vector<Eigen::Matrix<double, 9, 9> > nhess;
    Eigen::Vector3d n = faceNormal(mesh, curPos, face, edgeidx, (derivative || hessian ? &nderiv : NULL), hessian ? &nhess : NULL);

    int v2 = (edgeidx + 2) % 3;
    int v1 = (edgeidx + 1) % 3;
    Eigen::Vector3d q2 = curPos.row(mesh.faceVertex(face, v2)).transpose();
    Eigen::Vector3d q1 = curPos.row(mesh.faceVertex(face, v1)).transpose();

    Eigen::Vector3d e = q2 - q1;
    double nnorm = n.norm();
    double enorm = e.norm();
    double h = nnorm / enorm;

    if (derivative)
    {
        for (int i = 0; i < 3; i++)
        {
            *derivative += nderiv.row(i) * n[i] / nnorm / enorm;
        }
        derivative->block(0, 6, 1, 3) += -nnorm / enorm / enorm / enorm * e.transpose();
        derivative->block(0, 3, 1, 3) += nnorm / enorm / enorm / enorm * e.transpose();
    }

    if (hessian)
    {
        for (int i = 0; i < 3; i++)
        {
            *hessian += nhess[i] * n[i] / nnorm / enorm;
        }

        Eigen::Matrix3d idMat = Eigen::Matrix3d::Identity();

        Eigen::Matrix3d P = idMat / nnorm - n*n.transpose() / nnorm / nnorm / nnorm;
        *hessian += nderiv.transpose() * P * nderiv / enorm;
        hessian->block<3, 9>(6, 0) += -e * n.transpose() * nderiv / nnorm / enorm / enorm / enorm;
        hessian->block<3, 9>(3, 0) += e * n.transpose() * nderiv / nnorm / enorm / enorm / enorm;
        hessian->block<9, 3>(0, 6) += -nderiv.transpose() * n * e.transpose() / nnorm / enorm / enorm / enorm;
        hessian->block<9, 3>(0, 3) += nderiv.transpose() * n * e.transpose() / nnorm / enorm / enorm / enorm;
        hessian->block<3, 3>(6, 6) += -nnorm / enorm / enorm / enorm * idMat;
        hessian->block<3, 3>(6, 3) += nnorm / enorm / enorm / enorm * idMat;
        hessian->block<3, 3>(3, 6) += nnorm / enorm / enorm / enorm * idMat;
        hessian->block<3, 3>(3, 3) += -nnorm / enorm / enorm / enorm * idMat;

        Eigen::Matrix3d outer = e*e.transpose() * 3.0 * nnorm / enorm / enorm / enorm / enorm / enorm;
        hessian->block(6, 6, 3, 3) += outer;
        hessian->block(6, 3, 3, 3) += -outer;
        hessian->block(3, 6, 3, 3) += -outer;
        hessian->block(3, 3, 3, 3) += outer;
    }

    return h;
}

Eigen::Matrix2d firstFundamentalForm(
    const MeshConnectivity &mesh,
    const Eigen::MatrixXd &curPos,
    int face,
    Eigen::Matrix<double, 4, 9> *derivative, // F(face, i)
    std::vector <Eigen::Matrix<double, 9, 9> > *hessian)
{
    Eigen::Vector3d q0 = curPos.row(mesh.faceVertex(face, 0));
    Eigen::Vector3d q1 = curPos.row(mesh.faceVertex(face, 1));
    Eigen::Vector3d q2 = curPos.row(mesh.faceVertex(face, 2));
    Eigen::Matrix2d result;
    result << (q1 - q0).dot(q1 - q0), (q1 - q0).dot(q2 - q0),
        (q2 - q0).dot(q1 - q0), (q2 - q0).dot(q2 - q0);

    if (derivative)
    {
        derivative->setZero();
        derivative->block<1, 3>(0, 3) += 2.0 * (q1 - q0).transpose();
        derivative->block<1, 3>(0, 0) -= 2.0 * (q1 - q0).transpose();
        derivative->block<1, 3>(1, 6) += (q1 - q0).transpose();
        derivative->block<1, 3>(1, 3) += (q2 - q0).transpose();
        derivative->block<1, 3>(1, 0) += -(q1 - q0).transpose() - (q2 - q0).transpose();
        derivative->block<1, 3>(2, 6) += (q1 - q0).transpose();
        derivative->block<1, 3>(2, 3) += (q2 - q0).transpose();
        derivative->block<1, 3>(2, 0) += -(q1 - q0).transpose() - (q2 - q0).transpose();
        derivative->block<1, 3>(3, 6) += 2.0 * (q2 - q0).transpose();
        derivative->block<1, 3>(3, 0) -= 2.0 * (q2 - q0).transpose();
    }

    if (hessian)
    {
        hessian->resize(4);
        for (int i = 0; i < 4; i++)
        {
            (*hessian)[i].setZero();
        }
        Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
        (*hessian)[0].block<3, 3>(0, 0) += 2.0*I;
        (*hessian)[0].block<3, 3>(3, 3) += 2.0*I;
        (*hessian)[0].block<3, 3>(0, 3) -= 2.0*I;
        (*hessian)[0].block<3, 3>(3, 0) -= 2.0*I;

        (*hessian)[1].block<3, 3>(3, 6) += I;
        (*hessian)[1].block<3, 3>(6, 3) += I;
        (*hessian)[1].block<3, 3>(0, 3) -= I;
        (*hessian)[1].block<3, 3>(0, 6) -= I;
        (*hessian)[1].block<3, 3>(3, 0) -= I;
        (*hessian)[1].block<3, 3>(6, 0) -= I;
        (*hessian)[1].block<3, 3>(0, 0) += 2.0*I;

        (*hessian)[2].block<3, 3>(3, 6) += I;
        (*hessian)[2].block<3, 3>(6, 3) += I;
        (*hessian)[2].block<3, 3>(0, 3) -= I;
        (*hessian)[2].block<3, 3>(0, 6) -= I;
        (*hessian)[2].block<3, 3>(3, 0) -= I;
        (*hessian)[2].block<3, 3>(6, 0) -= I;
        (*hessian)[2].block<3, 3>(0, 0) += 2.0*I;

        (*hessian)[3].block<3, 3>(0, 0) += 2.0*I;
        (*hessian)[3].block<3, 3>(6, 6) += 2.0*I;
        (*hessian)[3].block<3, 3>(0, 6) -= 2.0*I;
        (*hessian)[3].block<3, 3>(6, 0) -= 2.0*I;
    }

    return result;
}


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             : 视觉SLAM十四讲/ch3/useGeometry/eigenGeometry.cpp 学习记录
								矩阵库
 * Reference               :
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2018.03.26, William Yu

=================================================================================*/
/// Include Files
#include <iostream>
#include <cmath>
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
using namespace std;

/// Global Variables

/// Function Definitions

/**
 * @function main
 * @author William Yu
 * @brief Eigen几何模块的使用方法
 * @param  None
 * @retval None
 */
int main ( int argc, char** argv )
{
    //---------------------------------------------------------------------------
    //  旋转平移表示
    //----------------------------------------------------------------
    // Eigen/Geometry 几何模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    //---------------------------------------------------------------------------
    //  欧拉角
    //----------------------------------------------------------------
    //-- 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    //---------------------------------------------------------------------------
    //  四元数
    //----------------------------------------------------------------
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}