#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/extract_clusters.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <pcl/common/angles.h>
#include <pcl/registration/transformation_estimation_svd.h>

using namespace std;
typedef pcl::PointXYZ PointType;
typedef struct myPointType
{
    double x;  //mm world coordinate x
    double y;  //mm world coordinate y
    double z;  //mm world coordinate z
	int num;   //point num
};

// Get N bits of the string from back to front.
char* Substrend(char*str,int n)
{
	char *substr=(char*)malloc(n+1);
	int length=strlen(str);
	if (n>=length)
	{
		strcpy(substr,str);
		return substr;
	}
	int k=0;
	for (int i=length-n;i<length;i++)
	{
		substr[k]=str[i];
		k++;
	}
	substr[k]='\0';
	return substr;
}

int main(int argc, char **argv)
{
	// create point cloud
	pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>());

	// load data
	char* fileType;
	if (argc>1)
	{
		fileType = Substrend(argv[1],3);
	}
	if (!strcmp(fileType,"pcd"))
	{
    	// load pcd file
		pcl::io::loadPCDFile(argv[1], *cloud);
	}
	else if(!strcmp(fileType,"txt"))
	{
		// load txt data file
		int number_Txt;
		myPointType txtPoint;
		vector<myPointType> points;
		FILE *fp_txt;
		fp_txt = fopen(argv[1], "r");
		if (fp_txt)
		{
		    while (fscanf(fp_txt, "%lf %lf %lf", &txtPoint.x, &txtPoint.y, &txtPoint.z) != EOF)
		    {
		        points.push_back(txtPoint);
		    }
		}
		else
		    std::cout << "txt数据加载失败！" << endl;
		number_Txt = points.size();

		cloud->width = number_Txt;
		cloud->height = 1;
		cloud->is_dense = false;
		cloud->points.resize(cloud->width * cloud->height);

		for (size_t i = 0; i < cloud->points.size(); ++i)
		{
		    cloud->points[i].x = points[i].x;
		    cloud->points[i].y = points[i].y;
		    cloud->points[i].z = 0;
		}
	}
	else
	{
		std::cout << "please input data file name"<<endl;
		return 0;
	}

	// start calculating time
    pcl::StopWatch time;

    Eigen::Vector4f pcaCentroid;
    pcl::compute3DCentroid(*cloud, pcaCentroid);
    Eigen::Matrix3f covariance;
    pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
    Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();
    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直
    eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
    eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

    std::cout << "特征值va(3x1):\n" << eigenValuesPCA << std::endl;
    std::cout << "特征向量ve(3x3):\n" << eigenVectorsPCA << std::endl;
    std::cout << "质心点(4x1):\n" << pcaCentroid << std::endl;
    /*
    // 另一种计算点云协方差矩阵特征值和特征向量的方式:通过pcl中的pca接口，如下，这种情况得到的特征向量相似特征向量
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCA<pcl::PointXYZ> pca;
    pca.setInputCloud(cloudSegmented);
    pca.project(*cloudSegmented, *cloudPCAprojection);
    std::cerr << std::endl << "EigenVectors: " << pca.getEigenVectors() << std::endl;//计算特征向量
    std::cerr << std::endl << "EigenValues: " << pca.getEigenValues() << std::endl;//计算特征值
    */
    Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
    tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();   //R.
    tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());//  -R*t
    tm_inv = tm.inverse();

    std::cout << "变换矩阵tm(4x4):\n" << tm << std::endl;
    std::cout << "逆变矩阵tm'(4x4):\n" << tm_inv << std::endl;

    pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
    pcl::transformPointCloud(*cloud, *transformedCloud, tm);

    PointType min_p1, max_p1;
    Eigen::Vector3f c1, c;
    pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);
    c1 = 0.5f*(min_p1.getVector3fMap() + max_p1.getVector3fMap());

    std::cout << "型心c1(3x1):\n" << c1 << std::endl;

    Eigen::Affine3f tm_inv_aff(tm_inv);
    pcl::transformPoint(c1, c, tm_inv_aff);

    Eigen::Vector3f whd, whd1;
    whd1 = max_p1.getVector3fMap() - min_p1.getVector3fMap();
    whd = whd1;
    float sc1 = (whd1(0) + whd1(1) + whd1(2)) / 3;  //点云平均尺度，用于设置主方向箭头大小

    std::cout << "width1=" << whd1(0) << endl;
    std::cout << "heght1=" << whd1(1) << endl;
    std::cout << "depth1=" << whd1(2) << endl;
    std::cout << "scale1=" << sc1 << endl;

    const Eigen::Quaternionf bboxQ1(Eigen::Quaternionf::Identity());
    const Eigen::Vector3f    bboxT1(c1);
    const Eigen::Quaternionf bboxQ(tm_inv.block<3, 3>(0, 0));
    const Eigen::Vector3f    bboxT(c);

    //变换到原点的点云主方向
    PointType op;
    op.x = 0.0;
    op.y = 0.0;
    op.z = 0.0;
    Eigen::Vector3f px, py, pz;
    Eigen::Affine3f tm_aff(tm);
    pcl::transformVector(eigenVectorsPCA.col(0), px, tm_aff);
    pcl::transformVector(eigenVectorsPCA.col(1), py, tm_aff);
    pcl::transformVector(eigenVectorsPCA.col(2), pz, tm_aff);
    PointType pcaX;
    pcaX.x = sc1 * px(0);
    pcaX.y = sc1 * px(1);
    pcaX.z = sc1 * px(2);
    PointType pcaY;
    pcaY.x = sc1 * py(0);
    pcaY.y = sc1 * py(1);
    pcaY.z = sc1 * py(2);
    PointType pcaZ;
    pcaZ.x = sc1 * pz(0);
    pcaZ.y = sc1 * pz(1);
    pcaZ.z = sc1 * pz(2);

    //初始点云的主方向
    PointType cp;
    cp.x = pcaCentroid(0);
    cp.y = pcaCentroid(1);
    cp.z = pcaCentroid(2);
    PointType pcX;
    pcX.x = sc1 * eigenVectorsPCA(0, 0) + cp.x;
    pcX.y = sc1 * eigenVectorsPCA(1, 0) + cp.y;
    pcX.z = sc1 * eigenVectorsPCA(2, 0) + cp.z;
    PointType pcY;
    pcY.x = sc1 * eigenVectorsPCA(0, 1) + cp.x;
    pcY.y = sc1 * eigenVectorsPCA(1, 1) + cp.y;
    pcY.z = sc1 * eigenVectorsPCA(2, 1) + cp.z;
    PointType pcZ;
    pcZ.x = sc1 * eigenVectorsPCA(0, 2) + cp.x;
    pcZ.y = sc1 * eigenVectorsPCA(1, 2) + cp.y;
    pcZ.z = sc1 * eigenVectorsPCA(2, 2) + cp.z;

	//Rectangular vertex
	pcl::PointCloud<PointType>::Ptr transVertexCloud(new pcl::PointCloud<PointType>);//存放变换后点云包围盒的6个顶点
	pcl::PointCloud<PointType>::Ptr VertexCloud(new pcl::PointCloud<PointType>);//存放原来点云中包围盒的6个顶点
	transVertexCloud->width = 6;
	transVertexCloud->height = 1;
	transVertexCloud->is_dense = false;
	transVertexCloud->points.resize(transVertexCloud->width * transVertexCloud->height);
	transVertexCloud->points[0].x = max_p1.x;
	transVertexCloud->points[0].y = max_p1.y;
	transVertexCloud->points[0].z = max_p1.z;
	transVertexCloud->points[1].x = max_p1.x;
	transVertexCloud->points[1].y = max_p1.y;
	transVertexCloud->points[1].z = min_p1.z;
	transVertexCloud->points[2].x = max_p1.x;
	transVertexCloud->points[2].y = min_p1.y;
	transVertexCloud->points[2].z = min_p1.z;
	transVertexCloud->points[3].x = min_p1.x;
	transVertexCloud->points[3].y = max_p1.y;
	transVertexCloud->points[3].z = max_p1.z;
	transVertexCloud->points[4].x = min_p1.x;
	transVertexCloud->points[4].y = min_p1.y;
	transVertexCloud->points[4].z = max_p1.z;
	transVertexCloud->points[5].x = min_p1.x;
	transVertexCloud->points[5].y = min_p1.y;
	transVertexCloud->points[5].z = min_p1.z;
	pcl::transformPointCloud(*transVertexCloud, *VertexCloud, tm_inv);

	// 逆变换回来的角度
	cout << whd1(0) << " "<< whd1(1) << " " << whd1(2) << endl;
	auto euler = bboxQ1.toRotationMatrix().eulerAngles(0, 1, 2);
	std::cout << "Euler from quaternion in roll, pitch, yaw"<< std::endl << euler/3.14*180 << std::endl<<std::endl;

	//Output time consumption
	std::cout << "运行时间" << time.getTime() << "ms" << std::endl;

    //visualization
    pcl::visualization::PCLVisualizer viewer;
    pcl::visualization::PointCloudColorHandlerCustom<PointType> tc_handler(transformedCloud, 0, 255, 0); //设置点云颜色
	//Visual transformed point cloud
    viewer.addPointCloud(transformedCloud, tc_handler, "transformCloud");
    viewer.addCube(bboxT1, bboxQ1, whd1(0), whd1(1), whd1(2), "bbox1");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox1");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "bbox1");

    viewer.addArrow(pcaX, op, 1.0, 0.0, 0.0, false, "arrow_X");
    viewer.addArrow(pcaY, op, 0.0, 1.0, 0.0, false, "arrow_Y");
    viewer.addArrow(pcaZ, op, 0.0, 0.0, 1.0, false, "arrow_Z");

    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_handler(cloud, 255, 0, 0);
    viewer.addPointCloud(cloud, color_handler, "cloud");
    viewer.addCube(bboxT, bboxQ, whd(0), whd(1), whd(2), "bbox");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "bbox");

    viewer.addArrow(pcX, cp, 1.0, 0.0, 0.0, false, "arrow_x");
    viewer.addArrow(pcY, cp, 0.0, 1.0, 0.0, false, "arrow_y");
    viewer.addArrow(pcZ, cp, 0.0, 0.0, 1.0, false, "arrow_z");

    viewer.addCoordinateSystem(0.5f*sc1);
    viewer.setBackgroundColor(0.0, 0.0, 0.0);

	viewer.addPointCloud(VertexCloud, "temp_cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "temp_cloud");
    while (!viewer.wasStopped())
    {
          viewer.spinOnce();
    }

    return 0;
}


/**
 * @file   main.cpp
 * @author Simon Pintarelli <simon@thinkpadX1>
 * @date   Wed Oct 21 17:37:10 2015
 *
 * @brief  Example for Polar->Nodal basis transformation and quadrature
 *         in the nodal basis
 *
 *
 */

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iostream>

#include <Eigen/Dense>
#include <algorithm>

#include "quadrature/qhermite.hpp"

//#include "spectral/hermite_to_nodal.hpp"
#include "aux/eigen2hdf.hpp"
#include "post_processing/mass.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/polar_to_hermite.hpp"
#include "spectral/polar_to_nodal.hpp"

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

