/** MIT License

Copyright (c) 2018 Benjamin Bercovici and Jay McMahon

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include "Tests.hpp"

#include <vtkObjectFactory.h>
#include <SBGATPolyhedronGravityModel.hpp>
#include <SBGATMassProperties.hpp>
#include <SBGATSphericalHarmo.hpp>
#include <SBGATObsRadar.hpp>
#include <SBGATObsLightcurve.hpp>
#include <SBGATFrameGraph.hpp>
#include <SBGATShapeUncertainty.hpp>
#include <SBGATTransformShape.hpp>
#include <SBGATObjWriter.hpp>
#include <SBGATPolyhedronGravityModelUQ.hpp>
#include <SBGATMassPropertiesUQ.hpp>
#include <SBGATFilter.hpp>
#include <SBGATFilterUQ.hpp>

#include <vtkCell.h>
#include <vtkDataObject.h>
#include <vtkIdList.h>
#include <vtkMath.h>
#include <vtkSmartPointer.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkPolyDataNormals.h>
#include <RigidBodyKinematics.hpp>
#include <vtkDoubleArray.h>
#include <vtkFloatArray.h>
#include <vtkCellData.h>
#include <vtkSphereSource.h>
#include <vtkCubeSource.h>
#include <vtkPolyData.h>
#include <assert.h>
#include <vtkTriangleFilter.h>
#include <vtkCleanPolyData.h>
#include <vtkOBJReader.h>
#include <vtkCellCenters.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkLinearSubdivisionFilter.h>
#include <boost/progress.hpp>

void TestsSBCore::run() {

	TestsSBCore::test_MassProperties_UQ_partials();
	TestsSBCore::test_sbgat_transform_shape();
	TestsSBCore::test_frame_conversion();
	TestsSBCore::test_sbgat_mass_properties();
	TestsSBCore::test_sbgat_pgm_cube();
	TestsSBCore::test_sbgat_pgm_sphere();
	TestsSBCore::test_sbgat_pgm_speed();
	TestsSBCore::test_spherical_harmonics_coefs_consistency();
	TestsSBCore::test_spherical_harmonics_partials_consistency();
	TestsSBCore::test_sbgat_shape_uq();
	TestsSBCore::test_PGM_UQ_partials();
	TestsSBCore::test_PGM_UQ_covariance_consistency();
	TestsSBCore::test_PGM_UQ_skewed_km();
	TestsSBCore::test_PGM_UQ_itokawa_m();
	TestsSBCore::test_PGM_UQ_itokawa_km();

	// TestsSBCore::test_lightcurve_obs();
	// TestsSBCore::test_radar_obs();

	std::cout << "All tests passed.\n";

}

/**
This test compares some geometric measures as computed by SBGAT
to analytical values
*/
void TestsSBCore::test_sbgat_mass_properties(){

	std::cout << "- Running test_sbgat_mass_properties ..." << std::endl;

	vtkSmartPointer<vtkCubeSource> source =
	vtkSmartPointer<vtkCubeSource>::New();
	source -> SetCenter(0.0, 0.0, 0.0);

	vtkSmartPointer<vtkTriangleFilter> triangleFilter =
	vtkSmartPointer<vtkTriangleFilter>::New();
	triangleFilter -> SetInputConnection(source->GetOutputPort());
	triangleFilter -> Update();

	vtkSmartPointer<vtkLinearSubdivisionFilter> subdivisionFilter = vtkSmartPointer<vtkLinearSubdivisionFilter>::New();

	subdivisionFilter -> SetInputConnection(triangleFilter -> GetOutputPort());
	subdivisionFilter -> SetNumberOfSubdivisions(6);
	subdivisionFilter -> Update();

	vtkSmartPointer<vtkCleanPolyData> cleanPolyData =
	vtkSmartPointer<vtkCleanPolyData>::New();
	cleanPolyData->SetInputConnection(subdivisionFilter->GetOutputPort());

	cleanPolyData->Update();

	vtkSmartPointer<vtkTransform> transform_rot = vtkSmartPointer<vtkTransform>::New();
	transform_rot->RotateWXYZ(10, 0, 1, 0);
	vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter_rot =
	vtkSmartPointer<vtkTransformPolyDataFilter>::New();
	transformFilter_rot->SetTransform(transform_rot);
	transformFilter_rot->SetInputConnection(cleanPolyData->GetOutputPort());
	transformFilter_rot->Update();

	vtkSmartPointer<vtkTransform> transform_trans = vtkSmartPointer<vtkTransform>::New();
	arma::vec translation_vector = { 1, 2, 3};
	transform_trans->Translate (translation_vector.colptr(0));
	vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter_trans =
	vtkSmartPointer<vtkTransformPolyDataFilter>::New();
	transformFilter_trans->SetTransform(transform_trans);
	transformFilter_trans->SetInputConnection(transformFilter_rot->GetOutputPort());
	transformFilter_trans->Update();

	vtkSmartPointer<SBGATMassProperties> mass_filter = vtkSmartPointer<SBGATMassProperties>::New();
	mass_filter -> SetScaleMeters();
	mass_filter -> SetInputConnection(transformFilter_trans -> GetOutputPort());
	mass_filter -> Update();
	auto com_sbgat = mass_filter -> GetCenterOfMass();

	assert(mass_filter -> CheckClosed());
	assert(std::abs(mass_filter -> GetVolume() - 1.) < 1e-8);

	// the inertia moments are invariant by rotation/translation
	arma::vec inertia_moments = {1./6,1./6,1./6};
	double r_avg = std::cbrt(3./ (4 * arma::datum::pi));
	inertia_moments = inertia_moments / (r_avg * r_avg) + arma::eig_sym(RBK::tilde(translation_vector) * RBK::tilde(translation_vector).t())/ (r_avg * r_avg);
	auto inertia_moments_sbgat = mass_filter -> GetNormalizedInertiaMoments();

	assert(arma::norm(inertia_moments - inertia_moments_sbgat)/arma::norm(inertia_moments_sbgat) < 1e-8);
	assert(arma::norm(translation_vector - com_sbgat)/arma::norm(com_sbgat) < 1e-8);

	double volume_itokawa_m,volume_itokawa_km;
	double surface_itokawa_m,surface_itokawa_km;

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName("../../resources/shape_models/itokawa_8_scaled.obj");
	reader -> Update();

	mass_filter -> SetInputConnection(reader -> GetOutputPort());
	mass_filter -> Modified();
	mass_filter -> SetScaleMeters();
	mass_filter -> Update();

	volume_itokawa_m = mass_filter -> GetVolume();
	surface_itokawa_m = mass_filter -> GetSurfaceArea();

	reader -> SetFileName("../../resources/shape_models/itokawa_8.obj");
	reader -> Modified();
	reader -> Update();

	mass_filter -> SetInputConnection(reader -> GetOutputPort());
	mass_filter -> Modified();
	mass_filter -> SetScaleKiloMeters();
	mass_filter -> Update();

	volume_itokawa_km = mass_filter -> GetVolume();
	surface_itokawa_km = mass_filter -> GetSurfaceArea();

	assert(std::abs(volume_itokawa_m - volume_itokawa_km) / volume_itokawa_m < 1e-7);
	assert(std::abs(surface_itokawa_km - surface_itokawa_m) / volume_itokawa_m < 1e-7);

	std::cout << "- Done running test_sbgat_mass_properties" << std::endl;

}

/**
This test computes the surface accelerations at the center of each facet over a polydata
of KW4 for benchmarking purposes
*/
void TestsSBCore::test_sbgat_pgm_speed(){
	std::cout << "- Running test_sbgat_pgm_speed ..." << std::endl;

	std::string filename  = "../../resources/shape_models/KW4Alpha.obj";

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(filename.c_str());
	reader -> Update();

	// Creating the PGM dyads
	std::cout << "-- Creating dyads...\n";
	double density = 2000;

	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(reader -> GetOutputPort());
	pgm_filter -> SetDensity(density); // density in kg/m^3
	pgm_filter -> SetScaleKiloMeters();
	pgm_filter -> Update();
	std::cout << "-- Done creating dyads...\n";

	// Creating a filter that will extract the center of each facet
	vtkSmartPointer<vtkPolyData> polydata = reader -> GetOutput();
	vtkSmartPointer<vtkCellCenters> cellCentersFilter =
	vtkSmartPointer<vtkCellCenters>::New();
	cellCentersFilter -> SetInputConnection(reader -> GetOutputPort());
	cellCentersFilter -> Update();

	// Preallocating
	arma::mat surface_accelerations(3,cellCentersFilter -> GetOutput() -> GetNumberOfPoints());

	assert(polydata -> GetNumberOfCells() == cellCentersFilter -> GetOutput() -> GetNumberOfPoints());

	int N_f = polydata -> GetNumberOfCells();
	std::cout << "-- Computing pgm accelerations at " << cellCentersFilter -> GetOutput() -> GetNumberOfPoints() << " facet centers over the surface of " << filename <<  " . This may take a few minutes ...\n";

	boost::progress_display progress(N_f);
	auto start = std::chrono::system_clock::now();
	#pragma omp parallel for
	for (vtkIdType i = 0; i < N_f; ++i){

		arma::vec::fixed<3> p = pgm_filter -> GetFacetCenter(i);

		surface_accelerations.col(i) = pgm_filter -> GetAcceleration(p);
		++progress;
	}

	auto end = std::chrono::system_clock::now();
	std::chrono::duration<double> elapsed_seconds = end-start;
	std::cout << "\n-- Done computing pgm accelerations in " << elapsed_seconds.count() << " s\n";

	std::vector<double> slopes;
	std::vector<double> inertial_potentials;
	std::vector<double> body_fixed_potentials;
	std::vector<double> inertial_acc_magnitudes;
	std::vector<double> body_fixed_acc_magnitudes;
	arma::vec::fixed<3> omega = {0,0,2 * arma::datum::pi / (2.7650 * 3600)};

	std::vector<unsigned int> queried_elements;
	for (int i = 0; i < polydata -> GetNumberOfCells() ; ++i){
		queried_elements.push_back(i);
	}

	start = std::chrono::system_clock::now();

	SBGATPolyhedronGravityModel::ComputeSurfacePGM(polydata,queried_elements,false,density,omega,
		slopes,
		inertial_potentials,
		body_fixed_potentials,
		inertial_acc_magnitudes,
		body_fixed_acc_magnitudes);

	end = std::chrono::system_clock::now();
	elapsed_seconds = end-start;
	std::cout << "-- Done computing pgm slopes, potentials and accelerations in " << elapsed_seconds.count() << " s\n";

	assert(N_f == (int)(inertial_acc_magnitudes.size()));

	for (int i = 0; i < N_f; ++i){
		assert(std::abs(arma::norm(surface_accelerations.col(i)) - inertial_acc_magnitudes[i]) / inertial_acc_magnitudes[i] < 1e-5);
	}

	vtkSmartPointer<SBGATMassProperties> mass_properties = vtkSmartPointer<SBGATMassProperties>::New();

	mass_properties -> SetInputData(polydata);
	mass_properties -> SetScaleKiloMeters();

	mass_properties -> Update();
	double mass = mass_properties -> GetVolume() * density;

	SBGATPolyhedronGravityModel::SaveSurfacePGM(polydata,
		queried_elements,
		false,
		mass,
		omega,
		slopes,
		inertial_potentials,
		body_fixed_potentials,
		inertial_acc_magnitudes,
		body_fixed_acc_magnitudes,
		"../output/kw4_pgm.json");

	std::cout << "- Done running test_sbgat_pgm_speed" << std::endl;

}

