#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Geometry>
#include <chrono>
#include "mpi.h"
#include <vector>
#include <random>

#include "General_Utilities.hpp"
#include "Physical_Parameters.hpp"
#include "IC_Generator.hpp"
#include "Trajectory_Simulation.hpp"
#include "PREM.hpp"
#include "RN_Generators.hpp"

using namespace std;
using namespace std::chrono;

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

//INITIALIZATION
////////////////////////////////////////////////////////////
	//MPI Enviroment
		// Initialize the MPI environment
	    	MPI_Init(NULL, NULL);
	    // Get the number of processes
	    	int numprocs;
	    	MPI_Comm_size(MPI_COMM_WORLD, &numprocs);
	    // Get the ID number of the process
	    	int myRank;
	    	MPI_Comm_rank(MPI_COMM_WORLD, &myRank);

	//Starting time
	    high_resolution_clock::time_point tStart,t1,t2,tEnd;
	    double durationIni=0.0;
	    double durationMain=0.0;
	    if(myRank==0)
	    {
	    	tStart = high_resolution_clock::now();
	    }
	//Read in configuration file, set up the detector and define the earth velocity
		Read_Config_File(argv[1]);
	//Initialize Logfile and create folders for inputfiles
		if(myRank==0)
		{
			Copy_Config_File(argv[1]);
			std::chrono::time_point<std::chrono::system_clock> start;
			start = std::chrono::system_clock::now();
			std::time_t start_time = std::chrono::system_clock::to_time_t(start);
			cout <<"\n##############################"<<endl
			<<"DaMaSCUS"<<version<<" - Simulation"<<endl<<endl
			<<"Starting Time: " <<std::ctime(&start_time)
			<<"Simulation ID: " <<SimID <<endl<<endl
			<<"Creating logfile." <<endl;
			LogFile_Initialize(numprocs);
		}
	//Initialize Random Number Generator:
		std::random_device rd;
		std::mt19937 PRNG(rd());
	//Synchronize processes:
		MPI_Barrier(MPI_COMM_WORLD);

////////////////////////////////////////////////////////////

	//1. Initial MC run without scatterings:
		//Initialize the earth model for initial run
			double sigmaInitial=1e-20*pb;
			Initialize_PREM(mChi,sigmaInitial);
		//Deactivate formfactor, if it is used:
			string formfactor0=FormFactor;
			FormFactor="None";
		if (myRank==0) cout <<"Start initial MC simulation run without DM scatterings." <<endl;
		//Desired Data Sample for inital and main MC simulation
			unsigned long long int Local_SampleSize_Initial =ceil((double)Global_SampleSize_Initial/numprocs);
			Global_SampleSize_Initial=Local_SampleSize_Initial*numprocs;
		//Particle counter per isodetection ring
			unsigned long long int Global_N0[Isodetection_Rings];
			unsigned long long int Local_N0[Isodetection_Rings];
			for (int i=0;i<Isodetection_Rings;i++)Local_N0[i]=0;
			long double Global_W0[Isodetection_Rings];
			long double Local_W0[Isodetection_Rings];
			for (int i=0;i<Isodetection_Rings;i++)Local_W0[i]=0.0;
			long double Global_W0sq[Isodetection_Rings];
			long double Local_W0sq[Isodetection_Rings];
			for (int i=0;i<Isodetection_Rings;i++)Local_W0sq[i]=0.0;

		//Depth crossing counter
			unsigned long long int Global_Counter_Crossings0=0;
			unsigned long long int Local_Counter_Crossings0=0;

		//Simulation of trajectories without scatterings and without formfactor implementation
			for(unsigned int i=0;i<Local_SampleSize_Initial;i++)
			{
				Event IC = InitialCondition(0,vEarth,PRNG);
				Trajectory trajectory=ParticleTrack(mChi,sigmaInitial,IC,vcut,PRNG);
				std::vector<Event> CrossingEvents=trajectory.DepthCrossing(Detector_Depth);
				Local_Counter_Crossings0+=CrossingEvents.size();
				//Increment the isodetection counters and record the weighted velocity norm.
				for (unsigned int j=0;j<CrossingEvents.size();j++)
				{
					int IsoRing=CrossingEvents[j].IsodetectionRing(Isodetection_Rings);
					Local_N0[IsoRing]++;

					double speed_ratio = IC.NormVelocity() / CrossingEvents[j].NormVelocity();
					double weight = CrossingEvents[j].DataWeight(speed_ratio);
					Local_W0[IsoRing]+=weight;
					Local_W0sq[IsoRing]+=weight*weight;
				}
			}
			//Reactivate form factor
				FormFactor=formfactor0;
		//Reduce the local isodetection counter
			MPI_Reduce(&Local_N0,&Global_N0,Isodetection_Rings,MPI_UNSIGNED_LONG_LONG,MPI_SUM,0,MPI_COMM_WORLD);
			MPI_Reduce(&Local_Counter_Crossings0,&Global_Counter_Crossings0,1,MPI_UNSIGNED_LONG_LONG,MPI_SUM,0,MPI_COMM_WORLD);
			MPI_Reduce(&Local_W0,&Global_W0,Isodetection_Rings,MPI_LONG_DOUBLE,MPI_SUM,0,MPI_COMM_WORLD);
			MPI_Reduce(&Local_W0sq,&Global_W0sq,Isodetection_Rings,MPI_LONG_DOUBLE,MPI_SUM,0,MPI_COMM_WORLD);

		//Status update on the console
			if(myRank==0)
			{
				//computing time
				t1 = high_resolution_clock::now();
				durationIni =1e-6*duration_cast<microseconds>( t1 - tStart ).count();
				cout <<"\tInitial run finished\t(" <<floor(durationIni) <<" s)." <<endl <<endl;
			}
	//2. MC run with scatterings and velocity data collection.
		//Initialize the earth model for the main run
			Initialize_PREM(mChi,sigma0);
		if (myRank==0) cout <<"Start main MC simulation run with scatterings." <<endl;
		//Particle counter per isodetection ring
			unsigned long long int Global_N[Isodetection_Rings];
			unsigned long long int Local_N[Isodetection_Rings];
			for (int i=0;i<Isodetection_Rings;i++)Local_N[i]=0;
		//Sum of data weights, necessary for determination of local DM number density
			long double Global_W[Isodetection_Rings];
			long double Local_W[Isodetection_Rings];
			for (int i=0;i<Isodetection_Rings;i++)Local_W[i]=0.0;
		//Sum of squared data weights, necessary for determination of the local DM number density uncertainty.
			long double Global_Wsq[Isodetection_Rings];
			long double Local_Wsq[Isodetection_Rings];
			for (int i=0;i<Isodetection_Rings;i++)Local_Wsq[i]=0.0;
		//Work Load Distribution: Desired Velocity Data Sample Size for each isodetection ring for each MPI process.
			unsigned int Local_SampleSize_Velocity=ceil((double)Global_SampleSize_Velocity/numprocs);
			Global_SampleSize_Velocity=Local_SampleSize_Velocity*numprocs;
		//Counter for simulated trajectories
			unsigned long long int Global_Counter_Tracks=0;
			unsigned long long int Local_Counter_Tracks=0;
		//Counter for scattering events
			unsigned long long int Global_Counter_Scatterings=0;
			unsigned long long int Local_Counter_Scatterings=0;
		//Counter for free particles:
			unsigned long long int Global_Counter_Free=0;
			unsigned long long int Local_Counter_Free=0;
		//Counter for depth crossing
			unsigned long long int Global_Counter_Crossings=0;
			unsigned long long int Local_Counter_Crossings=0;
		//Counter for velocity cutoffs
			unsigned long long int Global_Counter_vCutoff=0;
			unsigned long long int Local_Counter_vCutoff=0;
		//Local Counter for velocity data
			unsigned int Local_Counter_DatapointsTotal=0;
			unsigned int Local_Counter_Datapoints[Isodetection_Rings];
			for(int i=0;i<Isodetection_Rings;i++) Local_Counter_Datapoints[i]=0;

		//MPI Output File and Offset
			MPI_Offset offset;
 			MPI_File   file_Velocity[Isodetection_Rings];
 			MPI_File   file_Weights[Isodetection_Rings];
 		   	MPI_Status status;
 		//Velocity Output files including process dependent offset
 		   	for(int i=0;i<Isodetection_Rings;i++)
 		   	{
 		   		//Velocity output files
	 		   		string filename="../data/"+SimID+"_data/velocity."+std::to_string(i);
	 		   		int test=MPI_File_open(MPI_COMM_WORLD,const_cast<char*>(filename.c_str()),MPI_MODE_CREATE|MPI_MODE_EXCL|MPI_MODE_WRONLY,MPI_INFO_NULL, &file_Velocity[i]);
	 	   			//if it already exists, it will be overwritten!
		    			if(test != MPI_SUCCESS)
		    			{
		    				if (myRank == 0) MPI_File_delete(const_cast<char*>(filename.c_str()),MPI_INFO_NULL);
		    				//MPI_Barrier(MPI_COMM_WORLD);
		    				test=MPI_File_open(MPI_COMM_WORLD,const_cast<char*>(filename.c_str()),MPI_MODE_CREATE|MPI_MODE_EXCL|MPI_MODE_WRONLY,MPI_INFO_NULL, &file_Velocity[i]);
		    			}
		    		//Offset
		 		   		offset = myRank * Local_SampleSize_Velocity * sizeof(Eigen::Vector3d);
				 		MPI_File_seek(file_Velocity[i], offset,MPI_SEEK_SET);
			 	//Weights output files
			 		filename="../data/"+SimID+"_data/weights."+std::to_string(i);
	 		   		test=MPI_File_open(MPI_COMM_WORLD,const_cast<char*>(filename.c_str()),MPI_MODE_CREATE|MPI_MODE_EXCL|MPI_MODE_WRONLY,MPI_INFO_NULL, &file_Weights[i]);
	 	   			//if it already exists, it will be overwritten!
		    			if(test != MPI_SUCCESS)
		    			{
		    				if (myRank == 0) MPI_File_delete(const_cast<char*>(filename.c_str()),MPI_INFO_NULL);
		    				//MPI_Barrier(MPI_COMM_WORLD);
		    				test=MPI_File_open(MPI_COMM_WORLD,const_cast<char*>(filename.c_str()),MPI_MODE_CREATE|MPI_MODE_EXCL|MPI_MODE_WRONLY,MPI_INFO_NULL, &file_Weights[i]);
		    			}
		    		//Offset
	 		   			offset = myRank * Local_SampleSize_Velocity * sizeof(double);
			 			MPI_File_seek(file_Weights[i], offset,MPI_SEEK_SET);
 		   	}
 		//Simulation of Tracks
 		   	while(Local_Counter_DatapointsTotal<Isodetection_Rings*Local_SampleSize_Velocity)
 		   	{
 		   		//Simulate track.
					Event IC = InitialCondition(0,vEarth,PRNG);
 		   			Trajectory trajectory=ParticleTrack(mChi,sigma0,IC,vcut,PRNG);
 		   		//Increase counter of tracks and scatterings and vcutoff reachers
 		   			Local_Counter_Tracks++;
 		   			int scatterings = trajectory.NoOfScatterings();
 		   			if(scatterings==0) Local_Counter_Free++;
 		   			else Local_Counter_Scatterings+=scatterings;
 		   			if(trajectory.Trajectory_Type()==2)	Local_Counter_vCutoff++;
				//Where did the particle pass the isodetection rings.
					std::vector<Event> CrossingEvents=trajectory.DepthCrossing(Detector_Depth);
					Local_Counter_Crossings+=CrossingEvents.size();
					for (unsigned int j=0;j<CrossingEvents.size();j++)
					{
						//Which ring?
							int IsoRing=CrossingEvents[j].IsodetectionRing(Isodetection_Rings);
						//Increase the weight sums
							double speed_ratio = IC.NormVelocity() / CrossingEvents[j].NormVelocity();
							double weight = CrossingEvents[j].DataWeight(speed_ratio);
							Local_W[IsoRing]+=weight;
							Local_Wsq[IsoRing]+=weight*weight;
						//Increase the particle counter.
							Local_N[IsoRing]++;
						//If this isodetection ring is not finished collecting data, save the velocity and weight
							if(Local_Counter_Datapoints[IsoRing]<Local_SampleSize_Velocity)
							{
								//Save velocity.
									Eigen::Vector3d vel = CrossingEvents[j].Velocity();
								//Increase data counters
									Local_Counter_Datapoints[IsoRing]++;
									Local_Counter_DatapointsTotal++;
								//Save velocity and weights to file
									MPI_File_write(file_Velocity[IsoRing],vel.data(),vel.size(),MPI_DOUBLE,&status);
									MPI_File_write(file_Weights[IsoRing],&weight,1,MPI_DOUBLE,&status);
							}

					}
 		   	}
 		   	MPI_Barrier(MPI_COMM_WORLD);
 		//MPI Reductions
  	  		MPI_Reduce(&Local_Counter_Tracks,&Global_Counter_Tracks,1,MPI_UNSIGNED_LONG_LONG,MPI_SUM,0,MPI_COMM_WORLD);
  	  		MPI_Reduce(&Local_Counter_Scatterings,&Global_Counter_Scatterings,1,MPI_UNSIGNED_LONG_LONG,MPI_SUM,0,MPI_COMM_WORLD);
  	  		MPI_Reduce(&Local_Counter_vCutoff,&Global_Counter_vCutoff,1,MPI_UNSIGNED_LONG_LONG,MPI_SUM,0,MPI_COMM_WORLD);
  	  		MPI_Reduce(&Local_Counter_Free,&Global_Counter_Free,1,MPI_UNSIGNED_LONG_LONG,MPI_SUM,0,MPI_COMM_WORLD);
  	  		MPI_Reduce(&Local_Counter_Crossings,&Global_Counter_Crossings,1,MPI_UNSIGNED_LONG_LONG,MPI_SUM,0,MPI_COMM_WORLD);
  	  		MPI_Reduce(&Local_N,&Global_N,Isodetection_Rings,MPI_UNSIGNED_LONG_LONG,MPI_SUM,0,MPI_COMM_WORLD);
			MPI_Reduce(&Local_W,&Global_W,Isodetection_Rings,MPI_LONG_DOUBLE,MPI_SUM,0,MPI_COMM_WORLD);
			MPI_Reduce(&Local_Wsq,&Global_Wsq,Isodetection_Rings,MPI_LONG_DOUBLE,MPI_SUM,0,MPI_COMM_WORLD);

  	  	//Calculate DM Densities
  	  		vector<vector<double>> Edensity=DM_EnergyDensity(Global_W0,Global_SampleSize_Initial,Global_W,Global_Counter_Tracks,Global_W0sq,Global_Wsq,Isodetection_Rings);
  	  		vector<vector<double>> Ndensity=DM_NumberDensity(mChi,Edensity);
			double averageNdensity=DM_AverageDensity(Ndensity,Isodetection_Rings,Detector_Depth);
 	   	//Status update and save N(theta) and rho(theta)
 	   		if(myRank==0)
			{
				//Save density
					ofstream f;
					f.open("../data/"+SimID+".rho");
					for(int i=0;i<Isodetection_Rings;i++) f <<180.0 / Isodetection_Rings * i  <<"\t" <<InUnits(Edensity[i][0],GeV/cm/cm/cm)<<"\t" <<InUnits(Edensity[i][1],GeV/cm/cm/cm) <<endl;//<<"\t" <<InUnits(AverageVelocity_0[i],km/sec)<<"\t" <<InUnits(AverageVelocity[i],km/sec)<<endl;// <<"\t" <<Global_N0[i]<<"\t" <<Global_N[i] <<endl;
					f.close();
				//computing time
					t2 = high_resolution_clock::now();
					durationMain =1e-6*duration_cast<microseconds>( t2 - t1 ).count();
					cout <<"\tMain MC run finished\t" <<"("<<floor(durationMain) <<" s)." <<endl;
			}
		//Close all output files
		for(int i=0;i<Isodetection_Rings;i++)
		{
			MPI_File_close(&file_Velocity[i]);
			MPI_File_close(&file_Weights[i]);
		}