// obviously wrong ... But it is not used, see below.
void test1(int K)
{
  typedef Eigen::VectorXd vec_t;

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, K, K, 2, true);
  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  Polar2Nodal<polar_basis_t> p2n;
  p2n.init(polar_basis, 0.5);

  Mass mass(polar_basis);

  std::vector<size_t> elems = {0, 1, 2, 3, 4, 5, 6};
  // quadrature
  QHermiteW quad(0.5, K);
  auto& w = quad.wts();
  auto& x = quad.pts();

  // Achtung mit den Knoten und der Skalierung der Gewichte
  QHermiteW quad1(1, K);
  auto& xh = quad1.pts();

  std::string fname = std::string("P2N") + std::to_string(K) + ".h5";
  hid_t file = H5Fcreate(fname.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (size_t eidx : elems) {
    Eigen::VectorXd cp(polar_basis.n_dofs());
    cp.setZero();
    cp(eidx) = 1.0;

    const double mass_ref = mass.compute(cp.data());

    Eigen::MatrixXd cn(K, K);
    p2n.to_nodal(cn, cp);
    eigen2hdf::save(file, "cn" + std::to_string(eidx), cn);

    auto id = polar_basis.get_elem(eidx).id();

    double sum = 0;
    for (size_t q1 = 0; q1 < w.size(); ++q1) {
      for (size_t q2 = 0; q2 < w.size(); ++q2) {
        // (*) QUAD EXAMPLE
        sum += cn(q1, q2) * std::sqrt(w[q1] * w[q2]) *
               std::exp(-xh[q1] * xh[q1] / 2 - xh[q2] * xh[q2] / 2);
      }
    }
    cout << id.to_string() << "Sum: " << setprecision(5) << scientific << sum << endl;
    cout << id.to_string() << "Ref: " << setprecision(5) << scientific << mass_ref << "\n\n";
  }

  typedef Eigen::VectorXd vec_t;
  Eigen::Map<const vec_t> xq(quad.points_data(), K);
  Eigen::Map<const vec_t> wq(quad.weights_data(), K);

  eigen2hdf::save(file, "xq", xq);
  eigen2hdf::save(file, "wq", wq);

  auto& N2H = p2n.get_h2n()->get_n2h();
  auto& H2N = p2n.get_h2n()->get_h2n();

  eigen2hdf::save(file, "H2N", H2N);
  eigen2hdf::save(file, "N2H", N2H);

  H5Fclose(file);
}

// Polar-Laguerre coefficients with exponential decay
void test2(int K, const std::function<double(double)>& cfct)
{
  typedef Eigen::VectorXd vec_t;

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, K, K, 2, true);
  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  Polar2Nodal<polar_basis_t> p2n;
  p2n.init(polar_basis, 1.0);

  unsigned int N = polar_basis.n_dofs();
  Eigen::VectorXd cp(N);
  for (unsigned int i = 0; i < N; ++i) {
    cp(i) = cfct(float(i) / N);
  }
  Eigen::MatrixXd cn(K, K);
  p2n.to_nodal(cn, cp);
  Eigen::VectorXd cp2(N);
  p2n.to_polar(cp2, cn);

  auto diff = cp2;
  diff.setZero();
  for (unsigned int i = 0; i < N; ++i) {
    diff(i) = std::abs(cp2(i) - cp(i));
  }
  cout << " sum(err): " << diff.sum() << endl;
}

int main(int argc, char* argv[])
{
  if (argc < 2) {
    cout << "usage: " << argv[0] << " K\n";
    exit(1);
  }
  const int K = atoi(argv[1]);

  cout << "Test: P->N->P "
       << "\n";
  cout << "Exponential decaying Polar-Laguerre coefficients: cp[i] = exp(-20*i/N)"
       << "\n";
  test2(K, [](double i) { return std::exp(-20 * i); });

  cout << "Constant coefficients: cp[i] = 1.0"
       << "\n";
  test2(K, [](double i) { return 1.0; });
  return 0;
}


// #include <iostream>
// #include <string>
// #include <boost/lexical_cast.hpp>
// #include <fstream>
// #include <iomanip>

// #include "quadrature/qmaxwell.hpp"
// #include "quadrature/qmidpoint.hpp"
// #include "quadrature/tensor_product_quadrature.hpp"
// #include "quadrature/quadrature_handler.hpp"

// #include "spectral/basis/spectral_basis_factory_ks.hpp"
// #include "matrix/assembly/velocity_radial_integrator.hpp"
// #include "matrix/assembly/velocity_var_form.hpp"
// #include "matrix/assembly/weight.hpp"

// #include "spectral/laguerren.hpp"
// #include "spectral/laguerrenw.hpp"

// using namespace std;
// using namespace boltzmann;

// // typedef boltzmann::QuadratureHandler<
// //   boltzmann::TensorProductQuadratureC<boltzmann::QMidpoint, QMaxwell> > quad_type;

// typedef double numeric_t;

// template<typename MAP>
// void print(const MAP& m, std::ofstream& fout, string title)
// {
//   fout << "----- " << title << endl;
//   for (auto it = m.begin(); it != m.end(); ++it) {
//     fout << it->first.first << "\t"
//          << it->first.second
//          << "\t"
//          << setprecision(16) << it->second
//          << endl;
//   }
// }

// template<typename CONT>
// void print_basis(const CONT& cont, std::ofstream& fout)
// {
//   for (int i = 0; i < cont.size(); ++i) {
//     fout  << cont[i].get_id() << endl;
//   }
// }

// // ----------------------------------------------------------------------
// int main(int argc, char *argv[])
// {
//   const double beta = 2;

//   if ( argc < 3) {
//     cerr << "info: " << argv[0] << " K q"
//          << endl
//          << "q: No. quad. points\n";
//     return 1;
//   }
//   int K = atoi(argv[1]);
//   int N = atoi(argv[2]);
//   const int digits = 256;

//   QMaxwell qmaxwell(1, N, digits);

//   std::ofstream fout("quadrule_order" + boost::lexical_cast<string>(N) + "_" +
//   boost::lexical_cast<string>(digits) + ".dat");
//   for (unsigned int i = 0; i < qmaxwell.size(); ++i) {
//     fout << setprecision(30) << qmaxwell.pts(i)
//          << "\t"
//          << setprecision(30) << qmaxwell.wts(i)
//          << endl;
//   }
//   fout.close();

//   typedef boltzmann::SpectralBasisFactoryKS basis_factory_t;
//   typedef typename basis_factory_t::basis_type basis_type;

//   L2Weight weight(beta);

//   basis_type basis;
//   basis_factory_t::create(basis, K, K, beta);

//   typedef typename std::tuple_element<1, typename basis_type::elem_t::container_t>::type rad_t;
//   typedef typename basis_type::DimAcc::template get_vec<rad_t> accessor_t;

//   const auto& radial_basis = accessor_t()(basis);

//   LaguerreN<numeric_t> L(K);
//   std::vector<numeric_t> r2(N);
//   std::transform(qmaxwell.pts().begin(), qmaxwell.pts().end(), r2.begin(), [](double r) {return
//   r*r;} );
//   L.compute(r2);

//   ofstream foutn("errors-normalized.dat");
//   for (auto elem = radial_basis.begin(); elem != radial_basis.end(); ++elem) {
//     unsigned int n = elem->get_degree();
//     unsigned int alpha = elem->get_order();
//     unsigned int k = elem->get_id().k;
//     unsigned int j = elem->get_id().j;
//     const numeric_t* values = L.get(n, alpha);
//     double I = 0;
//     for (unsigned int q = 0; q < qmaxwell.size(); ++q) {
//       const double r2j = std::pow(qmaxwell.pts(q), 4*j+ 2* (k%2) );
//       I += r2j*values[q] * values[q] * qmaxwell.wts(q);
//     }
//     foutn << elem->get_id() << "\t" << setw(30) << setprecision(20) << scientific <<
//     std::abs(I-0.5) << endl;
//   }
//   foutn.close();

//   ofstream foutnw("errors-normalized-weighted.dat");
//   LaguerreNW<numeric_t> LW(K);
//   LW.compute(r2);
//   for (auto elem = radial_basis.begin(); elem != radial_basis.end(); ++elem) {
//     unsigned int n = elem->get_degree();
//     unsigned int alpha = elem->get_order();
//     unsigned int k = elem->get_id().k;
//     unsigned int j = elem->get_id().j;
//     const numeric_t* values = LW.get(n, alpha);
//     double I = 0;
//     for (unsigned int q = 0; q < qmaxwell.size(); ++q) {
//       const double r = qmaxwell.pts(q);
//       const double r2j = std::pow(r, 4*j+ 2* (k%2) );

//       I += r2j*(values[q] * ::math::exp(r*r*0.5))* (values[q] * ::math::exp(r*r*0.5)) *
//       qmaxwell.wts(q);
//     }
//     foutnw << elem->get_id() << "\t" << setw(30) << setprecision(20) << scientific <<
//     std::abs(I-0.5) << endl;
//   }
//   foutnw.close();

//   return 0;
// }


#include <iostream>
#include <string>
#include <unordered_map>
#include <vector>
#include <memory>
#include <Eigen/Dense>
#include "../include/sample_network.h"
#include "../datasets/include/mnist.h"

using namespace Eigen;

int main()
{
    using std::cout;
    using std::endl;
    using std::vector;
    using std::string;
    using std::unordered_map;
    using namespace MyDL;

    int num_iters = 1000;
    double learning_rate = 0.1;

    int batch_size = 100;
    int input_size = 28 * 28;
    int hidden_size = 50;
    int output_size = 10;

    MatrixXd train_X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd test_X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd train_y = MatrixXd::Zero(batch_size, 10);
    MatrixXd test_y = MatrixXd::Zero(batch_size, 10);

    MnistEigenDataset mnist(batch_size);

    TwoLayerNet net(input_size, hidden_size, output_size);

    vector<MatrixXd> inputs, loss, val_inputs;
    unordered_map<string, MatrixXd> grads;
    double accuracy;

    mnist.next_train(train_X, train_y, true);
    inputs.push_back(train_X);
    inputs.push_back(train_y);

    grads = net.gradient(inputs);

    // cout << "--- parameter b1 ---" << endl;
    // cout << *(net.params["b1"]) << endl;
    // cout << "--- parameter b1 update ---" << endl;
    // *(net.params["b1"]) -= -learning_rate * grads["b1"];
    // MatrixXd dParam = grads["b1"];
    // *(net.params["b1"]) -= -learning_rate * dParam;
    // cout << *(net.params["b1"]) << endl;
    // cout << dParam << endl;
    // *(net.params["b1"]) -= MatrixXd::Ones(1, hidden_size); // こちらは更新される->gradsによる更新がおかしい？
    // cout << *(net.params["b1"]) << endl;

    cout << "--- parameter b1 ---" << endl;
    cout << net.params["b1"] << endl;
    cout << "--- parameter b1 update ---" << endl;
    net.params["b1"] -= -learning_rate * grads["b1"];

    MatrixXd dParam = grads["b1"];
    net.params["b1"] -= -learning_rate * dParam;
    cout << net.params["b1"] << endl;
    cout << dParam << endl;
    net.params["b1"] -= MatrixXd::Ones(1, hidden_size); // こちらは更新される->gradsによる更新がおかしい？
    cout << net.params["b1"] << endl;

    return 0;
}


