#include "MirrorPlasma.hpp"
#include "PlasmaPhysics.hpp"
#include "AtomicPhysics.hpp"

#include <cmath>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <algorithm>
#include <boost/math/quadrature/gauss_kronrod.hpp>

Species Electron{ .type = Species::Electron, .Charge = -1, .Mass = ElectronMass,  .Name = "Electron" };
Species Proton{ .type = Species::Ion, .Charge = 1, .Mass = ProtonMass, .Name = "Proton"};
Species Deuteron{ .type = Species::Ion, .Charge = 1, .Mass = 1.999*ProtonMass, .Name = "Deuteron" };
Species NeutralHydrogen{ .type = Species::Neutral, .Charge = 0, .Mass = ProtonMass + ElectronMass, .Name = "Neutral Hydrogen"};

// Cross Section objects for integration
CrossSection protonImpactIonization( protonImpactIonizationCrossSection, 200, 1e6, Proton, NeutralHydrogen );
CrossSection HydrogenChargeExchange( HydrogenChargeExchangeCrossSection, 0.1, 1e6, Proton, NeutralHydrogen );
CrossSection electronImpactIonization( electronImpactIonizationCrossSection, 13.6, 1e6, Electron, NeutralHydrogen );

double neutralsRateCoefficientHot( CrossSection const & sigma, MirrorPlasma const & plasma )
{
	// E and T in eV, sigma in cm^2
	// k=<σv> in m^3/s
	// Assumes a Maxwellian distribution, COM energy (as opposed to incident)

	double temperature;
	if ( sigma.Particle.Name == "Electron" ){
		temperature = plasma.ElectronTemperature * ReferenceTemperature; // Convert to Joules
	}
	else{
		temperature = plasma.IonTemperature * ReferenceTemperature; // Convert to Joules
	}

	double Jacobian = ElectronCharge; // The integral is over Energy, which is in units of electronvolts, so transform the integrand back to eV
	auto integrand = [&]( double Energy ) {
		double sigmaM2 = sigma( Energy ) * 1e-4; // sigma is in cm^2, we need m^2
		return Energy * ElectronCharge * sigmaM2 * ::exp( -Energy * ElectronCharge / temperature ) * Jacobian;
	};

	constexpr double tolerance = 1e-5;
	constexpr unsigned MaxDepth = 10;
	double HotRateCoeff = 4.0 / ( ::sqrt(2 * M_PI * sigma.ReducedMass * temperature) * temperature )
	         * boost::math::quadrature::gauss_kronrod<double, 255>::integrate( integrand, sigma.MinEnergy, sigma.MaxEnergy, MaxDepth, tolerance );

#if defined( DEBUG ) && defined( ATOMIC_PHYSICS_DEBUG )
	std::cerr << "Computing a hot rate coefficient at T = " << plasma.ElectronTemperature/1000 << " eV and M = " << plasma.MachNumber << " gave <sigma v> = " << HotRateCoeff  << std::endl;
#endif

	return HotRateCoeff;

}

double neutralsRateCoefficientCold( CrossSection const & sigma, MirrorPlasma const & plasma )
{
	// sigma in cm^2
	// k=<σv> in m^3/s
	// Assumes ions are Maxwellian and neutrals are stationary
	double temperature;
	if ( sigma.Particle.Name == "Electron" ){
		temperature = plasma.ElectronTemperature * ReferenceTemperature; // Convert to Joules
	}
	else{
		temperature = plasma.IonTemperature * ReferenceTemperature; // Convert to Joules
	}

	double thermalSpeed = ::sqrt( 2.0 * temperature / sigma.Particle.Mass );
	double thermalMachNumber = plasma.MachNumber * ::sqrt( ::abs( sigma.Particle.Charge ) * plasma.ElectronTemperature * ReferenceTemperature / ( 2 * temperature ) );

	double Jacobian = ElectronCharge / (sigma.ReducedMass * thermalSpeed * thermalSpeed); // The integral is over Energy, which is in units of electronvolts, so transform the integrand back to eV, including change of variables from du to dE (less one power of u, which cancels with one in the integrand
	auto integrand = [&]( double Energy ) {
		double velocity = ::sqrt( 2.0 * Energy * ElectronCharge / sigma.ReducedMass );
		double u = velocity / thermalSpeed;
		double sigmaM2 = sigma( Energy ) * 1e-4; // sigma is in cm^2, we need m^2
		return u * sigmaM2 * ( ::exp( -::pow( thermalMachNumber - u, 2 ) ) - ::exp( -::pow( thermalMachNumber + u, 2 ) ) ) * Jacobian;
	};

	constexpr double tolerance = 1e-5;
	constexpr unsigned MaxDepth = 10;
	double ColdRateCoeff = thermalSpeed / ( thermalMachNumber * ::sqrt(M_PI) )
	        * boost::math::quadrature::gauss_kronrod<double, 255>::integrate( integrand, sigma.MinEnergy, sigma.MaxEnergy, MaxDepth, tolerance );

#if defined( DEBUG ) && defined( ATOMIC_PHYSICS_DEBUG )
	std::cerr << "Computing a cold rate coefficient at T = " << plasma.ElectronTemperature*1000 << " eV and M = " << plasma.MachNumber << " gave <sigma v> = " << ColdRateCoeff  << std::endl;
#endif

	return ColdRateCoeff;
}

/*
double meanFreePath( double density, double crossSection )
{
	return 1.0 / ( density * crossSection );
}

bool isShortMeanFreePathRegime( double meanFreePath, double characteristicLength, double minRatio = 10.0 )
{
	return ( characteristicLength / meanFreePath ) >= minRatio;
}

bool isCoronalEquilibrium( double excitationRate, double deexcitationRate, double minRatio = 10.0 )
{
	return ( deexcitationRate / excitationRate ) >= minRatio;
}
*/

double reactionRate( double density_1, double density_2, double rateCoefficient, std::shared_ptr<MirrorPlasma> pMirrorPlasma )
{
	return density_1 * density_2 * rateCoefficient * pMirrorPlasma->pVacuumConfig->PlasmaVolume();
}

double EvaluateJanevCrossSectionFit ( std::vector<double> PolynomialCoefficients, double Energy )
{
	constexpr unsigned int N_JANEV_COEFFS = 9;
	if ( PolynomialCoefficients.size() != N_JANEV_COEFFS )
		throw std::invalid_argument( "Janev uses fixed order fits, there are not " + std::to_string( N_JANEV_COEFFS ) + " numbers. Something is wrong." );
	double sum = 0.0;
	for ( size_t n = 0; n < PolynomialCoefficients.size(); n++ ) {
      sum += PolynomialCoefficients.at( n ) * ::pow( ::log(Energy), n );
   }
	double sigma = ::exp(sum);
	return sigma;
}