////////////////////////////////////////////////////////////

//Finalize simulation
////////////////////////////////////////////////////////////
    //Master process finishes logfile
		if(myRank==0)
		{
			//Ending time and computing time
				tEnd = high_resolution_clock::now();
				double durationTotal =1e-6*duration_cast<microseconds>( tEnd - tStart ).count();
				cout <<"\nProcessing Time:\t"<< durationTotal<<"s ("<< floor(durationTotal/3600.0)<<":"<<floor(fmod(durationTotal/60.0,60.0))<<":"<<floor(fmod(durationTotal,60.0))<<":"<<floor(fmod(1000*durationTotal,1000.0))<<")."<<endl
				<<"##############################"<<endl;
			//Finalize Logfile
				LogFile_Finalize(Global_Counter_Tracks,Global_Counter_Scatterings,Global_Counter_Free,Global_Counter_vCutoff,Global_SampleSize_Initial,Global_SampleSize_Velocity,Global_Counter_Crossings0,Global_Counter_Crossings,averageNdensity,durationIni,durationMain,durationTotal);
		}
	// Finalize the MPI environment.
	    MPI_Finalize();
    	return 0;
////////////////////////////////////////////////////////////
}


#include "kernel.h"
#include "structure.h"
#include "y_grad.h"
#include "sparse_gp.h"
#include "b2.h"
#include "b2_simple.h"
#include "b2_norm.h"
#include "b3.h"
#include "two_body.h"
#include "three_body.h"
#include "three_body_wide.h"
#include "four_body.h"
#include "squared_exponential.h"
#include "normalized_dot_product.h"
#include "norm_dot_icm.h"

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

#include <Eigen/Dense>
#include <vector>

namespace py = pybind11;