// Kolmogorov-Smirnov 1st order asymptotic distribution
// Copyright Evan Miller 2020
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0. (See accompanying file
// LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// The Kolmogorov-Smirnov test in statistics compares two empirical distributions,
// or an empirical distribution against any theoretical distribution. It makes
// use of a specific distribution which doesn't have a formal name, but which
// is often called the Kolmogorv-Smirnov distribution for lack of anything
// better. This file implements the limiting form of this distribution, first
// identified by Andrey Kolmogorov in
//
// Kolmogorov, A. (1933) "Sulla Determinazione Empirica di una Legge di
// Distribuzione." Giornale dell' Istituto Italiano degli Attuari
//
// This limiting form of the CDF is a first-order Taylor expansion that is
// easily implemented by the fourth Jacobi Theta function (setting z=0). The
// PDF is then implemented here as a derivative of the Theta function. Note
// that this derivative is with respect to x, which enters into \tau, and not
// with respect to the z argument, which is always zero, and so the derivative
// identities in DLMF 20.4 do not apply here.
//
// A higher order order expansion is possible, and was first outlined by
//
// Pelz W, Good IJ (1976). "Approximating the Lower Tail-Areas of the
// Kolmogorov-Smirnov One-sample Statistic." Journal of the Royal Statistical
// Society B.
//
// The terms in this expansion get fairly complicated, and as far as I know the
// Pelz-Good expansion is not used in any statistics software. Someone could
// consider updating this implementation to use the Pelz-Good expansion in the
// future, but the math gets considerably hairier with each additional term.
//
// A formula for an exact version of the Kolmogorov-Smirnov test is laid out in
// Equation 2.4.4 of
//
// Durbin J (1973). "Distribution Theory for Tests Based on the Sample
// Distribution Func- tion." In SIAM CBMS-NSF Regional Conference Series in
// Applied Mathematics. SIAM, Philadelphia, PA.
//
// which is available in book form from Amazon and others. This exact version
// involves taking powers of large matrices. To do that right you need to
// compute eigenvalues and eigenvectors, which are beyond the scope of Boost.
// (Some recent work indicates the exact form can also be computed via FFT, see
// https://cran.r-project.org/web/packages/KSgeneral/KSgeneral.pdf).
//
// Even if the CDF of the exact distribution could be computed using Boost
// libraries (which would be cumbersome), the PDF would present another
// difficulty. Therefore I am limiting this implementation to the asymptotic
// form, even though the exact form has trivial values for certain specific
// values of x and n. For more on trivial values see
//
// Ruben H, Gambino J (1982). "The Exact Distribution of Kolmogorov's Statistic
// Dn for n <= 10." Annals of the Institute of Statistical Mathematics.
//
// For a good bibliography and overview of the various algorithms, including
// both exact and asymptotic forms, see
// https://www.jstatsoft.org/article/view/v039i11
//
// As for this implementation: the distribution is parameterized by n (number
// of observations) in the spirit of chi-squared's degrees of freedom. It then
// takes a single argument x. In terms of the Kolmogorov-Smirnov statistical
// test, x represents the distribution of D_n, where D_n is the maximum
// difference between the CDFs being compared, that is,
//
//   D_n = sup|F_n(x) - G(x)|
//
// In the exact distribution, x is confined to the support [0, 1], but in this
// limiting approximation, we allow x to exceed unity (similar to how a normal
// approximation always spills over any boundaries).
//
// As mentioned previously, the CDF is implemented using the \tau
// parameterization of the fourth Jacobi Theta function as
//
// CDF=theta_4(0|2*x*x*n/pi)
//
// The PDF is a hand-coded derivative of that function. Actually, there are two
// (independent) derivatives, as separate code paths are used for "small x"
// (2*x*x*n < pi) and "large x", mirroring the separate code paths in the
// Jacobi Theta implementation to achieve fast convergence. Quantiles are
// computed using a Newton-Raphson iteration from an initial guess that I
// arrived at by trial and error.
//
// The mean and variance are implemented using simple closed-form expressions.
// Skewness and kurtosis use slightly more complicated closed-form expressions
// that involve the zeta function. The mode is calculated at run-time by
// maximizing the PDF. If you have an analytical solution for the mode, feel
// free to plop it in.
//
// The CDF and PDF could almost certainly be re-implemented and sped up using a
// polynomial or rational approximation, since the only meaningful argument is
// x * sqrt(n). But that is left as an exercise for the next maintainer.
//
// In the future, the Pelz-Good approximation could be added. I suggest adding
// a second parameter representing the order, e.g.
//
// kolmogorov_smirnov_dist<>(100) // N=100, order=1
// kolmogorov_smirnov_dist<>(100, 1) // N=100, order=1, i.e. Kolmogorov's formula
// kolmogorov_smirnov_dist<>(100, 4) // N=100, order=4, i.e. Pelz-Good formula
//
// The exact distribution could be added to the API with a special order
// parameter (e.g. 0 or infinity), or a separate distribution type altogether
// (e.g. kolmogorov_smirnov_exact_distribution).
//
#ifndef BOOST_MATH_DISTRIBUTIONS_KOLMOGOROV_SMIRNOV_HPP
#define BOOST_MATH_DISTRIBUTIONS_KOLMOGOROV_SMIRNOV_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/special_functions/jacobi_theta.hpp>
#include <boost/math/tools/tuple.hpp>
#include <boost/math/tools/roots.hpp> // Newton-Raphson
#include <boost/math/tools/minima.hpp> // For the mode

namespace boost { namespace math {

namespace detail {
template <class RealType>
inline RealType kolmogorov_smirnov_quantile_guess(RealType p) {
    // Choose a starting point for the Newton-Raphson iteration
    if (p > 0.9)
        return RealType(1.8) - 5 * (1 - p);
    if (p < 0.3)
        return p + RealType(0.45);
    return p + RealType(0.3);
}

// d/dk (theta2(0, 1/(2*k*k/M_PI))/sqrt(2*k*k*M_PI))
template <class RealType, class Policy>
RealType kolmogorov_smirnov_pdf_small_x(RealType x, RealType n, const Policy&) {
    BOOST_MATH_STD_USING
    RealType value = RealType(0), delta = RealType(0), last_delta = RealType(0);
    RealType eps = policies::get_epsilon<RealType, Policy>();
    int i = 0;
    RealType pi2 = constants::pi_sqr<RealType>();
    RealType x2n = x*x*n;
    if (x2n*x2n == 0.0) {
        return static_cast<RealType>(0);
    }
    while (1) {
        delta = exp(-RealType(i+0.5)*RealType(i+0.5)*pi2/(2*x2n)) * (RealType(i+0.5)*RealType(i+0.5)*pi2 - x2n);

        if (delta == 0.0)
            break;

        if (last_delta != 0.0 && fabs(delta/last_delta) < eps)
            break;

        value += delta + delta;
        last_delta = delta;
        i++;
    }

    return value * sqrt(n) * constants::root_half_pi<RealType>() / (x2n*x2n);
}

// d/dx (theta4(0, 2*x*x*n/M_PI))
template <class RealType, class Policy>
inline RealType kolmogorov_smirnov_pdf_large_x(RealType x, RealType n, const Policy&) {
    BOOST_MATH_STD_USING
    RealType value = RealType(0), delta = RealType(0), last_delta = RealType(0);
    RealType eps = policies::get_epsilon<RealType, Policy>();
    int i = 1;
    while (1) {
        delta = 8*x*i*i*exp(-2*i*i*x*x*n);

        if (delta == 0.0)
            break;

        if (last_delta != 0.0 && fabs(delta / last_delta) < eps)
            break;

        if (i%2 == 0)
            delta = -delta;

        value += delta;
        last_delta = delta;
        i++;
    }

    return value * n;
}

}; // detail

template <class RealType = double, class Policy = policies::policy<> >
    class kolmogorov_smirnov_distribution
{
    public:
        typedef RealType value_type;
        typedef Policy policy_type;

        // Constructor
    kolmogorov_smirnov_distribution( RealType n ) : n_obs_(n)
    {
        RealType result;
        detail::check_df(
                "boost::math::kolmogorov_smirnov_distribution<%1%>::kolmogorov_smirnov_distribution", n_obs_, &result, Policy());
    }

    RealType number_of_observations()const
    {
        return n_obs_;
    }

    private:

    RealType n_obs_; // positive integer
};

typedef kolmogorov_smirnov_distribution<double> kolmogorov_k; // Convenience typedef for double version.

#ifdef __cpp_deduction_guides
template <class RealType>
kolmogorov_smirnov_distribution(RealType)->kolmogorov_smirnov_distribution<typename boost::math::tools::promote_args<RealType>::type>;
#endif

namespace detail {
template <class RealType, class Policy>
struct kolmogorov_smirnov_quantile_functor
{
  kolmogorov_smirnov_quantile_functor(const boost::math::kolmogorov_smirnov_distribution<RealType, Policy> dist, RealType const& p)
    : distribution(dist), prob(p)
  {
  }

  boost::math::tuple<RealType, RealType> operator()(RealType const& x)
  {
    RealType fx = cdf(distribution, x) - prob;  // Difference cdf - value - to minimize.
    RealType dx = pdf(distribution, x); // pdf is 1st derivative.
    // return both function evaluation difference f(x) and 1st derivative f'(x).
    return boost::math::make_tuple(fx, dx);
  }
private:
  const boost::math::kolmogorov_smirnov_distribution<RealType, Policy> distribution;
  RealType prob;
};

template <class RealType, class Policy>
struct kolmogorov_smirnov_complementary_quantile_functor
{
  kolmogorov_smirnov_complementary_quantile_functor(const boost::math::kolmogorov_smirnov_distribution<RealType, Policy> dist, RealType const& p)
    : distribution(dist), prob(p)
  {
  }

  boost::math::tuple<RealType, RealType> operator()(RealType const& x)
  {
    RealType fx = cdf(complement(distribution, x)) - prob;  // Difference cdf - value - to minimize.
    RealType dx = -pdf(distribution, x); // pdf is the negative of the derivative of (1-CDF)
    // return both function evaluation difference f(x) and 1st derivative f'(x).
    return boost::math::make_tuple(fx, dx);
  }
private:
  const boost::math::kolmogorov_smirnov_distribution<RealType, Policy> distribution;
  RealType prob;
};

template <class RealType, class Policy>
struct kolmogorov_smirnov_negative_pdf_functor
{
    RealType operator()(RealType const& x) {
        if (2*x*x < constants::pi<RealType>()) {
            return -kolmogorov_smirnov_pdf_small_x(x, static_cast<RealType>(1), Policy());
        }
        return -kolmogorov_smirnov_pdf_large_x(x, static_cast<RealType>(1), Policy());
    }
};
} // namespace detail

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const kolmogorov_smirnov_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const kolmogorov_smirnov_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   // In the exact distribution, the upper limit would be 1.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
}

template <class RealType, class Policy>
inline RealType pdf(const kolmogorov_smirnov_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_FPU_EXCEPTION_GUARD
   BOOST_MATH_STD_USING  // for ADL of std functions.

   RealType n = dist.number_of_observations();
   RealType error_result;
   static const char* function = "boost::math::pdf(const kolmogorov_smirnov_distribution<%1%>&, %1%)";
   if(false == detail::check_x_not_NaN(function, x, &error_result, Policy()))
      return error_result;

   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

   if (x < 0 || !(boost::math::isfinite)(x))
   {
      return policies::raise_domain_error<RealType>(
         function, "Kolmogorov-Smirnov parameter was %1%, but must be > 0 !", x, Policy());
   }

   if (2*x*x*n < constants::pi<RealType>()) {
       return detail::kolmogorov_smirnov_pdf_small_x(x, n, Policy());
   }

   return detail::kolmogorov_smirnov_pdf_large_x(x, n, Policy());
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const kolmogorov_smirnov_distribution<RealType, Policy>& dist, const RealType& x)
{
    BOOST_MATH_STD_USING // for ADL of std function exp.
   static const char* function = "boost::math::cdf(const kolmogorov_smirnov_distribution<%1%>&, %1%)";
   RealType error_result;
   RealType n = dist.number_of_observations();
   if(false == detail::check_x_not_NaN(function, x, &error_result, Policy()))
      return error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;
   if((x < 0) || !(boost::math::isfinite)(x)) {
      return policies::raise_domain_error<RealType>(
         function, "Random variable parameter was %1%, but must be between > 0 !", x, Policy());
   }

   if (x*x*n == 0)
       return 0;

   return jacobi_theta4tau(RealType(0), 2*x*x*n/constants::pi<RealType>(), Policy());
} // cdf

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<kolmogorov_smirnov_distribution<RealType, Policy>, RealType>& c) {
    BOOST_MATH_STD_USING // for ADL of std function exp.
    RealType x = c.param;
   static const char* function = "boost::math::cdf(const complemented2_type<const kolmogorov_smirnov_distribution<%1%>&, %1%>)";
   RealType error_result;
   kolmogorov_smirnov_distribution<RealType, Policy> const& dist = c.dist;
   RealType n = dist.number_of_observations();

   if(false == detail::check_x_not_NaN(function, x, &error_result, Policy()))
      return error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

   if((x < 0) || !(boost::math::isfinite)(x))
      return policies::raise_domain_error<RealType>(
         function, "Random variable parameter was %1%, but must be between > 0 !", x, Policy());

   if (x*x*n == 0)
       return 1;

   if (2*x*x*n > constants::pi<RealType>())
       return -jacobi_theta4m1tau(RealType(0), 2*x*x*n/constants::pi<RealType>(), Policy());

   return RealType(1) - jacobi_theta4tau(RealType(0), 2*x*x*n/constants::pi<RealType>(), Policy());
} // cdf (complemented)

template <class RealType, class Policy>
inline RealType quantile(const kolmogorov_smirnov_distribution<RealType, Policy>& dist, const RealType& p)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::quantile(const kolmogorov_smirnov_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   RealType n = dist.number_of_observations();
   if(false == detail::check_probability(function, p, &error_result, Policy()))
      return error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

   RealType k = detail::kolmogorov_smirnov_quantile_guess(p) / sqrt(n);
   const int get_digits = policies::digits<RealType, Policy>();// get digits from policy,
   std::uintmax_t m = policies::get_max_root_iterations<Policy>(); // and max iterations.

   return tools::newton_raphson_iterate(detail::kolmogorov_smirnov_quantile_functor<RealType, Policy>(dist, p),
           k, RealType(0), boost::math::tools::max_value<RealType>(), get_digits, m);
} // quantile

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<kolmogorov_smirnov_distribution<RealType, Policy>, RealType>& c) {
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::quantile(const kolmogorov_smirnov_distribution<%1%>&, %1%)";
   kolmogorov_smirnov_distribution<RealType, Policy> const& dist = c.dist;
   RealType n = dist.number_of_observations();
   // Error check:
   RealType error_result;
   RealType p = c.param;

   if(false == detail::check_probability(function, p, &error_result, Policy()))
      return error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

   RealType k = detail::kolmogorov_smirnov_quantile_guess(RealType(1-p)) / sqrt(n);

   const int get_digits = policies::digits<RealType, Policy>();// get digits from policy,
   std::uintmax_t m = policies::get_max_root_iterations<Policy>(); // and max iterations.

   return tools::newton_raphson_iterate(
           detail::kolmogorov_smirnov_complementary_quantile_functor<RealType, Policy>(dist, p),
           k, RealType(0), boost::math::tools::max_value<RealType>(), get_digits, m);
} // quantile (complemented)

