#include <cmath>

#include <functional>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/Propulsion/costateBasedThrustGuidance.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/modifiedEquinoctialElementConversions.h"

namespace tudat
{

namespace propulsion
{

//! Constructor
MeeCostateBasedThrustGuidance::MeeCostateBasedThrustGuidance(
        const std::function< Eigen::Vector6d( ) > thrustingBodyStateFunction,
        const std::function< Eigen::Vector6d( ) > centralBodyStateFunction,
        const std::function< double( ) > centralBodyGravitationalParameterFunction,
        std::function< Eigen::VectorXd( const double ) > costateFunction,
        const std::function< Eigen::Vector3d( ) > bodyFixedForceDirection )
    : BodyFixedForceDirectionGuidance( bodyFixedForceDirection ),
      thrustingBodyStateFunction_( thrustingBodyStateFunction ),
      centralBodyStateFunction_( centralBodyStateFunction ),
      centralBodyGravitationalParameterFunction_( centralBodyGravitationalParameterFunction ),
      costateFunction_( costateFunction ){ }

//! Function to update the force direction to the current time.
void MeeCostateBasedThrustGuidance::updateForceDirection( const double time )
{
    if( !( time == currentTime_ ) )
    {
        Eigen::VectorXd costates_ = costateFunction_( time );

        // Get the current state in cartesian coordinates and keplerian elements, and some convenient parameters
        Eigen::Vector6d currentState = thrustingBodyStateFunction_( ) - centralBodyStateFunction_( );
        double centralBodyGravitationalParameter = centralBodyGravitationalParameterFunction_( );

        // Obtain ModifiedEquinoctial elements, flag of 0 indicates that singularity occurs at 180 deg inclination.
        Eigen::Vector6d modifiedEquinoctialElements =
                orbital_element_conversions::convertCartesianToModifiedEquinoctialElements(
                    currentState, centralBodyGravitationalParameter, 0 );

        // Optimal control laws local variables declared for clarity
        double auxiliaryParameterW = ( 1.0 + modifiedEquinoctialElements( 1 ) * cos( modifiedEquinoctialElements( 5 ) )
                     + modifiedEquinoctialElements( 2 ) * sin( modifiedEquinoctialElements( 5 ) ) );
        double auxiliaryParameterSSquared = 1.0 + modifiedEquinoctialElements( 3 ) * modifiedEquinoctialElements( 3 )
                + modifiedEquinoctialElements( 4 ) * modifiedEquinoctialElements( 4 );

        // Local variables for al constant terms for the calculation of pitch angle
        double Lap = costates_( 0 ) * 2.0 * modifiedEquinoctialElements( 0 ) / auxiliaryParameterW;
        double Laf1 = costates_( 1 )  * sin( modifiedEquinoctialElements( 5 ) ) ;
        double Laf2 = costates_( 1 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * cos( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 1 ) ) ;
        double Lag1 = costates_( 2 ) * cos( modifiedEquinoctialElements( 5 ) );
        double Lag2 = costates_( 2 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * sin( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 2 ) );

        // Calculate pitch angle, NOTE: denomitator ommitted since it is not relevant for the atan2 function,
        // since both denominators are the same.
        double thrustAngleAlpha = std::atan2( -Laf1+Lag1, -Lap-Laf2-Lag2);

        // Local variables for al constant terms for the calculation of yaw angle
        double Lbp = costates_( 0 ) * 2.0 * modifiedEquinoctialElements( 0 ) * cos( thrustAngleAlpha) / auxiliaryParameterW;
        double Lbf1 = costates_( 1 )  * sin( modifiedEquinoctialElements( 5 ) ) * sin( thrustAngleAlpha );
        double Lbf2 = costates_( 1 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * cos( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 1 ) ) * cos( thrustAngleAlpha );
        double Lbf3 = costates_( 1 ) / auxiliaryParameterW * ( modifiedEquinoctialElements( 2 ) *(
                                                 modifiedEquinoctialElements( 3 ) * sin( modifiedEquinoctialElements( 5 ) )
                                                 - modifiedEquinoctialElements( 4 ) * cos( modifiedEquinoctialElements( 5 ) ) ) );

        double Lbg1 = costates_( 2 ) * cos( modifiedEquinoctialElements( 5 ) ) * sin( thrustAngleAlpha);
        double Lbg2 = costates_( 2 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * sin( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 2 ) ) * cos( thrustAngleAlpha);
        double Lbg3 = costates_( 2 ) / auxiliaryParameterW * ( modifiedEquinoctialElements( 1 ) *(
                                                 modifiedEquinoctialElements( 3 ) * sin( modifiedEquinoctialElements( 5 ) )
                                                 - modifiedEquinoctialElements( 4 ) * cos( modifiedEquinoctialElements( 5 ) ) ) );
        double Lbh = costates_( 3 ) * auxiliaryParameterSSquared *
                cos( modifiedEquinoctialElements( 5 ) ) / ( 2.0 * auxiliaryParameterW );
        double Lbk = costates_( 4 ) * auxiliaryParameterSSquared *
                sin( modifiedEquinoctialElements( 5 ) ) / ( 2.0 * auxiliaryParameterW );

        // Calculate yaw angle, NOTE: denomitator ommitted since it is not relevant for the atan2 function,
        // since both denominators are the same.
        double thrustAngleBeta = std::atan2( Lbf3 - Lbg3 - Lbh - Lbk, - Lbp - Lbf1 - Lbf2 + Lbg1 - Lbg2 );

        // Calculate thrust direction
        currentForceDirection_ = reference_frames::getVelocityBasedLvlhToInertialRotation(
                    currentState, Eigen::Vector6d::Zero( ), false ) *
                ( ( Eigen::Vector3d( ) <<
                    cos( thrustAngleAlpha ) * cos( thrustAngleBeta ), sin( thrustAngleAlpha ) * cos( thrustAngleBeta ) ,
                    sin( thrustAngleBeta )  ).finished( ).normalized( ) );
        currentTime_ = time;
    }

}

} // namespace propulsion

} // namespace tudat


/**
 * @author      Mahdi Maghrebi <mahdi.maghrebi@nih.gov>
 * This code is an implementation of UMAP algorithm for dimension reduction.
 * The reference paper is “UMAP: Uniform Manifold Approximation and Projection for Dimension Reduction“, by McInnes et al., 2018 (https://arxiv.org/abs/1802.03426)
 * Jan 2020
 */

#include <vector>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <list>
#include <string>
#include <math.h>
#include <fstream>
#include <float.h>
#include <boost/filesystem.hpp>
#include "KNN_OpenMP_Code.h"
#include "highDComputes.h"
#include "Initialization.h"
#include "LMOptimization.h"
#include "SGD.h"
#include <exception>
#include <sstream>
#include <omp.h>
#include <Eigen/Sparse>

using namespace std;
using namespace Eigen;