PYBIND11_MODULE(_C_flare, m) {
  // Structure
  py::class_<Structure>(m, "Structure")
      .def(py::init<const Eigen::MatrixXd &, const std::vector<int> &,
                    const Eigen::MatrixXd &>())
      .def(py::init<const Eigen::MatrixXd &, const std::vector<int> &,
                    const Eigen::MatrixXd &, double,
                    std::vector<Descriptor *>>())
      .def_readwrite("noa", &Structure::noa)
      .def_readwrite("cell", &Structure::cell)
      .def_readwrite("species", &Structure::species)
      .def_readwrite("positions", &Structure::positions)
      .def_readwrite("cell_transpose", &Structure::cell_transpose)
      .def_readwrite("wrapped_positions", &Structure::wrapped_positions)
      .def_readwrite("volume", &Structure::volume)
      .def_readwrite("energy", &Structure::energy)
      .def_readwrite("forces", &Structure::forces)
      .def_readwrite("stresses", &Structure::stresses)
      .def_readwrite("mean_efs", &Structure::mean_efs)
      .def_readwrite("variance_efs", &Structure::variance_efs)
      .def_readwrite("local_uncertainties", &Structure::local_uncertainties)
      .def_readonly("descriptors", &Structure::descriptors)
      .def_readonly("descriptor_calculators",
                    &Structure::descriptor_calculators)
      .def("compute_descriptors", &Structure::compute_descriptors)
      .def("wrap_positions", &Structure::wrap_positions)
      .def_static("to_json", &Structure::to_json)
      .def_static("from_json", &Structure::from_json);

  // Descriptor values
  py::class_<DescriptorValues>(m, "DescriptorValues")
      .def(py::init<>())
      .def_readwrite("n_descriptors", &DescriptorValues::n_descriptors)
      .def_readwrite("n_types", &DescriptorValues::n_types)
      .def_readwrite("n_atoms", &DescriptorValues::n_atoms)
      .def_readwrite("volume", &DescriptorValues::volume)
      .def_readwrite("descriptors", &DescriptorValues::descriptors)
      .def_readwrite("descriptor_force_dervs",
                     &DescriptorValues::descriptor_force_dervs)
      .def_readwrite("neighbor_coordinates",
                     &DescriptorValues::neighbor_coordinates)
      .def_readwrite("descriptor_norms", &DescriptorValues::descriptor_norms)
      .def_readwrite("descriptor_force_dots",
                     &DescriptorValues::descriptor_force_dots)
      .def_readwrite("cutoff_values", &DescriptorValues::cutoff_values)
      .def_readwrite("cutoff_dervs", &DescriptorValues::cutoff_dervs)
      .def_readwrite("neighbor_counts", &DescriptorValues::neighbor_counts)
      .def_readwrite("cumulative_neighbor_counts",
                     &DescriptorValues::cumulative_neighbor_counts)
      .def_readwrite("atom_indices", &DescriptorValues::atom_indices)
      .def_readwrite("neighbor_indices", &DescriptorValues::neighbor_indices)
      .def_readwrite("n_clusters_by_type",
                     &DescriptorValues::n_clusters_by_type)
      .def_readwrite("n_neighbors_by_type",
                     &DescriptorValues::n_neighbors_by_type);

  py::class_<ClusterDescriptor>(m, "ClusterDescriptor")
      .def_readonly("descriptors", &ClusterDescriptor::descriptors);

  // Descriptor calculators
  py::class_<Descriptor>(m, "Descriptor")
      .def("compute_struc", &Descriptor::compute_struc);

  py::class_<TwoBody, Descriptor>(m, "TwoBody")
      .def(py::init<double, int, const std::string &,
                    const std::vector<double> &>());

  py::class_<ThreeBody, Descriptor>(m, "ThreeBody")
      .def(py::init<double, int, const std::string &,
                    const std::vector<double> &>());

  py::class_<ThreeBodyWide, Descriptor>(m, "ThreeBodyWide")
      .def(py::init<double, int, const std::string &,
                    const std::vector<double> &>());

  py::class_<FourBody, Descriptor>(m, "FourBody")
      .def(py::init<double, int, const std::string &,
                    const std::vector<double> &>());

  py::class_<B2, Descriptor>(m, "B2")
      .def(py::init<const std::string &, const std::string &,
                    const std::vector<double> &, const std::vector<double> &,
                    const std::vector<int> &>())
      .def(py::init<const std::string &, const std::string &,
                    const std::vector<double> &, const std::vector<double> &,
                    const std::vector<int> &,
                    const Eigen::MatrixXd &>())
      .def_readonly("radial_basis", &B2::radial_basis)
      .def_readonly("cutoff_function", &B2::cutoff_function)
      .def_readonly("radial_hyps", &B2::radial_hyps)
      .def_readonly("cutoff_hyps", &B2::cutoff_hyps)
      .def_readonly("cutoffs", &B2::cutoffs)
      .def_readonly("descriptor_settings", &B2::descriptor_settings);

  py::class_<B2_Simple, Descriptor>(m, "B2_Simple")
      .def(py::init<const std::string &, const std::string &,
                    const std::vector<double> &, const std::vector<double> &,
                    const std::vector<int> &>());

  py::class_<B2_Norm, Descriptor>(m, "B2_Norm")
      .def(py::init<const std::string &, const std::string &,
                    const std::vector<double> &, const std::vector<double> &,
                    const std::vector<int> &>());

  py::class_<B3, Descriptor>(m, "B3")
      .def(py::init<const std::string &, const std::string &,
                    const std::vector<double> &, const std::vector<double> &,
                    const std::vector<int> &>());

  // Kernel functions
  py::class_<Kernel>(m, "Kernel");

  py::class_<NormalizedDotProduct, Kernel>(m, "NormalizedDotProduct")
      .def(py::init<double, double>())
      .def_readonly("sigma", &NormalizedDotProduct::sigma)
      .def_readonly("power", &NormalizedDotProduct::power)
      .def_readonly("kernel_hyperparameters",
                    &NormalizedDotProduct::kernel_hyperparameters)
      .def("envs_envs", &NormalizedDotProduct::envs_envs)
      .def("envs_struc", &NormalizedDotProduct::envs_struc)
      .def("struc_struc", &NormalizedDotProduct::struc_struc);

  py::class_<NormalizedDotProduct_ICM, Kernel>(m, "NormalizedDotProduct_ICM")
      .def(py::init<double, double, Eigen::MatrixXd>());

  py::class_<SquaredExponential, Kernel>(m, "SquaredExponential")
      .def(py::init<double, double>());

  // Sparse GP DTC
  py::class_<SparseGP>(m, "SparseGP")
      .def(py::init<>())
      .def(py::init<std::vector<Kernel *>, double, double, double>())
      .def("set_hyperparameters", &SparseGP::set_hyperparameters)
      .def("predict_mean", &SparseGP::predict_mean)
      .def("predict_SOR", &SparseGP::predict_SOR)
      .def("predict_DTC", &SparseGP::predict_DTC)
      .def("predict_local_uncertainties",
           &SparseGP::predict_local_uncertainties)
      .def("add_all_environments", &SparseGP::add_all_environments)
      .def("add_specific_environments", &SparseGP::add_specific_environments)
      .def("add_random_environments", &SparseGP::add_random_environments)
      .def("add_uncertain_environments",
           &SparseGP::add_uncertain_environments)
      .def("add_training_structure", &SparseGP::add_training_structure)
      .def("update_matrices_QR", &SparseGP::update_matrices_QR)
      .def("compute_likelihood", &SparseGP::compute_likelihood)
      .def("compute_likelihood_stable", &SparseGP::compute_likelihood_stable)
      .def("compute_likelihood_gradient",
           &SparseGP::compute_likelihood_gradient)
      .def("write_mapping_coefficients", &SparseGP::write_mapping_coefficients)
      .def_readonly("varmap_coeffs", &SparseGP::varmap_coeffs) // for debugging and unit test
      .def("compute_cluster_uncertainties", &SparseGP::compute_cluster_uncertainties) // for debugging and unit test
      .def("write_varmap_coefficients", &SparseGP::write_varmap_coefficients)
      .def_readwrite("Kuu_jitter", &SparseGP::Kuu_jitter)
      .def_readonly("complexity_penalty", &SparseGP::complexity_penalty)
      .def_readonly("data_fit", &SparseGP::data_fit)
      .def_readonly("constant_term", &SparseGP::constant_term)
      .def_readwrite("log_marginal_likelihood",
                     &SparseGP::log_marginal_likelihood)
      .def_readwrite("likelihood_gradient", &SparseGP::likelihood_gradient)
      .def_readonly("kernels", &SparseGP::kernels)
      .def_readonly("hyperparameters", &SparseGP::hyperparameters)
      .def_readonly("training_structures", &SparseGP::training_structures)
      .def_readonly("sparse_indices", &SparseGP::sparse_indices)
      .def_readonly("sparse_descriptors", &SparseGP::sparse_descriptors)
      .def_readonly("n_energy_labels", &SparseGP::n_energy_labels)
      .def_readonly("n_force_labels", &SparseGP::n_force_labels)
      .def_readonly("n_stress_labels", &SparseGP::n_stress_labels)
      .def_readonly("force_noise", &SparseGP::force_noise)
      .def_readonly("energy_noise", &SparseGP::energy_noise)
      .def_readonly("stress_noise", &SparseGP::stress_noise)
      .def_readonly("noise_vector", &SparseGP::noise_vector)
      .def_readonly("Kuu", &SparseGP::Kuu)
      .def_readonly("Kuu_kernels", &SparseGP::Kuu_kernels)
      .def_readonly("Kuf", &SparseGP::Kuf)
      .def_readonly("Kuf_kernels", &SparseGP::Kuf_kernels)
      .def_readonly("alpha", &SparseGP::alpha)
      .def_readonly("Kuu_inverse", &SparseGP::Kuu_inverse)
      .def_readonly("Sigma", &SparseGP::Sigma)
      .def_readonly("n_sparse", &SparseGP::n_sparse)
      .def_readonly("n_labels", &SparseGP::n_labels)
      .def_readonly("y", &SparseGP::y)
      .def_static("to_json", &SparseGP::to_json)
      .def_static("from_json", &SparseGP::from_json);
}


/*
 * Copyright 2019 Xilinx, Inc.
 *
 * 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.
 */
/*--
 * ---------------------------------------------------------------------------------------------------------------------*/
/*-- DISCLAIMER AND CRITICAL APPLICATIONS */
/*--
 * ---------------------------------------------------------------------------------------------------------------------*/
/*-- */
/*-- (c) Copyright 2019 Xilinx, Inc. All rights reserved. */
/*-- */
/*-- This file contains confidential and proprietary information of Xilinx, Inc. and is protected under U.S. and */
/*-- international copyright and other intellectual property laws. */
/*-- */
/*-- DISCLAIMER */
/*-- This disclaimer is not a license and does not grant any rights to the materials distributed herewith. Except as */
/*-- otherwise provided in a valid license issued to you by Xilinx, and to the maximum extent permitted by applicable */
/*-- law: (1) THESE MATERIALS ARE MADE AVAILABLE "AS IS" AND WITH ALL FAULTS, AND XILINX HEREBY DISCLAIMS ALL WARRANTIES
 */
/*-- AND CONDITIONS, EXPRESS, IMPLIED, OR STATUTORY, INCLUDING BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON- */
/*-- INFRINGEMENT, OR FITNESS FOR ANY PARTICULAR PURPOSE; and (2) Xilinx shall not be liable (whether in contract or
 * tort,*/
/*-- including negligence, or under any other theory of liability) for any loss or damage of any kind or nature */
/*-- related to, arising under or in connection with these materials, including for any direct, or any indirect, */
/*-- special, incidental, or consequential loss or damage (including loss of data, profits, goodwill, or any type of */
/*-- loss or damage suffered as a retVal of any action brought by a third party) even if such damage or loss was */
/*-- reasonably foreseeable or Xilinx had been advised of the possibility of the same. */
/*-- */
/*-- CRITICAL APPLICATIONS */
/*-- Xilinx products are not designed or intended to be fail-safe, or for use in any application requiring fail-safe */
/*-- performance, such as life-support or safety devices or systems, Class III medical devices, nuclear facilities, */
/*-- applications related to the deployment of airbags, or any other applications that could lead to death, personal */
/*-- injury, or severe property or environmental damage (individually and collectively, "Critical */
/*-- Applications"). Customer assumes the sole risk and liability of any use of Xilinx products in Critical */
/*-- Applications, subject only to applicable laws and regulations governing limitations on product liability. */
/*-- */
/*-- THIS COPYRIGHT NOTICE AND DISCLAIMER MUST BE RETAINED AS PART OF THIS FILE AT ALL TIMES. */
/*--
 * ---------------------------------------------------------------------------------------------------------------------*/

#include <stdio.h>
#include <string.h>

#include <chrono>
#include <vector>
#include <boost/algorithm/string.hpp>

#include "xf_fintech_api.hpp"
#include "models/xf_fintech_fdbslv.hpp"
using namespace xf::fintech;

struct testcaseParams {
    unsigned int solverN;
    unsigned int solverM;
    double solverTheta;
    double modelS;
    double modelK;
    double boundaryLower;
    double boundaryUpper;
};

int ReadVector(const std::string filename, std::vector<float>& A, const unsigned int size) {
    std::ifstream file(filename);

    std::vector<std::string> v;
    std::string line;

    if (file.good()) {
        std::cout << "Opened " << filename << " OK" << std::endl;

        unsigned int i = 0;
        while (file.good()) {
            getline(file, line);
            if (line.length()) {
                A[i] = std::stof(line);
                i++;
            }
            if (i > size) {
                std::cout << "Warning! File has more than expected " << size << " lines..." << std::endl;
                break;
            }
        }
    } else {
        std::cout << "Couldn't open " << filename << std::endl;
        return 1;
    }
    return 0;
}

/// @brief Read testcase parameters
int ReadTestcaseParameters(std::string filename, testcaseParams& params) {
    std::ifstream file(filename);

    std::vector<std::string> v;
    std::string line;

    if (file.good()) {
        std::cout << "Opened " << filename << " OK" << std::endl;

        getline(file, line);
        boost::split(v, line, [](char c) { return c == ','; });
        params.solverN = std::stoi(v[0]);
        params.solverM = std::stoi(v[1]);
        params.solverTheta = std::stod(v[2]);
        params.modelS = std::stod(v[3]);
        params.modelK = std::stod(v[4]);
        params.boundaryLower = std::stod(v[5]);
        params.boundaryUpper = std::stod(v[6]);
    } else {
        std::cout << "Couldn't open " << filename << std::endl;
        return 1;
    }
    return 0;
}