template <class RealType, class Policy>
inline RealType mode(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::mode(const kolmogorov_smirnov_distribution<%1%>&)";
   RealType n = dist.number_of_observations();
   RealType error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

    std::pair<RealType, RealType> r = boost::math::tools::brent_find_minima(
            detail::kolmogorov_smirnov_negative_pdf_functor<RealType, Policy>(),
            static_cast<RealType>(0), static_cast<RealType>(1), policies::digits<RealType, Policy>());
    return r.first / sqrt(n);
}

// Mean and variance come directly from
// https://www.jstatsoft.org/article/view/v008i18 Section 3
template <class RealType, class Policy>
inline RealType mean(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::mean(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    return constants::root_half_pi<RealType>() * constants::ln_two<RealType>() / sqrt(n);
}

template <class RealType, class Policy>
inline RealType variance(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
   static const char* function = "boost::math::variance(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    return (constants::pi_sqr_div_six<RealType>()
            - constants::pi<RealType>() * constants::ln_two<RealType>() * constants::ln_two<RealType>()) / (2*n);
}

// Skewness and kurtosis come from integrating the PDF
// The alternating series pops out a Dirichlet eta function which is related to the zeta function
template <class RealType, class Policy>
inline RealType skewness(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::skewness(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    RealType ex3 = RealType(0.5625) * constants::root_half_pi<RealType>() * constants::zeta_three<RealType>() / n / sqrt(n);
    RealType mean = boost::math::mean(dist);
    RealType var = boost::math::variance(dist);
    return (ex3 - 3 * mean * var - mean * mean * mean) / var / sqrt(var);
}

template <class RealType, class Policy>
inline RealType kurtosis(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::kurtosis(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    RealType ex4 = 7 * constants::pi_sqr_div_six<RealType>() * constants::pi_sqr_div_six<RealType>() / 20 / n / n;
    RealType mean = boost::math::mean(dist);
    RealType var = boost::math::variance(dist);
    RealType skew = boost::math::skewness(dist);
    return (ex4 - 4 * mean * skew * var * sqrt(var) - 6 * mean * mean * var - mean * mean * mean * mean) / var / var;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
   static const char* function = "boost::math::kurtosis_excess(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    return kurtosis(dist) - 3;
}
}}
#endif


// solving A * X = B
// A hermitian in packed storage
// driver function hesv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/driver/hpsv.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cin;
using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::lower, ublas::column_major> cherml_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::upper, ublas::column_major> chermu_t;

int main() {

  cherml_t hcal (3, 3);   // hermitian matrix
  chermu_t hcau (3, 3);   // hermitian matrix
  cm_t cx (3, 1);
  cm_t cbl (3, 1), cbu (3, 1);  // RHS

  std::vector<fortran_int_t> ipiv (3);

  hcal (0, 0) = cmplx_t (3, 0);
  hcal (1, 0) = cmplx_t (4, -2);
  hcal (1, 1) = cmplx_t (5, 0);
  hcal (2, 0) = cmplx_t (-7, -5);
  hcal (2, 1) = cmplx_t (0, 3);
  hcal (2, 2) = cmplx_t (2, 0);

  hcau (0, 0) = cmplx_t (3, 0);
  hcau (0, 1) = cmplx_t (4, 2);
  hcau (0, 2) = cmplx_t (-7, 5);
  hcau (1, 1) = cmplx_t (5, 0);
  hcau (1, 2) = cmplx_t (0, -3);
  hcau (2, 2) = cmplx_t (2, 0);

  print_m (hcal, "hcal");
  cout << endl;
  print_m (hcau, "hcau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (hcal, cx);
  cbu = prod (hcau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

//  int ierr = lapack::hpsv (hcal, cbl);
//  no ipiv less version is currently provided, so fall back to using ipiv
  int ierr = lapack::hpsv (hcal, ipiv, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::hpsv (hcau, ipiv, cbu);
  if (ierr == 0) {
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu, "cxu");
  }
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;
}


// Copyright (c) 2009 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#include <cmath>
#include <limits>

#include <Eigen/SVD>
#include <Eigen/Geometry>

#include "libmv/base/vector.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/euclidean_resection.h"
#include "libmv/multiview/projection.h"

namespace libmv {
namespace euclidean_resection {

bool EuclideanResection(const Mat2X &x_camera,
                        const Mat3X &X_world,
                        Mat3 *R, Vec3 *t,
                        ResectionMethod method) {
  switch (method) {
    case RESECTION_ANSAR_DANIILIDIS:
      EuclideanResectionAnsarDaniilidis(x_camera, X_world, R, t);
      break;
    case RESECTION_EPNP:
      return EuclideanResectionEPnP(x_camera, X_world, R, t);
      break;
    default:
      LOG(FATAL) << "Unknown resection method.";
  }
  return false;
}

bool EuclideanResection(const Mat &x_image,
                        const Mat3X &X_world,
                        const Mat3 &K,
                        Mat3 *R, Vec3 *t,
                        ResectionMethod method) {
  CHECK(x_image.rows() == 2 || x_image.rows() == 3)
    << "Invalid size for x_image: "
    << x_image.rows() << "x" << x_image.cols();

  Mat2X x_camera;
  if (x_image.rows() == 2) {
    EuclideanToNormalizedCamera(x_image, K, &x_camera);
  } else if (x_image.rows() == 3) {
    HomogeneousToNormalizedCamera(x_image, K, &x_camera);
  }
  return EuclideanResection(x_camera, X_world, R, t, method);
}

void AbsoluteOrientation(const Mat3X &X,
                         const Mat3X &Xp,
                         Mat3 *R,
                         Vec3 *t) {
  int num_points = X.cols();
  Vec3 C  = X.rowwise().sum() / num_points;   // Centroid of X.
  Vec3 Cp = Xp.rowwise().sum() / num_points;  // Centroid of Xp.

  // Normalize the two point sets.
  Mat3X Xn(3, num_points), Xpn(3, num_points);
  for( int i = 0; i < num_points; ++i ){
    Xn.col(i)  = X.col(i) - C;
    Xpn.col(i) = Xp.col(i) - Cp;
  }

  // Construct the N matrix (pg. 635).
  double Sxx = Xn.row(0).dot(Xpn.row(0));
  double Syy = Xn.row(1).dot(Xpn.row(1));
  double Szz = Xn.row(2).dot(Xpn.row(2));
  double Sxy = Xn.row(0).dot(Xpn.row(1));
  double Syx = Xn.row(1).dot(Xpn.row(0));
  double Sxz = Xn.row(0).dot(Xpn.row(2));
  double Szx = Xn.row(2).dot(Xpn.row(0));
  double Syz = Xn.row(1).dot(Xpn.row(2));
  double Szy = Xn.row(2).dot(Xpn.row(1));

  Mat4 N;
  N << Sxx + Syy + Szz, Syz - Szy,        Szx - Sxz,        Sxy - Syx,
       Syz - Szy,       Sxx - Syy - Szz,  Sxy + Syx,        Szx + Sxz,
       Szx - Sxz,       Sxy + Syx,       -Sxx + Syy - Szz,  Syz + Szy,
       Sxy - Syx,       Szx + Sxz,        Syz + Szy,       -Sxx - Syy + Szz;

  // Find the unit quaternion q that maximizes qNq. It is the eigenvector
  // corresponding to the lagest eigenvalue.
  Vec4 q = N.jacobiSvd(Eigen::ComputeFullU).matrixU().col(0);

  // Retrieve the 3x3 rotation matrix.
  Vec4 qq = q.array() * q.array();
  double q0q1 = q(0) * q(1);
  double q0q2 = q(0) * q(2);
  double q0q3 = q(0) * q(3);
  double q1q2 = q(1) * q(2);
  double q1q3 = q(1) * q(3);
  double q2q3 = q(2) * q(3);

  (*R) << qq(0) + qq(1) - qq(2) - qq(3),
          2 * (q1q2 - q0q3),
          2 * (q1q3 + q0q2),
          2 * (q1q2+ q0q3),
          qq(0) - qq(1) + qq(2) - qq(3),
          2 * (q2q3 - q0q1),
          2 * (q1q3 - q0q2),
          2 * (q2q3 + q0q1),
          qq(0) - qq(1) - qq(2) + qq(3);

  // Fix the handedness of the R matrix.
  if (R->determinant() < 0) {
    R->row(2) = -R->row(2);
  }
  // Compute the final translation.
  *t = Cp - *R * C;
}

// Convert i and j indices of the original variables into their quadratic
// permutation single index. It follows that t_ij = t_ji.
static int IJToPointIndex(int i, int j, int num_points) {
  // Always make sure that j is bigger than i. This handles t_ij = t_ji.
  if (j < i) {
    std::swap(i, j);
  }
  int idx;
  int num_permutation_rows = num_points * (num_points - 1) / 2;

  // All t_ii's are located at the end of the t vector after all t_ij's.
  if (j == i) {
    idx = num_permutation_rows + i;
  } else {
    int offset = (num_points - i - 1) * (num_points - i) / 2;
    idx = (num_permutation_rows - offset + j - i - 1);
  }
  return idx;
};

// Convert i and j indexes of the solution for lambda to their linear indexes.
static int IJToIndex(int i, int j, int num_lambda) {
  if (j < i) {
    std::swap(i, j);
  }
  int A = num_lambda * (num_lambda + 1) / 2;
  int B = num_lambda - i;
  int C = B * (B + 1) / 2;
  int idx = A - C + j - i;
  return idx;
};

static int Sign(double value) {
  return (value < 0) ? -1 : 1;
};

// Organizes a square matrix into a single row constraint on the elements of
// Lambda to create the constraints in equation (5) in "Linear Pose Estimation
// from Points or Lines", by Ansar, A. and Daniilidis, PAMI 2003. vol. 25, no.
// 5.
static Vec MatrixToConstraint(const Mat &A,
                              int num_k_columns,
                              int num_lambda) {
  Vec C(num_k_columns);
  C.setZero();
  int idx = 0;
  for (int i = 0; i < num_lambda; ++i) {
    for( int j = i; j < num_lambda; ++j) {
      C(idx) = A(i, j);
      if (i != j){
        C(idx) += A(j, i);
      }
      ++ idx;
    }
  }
  return C;
}

// Normalizes the columns of vectors.
static void NormalizeColumnVectors(Mat3X *vectors) {
  int num_columns = vectors->cols();
  for (int i = 0; i < num_columns; ++i){
    vectors->col(i).normalize();
  }
}

void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
                                       const Mat3X &X_world,
                                       Mat3 *R,
                                       Vec3 *t) {
  CHECK(x_camera.cols() == X_world.cols());
  CHECK(x_camera.cols() > 3);

  int num_points = x_camera.cols();

  // Copy the normalized camera coords into 3 vectors and normalize them so
  // that they are unit vectors from the camera center.
  Mat3X x_camera_unit(3, num_points);
  x_camera_unit.block(0, 0, 2, num_points) = x_camera;
  x_camera_unit.row(2).setOnes();
  NormalizeColumnVectors(&x_camera_unit);

  int num_m_rows = num_points * (num_points - 1) / 2;
  int num_tt_variables = num_points * (num_points + 1) / 2;
  int num_m_columns = num_tt_variables + 1;
  Mat M(num_m_columns, num_m_columns);
  M.setZero();
  Matu ij_index(num_tt_variables, 2);

  // Create the constraint equations for the t_ij variables (7) and arrange
  // them into the M matrix (8). Also store the initial (i, j) indices.
  int row=0;
  for (int i = 0; i < num_points; ++i) {
    for (int j = i+1; j < num_points; ++j) {
      M(row, row) = -2 * x_camera_unit.col(i).dot(x_camera_unit.col(j));
      M(row, num_m_rows + i) = x_camera_unit.col(i).dot(x_camera_unit.col(i));
      M(row, num_m_rows + j) = x_camera_unit.col(j).dot(x_camera_unit.col(j));
      Vec3 Xdiff = X_world.col(i) - X_world.col(j);
      double center_to_point_distance = Xdiff.norm();
      M(row, num_m_columns - 1) =
          - center_to_point_distance * center_to_point_distance;
      ij_index(row, 0) = i;
      ij_index(row, 1) = j;
      ++row;
    }
    ij_index(i + num_m_rows, 0) = i;
    ij_index(i + num_m_rows, 1) = i;
  }

  int num_lambda = num_points + 1;  // Dimension of the null space of M.
  Mat V = M.jacobiSvd(Eigen::ComputeFullV).matrixV().block(0,
                                                           num_m_rows,
                                                           num_m_columns,
                                                           num_lambda);

  // TODO(vess): The number of constraint equations in K (num_k_rows) must be
  // (num_points + 1) * (num_points + 2)/2. This creates a performance issue
  // for more than 4 points. It is fine for 4 points at the moment with 18
  // instead of 15 equations.
  int num_k_rows = num_m_rows + num_points *
                   (num_points*(num_points-1)/2 - num_points+1);
  int num_k_columns = num_lambda * (num_lambda + 1) / 2;
  Mat K(num_k_rows, num_k_columns);
  K.setZero();

  // Construct the first part of the K matrix corresponding to (t_ii, t_jk) for
  // i != j.
  int counter_k_row = 0;
  for (int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) {
    for (int idx2 = 0; idx2 < num_m_rows; ++idx2) {

      unsigned int i = ij_index(idx1, 0);
      unsigned int j = ij_index(idx2, 0);
      unsigned int k = ij_index(idx2, 1);

      if( i != j && i != k ){
        int idx3 = IJToPointIndex(i, j, num_points);
        int idx4 = IJToPointIndex(i, k, num_points);

        K.row(counter_k_row) =
            MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2)-
                               V.row(idx3).transpose() * V.row(idx4),
                               num_k_columns,
                               num_lambda);
        ++counter_k_row;
      }
    }
  }

  // Construct the second part of the K matrix corresponding to (t_ii,t_jk) for
  // j==k.
  for (int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) {
    for (int idx2 = idx1 + 1; idx2 < num_tt_variables; ++idx2) {
      unsigned int i = ij_index(idx1, 0);
      unsigned int j = ij_index(idx2, 0);
      unsigned int k = ij_index(idx2, 1);

      int idx3 = IJToPointIndex(i, j, num_points);
      int idx4 = IJToPointIndex(i, k, num_points);

      K.row(counter_k_row) =
          MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2)-
                             V.row(idx3).transpose() * V.row(idx4),
                             num_k_columns,
                             num_lambda);
      ++counter_k_row;
    }
  }
  Vec L_sq = K.jacobiSvd(Eigen::ComputeFullV).matrixV().col(num_k_columns - 1);

  // Pivot on the largest element for numerical stability. Afterwards recover
  // the sign of the lambda solution.
  double max_L_sq_value = fabs(L_sq(IJToIndex(0, 0, num_lambda)));
  int max_L_sq_index = 1;
  for (int i = 1; i < num_lambda; ++i) {
    double abs_sq_value = fabs(L_sq(IJToIndex(i, i, num_lambda)));
    if (max_L_sq_value < abs_sq_value) {
      max_L_sq_value = abs_sq_value;
      max_L_sq_index = i;
    }
  }
  // Ensure positiveness of the largest value corresponding to lambda_ii.
  L_sq = L_sq * Sign(L_sq(IJToIndex(max_L_sq_index,
                                    max_L_sq_index,
                                    num_lambda)));

  Vec L(num_lambda);
  L(max_L_sq_index) = sqrt(L_sq(IJToIndex(max_L_sq_index,
                                          max_L_sq_index,
                                          num_lambda)));

  for (int i = 0; i < num_lambda; ++i) {
    if (i != max_L_sq_index) {
      L(i) = L_sq(IJToIndex(max_L_sq_index, i, num_lambda)) / L(max_L_sq_index);
    }
  }

  // Correct the scale using the fact that the last constraint is equal to 1.
  L = L / (V.row(num_m_columns - 1).dot(L));
  Vec X = V * L;

  // Recover the distances from the camera center to the 3D points Q.
  Vec d(num_points);
  d.setZero();
  for (int c_point = num_m_rows; c_point < num_tt_variables; ++c_point) {
    d(c_point - num_m_rows) = sqrt(X(c_point));
  }

  // Create the 3D points in the camera system.
  Mat X_cam(3, num_points);
  for (int c_point = 0; c_point < num_points; ++c_point ) {
    X_cam.col(c_point) = d(c_point) * x_camera_unit.col(c_point);
  }
  // Recover the camera translation and rotation.
  AbsoluteOrientation(X_world, X_cam, R, t);
}