/**
This test computes the pgm acceleration and potential
about a unit cube and compares the SBGAT outputs to analytical ones
*/
void TestsSBCore::test_sbgat_pgm_cube() {

	std::cout << "- Running test_sbgat_pgm_cube ..." << std::endl;

	{
		vtkSmartPointer<vtkCubeSource> source =
		vtkSmartPointer<vtkCubeSource>::New();

		source -> SetCenter(0.0, 0.0, 0.0);

		double density = 1e6;

		vtkSmartPointer<vtkTriangleFilter> triangleFilter =
		vtkSmartPointer<vtkTriangleFilter>::New();
		triangleFilter -> SetInputConnection(source->GetOutputPort());
		triangleFilter -> Update();

		vtkSmartPointer<vtkCleanPolyData> cleanPolyData =
		vtkSmartPointer<vtkCleanPolyData>::New();
		cleanPolyData->SetInputConnection(triangleFilter->GetOutputPort());
		cleanPolyData->Update();

		vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
		pgm_filter -> SetInputConnection(cleanPolyData -> GetOutputPort());

		pgm_filter -> SetDensity(density);
		pgm_filter -> SetScaleMeters();
		pgm_filter -> Update();

		// Points that should be outside
		double p0[3] = {0.7,0,0};
		double p1[3] = {0.50000001,0,0};

		// Points that should be inside
		double p2[3] = {0.4,0,0};
		double p3[3] = {0.49999999,0,0};

		assert(!pgm_filter -> Contains(p0));
		assert(!pgm_filter -> Contains(p1));
		assert(pgm_filter -> Contains(p2));
		assert(pgm_filter -> Contains(p3));

	// The attracting shape is a cube of dimensions 1 x 1 x 1 m of density rho = 1e6 kg/m^3
	// The analytic potential and acceleration at (1,2,3) (m) in the shape model's barycentric frame is computed
	// Assumes that G = 6.67408e-11 m^3 / (kg * s ^2)

		// Queried point for pgm validation
		arma::vec p4 = {1, 2, 3};

		arma::vec acc_true = {
			-1.273782722739791e-06,
			-2.548008881415967e-06,
			-3.823026510474731e-06
		};
		double pot_true = 0.26726619638669064 * arma::datum::G * density;

		arma::vec pgm_acc = pgm_filter -> GetAcceleration(p4.colptr(0));
		double pgm_pot = pgm_filter -> GetPotential(p4.colptr(0));

		assert(arma::norm(pgm_acc - acc_true)/arma::norm(acc_true) < 1e-10);
		assert(std::abs(pgm_pot - pot_true)/std::abs(pot_true) < 1e-10);

		pgm_acc = pgm_filter -> GetAcceleration(p4);
		pgm_pot = pgm_filter -> GetPotential(p4);

		double pgm_pot2;
		arma::vec::fixed<3> pgm_acc2;
		arma::mat::fixed<3,3> pgm_gravity_gradient_matrix;
		pgm_filter -> GetPotentialAccelerationGravityGradient(p4,pgm_pot2, pgm_acc2,pgm_gravity_gradient_matrix);

		assert(arma::norm(pgm_acc - acc_true)/arma::norm(acc_true) < 1e-10);
		assert(std::abs(pgm_pot - pot_true)/std::abs(pot_true) < 1e-10);

		assert(arma::norm(pgm_acc2 - acc_true)/arma::norm(acc_true) < 1e-10);
		assert(std::abs(pgm_pot2 - pot_true)/std::abs(pot_true) < 1e-10);

		arma::vec::fixed<3> p4_perturbed = (1 + 1e-3) * p4;

		arma::vec::fixed<3> pgm_acc2_perturbed = pgm_filter -> GetAcceleration(p4_perturbed);

		auto d_acc_true = pgm_acc2_perturbed - pgm_acc2;

		arma::vec::fixed<3> d_acc_linear = pgm_gravity_gradient_matrix * (p4_perturbed - p4);

		assert(100 * arma::norm(d_acc_linear - d_acc_true)/arma::norm(d_acc_true) < 1e0);

	}

	std::cout << "- Done running test_sbgat_pgm_cube" << std::endl;

}

/**
This test computes the pgm acceleration and potential
about a sphere and compares the SBGAT outputs to analytical ones
*/
void TestsSBCore::test_sbgat_pgm_sphere() {

	std::cout << "- Running test_sbgat_pgm_sphere ..." << std::endl;

	vtkSmartPointer<vtkSphereSource> source =
	vtkSmartPointer<vtkSphereSource>::New();

	source -> SetCenter(0.0, 0.0, 0.0);
	source -> SetRadius(6370);
	source -> SetThetaResolution (300);
	source -> SetPhiResolution (300);

	double density = 5.51e3;

	vtkSmartPointer<vtkTriangleFilter> triangleFilter =
	vtkSmartPointer<vtkTriangleFilter>::New();
	triangleFilter -> SetInputConnection(source->GetOutputPort());
	triangleFilter -> Update();

	vtkSmartPointer<vtkCleanPolyData> cleanPolyData =
	vtkSmartPointer<vtkCleanPolyData>::New();
	cleanPolyData->SetInputConnection(triangleFilter->GetOutputPort());
	cleanPolyData->Update();

	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(cleanPolyData -> GetOutputPort());
	pgm_filter -> SetDensity(density);
	pgm_filter -> SetScaleKiloMeters();
	pgm_filter -> Update();

	vtkSmartPointer<SBGATMassProperties> mass_properties = vtkSmartPointer<SBGATMassProperties>::New();

	mass_properties -> SetInputConnection(cleanPolyData -> GetOutputPort());
	mass_properties -> SetScaleKiloMeters();
	mass_properties -> Update();

		// Queried point for pgm validation
	arma::vec p = {6371e3, 0, 0};

	arma::vec acc_true = - arma::datum::G * density * mass_properties -> GetVolume() / arma::dot(p,p) * arma::normalise(p);
	double pot_true = arma::datum::G * density * mass_properties -> GetVolume() / arma::norm(p);

	arma::vec pgm_acc = pgm_filter -> GetAcceleration(p);
	double pgm_pot = pgm_filter -> GetPotential(p);

	assert(arma::norm(pgm_acc - acc_true)/arma::norm(acc_true) < 1e-4);
	assert(std::abs(pgm_pot - pot_true)/std::abs(pot_true) < 1e-4);

	pgm_acc = pgm_filter -> GetAcceleration(p);
	pgm_pot = pgm_filter -> GetPotential(p);

	double pgm_pot2;
	arma::vec::fixed<3> pgm_acc2;
	arma::mat::fixed<3,3> pgm_gravity_gradient_matrix;
	pgm_filter -> GetPotentialAccelerationGravityGradient(p,pgm_pot2, pgm_acc2,pgm_gravity_gradient_matrix);

	assert(arma::norm(pgm_acc - acc_true)/arma::norm(acc_true) < 1e-4);
	assert(std::abs(pgm_pot - pot_true)/std::abs(pot_true) < 1e-4);

	assert(arma::norm(pgm_acc2 - acc_true)/arma::norm(acc_true) < 1e-4);
	assert(std::abs(pgm_pot2 - pot_true)/std::abs(pot_true) < 1e-4);

	arma::vec::fixed<3> p4_perturbed = (1 + 1e-3) * p;

	arma::vec::fixed<3> pgm_acc2_perturbed = pgm_filter -> GetAcceleration(p4_perturbed);

	auto d_acc_true = pgm_acc2_perturbed - pgm_acc2;

	arma::vec::fixed<3> d_acc_linear = pgm_gravity_gradient_matrix * (p4_perturbed - p);

	assert(100 * arma::norm(d_acc_linear - d_acc_true)/arma::norm(d_acc_true) < 1e0);

	std::cout << "- Done running test_sbgat_pgm_sphere" << std::endl;

}

/**
This test checks the consistency of the spherical harmonics coefficients computation
about a shape model of KW4
*/

void TestsSBCore::test_spherical_harmonics_coefs_consistency() {

	std::cout << "- Running test_spherical_harmonics_coefs_consistency ..." << std::endl;

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName("../../resources/shape_models/KW4Alpha.obj");
	reader -> Update();

	// Harmonics up to degree five are computed
	int degree = 5;

	// Density of KW4 (kg/m^3).
	double density = 2000.0;

	// Reference radius of KW4 (m)
	double ref_radius = 1.317/2 * 1000;

	// An instance of SBGATPolyhedronGravityModel is created to evaluate the PGM of
	// the considered polytdata
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(reader -> GetOutputPort());
	pgm_filter -> SetDensity(density);
	pgm_filter -> SetScaleKiloMeters();// The input shape model has its coordinates expressed in km
	pgm_filter -> Update();

	// An instance of SBGATSphericalHarmo is created to compute and evaluate the spherical
	// expansion of the gravity field about the considered shape model
	vtkSmartPointer<SBGATSphericalHarmo> spherical_harmonics = vtkSmartPointer<SBGATSphericalHarmo>::New();
	spherical_harmonics -> SetInputConnection(reader -> GetOutputPort());
	spherical_harmonics -> SetDensity(density);
	spherical_harmonics -> SetScaleKiloMeters();
	spherical_harmonics -> SetReferenceRadius(ref_radius);
	spherical_harmonics -> IsNormalized(); // can be skipped as normalized coefficients is the default parameter
	spherical_harmonics -> SetDegree(degree);
	spherical_harmonics -> Update();

	// The spherical harmonics are saved to a file
	spherical_harmonics -> SaveToJson("../output/KW4Alpha_harmo.json");

	// The accelerations are evaluated at the query point
	arma::vec::fixed<3> pos = {3e3,5e3,-2e3};
	arma::vec::fixed<3> pgm_acc = pgm_filter -> GetAcceleration(pos);
	arma::vec::fixed<3> sharm_acc = spherical_harmonics -> GetAcceleration(pos);

	// The spherical harmonics are read from the just-saved JSON file and re-evaluated
	vtkSmartPointer<SBGATSphericalHarmo> spherical_harmonics_from_file = vtkSmartPointer<SBGATSphericalHarmo>::New();
	spherical_harmonics_from_file -> LoadFromJson("../output/KW4Alpha_harmo.json");
	arma::vec::fixed<3> sharm_acc_from_file = spherical_harmonics_from_file -> GetAcceleration(pos);

	// The accelerations should be consistent with the one previously computed

	vtkSmartPointer<SBGATMassProperties> mass_properties = vtkSmartPointer<SBGATMassProperties>::New();
	mass_properties -> SetInputConnection(reader -> GetOutputPort());
	mass_properties -> SetScaleKiloMeters();
	mass_properties -> Update();

	assert(arma::norm(pgm_acc - sharm_acc_from_file) / arma::norm(sharm_acc_from_file) * 100 < 1e-4);
	assert(arma::norm(sharm_acc_from_file - sharm_acc) / arma::norm(sharm_acc) * 100 < 1e-8);

	std::cout << "- Done running test_spherical_harmonics_coefs_consistency" << std::endl;

}

/**
This test checks the consistency of the spherical harmonics coefficients computation
about a shape model of KW4
*/

void TestsSBCore::test_spherical_harmonics_partials_consistency() {

	std::cout << "- Running test_spherical_harmonics_partials_consistency ..." << std::endl;

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName("../../resources/shape_models/KW4Alpha.obj");
	reader -> Update();

	// Harmonics up to degree five are computed
	int degree = 5;

	// Density of KW4 (kg/m^3).
	double density = 2000.0;

	// Reference radius of KW4 (km)
	double ref_radius = 1.317/2 * 1000;

	// An instance of SBGATSphericalHarmo is created to compute and evaluate the spherical
	// expansion of the gravity field about the considered shape model
	vtkSmartPointer<SBGATSphericalHarmo> spherical_harmonics = vtkSmartPointer<SBGATSphericalHarmo>::New();
	spherical_harmonics -> SetInputConnection(reader -> GetOutputPort());
	spherical_harmonics -> SetDensity(density);
	spherical_harmonics -> SetScaleKiloMeters();
	spherical_harmonics -> SetReferenceRadius(ref_radius);
	spherical_harmonics -> IsNormalized(); // can be skipped as normalized coefficients is the default parameter
	spherical_harmonics -> SetDegree(degree);
	spherical_harmonics -> Update();

	// The accelerations are evaluated at the query point
	arma::vec::fixed<3> pos = {3e3,5e3,-2e3};
	arma::vec::fixed<3> dpos = 1e-3 * pos;

	arma::vec::fixed<3> sharm_acc = spherical_harmonics -> GetAcceleration(pos);
	arma::vec::fixed<3> sharm_acc_pert = spherical_harmonics -> GetAcceleration(pos + dpos);
	arma::vec::fixed<3> dacc_true = sharm_acc_pert - sharm_acc;
	arma::mat::fixed<3,3> gravity_gradient_mat;
	spherical_harmonics -> GetGravityGradientMatrix(pos,gravity_gradient_mat);

	arma::vec::fixed<3> dacc_lin = gravity_gradient_mat * dpos;
	assert(arma::norm(dacc_lin - dacc_true ) / arma::norm(dacc_true) * 100 < 1e0);

	// Modifying the coefs and testing the partials
	arma::mat Cnm = spherical_harmonics-> GetCnm();
	arma::mat Snm = spherical_harmonics-> GetSnm();

	arma::mat dCnm = 1e-2 * Cnm;
	arma::mat dSnm = 1e-2 * Snm;

	dCnm(0,0) = 0;
	dSnm.col(0) *= 0;

	arma::vec dCnm_vec((degree + 1) * (degree + 2)/2);
	arma::vec dSnm_vec((degree + 1) * (degree + 2)/2 - degree - 1);

	int counter = 0;
	for (int i = 0; i <= degree; ++i){
		for (int j = 0; j <= i; ++j){
			dCnm_vec(counter) = dCnm(i,j);
			++counter;
		}

	}

	counter = 0;
	for (int i = 1; i <= degree; ++i){
		for (int j = 1; j <= i; ++j){
			dSnm_vec(counter) = dSnm(i,j);
			++counter;
		}

	}

	spherical_harmonics -> SetCnm(Cnm + dCnm);
	spherical_harmonics -> SetSnm(Snm + dSnm);

	sharm_acc_pert = spherical_harmonics -> GetAcceleration(pos);
	dacc_true = sharm_acc_pert - sharm_acc;

	arma::mat partial_C,partial_S;
	spherical_harmonics -> GetPartialHarmonics(pos,partial_C, partial_S);

	dacc_lin = partial_C * dCnm_vec + partial_S * dSnm_vec;

	assert(arma::norm(dacc_lin - dacc_true ) / arma::norm(dacc_true) * 100 < 1e-2);

	std::cout << "- Done running test_spherical_harmonics_partials_consistency" << std::endl;

}