/*
double evaluateJanevDFunction( double beta )
{
	// Function from Janev 1987 Appendix C
	// Used in some analytical fits for cross sections
	double DFunctionVal;
	if ( beta < 1e-3 ){
		DFunctionVal = 4 * beta * ::log( 1.4 / beta );
	} else if ( beta > 10 ) {
		DFunctionVal = beta / 2 * ::exp( - ::sqrt( 2 * beta ) );
	} else {
		// Create look up table and linearly interpolate to find DFunction
		std::vector<double> betaInverse = { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.8, 1.0, 1.25, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 6.0, 8.0, 10.0, 12.5, 15.0, 20.0, 25.0, 30.0, 40.0, 60.0, 80.0, 100.0, 15.0, 200.0, 300.0, 400.0, 600.0, 800.0, 1000.0 };
		std::vector<double> DFunction = { 0.057, 0.104, 0.135, 0.157, 0.175, 0.194, 0.230, 0.264, 0.296, 0.328, 0.367, 0.388, 0.399, 0.405, 0.410, 0.405, 0.399, 0.380, 0.345, 0.318, 0.285, 0.263, 0.227, 0.205, 0.190, 0.168, 0.141, 0.124, 0.110, 0.092, 0.080, 0.054, 0.042, 0.035, 0.028 };
		for (size_t i = 0; i < betaInverse.size(); i++ ){
			if ( 1.0 / beta < betaInverse.at( i ) ){}
			else {
				double x1 = betaInverse.at( i - 1 );
				double x2 = betaInverse.at ( i );
				double y1 = DFunction.at( i - 1 );
				double y2 = DFunction.at ( i );
				DFunctionVal = y1 + ( y2 - y1 ) * ( 1.0 / beta - x1 ) / ( x2 - x1 );
			}
		}
	}
	return DFunctionVal;
}
*/

double electronImpactIonizationCrossSection( double CoMEnergy )
{
	// Minimum energy of cross section in eV
	constexpr double ionizationEnergy = 13.6;
	constexpr double minimumEnergySigma = ionizationEnergy;

	// Contribution from ground state
	// Janev 1993, ATOMIC AND PLASMA-MATERIAL INTERACTION DATA FOR FUSION, Volume 4
	// Equation 1.2.1
	// e + H(1s) --> e + H+ + e
	// Accuracy is 10% or better
	constexpr double fittingParamA = 0.18450;
	constexpr std::array<double,5> fittingParamB{ -0.032226, -0.034539, 1.4003, -2.8115, 2.2986 };

	double sigma;
	if ( CoMEnergy < minimumEnergySigma ) {
		sigma = 0;
	}
	else {
		double sum = 0.0;
		double x = 1.0 - ionizationEnergy / CoMEnergy;
		for ( size_t n = 0; n < fittingParamB.size(); n++ ) {
	      sum += fittingParamB.at( n ) * ::pow( x, n );
	   }
		sigma = 1.0e-13 / ( ionizationEnergy * CoMEnergy ) * ( fittingParamA * ::log( CoMEnergy / ionizationEnergy ) + sum );
	}
	return sigma;
}

// Energy in electron volts, returns cross section in cm^2
double protonImpactIonizationCrossSection( double Energy )
{
	// Minimum energy of cross section in keV
	const double minimumEnergySigma = 0.2;
	// Convert to keV
	double CoMEnergy = Energy / 1000;

	// Contribution from ground state
	// Janev 1993, ATOMIC AND PLASMA-MATERIAL INTERACTION DATA FOR FUSION, Volume 4
	// Equation 2.2.1
	// H+ + H(1s) --> H+ + H+ + e
	// Accuracy is 30% or better
	constexpr double A1 = 12.899;
	constexpr double A2 = 61.897;
	constexpr double A3 = 9.2731e3;
	constexpr double A4 = 4.9749e-4;
	constexpr double A5 = 3.9890e-2;
	constexpr double A6 = -1.5900;
	constexpr double A7 = 3.1834;
	constexpr double A8 = -3.7154;

	double sigma;
	if ( CoMEnergy < minimumEnergySigma ) {
		sigma = 0;
	}
	else {
		// Energy is in units of keV
		sigma = 1e-16 * A1 * ( ::exp( -A2 / CoMEnergy ) * ::log( 1 + A3 * CoMEnergy ) / CoMEnergy + A4 * ::exp( -A5 * CoMEnergy ) / ( ::pow( CoMEnergy, A6 ) + A7 * ::pow( CoMEnergy, A8 ) ) );
	}
	return sigma;
}

double HydrogenChargeExchangeCrossSection( double CoMEnergy )
{
	// Minimum energy of cross section in eV
	// const double minimumEnergySigma_1s = 0.1;
	const double minimumEnergySigma_2p = 19.0;
	const double minimumEnergySigma_2s = 0.1;

	// Contribution from ground -> ground state
	// Janev 1987 3.1.8
	// p + H(1s) --> H(1s) + p
	double sigma_1s;
	if ( CoMEnergy < minimumEnergySigma_2p ) {
		sigma_1s = 0;
	} else {
		sigma_1s = 0.6937e-14 * ::pow( 1 - 0.155 * ::log10( CoMEnergy ), 2 ) / (1 + 0.1112e-14 * ::pow( CoMEnergy, 3.3 ));
	}

	// Janev 1987 3.1.9
	// p + H(1s) --> H(2p) + p
	std::vector<double> aSigma_2p = {-2.197571949935e+01, -4.742502251260e+01, 3.628013140596e+01, -1.423003075866e+01, 3.273090240144e+00, -4.557928912260e-01, 3.773588347458e-02, -1.707904867106e-03, 3.251203344615e-05};
	// Janev 1987 3.1.10
	// p + H(1s) --> H(2s) + p
	std::vector<double> aSigma_2s = {-1.327325087764e+04, 1.317576614520e+04, -5.683932157858e+03, 1.386309780149e+03, -2.089794561307e+02, 1.992976245274e+01, -1.173800576157e+00, 3.902422810767e-02, -5.606240339932e-04};

	// Contribution from ground -> 2p orbital
	double sigma_2p;
	if ( CoMEnergy < minimumEnergySigma_2p ) {
		sigma_2p = 0;
	} else {
		sigma_2p = EvaluateJanevCrossSectionFit( aSigma_2p, CoMEnergy );
	}

	// Contribution from ground -> 2s orbital
	double sigma_2s;
	if ( CoMEnergy < minimumEnergySigma_2s ) {
		sigma_2s = 0;
	} else {
		sigma_2s = EvaluateJanevCrossSectionFit( aSigma_2s, CoMEnergy );
	}

	return sigma_1s + sigma_2p + sigma_2s;
}