// Selects 4 virtual control points using mean and PCA.
void SelectControlPoints(const Mat3X &X_world,
                         Mat *X_centered,
                         Mat34 *X_control_points) {
  size_t num_points = X_world.cols();

  // The first virtual control point, C0, is the centroid.
  Vec mean, variance;
  MeanAndVarianceAlongRows(X_world, &mean, &variance);
  X_control_points->col(0) = mean;

  // Computes PCA
  X_centered->resize (3, num_points);
  for (size_t c = 0; c < num_points; c++) {
    X_centered->col(c) = X_world.col (c) - mean;
  }
  Mat3 X_centered_sq = (*X_centered) * X_centered->transpose();
  Eigen::JacobiSVD<Mat3> X_centered_sq_svd(X_centered_sq, Eigen::ComputeFullU);
  Vec3 w = X_centered_sq_svd.singularValues();
  Mat3 u = X_centered_sq_svd.matrixU();
  for (size_t c = 0; c < 3; c++) {
    double k = sqrt (w (c) / num_points);
    X_control_points->col (c + 1) = mean + k * u.col (c);
  }
}

// Computes the barycentric coordinates for all real points
void ComputeBarycentricCoordinates(const Mat3X &X_world_centered,
                                   const Mat34 &X_control_points,
                                   Mat4X *alphas) {
  size_t num_points = X_world_centered.cols();
  Mat3 C2 ;
  for (size_t c = 1; c < 4; c++) {
    C2.col(c-1) = X_control_points.col(c) - X_control_points.col(0);
  }

  Mat3 C2inv = C2.inverse();
  Mat3X a = C2inv * X_world_centered;

  alphas->resize(4, num_points);
  alphas->setZero();
  alphas->block(1, 0, 3, num_points) = a;
  for (size_t c = 0; c < num_points; c++) {
    (*alphas)(0, c) = 1.0 - alphas->col(c).sum();
  }
}

// Estimates the coordinates of all real points in the camera coordinate frame
void ComputePointsCoordinatesInCameraFrame(
    const Mat4X &alphas,
    const Vec4 &betas,
    const Eigen::Matrix<double, 12, 12> &U,
    Mat3X *X_camera) {
  size_t num_points = alphas.cols();

  // Estimates the control points in the camera reference frame.
  Mat34 C2b; C2b.setZero();
  for (size_t cu = 0; cu < 4; cu++) {
    for (size_t c = 0; c < 4; c++) {
      C2b.col(c) += betas(cu) * U.block(11 - cu, c * 3, 1, 3).transpose();
    }
  }

  // Estimates the 3D points in the camera reference frame
  X_camera->resize(3, num_points);
  for (size_t c = 0; c < num_points; c++) {
    X_camera->col(c) = C2b * alphas.col(c);
  }

  // Check the sign of the z coordinate of the points (should be positive)
  uint num_z_neg = 0;
  for (size_t i = 0; i < X_camera->cols(); ++i) {
    if ((*X_camera)(2,i) < 0) {
      num_z_neg++;
    }
  }

  // If more than 50% of z are negative, we change the signs
  if (num_z_neg > 0.5 * X_camera->cols()) {
    C2b = -C2b;
    *X_camera = -(*X_camera);
  }
}