void TestsSBCore::test_frame_conversion(){

	std::cout << "- Running test_frame_conversion ..." << std::endl;

	SBGATFrameGraph frame_graph;

	frame_graph.add_frame("N");
	frame_graph.add_frame("B");
	frame_graph.add_transform("N","B");
	arma::vec::fixed<3> mrp = {0.1,0.2,0.3};
	arma::vec::fixed<3> origin = {-0.1,0.2,-0.3};

	frame_graph.set_transform_mrp("N","B",mrp);
	frame_graph.set_transform_origin("N","B",origin);

	arma::vec::fixed<3> zero = {0,0,0};

	assert(arma::norm(frame_graph.convert(zero,"B","N") - origin) == 0);

	// TODO : complete with rotation tests
	std::cout << "- Done running test_frame_conversion" << std::endl;

}

void TestsSBCore::test_sbgat_shape_uq(){
	// std::cout << "- Running test_sbgat_shape_uq ..." << std::endl;

	// // Reading
	// vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	// reader -> SetFileName("../../resources/shape_models/skewed.obj");
	// reader -> Update();
	// vtkSmartPointer<vtkPolyData> shape = reader -> GetOutput();

	// SBGATTransformShape::ShiftShapeToBarycenter(shape);

	// vtkSmartPointer<SBGATMassProperties> mass_properties = vtkSmartPointer<SBGATMassProperties>::New();
	// mass_properties -> SetInputData(shape);
	// mass_properties -> Update();

	// std::cout << "Volume: \n" << mass_properties -> GetVolume();
	// std::cout << "\nCenter-of-mass: \n" << mass_properties -> GetCenterOfMass().t();
	// std::cout << "\nUnit-density inertia: \n" << mass_properties -> GetUnitDensityInertiaTensor();
	// std::cout << "\nPrincipal axes MRP: \n" << RBK::dcm_to_mrp(mass_properties -> GetPrincipalAxes()).t();

	// vtkSmartPointer<SBGATShapeUncertainty> shape_uq = vtkSmartPointer<SBGATShapeUncertainty>::New();
	// shape_uq -> SetInputData(shape);
	// shape_uq -> Update();

	// auto start = std::chrono::system_clock::now();
	// //
	// shape_uq -> ComputeInertiaStatistics(1,0.01);
	// auto end = std::chrono::system_clock::now();

	// std::chrono::duration<double> elapsed_seconds = end-start;
	// std::cout << "-- Done computing analytical shape uncertainty in " << elapsed_seconds.count() << " s\n";

	// double volume_variance_lin = shape_uq -> GetVolumeVariance();
	// arma::mat::fixed<3,3> com_covariance_lin = shape_uq -> GetCOMCovariance();
	// arma::mat::fixed<6,6> inertia_covariance_lin = shape_uq -> GetInertiaParametrizationCovariance();
	// arma::mat::fixed<3,3> principal_dims_covariance_lin = shape_uq -> GetPrincipalDimensionsCovariance();
	// arma::mat::fixed<4,4> principal_moments_covariance_lin = shape_uq -> GetPrincipalMomentsCovariance();
	// arma::mat::fixed<3,3> mrp_covariance_lin = shape_uq -> GetPrincipalAxesMRPCovariance();

	// std::cout << "\nVolume variance: \n";
	// std::cout << volume_variance_lin << std::endl;

	// std::cout << "\nCOM covariance: \n";
	// std::cout << com_covariance_lin << std::endl;

	// std::cout << "\nUnit-density Inertia parametrization covariance: \n";
	// std::cout << inertia_covariance_lin << std::endl;

	// std::cout << "\nPrincipal dimensions covariance: \n";
	// std::cout << principal_dims_covariance_lin << std::endl;

	// std::cout << "\nPrincipal moments covariance: \n";
	// std::cout << principal_moments_covariance_lin << std::endl;

	// std::cout << "\nMRP covariance: \n";
	// std::cout << mrp_covariance_lin << std::endl;

	// start = std::chrono::system_clock::now();
	// shape_uq -> ComputeInertiaStatisticsMC(100,1,0.01);
	// end = std::chrono::system_clock::now();

	// elapsed_seconds = end-start;

	// std::cout << "\n-- Error from linearized volume variance after 100 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << (shape_uq -> GetVolumeVariance() - volume_variance_lin)/volume_variance_lin * 100 << " %\n";
	// std::cout << "-- Max Error from linearized center-of-mass covariance after 100 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetCOMCovariance() - com_covariance_lin).max()/arma::abs(com_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized inertia parametrization covariance after 100 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetInertiaParametrizationCovariance() - inertia_covariance_lin).max()/arma::abs(inertia_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized principal moments covariance after 100 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalMomentsCovariance() - principal_moments_covariance_lin).max()/arma::abs(principal_moments_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized principal dimensions covariance after 100 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalDimensionsCovariance() - principal_dims_covariance_lin).max()/arma::abs(principal_dims_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized mrp covariances after 100 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalAxesMRPCovariance() - mrp_covariance_lin).max()/arma::abs(mrp_covariance_lin).max() * 100 << " %\n";

	// start = std::chrono::system_clock::now();
	// shape_uq -> ComputeInertiaStatisticsMC(1000,1,0.01);
	// end = std::chrono::system_clock::now();
	// elapsed_seconds = end-start;

	// std::cout << "\n-- Error from linearized volume variance after 1000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : "<< (shape_uq -> GetVolumeVariance() - volume_variance_lin)/volume_variance_lin * 100 << " %\n";
	// std::cout << "-- Max Error from linearized center-of-mass covariances after 1000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetCOMCovariance() - com_covariance_lin).max()/arma::abs(com_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized inertia parametrization covariance after 1000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetInertiaParametrizationCovariance() - inertia_covariance_lin).max()/arma::abs(inertia_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized principal moments covariances after 1000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalMomentsCovariance() - principal_moments_covariance_lin).max()/arma::abs(principal_moments_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized principal dimensions covariance after 1000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalDimensionsCovariance() - principal_dims_covariance_lin).max()/arma::abs(principal_dims_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized mrp covariances after 1000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalAxesMRPCovariance() - mrp_covariance_lin).max()/arma::abs(mrp_covariance_lin).max() * 100 << " %\n";

	// start = std::chrono::system_clock::now();
	// shape_uq -> ComputeInertiaStatisticsMC(10000,1,0.01);
	// end = std::chrono::system_clock::now();
	// elapsed_seconds = end-start;

	// std::cout << "\n-- Error from linearized volume variance after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : "<< (shape_uq -> GetVolumeVariance() - volume_variance_lin)/volume_variance_lin * 100 << " %\n";
	// std::cout << "-- Max Error from linearized center-of-mass covariances after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetCOMCovariance() - com_covariance_lin).max()/arma::abs(com_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized inertia parametrization covariance after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetInertiaParametrizationCovariance() - inertia_covariance_lin).max()/arma::abs(inertia_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized principal moments covariances after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalMomentsCovariance() - principal_moments_covariance_lin).max()/arma::abs(principal_moments_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized principal dimensions covariance after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalDimensionsCovariance() - principal_dims_covariance_lin).max()/arma::abs(principal_dims_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized mrp covariances after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalAxesMRPCovariance() - mrp_covariance_lin).max()/arma::abs(mrp_covariance_lin).max() * 100 << " %\n";

	// start = std::chrono::system_clock::now();
	// shape_uq -> ComputeInertiaStatisticsMC(10000,1,0.01);
	// end = std::chrono::system_clock::now();
	// elapsed_seconds = end-start;

	// std::cout << "\n-- Error from linearized volume variance after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : "<< (shape_uq -> GetVolumeVariance() - volume_variance_lin)/volume_variance_lin * 100 << " %\n";
	// std::cout << "-- Max Error from linearized center-of-mass covariances after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetCOMCovariance() - com_covariance_lin).max()/arma::abs(com_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized inertia parametrization covariance after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetInertiaParametrizationCovariance() - inertia_covariance_lin).max()/arma::abs(inertia_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized principal moments covariances after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalMomentsCovariance() - principal_moments_covariance_lin).max()/arma::abs(principal_moments_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized principal dimensions covariance after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalDimensionsCovariance() - principal_dims_covariance_lin).max()/arma::abs(principal_dims_covariance_lin).max() * 100 << " %\n";
	// std::cout << "-- Max Error from linearized mrp covariances after 10000 MC outcomes (in " << elapsed_seconds.count() <<  " seconds) : " << arma::abs(shape_uq -> GetPrincipalAxesMRPCovariance() - mrp_covariance_lin).max()/arma::abs(mrp_covariance_lin).max() * 100 << " %\n";

	// std::cout << "\n- Done running test_sbgat_shape_uq ..." << std::endl;

}

void TestsSBCore::test_sbgat_transform_shape(){

	std::cout << "- Running test_sbgat_transform_shape ..." << std::endl;

	vtkSmartPointer<vtkCubeSource> source =
	vtkSmartPointer<vtkCubeSource>::New();
	source -> SetCenter(0.0, 0.0, 0.0);

	vtkSmartPointer<vtkTriangleFilter> triangleFilter =
	vtkSmartPointer<vtkTriangleFilter>::New();
	triangleFilter -> SetInputConnection(source->GetOutputPort());
	triangleFilter -> Update();

	vtkSmartPointer<vtkLinearSubdivisionFilter> subdivisionFilter = vtkSmartPointer<vtkLinearSubdivisionFilter>::New();

	subdivisionFilter -> SetInputConnection(triangleFilter -> GetOutputPort());
	subdivisionFilter -> SetNumberOfSubdivisions(6);
	subdivisionFilter -> Update();

	vtkSmartPointer<vtkCleanPolyData> cleanPolyData =
	vtkSmartPointer<vtkCleanPolyData>::New();
	cleanPolyData->SetInputConnection(subdivisionFilter->GetOutputPort());
	cleanPolyData->Update();

	vtkSmartPointer<vtkPolyData> cube = cleanPolyData -> GetOutput();

	arma::vec x = {1,0,0};
	SBGATTransformShape::Translate(x,cube);

	vtkSmartPointer<SBGATMassProperties> mass_filter = vtkSmartPointer<SBGATMassProperties>::New();
	mass_filter -> SetInputData(cube);
	mass_filter -> Update();

	auto com_sbgat = mass_filter -> GetCenterOfMass();

	assert(arma::norm(x - com_sbgat)/arma::norm(x) * 100 < 1e-6);

	SBGATTransformShape::ShiftShapeToBarycenter(cube);
	mass_filter -> SetInputData(cube);
	mass_filter -> Update();

	com_sbgat = mass_filter -> GetCenterOfMass();

	std::cout << "- Done running test_sbgat_transform_shape ..." << std::endl;

}

void TestsSBCore::test_MassProperties_UQ_partials(){

	SBGATMassPropertiesUQ::TestPartials("../../resources/shape_models/itokawa_8_scaled.obj",5e-2,true);
	SBGATMassPropertiesUQ::TestPartials("../../resources/shape_models/skewed.obj",5e-2,false);
	SBGATMassPropertiesUQ::TestPartials("../../resources/shape_models/itokawa_8.obj",5e-2,false);

}

void TestsSBCore::test_PGM_UQ_partials(){

	SBGATPolyhedronGravityModelUQ::TestPartials("../../resources/shape_models/itokawa_8_scaled.obj",5e-2,true);
	SBGATPolyhedronGravityModelUQ::TestPartials("../../resources/shape_models/skewed.obj",5e-2,false);
	SBGATPolyhedronGravityModelUQ::TestPartials("../../resources/shape_models/itokawa_8.obj",5e-2,false);

}