// Energy in electron volts, returns cross section in cm^2
double radiativeRecombinationCrossSection( double Energy )
{
	// From https://iopscience-iop-org.proxy-um.researchport.umd.edu/article/10.1088/1402-4896/ab060a
	// Igor A Kotelnikov and Alexander I Milstein 2019 Phys. Scr. 94 055403
	// Equation 9
	// H+ + e --> H + hν

	int Z = 1;
	double IonizationEnergy = 13.59844; // eV
	double J_Z = ::pow( Z, 2 ) * IonizationEnergy;
	double eta = ::sqrt( J_Z / Energy );
	double sigma = ::pow( 2, 8 ) * ::pow( M_PI * BohrRadius, 2 ) / 3 * ::pow( eta, 6 ) * ::exp( - 4 * eta * atan( 1 / eta ) ) / ( ( 1 - ::exp( -2 * M_PI * eta ) ) * ::pow( ::pow( eta, 2 ) + 1, 2 ) ) * ::pow( FineStructureConstant, 3 );

	sigma *= 1e4; // convert to cm^2
	return sigma;
}

// double electronHydrogenExcitationN2CrossSection( double Te )
// {
// 	// Minimum energy of cross section in eV
// 	const double excitationEnergy = 10.2;
// 	const double cutoffEnergy1 = 11.56;
// 	const double cutoffEnergy2 = 12.23;
//
// 	// Contribution from ground state
// 	// Janev 1993, ATOMIC AND PLASMA-MATERIAL INTERACTION DATA FOR FUSION, Volume 4
// 	// Equation 1.1.3
// 	// e + H(1s) --> e + H*(n=2)
// 	// Accuracy is 10% or better
// 	std::vector<double> fittingParamA = { 1.4182, -20.877, 49.735, -46.249, 17.442, 4.4979 };
//
// 	double sigma;
// 	if ( Te < excitationEnergy ) {
// 		sigma = 0;
// 	}
// 	else if ( Te < cutoffEnergy1 ) {
// 		sigma = 1e-16 * ( 0.255 + 0.1865 * ( Te - excitationEnergy ) );
// 	}
// 	else if ( Te < cutoffEnergy2 ) {
// 		sigma = 5.025e-17;
// 	}
// 	else {
// 		double sum = 0.0;
// 		double XEnergy = Te / excitationEnergy;
// 		for ( size_t n = 0; n < fittingParamA.size() - 1; n++ ) {
// 	      sum += fittingParamA.at( n ) / ::pow( XEnergy, n - 1 );
// 	   }
// 		sigma = 5.984e-16 / Te * ( sum + fittingParamA.back() * ::log( XEnergy ) );
// 	}
// 	return sigma;
// }
//
// double protonHydrogenExcitationN2CrossSection( double Ti )
// {
// 	// Minimum energy of cross section in keV
// 	const double minimumEnergySigma = 0.6;
// 	double TiKEV = Ti / 1000;
//
// 	// Contribution from ground state
// 	// Janev 1993, ATOMIC AND PLASMA-MATERIAL INTERACTION DATA FOR FUSION, Volume 4
// 	// Equation 2.2.1
// 	// H+ + H(1s) --> H+ + H+ + e
// 	// Accuracy is 100% or better
// 	const double A1 = 34.433;
// 	const double A2 = 44.057;
// 	const double A3 = 0.56870;
// 	const double A4 = 8.5476;
// 	const double A5 = 7.8501;
// 	const double A6 = -9.2217;
// 	const double A7 = 1.8020e-2;
// 	const double A8 = 1.6931;
// 	const double A9 = 1.9422e-3;
// 	const double A10 = 2.9068;
//
// 	double sigma;
// 	if ( TiKEV < minimumEnergySigma ) {
// 		sigma = 0;
// 	}
// 	else {
// 		// Energy is in units of keV
// 		sigma = 1e-16 * A1 * ( ::exp( -A2 / TiKEV ) * ::log( 1 + A3 * TiKEV ) / TiKEV + A4 * ::exp( -A5 * TiKEV ) / ( ::pow( TiKEV, A6 ) ) + A7 * ::exp( -A8 / TiKEV ) / ( 1 + A9 * ::pow( TiKEV, A10 ) ) );
// 	}
// 	return sigma;
// }

/*
 * Use the above functions to set steady-state neutral density/source
 *
 */
void MirrorPlasma::ComputeSteadyStateNeutrals()
{
	// Do not recalculate if we're in fixed-neutral-density mode
	if ( FixedNeutralDensity )
		return;

	// Calculate the Ionization Rate of cold neutrals from proton and electron impact:
	double IonizationRate =
		neutralsRateCoefficientCold( protonImpactIonization, *this ) * IonDensity * ReferenceDensity +
		neutralsRateCoefficientCold( electronImpactIonization, *this ) * ElectronDensity * ReferenceDensity;

#if defined( DEBUG ) && defined( ATOMIC_PHYSICS_DEBUG )
	std::cerr << "Current Ionization Rate is " << IonizationRate << std::endl;
#endif

	// Assume mix of neutrals is such that the particle densities are maintained so we just need to produce enough electrons from the source gas to balance the losses
	double ElectronLossRate = ParallelElectronParticleLoss() + ClassicalElectronParticleLosses();
	// Steady State requires
	//		Losses = n_N * n_e * IonizationRateCoefficient * Volume
	// so n_N = (Losses/Volume) / ( n_e * IonizationRateCoefficient )
	//		    = (Losses/Volume) / IonizationRate
	NeutralDensity = ElectronLossRate / ( IonizationRate );
	// Normalize neutral density
	NeutralDensity = NeutralDensity / ReferenceDensity;
	NeutralSource = ElectronLossRate;
}

// Number of ions lost as neutrals per second per unit volume
// requires computed neutral density
double MirrorPlasma::CXLossRate() const
{
	if ( !pVacuumConfig->IncludeCXLosses )
		return 0.0;

	double CXRateCoefficient = neutralsRateCoefficientCold( HydrogenChargeExchange, *this );

#if defined( DEBUG ) && defined( ATOMIC_PHYSICS_DEBUG )
	std::cerr << "Current CX Loss Rate is " << CXRateCoefficient * ( NeutralDensity * ReferenceDensity * IonDensity * ReferenceDensity ) << " particles/s"<<std::endl;
#endif

	return CXRateCoefficient * ( NeutralDensity * ReferenceDensity * IonDensity * ReferenceDensity );
}


#include <opengv/optimization_tools/objective_function_tools/GlobalPnPFunctionInfo.hpp>
#include <Eigen/Dense>
#include <opengv/Indices.hpp>
#include <iostream>