int main(int argc, char ** argv) {
	/**
	 * The errors and informational messages are outputted to the log file
	 */
	ofstream logFile;
	string logFileName="Setting.txt";
	logFile.open(logFileName);
	/**
	 * The input parameters are read from command line which are as follow.
	 * filePath: The full path to the input file containig the dataset.
	 * outputPath: The full path to the output csv file containing the coordinates of data in the embedding space.
	 * K: K in K-NN that means the desired number of Nearest Neighbours to be computed.
	 * sampleRate: The rate at which we do sampling. This parameter plays a key role in the performance.
	 * This parameter is a trades-off between the performance and the accuracy of the results.
	 * Values closer to 1 provides more accurate results but the execution takes longer.
	 * DimLowSpace: Dimension of Low-D or embedding space (usually 1,2,or 3).
	 * randomInitializing: Defining the Method for Initialization of data in low-D space
	 * nepochs: is the number of training epochs to be used in optimizing. Larger values result in more accurate embeddings
	 * mindist defines how tight the points are from each other in Low-D space
	 * distanceMetric is the metric to compute the distance between the points in high-D space, by deafult should be euclidean
	 * distanceV1 is the first optional variable needed for computing distance in some metrics
	 * distanceV2 is the second optional variable needed for computing distance in some metrics
	 * inputPathOptionalArray is the full path to the directory that contains a csv file of the optional array needed for computing distance in some metrics.
	 */
	string filePath, filePathOptionalArray="", outputPath, LogoutputPath, inputPath;
	int K,DimLowSpace,nepochs;
	float sampleRate,mindist,distanceV1=0,distanceV2=0;
	bool randomInitializing;
	string distanceMetric="euclidean";

	for (int i=1; i<argc;++i){
		if (string(argv[i])=="--inputPath") {
			inputPath=argv[i+1];

			if(!boost::filesystem::exists(inputPath) || !boost::filesystem::is_directory(inputPath))
			{
				logFile << "Incorrect input path";
				cout << "Incorrect input path";
				return 1;
			}

			const std::string ext = ".csv";
			boost::filesystem::recursive_directory_iterator it(inputPath);
			boost::filesystem::recursive_directory_iterator endit;

			bool fileFound = false;
			while(it != endit) {
				if(boost::filesystem::is_regular_file(*it) && it->path().extension() == ext){
					fileFound = true;
					filePath = it->path().string();
					break;
				}
				++it;
			}
			if (!fileFound){
				logFile << "CSV file is not found in the input path";
				cout << "CSV file is not found in the input path";
				return 1;
			}
		}
		else if (string(argv[i])=="--K") K=atoi(argv[i+1]);
		else if (string(argv[i])=="--sampleRate") sampleRate=stof(argv[i+1]);
		else if (string(argv[i])=="--mindist") mindist=stof(argv[i+1]);
		else if (string(argv[i])=="--DimLowSpace") DimLowSpace=atoi(argv[i+1]);
		else if (string(argv[i])=="--randomInitializing") {
			std::stringstream ss(argv[i+1]);
			ss >> std::boolalpha >> randomInitializing;
		}
		else if (string(argv[i])=="--outputPath"){
			boost::filesystem::path p(argv[i+1]);

			if(!boost::filesystem::exists(p) || !boost::filesystem::is_directory(p))
			{
				logFile << "Incorrect output path";
				cout << "Incorrect output path";
				return 1;
			}

			LogoutputPath=argv[i+1];
			boost::filesystem::path joinedPath = p / boost::filesystem::path("ProjectedData_EmbeddedSpace.csv");
			outputPath = joinedPath.string();

		}
		else if (string(argv[i])=="--nepochs") nepochs=atoi(argv[i+1]);
		else if (string(argv[i])=="--distanceMetric") distanceMetric=argv[i+1];
		else if (string(argv[i])=="--distanceV1") distanceV1=stof(argv[i+1]);
		else if (string(argv[i])=="--distanceV2") distanceV2=stof(argv[i+1]);
		else if (string(argv[i])=="--inputPathOptionalArray") {
			string inputPathOptionalArray=argv[i+1];

			if(!boost::filesystem::exists(inputPathOptionalArray) || !boost::filesystem::is_directory(inputPathOptionalArray))
			{
				logFile << "Incorrect input path";
				cout << "Incorrect input path";
				return 1;
			}

			const std::string ext = ".csv";
			boost::filesystem::recursive_directory_iterator it(inputPathOptionalArray);
			boost::filesystem::recursive_directory_iterator endit;

			bool fileFound = false;
			while(it != endit) {
				if(boost::filesystem::is_regular_file(*it) && it->path().extension() == ext){
					fileFound = true;
					filePathOptionalArray = it->path().string();
					break;
				}
				++it;
			}
			if (!fileFound){
				logFile << "CSV file is not found in the input path";
				cout << "CSV file is not found in the input path";
				return 1;
			}
		}

	}
	logFile<<"------------The following Input Arguments were read------------"<<endl;
	logFile<<"The full path to the input file: "<< filePath<<endl;
	logFile<<"The full path to the output file: "<< outputPath<<endl;
	logFile<<"The desired number of NN to be computed: "<< K <<endl;
	logFile<<"The sampleRate(The rate at which we do sampling): "<< sampleRate <<endl;
	logFile<<"The Dimension of Low-D Space: "<< DimLowSpace <<endl;
	logFile << std::boolalpha;
	logFile<<"Random Initialization of Points in Low-D Space: "<< randomInitializing <<endl;
	logFile<<"The number of training epochs: "<< nepochs <<endl;
	logFile<<"The chosen mindist parameter: "<< mindist <<endl;
	logFile<<"The metric to compute the distance between the points in high-D space: "<< distanceMetric <<endl;
	logFile<<"The optional variable 1 for the distance: "<< distanceV1 <<endl;
	logFile<<"The optional variable 2 for the distance: "<< distanceV2 <<endl;
	logFile<<"The full path to optional array for the distance metric computation: "<< filePathOptionalArray <<endl;

	cout<<"------------The following Input Arguments were read------------"<<endl;
	cout<<"The full path to the input file: "<< filePath<<endl;
	cout<<"The full path to the output file: "<< outputPath<<endl;
	cout<<"The desired number of NN to be computed: "<< K <<endl;
	cout<<"The sampleRate(The rate at which we do sampling): "<< sampleRate <<endl;
	cout<<"The Dimension of Low-D Space: "<< DimLowSpace <<endl;
	cout << std::boolalpha;
	cout<<"Random Initialization of Points in Low-D Space: "<< randomInitializing <<endl;
	cout<<"The number of training epochs: "<< nepochs <<endl;
	cout<<"The chosen mindist parameter: "<< mindist <<endl;
	cout<<"The metric to compute the distance between the points in high-D space: "<< distanceMetric <<endl;
	cout<<"The optional variable 1 for the distance: "<< distanceV1 <<endl;
	cout<<"The optional variable 2 for the distance: "<< distanceV2 <<endl;
	cout<<"The full path to optional array for the distance metric computation: "<< filePathOptionalArray <<endl;
	/**
	 * Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 */
	string cmd="wc -l "+filePath;
	string outputCmd = exec(cmd.c_str());
	const int N=stoi(outputCmd.substr(0, outputCmd.find(" ")))-1;
	logFile<<"The Dimension of Dataset Records (Number of Rows in inputfile w/o header ): "<< N <<endl;
	cout<<"The Dimension of Dataset Records (Number of Rows in inputfile w/o header ): "<< N <<endl;
	/**
	 * Dimension of Dataset (#Columns)
	 */
	int Dim;
	string cmd2="head -n 1 "+ filePath + " |tr '\\,' '\\n' |wc -l ";
	Dim = stoi(exec(cmd2.c_str()));
	logFile<<"The Dimension of Dataset Features(Number of Columns in inputfile): "<< Dim <<endl;
	cout<<"The Dimension of Dataset Features(Number of Columns in inputfile): "<< Dim <<endl;

	if (K > N) {
		logFile<<" The desired number of NN has exceeded the size of dataset "<<endl;
		cout<<" The desired number of NN has exceeded the size of dataset "<<endl;
		return 1;
	}

	logFile<<"------------END of INPUT READING------------"<< endl;
	cout<<"------------END of INPUT READING------------"<< endl;

	/**
	 * Query about the number of available CPU processors and set it as OpenMP threads
	 */
	int nProcessors = omp_get_num_procs();
    omp_set_num_threads(nProcessors-1);
	cout <<"Total Number of Processes in the OpenMP Parallel Region = "<< nProcessors-1 <<endl;

	srand(17);

	/**
	 * convThreshold: Convergance Threshold of K-NN. A fixed integer is used here instead of delta*N*K.
	 */
	const int convThreshold=5;
	/**
	 * indices of K-NN for each data point
	 */
	int** B_Index = new int*[N];
	for (int i = 0; i < N; ++i) { B_Index[i] = new int[K]; }
	/**
	 * corresponding distance for K-NN indices stored in B_Index
	 */
	double** B_Dist = new double*[N];
	for (int i = 0; i < N; ++i) { B_Dist[i] = new double[K]; }
	/**
	 * Compute K-NN following the algorithm for shared-memory K-NN
	 * @param filePath The full path to the input file containig the dataset.
	 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 * @param Dim Dimension of Dataset (#Columns)
	 * @param K the desired number of Nearest Neighbours to be computed
	 * @param sampleRate The rate at which we do sampling
	 * @param convThreshold Convergance Threshold
	 * @param logFile The errors and informational messages are outputted to the log file
	 * @param distanceMetric is the metric to compute the distance between the points in high-D space, by deafult should be euclidean
	 * @param distanceV1 is the first optional variable needed for computing distance in some metrics
	 * @param distanceV2 is the second optional variable needed for computing distance in some metrics
	 * @param filePathOptionalArray The full path to optional array for the distance metric computation
	 * @return B_Index indices of K-NN for each data point
	 * @return B_Dist corresponding distance for K-NN indices stored in B_Index
	 */
	computeKNNs(filePath, N, Dim, K, sampleRate, convThreshold,B_Index,B_Dist, logFile, distanceMetric, distanceV1, distanceV2,filePathOptionalArray);

	bool flag=false;
	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < K; ++j) {
			if (B_Dist[i][j] < 0) {
				logFile<<"ALERT: A distance in high-D space was computed as negative, use this program with caution"<<endl;
				cout<<"ALERT: A distance in high-D space was computed as negative, use this program with caution"<<endl;
				flag=true;
				break;
			}
		}
		if (flag) break;
	}

	int* B_Index_Min = new int[N];
	double* B_Dist_Min = new double[N];
	/**
	 * Compute B_Index and B_Dist for the closest points (K-NNs)
	 * @param B_Index indices of K-NN for each data point
	 * @param B_Dist corresponding distance for K-NN indices stored in B_Index
	 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 * @param K the desired number of Nearest Neighbours to be computed
	 * @return B_Index_Min B_Index for the closest point
	 * @return B_Dist_Min B_Dist for the corresponding B_Index_Min
	 */
	findMin(B_Index,B_Dist, N,K,B_Index_Min,B_Dist_Min);

	double* SigmaValues = new double[N];
	/**
	 * Compute SigmaValues for each data point (Smooth approximator to K-NN distance) iteratively
	 * @param B_Dist corresponding distance for K-NN indices stored in B_Index
	 * @param B_Dist_Min B_Dist for the corresponding B_Index_Min
	 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 * @param K the desired number of Nearest Neighbours to be computed
	 * @return SigmaValues An array of Sigma Values for data
	 */
	findSigma(B_Dist, B_Dist_Min,SigmaValues, N, K);

	/**
	 * To save memory space, "Sparse Matrix" data structure is used here
	 * SparseMatrix by default is oriented column-major
	 */
	SparseMatrix<float> adjacencyMatrixA(N,N), adjacencyMatrixAT(N,N), graphSM(N,N);
	typedef Eigen::Triplet<float> T;
	std::vector<T> tripletList;
	tripletList.reserve(N*K);

	for (int i=0; i<N; ++i){
		for (int j=0; j<K; ++j){
			int point2=B_Index[i][j];
			float tmp=exp((B_Dist_Min[i]-B_Dist[i][j])/SigmaValues[i]);
			tripletList.push_back(T(i,point2,tmp));
		}
	}

	adjacencyMatrixA.setFromTriplets(tripletList.begin(), tripletList.end());
	adjacencyMatrixAT=adjacencyMatrixA.transpose();
	graphSM=adjacencyMatrixA+adjacencyMatrixAT;
	graphSM -=adjacencyMatrixA.cwiseProduct(adjacencyMatrixAT);

	float MaxWeight=0;
	for (int k=0; k<graphSM.outerSize(); ++k){
		float sum=0;
		for (SparseMatrix<float>::InnerIterator it(graphSM,k); it; ++it) {
			sum += it.value();
			if (it.value() > MaxWeight) MaxWeight=it.value();
		}
	}

	logFile<<"------------Setting Low-D Space Design------------"<<endl;
	cout<<"------------Setting Low-D Space Design------------"<<endl;

	/**
	 * embedding is the coordinates of the points in the low-D space
	 */
	double** embedding = new double*[N];
	for (int i = 0; i < N; ++i) { embedding[i] = new double[DimLowSpace]; }

	logFile<<"------------Starting Initialization in the Low-D Space------------"<<endl;
	cout<<"------------Starting Initialization in the Low-D Space------------"<<endl;
	/**
	 * Initializes the data points in low-D space
	 * @param randomInitializing the methodology for Initialization of data in low-D space
	 * @param logFile contains the errors and informational messages
	 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 * @param graph contains undirected weights (similarities) in the form of a matrix of size NxN
	 * @param DimLowSpace Dimension of Low-D space
	 * @return embedding is the coordinates of the points in the low-D space
	 */
	Initialization (randomInitializing, embedding, logFile, N, graphSM, MaxWeight, DimLowSpace, nepochs);

	logFile<<"------------Starting Estimating Hyper-Parameters a and b ------------"<<endl;
	cout<<"------------Starting Estimating Hyper-Parameters a and b ------------"<<endl;
	/**
	 * Hyper-Parameters a and b which needs to be estimated by data fitting.
	 */
	float aValue, bValue;
	float spread=1.0;
	/**
	 *  Estimation of Hyper-Parameters a and b by curve fitting and using Levenberg-Marquardt solution
	 */
	estimateParameters(aValue, bValue, mindist, spread, logFile);

	logFile<<"The Estimated Values for a is "<< aValue << " and for b is "<< bValue <<endl;
	cout<<"The Estimated Values for a is "<< aValue << " and for b is "<< bValue <<endl;

	logFile<<"------------Starting Solution for Stochastic Gradient Descent (SGD)------------"<<endl;
	cout<<"------------Starting Solution for Stochastic Gradient Descent (SGD)------------"<<endl;
	/**
	 *  alpha is the initial learning rate for the SGD. alpha starts from 1 and decreases in each epoch iteration
	 */
	float alpha=1.0;
	/**
	 * epochs_per_sample is a vector of edges with the values proportional to the values in graph
	 * epochs_per_sample represents the epoch weight for edges where the edge with the highest similarity will get the value of 1
	 * and all other edges will get a proportional epoch weight scaled from it. epochs_per_sample is used as a measure to include an edge in
	 * SGD computations. The edge with the highest similarity will be used at every epoch iteration.
	 * head is a vector containing the head index of the edge
	 * tail is a vector containing the tail index of the edge
	 */
	vector<int> head, tail;
	vector<float> epochs_per_sample;

	for (int k=0; k<graphSM.outerSize(); ++k){
		for (SparseMatrix<float>::InnerIterator it(graphSM,k); it; ++it) {
		    if (it.value() <  MaxWeight/nepochs) continue;
			epochs_per_sample.push_back(MaxWeight/it.value());
			head.push_back(it.col());
			tail.push_back(it.row());
		}
	}

	/**
	 * This section was adopted from SGD implementation at https://github.com/lmcinnes/umap/blob/8f2ef23ec835cc5071fe6351a0da8313d8e75706/umap/layouts.py#L136
	 * edgeCounts is total number of edges in the high-D space graph
	 * epoch_of_next_sample is an index of the epoch state of the edges. If it is less than epoch index, we will use the edge in the computation
	 * epoch_of_next_negative_sample is an index of the epoch state of the edges for sampling from non-connected surrounding points.
	 * negative_sample_rate is the rate at which we sample from the non-connected surrounding points as compared to the connected edges.
	 * Increasing this value will result in greater repulsive force being applied, greater optimization cost, but slightly more accuracy.
	 */
	int edgeCounts=epochs_per_sample.size();
	const int negative_sample_rate=5;
	int n_neg_samples;