void TestsSBCore::test_PGM_UQ_itokawa_m(){

	std::cout << "- Running test_PGM_UQ_itokawa_m ..." << std::endl;

	int N = 10000;
	arma::arma_rng::set_seed(N) ;

	arma::vec pos = {200,300,400};
	double density = 1970;

	std::string filename  = "../../resources/shape_models/itokawa_8_scaled.obj";

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(filename.c_str());
	reader -> Update();

	// Cleaning
	vtkSmartPointer<vtkCleanPolyData> cleaner = vtkSmartPointer<vtkCleanPolyData>::New();
	cleaner -> SetInputConnection (reader -> GetOutputPort());
	cleaner -> SetOutputPointsPrecision ( vtkAlgorithm::DesiredOutputPrecision::DOUBLE_PRECISION );

	cleaner -> Update();

	// Creating the PGM dyads
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(cleaner -> GetOutputPort());
	pgm_filter -> SetDensity(density);
	pgm_filter -> SetScaleMeters();
	pgm_filter -> Update();

	double period = 12 * 3600;
	arma::vec::fixed<3> Omega = 2 * arma::datum::pi / (period) * pgm_filter  -> GetPrincipalAxes().t() * arma::vec({0,0,1});

	pgm_filter -> SetOmega(Omega);

	arma::vec::fixed<3> nom_acc;
	double nom_pot;

	int f = 0;

	double nom_slope = pgm_filter  -> GetSlope(f);
	pgm_filter -> GetPotentialAcceleration(pos,nom_pot,nom_acc);
	std::cout << "Nominal potential : " << nom_pot << std::endl;
	std::cout << "Nominal slope : " << nom_slope << std::endl;

	std::cout << "Nominal acceleration : " << nom_acc.t();

	int N_C = vtkPolyData::SafeDownCast(pgm_filter -> GetInput()) -> GetNumberOfPoints();

	arma::vec U_mc(N);
	arma::vec slopes_mc(N);
	arma::mat A_mc(3,N);
	arma::mat deviations(3 * N_C,N);

	double period_standard_deviation = 3600 / 3;

	arma::vec period_error = period_standard_deviation * arma::randn<arma::vec>(N);

	SBGATPolyhedronGravityModelUQ shape_uq;
	shape_uq.SetModel(pgm_filter);
	shape_uq.PrecomputeMassPropertiesPartials();

	shape_uq.SetPeriodErrorStandardDeviation(period_standard_deviation);
	shape_uq.ComputeVerticesCovarianceGlobal(10,100);

	int regularized_eigen_values = shape_uq.RegularizeCovariance();
	std::cout << regularized_eigen_values << " negative covariance eigenvalues were clamped to zero\n";
	arma::mat C_CC = shape_uq.GetCovarianceSquareRoot();
	arma::mat P_CC = shape_uq.GetVerticesCovariance();

	auto start = std::chrono::system_clock::now();
	double variance_U_analytical = shape_uq.GetVariancePotential(pos);
	double variance_slope_analytical = shape_uq.GetVarianceSlope(f);
	arma::mat::fixed<3,3> covariance_A_analytical = shape_uq.GetCovarianceAcceleration(pos);
	auto end = std::chrono::system_clock::now();

	std::chrono::duration<double> elapsed_seconds = end-start;
	std::cout << "Analytical statistics in potential and acceleration computed in " << elapsed_seconds.count() << " seconds\n";
	std::cout << "\tAnalytical variance in potential: " << variance_U_analytical << std::endl;
	std::cout << "\tAnalytical variance in slope: " << variance_slope_analytical << std::endl;
	std::cout << "\tAnalytical covariance in acceleration: \n" << covariance_A_analytical << std::endl;

	for (int i = 0; i < N ; ++i){
		deviations.col(i) = C_CC * arma::randn<arma::vec>(3 * N_C);
	}

	// MC
	start = std::chrono::system_clock::now();
	boost::progress_display progress(N);

	#pragma omp parallel for
	for (int i = 0; i < N ; ++i){

		// Reading
		vtkSmartPointer<vtkOBJReader> reader_mc = vtkSmartPointer<vtkOBJReader>::New();
		reader_mc -> SetFileName(filename.c_str());
		reader_mc -> Update();

	// Cleaning
		vtkSmartPointer<vtkCleanPolyData> cleaner_mc = vtkSmartPointer<vtkCleanPolyData>::New();
		cleaner_mc -> SetInputConnection (reader_mc -> GetOutputPort());
		cleaner_mc -> SetOutputPointsPrecision ( vtkAlgorithm::DesiredOutputPrecision::DOUBLE_PRECISION );

		cleaner_mc -> Update();

	// Creating the PGM dyads
		vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter_mc = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
		pgm_filter_mc -> SetInputConnection(cleaner_mc -> GetOutputPort());
		pgm_filter_mc -> SetDensity(density);
		pgm_filter_mc -> SetScaleMeters();
		pgm_filter_mc -> Update();

		arma::vec::fixed<3> Omega = 2 * arma::datum::pi / (12 * 3600 ) * pgm_filter_mc -> GetPrincipalAxes().t() * arma::vec({0,0,1});

		pgm_filter_mc -> SetOmega(Omega);

		SBGATPolyhedronGravityModelUQ shape_uq_mc;
		shape_uq_mc.SetModel(pgm_filter_mc);

		shape_uq_mc.ApplyDeviation(deviations.col(i));

		arma::vec::fixed<3> acc;
		double pot;
		pgm_filter_mc -> GetPotentialAcceleration(pos,pot,acc);

		arma::vec::fixed<3> Omega_p = 2 * arma::datum::pi / (12 * 3600 + period_error(i)) * pgm_filter_mc -> GetPrincipalAxes().t() * arma::vec({0,0,1});

		pgm_filter_mc -> SetOmega(Omega_p);

		U_mc(i) = pot;
		A_mc.col(i) = acc;
		slopes_mc(i) = pgm_filter_mc -> GetSlope(f);

		if (i < 20){
			vtkSmartPointer<SBGATObjWriter> writer = SBGATObjWriter::New();
			writer -> SetInputData(vtkPolyData::SafeDownCast(pgm_filter_mc -> GetInput()));
			std::string path = "../output/itokawa_m_mc_shape_" + std::to_string(i) + ".obj";
			writer -> SetFileName(path.c_str());
			writer -> Update();
		}
		++progress;
	}
	end = std::chrono::system_clock::now();
	elapsed_seconds = end-start;
	std::cout << "\nMC of potential and acceleration computed in " << elapsed_seconds.count() << " seconds\n";

	std::vector<int> steps = {10,100,1000,N};

	for (auto step : steps){

		std::cout << "\t After " << step << " MC outcomes:\n";

		std::cout << "\t\tMC mean in slope: " << arma::mean(slopes_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::mean(slopes_mc.subvec(0,step - 1)) - nom_slope)/nom_slope * 100 << std::endl;

		std::cout << "\t\tMC variance in slope: " << arma::var(slopes_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::var(slopes_mc.subvec(0,step - 1)) - variance_slope_analytical)/arma::var(slopes_mc.subvec(0,step - 1)) * 100 << std::endl;

		std::cout << "\t\tMC variance in potential: " << arma::var(U_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::var(U_mc.subvec(0,step - 1)) - variance_U_analytical)/arma::var(U_mc.subvec(0,step - 1)) * 100 << std::endl;

		std::cout << "\t\tMC Covariance in acceleration: \n" << arma::cov(A_mc.cols(0,step - 1).t()) << std::endl;
		std::cout << "\t\tError (%): " << arma::norm(arma::cov(A_mc.cols(0,step - 1).t()) - covariance_A_analytical)/arma::trace(arma::cov(A_mc.cols(0,step - 1).t())) * 100 << std::endl;

	}

	arma::mat P_CC_mc = arma::cov(deviations.t(),deviations.t());
	arma::vec sigma_sq_mc = arma::eig_sym(P_CC_mc);;
	arma::vec sigma_sq_true = arma::eig_sym(P_CC);

	std::cout << "\t Relative error in MC vs prescribed deviation covariance eigenvalues (%): " << arma::norm(sigma_sq_mc - sigma_sq_true)/arma::norm(sigma_sq_true) * 100 << std::endl;
	std::cout << "\t Absolute difference in MC vs prescribed deviation covariance divided by mean prescribed deviation covariance eigenvalues (%): " << arma::abs(P_CC - P_CC_mc).max()/arma::mean(sigma_sq_true) * 100 << std::endl;

	std::cout << "- Done running test_PGM_UQ_itokawa_m ..." << std::endl;

}