GlobalPnPFunctionInfo::GlobalPnPFunctionInfo(const opengv::absolute_pose::AbsoluteAdapterBase & adapter){

  opengv::Indices indices(adapter.getNumberCorrespondences());
  int total_points = (int) indices.size();
  //std::cout << "Total points: " << total_points << std::endl;
  Mt =  Eigen::Matrix3d::Zero(3,3);
  Mrt = Eigen::MatrixXd::Zero(3,9);
  Mr =  Eigen::MatrixXd::Zero(9,9);
  vt =  Eigen::VectorXd::Zero(3,1);
  vr =  Eigen::VectorXd::Zero(9,1);
  //Eigen::MatrixXd Constant = Eigen::MatrixXd::Zero(1,1);

  //Variables defined for debugging
  /*Eigen::MatrixXd C_all = Eigen::MatrixXd::Zero(3, total_points);
  Eigen::MatrixXd V_all = Eigen::MatrixXd::Zero(3, total_points);
  Eigen::MatrixXd X_all = Eigen::MatrixXd::Zero(3, total_points);*/
  Eigen::Matrix3d id = Eigen::Matrix3d::Identity(3,3);

  Eigen::Matrix<double,3,1> vi;
  Eigen::Matrix<double,3,1> xi;
  Eigen::Matrix<double,3,1> ci;
  Eigen::Matrix3d Qi;
  Eigen::Matrix3d Vi;
  Eigen::MatrixXd Mr_i  = Eigen::MatrixXd::Zero(3,9);
  Eigen::MatrixXd Mrt_i = Eigen::MatrixXd::Zero(3,9);
  Eigen::Matrix3d I_V   = Eigen::MatrixXd::Zero(3,3);
  Eigen::MatrixXd vr_1  = Eigen::MatrixXd::Zero(1,3);
  Eigen::MatrixXd vr_2  = Eigen::MatrixXd::Zero(1,9);
  for( int i = 0; i < total_points; i++ )
  {
    vi = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    xi = adapter.getPoint(indices[i]);
    ci = adapter.getCamOffset(indices[i]);
    /*C_all.block<3,1>(0,i) = ci;
    X_all.block<3,1>(0,i) = xi;
    V_all.block<3,1>(0,i) = vi;*/
    Vi = vi * vi.transpose() / (vi.transpose() * vi);
    Qi = (id - Vi).transpose() * (id - Vi);

    I_V = id - Vi;
    Mt = Mt + Qi;
    vt = vt - (2 * Qi * ci);
    //Constant = Constant + ci.transpose() * Qi * ci;

    //Calculate Mr
    Mr_i.block<3,3>(0,0) = xi(0,0) * I_V;
    Mr_i.block<3,3>(0,3) = xi(1,0) * I_V;
    Mr_i.block<3,3>(0,6) = xi(2,0) * I_V;

    Mr = Mr + Mr_i.transpose() * Mr_i;

    //Calculate Mrt
    Mrt_i.block<3,3>(0,0) = xi(0,0) * Qi;
    Mrt_i.block<3,3>(0,3) = xi(1,0) * Qi;
    Mrt_i.block<3,3>(0,6) = xi(2,0) * Qi;

    Mrt = Mrt + 2*Mrt_i;

    //Calculate vr
    vr_1 = ci.transpose() * Qi;
    vr_2.block<1,3>(0,0) = -2 * xi(0,0) * vr_1;
    vr_2.block<1,3>(0,3) = -2 * xi(1,0) * vr_1;
    vr_2.block<1,3>(0,6) = -2 * xi(2,0) * vr_1;
    vr = vr + vr_2.transpose();
  }
  /*std::cout << "Data:" << std::endl;
  std::cout << "ci" << std::endl << C_all << std::endl;
  std::cout << "xi" << std::endl << X_all << std::endl;
  std::cout << "vi" << std::endl << V_all << std::endl;

  std::cout << "Final results: " << std::endl;
  std::cout << "Mt: "  << std::endl << Mt << std::endl;
  std::cout << "Mrt: " << std::endl << Mrt << std::endl;
  std::cout << "Mr:"   << std::endl << Mr  << std::endl;
  std::cout << "vt:"   << std::endl << vt  << std::endl;
  std::cout << "vr:"   << std::endl << vr   << std::endl;
  std::cout << "Const: " << std::endl << Constant << std::endl;*/
}

GlobalPnPFunctionInfo::~GlobalPnPFunctionInfo(){};

double GlobalPnPFunctionInfo::objective_function_value(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  const double * p = &rotation(0);
  Map<const Matrix<double,1,9> > r(p, 1, 9);
  Eigen::MatrixXd e = (translation.transpose() * Mt * translation)
    + (translation.transpose() * Mrt *  r.transpose())
    + (vt.transpose() * translation)
    + (r * Mr * r.transpose() )
    + (vr.transpose() * r.transpose());
  return ( e(0,0));
}

opengv::rotation_t GlobalPnPFunctionInfo::rotation_gradient(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  const double * p = &rotation(0);
  Map<const Matrix<double,1,9> > r(p, 1, 9);
  Eigen::MatrixXd result = (2 * Mr * r.transpose()) + ( Mrt.transpose() * translation ) + vr;
  double * ptr = &result(0);
  Map<Matrix<double, 3,3> > m(ptr, 3, 3);
  return m;
}