//Substituting with Vectors due to Stacksize run-time error
//	float epoch_of_next_sample[edgeCounts];
//	float epochs_per_negative_sample[edgeCounts];
//	float epoch_of_next_negative_sample[edgeCounts];
    vector<float> epoch_of_next_sample,epochs_per_negative_sample,epoch_of_next_negative_sample;

	for (int i = 0; i < edgeCounts; ++i) {
//		epoch_of_next_sample[i]=epochs_per_sample[i];
//		epochs_per_negative_sample[i]=epochs_per_sample[i]/negative_sample_rate;
//		epoch_of_next_negative_sample[i]=epochs_per_negative_sample[i];
        epoch_of_next_sample.push_back(epochs_per_sample[i]);
        epochs_per_negative_sample.push_back(epochs_per_sample[i]/negative_sample_rate);
        epoch_of_next_negative_sample.push_back(epochs_per_negative_sample[i]);
	}
	/**
	 *  move_other is equal to 1 if not embedding new previously unseen points to low-D space
	 */
	const int move_other=1;
	/**
	 *  dEpsilon is zero approximation in double precision
	 */
	const double dEpsilon=1e-14;
	double dist_squared;
	// The main training loop
	for (int n = 1; n < nepochs; ++n) {

		//Loop over all edges of the graph
		if (n%100 == 0){
			logFile << "SGD iteration = "<<n<<" from "<< nepochs <<endl;
			cout << "SGD iteration = "<<n<<" from "<< nepochs <<endl;
		}

		for (int i = 0; i < edgeCounts; ++i) {
			if (epoch_of_next_sample[i] <= n){

				int headIndex = head[i];
				int tailIndex = tail[i];

				dist_squared = rdist(embedding, DimLowSpace, headIndex, tailIndex);

				double grad_coeff;
				if (dist_squared<dEpsilon) grad_coeff=0;
				else {grad_coeff= -2.0*aValue*bValue*pow(dist_squared,bValue-1)/(1.0+aValue*pow(dist_squared,bValue)); }

                double grad_d;
				for (int jj = 0; jj < DimLowSpace; ++jj) {
					grad_d = alpha*clip(grad_coeff*(embedding[headIndex][jj]-embedding[tailIndex][jj]));
                    embedding[headIndex][jj] += grad_d;
					//if (move_other==1) 	{
						embedding[tailIndex][jj] -= grad_d;
					//}
				}

				epoch_of_next_sample[i] += epochs_per_sample[i];
				n_neg_samples = int((float(n) - epoch_of_next_negative_sample[i])/ epochs_per_negative_sample[i]);

				for (int ll = 0; ll < n_neg_samples; ++ll) {
					int randomIndex = rand() % N;
					if (randomIndex==headIndex) continue;

					dist_squared= rdist(embedding, DimLowSpace, headIndex, randomIndex);

					if (dist_squared < dEpsilon) grad_coeff=0;
					else{ grad_coeff = 2.0*bValue/((0.001+dist_squared)*(1.0+aValue*pow(dist_squared,bValue))); }

					for (int jj = 0; jj < DimLowSpace; ++jj) {
						if  (grad_coeff > 0) {
							embedding[headIndex][jj] += alpha*clip(grad_coeff*(embedding[headIndex][jj]-embedding[randomIndex][jj]));
						} else  {
							embedding[headIndex][jj] += alpha*4.0;
						}
					}
				}
				epoch_of_next_negative_sample[i] += (n_neg_samples * epochs_per_negative_sample[i]);
			}
		}
		alpha=1.0-((float)n)/nepochs;
	}

	logFile<<"------------Starting Outputing the Results------------"<<endl;
	cout<<"------------Starting Outputing the Results------------"<<endl;
	/**
	 * Output the coordinates of the projected data in the low-D space
	 */
	ofstream embeddedSpacefile;
	embeddedSpacefile.open(outputPath);

	for (int j = 0; j < DimLowSpace; ++j) {
		if (j != DimLowSpace-1) embeddedSpacefile<<"Dimension"<<j+1<<",";
		else embeddedSpacefile<<"Dimension"<<j+1<<endl;
	}

	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < DimLowSpace; ++j) {
			if (j==DimLowSpace-1) {
				embeddedSpacefile<< embedding[i][j]<<endl;}
			else {embeddedSpacefile<< embedding[i][j]<<",";}
		}
	}

	embeddedSpacefile.close();
	logFile.close();
	/**
	 * copy Logfile to the file system which could be accessed outside the docker container
	 */
	string cmd3="cp "+ logFileName+"  "+LogoutputPath;
	// To remove the returning messages, we can switch to the following command
	//	string cmd3="cp "+ logFileName+"  "+LogoutputPath+ " 2>&1 /dev/null";
	string outputCmd3 = exec(cmd3.c_str());

	return 0;
}


//
// Created by Zhongshi Jiang on 5/8/17.
//

#include <igl/copyleft/tetgen/tetrahedralize.h>
#include <Eigen/Core>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/triangle/triangulate.h>
#include <igl/cat.h>
#include <igl/writeOBJ.h>
#include <igl/readOBJ.h>
#include <igl/writeMESH.h>
#include <igl/readMESH.h>
#include <igl/barycenter.h>
#include <igl/read_triangle_mesh.h>
#include <iostream>
#include <igl/Timer.h>
#include <fstream>
#include <igl/boundary_loop.h>
#include <igl/harmonic.h>
#include <igl/map_vertices_to_circle.h>
#include <igl/doublearea.h>
#include <igl/PI.h>
#include <igl/flipped_triangles.h>
#include <igl/file_dialog_open.h>

void parameterization_init(std::string filename, Eigen::MatrixXd& V_ref,
                           Eigen::MatrixXi &F_ref,
                           Eigen::MatrixXd& V_all, Eigen::MatrixXi &F_scaf,
  Eigen::VectorXi &frame_id, Eigen::MatrixXi& display_F) {
  using namespace std;
  using namespace Eigen;
  while (!igl::read_triangle_mesh(filename, V_ref,
                                  F_ref)) {
    std::cerr << "Cannot Open Mesh!" << std::endl;
    filename = igl::file_dialog_open();
  }

  Eigen::MatrixXd uv_init;
  Eigen::VectorXi bnd;
  Eigen::MatrixXd bnd_uv;
  igl::Timer timer;
  timer.start();
  igl::boundary_loop(F_ref, bnd);
  cout << "bndloop = " << timer.getElapsedTime() << endl;
  timer.start();

  timer.start();
  VectorXd M;
  igl::doublearea(V_ref, F_ref, M);
 std::cout<<"sqrtM/2pi"<< sqrt(M.sum()/(2*igl::PI))<<std::endl;
//  M /= M.sum()/igl::PI;

  igl::map_vertices_to_circle(V_ref, bnd, bnd_uv);
  V_ref *= 2;
  bnd_uv *= sqrt(M.sum()/(2*igl::PI));
  cout << "v2circle = " << timer.getElapsedTime() << endl;
  igl::harmonic(V_ref, F_ref, bnd, bnd_uv, 1, uv_init);
  if (igl::flipped_triangles(uv_init, F_ref).size() != 0) {
    igl::harmonic(F_ref, bnd, bnd_uv, 1, uv_init); // use uniform laplacian
  }
  cout << "Harmonic = " << timer.getElapsedTime() << endl;

//  scaffold_generator(uv_init, F_ref, scaf_data.density, Vall, Fscaf);

  MatrixXd V_bnd;
  V_bnd.resize(bnd.size(), uv_init.cols());
  for (int i = 0; i < bnd.size(); i++) // redoing step 1.
  {
    V_bnd.row(i) = uv_init.row(bnd(i));
  }
  Matrix2d ob;// = rect_corners;
  {
    VectorXd uv_max = uv_init.colwise().maxCoeff();
    VectorXd uv_min = uv_init.colwise().minCoeff();
    VectorXd uv_mid = (uv_max + uv_min) / 2.;

    double scaf_range = 8;
    ob.row(0) = uv_mid + scaf_range * (uv_min - uv_mid);
    ob.row(1) = uv_mid + scaf_range * (uv_max - uv_mid);
  }
  Vector2d rect_len;
  rect_len << ob(1, 0) - ob(0, 0), ob(1, 1) - ob(0, 1);

  int frame_points = 5;
  MatrixXd V_rect;
  V_rect.resize(4 * frame_points, 2);
  for (int i = 0; i < frame_points; i++) {
    // 0,0;0,1
    V_rect.row(i) << ob(0, 0), ob(0, 1) + i * rect_len(1) / frame_points;
    // 0,0;1,1
    V_rect.row(i + frame_points)
        << ob(0, 0) + i * rect_len(0) / frame_points, ob(1, 1);
    // 1,0;1,1
    V_rect.row(i + 2 * frame_points) << ob(1, 0), ob(1, 1) - i * rect_len(1) /
        frame_points;
    // 1,0;0,1
    V_rect.row(i + 3 * frame_points)
        << ob(1, 0) - i * rect_len(0) / frame_points, ob(0, 1);
    // 0,0;0,1
  }

  // Concatenate Vert and Edge
  MatrixXd V;
  MatrixXi E;
  igl::cat(1, V_bnd, V_rect, V);
  E.resize(V.rows(), 2);
  for (int i = 0; i < E.rows(); i++)
    E.row(i) << i, i + 1;
  E(bnd.size() - 1, 1) = 0;
  E(V.rows() - 1, 1) = static_cast<int>(bnd.size());

  MatrixXd H = MatrixXd::Zero(10, 2);
  for (int f = 0; f < H.rows(); f++)
    for (int i = 0; i < 3; i++)
      H.row(f) += uv_init.row(F_ref(f, i)); // redoing step 2
  H /= 3.;
  timer.start();
  MatrixXd uv2;
  igl::triangle::triangulate(V, E, H, "qYYQ", uv2, F_scaf);

  auto bnd_n = bnd.size();
  V_all.resize(uv_init.rows() - bnd_n + uv2.rows(), 2);
  V_all.topRows(uv_init.rows()) = uv_init;
  V_all.bottomRows(uv2.rows() - bnd_n) = uv2.bottomRows(-bnd_n + uv2.rows());

  for (auto i = 0; i < F_scaf.rows(); i++)
    for (auto j = 0; j < F_scaf.cols(); j++) {
      auto &x = F_scaf(i, j);
      if (x < bnd_n) x = bnd(x);
      else x += uv_init.rows() - bnd_n;
    }

  cout << "New Cat = " << timer.getElapsedTime() << endl;

  frame_id = Eigen::VectorXi::LinSpaced(
      V_rect.rows(), V_ref.rows(), V_ref.rows() + V_rect.rows() - 1);
  igl::cat(1, F_ref, F_scaf, display_F);
}


/*
Copyright (c) 2014, Aaron S Wishnick
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.
* Neither the name of the Aaron S Wishnick, iZotope, Inc., 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.
*/
#include <iostream>
#include <cmath>
#include <array>
#include <vector>
#include <iomanip>
#include <string>
#include <complex>
#include <functional>
#include <map>
#include <random>
#include <Eigen/Dense>
#include <memory>
#include "sndfile.h"
using namespace std;
using namespace Eigen;

static array<double, 5> lowpass_coefficients(double w0, double q) {
  array<double, 5> coeffs;

  const double cosw0 = cos(w0);
  const double alpha = sin(w0) / (2 * q);

  const double a0 = 1 + alpha;

  coeffs[0] = 0.5 * (1 - cosw0);
  coeffs[1] = 1 - cosw0;
  coeffs[2] = 0.5 * (1 - cosw0);
  coeffs[3] = -2 * cosw0;
  coeffs[4] = 1 - alpha;

  for (auto& x : coeffs) {
    x /= a0;
  }

  return coeffs;
}