void TestsSBCore::test_PGM_UQ_itokawa_km(){

	std::cout << "- Running test_PGM_UQ_itokawa_km ..." << std::endl;

	int N = 10000;
	arma::arma_rng::set_seed(N) ;

	arma::vec pos = {200,300,400};
	double density = 1970;

	std::string filename  = "../../resources/shape_models/itokawa_8.obj";

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(filename.c_str());
	reader -> Update();

	// Cleaning
	vtkSmartPointer<vtkCleanPolyData> cleaner = vtkSmartPointer<vtkCleanPolyData>::New();
	cleaner -> SetInputConnection (reader -> GetOutputPort());
	cleaner -> SetOutputPointsPrecision ( vtkAlgorithm::DesiredOutputPrecision::DOUBLE_PRECISION );

	cleaner -> Update();

	// Creating the PGM dyads
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(cleaner -> GetOutputPort());
	pgm_filter -> SetDensity(density);
	pgm_filter -> SetScaleKiloMeters();
	pgm_filter -> Update();

	arma::vec::fixed<3> nom_acc;
	double nom_pot;

	int f = 0;
	double period = 12 * 3600;
	arma::vec::fixed<3> Omega = 2 * arma::datum::pi / (period) * pgm_filter  -> GetPrincipalAxes().t() * arma::vec({0,0,1});
	pgm_filter -> SetOmega(Omega);
	double nom_slope = pgm_filter  -> GetSlope(f);

	pgm_filter -> GetPotentialAcceleration(pos,nom_pot,nom_acc);
	std::cout << "Nominal potential : " << nom_pot << std::endl;
	std::cout << "Nominal slope : " << nom_slope << std::endl;
	std::cout << "Nominal acceleration : " << nom_acc.t();

	int N_C = vtkPolyData::SafeDownCast(pgm_filter -> GetInput()) -> GetNumberOfPoints();

	arma::vec U_mc(N);
	arma::vec slopes_mc(N);
	arma::mat A_mc(3,N);
	arma::mat deviations(3 * N_C,N);

	double period_standard_deviation = 3600 / 3;

	arma::vec period_error = period_standard_deviation * arma::randn<arma::vec>(N);

	SBGATPolyhedronGravityModelUQ shape_uq;
	shape_uq.SetModel(pgm_filter);
	shape_uq.PrecomputeMassPropertiesPartials();

	shape_uq.SetPeriodErrorStandardDeviation(period_standard_deviation);
	shape_uq.ComputeVerticesCovarianceGlobal(10,100);

	int regularized_eigen_values = shape_uq.RegularizeCovariance();
	std::cout << regularized_eigen_values << " negative covariance eigenvalues were clamped to zero\n";
	arma::mat C_CC = shape_uq.GetCovarianceSquareRoot();
	arma::mat P_CC = shape_uq.GetVerticesCovariance();

	auto start = std::chrono::system_clock::now();
	double variance_U_analytical = shape_uq.GetVariancePotential(pos);
	double variance_slope_analytical = shape_uq.GetVarianceSlope(f);
	arma::mat::fixed<3,3> covariance_A_analytical = shape_uq.GetCovarianceAcceleration(pos);
	auto end = std::chrono::system_clock::now();

	std::chrono::duration<double> elapsed_seconds = end-start;
	std::cout << "Analytical statistics in potential and acceleration computed in " << elapsed_seconds.count() << " seconds\n";
	std::cout << "\tAnalytical variance in potential: " << variance_U_analytical << std::endl;
	std::cout << "\tAnalytical variance in slope: " << variance_slope_analytical << std::endl;
	std::cout << "\tAnalytical covariance in acceleration: \n" << covariance_A_analytical << std::endl;

	for (int i = 0; i < N ; ++i){
		deviations.col(i) = C_CC * arma::randn<arma::vec>(3 * N_C);
	}

	// MC
	start = std::chrono::system_clock::now();
	boost::progress_display progress(N);

	#pragma omp parallel for
	for (int i = 0; i < N ; ++i){

		// Reading
		vtkSmartPointer<vtkOBJReader> reader_mc = vtkSmartPointer<vtkOBJReader>::New();
		reader_mc -> SetFileName(filename.c_str());
		reader_mc -> Update();

	// Cleaning
		vtkSmartPointer<vtkCleanPolyData> cleaner_mc = vtkSmartPointer<vtkCleanPolyData>::New();
		cleaner_mc -> SetInputConnection (reader_mc -> GetOutputPort());
		cleaner_mc -> SetOutputPointsPrecision ( vtkAlgorithm::DesiredOutputPrecision::DOUBLE_PRECISION );

		cleaner_mc -> Update();

	// Creating the PGM dyads
		vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter_mc = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
		pgm_filter_mc -> SetInputConnection(cleaner_mc -> GetOutputPort());
		pgm_filter_mc -> SetDensity(density);
		pgm_filter_mc -> SetScaleKiloMeters();
		pgm_filter_mc -> Update();

		arma::vec::fixed<3> Omega = 2 * arma::datum::pi / (12 * 3600 ) * pgm_filter_mc -> GetPrincipalAxes().t() * arma::vec({0,0,1});
		pgm_filter_mc -> SetOmega(Omega);

		SBGATPolyhedronGravityModelUQ shape_uq_mc;
		shape_uq_mc.SetModel(pgm_filter_mc);

		shape_uq_mc.ApplyDeviation(deviations.col(i));

		arma::vec::fixed<3> acc;
		double pot;
		pgm_filter_mc -> GetPotentialAcceleration(pos,pot,acc);

		arma::vec::fixed<3> Omega_p = 2 * arma::datum::pi / (12 * 3600 + period_error(i)) * pgm_filter_mc -> GetPrincipalAxes().t() * arma::vec({0,0,1});
		pgm_filter_mc -> SetOmega(Omega_p);

		U_mc(i) = pot;
		A_mc.col(i) = acc;
		slopes_mc(i) = pgm_filter_mc -> GetSlope(f);

		if (i < 20){
			vtkSmartPointer<SBGATObjWriter> writer = SBGATObjWriter::New();
			writer -> SetInputData(vtkPolyData::SafeDownCast(pgm_filter_mc -> GetInput()));
			std::string path = "../output/itokawa_km_mc_shape_" + std::to_string(i) + ".obj";
			writer -> SetFileName(path.c_str());
			writer -> Update();
		}
		++progress;
	}
	end = std::chrono::system_clock::now();
	elapsed_seconds = end-start;
	std::cout << "\nMC of potential and acceleration computed in " << elapsed_seconds.count() << " seconds\n";

	std::vector<int> steps = {10,100,1000,N};

	for (auto step : steps){

		std::cout << "\t After " << step << " MC outcomes:\n";

		std::cout << "\t\tMC mean in slope: " << arma::mean(slopes_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::mean(slopes_mc.subvec(0,step - 1)) - nom_slope)/nom_slope * 100 << std::endl;

		std::cout << "\t\tMC variance in slope: " << arma::var(slopes_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::var(slopes_mc.subvec(0,step - 1)) - variance_slope_analytical)/arma::var(slopes_mc.subvec(0,step - 1)) * 100 << std::endl;

		std::cout << "\t\tMC variance in potential: " << arma::var(U_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::var(U_mc.subvec(0,step - 1)) - variance_U_analytical)/arma::var(U_mc.subvec(0,step - 1)) * 100 << std::endl;

		std::cout << "\t\tMC Covariance in acceleration: \n" << arma::cov(A_mc.cols(0,step - 1).t()) << std::endl;
		std::cout << "\t\tError (%): " << arma::norm(arma::cov(A_mc.cols(0,step - 1).t()) - covariance_A_analytical)/arma::trace(arma::cov(A_mc.cols(0,step - 1).t())) * 100 << std::endl;

	}

	arma::mat P_CC_mc = arma::cov(deviations.t(),deviations.t());
	arma::vec sigma_sq_mc = arma::eig_sym(P_CC_mc);
	arma::vec sigma_sq_true = arma::eig_sym(P_CC);

	std::cout << "\t Relative error in MC vs prescribed deviation covariance eigenvalues (%): " << arma::norm(sigma_sq_mc - sigma_sq_true)/arma::norm(sigma_sq_true) * 100 << std::endl;
	std::cout << "\t Absolute difference in MC vs prescribed deviation covariance divided by mean prescribed deviation covariance eigenvalues (%): " << arma::abs(P_CC - P_CC_mc).max()/arma::mean(sigma_sq_true) * 100 << std::endl;

	std::cout << "- Done running test_PGM_UQ_itokawa_km ..." << std::endl;

}

void TestsSBCore::test_PGM_UQ_skewed_km(){

	std::cout << "- Running test_PGM_UQ_skewed_km ..." << std::endl;

	int N = 10000;
	arma::arma_rng::set_seed(N) ;

	arma::vec pos = {200,300,400};
	double density = 1970;

	std::string filename  = "../../resources/shape_models/skewed.obj";

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(filename.c_str());
	reader -> Update();

	// Cleaning
	vtkSmartPointer<vtkCleanPolyData> cleaner = vtkSmartPointer<vtkCleanPolyData>::New();
	cleaner -> SetInputConnection (reader -> GetOutputPort());
	cleaner -> SetOutputPointsPrecision ( vtkAlgorithm::DesiredOutputPrecision::DOUBLE_PRECISION );

	cleaner -> Update();

	// Creating the PGM dyads
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(cleaner -> GetOutputPort());
	pgm_filter -> SetDensity(density);
	pgm_filter -> SetScaleKiloMeters();
	pgm_filter -> Update();
	int f = 0;

	double period = 12 * 3600;
	arma::vec::fixed<3> Omega = 2 * arma::datum::pi / (period) * pgm_filter  -> GetPrincipalAxes().t() * arma::vec({0,0,1});
	pgm_filter -> SetOmega(Omega);

	arma::vec::fixed<3> nom_acc;
	double nom_pot;

	double nom_slope = pgm_filter  -> GetSlope(f);
	pgm_filter -> GetPotentialAcceleration(pos,nom_pot,nom_acc);
	std::cout << "Nominal potential : " << nom_pot << std::endl;
	std::cout << "Nominal slope : " << nom_slope << std::endl;
	std::cout << "Nominal acceleration : " << nom_acc.t();

	int N_C = vtkPolyData::SafeDownCast(pgm_filter -> GetInput()) -> GetNumberOfPoints();

	arma::vec U_mc(N);
	arma::vec slopes_mc(N);
	arma::mat A_mc(3,N);
	arma::mat deviations(3 * N_C,N);

	double period_standard_deviation = 3600 / 3;

	arma::vec period_error = period_standard_deviation * arma::randn<arma::vec>(N);

	SBGATPolyhedronGravityModelUQ shape_uq;
	shape_uq.SetModel(pgm_filter);
	shape_uq.PrecomputeMassPropertiesPartials();

	shape_uq.SetPeriodErrorStandardDeviation(period_standard_deviation);
	shape_uq.ComputeVerticesCovarianceGlobal(30,300);

	int regularized_eigen_values = shape_uq.RegularizeCovariance();
	std::cout << regularized_eigen_values << " negative covariance eigenvalues were clamped to zero\n";
	arma::mat C_CC = shape_uq.GetCovarianceSquareRoot();
	arma::mat P_CC = shape_uq.GetVerticesCovariance();

	auto start = std::chrono::system_clock::now();
	double variance_U_analytical = shape_uq.GetVariancePotential(pos);
	double variance_slope_analytical = shape_uq.GetVarianceSlope(f);
	arma::mat::fixed<3,3> covariance_A_analytical = shape_uq.GetCovarianceAcceleration(pos);
	auto end = std::chrono::system_clock::now();

	std::chrono::duration<double> elapsed_seconds = end-start;
	std::cout << "Analytical statistics in potential and acceleration computed in " << elapsed_seconds.count() << " seconds\n";
	std::cout << "\tAnalytical variance in potential: " << variance_U_analytical << std::endl;
	std::cout << "\tAnalytical variance in slope: " << variance_slope_analytical << std::endl;
	std::cout << "\tAnalytical covariance in acceleration: \n" << covariance_A_analytical << std::endl;

	for (int i = 0; i < N ; ++i){
		deviations.col(i) = C_CC * arma::randn<arma::vec>(3 * N_C);
	}

	// MC
	start = std::chrono::system_clock::now();
	boost::progress_display progress(N);

	#pragma omp parallel for
	for (int i = 0; i < N ; ++i){

		// Reading
		vtkSmartPointer<vtkOBJReader> reader_mc = vtkSmartPointer<vtkOBJReader>::New();
		reader_mc -> SetFileName(filename.c_str());
		reader_mc -> Update();

	// Cleaning
		vtkSmartPointer<vtkCleanPolyData> cleaner_mc = vtkSmartPointer<vtkCleanPolyData>::New();
		cleaner_mc -> SetInputConnection (reader_mc -> GetOutputPort());
		cleaner_mc -> SetOutputPointsPrecision ( vtkAlgorithm::DesiredOutputPrecision::DOUBLE_PRECISION );

		cleaner_mc -> Update();

	// Creating the PGM dyads
		vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter_mc = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
		pgm_filter_mc -> SetInputConnection(cleaner_mc -> GetOutputPort());
		pgm_filter_mc -> SetDensity(density);
		pgm_filter_mc -> SetScaleKiloMeters();
		pgm_filter_mc -> Update();

		arma::vec::fixed<3> Omega = 2 * arma::datum::pi / (12 * 3600 ) * pgm_filter_mc -> GetPrincipalAxes().t() * arma::vec({0,0,1});

		pgm_filter_mc -> SetOmega(Omega);
		SBGATPolyhedronGravityModelUQ shape_uq_mc;
		shape_uq_mc.SetModel(pgm_filter_mc);

		shape_uq_mc.ApplyDeviation(deviations.col(i));

		arma::vec::fixed<3> acc;
		double pot;
		pgm_filter_mc -> GetPotentialAcceleration(pos,pot,acc);

		arma::vec::fixed<3> Omega_p = 2 * arma::datum::pi / (12 * 3600 + period_error(i)) * pgm_filter_mc -> GetPrincipalAxes().t() * arma::vec({0,0,1});
		pgm_filter_mc -> SetOmega(Omega_p);

		U_mc(i) = pot;
		A_mc.col(i) = acc;
		slopes_mc(i) = pgm_filter_mc -> GetSlope(f);

		if (i < 20){
			vtkSmartPointer<SBGATObjWriter> writer = SBGATObjWriter::New();
			writer -> SetInputData(vtkPolyData::SafeDownCast(pgm_filter_mc -> GetInput()));
			std::string path = "../output/skewed_mc_shape_" + std::to_string(i) + ".obj";
			writer -> SetFileName(path.c_str());
			writer -> Update();
		}
		++progress;
	}
	end = std::chrono::system_clock::now();
	elapsed_seconds = end-start;
	std::cout << "\nMC of potential and acceleration computed in " << elapsed_seconds.count() << " seconds\n";

	std::vector<int> steps = {10,100,1000,N};

	for (auto step : steps){

		std::cout << "\t After " << step << " MC outcomes:\n";

		std::cout << "\t\tMC mean in slope: " << arma::mean(slopes_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::mean(slopes_mc.subvec(0,step - 1)) - nom_slope)/nom_slope * 100 << std::endl;

		std::cout << "\t\tMC variance in slope: " << arma::var(slopes_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::var(slopes_mc.subvec(0,step - 1)) - variance_slope_analytical)/arma::var(slopes_mc.subvec(0,step - 1)) * 100 << std::endl;

		std::cout << "\t\tMC variance in potential: " << arma::var(U_mc.subvec(0,step - 1)) << std::endl;
		std::cout << "\t\tError (%): " << (arma::var(U_mc.subvec(0,step - 1)) - variance_U_analytical)/arma::var(U_mc.subvec(0,step - 1)) * 100 << std::endl;

		std::cout << "\t\tMC Covariance in acceleration: \n" << arma::cov(A_mc.cols(0,step - 1).t()) << std::endl;
		std::cout << "\t\tError (%): " << arma::norm(arma::cov(A_mc.cols(0,step - 1).t()) - covariance_A_analytical)/arma::trace(arma::cov(A_mc.cols(0,step - 1).t())) * 100 << std::endl;

	}

	arma::mat P_CC_mc = arma::cov(deviations.t(),deviations.t());
	arma::vec sigma_sq_mc = arma::eig_sym(P_CC_mc);
	arma::vec sigma_sq_true = arma::eig_sym(P_CC);

	std::cout << "\t Relative error in MC vs prescribed deviation covariance eigenvalues (%): " << arma::norm(sigma_sq_mc - sigma_sq_true)/arma::norm(sigma_sq_true) * 100 << std::endl;
	std::cout << "\t Absolute difference in MC vs prescribed deviation covariance divided by mean prescribed deviation covariance eigenvalues (%): " << arma::abs(P_CC - P_CC_mc).max()/arma::mean(sigma_sq_true) * 100 << std::endl;

	std::cout << "- Done running test_PGM_UQ_skewed_km ..." << std::endl;

}