opengv::translation_t GlobalPnPFunctionInfo::translation_gradient(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  const double * p = &rotation(0);
  Map<const Matrix<double,1,9> > r(p, 1, 9);
  return ( (2 * Mt * translation) + (  Mrt * r.transpose() ) + vt );
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Matrix types for filter classes
 *  Provides the predefined type 'Vec' and a variety of 'Matrix' types
 *  Replace this header to substitute alternative matrix support
 *
 * Everything in namespace Bayes_filter_matrix is intended to support the matrix storage
 * and algebra requirements of the library. Therefore the interfaces and implementation is
 * not intended to be stable.
 */

/*
 * Use the Boost uBLAS Basic Linear Algebra library
 * That is boost::numeric::ublas
 *  Thanks to Joerg Walter and Mathias Koch for an excellent library!
 *
 * Gappy matrix support: The macros BAYES_FILTER_(SPARSE/COMPRESSED/COORDINATE) control experimental gappy matrix support
 * When enabled the default storage types are replaced with their sparse equivalents
 */

#include <boost/version.hpp>
#if !(BOOST_VERSION >= 103200)
#error Requires Boost 1.32.0 or later
#endif

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/banded.hpp>
#if defined(BAYES_FILTER_MAPPED) || defined(BAYES_FILTER_COMPRESSED) || defined(BAYES_FILTER_COORDINATE)
#include <map>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#define BAYES_FILTER_GAPPY
#endif

/* Filter Matrix Namespace */
namespace Bayesian_filter_matrix
{
						// Allow use of a local ublas namespace
namespace ublas = boost::numeric::ublas;

/*
 * Declare the value used for ALL linear algebra operations
 * Also required as the matrix/vector container value_type
 */
typedef double Float;

/*
 * uBlas base types - these will be wrapper to provide the actual vector and matrix types
 *  Symmetric types don't appear. They are defined later by adapting these base types
 */
namespace detail {
							// Dense types
typedef ublas::vector<Float> BaseDenseVector;
typedef ublas::matrix<Float, ublas::row_major> BaseDenseRowMatrix;
typedef ublas::matrix<Float, ublas::column_major> BaseDenseColMatrix;
typedef ublas::triangular_matrix<Float, ublas::upper, ublas::row_major> BaseDenseUpperTriMatrix;
typedef ublas::triangular_matrix<Float, ublas::lower, ublas::row_major> BaseDenseLowerTriMatrix;
typedef ublas::banded_matrix<Float> BaseDenseDiagMatrix;
							// Mapped types
#if defined(BAYES_FILTER_MAPPED)
typedef ublas::mapped_vector<Float, std::map<std::size_t,Float> > BaseSparseVector;
typedef ublas::mapped_matrix<Float, ublas::row_major, std::map<std::size_t,Float> > BaseSparseRowMatrix;
typedef ublas::mapped_matrix<Float, ublas::column_major, std::map<std::size_t,Float> > BaseSparseColMatrix;
							// OR Compressed types
#elif defined(BAYES_FILTER_COMPRESSED)
typedef ublas::compressed_vector<Float> BaseSparseVector;
typedef ublas::compressed_matrix<Float, ublas::row_major> BaseSparseRowMatrix;
typedef ublas::compressed_matrix<Float, ublas::column_major> BaseSparseColMatrix;
							// OR Coordinate types
#elif defined(BAYES_FILTER_COORDINATE)
typedef ublas::coordinate_vector<Float> BaseSparseVector;
typedef ublas::coordinate_matrix<Float, ublas::row_major> BaseSparseRowMatrix;
typedef ublas::coordinate_matrix<Float, ublas::column_major> BaseSparseColMatrix;
#endif

							// Default types Dense or Gappy
#ifndef BAYES_FILTER_GAPPY
typedef BaseDenseVector BaseVector;
typedef BaseDenseRowMatrix BaseRowMatrix;
typedef BaseDenseColMatrix BaseColMatrix;
typedef BaseDenseUpperTriMatrix BaseUpperTriMatrix;
typedef BaseDenseLowerTriMatrix BaseLowerTriMatrix;
typedef BaseDenseDiagMatrix BaseDiagMatrix;
#else
typedef BaseSparseVector BaseVector;
typedef BaseSparseRowMatrix BaseRowMatrix;
typedef BaseSparseColMatrix BaseColMatrix;
typedef BaseDenseUpperTriMatrix BaseUpperTriMatrix;		// No sparse triangular or banded
typedef BaseDenseLowerTriMatrix BaseLowerTriMatrix;
typedef BaseDenseDiagMatrix BaseDiagMatrix;
#endif

}

}//namespace

/*
 * Common type independent uBlas interface
 */
#include "uBLASmatrix.hpp"


#include <Eigen/Eigen>
#include <entt/entt.hpp>
#include <lmng/physics.h>
#include <lmng/shapes.h>

namespace lmng
{
Eigen::Vector3f get_extents(entt::registry const &registry, entt::entity entity)
{
    auto maybe_box_render = registry.try_get<lmng::box_render>(entity);
    if (maybe_box_render)
        return maybe_box_render->extents;

    auto maybe_box_collider = registry.try_get<lmng::box_collider>(entity);
    if (maybe_box_collider)
        return maybe_box_collider->extents;

    return Eigen::Vector3f{0.f, 0.f, 0.f};
}
} // namespace lmng


//=============================================================================
//
//  CLASS EigenArpackMatrixT
//
//=============================================================================

#ifndef COMISO_EIGENARPACKMATRIXT_HH
#define COMISO_EIGENARPACKMATRIXT_HH

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if (COMISO_SUITESPARSE_AVAILABLE && COMISO_EIGEN3_AVAILABLE)
//=============================================================================

//== INCLUDES =================================================================

#include <iostream>
#include <Eigen/Eigen>

#if EIGEN_VERSION_AT_LEAST(3,1,0)
  #include <Eigen/CholmodSupport>
#else
  #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
  #include <unsupported/Eigen/CholmodSupport>
#endif
#include <Eigen/Sparse>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class EigenArpackMatrixT EigenArpackMatrixT.hh <COMISO/.../EigenArpackMatrixT.hh>

    Brief Description.

    A more elaborate description follows.
*/

template <class RealT,class MatrixT>
class EigenArpackMatrixT
{
public:

  typedef MatrixT Matrix;
  typedef RealT   Real;

  /// Constructor
  template<class MatrixT2>
  EigenArpackMatrixT(const MatrixT2& _m, bool _use_inverse = false)
  {
    mat_ = _m;

    if(_use_inverse)
    {
      sllt_.compute(mat_);

#if EIGEN_VERSION_AT_LEAST(3,1,0)
      if ( !sllt_.info() != Eigen::Success )
#else
      if ( !sllt_.succeeded() )
#endif
        std::cout << "[ERROR] EigenArpackMatrix(): Could not compute llt factorization." << std::endl;
    }
  }

  /// Destructor
  ~EigenArpackMatrixT() {}

  // get reference on matrix
  Matrix& matrix() { return mat_; }

  // matrix-vector multiplication _w = mat_*_v
  void mult_Mv(Real* _v, Real* _w)
  {
    Eigen::Map<Eigen::Matrix<Real, Eigen::Dynamic, 1> >  v(_v,mat_.rows());     // uses v as a ArrayXf object
    Eigen::Map<Eigen::Matrix<Real, Eigen::Dynamic, 1> >  w(_w,mat_.cols());     // uses w as a ArrayXf object

    w = mat_*v;
  }

  // matrix-vector multiplication _w = mat_*_v
  void mult_M_inv_v(Real* _v, Real* _w)
  {
    Eigen::Map<Eigen::Matrix<Real, Eigen::Dynamic, 1> >  v(_v,mat_.rows());     // uses v as a ArrayXf object
    Eigen::Map<Eigen::Matrix<Real, Eigen::Dynamic, 1> >  w(_w,mat_.cols());     // uses w as a ArrayXf object

    w = sllt_.solve(v);

//    std::cerr << "input:" << std::endl;
//    std::cerr << v << std::endl;
//    std::cerr << "output:" << std::endl;
//    std::cerr << w << std::endl;
  }

private:

  Matrix mat_;

#if EIGEN_VERSION_AT_LEAST(3,1,0)
  Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<Real> > sllt_;
#else
  Eigen::SparseLLT<Eigen::SparseMatrix<Real>, Eigen::Cholmod> sllt_;
#endif
};

//=============================================================================
} // namespace COMISO
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(COMISO_EIGENARPACKMATRIXT_C)
#define COMISO_EIGENARPACKMATRIXT_TEMPLATES
#include "EigenArpackMatrixT.cc"
#endif
//=============================================================================
#endif // COMISO_SUITESPARSE_AVAILABLE
//=============================================================================
#endif // COMISO_EIGENARPACKMATRIXT_HH defined
//=============================================================================