bool EuclideanResectionEPnP(const Mat2X &x_camera,
                            const Mat3X &X_world,
                            Mat3 *R, Vec3 *t) {
  CHECK(x_camera.cols() == X_world.cols());
  CHECK(x_camera.cols() > 3);
  size_t num_points = X_world.cols();

  // Select the control points.
  Mat34 X_control_points;
  Mat X_centered;
  SelectControlPoints(X_world, &X_centered, &X_control_points);

  // Compute the barycentric coordinates.
  Mat4X alphas(4, num_points);
  ComputeBarycentricCoordinates(X_centered, X_control_points, &alphas);

  // Estimates the M matrix with the barycentric coordinates
  Mat M(2 * num_points, 12);
  Eigen::Matrix<double, 2, 12> sub_M;
  for (size_t c = 0; c < num_points; c++) {
    double a0 = alphas(0, c);
    double a1 = alphas(1, c);
    double a2 = alphas(2, c);
    double a3 = alphas(3, c);
    double ui = x_camera(0, c);
    double vi = x_camera(1, c);
    M.block(2*c, 0, 2, 12) << a0, 0,
                              a0*(-ui), a1, 0,
                              a1*(-ui), a2, 0,
                              a2*(-ui), a3, 0,
                              a3*(-ui), 0,
                              a0, a0*(-vi), 0,
                              a1, a1*(-vi), 0,
                              a2, a2*(-vi), 0,
                              a3, a3*(-vi);
  }

  // TODO(julien): Avoid the transpose by rewriting the u2.block() calls.
  Eigen::JacobiSVD<Mat> MtMsvd(M.transpose()*M, Eigen::ComputeFullU);
  Eigen::Matrix<double, 12, 12> u2 = MtMsvd.matrixU().transpose();

  // Estimate the L matrix.
  Eigen::Matrix<double, 6, 3> dv1;
  Eigen::Matrix<double, 6, 3> dv2;
  Eigen::Matrix<double, 6, 3> dv3;
  Eigen::Matrix<double, 6, 3> dv4;

  dv1.row(0) = u2.block(11, 0, 1, 3) - u2.block(11, 3, 1, 3);
  dv1.row(1) = u2.block(11, 0, 1, 3) - u2.block(11, 6, 1, 3);
  dv1.row(2) = u2.block(11, 0, 1, 3) - u2.block(11, 9, 1, 3);
  dv1.row(3) = u2.block(11, 3, 1, 3) - u2.block(11, 6, 1, 3);
  dv1.row(4) = u2.block(11, 3, 1, 3) - u2.block(11, 9, 1, 3);
  dv1.row(5) = u2.block(11, 6, 1, 3) - u2.block(11, 9, 1, 3);
  dv2.row(0) = u2.block(10, 0, 1, 3) - u2.block(10, 3, 1, 3);
  dv2.row(1) = u2.block(10, 0, 1, 3) - u2.block(10, 6, 1, 3);
  dv2.row(2) = u2.block(10, 0, 1, 3) - u2.block(10, 9, 1, 3);
  dv2.row(3) = u2.block(10, 3, 1, 3) - u2.block(10, 6, 1, 3);
  dv2.row(4) = u2.block(10, 3, 1, 3) - u2.block(10, 9, 1, 3);
  dv2.row(5) = u2.block(10, 6, 1, 3) - u2.block(10, 9, 1, 3);
  dv3.row(0) = u2.block( 9, 0, 1, 3) - u2.block( 9, 3, 1, 3);
  dv3.row(1) = u2.block( 9, 0, 1, 3) - u2.block( 9, 6, 1, 3);
  dv3.row(2) = u2.block( 9, 0, 1, 3) - u2.block( 9, 9, 1, 3);
  dv3.row(3) = u2.block( 9, 3, 1, 3) - u2.block( 9, 6, 1, 3);
  dv3.row(4) = u2.block( 9, 3, 1, 3) - u2.block( 9, 9, 1, 3);
  dv3.row(5) = u2.block( 9, 6, 1, 3) - u2.block( 9, 9, 1, 3);
  dv4.row(0) = u2.block( 8, 0, 1, 3) - u2.block( 8, 3, 1, 3);
  dv4.row(1) = u2.block( 8, 0, 1, 3) - u2.block( 8, 6, 1, 3);
  dv4.row(2) = u2.block( 8, 0, 1, 3) - u2.block( 8, 9, 1, 3);
  dv4.row(3) = u2.block( 8, 3, 1, 3) - u2.block( 8, 6, 1, 3);
  dv4.row(4) = u2.block( 8, 3, 1, 3) - u2.block( 8, 9, 1, 3);
  dv4.row(5) = u2.block( 8, 6, 1, 3) - u2.block( 8, 9, 1, 3);

  Eigen::Matrix<double, 6, 10> L;
  for (size_t r = 0; r < 6; r++) {
    L.row(r) << dv1.row(r).dot(dv1.row(r)),
          2.0 * dv1.row(r).dot(dv2.row(r)),
                dv2.row(r).dot(dv2.row(r)),
          2.0 * dv1.row(r).dot(dv3.row(r)),
          2.0 * dv2.row(r).dot(dv3.row(r)),
                dv3.row(r).dot(dv3.row(r)),
          2.0 * dv1.row(r).dot(dv4.row(r)),
          2.0 * dv2.row(r).dot(dv4.row(r)),
          2.0 * dv3.row(r).dot(dv4.row(r)),
                dv4.row(r).dot(dv4.row(r));
  }
  Vec6 rho;
  rho << (X_control_points.col(0) - X_control_points.col(1)).squaredNorm(),
         (X_control_points.col(0) - X_control_points.col(2)).squaredNorm(),
         (X_control_points.col(0) - X_control_points.col(3)).squaredNorm(),
         (X_control_points.col(1) - X_control_points.col(2)).squaredNorm(),
         (X_control_points.col(1) - X_control_points.col(3)).squaredNorm(),
         (X_control_points.col(2) - X_control_points.col(3)).squaredNorm();

  // There are three possible solutions based on the three approximations of L
  // (betas). Below, each one is solved for then the best one is chosen.
  Mat3X X_camera;
  Mat3 K; K.setIdentity();
  vector<Mat3> Rs(3);
  vector<Vec3> ts(3);
  Vec rmse(3);

  // TODO(julien): Document where the "1e-3" magical constant comes from below.

  // Find the first possible solution for R, t corresponding to:
  // Betas          = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
  // Betas_approx_1 = [b00 b01     b02         b03]
  Vec4 betas = Vec4::Zero();
  Eigen::Matrix<double, 6, 4> l_6x4;
  for (size_t r = 0; r < 6; r++) {
    l_6x4.row(r) << L(r, 0), L(r, 1), L(r, 3), L(r, 6);
  }
  Eigen::JacobiSVD<Mat> svd_of_l4(l_6x4,
                                  Eigen::ComputeFullU | Eigen::ComputeFullV);
  Vec4 b4 = svd_of_l4.solve(rho);
  if ((l_6x4 * b4).isApprox(rho, 1e-3)) {
    if (b4(0) < 0) {
      b4 = -b4;
    }
    b4(0) =  std::sqrt(b4(0));
    betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0);
    ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
    AbsoluteOrientation(X_world, X_camera, &Rs[0], &ts[0]);
    rmse(0) = RootMeanSquareError(x_camera, X_world, K, Rs[0], ts[0]);
  } else {
    LOG(ERROR) << "First approximation of beta not good enough.";
    ts[0].setZero();
    rmse(0) = std::numeric_limits<double>::max();
  }

  // Find the second possible solution for R, t corresponding to:
  // Betas          = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
  // Betas_approx_2 = [b00 b01 b11]
  betas.setZero();
  Eigen::Matrix<double, 6, 3> l_6x3;
  l_6x3 = L.block(0, 0, 6, 3);
  Eigen::JacobiSVD<Mat> svdOfL3(l_6x3,
                                Eigen::ComputeFullU | Eigen::ComputeFullV);
  Vec3 b3 = svdOfL3.solve(rho);
  VLOG(2) << " rho = " << rho;
  VLOG(2) << " l_6x3 * b3 = " << l_6x3 * b3;
  if ((l_6x3 * b3).isApprox(rho, 1e-3)) {
    if (b3(0) < 0) {
      betas(0) = std::sqrt(-b3(0));
      betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0;
    } else {
      betas(0) = std::sqrt(b3(0));
      betas(1) = (b3(2) > 0) ? std::sqrt(b3(2)) : 0;
    }
    if (b3(1) < 0) {
      betas(0) = -betas(0);
    }
    betas(2) = 0;
    betas(3) = 0;
    ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
    AbsoluteOrientation(X_world, X_camera, &Rs[1], &ts[1]);
    rmse(1) = RootMeanSquareError(x_camera, X_world, K, Rs[1], ts[1]);
  } else {
    LOG(ERROR) << "Second approximation of beta not good enough.";
    ts[1].setZero();
    rmse(1) = std::numeric_limits<double>::max();
  }

  // Find the third possible solution for R, t corresponding to:
  // Betas          = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
  // Betas_approx_3 = [b00 b01 b11 b02 b12]
  betas.setZero();
  Eigen::Matrix<double, 6, 5> l_6x5;
  l_6x5 = L.block(0, 0, 6, 5);
  Eigen::JacobiSVD<Mat> svdOfL5(l_6x5,
                                Eigen::ComputeFullU | Eigen::ComputeFullV);
  Vec5 b5 = svdOfL5.solve(rho);
  if ((l_6x5 * b5).isApprox(rho, 1e-3)) {
    if (b5(0) < 0) {
      betas(0) = std::sqrt(-b5(0));
      if (b5(2) < 0) {
        betas(1) = std::sqrt(-b5(2));
      } else {
        b5(2) = 0;
      }
    } else {
      betas(0) = std::sqrt(b5(0));
      if (b5(2) > 0) {
        betas(1) = std::sqrt(b5(2));
      } else {
        b5(2) = 0;
      }
    }
    if (b5(1) < 0) {
      betas(0) = -betas(0);
    }
    betas(2) = b5(3) / betas(0);
    betas(3) = 0;
    ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
    AbsoluteOrientation(X_world, X_camera, &Rs[2], &ts[2]);
    rmse(2) = RootMeanSquareError(x_camera, X_world, K, Rs[2], ts[2]);
  } else {
    LOG(ERROR) << "Third approximation of beta not good enough.";
    ts[2].setZero();
    rmse(2) = std::numeric_limits<double>::max();
  }

  // Finally, with all three solutions, select the (R, t) with the best RMSE.
  VLOG(2) << "RMSE for solution 0: " << rmse(0);
  VLOG(2) << "RMSE for solution 1: " << rmse(0);
  VLOG(2) << "RMSE for solution 2: " << rmse(0);
  size_t n = 0;
  if (rmse(1) < rmse(0)) {
    n = 1;
  }
  if (rmse(2) < rmse(n)) {
    n = 2;
  }
  if (rmse(n) == std::numeric_limits<double>::max()) {
    LOG(ERROR) << "All three possibilities failed. Reporting failure.";
    return false;
  }

  VLOG(1) << "RMSE for best solution #" << n << ": " << rmse(n);
  *R = Rs[n];
  *t = ts[n];

  // TODO(julien): Improve the solutions with non-linear refinement.
  return true;
}

} // namespace resection
} // namespace libmv


// Software License Agreement (BSD-3-Clause)
//
// Copyright 2018 The University of North Carolina at Chapel Hill
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder 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 HOLDER 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.

//! @author Jeff Ichnowski

#pragma once
#ifndef NIGH_METRIC_IMPL_SO2_HPP
#define NIGH_METRIC_IMPL_SO2_HPP

#include <type_traits>
#include <cmath>
#include <Eigen/Dense>
#include "constants.hpp"

namespace unc::robotics::nigh::impl::so2 {
    template <typename S>
    std::enable_if_t<std::is_floating_point_v<S>, S>
    angularDistance(S a, S b) {
        S d = std::fmod(std::abs(a - b), impl::PI<S> * 2);
        return std::min(d, impl::PI<S>*2 - d);
    }

    template <int p, typename A, typename B>
    std::common_type_t<typename A::Scalar, typename B::Scalar>
    angularDistance(const Eigen::ArrayBase<A>& a, const Eigen::ArrayBase<B>& b) {
        using Scalar = std::common_type_t<typename A::Scalar, typename B::Scalar>;
        Eigen::Array<Scalar, A::RowsAtCompileTime, A::ColsAtCompileTime> r = (a - b).cwiseAbs();
        r -= (r / (2*impl::PI<Scalar>)).floor() * (2*impl::PI<Scalar>);
        r = (r < -impl::PI<Scalar>).select(r + 2*impl::PI<Scalar>, r);
        r = (r >  impl::PI<Scalar>).select(r - 2*impl::PI<Scalar>, r);
        return r.matrix().template lpNorm<p>();
    }

    template <int p, typename A, typename B>
    auto angularDistance(const Eigen::MatrixBase<A>& a, const Eigen::MatrixBase<B>& b) {
        return angularDistance<p>(a.array(), b.array());
    }

        // Returns the argument bound to the range -PI..PI
    template <typename Scalar>
    std::enable_if_t<std::is_floating_point_v<Scalar>, Scalar>
    bound(Scalar a) {
        if ((a = std::fmod(a, PI<Scalar>*2)) <= -PI<Scalar>)
            return a + PI<Scalar>*2;
        return (a > PI<Scalar>) ? a - PI<Scalar>*2 : a;
    }

    // computes the counter-clockwise distance from a to b
    template <typename Scalar>
    Scalar ccwDist(Scalar a, Scalar b) {
        Scalar d = std::fmod(b - a, PI<Scalar>*2);
        return d < 0 ? d + PI<Scalar>*2 : d;
    }

    // This method is exactly the same ad ccwDist except that two
    // overlapping values are considered 2*PI instead of 0.
    template <typename Scalar>
    Scalar ccwRange(Scalar a, Scalar b) {
        Scalar d = std::fmod(b - a, PI<Scalar>*2);
        return d <= 0 ? d + PI<Scalar>*2 : d;
    }

    template <typename Scalar>
    Scalar antipode(Scalar a) {
        return a <= 0 ? (a + PI<Scalar>) : (a - PI<Scalar>);
    }