static array<double, 5> peaking_coefficients(double w0, double q, double gain) {
  array<double, 5> coeffs;

  const double A = pow(10.0, gain / 40);
  const double alpha = sin(w0) / (2 * q);

  coeffs[0] = 1 + alpha * A;
  coeffs[1] = -2 * cos(w0);
  coeffs[2] = 1 - alpha * A;
  double a0 = 1 + alpha / A;
  coeffs[3] = -2 * cos(w0);
  coeffs[4] = 1 - alpha / A;

  for (auto& x : coeffs) {
    x /= a0;
  }

  return coeffs;
}

static vector<double> generate_sine_wave(int length, double cycles_per_sample) {
  vector<double> signal(length);

  if (cycles_per_sample == 0) {
    signal.assign(length, 1.0);
    return signal;
  }

  for (int i = 0; i < length; ++i) {
    signal[i] = sin(2 * M_PI * i * cycles_per_sample);
  }
  return signal;
}

// Smooth a signal with a linear-phase Hann filter.
static vector<double> smooth_hann(const vector<double>& x, int radius) {
  int n = x.size();
  int m = 2 * radius + 1;

  vector<double> y(n + m - 1);

  vector<double> h(m);
  double norm = 0;
  for (int i = 0; i < m; ++i) {
    h[i] = 0.5 - 0.5 * cos((i + 0.5) * 2.0 * M_PI / m);
    norm += h[i];
  }
  for (int i = 0; i < m; ++i) {
    h[i] /= norm;
  }

  for (int i = 0; i < n + m - 1; ++i) {
    double sum = 0;
    for (int j = 0; j < m; ++j) {
      int idx = radius + i - j;
      if (idx < 0) {
        break;
      }
      sum += h[j] * x[idx];
    }
    y[i] = sum;
  }

  return y;
}

// Generate a Hann window.
static vector<double> generate_hann_window(int n) {
  vector<double> w(n);
  for (int i = 0; i < n; ++i) {
    w[i] = 0.5 - 0.5 * cos(2.0 * M_PI * i / (n - 1));
  }
  return w;
}

// Compute the gain of a biquad at frequency w0.
static double biquad_gain(array<double, 5> coeffs, double w0) {
  const double b0 = coeffs[0], b1 = coeffs[1], b2 = coeffs[2];
  const double a1 = coeffs[3], a2 = coeffs[4];

  auto z = polar(1.0, w0);
  auto z2 = z * z;

  auto h = (b0 + b1 / z + b2 / z2) / (1.0 + a1 / z + a2 / z2);
  return abs(h);
}

// High-quality reference signal. Assumes that the filter input is a sinusoid.
// Smooths the gain of the filter coefficients, and applies it to the signal.
static vector<double> reference(vector<double> x,
                                const vector<array<double, 5>>& coeffs,
                                double w0, int radius) {
  const int n = x.size();

  vector<double> gains(n);
  transform(begin(coeffs), end(coeffs), begin(gains),
            [w0](array<double, 5> coeffs) { return biquad_gain(coeffs, w0); });

  gains = smooth_hann(gains, radius);

  transform(begin(x), end(x), begin(gains), begin(x),
            [](double sig, double gain) { return sig * gain; });

  return x;
}

// Low-quality anchor. Applies the gain to the signal, with no smoothing, and
// inserts an impulse whenever coefficients change.
static vector<double>
anchor(vector<double> x, const vector<array<double, 5>>& coeffs, double w0) {
  const int n = x.size();

  vector<double> gains(n);
  transform(begin(coeffs), end(coeffs), begin(gains),
            [w0](array<double, 5> coeffs) { return biquad_gain(coeffs, w0); });

  double max_gain = accumulate(begin(gains), end(gains), 0.0,
                               [](double a, double b) { return max(a, b); });
  double transient_gain = 3 * max_gain;

  for (int i = 0; i < n; ++i) {
    if (i != 0 && gains[i] != gains[i - 1]) {
      // If there was a change in gain, insert a click.
      x[i] += transient_gain;
    } else {
      x[i] *= gains[i];
    }
  }

  return x;
}

// Vanilla Direct Form II implementation.
static vector<double> time_varying_df2(vector<double> x,
                                       const vector<array<double, 5>>& coeffs) {
  double w1 = 0, w2 = 0;
  const int n = x.size();
  for (int i = 0; i < n; ++i) {
    auto cur_coeffs = coeffs[i];

    double w = x[i] - cur_coeffs[3] * w1 - cur_coeffs[4] * w2;
    double y = cur_coeffs[0] * w + cur_coeffs[1] * w1 + cur_coeffs[2] * w2;
    x[i] = y;

    w2 = w1;
    w1 = w;
  }

  return x;
}

// Vanilla SVF implementation.
static vector<double> time_varying_svf(vector<double> x,
                                       const vector<array<double, 5>>& coeffs) {
  double s1 = 0, s2 = 0;
  const int n = x.size();

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    const complex<double> neg_sqrt = sqrt(complex<double>(-1 - a1 - a2));
    const complex<double> pos_sqrt = sqrt(complex<double>(-1 + a1 - a2));

    const double g = (neg_sqrt / pos_sqrt).real();
    const double R = (a2 - 1) / (neg_sqrt * pos_sqrt).real();
    const double TwoR = 2 * R;
    const double g2 = g * g;
    const double chp = (b0 - b1 + b2) / (1 - a1 + a2);
    const double cbp = -2 * (b0 - b2) / (neg_sqrt * pos_sqrt).real();
    const double clp = (b0 + b1 + b2) / (1 + a1 + a2);

    double u1 = (x[i] - (g + TwoR) * s1 - s2) / (1 + g2 + TwoR * g);
    double u2 = g * u1 + s1;
    double u3 = g * u2 + s2;

    s1 += 2.0 * g * u1;
    s2 += 2.0 * g * u2;

    x[i] = chp * u1 + cbp * u2 + clp * u3;
  }

  return x;
}

// SVF, with the stabilization method from "Stability of Recursive Time-Varying
// Digital Filters by State Vector Transformation."
static vector<double>
svf_rabenstein_czarnach(vector<double> x,
                        const vector<array<double, 5>>& coeffs) {
  Matrix<double, 2, 1> s = Matrix<double, 2, 1>::Zero();
  const int n = x.size();

  double R_prev;

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    const complex<double> neg_sqrt = sqrt(complex<double>(-1 - a1 - a2));
    const complex<double> pos_sqrt = sqrt(complex<double>(-1 + a1 - a2));

    const double g = (neg_sqrt / pos_sqrt).real();
    const double R = (a2 - 1) / (neg_sqrt * pos_sqrt).real();
    const double g2 = g * g;

    const double chp = (b0 - b1 + b2) / (1 - a1 + a2);
    const double cbp = -2 * (b0 - b2) / (neg_sqrt * pos_sqrt).real();
    const double clp = (b0 + b1 + b2) / (1 + a1 + a2);

    Matrix<double, 2, 2> A;
    A(0, 0) = -2 * R;
    A(0, 1) = -1;
    A(1, 0) = 1;
    A(1, 1) = 0;

    Matrix<double, 2, 1> B;
    B(0, 0) = 1;
    B(1, 0) = 0;

    Matrix<double, 2, 2> H =
        (Matrix<double, 2, 2>::Identity() - g * A).inverse();

    Matrix<double, 2, 2> Pa = Matrix<double, 2, 2>::Identity() + 2 * g * A * H;
    Matrix<double, 2, 1> Pb = 2 * g2 * A * H * B + 2 * g * B;
    Matrix<double, 2, 1> Pc;
    Pc(0, 0) = cbp - 2 * R * chp;
    Pc(1, 0) = clp - chp;
    double Pd = chp;

    Matrix<double, 2, 2> TT;
    double Rn = i != 0 ? R_prev : R;
    TT(0, 0) = (sqrt(complex<double>(Rn * Rn - 1)) /
                sqrt(complex<double>(R * R - 1))).real();
    TT(0, 1) = -(complex<double>(0, 1) * (R - Rn) /
                 sqrt(complex<double>(R * R - 1))).real();
    TT(1, 0) = 0;
    TT(1, 1) = 1;

    s = TT * s;

    // Compute output.
    double y = Pc.transpose() * s + Pd * x[i];

    // State update
    s = Pa * s + Pb * x[i];

    R_prev = R;

    x[i] = y;
  }

  return x;
}

template <int N>
static Matrix<double, N, N>
ComputeRabensteinK(Matrix<double, N, N> A1, Matrix<double, N, 1> b1,
                   Matrix<double, N, N> A2, Matrix<double, N, 1> b2) {
  Matrix<double, N, N> K = b1 * b2.transpose();

  auto Ae1 = A1;
  auto Ae2 = A2;
  int i = 0;
  while (i++ < 1000000 && (Ae1.template lpNorm<Infinity>() > 1e-12 ||
                           Ae2.template lpNorm<Infinity>() > 1e-12)) {
    Matrix<double, N, 1> f1 = Ae1 * b1;
    Matrix<double, N, 1> f2 = Ae2 * b2;

    K += f1 * f2.transpose();

    Ae1 *= A1;
    Ae2 *= A2;
  }

  return K;
}

// Direct Form 2 Transposed
static vector<double> tdf2(vector<double> x,
                           const vector<array<double, 5>>& coeffs) {
  const int n = x.size();
  Matrix<double, 2, 1> s = Matrix<double, 2, 1>::Zero();

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    Matrix<double, 2, 2> A;
    A << -a1, 1.0, -a2, 0.0;

    Matrix<double, 2, 1> b;
    b << b1 - b0* a1, b2 - b0* a2;

    Matrix<double, 2, 1> c;
    c << 1.0, 0.0;

    double d = b2;

    double y = c.transpose() * s + d * x[i];
    s = A * s + b * x[i];

    x[i] = y;
  }

  return x;
}

static vector<double>
tdf2_rabenstein_czarnach(vector<double> x,
                         const vector<array<double, 5>>& coeffs) {
  const int n = x.size();
  Matrix<double, 2, 1> s = Matrix<double, 2, 1>::Zero();

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    Matrix<double, 2, 2> A;
    A << -a1, 1.0, -a2, 0.0;

    Matrix<double, 2, 1> b;
    b << b1 - b0* a1, b2 - b0* a2;

    Matrix<double, 2, 1> c;
    c << 1.0, 0.0;

    double d = b2;

    double r = sqrt(a2);
    double t = acos(a1 / (-2 * r));

    auto prev_coeffs = i != 0 ? coeffs[i - 1] : coeffs[i];
    double rp = sqrt(prev_coeffs[4]);
    double tp = acos(prev_coeffs[3] / (-2 * rp));

    Matrix<double, 2, 2> TT;
    TT << 1, 0, r* sin(t - tp) / sin(tp), r * sin(t) / (rp * sin(tp));
    s = TT * s;

    double y = c.transpose() * s + d * x[i];
    s = A * s + b * x[i];

    x[i] = y;
  }

  return x;
}