void TestsSBCore::test_PGM_UQ_covariance_consistency(){

	std::cout << "- Running test_PGM_UQ_covariance_consistency ..." << std::endl;

	double density = 1970;

	std::string filename  = "../../resources/shape_models/itokawa_8.obj";

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(filename.c_str());
	reader -> Update();

	// Cleaning
	vtkSmartPointer<vtkCleanPolyData> cleaner = vtkSmartPointer<vtkCleanPolyData>::New();
	cleaner -> SetInputConnection (reader -> GetOutputPort());
	cleaner -> SetOutputPointsPrecision ( vtkAlgorithm::DesiredOutputPrecision::DOUBLE_PRECISION );

	cleaner -> Update();

	// Creating the PGM dyads
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(cleaner -> GetOutputPort());
	pgm_filter -> SetDensity(density);
	pgm_filter -> SetScaleKiloMeters();
	pgm_filter -> Update();

	SBGATPolyhedronGravityModelUQ shape_uq;
	shape_uq.SetModel(pgm_filter);

	shape_uq.ComputeVerticesCovarianceGlobal(10,50);

	shape_uq.SaveNonZeroVerticesCovariance("../output/shape_covariance.json");
	shape_uq.SaveVerticesCovariance("../output/shape_covariance.txt");

	arma::mat P_CC;
	P_CC = shape_uq.GetVerticesCovariance();

	assert(shape_uq.LoadVerticesCovarianceFromJson("../output/shape_covariance.json"));

	arma::mat P_CC_from_Json = shape_uq.GetVerticesCovariance();

	assert(arma::abs(P_CC - P_CC_from_Json).max() / std::pow(10,2) < 1e-10);

	std::cout << "- Done running test_PGM_UQ_covariance_consistency ..." << std::endl;

}

// TODO : reimplement these tests

// /**
// This test computes simulated radar images for benchmarking purposes
// */
// void TestsSBCore::test_radar_obs(){

// 	std::cout << "- Running test_radar_obs ..." << std::endl;

// 	// Loading in the shape model
// 	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
// 	reader -> SetFileName("../../resources/shape_models/KW4Alpha.obj");
// 	reader -> Update();

// 	// Creating the radar object
// 	vtkSmartPointer<SBGATObsRadar> radar = vtkSmartPointer<SBGATObsRadar>::New();
// 	radar -> SetInputConnection(reader -> GetOutputPort());
// 	radar -> SetScaleKiloMeters();
// 	radar -> Update();

// 	// Arguments
// 	arma::vec spin = {0,0,1};
// 	arma::vec dir = {1,0,0};
// 	double period = 4 * 3600; // 4 hours
// 	int images = 48;
// 	int N = 100;

// 	double r_bin = 7.5;//(m)
// 	double rr_bin = 7.9e-3;//(m/s)

// 	// A sequence of images is collected
// 	SBGATRadarObsSequence measurement_sequence;
// 	auto start = std::chrono::system_clock::now();

// 	for (int i  = 0; i < images; ++i){
// 		double dt = 1.5 * double(i) / ((images - 1)) * period;

// 		std::cout << " --- Ray tracing " +std::to_string(i + 1) + "/" +std::to_string(images) + " ...\n";

// 		radar -> CollectMeasurementsSimpleSpin(measurement_sequence,N,dt,period,dir,spin);
// 	}

// 	radar -> BinObservations(measurement_sequence,r_bin,rr_bin);
// 	std::cout << " --- Done binning ...\n";

// 	radar -> SaveImages("../radar_output/");
// 	std::cout << " --- Done saving ...\n";

// 	auto end = std::chrono::system_clock::now();

// 	std::chrono::duration<double> elapsed_seconds = end-start;
// 	std::cout << "-- Done collecting radar images in " << elapsed_seconds.count() << " s\n";
// 	std::cout << "-- test_radar_obs successful" << std::endl;

// }

// *
// This test computes simulated lightcurves for benchmarking purposes

// void TestsSBCore::test_lightcurve_obs(){

// 	std::cout << "- Running test_lightcurve_obs ..." << std::endl;

// 	// Loading in the shape model
// 	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
// 	reader -> SetFileName("../../resources/shape_models/KW4Alpha.obj");
// 	reader -> Update();

// 	// Creating the radar object
// 	vtkSmartPointer<SBGATObsLightcurve> lightcurve = vtkSmartPointer<SBGATObsLightcurve>::New();
// 	lightcurve -> SetInputConnection(reader -> GetOutputPort());
// 	lightcurve -> SetScaleKiloMeters();
// 	lightcurve -> Update();

// 	// Arguments
// 	arma::vec spin = {0,0,1};
// 	arma::vec target_pos = {1e6,0,0};
// 	arma::vec observer_pos = {1e6,1e6,0};
// 	double period = 4 * 3600; // 4 hours
// 	int images = 100;
// 	int N = 100;

// 	// A sequence of images is collected
// 	std::vector<std::array<double, 2> > measurements;
// 	auto start = std::chrono::system_clock::now();

// 	for (int i  = 0; i < images; ++i){
// 		double dt = 360 * double(i) ; // one data point evey 360 s

// 		std::cout << " --- Ray tracing " +std::to_string(i + 1) + "/" +std::to_string(images) + " ...\n";
// 		lightcurve -> CollectMeasurementsSimpleSpin(measurements,N,dt,period,target_pos,observer_pos,spin);
// 		std::cout << measurements.back()[1] << std::endl;
// 	}

// 	auto end = std::chrono::system_clock::now();

// 	std::chrono::duration<double> elapsed_seconds = end-start;
// 	std::cout << "-- Done collecting lightcurve " << elapsed_seconds.count() << " s\n";
// 	std::cout << "-- test_lightcurve_obs successful" << std::endl;

// 	arma::vec mes_arma(measurements.size());
// 	for (unsigned int i = 0; i < measurements.size(); ++i){
// 		mes_arma(i) = measurements[i][1];
// 	}
// 	mes_arma.save("lightcurve.txt",arma::raw_ascii);

// }


//==================================================================================================
/*!
  @file

  @copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ALGORITHM_NONE_OF_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_NONE_OF_HPP_INCLUDED

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/detail/identity.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/pack.hpp>
#include <algorithm>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns true if f returns false for all the elements in the range [first,last)
    or if the range is empty, and false otherwise.

    @param first  Beginning of the range of elements
    @param last   End of the range of elements
    @param f   predicate function object that will be applied.

    @par Requirement

      - @c first and @c last must be pointer to Vectorizable type.

      - @c f must be a polymorphic unary function object, i.e cnoneable on generic types.
      - if @c f is not present the function test if all elements are zero.

    @par Example:

      @snippet none_of.cpp none_of

    @par Possible output:

      @snippet none_of.txt none_of
  **/
  template<typename T, typename Pred>
  bool none_of(T const* first, T const* last, Pred f)
  {
    auto pr = segmented_aligned_range(first,last);

    if(std::any_of(pr.head.begin(), pr.head.end(), f))
      return false;

    if(std::any_of(pr.body.begin(), pr.body.end(), [&f](const pack<T>&  x){ return any(f(x)); }))
      return false;

    if(std::any_of(pr.tail.begin(), pr.tail.end(), f))
      return false;

    return true;
  }

  template<typename T> BOOST_FORCEINLINE bool none_of(T const* first, T const* last)
  {
    return none_of(first, last, detail::identity());
  }
} }

#endif


#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iomanip>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "camodocal/calib/CameraCalibration.h"
#include "camodocal/chessboard/Chessboard.h"
#include "camodocal/gpl/gpl.h"

int
main( int argc, char** argv )
{
    cv::Size boardSize;
    float squareSize;
    std::string inputDir;
    std::string cameraModel;
    std::string cameraName;
    std::string prefix;
    std::string fileExtension;
    bool useOpenCV;
    bool viewResults;
    bool verbose;

    //========= Handling Program options =========
    boost::program_options::options_description desc( "Allowed options" );
    desc.add_options( )( "help", "produce help message" )(
    "width,w",
    boost::program_options::value< int >( &boardSize.width )->default_value( 8 ),
    "Number of inner corners on the chessboard pattern in x direction" )(
    "height,h",
    boost::program_options::value< int >( &boardSize.height )->default_value( 12 ),
    "Number of inner corners on the chessboard pattern in y direction" )(
    "size,s",
    boost::program_options::value< float >( &squareSize )->default_value( 7.f ),
    "Size of one square in mm" )( "input,i",
                                  boost::program_options::value< std::string >( &inputDir )->default_value( "calibrationdata" ),
                                  "Input directory containing chessboard images" )(
    "prefix,p",
    boost::program_options::value< std::string >( &prefix )->default_value( "left-" ),
    "Prefix of images" )( "file-extension,e",
                          boost::program_options::value< std::string >( &fileExtension )->default_value( ".png" ),
                          "File extension of images" )(
    "camera-model",
    boost::program_options::value< std::string >( &cameraModel )->default_value( "mei" ),
    "Camera model: kannala-brandt | mei | pinhole" )(
    "camera-name",
    boost::program_options::value< std::string >( &cameraName )->default_value( "camera" ),
    "Name of camera" )( "opencv",
                        boost::program_options::bool_switch( &useOpenCV )->default_value( true ),
                        "Use OpenCV to detect corners" )(
    "view-results",
    boost::program_options::bool_switch( &viewResults )->default_value( false ),
    "View results" )( "verbose,v",
                      boost::program_options::bool_switch( &verbose )->default_value( true ),
                      "Verbose output" );

    boost::program_options::positional_options_description pdesc;
    pdesc.add( "input", 1 );

    boost::program_options::variables_map vm;
    boost::program_options::store( boost::program_options::command_line_parser( argc, argv )
                                   .options( desc )
                                   .positional( pdesc )
                                   .run( ),
                                   vm );
    boost::program_options::notify( vm );

    if ( vm.count( "help" ) )
    {
        std::cout << desc << std::endl;
        return 1;
    }

    if ( !boost::filesystem::exists( inputDir ) && !boost::filesystem::is_directory( inputDir ) )
    {
        std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl;
        return 1;
    }

    camodocal::Camera::ModelType modelType;
    if ( boost::iequals( cameraModel, "kannala-brandt" ) )
    {
        modelType = camodocal::Camera::KANNALA_BRANDT;
    }
    else if ( boost::iequals( cameraModel, "mei" ) )
    {
        modelType = camodocal::Camera::MEI;
    }
    else if ( boost::iequals( cameraModel, "pinhole" ) )
    {
        modelType = camodocal::Camera::PINHOLE;
    }
    else if ( boost::iequals( cameraModel, "pinhole_full" ) )
    {
        modelType = camodocal::Camera::PINHOLE_FULL;
    }
    else if ( boost::iequals( cameraModel, "scaramuzza" ) )
    {
        modelType = camodocal::Camera::SCARAMUZZA;
    }
    else
    {
        std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl;
        return 1;
    }

    switch ( modelType )
    {
        case camodocal::Camera::KANNALA_BRANDT:
            std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl;
            break;
        case camodocal::Camera::MEI:
            std::cout << "# INFO: Camera model: Mei" << std::endl;
            break;
        case camodocal::Camera::PINHOLE:
            std::cout << "# INFO: Camera model: Pinhole" << std::endl;
            break;
        case camodocal::Camera::PINHOLE_FULL:
            std::cout << "# INFO: Camera model: PinholeFull" << std::endl;
            break;
        case camodocal::Camera::SCARAMUZZA:
            std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl;
            break;
    }

    // look for images in input directory
    std::vector< std::string > imageFilenames;
    boost::filesystem::directory_iterator itr;
    for ( boost::filesystem::directory_iterator itr( inputDir );
          itr != boost::filesystem::directory_iterator( );
          ++itr )
    {
        if ( !boost::filesystem::is_regular_file( itr->status( ) ) )
        {
            continue;
        }

        std::string filename = itr->path( ).filename( ).string( );

        // check if prefix matches
        if ( !prefix.empty( ) )
        {
            if ( filename.compare( 0, prefix.length( ), prefix ) != 0 )
            {
                continue;
            }
        }

        // check if file extension matches
        if ( filename.compare( filename.length( ) - fileExtension.length( ), fileExtension.length( ), fileExtension )
             != 0 )
        {
            continue;
        }

        imageFilenames.push_back( itr->path( ).string( ) );

        if ( verbose )
        {
            std::cerr << "# INFO: Adding " << imageFilenames.back( ) << std::endl;
        }
    }

    if ( imageFilenames.empty( ) )
    {
        std::cerr << "# ERROR: No chessboard images found." << std::endl;
        return 1;
    }

    if ( verbose )
    {
        std::cerr << "# INFO: # images: " << imageFilenames.size( ) << std::endl;
    }

    std::sort( imageFilenames.begin( ), imageFilenames.end( ) );

    cv::Mat image            = cv::imread( imageFilenames.front( ), -1 );
    const cv::Size frameSize = image.size( );

    camodocal::CameraCalibration calibration( modelType, cameraName, frameSize, boardSize, squareSize );
    calibration.setVerbose( verbose );

    std::vector< bool > chessboardFound( imageFilenames.size( ), false );
    for ( size_t i = 0; i < imageFilenames.size( ); ++i )
    {
        image = cv::imread( imageFilenames.at( i ), -1 );

        camodocal::Chessboard chessboard( boardSize, image );

        chessboard.findCorners( useOpenCV );
        if ( chessboard.cornersFound( ) )
        {
            if ( verbose )
            {
                std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", "
                          << imageFilenames.at( i ) << std::endl;
            }

            calibration.addChessboardData( chessboard.getCorners( ) );

            cv::Mat sketch;
            chessboard.getSketch( ).copyTo( sketch );

            cv::imshow( "Image", sketch );
            cv::waitKey( 50 );
        }
        else if ( verbose )
        {
            std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl;
        }
        chessboardFound.at( i ) = chessboard.cornersFound( );
    }
    cv::destroyWindow( "Image" );

    if ( calibration.sampleCount( ) < 10 )
    {
        std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl;
        return 1;
    }

    if ( verbose )
    {
        std::cerr << "# INFO: Calibrating..." << std::endl;
    }

    double startTime = camodocal::timeInSeconds( );

    calibration.calibrate( );
    calibration.writeParams( cameraName + "_camera_calib.yaml" );
    calibration.writeChessboardData( cameraName + "_chessboard_data.dat" );

    if ( verbose )
    {
        std::cout << "# INFO: Calibration took a total time of " << std::fixed
                  << std::setprecision( 3 ) << camodocal::timeInSeconds( ) - startTime << " sec.\n";
    }

    if ( verbose )
    {
        std::cerr << "# INFO: Wrote calibration file to "
                  << cameraName + "_camera_calib.yaml" << std::endl;
    }

    if ( viewResults )
    {
        std::vector< cv::Mat > cbImages;
        std::vector< std::string > cbImageFilenames;

        for ( size_t i = 0; i < imageFilenames.size( ); ++i )
        {
            if ( !chessboardFound.at( i ) )
            {
                continue;
            }

            cbImages.push_back( cv::imread( imageFilenames.at( i ), -1 ) );
            cbImageFilenames.push_back( imageFilenames.at( i ) );
        }

        // visualize observed and reprojected points
        calibration.drawResults( cbImages );

        for ( size_t i = 0; i < cbImages.size( ); ++i )
        {
            cv::putText( cbImages.at( i ),
                         cbImageFilenames.at( i ),
                         cv::Point( 10, 20 ),
                         cv::FONT_HERSHEY_COMPLEX,
                         0.5,
                         cv::Scalar( 255, 255, 255 ),
                         1,
                         CV_AA );
            cv::imshow( "Image", cbImages.at( i ) );
            cv::waitKey( 0 );
        }
    }

    return 0;
}