int main(int argc, char** argv) {
    // xclbin file
    std::string path = std::string(argv[1]);

    // test data
    std::string test_dir = std::string(argv[2]);

    // Get the testcase parameters
    testcaseParams params;
    std::cout << "Loading testcase..." << std::endl;
    if (ReadTestcaseParameters(test_dir + "/parameters.csv", params)) return EXIT_FAILURE;

    // Extract N, M to aid readability
    const unsigned int N = params.solverN;
    const unsigned int M = params.solverM;

    // device
    std::string device = TOSTRING(DEVICE_PART);
    if (argc == 4) {
        device = std::string(argv[3]);
    }

    // Create the FD solver object
    fdbslv fdbslv(N, M, path);

    int retval = XLNX_OK;

    std::vector<Device*> deviceList;
    Device* pChosenDevice;

    deviceList = DeviceManager::getDeviceList(device);

    if (deviceList.size() == 0) {
        printf("No matching devices found\n");
        exit(0);
    }

    printf("Found %zu matching devices\n", deviceList.size());

    // we'll just pick the first device in the...
    pChosenDevice = deviceList[0];

    if (retval == XLNX_OK) {
        // turn off trace output...turn it on here if you want extra debug output...
        Trace::setEnabled(true);
    }

    // clain the device
    printf("\n\n\n");
    printf("[XF_FINTECH] trying to claim device...\n");
    retval = fdbslv.claimDevice(pChosenDevice);
    if (retval != XLNX_OK) {
        printf("[XF_FINTECH] Failed to claim device - error = %d\n", retval);
    }

    // read input data
    std::vector<float> xGrid(N);
    std::vector<float> tGrid(M);
    std::vector<float> sigma(N * M);
    std::vector<float> rate(M);
    std::vector<float> initialCondition(N);
    std::vector<float> solution(N);
    std::vector<float> reference(N);

    if (ReadVector(test_dir + "/xGrid.csv", xGrid, N)) return 1;
    if (ReadVector(test_dir + "/tGrid.csv", tGrid, M)) return 1;
    if (ReadVector(test_dir + "/sigma.csv", sigma, N * M)) return 1;
    if (ReadVector(test_dir + "/rate.csv", rate, M)) return 1;
    if (ReadVector(test_dir + "/initialCondition.csv", initialCondition, N)) return 1;
    if (ReadVector(test_dir + "/reference.csv", reference, N)) return 1;

    // run the kernel
    retval = fdbslv.run(xGrid, tGrid, sigma, rate, initialCondition, params.solverTheta, params.boundaryLower,
                        params.boundaryUpper, solution);

    // check the results
    float max_diff = 0;
    int index = 0;
    std::cout << "actual (expected)" << std::endl;
    for (unsigned int i = 0; i < N; i++) {
        std::cout << solution[i] << "  (" << reference[i] << ")" << std::endl;
        if (std::abs(solution[i] - reference[i]) > max_diff) {
            max_diff = std::abs(solution[i] - reference[i]);
            index = i;
        }
    }
    std::cout << "Max difference = " << max_diff << " at index " << index << std::endl;

    // release the device
    retval = fdbslv.releaseDevice();

    int ret = 0; // assume pass
    if (max_diff > 0.003) {
        std::cout << "FAIL" << std::endl;
        ret = 1;
    } else {
        std::cout << "PASS" << std::endl;
    }

    return ret;
}


#include <cmath>
#include <ctime>
#include <regex>

#include <boost/filesystem.hpp>

#include "util.h"

void ReadFileNames(const std::string& directory, std::vector<std::string>& fileNames, int nDocs) {
    boost::filesystem::path dirPath(directory);
    boost::filesystem::recursive_directory_iterator start(dirPath);
    boost::filesystem::recursive_directory_iterator end;
    for (auto it = start; it != end; it++) {
        if (boost::filesystem::is_directory(it->path())) {
            continue;
        }
        std::string path = it->path().string();
        if (path.substr(path.length() - 5) == ".html") {
            fileNames.push_back(path);
        }
        if (nDocs != -1 && fileNames.size() == static_cast<size_t>(nDocs)) {
            break;
        }
    }
}

const std::regex hostRegex("(http|https)://(?:www\\.)?([^/ :]+):?([^/ ]*)(/?[^ #?]*)\\x3f?([^ #]*)#?([^ ]*)");

std::string GetHost(const std::string& url) {
    // if u know better way to do it - it would be nice if u rewrite it
    std::string output = "";
    // https://stackoverflow.com/questions/2616011/easy-way-to-parse-a-url-in-c-cross-platform
    try {
        std::smatch what;
        if (std::regex_match(url, what, hostRegex) && what.size() >= 3) {
            output = std::string(what[2].first, what[2].second);
        }
    } catch (...) {
        LOG_DEBUG("Can't parse host from url: " << url);
        return output;
    }
    return output;
}

std::string CleanFileName(const std::string& fileName) {
    return fileName.substr(fileName.find_last_of("/") + 1);
}

float Sigmoid(float x) {
    if (x >= 0.0f) {
        float z = exp(-x);
        return 1.0f / (1.0f + z);
    }
    float z = exp(x);
    return z / (1.0f + z);
}

double Sigmoid(double x) {
    if (x >= 0.0) {
        double z = exp(-x);
        return 1.0 / (1.0 + z);
    }
    double z = exp(x);
    return z / (1.0 + z);
}