// SVF, with transient minimization as "Minimization of Transient Signals in
// Recursive Time-Varying Digital Filters".
static vector<double> svf_rabenstein(vector<double> x,
                                     const vector<array<double, 5>>& coeffs) {
  Matrix<double, 2, 1> s = Matrix<double, 2, 1>::Zero();
  const int n = x.size();

  Matrix<double, 2, 2> Pa_prev;
  Matrix<double, 2, 1> Pb_prev;

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    const complex<double> neg_sqrt = sqrt(complex<double>(-1 - a1 - a2));
    const complex<double> pos_sqrt = sqrt(complex<double>(-1 + a1 - a2));

    const double g = (neg_sqrt / pos_sqrt).real();
    const double R = (a2 - 1) / (neg_sqrt * pos_sqrt).real();
    const double g2 = g * g;

    const double chp = (b0 - b1 + b2) / (1 - a1 + a2);
    const double cbp = -2 * (b0 - b2) / (neg_sqrt * pos_sqrt).real();
    const double clp = (b0 + b1 + b2) / (1 + a1 + a2);

    Matrix<double, 2, 2> A;
    A(0, 0) = -2 * R;
    A(0, 1) = -1;
    A(1, 0) = 1;
    A(1, 1) = 0;

    Matrix<double, 2, 1> B;
    B(0, 0) = 1;
    B(1, 0) = 0;

    Matrix<double, 2, 2> H =
        (Matrix<double, 2, 2>::Identity() - g * A).inverse();

    Matrix<double, 2, 2> Pa = Matrix<double, 2, 2>::Identity() + 2 * g * A * H;
    Matrix<double, 2, 1> Pb = 2 * g2 * A * H * B + 2 * g * B;
    Matrix<double, 2, 1> Pc;
    Pc(0, 0) = cbp - 2 * R * chp;
    Pc(1, 0) = clp - chp;
    double Pd = chp;

    if (i != 0 && coeffs[i] != coeffs[i - 1]) {
      auto Knn = ComputeRabensteinK(Pa_prev, Pb_prev, Pa_prev, Pb_prev);
      auto Knp = ComputeRabensteinK(Pa_prev, Pb_prev, Pa, Pb);

      Pa_prev = Pa;
      Pb_prev = Pb;

      Matrix<double, 2, 2> T = Knp.transpose() * Knn.inverse();
      Pa = Pa * T;
      Pc = (Pc.transpose() * T).transpose();
    } else {
      Pa_prev = Pa;
      Pb_prev = Pb;
    }

    // Compute output.
    double y = Pc.transpose() * s + Pd * x[i];

    // State update
    s = Pa * s + Pb * x[i];

    x[i] = y;
  }

  return x;
}

// Truncated output switching, as in "Suppression of Transients in Time-Varying
// Recursive Filters for Audio Signals".
static vector<double> truncated_output_switching_filter(
    vector<double> x, const vector<array<double, 5>> coeffs, int Na) {
  const int n = x.size();
  vector<double> ys(n);
  for (int i = 0; i < n; ++i) {
    auto cur_coeffs = coeffs[i];
    double w1 = 0, w2 = 0;
    double y = 0;

    int start = max(i - Na + 1, 0);
    for (int j = start; j <= i; ++j) {
      double w = x[j] - cur_coeffs[3] * w1 - cur_coeffs[4] * w2;
      y = cur_coeffs[0] * w + cur_coeffs[1] * w1 + cur_coeffs[2] * w2;

      w2 = w1;
      w1 = w;
    }

    ys[i] = y;
  }

  return ys;
}

// Output switching, equivalent to Zetterberg-Zhang.
static vector<double>
output_switching_filter(vector<double> x,
                        const vector<array<double, 5>> coeffs) {
  return truncated_output_switching_filter(x, coeffs, x.size() + 1);
  const int n = x.size();
  vector<double> ys(n);
  for (int i = 0; i < n; ++i) {
    auto cur_coeffs = coeffs[i];
    double w1 = 0, w2 = 0;
    double y = 0;

    for (int j = 0; j <= i; ++j) {
      double w = x[j] - cur_coeffs[3] * w1 - cur_coeffs[4] * w2;
      y = cur_coeffs[0] * w + cur_coeffs[1] * w1 + cur_coeffs[2] * w2;

      w2 = w1;
      w1 = w;
    }

    ys[i] = y;
  }

  return ys;
}

// Gold/Rader or coupled/normal form.
static vector<double> gold_rader(vector<double> x,
                                 const vector<array<double, 5>> coeffs) {
  const int n = x.size();
  double u1 = 0, u2 = 0;

  for (int i = 0; i < n; ++i) {
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];
    double alpha = -a1 / 2;
    double beta = -0.5 * sqrt(4 * a2 - a1 * a1);

    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    double alpha2 = alpha * alpha, beta2 = beta * beta;
    double k1 = b0 - b2 / (alpha2 + beta2);
    double k2 = (b2 * alpha + (b1 + b0 * alpha) * (alpha2 + beta2)) /
                (beta * (alpha2 + beta2));
    double k3 = b2 / (alpha2 + beta2);

    double y = k1 * u1 + k2 * u2 + k3 * x[i];

    double u1_new = alpha * u1 - beta * u2 + x[i];
    double u2_new = beta * u1 + alpha * u2;
    u1 = u1_new;
    u2 = u2_new;
    x[i] = y;
  }

  return x;
}

static void write_wav(const char* path, double sampling_rate, int channel_count,
                      const vector<double>& samples) {
  SF_INFO sfinfo;
  sfinfo.samplerate = static_cast<int>(sampling_rate + 0.5);
  sfinfo.channels = channel_count;
  sfinfo.format = SF_FORMAT_WAV | SF_FORMAT_FLOAT;
  unique_ptr<SNDFILE, int (*)(SNDFILE*)> file(sf_open(path, SFM_WRITE, &sfinfo),
                                              &sf_close);

  if (!file) {
    cerr << "Error opening \"" << path << "\" for writing. "
         << sf_strerror(nullptr) << endl;
    return;
  }

  // Make the data multichannel.
  vector<float> multichannel_samples;
  multichannel_samples.reserve(channel_count * samples.size());
  for (auto x : samples) {
    for (int i = 0; i < channel_count; ++i) {
      multichannel_samples.push_back(x);
    }
  }

  sf_write_float(file.get(), multichannel_samples.data(),
                 multichannel_samples.size());
}

int main(int argc, char* argv[]) {
  if (argc != 4) {
    cerr << "Usage: " << argv[0]
         << " [filter type] [sine_hz|dc] [output_dir]\n";
    return -1;
  }

  double cycles_per_second;
  bool dc_input;
  try {
    if (strcmp(argv[2], "dc") == 0) {
      dc_input = true;
      cycles_per_second = 100;
    } else {
      dc_input = false;
      cycles_per_second = stod(argv[2]);
    }
  }
  catch (...) {
    cerr << "Invalid stimulus \"" << argv[2] << "\".\n";
    return -2;
  }

  string output_dir(argv[3]);

  const double sampling_rate = 48000;
  const double cycles_per_sample = cycles_per_second / sampling_rate;
  const double q = 6.0;
  const double cutoff_scale = 0.2;
  const double start_cutoff =
      max(cycles_per_second, 100.0) * (1 - cutoff_scale);
  const double end_cutoff = max(cycles_per_second, 100.0) * (1 + cutoff_scale);
  const int constant_length = 48000;
  const int end_constant_length = sampling_rate;
  const int start_vary_pos = constant_length;
  const int max_ramp_length = 200;
  const int length = constant_length + max_ramp_length + end_constant_length;

  auto ramp_fn = [=](int i, int ramp_length, double start_val, double end_val) {
    if (i < start_vary_pos) {
      return start_val;
    }
    if (i < start_vary_pos + ramp_length) {
      return start_val + static_cast<double>(i - start_vary_pos) / ramp_length *
                             (end_val - start_val);
    }
    return end_val;
  };

  const double start_w0 = start_cutoff * 2 * M_PI / sampling_rate;
  const double end_w0 = end_cutoff * 2 * M_PI / sampling_rate;
  auto vary_freq_fn = [=](int i, int ramp_length = 0) {
    return ramp_fn(i, ramp_length, start_w0, end_w0);
  };

  const double start_gain = -4, end_gain = 4;
  const double w0 = cycles_per_second * 2 * M_PI / sampling_rate;
  auto vary_gain_fn = [=](int i, int ramp_length = 0) {
    return ramp_fn(i, ramp_length, start_gain, end_gain);
  };

  const double start_q = 0.6, end_q = 4;
  auto vary_q_fn = [=](int i, int ramp_length = 0) {
    return ramp_fn(i, ramp_length, start_q, end_q);
  };

  auto input_signal =
      generate_sine_wave(length, dc_input ? 0 : cycles_per_sample);

  map<string, function<array<double, 5>(int i)>> filter_types = {
      {"lowpass_freq",
       [=](int i) { return lowpass_coefficients(vary_freq_fn(i), q); }},
      {"lowpass_q",
       [=](int i) { return lowpass_coefficients(w0, vary_q_fn(i)); }},
      {"peaking_freq", [=](int i) {
        return peaking_coefficients(vary_freq_fn(i), q, end_gain);
      }},
      {"peaking_gain",
       [=](int i) { return peaking_coefficients(w0, q, vary_gain_fn(i)); }},
      {"peaking_q", [=](int i) {
        return peaking_coefficients(end_w0, vary_q_fn(i), end_gain);
      }},
  };

  auto filter_iter = filter_types.find(argv[1]);
  if (filter_iter == end(filter_types)) {
    cerr << "Invalid filter type \"" << argv[1] << "\". Types:\n";
    for (const auto& p : filter_types) {
      cerr << "\t" << p.first << endl;
    }
    return -1;
  }
  auto coeffs_fn = filter_iter->second;

  vector<array<double, 5>> coeffs(length);
  for (int i = 0; i < length; ++i) {
    coeffs[i] = coeffs_fn(i);
  }

  typedef function<vector<double>(vector<double>,
                                  const vector<array<double, 5>>&)> filter_fn;
  map<string, filter_fn> methods = {
      {"df2", &time_varying_df2},
      {"gr", &gold_rader},
      {"svf", &time_varying_svf},
      {"svf_r", &svf_rabenstein},
      {"svf_rc", &svf_rabenstein_czarnach},
      {"tdf2", &tdf2},
      {"tdf2_rc", &tdf2_rabenstein_czarnach},
      {"zz", &output_switching_filter},
      {"reference",
       [=](vector<double> x, const vector<array<double, 5>>& coeffs) {
        return reference(std::move(x), coeffs, w0,
                         static_cast<int>(sampling_rate * 0.01));
      }},
      {
       "anchor", [=](vector<double> x, const vector<array<double, 5>>& coeffs) {
                   return anchor(std::move(x), coeffs, w0);
                 },
      }};

  // Reference and anchor aren't needed for testing DC input.
  if (dc_input) {
    methods.erase("reference");
    methods.erase("anchor");
  }

  // Process with each filter structure.
  map<string, vector<double>> outputs;
  transform(begin(methods), end(methods), inserter(outputs, outputs.end()),
            [&](pair<string, filter_fn> cur_method) {
    return make_pair(cur_method.first, cur_method.second(input_signal, coeffs));
  });

  // Normalize peak levels.
  double max_level =
      accumulate(begin(outputs), end(outputs), 0.0,
                 [](double cur, const pair<string, vector<double>>& p) {
        double new_max_level =
            accumulate(begin(p.second), end(p.second), 0.0,
                       [](double cur, double x) { return max(cur, abs(x)); });
        return max(cur, new_max_level);
      });
  for (auto& p : outputs) {
    transform(begin(p.second), end(p.second), begin(p.second),
              [max_level](double x) { return x / max_level; });
  }

  // Apply fade-in and fade-out, so the signal starting and ending doesn't
  // distract from the filter transition.
  // This only gets done for sinusoidal input, since it's used for the
  // subjective tests. For the objective tests, the transition doesn't matter
  // because there's no listening component.
  if (!dc_input) {
    auto fade_samples = static_cast<int>(sampling_rate * 0.05 + 0.5);
    auto hann_window = generate_hann_window(2 * fade_samples);
    hann_window.resize(fade_samples);
    for (auto& p : outputs) {
      auto& xs = p.second;
      transform(begin(xs), begin(xs) + fade_samples, begin(hann_window),
                begin(xs), std::multiplies<double>());
      transform(end(xs) - fade_samples, end(xs), begin(hann_window),
                end(xs) - fade_samples,
                [](double x, double w) { return x * (1.0 - w); });
    }
  }

  // Write the output.
  const int channel_count = 2;
  for (auto& output : outputs) {
    auto output_path = output_dir + "/" + output.first + ".wav";
    write_wav(output_path.c_str(), sampling_rate, channel_count, output.second);
  }
}