    // Finds a split on a set of SO(2) values normalized to the range
    // [-pi,pi].  The split divides the values into two evenly sized
    // (_N/2) sets, and maximizes the distance of the bounds to the
    // split.
    template <typename Iter>
    auto split(Iter first, Iter last) {
        using Scalar = typename std::iterator_traits<Iter>::value_type;
        std::size_t n = std::distance(first, last);
        assert(n > 1); // can only split more than 1 element

        // The loop finds the `i` at which half elements are to the
        // left of the line bisecting line it define.  To find it we
        // test for `j = i + N/2`, that the ccw distance to `[j] < pi`
        // and `[j+1] > pi`.  The "best" split is the one that
        // maximizes the distance between the points closest to the
        // split, thus `[i]`, `[i+1]`, `[j]`, and `[j+1]`.

        // distances: pi-D(i,j), D(i,j+1)-pi, D(i,i+1)
        //
        //        i
        //        |@@@
        //        |@@@@
        //   -----X@@@@
        //   ####/|\@@@
        //    ##/ | \@
        //     j    j+1

        //          /
        // j+1 ----X
        //     ###/|\     |
        //     ##/ |@\    |
        //     #/  |@@\   |
        //     i       j

        //        j+1 j
        //      ###| /@
        //     ####|/@@@
        //     ####X@@@@
        //     ###/ \@@@
        //      #/   \@
        //      i

        std::sort(first, last);
        Scalar dBest = -1, split = 0;
        Iter i1 = first;
        Iter j1 = first + n/2;
        do {
            Iter i0 = i1;
            Iter j0 = j1;
            if (++i1 == last) i1 = first;
            if (++j1 == last) j1 = first;
            Scalar d0 = PI<Scalar> - so2::ccwDist(*i0, *j0); // vals[i], vals[j%n]);
            Scalar d1 = PI<Scalar> - so2::ccwDist(*j1, *i0); // vals[(j+1)%n], vals[i]);

            if (d0 >= 0 && d1 >= 0) {
                Scalar di = so2::ccwDist(*i0, *i1); // vals[i], vals[(i+1)%n]);
                // Scalar split = vals[i] + std::min(di, d1) * 0.5;

                Scalar range = 2*PI<Scalar> - (d0+d1);

                if (range < PI<Scalar>) {
                    // The range of values is less than half a circle.
                    // split halfway between i0 and i1
                    if (range > dBest) {
                        dBest = range;
                        split = *i0 + di * 0.5;
                    }
                } else {
                    // The range of values is more than half a circle
                    // split considering both sides of split plane.
                    // we cannot split halfway between i0 and i1 since
                    // it could result in moving j1 to the other side
                    // of the split.
                    //
                    // An easy split to do would be i0 + min(di,d1)/2,
                    // since that would be halfway from the split to
                    // the next bound.  This is also guaranteed to be
                    // in the bounds.
                    //
                    // A possibly better split is to try to maximize
                    // the sum of square distances from the split.
                    //
                    // define x as the split offset from i0.  The
                    // distances from the split plane are thus:
                    //
                    //   i0 to split = x
                    //   split to i1 = di - x
                    //   j0 to split = d0 + x
                    //   split to j1 = d1 - x
                    //
                    // summing the square of the above quantities,
                    // then differentiating and solving for 0, we get:
                    //
                    // x = (di + d1 - d0) / 4;

                    // Compute the range as the sum of distances from
                    // the split.  We add PI so that we prefer
                    // splitting these axes over axes that are already
                    // split.
                    Scalar dSum = PI<Scalar> + (di+d0+d1)/2; // std::min({di, d0, d1});
                    if (dSum > dBest) {
                        dBest = dSum;
                        split = *i0 + std::min(di, d1) * Scalar(0.5);
                    }
                }
            }
        } while (i1 != first);

        //if (sBest > PI<Scalar>) sBest -= 2*PI<Scalar>;
        split = so2::bound(split);

        assert(0 <= dBest && dBest <= 2*PI<Scalar>);
        assert(-PI<Scalar> <= split && split <= PI<Scalar>);

        return std::make_pair(dBest, split);
    }

    template <typename Container>
    auto split(Container& container) {
        return split(container.begin(), container.end());
    }
}

#endif


/*
 * Author: Patrick Schmidt
 */

#include "CohomologyBasis.hh"

#include <HomologyInference/Genus.hh>
#include <HomologyInference/Utils/BarycentricPoint.hh>
#include <HomologyInference/Utils/CotanWeights.hh>
#include <HomologyInference/Utils/Timer.hh>
#include <Eigen/SparseCholesky>
#include <queue>

namespace HomologyInference
{

std::vector<ExternalProperty<HEH, double>>
cohomology_basis(
        const PrimalLoops& _loops,
        const TriMesh& _mesh)
{
    Timer timer(__FUNCTION__);

    // Set up linear system
    const int n_fields = _loops.size();
    const int n_constr = _mesh.n_faces() + _mesh.n_vertices() + n_fields;
    const int n_edges = _mesh.n_edges();
    std::vector<Triplet> triplets;
    MatXd rhs = MatXd::Zero(n_constr, n_fields);

    // 1 if canonical direction, -1 otherwise
    auto he_sign = [&] (const HEH& heh)
    {
        const auto& eh = _mesh.edge_handle(heh);
        const auto& heh0 = _mesh.halfedge_handle(eh, 0);
        return (heh == heh0) ? 1.0 : -1.0;
    };

    // Closedness condition:
    // Integral along every contractible loop is zero.
    for (auto f : _mesh.faces())
    {
        for (auto h : f.halfedges())
            triplets.push_back(Triplet(f.idx(), h.edge().idx(), he_sign(h)));
    }

    // Harmonicity condition:
    // Laplace of field is zero at every vertex
    int offset = _mesh.n_faces();
    for (auto v : _mesh.vertices())
    {
        if (v.is_boundary())
        {
            for (auto h : v.outgoing_halfedges())
                if (h.edge().is_boundary())
                    triplets.push_back(Triplet(offset + v.idx(), h.edge().idx(), he_sign(h) * 1.0 / _mesh.calc_edge_length(h.edge())));
        }
        else
        {
            for (auto h : v.outgoing_halfedges())
                triplets.push_back(Triplet(offset + v.idx(), h.edge().idx(), he_sign(h) * cotan_weight(_mesh, h)));
        }
    }

    // Duality condition:
    // Integral of field i along loop i is 1. All others are 0.
    offset += _mesh.n_vertices();
    for (int i = 0; i < _loops.size(); ++i)
    {
        for (auto heh : _loops[i].hehs)
        {
            const auto& eh = _mesh.edge_handle(heh);
            triplets.push_back(Triplet(offset + i, eh.idx(), he_sign(heh)));
        }
    }
    rhs.block(offset, 0, n_fields, n_fields) = MatXd::Identity(n_fields, n_fields);

    SparseMatrix A(n_constr, n_edges);
    A.setFromTriplets(triplets.begin(), triplets.end());

    // Solve A^T*A * x = A^T*b via sparse Cholesky
    const SparseMatrix AtA = A.transpose() * A;
    const MatXd Atrhs = A.transpose() * rhs;
    Eigen::SimplicialLDLT<SparseMatrix> solver;
    solver.compute(AtA);
    ISM_ASSERT(solver.info() == Eigen::Success);
    MatXd x = solver.solve(Atrhs);
    ISM_ASSERT(solver.info() == Eigen::Success);
    ISM_ASSERT_EQ(x.rows(), n_edges);
    ISM_ASSERT_EQ(x.cols(), n_fields);

    double residual = (A * x - rhs).norm();
    ISM_DEBUG_VAR(residual);

    // Convert to halfedge properties
    auto gradients = std::vector<ExternalProperty<HEH, double>>(n_fields, ExternalProperty<HEH, double>(_mesh));
    for (int i = 0; i < n_fields; ++i)
        for (auto h : _mesh.halfedges())
            gradients[i][h] = he_sign(h) * x(h.edge().idx(), i);

    return gradients;
}

ExternalProperty<VH, double>
integrate_field_real(
        const ExternalProperty<HEH, double>& _gradient,
        const TriMesh& _mesh,
        const VH _seed_vh,
        const double _seed_value)
{
    // Flood fill starting from first vertex
    ExternalProperty<VH, double> u(_mesh, NAN_DOUBLE); // integrated function
    ExternalProperty<VH, bool> visited(_mesh, false);

    std::queue<SHEH> queue;
    for (auto h : _mesh.voh_range(_seed_vh))
        queue.push(h);
    u[_seed_vh] = _seed_value;
    visited[_seed_vh] = true;

    while (!queue.empty())
    {
        const auto h = queue.front();
        queue.pop();

        if (visited[h.to()])
            continue;

        u[h.to()] = u[h.from()] + _gradient[h];
        visited[h.to()] = true;

        for (auto h_enq : h.to().outgoing_halfedges())
        {
            if (!visited[h_enq.to()])
                queue.push(h_enq);
        }
    }

    return u;
}

std::vector<ExternalProperty<VH, double>>
integrated_fields_real(
        const std::vector<ExternalProperty<HEH, double>>& _gradients,
        const TriMesh& _mesh,
        const VH _seed_vh,
        const double _seed_value)
{
    std::vector<ExternalProperty<VH, double>> result;
    for (const auto& g : _gradients)
        result.push_back(integrate_field_real(g, _mesh, _seed_vh, _seed_value));
    return result;
}

ExternalProperty<VH, Complex>
integrate_field_complex(
        const ExternalProperty<HEH, double>& _gradient,
        const TriMesh& _mesh,
        const VH _seed_vh,
        const Complex _seed_value)
{
    ExternalProperty<VH, double> u_real = integrate_field_real(_gradient, _mesh, _seed_vh, 0.0);
    ExternalProperty<VH, Complex> u_complex(_mesh, NAN_DOUBLE);
    for (const auto& v : _mesh.vertices())
    {
        const double angle = u_real[v] * 2 * M_PI;
        const Complex rot = std::polar(1.0, angle);
        u_complex[v] = rot * _seed_value;
    }
    return u_complex;
}

std::vector<ExternalProperty<VH, Complex>>
integrated_fields_complex(
        const std::vector<ExternalProperty<HEH, double>>& _gradients,
        const TriMesh& _mesh,
        const VH _seed_vh,
        const Complex _seed_value)
{
    Timer timer(__FUNCTION__);

    std::vector<ExternalProperty<VH, Complex>> result;
    for (const auto& g : _gradients)
        result.push_back(integrate_field_complex(g, _mesh, _seed_vh, _seed_value));
    return result;
}

ExternalProperty<HEH, double>
differentiate_field(
        const ExternalProperty<VH, Complex>& _u,
        const TriMesh& _mesh)
{
    ExternalProperty<HEH, double> gradient(_mesh);
    for (const auto& heh : _mesh.halfedges())
    {
        const VH vh0 = _mesh.from_vertex_handle(heh);
        const VH vh1 = _mesh.to_vertex_handle(heh);
        const Complex u0 = _u[vh0];
        const Complex u1 = _u[vh1];
        const Complex rot = u1 / u0;
        const double angle_diff = std::arg(rot) / (2 * M_PI);
        gradient[heh] = angle_diff;
    }
    return gradient;
}

std::vector<ExternalProperty<HEH, double>>
differentiate_fields(
        const std::vector<ExternalProperty<VH, Complex>>& _us,
        const TriMesh& _mesh)
{
    Timer timer(__FUNCTION__);

    std::vector<ExternalProperty<HEH, double>> result;
    for (const auto& u : _us)
        result.push_back(differentiate_field(u, _mesh));
    return result;
}

double
integrate_loop(
        const ExternalProperty<HEH, double>& _gradient,
        const PrimalLoop& _loop)
{
    double res = 0.0;
    for (auto h : _loop.hehs)
        res += _gradient[h];
    return res;
}

MatXd
integrate_loops(
        const std::vector<ExternalProperty<HEH, double>>& _fields,
        const PrimalLoops& _loops)
{
    ISM_ASSERT(_loops.size() == _fields.size());
    const int n = _loops.size();
    MatXd M_integrated = MatXd::Zero(n, n);
    for (int row = 0; row < n; ++row)
        for (int col = 0; col < n; ++col)
            M_integrated(row, col) = integrate_loop(_fields[col], _loops[row]);
    return M_integrated;
}

ExternalProperty<HEH, double>
combine_fields(
        const TriMesh& _mesh,
        const VecXi& _coeffs,
        const std::vector<ExternalProperty<HEH, double>>& _fields)
{
    const VecXd coeffs_d = _coeffs.cast<double>();
    return combine_fields(_mesh, coeffs_d, _fields);
}

ExternalProperty<HEH, double>
combine_fields(
        const TriMesh& _mesh,
        const VecXd& _coeffs,
        const std::vector<ExternalProperty<HEH, double>>& _fields)
{
    ISM_ASSERT_EQ(_coeffs.size(), _fields.size());
    ExternalProperty<HEH, double> field(_mesh, 0.0);
    for (int i = 0; i < _coeffs.size(); ++i)
    {
        const auto& input_field = _fields[i];
        ISM_ASSERT(input_field.size_okay(_mesh));
        for (const auto& heh : _mesh.halfedges())
            field[heh] += _coeffs[i] * input_field[heh];
    }
    return field;
}

std::vector<ExternalProperty<HEH, double>>
transform_fields(
        const TriMesh& _mesh,
        const MatXi& _M,
        const std::vector<ExternalProperty<HEH, double>>& _fields)
{
    ISM_ASSERT_EQ(_M.cols(), _fields.size());
    ISM_ASSERT(!_fields.empty());
    std::vector<ExternalProperty<HEH, double>> result;
    for (int row = 0; row < _M.rows(); ++row)
    {
        const VecXi M_row = _M.row(row);
        ExternalProperty<HEH, double> field = combine_fields(_mesh, M_row, _fields);
        result.push_back(field);
    }
    return result;
}

}