/* boost random/variate_generator.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Copyright Steven Watanabe 2011
 * Distributed under 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: variate_generator.hpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#ifndef BOOST_RANDOM_RANDOM_GENERATOR_HPP
#define BOOST_RANDOM_RANDOM_GENERATOR_HPP

#include <boost/random/detail/ptr_helper.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace boost {

/// \cond hide_private_members

namespace random {

///\endcond

/**
 * A random variate generator is used to join a random number
 * generator together with a random number distribution.
 * Boost.Random provides a vast choice of \generators as well
 * as \distributions.
 *
 * The argument for the template parameter Engine shall be of
 * the form U, U&, or U*, where U models a
 * \uniform_random_number_generator.  Then, the member
 * engine_value_type names U (not the pointer or reference to U).
 *
 * Specializations of @c variate_generator satisfy the
 * requirements of CopyConstructible. They also satisfy the
 * requirements of Assignable unless the template parameter
 * Engine is of the form U&.
 *
 * The complexity of all functions specified in this section
 * is constant. No function described in this section except
 * the constructor throws an exception.
 */
template<class Engine, class Distribution>
class variate_generator
{
private:
    typedef boost::random::detail::ptr_helper<Engine> helper_type;
public:
    typedef typename helper_type::value_type engine_value_type;
    typedef Engine engine_type;
    typedef Distribution distribution_type;
    typedef typename Distribution::result_type result_type;

    /**
     * Constructs a @c variate_generator object with the associated
     * \uniform_random_number_generator eng and the associated
     * \random_distribution d.
     *
     * Throws: If and what the copy constructor of Engine or
     * Distribution throws.
     */
    variate_generator(Engine e, Distribution d)
      : _eng(e), _dist(d) { }

    /** Returns: distribution()(engine()) */
    result_type operator()() { return _dist(engine()); }
    /**
     * Returns: distribution()(engine(), value).
     */
    template<class T>
    result_type operator()(const T& value) { return _dist(engine(), value); }

    /**
     * Returns: A reference to the associated uniform random number generator.
     */
    engine_value_type& engine() { return helper_type::ref(_eng); }
    /**
     * Returns: A reference to the associated uniform random number generator.
     */
    const engine_value_type& engine() const { return helper_type::ref(_eng); }

    /** Returns: A reference to the associated \random_distribution. */
    distribution_type& distribution() { return _dist; }
    /**
     * Returns: A reference to the associated random distribution.
     */
    const distribution_type& distribution() const { return _dist; }

    /**
     * Precondition: distribution().min() is well-formed
     *
     * Returns: distribution().min()
     */
    result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return (distribution().min)(); }
    /**
     * Precondition: distribution().max() is well-formed
     *
     * Returns: distribution().max()
     */
    result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return (distribution().max)(); }

private:
    Engine _eng;
    distribution_type _dist;
};

} // namespace random

using random::variate_generator;

} // namespace boost

#include <boost/random/detail/enable_warnings.hpp>

#endif // BOOST_RANDOM_RANDOM_GENERATOR_HPP


#include "ros/ros.h"
#include "OptiTrackFeedBackRigidBody.h"
#include "KeyboardEvent.h"
#include <Eigen/Eigen>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv)
{
    double Control_Rate = 40;// Hz the rate

    // Initialize ros node
    ros::init(argc, argv, "OptiTrackTest");
    ros::NodeHandle n;
    // Initialize Data Recorder
    //DataRecorder Rigidbody1_recorder("OptiTestRigidbody1.txt",31,Control_Rate);
        double Rigidbody1state[16];
    /*  0 OptTrack Flag
        1 - 3 Rigidbody1 positition from OpTiFeedback (hx,hy,Altitude)
        4 - 6 Rigidbody1 filtered velocity from OpTiFeedback
        7 - 9 Rigidbody1 raw velocity from Optifeedback
        10 - 12 Rigidbody1 raw angular velocity from Optifeedback
        13 - 15 Rigidbody1 filtered angular velocity from Optifeedback
        16 - 18 Rigidbody1 euler angle from OpTiFeedback
        19 - 22 Rigidbody1 quaterion from OpTiFeedback
        23 - 31 Rigidbody1 Rotation matrix R_IB from OptiFeedback
     */
    // Initialize OptiTrack System
    OptiTrackFeedBackRigidBody Opti_RigidBody1("/vrpn_client_node/UAV/pose",n,3,3);
    OptiTrackFeedBackRigidBody Opti_RigidBody2("/vrpn_client_node/RigidBody2/pose",n,3,3);
    KeyboardEvent keyboardcontrol;
    rigidbody_state RigidBody1state;
    ros::Rate loop_rate(Control_Rate);
  while (ros::ok())
  {
      Opti_RigidBody1.RosWhileLoopRun();
      Opti_RigidBody2.RosWhileLoopRun();
      keyboardcontrol.RosWhileLoopRun();
      switch (keyboardcontrol.GetPressedKey())
      {

        case U_KEY_E:
        {
          //Rigidbody1_recorder.StartRecording();
        }
        case U_KEY_Q:
        {
           Opti_RigidBody1.GetOptiTrackState();
           Opti_RigidBody2.GetOptiTrackState();
           Opti_RigidBody1.GetState(RigidBody1state);
           cout<< "Rigid 1 Position: \n" << RigidBody1state.Position << endl;
           cout<< "Rigid 1 Velocity: \n" << RigidBody1state.V_I << endl;
           cout<< "Rigid 1 AngularVelocity: \n" << RigidBody1state.Omega_BI << endl;
           cout<< "Rigid 1 R_IB: \n" << RigidBody1state.R_IB << endl;
           cout<< "Rigid 1 R_BI: \n" << RigidBody1state.R_BI << endl;
           cout<< "Rigid 1 Rotation Test: \n" << RigidBody1state.R_IB * RigidBody1state.R_BI<< endl;
           cout<< "Rigid 1 Euler: \n" <<RigidBody1state.Euler*57.3<<endl;
           cout<< "Rigid 1 quaterion: \n" <<RigidBody1state.quaterion<<endl;
           //Rigidbody1_recorder.StopRecording();
           break;
        }
        case U_KEY_NONE:
        {
          break;
        }
      }
      //Rigidbody1_recorder.RecorderUpdate(Rigidbody1state);
      ros::spinOnce();// do the loop once
      loop_rate.sleep();

  }
  return 0;
}


// $Revision: 1.5 $
// $Date: 2009-05-14 22:45:57 $
// $Source: /usr/local/cvs/OpenSees/SRC/system_of_eqn/eigenSOE/EigenSOE.cpp,v $

// Written: Jun Peng
// Created: Sat Feb. 6, 1999
// Revision: A
//
// Description: This file contains the class definition of EigenSOE.
// EigenSOE is a subclass of SystemOfEqn.
// It has pure virtual functions which must be implemented in it's derived
// subclasses. To solve the genreal eigen value equations means that
// by the given K and M, find the corresponding eigen value and eigen
// vectors.

#include <EigenSOE.h>
#include <EigenSolver.h>
#include <AnalysisModel.h>

EigenSOE::EigenSOE(EigenSolver &theEigenSolver, int classTag)
  :MovableObject(classTag), theSolver(&theEigenSolver)
{

}

EigenSOE::EigenSOE(int classTag)
  :MovableObject(classTag), theSolver(0)
{

}

EigenSOE::~EigenSOE()
{
  if (theSolver != 0)
    delete theSolver;
}

int
EigenSOE::solve(int numModes, bool generalized, bool findSmallest)
{
  if (theSolver == 0)
    return -1;
  else
    return (theSolver->solve(numModes, generalized, findSmallest));
}

int
EigenSOE::setSolver(EigenSolver &newSolver)
{
    theSolver = &newSolver;
    return 0;
}

EigenSolver *
EigenSOE::getSolver(void)
{
    return theSolver;
}

const Vector &
EigenSOE::getEigenvector(int mode) {
    return theSolver->getEigenvector(mode);
}

double
EigenSOE::getEigenvalue(int mode) {
    return theSolver->getEigenvalue(mode);
}