#include <CGAL/Simple_cartesian.h>

#include <CGAL/Surface_mesh.h>

#include <CGAL/Surface_mesh_parameterization/IO/File_off.h>
#include <CGAL/Surface_mesh_parameterization/Circular_border_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_authalic_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Error_code.h>
#include <CGAL/Surface_mesh_parameterization/parameterize.h>

#include <CGAL/Polygon_mesh_processing/measure.h>

#include <boost/foreach.hpp>

#include <cstdlib>
#include <iostream>
#include <fstream>

typedef CGAL::Simple_cartesian<double>       Kernel;
typedef Kernel::Point_2                      Point_2;
typedef Kernel::Point_3                      Point_3;
typedef CGAL::Surface_mesh<Kernel::Point_3>  SurfaceMesh;

typedef boost::graph_traits<SurfaceMesh>::halfedge_descriptor  halfedge_descriptor;
typedef boost::graph_traits<SurfaceMesh>::vertex_descriptor    vertex_descriptor;
typedef boost::graph_traits<SurfaceMesh>::face_descriptor      face_descriptor;

namespace SMP = CGAL::Surface_mesh_parameterization;

int main(int argc, char * argv[])
{
  std::ifstream in((argc>1) ? argv[1] : "data/three_peaks.off");
  if(!in) {
    std::cerr << "Problem loading the input data" << std::endl;
    return 1;
  }

  SurfaceMesh sm;
  in >> sm;

  // A halfedge on the border
  halfedge_descriptor bhd = CGAL::Polygon_mesh_processing::longest_border(sm).first;

  // The 2D points of the uv parametrisation will be written into this map
  typedef SurfaceMesh::Property_map<vertex_descriptor, Point_2>  UV_pmap;
  UV_pmap uv_map = sm.add_property_map<vertex_descriptor, Point_2>("v:uv").first;

  typedef SMP::Circular_border_arc_length_parameterizer_3<SurfaceMesh>  Border_parameterizer;
  typedef SMP::Discrete_authalic_parameterizer_3<SurfaceMesh, Border_parameterizer> Parameterizer;

  SMP::Error_code err = SMP::parameterize(sm, Parameterizer(), bhd, uv_map);

  if(err != SMP::OK) {
    std::cerr << "Error: " << SMP::get_error_message(err) << std::endl;
    return 1;
  }

  std::ofstream out("result.off");
  SMP::IO::output_uvmap_to_off(sm, bhd, uv_map, out);

  return 0;
}


// Copyright (c) 2009 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
// You can redistribute it and/or modify it under the terms of the GNU
// General Public License as published by the Free Software Foundation,
// either version 3 of the License, or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
//
// Author(s)     : Stéphane Tayeb
//
//******************************************************************************
// File Description :
// Outputs to out.mesh a mesh of implicit domains. These domains are defined
// by a vector of functions. Each n-uplet of sign of function values defines a
// subdomain.
//******************************************************************************

#include "debug.h"
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>

#include <CGAL/Implicit_to_labeling_function_wrapper.h>
#include <CGAL/Labeled_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>
#include "../examples/Mesh_3/implicit_functions.h"

#include <CGAL/Mesh_3/Mesh_global_optimizer.h>

// IO
#include <CGAL/IO/File_medit.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

using namespace CGAL::parameters;

// Domain
struct K: public CGAL::Exact_predicates_inexact_constructions_kernel {};
typedef FT_to_point_function_wrapper<K::FT, K::Point_3> Function;
typedef CGAL::Implicit_multi_domain_to_labeling_function_wrapper<Function>
                                                        Function_wrapper;
typedef Function_wrapper::Function_vector Function_vector;
typedef CGAL::Labeled_mesh_domain_3<Function_wrapper, K> Mesh_domain;

// Triangulation
typedef CGAL::Mesh_triangulation_3<Mesh_domain>::type Tr;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr> C3t3;

// Mesh Criteria
typedef CGAL::Mesh_criteria_3<Tr> Mesh_criteria;
typedef Mesh_criteria::Facet_criteria    Facet_criteria;
typedef Mesh_criteria::Cell_criteria     Cell_criteria;

template <typename T>
double set_arg(const std::string& param_name,
               const std::string& param_string,
               const po::variables_map& vm)
{
  T param_value(0);

  if ( vm.count(param_name) )
  {
    param_value = vm[param_name].as<T>();
    std::cout << param_string << ": " << param_value << "\n";
  }
  else
  {
    std::cout << param_string << " ignored.\n";
  }

  return param_value;
}

void set_function(Function_vector& v,
                  Function& f,
                  const std::string& function_name,
                  const po::variables_map& vm)
{
  if ( vm.count(function_name) )
  {
    v.push_back(f);
    std::cout << function_name << " ";
  }
}

int main(int argc, char* argv[])
{
  po::options_description generic("Generic options");
  generic.add_options() ("help", "Produce help message");

  po::options_description mesh("Mesh generation parameters");
  mesh.add_options()("facet_angle", po::value<double>(), "Set facet angle bound")
  ("facet_size", po::value<double>(), "Set facet size bound")
  ("facet_error", po::value<double>(), "Set facet approximation error bound")
  ("tet_shape", po::value<double>(), "Set tet radius-edge bound")
  ("tet_size", po::value<double>(), "Set tet size bound");

  po::options_description functions("Implicit functions");
  functions.add_options()("torus", "Mesh torus function")
  ("sphere", "Mesh sphere function")
  ("chair", "Mesh chair function")
  ("tanglecube", "Mesh tanglecube function")
  ("cube", "Mesh cube function")
  ("ellipsoid", "Mesh ellipsoid function")
  ("heart", "Mesh heart function")
  ("octic", "Mesh octic function");

  po::options_description desc("Options");
  desc.add_options()
  ("exude", po::value<double>(), "Exude mesh after refinement. arg is time_limit.")
  ("perturb", po::value<double>(), "Perturb (sliver removal) mesh after refinement. arg is time_limit.")
  ("lloyd", po::value<int>(), "Lloyd-smoothing after refinement. arg is max_iteration_nb")
  ("odt", po::value<int>(), "ODT-smoothing after refinement. arg is max_iteration_nb")
  ("convergence", po::value<double>()->default_value(0.02), "Convergence ratio for smoothing functions")
  ("min_displacement", po::value<double>()->default_value(0.01), "Minimal displacement ratio for smoothing functions (moves that are below that ratio will not be done)")
  ("time_limit", po::value<double>()->default_value(0), "Max time for smoothing functions")
  ("no_label_rebind", "Don't rebind cell labels in medit output")
  ("show_patches", "Show surface patches in medit output");

  po::options_description cmdline_options("Usage",1);
  cmdline_options.add(generic).add(mesh).add(functions).add(desc);

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
  po::notify(vm);

  if (vm.count("help") || argc < 2)
  {
    std::cout << cmdline_options << std::endl;
    return 1;
  }

  std::cout << "=========== Params ==========="<< std::endl;

  double facet_angle = set_arg<double>("facet_angle","Facet angle",vm);
  double facet_size = set_arg<double>("facet_size","Facet size",vm);
  double facet_error = set_arg<double>("facet_error","Facet approximation error",vm);

  double tet_shape = set_arg<double>("tet_shape","Tet shape (radius-edge)",vm);
  double tet_size = set_arg<double>("tet_size","Tet size",vm);

  // Define functions
  Function f1(&torus_function);
  Function f2(&sphere_function<3>);
  Function f3(&chair_function);
  Function f4(&tanglecube_function);
  Function f5(&cube_function);
  Function f6(&ellipsoid_function);
  Function f7(&heart_function);
  Function f8(&octic_function);

  Function_vector v;

  std::cout << "\nFunction(s): ";

  set_function(v,f1,"torus",vm);
  set_function(v,f2,"sphere",vm);
  set_function(v,f3,"chair",vm);
  set_function(v,f4,"tanglecube",vm);
  set_function(v,f5,"cube",vm);
  set_function(v,f6,"ellipsoid",vm);
  set_function(v,f7,"heart",vm);
  set_function(v,f8,"octic",vm);

  std::cout << "\n=============================="<< std::endl;
  std::cout << std::endl;

  if ( v.empty() )
  {
    std::cout << "No function set. Exit.\n";
    return 0;
  }

  // Domain (Warning: Sphere_3 constructor uses square radius !)
  Mesh_domain domain(v, K::Sphere_3(CGAL::ORIGIN, 7.*7.), 1e-8);

  // Set mesh criteria
  Facet_criteria facet_criteria(facet_angle, facet_size, facet_error); // angle, size, approximation
  Cell_criteria cell_criteria(tet_shape, tet_size); // radius-edge ratio, size
  Mesh_criteria criteria(facet_criteria, cell_criteria);

  // Mesh generation
  C3t3 c3t3 = CGAL::make_mesh_3<C3t3>(domain, criteria, no_exude(), no_perturb());

  // Odt
  if (  vm.count("odt") )
  {
    CGAL::odt_optimize_mesh_3(c3t3, domain,
                              max_iteration_number=vm["odt"].as<int>(),
                              convergence=vm["convergence"].as<double>(),
                              sliver_bound=vm["min_displacement"].as<double>(),
                              time_limit=vm["time_limit"].as<double>());
  }

  // Lloyd
  if ( vm.count("lloyd") )
  {
    CGAL::lloyd_optimize_mesh_3(c3t3, domain,
                                max_iteration_number=vm["lloyd"].as<int>(),
                                convergence=vm["convergence"].as<double>(),
                                sliver_bound=vm["min_displacement"].as<double>(),
                                time_limit=vm["time_limit"].as<double>());
  }

  // Perturbation
  if ( vm.count("perturb") )
  {
    CGAL::perturb_mesh_3(c3t3, domain, time_limit = vm["perturb"].as<double>() );
  }

  // Exudation
  if ( vm.count("exude") )
  {
    CGAL::exude_mesh_3(c3t3, time_limit = vm["exude"].as<double>());
  }

  double min_angle = 181.;
  for ( C3t3::Cell_iterator cit = c3t3.cells_begin() ;
       cit != c3t3.cells_end() ;
       ++cit )
  {
    min_angle = (std::min)(min_angle,
                           CGAL::to_double(CGAL::Mesh_3::minimum_dihedral_angle(c3t3.triangulation().tetrahedron(cit))));
  }

  std::cerr << "Min angle: " << min_angle << std::endl;

  // Output
  std::ofstream medit_file("out.mesh");
  CGAL::output_to_medit(medit_file, c3t3, !vm.count("no_label_rebind"), vm.count("show_patches"));

  return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2015 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-2
 */

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>

#include <fstream>

using namespace dealii;

void
make_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 5);

  static const SphericalManifold<2> manifold_description(center);
  triangulation.set_all_manifold_ids(0);
  triangulation.set_manifold(0, manifold_description);

  for (unsigned int step = 0; step < 3; ++step)
    {
      Triangulation<2>::active_cell_iterator cell =
                                               triangulation.begin_active(),
                                             endc = triangulation.end();

      for (; cell != endc; ++cell)
        for (unsigned int v = 0; v < GeometryInfo<2>::vertices_per_cell; ++v)
          {
            const double distance_from_center =
              center.distance(cell->vertex(v));

            if (std::fabs(distance_from_center - inner_radius) < 1e-10)
              {
                cell->set_refine_flag();
                break;
              }
          }

      triangulation.execute_coarsening_and_refinement();
    }
}

void
make_square_grid(Triangulation<2> &triangulation)
{
  GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(3);
}