#include <fstream>
#include <armadillo>
#include <thread>

using namespace std;
using namespace arma;

// Armadillo documentation is available at:
// http://arma.sourceforge.net/docs.html

bool intersect(const vec& origin, const vec& direction, const vec& center, double radius, vec& normal, vec& hit) {
	vec oc = origin - center;
	double a = dot(direction, direction);
	double b = 2.0*dot(direction, oc);
	double c = dot(oc, oc) - radius*radius;

	double delta = b*b - 4.0*a*c;
	if (delta < 0.0) {
		return false;
	}
	double t1 = (-b + sqrt(delta)) / (2.0*a);
	double t2 = (-b - sqrt(delta)) / (2.0*a);
	double t = (t1 < t2) ? t1 : t2;

	hit = origin + direction*t;
	normal = hit - center;
	return true;
}

int main() {
	vec centro_esfera = { 0.0, 0.0, 10.0 };
	vec origin = { 0.0, 0.0, 0.0 };
	double intensidade = 0.9;
	vec posicao_luz = { 0.0, 4.0, 4.0 };
	vec radiancia_luz = { intensidade, intensidade, intensidade };
	mat k;
	k << 1000.0 << 0.0 << 400.0 << endr
		<< 0.0 << -1000.0 << 300.0 << endr
		<< 0.0 << 0.0 << 1.0;
	mat invk = k.i();

	ofstream output;
	output.open("imagem.pgm");
	output << "P3" << endl;
	output << "800 600" << endl;
	output << "255" << endl;

	for (int linha = 0; linha < 600; linha++) {
		for (int coluna = 0; coluna < 800; coluna++) {
			vec r = { double(coluna), double(linha), 1.0 };
			vec normal, hit;

			r = invk * r;
			r *= 5.0 / r(2);
			if (intersect(origin, r, centro_esfera, 2.0, normal, hit)) {
				normal /= norm(normal);
				vec l = posicao_luz - hit;
				l /= norm(l);

				vec cor = { 255.0, 0.0, 0.0 };
				cor = (cor % radiancia_luz)*std::max(0.0, dot(normal, l));
				output << std::min(255, int(cor(0))) << " "
					   << std::min(255, int(cor(1))) << " "
					   << std::min(255, int(cor(2))) << " ";
			} else {
				output << "255 255 255 ";
			}
		}
	}
	output.close();
	return 0;
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <Eigen/Core>
#include <smooth/bundle.hpp>
#include <smooth/feedback/compat/ipopt.hpp>
#include <smooth/feedback/ocp.hpp>
#include <smooth/se2.hpp>

#include <chrono>
#include <iostream>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

template<typename T>
using X = smooth::Bundle<smooth::SE2<T>, Eigen::Vector3<T>>;

template<typename T>
using U = Eigen::Vector2<T>;

template<typename T>
using Vec = Eigen::VectorX<T>;

/// @brief Objective function
const auto obj = []<typename T>(T tf, const X<T> &, const X<T> &, const Vec<T> & q) -> T {
  return tf + q.x();
};

/// @brief Dynamics
const auto f = []<typename T>(T, const X<T> & x, const U<T> & u) -> smooth::Tangent<X<T>> {
  smooth::Tangent<X<T>> ret;
  ret.segment(0, 3) = x.template part<1>();
  ret(3)            = u.x();
  ret(4)            = T(0);
  ret(5)            = u.y();
  return ret;
};

/// @brief Integrals
const auto g = []<typename T>(T, const X<T> &, const U<T> & u) -> Vec<T> {
  return Vec<T>{{u.squaredNorm()}};
};

/// @brief Running constraints
const auto cr = []<typename T>(T, const X<T> &, const U<T> & u) -> Vec<T> { return u; };

/// @brief End constraints
const auto ce = []<typename T>(T tf, const X<T> & x0, const X<T> & xf, const Vec<T> &) -> Vec<T> {
  const smooth::SE2<T> target(smooth::SO2<T>(-0.5), Eigen::Vector2<T>{2, 0.5});
  Vec<T> ret(10);
  ret << tf, x0.template part<0>().log(), x0.template part<1>(), xf.template part<0>() - target;
  return ret;
};

/// @brief Range to std::vector
const auto r2v = []<std::ranges::range R>(const R & r) {
  return std::vector(std::ranges::begin(r), std::ranges::end(r));
};

int main()
{
  // define optimal control problem
  smooth::feedback::
    OCP<X<double>, U<double>, decltype(obj), decltype(f), decltype(g), decltype(cr), decltype(ce)>
      ocp{
        .nx    = smooth::Dof<X<double>>,
        .nu    = smooth::Dof<U<double>>,
        .nq    = 1,
        .ncr   = 2,
        .nce   = 10,
        .theta = obj,
        .f     = f,
        .g     = g,
        .cr    = cr,
        .crl   = Vec<double>{{-1, -1}},
        .cru   = Vec<double>{{1, 1}},
        .ce    = ce,
        .cel   = Vec<double>{{3, 0, 0, 0, 0, 0, 0, 0, 0, 0}},
        .ceu   = Vec<double>{{15, 0, 0, 0, 0, 0, 0, 0, 0, 0}},
      };

  const auto xl = []<typename T>(T) -> X<T> { return X<T>::Identity(); };
  const auto ul = []<typename T>(T) -> U<T> { return Eigen::Vector2<T>::Constant(0.01); };

  assert(smooth::feedback::check_ocp(ocp));

  const auto flatocp = smooth::feedback::flatten_ocp(ocp, xl, ul);

  assert(smooth::feedback::check_ocp(flatocp));

  // target optimality
  const double target_err = 1e-6;

  // define mesh
  smooth::feedback::Mesh<5, 10> mesh;

  // declare solution variable
  std::vector<smooth::feedback::OCPSolution<X<double>, U<double>>> sols;
  std::optional<smooth::feedback::NLPSolution> nlpsol;

  const auto t0 = std::chrono::high_resolution_clock::now();

  for (auto iter = 0u; iter < 10; ++iter) {
    std::cout << "---------- ITERATION " << iter << " ----------" << std::endl;
    std::cout << "mesh: " << mesh.N_ivals() << " intervals, " << mesh.N_colloc()
              << " collocation pts" << std::endl;

    // transcribe optimal control problem to nonlinear programming problem
    const auto nlp = smooth::feedback::ocp_to_nlp(flatocp, mesh);

    // solve nonlinear programming problem
    std::cout << "solving..." << std::endl;
    nlpsol = smooth::feedback::solve_nlp_ipopt(
      nlp,
      nlpsol,
      {
        {"print_level", 5},
      },
      {
        {"linear_solver", "mumps"}, {"hessian_approximation", "limited-memory"},
        // {"derivative_test", "first-order"},
        // {"print_timing_statistics", "yes"},
      },
      {
        {"tol", 1e-6},
      });

    // convert solution of nlp insto solution of ocp
    auto flatsol = smooth::feedback::nlpsol_to_ocpsol(flatocp, mesh, nlpsol.value());

    // store unflattened solution
    sols.push_back(smooth::feedback::unflatten_ocpsol<X<double>, U<double>>(flatsol, xl, ul));

    // calculate errors
    auto errs = smooth::feedback::mesh_dyn_error(
      flatocp.nx, flatocp.f, mesh, flatsol.t0, flatsol.tf, flatsol.x, flatsol.u);

    std::cout << "interval errors " << errs.transpose() << std::endl;

    if (errs.maxCoeff() > target_err) {
      smooth::feedback::mesh_refine(mesh, errs, 0.1 * target_err);
      nlpsol = smooth::feedback::ocpsol_to_nlpsol(flatocp, mesh, flatsol);
    } else {
      break;
    }
  }

  const auto dur = std::chrono::high_resolution_clock::now() - t0;

  std::cout << "TOTAL TIME: " << std::chrono::duration_cast<std::chrono::milliseconds>(dur).count()
            << "ms" << std::endl;

#ifdef ENABLE_PLOTTING
  using namespace matplot;

  const auto [nodes, weights] = mesh.all_nodes_and_weights();

  const auto tt       = linspace(0., sols.back().tf, 500);
  const auto tt_nodes = r2v(sols.back().tf * nodes);

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(
      transform(tt, [&](double t) { return sol.x(t).part<0>().r2().x(); }),
      transform(tt, [&](double t) { return sol.x(t).part<0>().r2().y(); }),
      "-r")
      ->line_width(lw);
  }
  legend(std::vector<std::string>{"path"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.x(t).part<1>().x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.x(t).part<1>().y(); }), "-g")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.x(t).part<1>().z(); }), "-b")->line_width(lw);
  }
  legend({"vx", "vy", "wz"});

  figure();
  hold(on);
  plot(tt_nodes, transform(tt_nodes, [](auto) { return 0; }), "xk")->marker_size(10);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.lambda_dyn(t).x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.lambda_dyn(t).y(); }), "-b")->line_width(lw);
  }
  legend({"nodes", "lambda_x", "lambda_y"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.lambda_cr(t).x(); }), "-r")->line_width(lw);
  }
  legend(std::vector<std::string>{"lambda_{cr}"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&sol](double t) { return sol.u(t).x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&sol](double t) { return sol.u(t).y(); }), "-b")->line_width(lw);
  }
  legend({"throttle", "steering"});

  show();
#endif

  return EXIT_SUCCESS;
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * 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.
 *
 *
 * Gicp.cpp
 *
 *  Created on: Jan 31, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include <Eigen/LU> // for inverse and determinant

#include <memory>
#include <iostream>
#include "mrob/pc_registration.hpp" // GICP function is defined here

using namespace mrob;

int PCRegistration::gicp(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y,
           const Eigen::Ref<const MatX> covX, const Eigen::Ref<const MatX> covY, SE3 &T, double tol)
{
    assert(X.cols() == 3  && "PCRegistration::Gicp: Incorrect sizing, we expect Nx3");
    assert(X.rows() >= 3  && "PCRegistration::Gicp: Incorrect sizing, we expect at least 3 correspondences (not aligned)");
    assert(Y.rows() == X.rows()  && "PCRegistration::Gicp: Same number of correspondences");
    uint_t N = X.rows();
    // TODO precalculation of T by reduced Arun
    // TODO different number of iterations and convergence criterion

    // Initialize Jacobian and Hessian
    Mat61 J = Mat61::Zero();
    Mat6 H = Mat6::Zero();
    uint_t iters = 0;
    double deltaUpdate = 1e3;
    do
    {
        J.setZero();
        H.setZero();
        // not vectoried operations (due to Jacobian)
        for ( uint_t i = 0; i < N ; ++i)
        {
            // 1) Calculate residual r = y - Tx and the inverse of joint covariance
            Mat31 Txi = T.transform(X.row(i));
            Mat31 r = Y.row(i).transpose() - Txi;
            Mat3 Li = (covY.block<3,3>(3*i,0) + T.R() * covX.block<3,3>(3*i,0) * T.R().transpose()).inverse();

            // 2) Calculate Jacobian for residual Jf = df1/d xi = r1' Li * Jr, where Jr = [(Tx)^ ; -I])
            Mat<3,6> Jr;
            Jr << hat3(Txi) , -Mat3::Identity();
            Mat<1,6> Ji = r.transpose() * Li * Jr;
            J += Ji;//Eigen manages this for us

            // 3) Hessian Hi ~ Jr' * Li * Jr
            Mat6 Hi = Jr.transpose() * Li * Jr;
            H += Hi;
        }
        // 4) Update Solution
        Mat61 dxi = -H.inverse()*J;
        T.update_lhs(dxi); //Left side update
        deltaUpdate = dxi.norm();
        iters++;

    }while(deltaUpdate > tol && iters < 20);

    return iters; // number of iterations
}