int
EigenSOE::setLinks(AnalysisModel &theModel)
{
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_SPLIT_HPP_INCLUDED
#define NT2_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_SPLIT_HPP_INCLUDED
#include <nt2/swar/functions/split.hpp>
#include <nt2/include/functions/load.hpp>
#include <nt2/include/functions/store.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_real.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/real.hpp>
// TODO pb simd_ is not recognized here !?
namespace nt2 { namespace ext
{
//   NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::split_, tag::cpu_,
//                               (A0)(A1)(X),
//                               ((simd_<complex_<arithmetic_<A0> >,X>))
//                               ((simd_<complex_<arithmetic_<A1> >,X>))
//                               ((simd_<complex_<arithmetic_<A1> >,X>))
//                             )
//   {
//     typedef int result_type;
//     inline result_type operator()(A0 const& a0,A1 & a1, A1 & a2) const
//     {
//       typedef typename meta::as_real<A1>::type rA1;
//       rA1 ar0, ar1, ai0, ai1;
//       split(nt2::real(a0), ar0, ar1);
//       split(nt2::imag(a0), ai0, ai1);
//       a1 = A1(ar0, ai0);
//       a2 = A1(ar1, ai1);
//       return 0;
//     }
//   };

//   NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::split_, tag::cpu_,
//                               (A0)(X),
//                               ((simd_<complex_<arithmetic_<A0> >,X>))
//                             )
//   {
//     typedef typename meta::as_real<A0>::type rA0;
//     typedef typename boost::dispatch::meta::upgrade<rA0>::type rU;
//     typedef typename meta::as_complex<rU>::type crU;
//     typedef boost::fusion::tuple<crU, crU> result_type;

//     NT2_FUNCTOR_CALL_REPEAT(1)
//     {
//       result_type res;
//       nt2::split(a0,  boost::fusion::at_c<0>(res),  boost::fusion::at_c<1>(res));
//       return res;
//     }
//   };

//    NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::split_, tag::cpu_,
//                               (A0)(A1)(X),
//                               ((simd_<imaginary_<arithmetic_<A0> >,X>))
//                               ((simd_<imaginary_<arithmetic_<A1> >,X>))
//                               ((simd_<imaginary_<arithmetic_<A1> >,X>))
//                             )
//   {
//     typedef int result_type;
//     inline result_type operator()(A0 const& a0,A1 & a1, A1 & a2) const
//     {
//       typedef typename meta::as_real<A1>::type rA1;
//       rA1 ai0, ai1;
//       split(nt2::imag(a0), ai0, ai1);
//       a1 = A1(ai0);
//       a2 = A1(ai1);
//       return 0;
//     }
//   };

//   NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::split_, tag::cpu_,
//                               (A0)(X),
//                               ((simd_<imaginary_<arithmetic_<A0> >,X>))
//                             )
//   {
//     typedef typename meta::as_real<A0>::type rA0;
//     typedef typename boost::dispatch::meta::upgrade<rA0>::type rU;
//     typedef typename meta::as_imaginary<rU>::type crU;
//     typedef boost::fusion::tuple<crU, crU> result_type;

//     NT2_FUNCTOR_CALL_REPEAT(1)
//     {
//       result_type res;
//       nt2::split(a0,  boost::fusion::at_c<0>(res),  boost::fusion::at_c<1>(res));
//       return res;
//     }
//   };

//   NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::split_, tag::cpu_,
//                               (A0)(A1)(X),
//                               ((simd_<dry_<arithmetic_<A0> >,X>))
//                               ((simd_<dry_<arithmetic_<A1> >,X>))
//                               ((simd_<dry_<arithmetic_<A1> >,X>))
//                             )
//   {
//     typedef int result_type;
//     inline result_type operator()(A0 const& a0,A1 & a1, A1 & a2) const
//     {
//       typedef typename meta::as_real<A1>::type rA1;
//       rA1 ai0, ai1;
//       split(nt2::imag(a0), ai0, ai1);
//       a1 = A1(ai0);
//       a2 = A1(ai1);
//       return 0;
//     }
//   };

//   NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::split_, tag::cpu_,
//                               (A0)(X),
//                               ((simd_<dry_<arithmetic_<A0> >,X>))
//                             )
//   {
//     typedef typename meta::as_real<A0>::type rA0;
//     typedef typename boost::dispatch::meta::upgrade<rA0>::type rU;
//     typedef typename meta::as_dry<rU>::type crU;
//     typedef boost::fusion::tuple<crU, crU> result_type;

//     NT2_FUNCTOR_CALL_REPEAT(1)
//     {
//       result_type res;
//       nt2::split(a0,  boost::fusion::at_c<0>(res),  boost::fusion::at_c<1>(res));
//       return res;
//     }
//   };
} }

#endif


/*!
@file
Defines forward `gem::dimension`.

@copyright Romain Brault 2013-2016
(See accompanying file LICENSE)
 */

#ifndef FWD_DIMENSIONS_HPP_INCLUDED
#define FWD_DIMENSIONS_HPP_INCLUDED

#include <boost/hana/core/tag_of.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/type.hpp>

#include <gem/concept/dimensions.hpp>

//////////////////////////////////////////////////////////////////////////
// Dimension forward declarations
//////////////////////////////////////////////////////////////////////////

namespace gem {

concepts::detail::UnsignedIntegral {T}
class DimensionTag;

concepts::detail::UnsignedIntegral {T}
class RuntimeDimensionTag;

concepts::detail::UnsignedIntegral {T}
class CompileTimeDimensionTag;

template <typename T, T cv, T max, T min>
class Dimension;

}  // namespace gem

//////////////////////////////////////////////////////////////////////////
// Dimension traits
//////////////////////////////////////////////////////////////////////////

namespace gem::detail {

template <typename>
struct is_dimension_impl;

template <typename T>
using is_dimension = is_dimension_impl<std::decay_t<T>>;

template <typename T>
using is_dimension_t = typename is_dimension<T>::type;

template <typename T>
constexpr auto is_dimension_v = is_dimension<T>::value;

}  // namespace gem::detail

//////////////////////////////////////////////////////////////////////////
// Dimension template deduction guide
//////////////////////////////////////////////////////////////////////////

namespace gem::hack {
    /* FIXME: concept simulation hack to avoid compile time error when calling
     * make_unsigned on integral_constant.
     *
     * Note that it cannot be done at concept level since gcc still try to
     * instanciate all template deduction guides to choose the most suitable.
     */

    gem::concepts::detail::Integral {T}
    struct safe_make_unsigned
    {
        static_assert(!std::is_same_v<boost::hana::tag_of_t<T>,
                      boost::hana::integral_constant_tag<T>>);
        using type = std::make_unsigned_t<T>;
    };

    /* Shortcuts */
    gem::concepts::detail::Integral {T}
    using safe_make_unsigned_t =
        typename safe_make_unsigned<T>::type;

    gem::concepts::detail::Integral {T}
    constexpr auto safe_make_unsigned_max =
        std::numeric_limits<safe_make_unsigned_t<T>>::max();

}  // namespace gem::hack

namespace gem {

gem::concepts::detail::IntegralConstant {T, cv}
Dimension(const boost::hana::integral_constant<T, cv> &)
    -> Dimension<hack::safe_make_unsigned_t<T>, cv, cv, cv>;

gem::concepts::detail::Integral {T}
Dimension(const T & rv)
    -> Dimension<hack::safe_make_unsigned_t<T>,
                 (hack::safe_make_unsigned_max<T> - 1) / 2 + 1,
                 hack::safe_make_unsigned_max<T>, 1>;

template <gem::concepts::detail::Integral T, T max>
Dimension(const T& value,
          const boost::hana::integral_constant<T, max>&)
    -> Dimension<hack::safe_make_unsigned_t<T>,
                 (max - 1) / 2 + 1, max, 1>;

template <gem::concepts::detail::Integral T, T max, T min>
Dimension(const T& value,
          const boost::hana::integral_constant<T, max>&,
          const boost::hana::integral_constant<T, min>&)
    -> Dimension<hack::safe_make_unsigned_t<T>,
                 (max - min) / 2 + min, max, min>;

}  // namespace gem

//////////////////////////////////////////////////////////////////////////
// Dimension concepts
//////////////////////////////////////////////////////////////////////////

namespace gem::concepts {

template <class D>
concept bool Dimension =
    boost::hana::value(gem::detail::is_dimension_impl<D> {});

Dimension {D}
concept bool CompileTimeDimension =
    boost::hana::value(D::min == D::max);

Dimension {D}
concept bool RuntimeDimension =
    !CompileTimeDimension<D>;

}  // namespace gem::concepts

#endif  // !FWD_DIMENSIONS_HPP_INCLUDED


// Copyright (C) 2013 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/vision/sfm/pose/util.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <glog/logging.h>
#include "theia/util/random.h"

namespace theia {

using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

void ComposeProjectionMatrix(const double focal_length[2],
                             const double principle_point[2],
                             const double rotation[9],
                             const double translation[3],
                             double projection_matrix[12]) {
  Matrix3d camera_matrix;
  camera_matrix << focal_length[0], 0.0, principle_point[0],
      0.0, focal_length[1], principle_point[1],
      0.0, 0.0, 1.0;

  Map<Matrix<double, 3, 4> > proj_mat(projection_matrix);
  proj_mat.block<3, 3>(0, 0) = Map<const Matrix3d>(rotation);
  proj_mat.block<3, 1>(0, 3) = Map<const Vector3d>(translation);
}

// Adds noise to the 3D point passed in.
void AddNoiseToPoint(const double noise_factor, Vector3d* point) {
  *point += Vector3d((-0.5 + RandDouble(0.0, 1.0)) * 2.0 * noise_factor,
                     (-0.5 + RandDouble(0.0, 1.0)) * 2.0 * noise_factor,
                     (-0.5 + RandDouble(0.0, 1.0)) * 2.0 * noise_factor);
}

// Adds noise to the ray i.e. the projection of the point.
void AddNoiseToProjection(const double noise_factor, Vector2d* ray) {
  const double noise_x = (-0.5 + RandDouble(0.0, 1.0)) * 2.0 * noise_factor;
  const double noise_y = (-0.5 + RandDouble(0.0, 1.0)) * 2.0 * noise_factor;

  *ray = Vector2d(ray->x() + noise_x, ray->y() + noise_y);
}

void AddGaussianNoise(const double noise_factor, Vector2d* ray) {
  const double noise_x = RandGaussian(0.0, noise_factor);
  const double noise_y = RandGaussian(0.0, noise_factor);
  *ray = Vector2d(ray->x() + noise_x, ray->y() + noise_y);
}

void CreateRandomPointsInFrustum(const double near_plane_width,
                                 const double near_plane_height,
                                 const double near_plane_depth,
                                 const double far_plane_depth,
                                 const int num_points,
                                 std::vector<Eigen::Vector3d>* random_points) {
  random_points->reserve(num_points);
  for (int i = 0; i < num_points; i++) {
    const double rand_depth = RandDouble(near_plane_depth, far_plane_depth);
    const double x_radius = near_plane_width * rand_depth / near_plane_depth;
    const double y_radius = near_plane_height * rand_depth / near_plane_depth;
    Vector3d rand_point(RandDouble(-x_radius, x_radius),
                        RandDouble(-y_radius, y_radius), rand_depth);
    random_points->push_back(rand_point);
  }
}

// For an E or F that is defined such that y^t * E * x = 0
double SampsonDistance(const Matrix3d& F, const Vector2d& x,
                       const Vector2d& y) {
  const Vector3d epiline_x = F * x.homogeneous();
  const Vector3d epiline_y = F.transpose() * y.homogeneous();

  const double numerator_sqrt = y.homogeneous().dot(epiline_x);
  const double denominator = epiline_x.hnormalized().squaredNorm() +
                             epiline_y.hnormalized().squaredNorm();

  // Finally, return the complete Sampson distance.
  return numerator_sqrt * numerator_sqrt / denominator;
}

Eigen::Matrix3d CrossProductMatrix(const Vector3d& cross_vec) {
  Matrix3d cross;
  cross << 0.0, -cross_vec.z(), cross_vec.y(),
      cross_vec.z(), 0.0, -cross_vec.x(),
      -cross_vec.y(), cross_vec.x(), 0.0;
  return cross;
}

// Computes the normalization matrix transformation that centers image points
// around the origin with an average distance of sqrt(2) to the centroid.
// Returns the transformation matrix and the transformed points. This assumes
// that no points are at infinity.
bool NormalizeImagePoints(
    const std::vector<Vector2d>& image_points,
    std::vector<Vector2d>* normalized_image_points,
    Matrix3d* normalization_matrix) {
  Eigen::Map<const Matrix<double, 2, Eigen::Dynamic> > image_points_mat(
      image_points[0].data(), 2, image_points.size());

  // Allocate the output vector and map an Eigen object to the underlying data
  // for efficient calculations.
  normalized_image_points->clear();
  normalized_image_points->resize(image_points.size());
  Eigen::Map<Matrix<double, 2, Eigen::Dynamic> >
      normalized_image_points_mat((*normalized_image_points)[0].data(), 2,
                                  image_points.size());

  // Compute centroid.
  const Vector2d centroid(image_points_mat.rowwise().mean());

  // Calculate average distance to centroid.
  const double mean_dist = (image_points_mat.colwise() - centroid).norm();

  // Create normalization matrix.
  const double norm_factor = sqrt(2.0) / mean_dist;
  *normalization_matrix << norm_factor, 0, -1.0 * norm_factor* centroid.x(),
      0, norm_factor, -1.0 * norm_factor * centroid.y(),
      0, 0, 1;

  // Normalize image points.
  const Matrix<double, 3, Eigen::Dynamic> normalized_homog_points =
      (*normalization_matrix) * image_points_mat.colwise().homogeneous();
  normalized_image_points_mat = normalized_homog_points.colwise().hnormalized();

  return true;
}

}  // namespace theia