SparsityPattern
distribute_dofs(DoFHandler<2> &dof_handler)
{
  static const FE_Q<2> finite_element(1);
  dof_handler.distribute_dofs(finite_element);

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());

  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

  std::ofstream out("sparsity_pattern1.svg");
  sparsity_pattern.print_svg(out);

  return sparsity_pattern;
}

SparsityPattern
renumber_dofs(DoFHandler<2> &dof_handler)
{
  DoFRenumbering::Cuthill_McKee(dof_handler);

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

  std::ofstream out("sparsity_pattern2.svg");
  sparsity_pattern.print_svg(out);

  return sparsity_pattern;
}

void
row_lenths(const SparsityPattern &sparsity_pattern)
{
  for (unsigned int i = 0; i < sparsity_pattern.n_rows(); i++)
    {
      std::cout << "Row" << i << "- - row length "
                << sparsity_pattern.row_length(i) << std::endl;
    }
}

std::tuple<int, int, double, double>
compute_pattern_statistics(const SparsityPattern &sparsity_pattern)
{
  double avarage_per_row = 0.;
  for (unsigned int i = 0; i < sparsity_pattern.n_rows(); i++)
    {
      avarage_per_row += sparsity_pattern.row_length(i);
    }

  double fill_ratio = avarage_per_row / (double)(sparsity_pattern.n_rows() *
                                                 sparsity_pattern.n_cols());
  avarage_per_row /= (double)sparsity_pattern.n_rows();
  return std::make_tuple(sparsity_pattern.n_rows(),
                         sparsity_pattern.bandwidth(),
                         avarage_per_row,
                         fill_ratio);
}

int
main()
{
  Triangulation<2> triangulation;
  // make_grid(triangulation);
  make_square_grid(triangulation);

  DoFHandler<2> dof_handler(triangulation);

  auto sparcity_pattern  = distribute_dofs(dof_handler);
  auto sparcity_pattern2 = renumber_dofs(dof_handler);

  row_lenths(sparcity_pattern2);

  std::cout << sparcity_pattern.row_length(41) << std::endl;

  auto patern_statistics = compute_pattern_statistics(sparcity_pattern2);
  std::cout << std::get<0>(patern_statistics) << " "
            << std::get<1>(patern_statistics) << "  "
            << std::get<2>(patern_statistics) << "  "
            << std::get<3>(patern_statistics) << std::endl;
}


#pragma once

#include <cstdint>
#include <functional>
#include <limits>
#include <stdexcept>
#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace simple_dtw
{
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
class SimpleDTW
{
protected:
  void InitializeMatrix(const ssize_t first_sequence_size,
                        const ssize_t second_sequence_size)
  {
    const ssize_t rows = first_sequence_size + 1;
    const ssize_t cols = second_sequence_size + 1;
    if (dtw_matrix_.rows() < rows || dtw_matrix_.cols() < cols)
    {
      dtw_matrix_ = Eigen::MatrixXd::Zero(rows, cols);
      if (rows > 1 && cols > 1)
      {
        for (ssize_t row = 1; row < rows; row++)
        {
          dtw_matrix_(row, 0) = std::numeric_limits<double>::infinity();
        }
        for (ssize_t col = 1; col < cols; col++)
        {
          dtw_matrix_(0, col) = std::numeric_limits<double>::infinity();
        }
      }
    }
  }

  Eigen::MatrixXd dtw_matrix_;

public:
  SimpleDTW()
  {
    InitializeMatrix(0, 0);
  }

  SimpleDTW(const ssize_t first_sequence_size,
            const ssize_t second_sequence_size)
  {
    InitializeMatrix(first_sequence_size, second_sequence_size);
  }

  double EvaluateWarpingCost(
      const FirstContainer& first_sequence,
      const SecondContainer& second_sequence,
      const std::function<double(const FirstDatatype&,
                                 const SecondDatatype&)>& distance_fn)
  {
    if (first_sequence.empty())
    {
      throw std::invalid_argument("first_sequence is empty");
    }
    if (second_sequence.empty())
    {
      throw std::invalid_argument("second_sequence is empty");
    }
    const ssize_t first_sequence_size
        = static_cast<ssize_t>(first_sequence.size());
    const ssize_t second_sequence_size
        = static_cast<ssize_t>(second_sequence.size());
    InitializeMatrix(first_sequence_size, second_sequence_size);
    //Compute DTW cost for the two sequences
    for (ssize_t i = 1; i <= first_sequence_size; i++)
    {
      const FirstDatatype& first_item
          = first_sequence[static_cast<size_t>(i) - 1];
      for (ssize_t j = 1; j <= second_sequence_size; j++)
      {
        const SecondDatatype& second_item
            = second_sequence[static_cast<size_t>(j) - 1];
        const double index_cost = distance_fn(first_item, second_item);
        double prev_cost = 0.0;
        // Get the next neighboring values from the matrix to use for the update
        double im1j = dtw_matrix_(i - 1, j);
        double im1jm1 = dtw_matrix_(i - 1, j - 1);
        double ijm1 = dtw_matrix_(i, j - 1);
        // Start the update step
        if (im1j < im1jm1 && im1j < ijm1)
        {
          prev_cost = im1j;
        }
        else if (ijm1 < im1j && ijm1 < im1jm1)
        {
          prev_cost = ijm1;
        }
        else
        {
          prev_cost = im1jm1;
        }
        // Update the value in the matrix
        const double new_cost = index_cost + prev_cost;
        dtw_matrix_(i, j) = new_cost;
      }
    }
    //Return total path cost
    const double warping_cost
        = dtw_matrix_(first_sequence_size, second_sequence_size);
    return warping_cost;
  }
};

template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double EvaluateWarpingCost(
    const FirstContainer& first_sequence,
    const SecondContainer& second_sequence,
    const std::function<double(const FirstDatatype&,
                               const SecondDatatype&)>& distance_fn)
{
  SimpleDTW<FirstDatatype, SecondDatatype,
            FirstContainer, SecondContainer> dtw_evaluator;
  return dtw_evaluator.EvaluateWarpingCost(
      first_sequence, second_sequence, distance_fn);
}
}  // namespace simple_dtw
}  // namespace common_robotics_utilities


#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <random>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>
#include <time.h>

// this is an example in how to use a class in one executable using full namespaces

// file that defines the class you want to use
#include "calibration/Calibration.h"

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

std::ofstream myfile;
myfile.open ("example.txt");
myfile << "iPos\ti_den\ti_times\t\terrorR\t\t\terrorT \n";

std::vector<double> comp_time_vec, vect1, vect2;
//int i_time = 0;

for (int i_5times = 1; i_5times <= 5; i_5times++) {

int i_pos = 1280;

	std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(-1, 1);

	// AX = ZB
	Eigen::Matrix4f Z = Eigen::Matrix4f::Identity();
	Eigen::Vector4f qz_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
	Eigen::Quaternionf qz( qz_use[0], qz_use[1], qz_use[2], qz_use[3] );
	qz.normalize();
	Eigen::Vector3f tz = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );
	Z.block<3,3>(0,0) = qz.toRotationMatrix();
	Z.block<3,1>(0,3) = tz;
	//std::cout << "Z: " << std::endl << Z << std::endl;

 	Eigen::Matrix4f X = Eigen::Matrix4f::Identity();;
	Eigen::Vector4f qx_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
	Eigen::Quaternionf qx( qx_use[0], qx_use[1], qx_use[2], qx_use[3] );
	qx.normalize();
	Eigen::Vector3f tx = Eigen::Vector3f(dis(gen),dis(gen), dis(gen));
	X.block<3,3>(0,0) = qx.toRotationMatrix();
	X.block<3,1>(0,3) = tx;
	//std::cout << "X: " << std::endl << X << std::endl;

 	int NPoses = i_pos;

	std::vector<Matrix4f> B, A;

	Eigen::Matrix4f a = Eigen::Matrix4f::Identity();

	Eigen::Vector4f q_use = Eigen::Vector4f::Random();
	Eigen::Quaternionf q( q_use[0], q_use[1], q_use[2], q_use[3]);
	q.normalize();

	Eigen::Vector3f t = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );

	a.block<3,3>(0,0) = q.toRotationMatrix();
	a.block<3,1>(0,3) = t;

	int den_test = 1024; // grid resolution

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

		// test data
		Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
		b = Z.inverse()*a*X;

		Eigen::Matrix4f d = Eigen::Matrix4f::Identity();

		Eigen::Vector3f t = 0.1*(1/den_test)*Eigen::Vector3f( dis(gen),dis(gen), dis(gen) );

		float roll = dis(gen)*M_PI/den_test;
		float pitch = dis(gen)*M_PI/den_test;
		float yaw = dis(gen)*M_PI/den_test;
		Eigen::Matrix3f R;
		R = AngleAxisf(roll, Vector3f::UnitX())
			*AngleAxisf(pitch, Vector3f::UnitY())
  			*AngleAxisf(yaw, Vector3f::UnitZ());
		d.block<3,3>(0,0) = R;
		d.block<3,1>(0,3) = t;

		a = d*a;

		A.push_back(a);
		B.push_back(b);

	}

	//i_den it's the sampling fraction: pick one champion every i_den
	for (int i_den = 1; i_den <= 32; i_den *= 2) {

	std::cout << std::endl << "i_pos: " << i_pos << ", " << "i_den: " << i_den << ", " << std::endl << "i_5times: " << i_5times << std::endl;

	std::vector<Matrix4f> Asampled, Bsampled; //A, B sampled
	Asampled.clear();
	Bsampled.clear();

	//fulfilling matrices A,B with choosen champions, limit per matrix sampled is NPoses (total available) over the sampling fraction i_den

	for (int i_sam = 0; i_sam < NPoses/i_den; i_sam +=i_den){
		Asampled.push_back(A[i_sam]);
		Bsampled.push_back(B[i_sam]);
	}

	// first, create an object of your class
	calibration::Calibration mycalib;

	mycalib.setInput(Asampled, Bsampled);

	clock_t t1,t2;
    t1=clock();
    vect1.push_back((float(t1)*1000)/CLOCKS_PER_SEC);

	mycalib.computeClosedForm();

	t2=clock();
	vect2.push_back((float(t2)*1000)/CLOCKS_PER_SEC);
    float diff ((float)t2-(float)t1);
    comp_time_vec.push_back( (diff*10000) / CLOCKS_PER_SEC );
    //i_time++;
	//sleep(2);

	mycalib.computeNonLinOpt();

	Eigen::Matrix4f Tx, Tz;
	float Er, Et;
	Tx = Eigen::Matrix4f::Identity();
	Tz = Eigen::Matrix4f::Identity();
	mycalib.getFullOutput(Tz, Tx, Er, Et);

	myfile << i_pos << "\t\t" << i_den << "\t\t" << i_5times << "\t\t" << Er << "\t\t" <<  Et << "\t\t" << "\n";

	// print out the result

	std::cout << "combo: " << i_pos << " / " << i_den << " / " << i_5times << std::endl;

	std::cout << "Input Z: " << std::endl << Z << std::endl;
	std::cout << "Estimated Z:" << std::endl << Tz << std::endl;
	std::cout << "Error over Z estimation: " << std::endl << Z-Tz << std::endl << std::endl;

	std::cout << "Input X: " << std::endl << X << std::endl;
	std::cout << "Estimated X:" << std::endl << Tx << std::endl;
	std::cout << "Error over X estimation: " << std::endl << X-Tx << std::endl;

// compute Rotation and translation errors over Non Linear results

	/*Eigen::Matrix3f RxOK_NL, RzOK_NL;
	Eigen::Vector3f txOK_NL, tzOK_NL;

	RxOK_NL = Tx.block<3,3>(0,0);
	RzOK_NL = Tz.block<3,3>(0,0);
	txOK_NL = Tx.block<3,1>(0,3);
	tzOK_NL = Tz.block<3,1>(0,3);

	float Er_NL = 0;

	float EtNum_NL = 0, EtDen_NL = 1;

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

		Er_NL += Matrix3f( A[i].block<3,3>(0,0)*RxOK_NL - RzOK_NL*B[i].block<3,3>(0,0) ).squaredNorm();

		EtDen_NL += Vector3f( A[i].block<3,3>(0,0)*txOK_NL - A[i].block<3,1>(0,3) ).squaredNorm();

		EtNum_NL += Vector3f( A[i].block<3,3>(0,0)*txOK_NL + A[i].block<3,1>(0,3) - RzOK_NL*B[i].block<3,1>(0,3) - tzOK_NL ).squaredNorm();

	}

	float Et_NL = sqrt(EtNum_NL/EtDen_NL);

	std::cout << "Rotation error on Non Linear form is " << Er_NL << std::endl;
	std::cout << "Translation error on Non Linear form is " << Et_NL << std::endl;
	std::cout << "combo " << NPoses << "/ " << den_test << "/ " << i_5times << std::endl;*/

}
}
// chiusura dei tre for
std::cout << "closed form time computation in microseconds :"<< std::endl;
  for (unsigned i=0; i<comp_time_vec.size(); i++)
    std::cout << "i " << i<< " diff " << comp_time_vec[i] << " t1 " << vect1[i] <<" t2 " << vect2[i] << std::endl;
  std::cout << '\n';