//Machine-generated by miind.py. Edit at your own risk.

#include <boost/timer/timer.hpp>
#include <GeomLib.hpp>
#include <TwoDLib.hpp>
#include <MPILib/include/MPINetworkCode.hpp>
#include <MPILib/include/RateAlgorithmCode.hpp>
#include <MPILib/include/SimulationRunParameter.hpp>
#include <MPILib/include/report/handler/RootReportHandler.hpp>
#include <MPILib/include/WilsonCowanAlgorithm.hpp>
#include <MPILib/include/PersistantAlgorithm.hpp>
#include <MPILib/include/DelayAlgorithmCode.hpp>
#include <MPILib/include/RateFunctorCode.hpp>
#include "Euler.hpp"

typedef MPILib::MPINetwork<MPILib::DelayedConnection, MPILib::utilities::CircularDistribution> Network;
	// defining variables
const double TIME_END = 10000.0;

vector<TwoDLib::Redistribution> RetrieveMappingFromXML(const std::string& type, pugi::xml_node root)
{
	Pred pred(type);
	pugi::xml_node rev_node = root.find_child(pred);

	if (rev_node.name() != std::string("Mapping") ||
	    rev_node.attribute("type").value() != type)
	  throw TwoDLib::TwoDLibException("Couldn't find mapping in model file");

	std::ostringstream ostrev;
	rev_node.print(ostrev);
	std::istringstream istrev(ostrev.str());
	vector<TwoDLib::Redistribution> vec_rev = TwoDLib::ReMapping(istrev);
	return vec_rev;
}

TwoDLib::Mesh RetrieveMeshFromXML(pugi::xml_node root)
{
        pugi::xml_node mesh_node = root.first_child();
	if (mesh_node.name() != std::string("Mesh") )
	  throw TwoDLib::TwoDLibException("Couldn't find mesh node in model file");
	std::ostringstream ostmesh;
	mesh_node.print(ostmesh);
	std::istringstream istmesh(ostmesh.str());
	TwoDLib::Mesh mesh(istmesh);
	return mesh;
}

MPILib::Rate RateFunction_1(MPILib::Time t){
	return 3.5;
}
MPILib::Rate RateFunction_2(MPILib::Time t){
	return t > 1500 ? 3.5 : 0;
}

int main(int argc, char *argv[]){
	Network network;
	boost::timer::auto_cpu_timer t;

#ifdef ENABLE_MPI
	// initialise the mpi environment this cannot be forwarded to a class
	boost::mpi::environment env(argc, argv);
#endif

	try {	// generating algorithms

	pugi::xml_document doc;
	pugi::xml_parse_result result = doc.load_file("rinzel.model");
	pugi::xml_node  root = doc.first_child();

	TwoDLib::Mesh mesh1 = RetrieveMeshFromXML(root);
	TwoDLib::Mesh mesh2 = mesh1;

	std::vector<TwoDLib::Redistribution> vec_rev1 = RetrieveMappingFromXML("Reversal",root);
	std::vector<TwoDLib::Redistribution> vec_rev2 = vec_rev1;
	std::vector<TwoDLib::Redistribution> vec_res1 = RetrieveMappingFromXML("Reset",root);
	std::vector<TwoDLib::Redistribution> vec_res2 = vec_res1;

	std::vector<TwoDLib::Mesh> vec_vec_mesh { mesh1, mesh2 };
	std::vector< std::vector<TwoDLib::Redistribution> > vec_vec_rev { vec_rev1, vec_rev2 };
	std::vector< std::vector<TwoDLib::Redistribution> > vec_vec_res { vec_res1, vec_res2 };

	TwoDLib::Ode2DSystemGroup group(vec_vec_mesh,vec_vec_rev, vec_vec_res);
	group.Initialize(0,0,0);
	group.Initialize(1,0,0);

	TwoDLib::TransitionMatrix mat1("rinzel_0.1_0_0_0_.mat");
        TwoDLib::TransitionMatrix mat2("rinzel_-0.1_0_0_0_.mat");

	TwoDLib::CSRMatrix csr01(mat2,group,0); // inhibitory so mat2
	TwoDLib::CSRMatrix csr02(mat1,group,0); // excitatory so mat1; mesh id 0 relates to the first mesh
	TwoDLib::CSRMatrix csr10(mat2,group,1); // inhibitory so mat2; mesh id 1 relates to the second mesh
	TwoDLib::CSRMatrix csr13(mat1,group,1); // excitatory so mat 1

	std::vector<TwoDLib::CSRMatrix> vecmat{csr01, csr02, csr10, csr13}; // the input rates corresponding to these matrices must be presented in the same order

	// Establish number of steps
	MPILib::Time t_step = mesh1.TimeStep();
	MPILib::Number n_steps = static_cast<MPILib::Number>(floor(TIME_END/t_step));
	std::cout << "Number of simulation steps: " << n_steps << std::endl;;

	// Placeholder for firing rates
	const MPILib::Number nr_populations = 4;
	std::vector<MPILib::Rate> vec_rates(nr_populations,0.0);
	std::vector<MPILib::Rate> vec_input(nr_populations,0.0);

	// create a vector for the derivative
	std::vector<double> dydt(group.Mass().size());

	// Euler step parameter
	TwoDLib::MasterParameter par(500);
	double h = 1./par._N_steps*mesh1.TimeStep();

	// generating connections
	DelayedConnection con_0_2_0(1.,0.1,0);
	DelayedConnection con_1_3_0(1.,0.1,0);
	DelayedConnection con_1_0_0(500.,-0.1,0);
	DelayedConnection con_0_1_0(500.,-0.1,0);

	for(MPILib::Index istep = 0; istep < n_steps; istep++){
	  MPILib::Time t = t_step*istep;
	  group.Evolve();
	  // retain Hugh's original numbering convention

	  vec_input[0] = con_0_1_0._number_of_connections*vec_rates[1];
	  vec_input[1] = con_0_2_0._number_of_connections*vec_rates[2];
	  vec_input[2] = con_1_0_0._number_of_connections*vec_rates[0];
	  vec_input[3] = con_1_3_0._number_of_connections*vec_rates[3];

	  for (MPILib::Index i_part = 0; i_part < par._N_steps; i_part++ ){
	    TwoDLib::ClearDerivative(dydt);
	    TwoDLib::CalculateDerivative(group,dydt,vecmat,vec_input);
	    TwoDLib::AddDerivative(group.Mass(),dydt,h);
	  }

	  group.RedistributeProbability();
	  group.RemapReversal();

	  vec_rates[0] = group.F()[0];
	  vec_rates[1] = group.F()[1];
	  vec_rates[2] = RateFunction_1(t);
	  vec_rates[3] = RateFunction_2(t);

	  if (istep%1 == 0) std::cout << mesh1.TimeStep()*istep << " " << group.F()[0] << " " << group.F()[1] << std::endl;
	}

	std::ofstream ofst1("dens1.dat");
	std::ofstream ofst2("dens2.dat");

	std::vector<std::ostream*> vec_stream{ &ofst1, &ofst2};
	group.Dump(vec_stream);

	} catch(std::exception& exc){
		std::cout << exc.what() << std::endl;
#ifdef ENABLE_MPI
	//Abort the MPI environment in the correct way :
	env.abort(1);
	#endif
	}

	std::cout << "Overall time spend\n";
	t.report();

	return 0;
}