uint64_t DateToTimestamp(const std::string& date) {
    std::regex ex("(\\d\\d\\d\\d)-(\\d\\d)-(\\d\\d)T(\\d\\d):(\\d\\d):(\\d\\d)([+-])(\\d\\d):(\\d\\d)");
    std::smatch what;
    if (!std::regex_match(date, what, ex) || what.size() < 10) {
        throw std::runtime_error("wrong date format");
    }
    std::tm t = {};
    t.tm_sec = std::stoi(what[6]);
    t.tm_min = std::stoi(what[5]);
    t.tm_hour = std::stoi(what[4]);
    t.tm_mday = std::stoi(what[3]);
    t.tm_mon = std::stoi(what[2]) - 1;
    t.tm_year = std::stoi(what[1]) - 1900;

    time_t timestamp = timegm(&t);
    uint64_t zone_ts = std::stoi(what[8]) * 60 * 60 + std::stoi(what[9]) * 60;
    if (what[7] == "+") {
        timestamp = timestamp - zone_ts;
    } else if (what[7] == "-") {
        timestamp = timestamp + zone_ts;
    }
    return timestamp > 0 ? timestamp : 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017-2020.
// Modifications copyright (c) 2017-2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP
#define BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP

#include <map>
#include <string>

#include <boost/shared_ptr.hpp>

#include <boost/geometry/core/static_assert.hpp>

#include <boost/geometry/srs/projections/dpar.hpp>
#include <boost/geometry/srs/projections/proj4.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/proj/aea.hpp>
#include <boost/geometry/srs/projections/proj/aeqd.hpp>
#include <boost/geometry/srs/projections/proj/airy.hpp>
#include <boost/geometry/srs/projections/proj/aitoff.hpp>
#include <boost/geometry/srs/projections/proj/august.hpp>
#include <boost/geometry/srs/projections/proj/bacon.hpp>
#include <boost/geometry/srs/projections/proj/bipc.hpp>
#include <boost/geometry/srs/projections/proj/boggs.hpp>
#include <boost/geometry/srs/projections/proj/bonne.hpp>
#include <boost/geometry/srs/projections/proj/cass.hpp>
#include <boost/geometry/srs/projections/proj/cc.hpp>
#include <boost/geometry/srs/projections/proj/cea.hpp>
#include <boost/geometry/srs/projections/proj/chamb.hpp>
#include <boost/geometry/srs/projections/proj/collg.hpp>
#include <boost/geometry/srs/projections/proj/crast.hpp>
#include <boost/geometry/srs/projections/proj/denoy.hpp>
#include <boost/geometry/srs/projections/proj/eck1.hpp>
#include <boost/geometry/srs/projections/proj/eck2.hpp>
#include <boost/geometry/srs/projections/proj/eck3.hpp>
#include <boost/geometry/srs/projections/proj/eck4.hpp>
#include <boost/geometry/srs/projections/proj/eck5.hpp>
#include <boost/geometry/srs/projections/proj/eqc.hpp>
#include <boost/geometry/srs/projections/proj/eqdc.hpp>
#include <boost/geometry/srs/projections/proj/etmerc.hpp>
#include <boost/geometry/srs/projections/proj/fahey.hpp>
#include <boost/geometry/srs/projections/proj/fouc_s.hpp>
#include <boost/geometry/srs/projections/proj/gall.hpp>
#include <boost/geometry/srs/projections/proj/geocent.hpp>
#include <boost/geometry/srs/projections/proj/geos.hpp>
#include <boost/geometry/srs/projections/proj/gins8.hpp>
#include <boost/geometry/srs/projections/proj/gn_sinu.hpp>
#include <boost/geometry/srs/projections/proj/gnom.hpp>
#include <boost/geometry/srs/projections/proj/goode.hpp>
#include <boost/geometry/srs/projections/proj/gstmerc.hpp>
#include <boost/geometry/srs/projections/proj/hammer.hpp>
#include <boost/geometry/srs/projections/proj/hatano.hpp>
#include <boost/geometry/srs/projections/proj/healpix.hpp>
#include <boost/geometry/srs/projections/proj/krovak.hpp>
#include <boost/geometry/srs/projections/proj/igh.hpp>
#include <boost/geometry/srs/projections/proj/imw_p.hpp>
#include <boost/geometry/srs/projections/proj/isea.hpp>
#include <boost/geometry/srs/projections/proj/laea.hpp>
#include <boost/geometry/srs/projections/proj/labrd.hpp>
#include <boost/geometry/srs/projections/proj/lagrng.hpp>
#include <boost/geometry/srs/projections/proj/larr.hpp>
#include <boost/geometry/srs/projections/proj/lask.hpp>
#include <boost/geometry/srs/projections/proj/latlong.hpp>
#include <boost/geometry/srs/projections/proj/lcc.hpp>
#include <boost/geometry/srs/projections/proj/lcca.hpp>
#include <boost/geometry/srs/projections/proj/loxim.hpp>
#include <boost/geometry/srs/projections/proj/lsat.hpp>
#include <boost/geometry/srs/projections/proj/mbtfpp.hpp>
#include <boost/geometry/srs/projections/proj/mbtfpq.hpp>
#include <boost/geometry/srs/projections/proj/mbt_fps.hpp>
#include <boost/geometry/srs/projections/proj/merc.hpp>
#include <boost/geometry/srs/projections/proj/mill.hpp>
#include <boost/geometry/srs/projections/proj/mod_ster.hpp>
#include <boost/geometry/srs/projections/proj/moll.hpp>
#include <boost/geometry/srs/projections/proj/natearth.hpp>
#include <boost/geometry/srs/projections/proj/nell.hpp>
#include <boost/geometry/srs/projections/proj/nell_h.hpp>
#include <boost/geometry/srs/projections/proj/nocol.hpp>
#include <boost/geometry/srs/projections/proj/nsper.hpp>
#include <boost/geometry/srs/projections/proj/nzmg.hpp>
#include <boost/geometry/srs/projections/proj/ob_tran.hpp>
#include <boost/geometry/srs/projections/proj/ocea.hpp>
#include <boost/geometry/srs/projections/proj/oea.hpp>
#include <boost/geometry/srs/projections/proj/omerc.hpp>
#include <boost/geometry/srs/projections/proj/ortho.hpp>
#include <boost/geometry/srs/projections/proj/qsc.hpp>
#include <boost/geometry/srs/projections/proj/poly.hpp>
#include <boost/geometry/srs/projections/proj/putp2.hpp>
#include <boost/geometry/srs/projections/proj/putp3.hpp>
#include <boost/geometry/srs/projections/proj/putp4p.hpp>
#include <boost/geometry/srs/projections/proj/putp5.hpp>
#include <boost/geometry/srs/projections/proj/putp6.hpp>
#include <boost/geometry/srs/projections/proj/robin.hpp>
#include <boost/geometry/srs/projections/proj/rouss.hpp>
#include <boost/geometry/srs/projections/proj/rpoly.hpp>
#include <boost/geometry/srs/projections/proj/sconics.hpp>
#include <boost/geometry/srs/projections/proj/somerc.hpp>
#include <boost/geometry/srs/projections/proj/stere.hpp>
#include <boost/geometry/srs/projections/proj/sterea.hpp>
#include <boost/geometry/srs/projections/proj/sts.hpp>
#include <boost/geometry/srs/projections/proj/tcc.hpp>
#include <boost/geometry/srs/projections/proj/tcea.hpp>
#include <boost/geometry/srs/projections/proj/tmerc.hpp>
#include <boost/geometry/srs/projections/proj/tpeqd.hpp>
#include <boost/geometry/srs/projections/proj/urm5.hpp>
#include <boost/geometry/srs/projections/proj/urmfps.hpp>
#include <boost/geometry/srs/projections/proj/vandg.hpp>
#include <boost/geometry/srs/projections/proj/vandg2.hpp>
#include <boost/geometry/srs/projections/proj/vandg4.hpp>
#include <boost/geometry/srs/projections/proj/wag2.hpp>
#include <boost/geometry/srs/projections/proj/wag3.hpp>
#include <boost/geometry/srs/projections/proj/wag7.hpp>
#include <boost/geometry/srs/projections/proj/wink1.hpp>
#include <boost/geometry/srs/projections/proj/wink2.hpp>

namespace boost { namespace geometry { namespace projections
{

namespace detail
{

template <typename Params>
struct factory_key
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Invalid parameters type.",
        Params);
};

template <>
struct factory_key<srs::detail::proj4_parameters>
{
    typedef std::string type;
    template <typename ProjParams>
    static type const& get(ProjParams const& par)
    {
        return par.id.name;
    }
    static const char* get(const char* name, srs::dpar::value_proj id)
    {
        return name;
    }
};

template <typename T>
struct factory_key<srs::dpar::parameters<T> >
{
    typedef srs::dpar::value_proj type;
    template <typename ProjParams>
    static type const& get(ProjParams const& par)
    {
        return par.id.id;
    }
    static srs::dpar::value_proj get(const char* name, srs::dpar::value_proj id)
    {
        return id;
    }
};

template <typename Params, typename CT, typename ProjParams>
class factory
{
private:
    typedef detail::factory_entry
        <
            Params,
            CT,
            ProjParams
        > entry_base;

    typedef factory_key<Params> key;
    typedef typename key::type key_type;
    typedef boost::shared_ptr<entry_base> entry_ptr;

    typedef std::map<key_type, entry_ptr> entries_map;

    entries_map m_entries;

public:

    factory()
    {
        detail::aea_init(*this);
        detail::aeqd_init(*this);
        detail::airy_init(*this);
        detail::aitoff_init(*this);
        detail::august_init(*this);
        detail::bacon_init(*this);
        detail::bipc_init(*this);
        detail::boggs_init(*this);
        detail::bonne_init(*this);
        detail::cass_init(*this);
        detail::cc_init(*this);
        detail::cea_init(*this);
        detail::chamb_init(*this);
        detail::collg_init(*this);
        detail::crast_init(*this);
        detail::denoy_init(*this);
        detail::eck1_init(*this);
        detail::eck2_init(*this);
        detail::eck3_init(*this);
        detail::eck4_init(*this);
        detail::eck5_init(*this);
        detail::eqc_init(*this);
        detail::eqdc_init(*this);
        detail::etmerc_init(*this);
        detail::fahey_init(*this);
        detail::fouc_s_init(*this);
        detail::gall_init(*this);
        detail::geocent_init(*this);
        detail::geos_init(*this);
        detail::gins8_init(*this);
        detail::gn_sinu_init(*this);
        detail::gnom_init(*this);
        detail::goode_init(*this);
        detail::gstmerc_init(*this);
        detail::hammer_init(*this);
        detail::hatano_init(*this);
        detail::healpix_init(*this);
        detail::krovak_init(*this);
        detail::igh_init(*this);
        detail::imw_p_init(*this);
        detail::isea_init(*this);
        detail::labrd_init(*this);
        detail::laea_init(*this);
        detail::lagrng_init(*this);
        detail::larr_init(*this);
        detail::lask_init(*this);
        detail::latlong_init(*this);
        detail::lcc_init(*this);
        detail::lcca_init(*this);
        detail::loxim_init(*this);
        detail::lsat_init(*this);
        detail::mbtfpp_init(*this);
        detail::mbtfpq_init(*this);
        detail::mbt_fps_init(*this);
        detail::merc_init(*this);
        detail::mill_init(*this);
        detail::mod_ster_init(*this);
        detail::moll_init(*this);
        detail::natearth_init(*this);
        detail::nell_init(*this);
        detail::nell_h_init(*this);
        detail::nocol_init(*this);
        detail::nsper_init(*this);
        detail::nzmg_init(*this);
        detail::ob_tran_init(*this);
        detail::ocea_init(*this);
        detail::oea_init(*this);
        detail::omerc_init(*this);
        detail::ortho_init(*this);
        detail::qsc_init(*this);
        detail::poly_init(*this);
        detail::putp2_init(*this);
        detail::putp3_init(*this);
        detail::putp4p_init(*this);
        detail::putp5_init(*this);
        detail::putp6_init(*this);
        detail::robin_init(*this);
        detail::rouss_init(*this);
        detail::rpoly_init(*this);
        detail::sconics_init(*this);
        detail::somerc_init(*this);
        detail::stere_init(*this);
        detail::sterea_init(*this);
        detail::sts_init(*this);
        detail::tcc_init(*this);
        detail::tcea_init(*this);
        detail::tmerc_init(*this);
        detail::tpeqd_init(*this);
        detail::urm5_init(*this);
        detail::urmfps_init(*this);
        detail::vandg_init(*this);
        detail::vandg2_init(*this);
        detail::vandg4_init(*this);
        detail::wag2_init(*this);
        detail::wag3_init(*this);
        detail::wag7_init(*this);
        detail::wink1_init(*this);
        detail::wink2_init(*this);
    }

    void add_to_factory(const char* name, srs::dpar::value_proj id, entry_base* entry)
    {
        // The pointer has to be owned before std::map::operator[] in case it thrown an exception.
        entry_ptr ptr(entry);
        m_entries[key::get(name, id)] = ptr;
    }

    detail::dynamic_wrapper_b<CT, ProjParams>* create_new(Params const& params, ProjParams const& proj_par) const
    {
        typedef typename entries_map::const_iterator const_iterator;
        const_iterator it = m_entries.find(key::get(proj_par));
        if (it != m_entries.end())
        {
            return it->second->create_new(params, proj_par);
        }

        return 0;
    }
};

template <typename T>
inline detail::dynamic_wrapper_b<T, projections::parameters<T> >*
    create_new(srs::detail::proj4_parameters const& params,
               projections::parameters<T> const& parameters)
{
    static factory<srs::detail::proj4_parameters, T, projections::parameters<T> > const fac;
    return fac.create_new(params, parameters);
}

template <typename T>
inline detail::dynamic_wrapper_b<T, projections::parameters<T> >*
    create_new(srs::dpar::parameters<T> const& params,
               projections::parameters<T> const& parameters)
{
    static factory<srs::dpar::parameters<T>, T, projections::parameters<T> > const fac;
    return fac.create_new(params, parameters);
}

} // namespace detail

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  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 Willow Garage 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 OWNER 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.
 *********************************************************************/

//
// Sparse Pose Adjustment classes and functions
//

//
// Ok, we're going with 2-pose constraints, and added scale variables.
// Format for files:
// Format for files:
//   #nodes #scales #p2p_constraints #scale_constraints
//   x y z rx ry rz   ; first node, vector part of unit quaternion
//   ...
//   p1 p2 Lambda_12  ; first p2p constraint
//   ...
//   p1 p2 s1 K_12 w  ; first scale constraint,
//                    ; (p1-p2)^2 = s1*K_12 with weight w

#include <stdio.h>
#include "sparse_bundle_adjustment/sba.h"
#include <Eigen/Cholesky>
#include <chrono>

using namespace Eigen;
using namespace std;

#include <iostream>
#include <iomanip>
#include <fstream>
#include <utility>

// elapsed time in microseconds
static long long utime()
{
  auto duration = std::chrono::system_clock::now().time_since_epoch();
  return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
}

namespace sba
{
  // reads in a file of pose constraints
  bool readP2File(char *fname, SysSPA spa)
  {
    // Format for files:
    //   #nodes #scales #p2p_constraints #scale_constraints
    //   x y z rx ry rz   ; first node, vector part of unit quaternion
    //   ...
    //   p1 p2 Lambda_12  ; first p2p constraint
    //   ...
    //   p1 p2 s1 K_12 w  ; first scale constraint,
    //                    ; (p1-p2)^2 = s1*K_12 with weight w

    ifstream ifs(fname);
    if (!ifs)
      {
        cout << "Can't open file " << fname << endl;
        return false;
      }
    ifs.precision(10);          // what is this for???

    // read header
    string line;
    if (!getline(ifs,line) || line != "# P2 Constraint File")
    {
      cout << "Bad header" << endl;
      return false;
    }
    cout << "Found P2 constraint file" << endl;

    // read number of cameras, scalers, and p2p constraints
    int ncams, nss, nscs, np2s;
    if (!(ifs >> ncams >> nss >> np2s >> nscs))
    {
      cout << "Bad entity count" << endl;
      return false;
    }
    cout << "Number of cameras: " << ncams
         << "  Number of scalers: " << nss
         << "  Number of p2p constraints: " << np2s
         << "  Number of scale constraints: " << nscs << endl;

    cout << "Reading in camera data..." << flush;
    std::vector<Node,Eigen::aligned_allocator<Node> > &nodes = spa.nodes;
    Node nd;

    for (int i=0; i<ncams; i++)
      {
        double v1,v2,v3;
        if (!(ifs >> v1 >> v2 >> v3))
          {
            cout << "Bad node translation params at number " << i << endl;
            return false;
          }
        nd.trans = Vector4d(v1,v2,v3,1.0);

        if (!(ifs >> v1 >> v2 >> v3))
          {
            cout << "Bad node rotation quaternion at number " << i << endl;
            return false;
          }
        Matrix3d m;
        nd.qrot = Quaternion<double>(v1,v2,v2,0.0);
        nd.normRot();           // normalize to unit vector and compute w

        nodes.push_back(nd);
      }
    cout << "done" << endl;

    // read in p2p constraints
    cout << "Reading in constraint data..." << flush;
    std::vector<ConP2,Eigen::aligned_allocator<ConP2> > &cons = spa.p2cons;
    ConP2 con;

    for (int i=0; i<np2s; i++)
      {
        int p1, p2;
        double vz[6];           // mean
        double vv[36];          // prec

        if (!(ifs >> p1 >> p2))
          {
            cout << "Bad node indices at constraint " << i << endl;
            return false;
          }

        for (int i=0; i<6; i++)
          {
            if (!(ifs >> vz[i]))
              {
                cout << "Bad constraint mean at constraint " << i << endl;
                return false;
              }
          }

        for (int i=0; i<36; i++)
          {
            if (!(ifs >> vv[i]))
              {
                cout << "Bad constraint precision at constraint " << i << endl;
                return false;
              }
          }

        con.ndr = p1;
        con.nd1 = p2;

        //        con.setMean(Matrix<double,6,1>(vz));
        con.prec = Matrix<double,6,6>(vv);

        cons.push_back(con);
      }

    // read in scale constraints
    cout << "Reading in scale constraint data..." << flush;
    std::vector<ConScale,Eigen::aligned_allocator<ConScale> > &scons = spa.scons;
    ConScale scon;

    for (int i=0; i<nscs; i++)
      {
        int p1, p2, sv;
        double ks, w;

        if (!(ifs >> p1 >> p2 >> sv >> ks >> w))
          {
            cout << "Bad scale constraint at constraint " << i << endl;
            return false;
          }

        scon.nd0 = p1;
        scon.nd1 = p2;
        scon.sv  = sv;
        scon.ks  = ks;
        scon.w   = w;

        scons.push_back(scon);
      }

    return true;
  }

// Do we want to normalize quaternion vectors based on the sign of w?
#define NORMALIZE_Q

  // set up Jacobians
  // see Konolige RSS 2010 submission for details

  void ConP2::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
  {
    // node references
    Node &nr = nodes[ndr];
    Matrix<double,4,1> &tr = nr.trans;
    Quaternion<double> &qr = nr.qrot;
    Node &n1 = nodes[nd1];
    Matrix<double,4,1> &t1 = n1.trans;
    Quaternion<double> &q1 = n1.qrot;

    // first get the second frame in first frame coords
    Eigen::Matrix<double,3,1> pc = nr.w2n * t1;

    // Jacobians wrt first frame parameters

    // translational part of 0p1 wrt translational vars of p0
    // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
    J0.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    Eigen::Matrix<double,3,1> pwt;
    pwt = (t1-tr).head(3);   // transform translations

    // dx
    Eigen::Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,3) = dp;
    // dy
    dp = nr.dRdy * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,4) = dp;
    // dz
    dp = nr.dRdz * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,5) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J0.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = qpmean * s0' * q0' * q1

    // dqdx
    Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
    qr1.coeffs() = q1.coeffs();
    qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x());  // qpmean * ds0'/dx
    qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
    qr0 = qr0*qr1;              // rotate to zero mean
    qrd = qpmean*qr0;           // for normalization check
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y());  // qpmean * ds0'/dy
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z());  // qpmean * ds0'/dz
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,5) = qrn.vec();

    // transpose
    J0t = J0.transpose();

    //  cout << endl << "J0 " << ndr << endl << J0 << endl;

    // Jacobians wrt second frame parameters
    // translational part of 0p1 wrt translational vars of p1
    // this is just R0'  [from 0t1 = R0'(t1 - t0)]
    J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p1: zero
    J1.block<3,3>(0,3).setZero();

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J1.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = q0'*s1*q1

    Eigen::Quaternion<double> qrc;
    qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
    qrc = qpmean*qrc*qr1;       // mean' * qr0' * qr1
    qrc.normalize();

    //    cout << endl << "QRC  : " << qrc.coeffs().transpose() << endl;

    double dq = 1.0e-8;
    double wdq = 1.0 - dq*dq;
    qr1.coeffs() = Vector4d(dq,0,0,wdq);
    //    cout << "QRC+x: " << (qrc*qr1).coeffs().transpose() << endl;
    //    cout << "QRdx:  " << ((qrc*qr1).coeffs().transpose() - qrc.coeffs().transpose())/dq << endl;

    // dqdx
    qrn.coeffs() = Vector4d(1,0,0,0);
    qrn = qrc*qrn;

    //    cout << "J1dx:  " << qrn.coeffs().transpose() << endl;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(0,1,0,0);
    qrn = qrc*qrn;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(0,0,1,0);
    qrn = qrc*qrn;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,5) = qrn.vec();

    // transpose
    J1t = J1.transpose();

    //  cout << endl << "J1 " << nd1 << endl << J1 << endl;

  };

  // set up Jacobians
  // see Konolige RSS 2010 submission for details

  void ConScale::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
  {
    // node references
    Node &n0 = nodes[nd0];
    Matrix<double,4,1> &t0 = n0.trans;
    Node &n1 = nodes[nd1];
    Matrix<double,4,1> &t1 = n1.trans;

    Eigen::Matrix<double,3,1> td = (t1-t0).head(3);

    // Jacobians wrt first frame parameters
    //  (ti - tj)^2 - a*kij

    // scale error wrt translational vars t0
    J0 = -2.0 * td;

    // scale error wrt translational vars t1
    J1 =  2.0 * td;

    // scale error wrt scale variable is just -kij
  };

  //
  // This hasn't been tested, and is probably wrong.
  //

  void ConP3P::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > nodes)
  {
    // node references
    Node nr = nodes[ndr];
    Matrix<double,4,1> &tr = nr.trans;
    Quaternion<double> &qr = nr.qrot;
    Node n1 = nodes[nd1];
    Matrix<double,4,1> &t1 = n1.trans;
    Quaternion<double> &q1 = n1.qrot;
    Node n2 = nodes[nd2];
    Matrix<double,4,1> &t2 = n2.trans;
    Quaternion<double> &q2 = n2.qrot;

    // calculate J10 -> d(0p1)/d(p0)
    // rows are indexed by 0p1 parameters
    // cols are indexed by p0 parameters

    // translational part of 0p1 wrt translational vars of p0
    // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
    J10.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    Matrix<double,3,1> pwt = (t1-tr).head(3);
    Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dqx * [pw - t]
    J10.block<3,1>(0,3) = dp;
    dp = nr.dRdy * pwt; // dR'/dqy * [pw - t]
    J10.block<3,1>(0,4) = dp;
    dp = nr.dRdz * pwt; // dR'/dqz * [pw - t]
    J10.block<3,1>(0,5) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J10.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = q0'*q1

    // dqdx
    double wn = -1.0/qr.w();  // need to switch params for small qr(3)
    dp[0] = wn*t1[0]*qr.x() - t1[3];
    dp[1] = wn*t1[1]*qr.x() + t1[2];
    dp[2] = wn*t1[2]*qr.x() - t1[1];
    J10.block<3,1>(3,3) = dp;

    // dqdy
    dp[0] = wn*t1[0]*qr.y() - t1[2];
    dp[1] = wn*t1[1]*qr.y() - t1[3];
    dp[2] = wn*t1[2]*qr.y() + t1[0];
    J10.block<3,1>(3,4) = dp;

    // dqdz
    dp[0] = wn*t1[0]*qr.z() + t1[1];
    dp[1] = wn*t1[1]*qr.z() - t1[0];
    dp[2] = wn*t1[2]*qr.z() - t1[3];
    J10.block<3,1>(3,5) = dp;

    // whew, J10 done...

    // J20 is similar, just using p2
    // translational part of 0p2 wrt translational vars of p0
    // this is just -R0'  [from 0t2 = R0'(t2 - t0)]
    J20.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);

    // translational part of 0p2 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    pwt = (t2-tr).head(3);
    dp = nr.dRdx * pwt; // dR'/dqx * [pw - t]
    J20.block<3,1>(0,3) = dp;
    dp = nr.dRdy * pwt; // dR'/dqy * [pw - t]
    J20.block<3,1>(0,4) = dp;
    dp = nr.dRdz * pwt; // dR'/dqz * [pw - t]
    J20.block<3,1>(0,5) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J20.block<3,3>(3,0).setZero();

    // rotational part of 0p2 wrt rotational vars of p0
    // from 0q1 = q0'*q2

    // dqdx
    wn = -1.0/qr.w();  // need to switch params for small qr(3)
    dp[0] = wn*q2.x()*qr.x() - q2.w();
    dp[1] = wn*q2.y()*qr.x() + q2.z();
    dp[2] = wn*q2.z()*qr.x() - q2.y();
    J20.block<3,1>(3,3) = dp;

    // dqdy
    dp[0] = wn*q2.x()*qr.y() - q2.z();
    dp[1] = wn*q2.y()*qr.y() - q2.w();
    dp[2] = wn*q2.z()*qr.y() + q2.x();
    J20.block<3,1>(3,4) = dp;

    // dqdz
    dp[0] = wn*q2.x()*qr.z() + q2.y();
    dp[1] = wn*q2.y()*qr.z() - q2.x();
    dp[2] = wn*q2.z()*qr.z() - q2.w();
    J20.block<3,1>(3,5) = dp;

    // calculate J11 -> d(0p1)/d(p1)
    // rows are indexed by 0p1 parameters
    // cols are indexed by p1 parameters

    // translational part of 0p1 wrt translational vars of p1
    // this is just R0'  [from 0t1 = R0'(t1 - t0)]
    J11.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p1 => zero
    J11.block<3,3>(0,3).setZero();

    // rotational part of 0p1 wrt translation vars of p1 => zero
    J11.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p1
    // from 0q1 = q0'*q1

    // dqdx1
    wn = 1.0/q1.w();  // need to switch params for small q1(3)
    dp[0] = wn*qr.x()*q1.x() + qr.w();
    dp[1] = wn*qr.y()*q1.x() - qr.z();
    dp[2] = wn*qr.z()*q1.x() + qr.y();
    J11.block<3,1>(3,3) = dp;

    // dqdy1
    dp[0] = wn*qr.x()*q1.y() + qr.z();
    dp[1] = wn*qr.y()*q1.y() + qr.w();
    dp[2] = wn*qr.z()*q1.y() - qr.x();
    J11.block<3,1>(3,4) = dp;

    // dqdz1
    dp[0] = wn*qr.x()*q1.z() - qr.y();
    dp[1] = wn*qr.y()*q1.z() + qr.x();
    dp[2] = wn*qr.z()*q1.z() + qr.w();
    J11.block<3,1>(3,5) = dp;

    // whew, J11 done...

    // J22 is similar to J11
    // rows are indexed by 0p2 parameters
    // cols are indexed by p2 parameters

    // translational part of 0p2 wrt translational vars of p2
    // this is just R0'  [from 0t2 = R0'(t2 - t0)]
    J22.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p2 wrt rotational vars of p2 => zero
    J22.block<3,3>(0,3).setZero();

    // rotational part of 0p2 wrt translation vars of p2 => zero
    J22.block<3,3>(3,0).setZero();

    // rotational part of 0p2 wrt rotational vars of p2
    // from 0q2 = q0'*q2

    // dqdx2
    wn = 1.0/q2.w();  // need to switch params for small q2.w()
    dp[0] = wn*qr.x()*q2.x() + qr.w();
    dp[1] = wn*qr.y()*q2.x() - qr.z();
    dp[2] = wn*qr.z()*q2.x() + qr.y();
    J22.block<3,1>(3,3) = dp;

    // dqdy1
    dp[0] = wn*qr.x()*q2.y() + qr.z();
    dp[1] = wn*qr.y()*q2.y() + qr.w();
    dp[2] = wn*qr.z()*q2.y() - qr.x();
    J22.block<3,1>(3,4) = dp;

    // dqdz1
    dp[0] = wn*qr.x()*q2.z() - qr.y();
    dp[1] = wn*qr.y()*q2.z() + qr.x();
    dp[2] = wn*qr.z()*q2.z() + qr.w();
    J22.block<3,1>(3,5) = dp;

    // whew, J22 done...

  };

  // error function
  inline double ConP2::calcErr(const Node &nd0, const Node &nd1)
    {
      Quaternion<double> q0p,q1;
      q0p.vec()   = -nd0.qrot.coeffs().head(3); // invert quaternion
      q0p.w()     =  nd0.qrot.w();
      q1          =  nd1.qrot;
      err.block<3,1>(0,0) = nd0.w2n * nd1.trans - tmean;

      //      cout << endl;
      //      cout << q0p.coeffs().transpose() << endl;
      //      cout << q1.coeffs().transpose() << endl;
      //      cout << qpmean.coeffs().transpose() << endl;

      q1 = qpmean * q0p * q1;

      //      cout << q1.coeffs().transpose() << endl << endl;

// this seems to mess up convergence...
#ifdef NORMALIZE_Q
      if (q1.w() < 0.0)
        err.block<3,1>(3,0) = -q1.vec(); // normalized
      else
#endif
        err.block<3,1>(3,0) = q1.vec(); // do we have to normalize w???
      //      cout << endl << "Error: " << err.transpose() << endl << endl;
      return err.dot(prec * err);
    }

  // error function for distance cost
  double ConP2::calcErrDist(const Node &nd0, const Node &nd1)
    {
      Vector3d derr;
      Quaternion<double> q0p,q1;
      q0p.vec()   = -nd0.qrot.vec(); // invert quaternion
      q0p.w()     =  nd0.qrot.w();
      q1          =  nd1.qrot;
      derr = nd0.w2n * nd1.trans - tmean;
      return derr.dot(derr);
    }

  // error function
  inline double ConScale::calcErr(const Node &nd0, const Node &nd1, double alpha)
    {
      err = (nd1.trans - nd0.trans).squaredNorm();
      err -= ks*alpha;
      //      cout << "Scale err: " << err << endl;
      return w*err*err;
    }

  // Adds a node to the system.
  // \return the index of the node added.
  int SysSPA::addNode(Eigen::Matrix<double,4,1> &trans,
                      Eigen::Quaternion<double> &qrot,
                      bool isFixed)
  {
    Node nd;
    nd.trans = trans;
    nd.qrot = qrot;
    nd.isFixed = isFixed;
    nd.setTransform(); // set up world2node transform
    nd.setDr(true); // set rotational derivatives
    // Should this be local or global?
    nd.normRot();//Local();
    nodes.push_back(nd);
    return nodes.size()-1;
  }

  // add a constraint
  // <nd0>, <nd1> are node id's
  // <mean> is x,y,th, with th in radians
  // <prec> is a 3x3 precision matrix (inverse covariance
  // returns true if nodes are found
  bool SysSPA::addConstraint(int nd0, int nd1,
                             Eigen::Vector3d &tmean,
                             Eigen::Quaterniond &qpmean,
                             Eigen::Matrix<double,6,6> &prec)
  {
    if (nd0 >= (int)nodes.size() || nd1 >= (int)nodes.size())
      return false;

    ConP2 con;
    con.ndr = nd0;
    con.nd1 = nd1;

    con.tmean = tmean;
    Quaternion<double> qr;
    qr = qpmean;
    qr.normalize();
    con.qpmean = qr.inverse(); // inverse of the rotation measurement
    con.prec = prec;

    p2cons.push_back(con);
    return true;
  }

  // error measure, squared
  // assumes node transforms have already been calculated
  // <tcost> is true if we just want the distance offsets
  double SysSPA::calcCost(bool tcost)
  {
    double cost = 0.0;

    // do distance offset
    if (tcost)
      {
        for(size_t i=0; i<p2cons.size(); i++)
          {
            ConP2 &con = p2cons[i];
            double err = con.calcErrDist(nodes[con.ndr],nodes[con.nd1]);
            cost += err;
          }
      }

    // full cost
    else
      {
        for(size_t i=0; i<p2cons.size(); i++)
          {
            ConP2 &con = p2cons[i];
            double err = con.calcErr(nodes[con.ndr],nodes[con.nd1]);
            cost += err;
          }
        if (scons.size() > 0)       // can have zero scale constraints
          for(size_t i=0; i<scons.size(); i++)
            {
              ConScale &con = scons[i];
              double err = con.calcErr(nodes[con.nd0],nodes[con.nd1],scales[con.sv]);
              cost += err;
            }
      }

    return cost;
  }

  // Set up linear system, from RSS submission (konolige 2010)
  // This is a relatively compact version of the algorithm!
  // but not Jacobians

  void SysSPA::setupSys(double sLambda)
  {
    // set matrix sizes and clear
    // assumes scales vars are all free
    int nFree = nodes.size() - nFixed;
    int nscales = scales.size();
    A.setZero(6*nFree+nscales,6*nFree+nscales);
    B.setZero(6*nFree+nscales);
    VectorXi dcnt(nFree);
    dcnt.setZero(nFree);

    // lambda augmentation
    double lam = 1.0 + sLambda;

    // loop over P2 constraints
    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        ConP2 &con = p2cons[pi];
        con.setJacobians(nodes);

        // add in 4 blocks of A; actually just need upper triangular
        // i0 < i1
        int i0 = 6*(con.ndr-nFixed); // will be negative if fixed
        int i1 = 6*(con.nd1-nFixed); // will be negative if fixed

        if (i0>=0)
          {
            A.block<6,6>(i0,i0) += con.J0t * con.prec * con.J0;
            dcnt(con.ndr - nFixed)++;
          }
        if (i1>=0)
          {
            dcnt(con.nd1 - nFixed)++;
            Matrix<double,6,6> tp = con.prec * con.J1;
            A.block<6,6>(i1,i1) += con.J1t * tp;
            if (i0>=0)
              {
                A.block<6,6>(i0,i1) += con.J0t * con.prec * con.J1;
                A.block<6,6>(i1,i0) += con.J1t * con.prec * con.J0;
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
          B.block<6,1>(i0,0) -= con.J0t * con.prec * con.err;
        if (i1>=0)
          B.block<6,1>(i1,0) -= con.J1t * con.prec * con.err;
      } // finish P2 constraints

    // loop over Scale constraints
    if (scons.size() > 0)       // could be zero
      for(size_t pi=0; pi<scons.size(); pi++)
        {
          ConScale &con = scons[pi];
          con.setJacobians(nodes);
        // add in 4 blocks of A for t0, t1; actually just need upper triangular
        // i0 < i1
        int i0 = 6*(con.nd0-nFixed); // will be negative if fixed
        int i1 = 6*(con.nd1-nFixed); // will be negative if fixed

        if (i0>=0)
          {
            A.block<3,3>(i0,i0) += con.w * con.J0 * con.J0.transpose();
          }
        if (i1>=0)
          {
            A.block<3,3>(i1,i1) += con.w * con.J1 * con.J1.transpose();
            if (i0>=0)
              {
                A.block<3,3>(i0,i1) += con.w * con.J0 * con.J1.transpose();
                A.block<3,3>(i1,i0) = A.block<3,3>(i0,i1).transpose();
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
          B.block<3,1>(i0,0) -= con.w * con.J0 * con.err;
        if (i1>=0)
          B.block<3,1>(i1,0) -= con.w * con.J1 * con.err;

        // scale variable, add in 5 blocks; actually just need upper triangular
        int is = 6*nFree+con.sv;
        A(is,is) += con.w * con.ks * con.ks;
        if (i0>=0)
          {
            A.block<3,1>(i0,is) += con.w * con.J0 * -con.ks;
            A.block<1,3>(is,i0) = A.block<3,1>(i0,is).transpose();
          }
        if (i1>=0)
          {
            A.block<3,1>(i1,is) += con.w * con.J1 * -con.ks;
            A.block<1,3>(is,i1) = A.block<3,1>(i1,is).transpose();
          }

        // add in scale block of B
        B(is) -= con.w * (-con.ks) * con.err;

      } // finish Scale constraints

    // augment diagonal
    A.diagonal() *= lam;

    // check the matrix and vector
    for (int i=0; i<6*nFree; i++)
      for (int j=0; j<6*nFree; j++)
        if (std::isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }

    for (int j=0; j<6*nFree; j++)
      if (std::isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }

    int ndc = 0;
    for (int i=0; i<nFree; i++)
      if (dcnt(i) == 0) ndc++;

    if (ndc > 0)
      cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
  }

  // Set up sparse linear system; see setupSys for algorithm.
  // Currently doesn't work with scale variables
  void SysSPA::setupSparseSys(double sLambda, int iter, int sparseType)
  {
    // set matrix sizes and clear
    // assumes scales vars are all free
    int nFree = nodes.size() - nFixed;

    //    long long t0, t1, t2, t3;
    //    t0 = utime();

    if (iter == 0)
      csp.setupBlockStructure(nFree); // initialize CSparse structures
    else
      csp.setupBlockStructure(0); // zero out CSparse structures

    //    t1 = utime();

    VectorXi dcnt(nFree);
    dcnt.setZero(nFree);

    // lambda augmentation
    double lam = 1.0 + sLambda;

    // loop over P2 constraints
    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        ConP2 &con = p2cons[pi];
        con.setJacobians(nodes);

        // add in 4 blocks of A; actually just need upper triangular
        int i0 = con.ndr-nFixed; // will be negative if fixed
        int i1 = con.nd1-nFixed; // will be negative if fixed

        if (i0>=0)
          {
           Matrix<double,6,6> m = con.J0t*con.prec*con.J0;
            csp.addDiagBlock(m,i0);
            dcnt(con.ndr - nFixed)++;
          }
        if (i1>=0)
          {
            dcnt(con.nd1 - nFixed)++;
            Matrix<double,6,6> tp = con.prec * con.J1;
            Matrix<double,6,6> m = con.J1t * tp;
            csp.addDiagBlock(m,i1);
            if (i0>=0)
              {
                Matrix<double,6,6> m2 = con.J0t * tp;
                if (i1 < i0)
                  {
                    m = m2.transpose();
                    csp.addOffdiagBlock(m,i1,i0);
                  }
                else
                  csp.addOffdiagBlock(m2,i0,i1);
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
          csp.B.block<6,1>(i0*6,0) -= con.J0t * con.prec * con.err;
        if (i1>=0)
          csp.B.block<6,1>(i1*6,0) -= con.J1t * con.prec * con.err;
      } // finish P2 constraints

    //    t2 = utime();

    // set up sparse matrix structure from blocks
    if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
      csp.incDiagBlocks(lam);   // increment diagonal block
    else
      csp.setupCSstructure(lam,iter==0);

    //    t3 = utime();

    //    printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
    //           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);

    int ndc = 0;
    for (int i=0; i<nFree; i++)
      if (dcnt(i) == 0) ndc++;

    if (ndc > 0)
      cout << "[SetupSparseSys] " << ndc << " disconnected nodes" << endl;
  }

  /// Run the LM algorithm that computes a nonlinear SPA estimate.
  /// <niter> is the max number of iterations to perform; returns the
  /// number actually performed.
  /// <lambda> is the diagonal augmentation for LM.
  /// <useCSparse> is true for sparse Cholesky.
  ///                2 for gradient system, 3 for block jacobian PCG
  /// <initTol> is the initial tolerance for CG
  /// <maxCGiters> is max # of iterations in BPCG

  int SysSPA::doSPA(int niter, double sLambda, int useCSparse, double initTol,
                      int maxCGiters)
  {
    Node::initDr();
    int nFree = nodes.size() - nFixed; // number of free nodes

    // number of nodes
    int ncams = nodes.size();
    // number of scale variables
    int nscales = scales.size();

    // save old scales
    vector<double> oldscales;
    oldscales.resize(nscales);

    // initialize vars
    if (sLambda > 0.0)          // do we initialize lambda?
      lambda = sLambda;

    // set number of constraints
    int ncons = p2cons.size();

    // check for fixed frames
    for (int i=0; i<ncams; i++)
      {
        Node &nd = nodes[i];
        if (i >= nFixed)
          nd.isFixed = false;
        else
          nd.isFixed = true;
        nd.setTransform();      // set up world-to-node transform for cost calculation
        nd.setDr(true);         // always use local angles
      }

    // initialize vars
    double laminc = 2.0;        // how much to increment lambda if we fail
    double lamdec = 0.5;        // how much to decrement lambda if we succeed
    int iter = 0;               // iterations
    sqMinDelta = 1e-8 * 1e-8;
    double cost = calcCost();
    if (verbose)
      cout << iter << " Initial squared cost: " << cost << " which is "
           << sqrt(cost/ncons) << " rms error" << endl;

    int good_iter = 0;
    for (; iter<niter; iter++)  // loop at most <niter> times
    {
        // set up and solve linear system
        // NOTE: shouldn't need to redo all calcs in setupSys if we
        //   got here from a bad update

        long long t0, t1, t2, t3;
        t0 = utime();
        if (useCSparse)
          setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
        else
          setupSys(lambda);     // set up linear system

        // use appropriate linear solver
        if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
          {
            if (csp.B.rows() != 0)
              {
                int iters = csp.doBPCG(maxCGiters,initTol,iter);
                if (verbose)
                  cout << "[Block PCG] " << iters << " iterations" << endl;
              }
          }
        else if (useCSparse > 0)
        {
            bool ok = csp.doChol();
            if (!ok)
              cout << "[DoSPA] Sparse Cholesky failed!" << endl;
        }
        else
          A.ldlt().solveInPlace(B); // Cholesky decomposition and solution

        // get correct result vector
        VectorXd &BB = useCSparse ? csp.B : B;

        // check for convergence
        // this is a pretty crummy convergence measure...
        double sqDiff = BB.squaredNorm();
        if (sqDiff < sqMinDelta) // converged, done...
        {
          break;
        }

        // update the frames
        int ci = 0;
        for(int i=0; i < ncams; i++)
        {
            Node &nd = nodes[i];
            if (nd.isFixed) continue; // not to be updated
            nd.oldtrans = nd.trans; // save in case we don't improve the cost
            nd.oldqrot = nd.qrot;
            nd.trans.head<3>() += BB.segment<3>(ci);

            Quaternion<double> qr;
            qr.vec() = BB.segment<3>(ci+3);
            qr.w() = sqrt(1.0 - qr.vec().squaredNorm());

            Quaternion<double> qrn,qrx;
            qrn = nd.qrot;
            qr = qrn*qr;        // post-multiply
            qr.normalize();
            if (qr.w() < 0.0)
              nd.qrot.coeffs() = -qr.coeffs();
            else
              nd.qrot.coeffs() = qr.coeffs();

            nd.setTransform();  // set up projection matrix for cost calculation
            nd.setDr(true);     // set rotational derivatives
            ci += 6;            // advance B index
        }

        // update the scales
        ci = 6*nFree;       // head of scale vars
        if (nscales > 0)        // could be empty
          for(int i=0; i < nscales; i++)
          {
              oldscales[i] = scales[i];
              scales[i] += B(ci);
              ci++;
          }

        // new cost
        double newcost = calcCost();
        if (verbose)
          cout << iter << " Updated squared cost: " << newcost << " which is "
           << sqrt(newcost/ncons) << " rms error" << endl;

        // check if we did good
        if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
        {
            cost = newcost;
            lambda *= lamdec;   // decrease lambda
            //      laminc = 2.0;       // reset bad lambda factor; not sure if this is a good idea...
            good_iter++;
        }
        else
        {
            lambda *= laminc;   // increase lambda
            laminc *= 2.0;      // increase the increment

            // reset nodes
            for(int i=0; i<ncams; i++)
            {
                Node &nd = nodes[i];
                if (nd.isFixed) continue; // not to be updated
                nd.trans = nd.oldtrans;
                nd.qrot = nd.oldqrot;
                nd.setTransform(); // set up projection matrix for cost calculation
                nd.setDr(true);
            }

            // reset scales
            if (nscales > 0)    // could be empty
              for(int i=0; i < nscales; i++)
                scales[i] = oldscales[i];

            cost = calcCost();  // need to reset errors
            if (verbose)
              cout << iter << " Downdated cost: " << cost << endl;
            // NOTE: shouldn't need to redo all calcs in setupSys
        }
      }

    // return number of iterations performed
    return good_iter;

  }

  // write out the precision matrix for CSparse
  void SysSPA::writeSparseA(char *fname, bool useCSparse)
  {
    ofstream ofs(fname);
    if (!ofs)
      {
        cout << "Can't open file " << fname << endl;
        return;
      }

    // cameras
    if (useCSparse)
      {
        setupSparseSys(0.0,0,useCSparse);

        int *Ai = csp.A->i;
        int *Ap = csp.A->p;
        double *Ax = csp.A->x;

        for (int i=0; i<csp.csize; i++)
          for (int j=Ap[i]; j<Ap[i+1]; j++)
            if (Ai[j] <= i)
              ofs << Ai[j] << " " << i << setprecision(16) << " " << Ax[j] << endl;
      }
    else
      {
        Eigen::IOFormat pfmt(16);

        int nrows = A.rows();
        int ncols = A.cols();

        for (int i=0; i<nrows; i++)
          for (int j=i; j<ncols; j++)
            {
              double a = A(i,j);
              if (A(i,j) != 0.0)
                ofs << i << " " << j << setprecision(16) << " " << a << endl;
            }
      }

    ofs.close();
  }

  // Set up spanning tree initialization
  void SysSPA::spanningTree(int node)
  {
    int nnodes = nodes.size();

    // set up an index from nodes to their constraints
    vector<vector<int> > cind;
    cind.resize(nnodes);

    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        ConP2 &con = p2cons[pi];
        int i0 = con.ndr;
        int i1 = con.nd1;
        cind[i0].push_back(i1);
        cind[i1].push_back(i0);
      }

    // set up breadth-first algorithm
    VectorXd dist(nnodes);
    dist.setConstant(1e100);
    if (node >= nnodes)
      node = 0;
    dist[node] = 0.0;
    std::multimap<double,int> open;  // open list, priority queue - can have duplicates
    open.emplace(0.0,node);

    // do breadth-first computation
    while (!open.empty())
      {
        // get top node, remove it
        int ni = open.begin()->second;
        double di = open.begin()->first;
        open.erase(open.begin());
        if (di > dist[ni]) continue; // already dealt with

        // update neighbors
        Node &nd = nodes[ni];
        Matrix<double,3,4> n2w;
        transformF2W(n2w,nd.trans,nd.qrot); // from node to world coords

        vector<int> &nns = cind[ni];
        for (int i=0; i<(int)nns.size(); i++)
          {
            ConP2 &con = p2cons[nns[i]];
            double dd = con.tmean.norm(); // incremental distance
            // neighbor node index
            int nn = con.nd1;
            if (nn == ni)
              nn = con.ndr;
            Node &nd2 = nodes[nn];
            Vector3d tmean = con.tmean;
            Quaterniond qpmean = con.qpmean;
            if (nn == con.ndr)       // wrong way, reverse
              {
                qpmean = qpmean.inverse();
                tmean = nd.qrot.toRotationMatrix().transpose()*nd2.qrot.toRotationMatrix()*tmean;
              }

            if (dist[nn] > di + dd) // is neighbor now closer?
              {
                // set priority queue
                dist[nn] = di+dd;
                open.emplace(di+dd,nn);
                // update initial pose
                Vector4d trans;
                trans.head(3) = tmean;
                trans(3) = 1.0;
                nd2.trans.head(3) = n2w*trans;
                nd2.qrot = qpmean*nd.qrot;
                nd2.normRot();
                nd2.setTransform();
                nd2.setDr(true);
              }
          }
      }

  }

}  // namespace sba