//std::cout<< "closed form time computation in milliseconds " << std::endl << comp_time_vec << std::endl;

myfile.close();

return 0;

}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2012 Scientific Computing and Imaging Institute,
   University of Utah.

   License for the specific language governing rights and limitations under
   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 <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Math/SolveLinearSystemWithEigen.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Eigen/Sparse>

using namespace SCIRun::Core::Algorithms::Math;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core;

namespace
{
  class SolveLinearSystemAlgorithmEigenCGImpl
  {
  public:
    SolveLinearSystemAlgorithmEigenCGImpl(const DenseColumnMatrix& rhs, double tolerance, int maxIterations) :
        rhs_(rhs), tolerance_(tolerance), maxIterations_(maxIterations) {}

    template <class MatrixType>
    DenseColumnMatrix::EigenBase solveWithEigen(const MatrixType& lhs)
    {
      Eigen::ConjugateGradient<typename MatrixType::EigenBase> cg;
      cg.compute(lhs);

      if (cg.info() != Eigen::Success)
        BOOST_THROW_EXCEPTION(AlgorithmInputException()
          << LinearAlgebraErrorMessage("Conjugate gradient initialization was unsuccessful")
          << EigenComputationInfo(cg.info()));

      cg.setTolerance(tolerance_);
      cg.setMaxIterations(maxIterations_);
      auto solution = cg.solve(rhs_).eval();
      tolerance_ = cg.error();
      maxIterations_ = cg.iterations();
      return solution;
    }

    double tolerance_;
    int maxIterations_;
  private:
    const DenseColumnMatrix& rhs_;
  };
}

SolveLinearSystemAlgorithm::Outputs SolveLinearSystemAlgorithm::run(const Inputs& input, const Parameters& params) const
{
  auto A = input.get<0>();
  ENSURE_ALGORITHM_INPUT_NOT_NULL(A, "Null input matrix");

  auto b = input.get<1>();
  ENSURE_ALGORITHM_INPUT_NOT_NULL(b, "Null rhs vector");

  double tolerance = params.get<0>();
  ENSURE_POSITIVE_DOUBLE(tolerance, "Tolerance out of range!");

  int maxIterations = params.get<1>();
  ENSURE_POSITIVE_INT(maxIterations, "Max iterations out of range!");

  SolveLinearSystemAlgorithmEigenCGImpl impl(*b, tolerance, maxIterations);
  DenseColumnMatrix x;
  if (matrix_is::dense(A))
  {
    x = impl.solveWithEigen(*matrix_cast::as_dense(A));
  }
  else if (matrix_is::sparse(A))
  {
    x = impl.solveWithEigen(*matrix_cast::as_sparse(A));
  }
  else
    BOOST_THROW_EXCEPTION(AlgorithmProcessingException() << ErrorMessage("solveWithEigen can only handle dense and sparse matrices."));

  if (x.size() != 0)
  {
    /// @todo: move ctor
    DenseColumnMatrixHandle solution(boost::make_shared<DenseColumnMatrix>(x));
    return SolveLinearSystemAlgorithm::Outputs(solution, impl.tolerance_, impl.maxIterations_);
  }
  else
    BOOST_THROW_EXCEPTION(AlgorithmProcessingException() << ErrorMessage("solveWithEigen produced an empty solution."));
}

AlgorithmOutput SolveLinearSystemAlgorithm::run_generic(const AlgorithmInput& input) const
{
  throw 2;
}


/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2) ICP algorithms in C++
 * Copyright (C) 2018-2019 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   optimal_tf_gauss_newton.cpp
 * @brief  Simple non-linear optimizer to find the SE(3) optimal transformation
 * @author Jose Luis Blanco Claraco
 * @date   Jun 16, 2019
 */

#include <mp2p_icp/optimal_tf_gauss_newton.h>
#include <mrpt/poses/Lie/SE.h>
#include <Eigen/Dense>
#include <iostream>

using namespace mp2p_icp;

void mp2p_icp::optimal_tf_gauss_newton(
    const Pairings_GaussNewton& in, OptimalTF_Result& result)
{
    using std::size_t;

    MRPT_START

    // Run Gauss-Newton steps, using SE(3) relinearization at the current
    // solution:
    result.optimal_pose = in.initial_guess;

    const auto nPt2Pt = in.paired_points.size();
    const auto nPt2Pl = in.paired_pt2pl.size();
    const auto nPl2Pl = in.paired_planes.size();

    const auto nErrorTerms = (nPt2Pt + nPl2Pl) * 3 + nPt2Pl;

    Eigen::VectorXd                          err(nErrorTerms);
    Eigen::Matrix<double, Eigen::Dynamic, 6> J(nErrorTerms, 6);

    double       w_pt = in.weight_point2point;
    const double w_pl = in.weight_point2plane, w_pl2pl = in.weight_plane2plane;

    const bool  has_per_pt_weight       = !in.point_weights.empty();
    auto        cur_point_block_weights = in.point_weights.begin();
    std::size_t cur_point_block_start   = 0;

    MRPT_TODO("Implement robust Kernel in this solver");

    for (size_t iter = 0; iter < in.max_iterations; iter++)
    {
        // (12x6 Jacobian)
        const auto dDexpe_de =
            mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(result.optimal_pose);

        // Point-to-point:
        for (size_t idx_pt = 0; idx_pt < nPt2Pt; idx_pt++)
        {
            // Error:
            const auto&  p  = in.paired_points[idx_pt];
            const double lx = p.other_x, ly = p.other_y, lz = p.other_z;
            double       gx, gy, gz;
            result.optimal_pose.composePoint(lx, ly, lz, gx, gy, gz);
            err[idx_pt * 3 + 0] = gx - p.this_x;
            err[idx_pt * 3 + 1] = gy - p.this_y;
            err[idx_pt * 3 + 2] = gz - p.this_z;

            // Eval Jacobian:
            // clang-format off
            const Eigen::Matrix<double, 3, 12> J1 =
                (Eigen::Matrix<double, 3, 12>() <<
                   lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1,  0,  0,
                    0, lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1,  0,
                    0,  0, lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1
                 ).finished();
            // clang-format on

            // Get weight:
            if (has_per_pt_weight)
            {
                if (idx_pt >=
                    cur_point_block_start + cur_point_block_weights->first)
                {
                    ASSERT_(cur_point_block_weights != in.point_weights.end());
                    ++cur_point_block_weights;  // move to next block
                    cur_point_block_start = idx_pt;
                }
                w_pt = cur_point_block_weights->second;
            }

            // Build Jacobian:
            J.block<3, 6>(idx_pt * 3, 0) = w_pt * J1 * dDexpe_de.asEigen();
        }

        // Point-to-plane:
        auto base_idx = nPt2Pt * 3;
        for (size_t idx_pl = 0; idx_pl < nPt2Pl; idx_pl++)
        {
            // Error:
            const auto& p = in.paired_pt2pl[idx_pl];

            const double lx = p.pt_other.x, ly = p.pt_other.y,
                         lz = p.pt_other.z;
            mrpt::math::TPoint3D g;
            result.optimal_pose.composePoint(lx, ly, lz, g.x, g.y, g.z);

            err(idx_pl + base_idx) = p.pl_this.plane.evaluatePoint(g);

            // Eval Jacobian:
            // clang-format off
            const Eigen::Matrix<double, 3, 12> J1 =
                (Eigen::Matrix<double, 3, 12>() <<
                   lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1,  0,  0,
                    0, lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1,  0,
                    0,  0, lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1
                 ).finished();
            // clang-format on

            const Eigen::Matrix<double, 1, 3> Jpl =
                (Eigen::Matrix<double, 1, 3>() << p.pl_this.plane.coefs[0],
                 p.pl_this.plane.coefs[1], p.pl_this.plane.coefs[2])
                    .finished();

            const Eigen::Matrix<double, 1, 6> Jb =
                Jpl * J1 * dDexpe_de.asEigen();

            J.block<1, 6>(idx_pl + base_idx, 0) = w_pl * Jb;
        }

        // Plane-to-plane (only direction of normal vectors):
        base_idx += nPt2Pl * 1;
        for (size_t idx_pl = 0; idx_pl < nPl2Pl; idx_pl++)
        {
            // Error term:
            const auto& p = in.paired_planes[idx_pl];

            const auto nl = p.p_other.plane.getNormalVector();
            const auto ng = p.p_this.plane.getNormalVector();

            const auto p_oplus_nl = result.optimal_pose.rotateVector(nl);

            for (int i = 0; i < 3; i++)
                err(i + idx_pl * 3 + base_idx) = ng[i] - p_oplus_nl[i];

            // Eval Jacobian:

            // df_oplus(A,p)/d_A. Section 7.3.2 tech. report:
            // "A tutorial on SE(3) transformation parameterizations and
            // on-manifold optimization"
            // Modified, to discard the last I_3 block, since this particular
            // cost function is insensible to translations.

            // clang-format off
            const Eigen::Matrix<double, 3, 12> J1 =
                (Eigen::Matrix<double, 3, 12>() <<
                   nl.x,  0,  0,  nl.y,  0,  0, nl.z,  0,  0,  0,  0,  0,
                    0, nl.x,  0,  0,  nl.y,  0,  0, nl.z,  0,  0,  0,  0,
                    0,  0, nl.x,  0,  0,  nl.y,  0,  0, nl.z,  0,  0,  0
                 ).finished();
            // clang-format on

            J.block<3, 6>(3 * idx_pl + base_idx, 0) =
                w_pl2pl * J1 * dDexpe_de.asEigen();
        }

        // 3) Solve Gauss-Newton:
        const Eigen::VectorXd             g = J.transpose() * err;
        const Eigen::Matrix<double, 6, 6> H = J.transpose() * J;
        const Eigen::Matrix<double, 6, 1> delta =
            -H.colPivHouseholderQr().solve(g);

        // 4) add SE(3) increment:
        const auto dE = mrpt::poses::Lie::SE<3>::exp(
            mrpt::math::CVectorFixed<double, 6>(delta));

        result.optimal_pose = result.optimal_pose + dE;

        if (in.verbose)
        {
            std::cout << "[P2P GN] iter:" << iter << " err:" << err.norm()
                      << " delta:" << delta.transpose() << "\n";
        }

        // Simple convergence test:
        if (delta.norm() < in.min_delta) break;

    }  // for each iteration
    MRPT_END
}