#include <test/unit/math/prim/prob/hmm_util.hpp>
#include <stan/math/prim/prob/hmm_marginal.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random.hpp>
#include <test/unit/math/test_ad.hpp>
#include <test/unit/util.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

// For evaluation of the density, the C++ code is benchmarked against
// a forward algorithm written in R.
// TODO(charlesm93): Add public repo link with R script.
TEST_F(hmm_test, ten_transitions) {
  using stan::math::hmm_marginal;

  EXPECT_FLOAT_EQ(-18.37417, hmm_marginal(log_omegas_, Gamma_, rho_));

  // Differentiation tests
  auto hmm_functor = [](const auto& log_omegas, const auto& Gamma_unconstrained,
                        const auto& rho_unconstrained) {
    return hmm_marginal_test_wrapper(log_omegas, Gamma_unconstrained,
                                     rho_unconstrained);
  };

  stan::test::expect_ad(tols_, hmm_functor, log_omegas_, Gamma_unconstrained_,
                        rho_unconstrained_);
}

TEST_F(hmm_test, zero_transitions) {
  using stan::math::hmm_marginal;

  EXPECT_FLOAT_EQ(-1.520827, hmm_marginal(log_omegas_zero_, Gamma_, rho_));

  // Differentiation tests
  auto hmm_functor = [](const auto& log_omegas, const auto& Gamma_unconstrained,
                        const auto& rho_unconstrained) {
    return hmm_marginal_test_wrapper(log_omegas, Gamma_unconstrained,
                                     rho_unconstrained);
  };

  stan::test::expect_ad(tols_, hmm_functor, log_omegas_zero_,
                        Gamma_unconstrained_, rho_unconstrained_);
}

TEST(hmm_marginal, one_state) {
  using stan::math::hmm_marginal;
  int n_states = 1, p1_init = 1, gamma1 = 1, n_transitions = 10, abs_mu = 1,
      sigma = 1;
  Eigen::VectorXd rho(n_states);
  rho << p1_init;
  Eigen::MatrixXd Gamma(n_states, n_states);
  Gamma << gamma1;
  Eigen::VectorXd obs_data(n_transitions + 1);
  obs_data << -0.9692032, 1.6367754, 1.0339449, 0.9798393, 0.4829358, 2.7508704,
      0.3122448, 1.8316583, 1.6327319, 1.2097332, 0.4087620;
  Eigen::MatrixXd log_omegas(n_states, n_transitions + 1);
  for (int n = 0; n < n_transitions + 1; n++)
    log_omegas.col(n)[0] = state_lpdf(obs_data[n], abs_mu, sigma, 0);

  EXPECT_FLOAT_EQ(-14.89646, hmm_marginal(log_omegas, Gamma, rho));

  // Differentiation tests
  // In the case where we have one state, Gamma and rho
  // are fixed (i.e = 1)
  auto hmm_functor = [](const auto& log_omegas) {
    Eigen::MatrixXd Gamma(1, 1);
    Gamma << 1;
    Eigen::VectorXd rho(1);
    rho << 1;

    return hmm_marginal(log_omegas, Gamma, rho);
  };

  stan::test::ad_tolerances tols;
  stan::test::expect_ad(tols, hmm_functor, log_omegas);
}

TEST(hmm_marginal, exceptions) {
  using Eigen::MatrixXd;
  using Eigen::VectorXd;
  using stan::math::hmm_marginal;

  int n_states = 2;
  int n_transitions = 2;
  MatrixXd log_omegas(n_states, n_transitions + 1);
  MatrixXd Gamma(n_states, n_states);
  VectorXd rho(n_states);

  for (int i = 0; i < n_states; i++)
    for (int j = 0; j < n_transitions + 1; j++)
      log_omegas(i, j) = 1;

  rho(0) = 0.65;
  rho(1) = 0.35;
  Gamma << 0.8, 0.2, 0.6, 0.4;

  // Gamma is not square.
  MatrixXd Gamma_rec(n_states, n_states + 1);
  EXPECT_THROW_MSG(hmm_marginal(log_omegas, Gamma_rec, rho),
                   std::invalid_argument,
                   "hmm_marginal: Expecting a square matrix; rows of Gamma (2) "
                   "and columns of Gamma (3) must match in size");

  // Gamma has a column that is not a simplex.
  MatrixXd Gamma_bad = Gamma;
  Gamma_bad(0, 0) = Gamma(0, 0) + 1;
  EXPECT_THROW_MSG(hmm_marginal(log_omegas, Gamma_bad, rho), std::domain_error,
                   "hmm_marginal: Gamma[i, ] is not a valid simplex. "
                   "sum(Gamma[i, ]) = 2, but should be 1")

  // The size of Gamma is 0, even though there is at least one transition
  MatrixXd Gamma_empty(0, 0);
  EXPECT_THROW_MSG(
      hmm_marginal(log_omegas, Gamma_empty, rho), std::invalid_argument,
      "hmm_marginal: Gamma has size 0, but must have a non-zero size")

  // The size of Gamma is inconsistent with that of log_omega
  MatrixXd Gamma_wrong_size(n_states + 1, n_states + 1);

  EXPECT_THROW_MSG(hmm_marginal(log_omegas, Gamma_wrong_size, rho),
                   std::invalid_argument,
                   "hmm_marginal: Columns of Gamma (3)"
                   " and Rows of log_omegas (2) must match in size")

  // rho is not a simplex.
  VectorXd rho_bad = rho;
  rho_bad(0) = rho(0) + 1;
  EXPECT_THROW_MSG(hmm_marginal(log_omegas, Gamma, rho_bad), std::domain_error,
                   "hmm_marginal: rho is not a valid simplex. "
                   "sum(rho) = 2, but should be 1")

  // The size of rho is inconsistent with that of log_omega
  VectorXd rho_wrong_size(n_states + 1);
  EXPECT_THROW_MSG(
      hmm_marginal(log_omegas, Gamma, rho_wrong_size), std::invalid_argument,
      "hmm_marginal: rho has dimension = 3, expecting dimension = 2;"
      " a function was called with arguments of different scalar,"
      " array, vector, or matrix types, and they were not consistently sized;"
      "  all arguments must be scalars or multidimensional values of"
      " the same shape.")
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/range.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
using namespace boost::hana;

template <typename T>
void test() {
    BOOST_HANA_CONSTANT_ASSERT(range_c<T, 0, 0> == range(integral<T, 0>, integral<T, 0>));
    BOOST_HANA_CONSTANT_ASSERT(range_c<T, 0, 1> == range(integral<T, 0>, integral<T, 1>));
    BOOST_HANA_CONSTANT_ASSERT(range_c<T, 0, 2> == range(integral<T, 0>, integral<T, 2>));

    BOOST_HANA_CONSTANT_ASSERT(range_c<T, 1, 1> == range(integral<T, 1>, integral<T, 1>));
    BOOST_HANA_CONSTANT_ASSERT(range_c<T, 1, 2> == range(integral<T, 1>, integral<T, 2>));
    BOOST_HANA_CONSTANT_ASSERT(range_c<T, 1, 3> == range(integral<T, 1>, integral<T, 3>));
}

int main() {
    test<int>();
    test<long>();
    test<unsigned long>();
    test<unsigned long long>();
}


#include "kmeans_clustering.h"
#include "ocv_kmeans_wrapper.h"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <igl/list_to_matrix.h>

using namespace Eigen;

int kmeans_muscles_and_tendons(VectorXi& idx, int clusters_per_tendon, int clusters, int m, int clusters_per_muscle, std::vector<VectorXi>& imuscle, VectorXd& CAx0, VectorXd& relStiff){
    // std::cout<<"     kmeans1.5 deal with tendons"<<std::endl;
    // std::vector<int> tendon_elements_list;
    // std::vector<int> muscle_elements_list;
    // for(int i=0; i<imuscle[m].size(); i++){
    //     if(relStiff[imuscle[m][i]]>10){
    //         tendon_elements_list.push_back(imuscle[m][i]);
    //     }else{
    //         muscle_elements_list.push_back(imuscle[m][i]);
    //     }
    // }

    VectorXi muscle_els = imuscle[m];
    // igl::list_to_matrix(tendon_elements_list, tendon_els);
    // igl::list_to_matrix(muscle_elements_list, muscle_els);

    // //--------------DEAL WITH TENDONS
    // if(tendon_els.size()>0){
    //     MatrixXd TCentroids;
    //     VectorXi Tlabels;
    //     MatrixXd TData = MatrixXd::Zero(tendon_els.size(), 3);
    //     for(int i=0; i<TData.rows(); i++){
    //         TData.row(i) = RowVector3d(CAx0[12*tendon_els[i]+0],CAx0[12*tendon_els[i]+1],CAx0[12*tendon_els[i]+2]);
    //     }

    //     if(m==tendon_els.size()-1){
    //         //deal with remainder clusters
    //         ocv_kmeans(TData, clusters_per_tendon, 1000, TCentroids, Tlabels);
    //     }else{
    //         ocv_kmeans(TData, clusters_per_tendon, 1000, TCentroids, Tlabels);
    //     }

    //     std::cout<<"     kmeans4 create element_cluster_map"<<std::endl;
    //     for(int q=0; q<tendon_els.size(); q++){
    //         idx[tendon_els[q]] = clusters_per_muscle*m + Tlabels[q];
    //     }
    //     clusters = clusters - clusters_per_tendon;
    //     clusters_per_muscle -= clusters_per_tendon;
    //     //-------------------------------
    // }
    clusters_per_tendon=0;

    std::cout<<"     kmeans2 un"<<std::endl;
    MatrixXd Centroids;
    VectorXi labels;
    MatrixXd Data = MatrixXd::Zero(muscle_els.size(), 3);

    for(int i=0; i<Data.rows(); i++){
        Data.row(i) = RowVector3d(CAx0[12*muscle_els[i]+0],CAx0[12*muscle_els[i]+1],CAx0[12*muscle_els[i]+2]);
    }

    std::cout<<"     kmeans3 do clustering"<<std::endl;
    std::cout<<clusters<<std::endl;
    if(m==muscle_els.size()-1){
        //deal with remainder clusters
        ocv_kmeans(Data, clusters, 1000, Centroids, labels);
    }else{
        ocv_kmeans(Data, clusters_per_muscle, 1000, Centroids, labels);
    }

    std::cout<<"     kmeans4 create element_cluster_map"<<std::endl;
    for(int q=0; q<muscle_els.size(); q++){
        idx[muscle_els[q]] = (clusters_per_muscle)*m + labels[q]+clusters_per_tendon;
    }
    clusters = clusters - clusters_per_muscle;
    std::cout<<clusters<<std::endl;
    return clusters;
}

void kmeans_clustering(VectorXi& idx, int clusters, int clusters_per_tendon, std::vector<VectorXi>& ibones, std::vector<VectorXi>& imuscle, MatrixXd& mG, SparseMatrix<double>& mC, SparseMatrix<double>& mA, VectorXd& mx0, VectorXd& relStiff){
        idx.resize(mC.rows()/12);
        idx.setZero();
        std::cout<<"    kmeans0 un "<<std::endl;

        std::cout<<clusters<<std::endl;
        for(int b=0; b<ibones.size(); b++){
            for(int i=0; i<ibones[b].size(); i++){
                idx[ibones[b][i]] = clusters - 1 - b;
            }
        }
        clusters = clusters - ibones.size();

        std::cout<<"     kmeans1 un"<<std::endl;
        std::cout<<clusters<<std::endl;
        VectorXd CAx0 = mC*mA*mx0;

        int clusters_per_muscle = clusters/imuscle.size();

        for(int m=0; m<imuscle.size(); m++){
            clusters = kmeans_muscles_and_tendons(idx, clusters_per_tendon, clusters, m, clusters_per_muscle, imuscle, CAx0, relStiff);
        }

        std::cout<<"    kmeans5 un"<<std::endl;

        assert(clusters==0);
        return;
    }