// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#define _USE_MATH_DEFINES
#include <string>
#include <vector>
#include <exception>

#include <boost/make_shared.hpp>

#include <cmath>

#include "rttbIntegration.h"
#include "rttbNTCPLKBModel.h"
#include "rttbDvhBasedModels.h"
#include "rttbInvalidParameterException.h"
#include "rttbExceptionMacros.h"

namespace rttb
{

	namespace models
	{
		NTCPLKBModel::NTCPLKBModel() : NTCPModel(){
			_name = "NTCPLKBModel";
			fillParameterMap();
		}

		NTCPLKBModel::NTCPLKBModel(DVHPointer aDvh, BioModelParamType aD50, BioModelParamType aM,
		                           BioModelParamType aA):
								   NTCPModel(aDvh, aD50), _m(aM), _a(aA) {
									   _name = "NTCPLKBModel";
									   fillParameterMap();
								   }

		void NTCPLKBModel::setA(const BioModelParamType aA)
		{
			_a = aA;
		}

		const BioModelParamType NTCPLKBModel::getA()
		{
			return _a;
		}

		void NTCPLKBModel::setM(const BioModelParamType aM)
		{
			_m = aM;
		}

		const BioModelParamType NTCPLKBModel::getM()
		{
			return _m;
		}

		void NTCPLKBModel::setParameterVector(const ParamVectorType& aParameterVector)
		{
			if (aParameterVector.size() != 3)
			{
				throw core::InvalidParameterException("Parameter invalid: aParameterVector.size must be 3! ");
			}
			else
			{
				_d50 = aParameterVector.at(0);
				_m = aParameterVector.at(1);
				_a = aParameterVector.at(2);
			}
		}

		void NTCPLKBModel::setParameterByID(const int aParamId, const BioModelParamType aValue)
		{
			if (aParamId == 0)
			{
				_d50 = aValue;
			}
			else if (aParamId == 1)
			{
				_m = aValue;
			}
			else if (aParamId == 2)
			{
				_a = aValue;
			}
			else
			{
				throw core::InvalidParameterException("Parameter invalid: aParamID must be 0(for d50) or 1(for m) or 2(for a)! ");
			}
		}

		const int NTCPLKBModel::getParameterID(const std::string& aParamName) const
		{
			if (aParamName == "d50")
			{
				return 0;
			}
			else if (aParamName == "m")
			{
				return 1;
			}
			else if (aParamName == "a")
			{
				return 2;
			}
			else
			{
				rttbExceptionMacro(core::InvalidParameterException,
				                   << "Parameter name " << aParamName << " invalid: it should be d50 or m or a!");

			}
		}

		std::map<std::string, double> NTCPLKBModel::getParameterMap() const{
			return parameterMap;
		}

		void NTCPLKBModel::fillParameterMap(){
			parameterMap["d50"] = getD50();
			parameterMap["m"] = getM();
			parameterMap["a"] = getA();
		}

		std::string NTCPLKBModel::getModelType() const{
			return _name;
		}

		BioModelValueType NTCPLKBModel::calcModel(const double doseFactor)
		{
			if (_a == 0)
			{
				throw core::InvalidParameterException("_a must not be zero");
			}

			if (_m == 0)
			{
				throw core::InvalidParameterException("_m must not be zero");
			}

			core::DVH variantDVH = core::DVH(_dvh->getDataDifferential(),
			                                 (DoseTypeGy)(_dvh->getDeltaD() * doseFactor),
			                                 _dvh->getDeltaV(), "temporary", "temporary");

			auto spDVH = boost::make_shared<core::DVH>(variantDVH);
			double eud = getEUD(spDVH, this->_a);
			//_m must not be zero
			double t = (eud - this->_d50) / (this->_m * this->_d50);
			double value = 1 / pow(2 * M_PI, 0.5);

			double result = integrateLKB(t);

			if (result != -100)
			{
				value *= result;

				return value;
			}
			else
			{
				return false;
			}
		}

	}//end namespace models
}//end namespace rttb


//! [mandelbrot-all]
#include <chrono>
#include <iostream>
#include <numeric>
#include <vector>

#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/pack.hpp>

#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/sqr.hpp>

// if you want to see the julia set
// uncomment the two include lines and the two other commented areas with the
// same front line as this one
// this suppose you have opencv installed and the proper libraries on your
// compilation command
// opencv_core opencv_imgproc opencv_highgui
// #include <opencv2/contrib/contrib.hpp>
// #include <opencv2/highgui/highgui.hpp>
// #include <opencv2/imgproc/imgproc.hpp>

typedef float T;
namespace bs = boost::simd;
using pack_t = bs::pack<T>;
using pack_i = bs::pack<int>;
using pack_l = bs::as_logical_t<pack_t>;

struct mandelbrot
{
  int size, max_iter;
  T x_min, x_max, y_min, y_max, x_range, y_range;
  std::vector<int, bs::allocator<int>> iterations;

  mandelbrot(int size_, int max_iter_)
    : size(size_)
    , max_iter(max_iter_)
  {
    x_min   = -2.5;
    x_max   = 1;
    y_min   = -1;
    y_max   = 1;
    x_range = x_max - x_min;
    y_range = y_max - y_min;
    iterations.resize(size * size);
  }

  //! [mandelbrot-scalar]
  void evaluate_scalar()
  {
    for (int i = 0; i < size; ++i) {
      T x0 = T(i) / T(size) * x_range + x_min;
      for (int j = 0; j < size; ++j) {
        int iteration = 0;
        T y0          = T(j) / T(size) * y_range + y_min;
        T x           = 0;
        T y           = 0;
        T x2          = x * x;
        T y2          = y * y;
        while (x2 + y2 < 4 && iteration < max_iter) {
          x2 = x * x;
          y2 = y * y;
          T x_temp = x2 - y2 + x0;
          y        = 2 * x * y + y0;
          x        = x_temp;
          ++iteration;
        }
        iterations[j + i * size] = iteration;
      }
    }
  }
  //! [mandelbrot-scalar]

  //! [mandelbrot-simd]
  void evaluate_simd()
  {
    pack_t step =
      bs::enumerate<pack_t>(0); // produce a vector containing {0, 1, ..., pack_t::static_size-1}
    for (int i = 0; i < size; ++i) {
      pack_t x0{T(i) / T(size) * x_range + x_min};
      pack_t fac{y_range / T(size)};
      pack_t y_min_t{y_min};
      for (int j = 0; j < size; j += pack_t::static_size) {
        int iteration = 0;

        pack_t y0 = bs::fma(step + j, fac, y_min_t);
        pack_t x{0};
        pack_t y{0};
        pack_i iter{0};
        pack_l mask;
        do {
          pack_t x2 = bs::sqr(x);
          pack_t y2 = bs::sqr(y);

          y    = bs::fma(x + x, y, y0);
          x    = x2 - y2 + x0;
          mask = x2 + y2 < 4;
          ++iteration;
          iter = bs::if_inc(mask, iter);
        } while (bs::any(mask) && iteration < max_iter);
        bs::aligned_store(iter, &iterations[j + i * size]);
      }
    }
  }
  //! [mandelbrot-simd]

  // if you want to see the julia set
  //    void display() {
  //      cv::Mat display;
  //      cv::Mat A(size, size, CV_32SC1, iterations.data());
  //      A.convertTo(display, CV_8UC1, 255.0 / 1000.0);
  //      cv::applyColorMap(display, display, cv::COLORMAP_JET);
  //      cv::imshow("Mandelbrot", display);
  //      cv::waitKey(0);
  //    }
};

int main(int argc, char** argv)
{
  namespace chr = std::chrono;
  using hrc     = chr::high_resolution_clock;

  int size          = 1024;
  int max_iteration = 1000;
  mandelbrot image(size, max_iteration);
  auto t0 = hrc::now();
  image.evaluate_scalar();
  auto t1 = hrc::now();
  std::cout << " scalar " << chr::duration_cast<chr::milliseconds>(t1 - t0).count() << std::endl;

  t0 = hrc::now();
  image.evaluate_simd();
  t1 = hrc::now();
  std::cout << " simd " << chr::duration_cast<chr::milliseconds>(t1 - t0).count() << std::endl;
  // if you want to see the julia set
  //  image.display();
}
//! [mandelbrot-all]

// This code can be compiled using
//    g++ mandelbrot.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o mandelbrot
//    -I/pathto/boost_simd//include -I/pathto/boost
// or if you uncomment the opencv related lines to see the generated julia set
// and use:
//    g++ mandelbrot.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o mandelbrot
//    -I/pathto/boost_simd//include -I/pathto/boost -lopencv_core
//    -lopencv_imgproc -lopencv_highgui -lopencv_contrib


#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <Eigen/Dense>

//TODO: this might be better off in a util class / module

#define D2R(x) ((M_PI*x)/180.0)

void geo2ecef(Eigen::Vector3d & positionECEF, double latitude, double longitude, double ellipsoidalHeight) {
	/**WGS84 ellipsoid semi-major axis*/
	double a = 6378137.0;

	/**WGS84 ellipsoid first eccentricity squared*/
	double e2 = 0.081819190842622 * 0.081819190842622;

	double slat = sin(D2R(latitude));
	double clat = cos(D2R(latitude));
	double slon = sin(D2R(longitude));
	double clon = cos(D2R(longitude));

	double N = a / (sqrt(1 - e2 * slat * slat));
	double xTRF = (N + ellipsoidalHeight) * clat * clon;
	double yTRF = (N + ellipsoidalHeight) * clat * slon;
	double zTRF = (N * (1 - e2) + ellipsoidalHeight) * slat;

	positionECEF << xTRF, yTRF, zTRF;
};

class GeoreferenceProvider{
public:
	GeoreferenceProvider(): ecefLocalOrigin(0,0,0){
                odomPublisher=n.advertise<nav_msgs::Odometry>("odom", 50);

		depthRepublisher = n.advertise<geometry_msgs::PointStamped>("depth_ned",50);

                gpsSubscriber = n.subscribe("fix", 50, &GeoreferenceProvider::processGpsCallback,this);
                //attitudeSubscriber = n.subscribe("/imu/pos_ecef", 50, &GeoreferenceProvider::processPositionCallback,this);
		attitudeSubscriber = n.subscribe("/imu/data", 50, &GeoreferenceProvider::processAttitudeCallback,this);
		lidarSubscriber = n.subscribe("/velodyne_points",100,&GeoreferenceProvider::processLidarCallback,this);
		depthSubscriber = n.subscribe("/depth",100,&GeoreferenceProvider::processDepthCallback,this);

	}

	void processGpsCallback(const sensor_msgs::NavSatFix & msg){

		//Init ENU frame origin once we receive a first fix
		if(!gpsFixObtained && msg.status.status >= 0){
			gpsFixObtained = true;
			latitudeLocalOrigin = msg.latitude;
			longitudeLocalOrigin = msg.longitude;
			altitudeLocalOrigin = msg.altitude;

			ROS_INFO("GPS Fix obtained. Initializing NED transform at (%f,%f,%f)",latitudeLocalOrigin,longitudeLocalOrigin,altitudeLocalOrigin);

			geo2ecef(ecefLocalOrigin,latitudeLocalOrigin,longitudeLocalOrigin,altitudeLocalOrigin);

			ROS_INFO_STREAM("Local Origin:" << ecefLocalOrigin);

			//ENU
			//ecef2local << 	-sin(D2R(longitudeLocalOrigin)),	cos(D2R(longitudeLocalOrigin)),	0,
			//		-sin(D2R(latitudeLocalOrigin))*cos(D2R(longitudeLocalOrigin)), -sin(D2R(latitudeLocalOrigin))*sin(D2R(longitudeLocalOrigin)), cos(D2R(latitudeLocalOrigin)),
			//		cos(D2R(latitudeLocalOrigin))*cos(D2R(longitudeLocalOrigin)),cos(D2R(latitudeLocalOrigin))*sin(D2R(longitudeLocalOrigin)),sin(D2R(latitudeLocalOrigin));

			//NED
			ecef2local <<	-sin(D2R(latitudeLocalOrigin)) * cos(D2R(longitudeLocalOrigin)), -sin(D2R(latitudeLocalOrigin))*sin(D2R(longitudeLocalOrigin)),cos(D2R(latitudeLocalOrigin)),
				   	-sin(D2R(longitudeLocalOrigin)) , cos(D2R(longitudeLocalOrigin)), 0,
					-cos(D2R(latitudeLocalOrigin))*cos(D2R(longitudeLocalOrigin)), -cos(D2R(latitudeLocalOrigin))*sin(D2R(longitudeLocalOrigin)), -sin(D2R(latitudeLocalOrigin)) ;

			//ROS_INFO_STREAM("DCM:" << ecef2local);
		}
		else if(gpsFixObtained){
			processPosition(msg);
		}
	}

	void processAttitudeCallback(const sensor_msgs::Imu & msg){
		lastAttitude = msg;
	}

	void processLidarCallback(const sensor_msgs::PointCloud2 & msg){
		//std::cout << "Got lidar data" << std::endl;
	}

	void processDepthCallback(const geometry_msgs::PointStamped & msg){
		//FIXME: this is solved in the imagenex driver but not in the rosbag so we need to republish to NED frame
		geometry_msgs::PointStamped newMsg = msg;
		newMsg.point.z=-1 * msg.point.z;
		depthRepublisher.publish(newMsg);
	}

	/* Publish the odometry transform and Odometry message */
	void publishOdometry(Eigen::Vector3d & prpPointLocal){

		/* Broadcast the base_link to map transform */
                geometry_msgs::TransformStamped map_transform;
                map_transform.header.stamp = ros::Time::now(); //msg.header.stamp;
                map_transform.header.frame_id = "map"; //FIXME: we are assuming that the prp is the base_link origin
                map_transform.child_frame_id = "base_link";

                map_transform.transform.translation.x = prpPointLocal[0];
                map_transform.transform.translation.y = prpPointLocal[1];
                map_transform.transform.translation.z = prpPointLocal[2];
                map_transform.transform.rotation = lastAttitude.orientation;

                transformBroadcaster.sendTransform(map_transform);

                /* Publish the odometry message over ROS */
                nav_msgs::Odometry odom;
                odom.header.stamp = ros::Time::now();//msg.header.stamp;
                odom.header.frame_id = "odom";

                //set the position
                odom.pose.pose.position.x = prpPointLocal[0];
                odom.pose.pose.position.y = prpPointLocal[1];
                odom.pose.pose.position.z = prpPointLocal[2];

		//TODO: compensate attitude value using delta t, since our last attitude value dates from a little while back
                odom.pose.pose.orientation = lastAttitude.orientation;

                //set the velocity
                odom.child_frame_id = "base_link";
                //odom.twist.twist.linear.x = vx;
                //odom.twist.twist.linear.y = vy;
                //odom.twist.twist.angular.z = vth;

                //publish the message
                odomPublisher.publish(odom);
	}

	void processPosition(const sensor_msgs::NavSatFix & msg){
		if(gpsFixObtained){
			lastPosition = msg;

			//transform to ENU
			Eigen::Vector3d pointEcef;

			geo2ecef(pointEcef,msg.latitude,msg.longitude,msg.altitude);

			Eigen::Vector3d pointLocal = ecef2local * (pointEcef - ecefLocalOrigin);

			//Update odometry
			publishOdometry(pointLocal);
		}
	}

	void run(){
		ros::Rate r(10.0);

		//just spin...
		while(n.ok()){
			ros::spinOnce();
			r.sleep();
		}
	}

private:
	ros::NodeHandle n;
        ros::Publisher  odomPublisher;

	ros::Publisher  depthRepublisher;//FIXME: delete this

        ros::Subscriber gpsSubscriber;

        ros::Subscriber positionSubscriber;
	ros::Subscriber attitudeSubscriber;
	ros::Subscriber lidarSubscriber;
	ros::Subscriber depthSubscriber;

        tf2_ros::TransformBroadcaster transformBroadcaster;

	//These are used to georeference
	sensor_msgs::Imu 	lastAttitude;
	sensor_msgs::NavSatFix  lastPosition;

	bool gpsFixObtained = false;

	//This DCM is used to transform ECEF coordinates into a local NED frame centered at ecefEnuOrigin
	Eigen::Matrix3d	ecef2local;

	//The NED origin is set as the first position when a proper GPS fix is obtained
	Eigen::Vector3d ecefLocalOrigin;

	//NED origin in geodesic coordinates
	double latitudeLocalOrigin;
	double longitudeLocalOrigin;
	double altitudeLocalOrigin;
};

int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  GeoreferenceProvider georefProvider;

  georefProvider.run();
}


/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2014 Basil Fierz
 *
 * 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.
 */

// VCL configuration
#include <vcl/config/global.h>

// C++ standard library
#include <fstream>
#include <iostream>
#include <random>

// Eigen library
#include <Eigen/Dense>

// VCL
#include <vcl/core/simd/vectorscalar.h>
#include <vcl/core/interleavedarray.h>
#include <vcl/math/jacobieigen33_selfadjoint.h>
#include <vcl/math/jacobieigen33_selfadjoint_quat.h>
#include <vcl/util/precisetimer.h>

template<typename Scalar>
Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> createProblems(size_t nr_problems)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;

	Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> F(nr_problems);

	// Initialize data
	for (int i = 0; i < (int) nr_problems; i++)
	{
		Eigen::Matrix<Scalar, 3, 3> rnd;
		rnd << d(rng), d(rng), d(rng),
			   d(rng), d(rng), d(rng),
			   d(rng), d(rng), d(rng);
		F.template at<Scalar>(i) = rnd.transpose() * rnd;
	}

	return std::move(F);
}

template<typename Scalar>
void computeReferenceSolution
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& ATA,
	Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& U,
	Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& S
)
{
	// Compute reference using Eigen
	for (int i = 0; i < static_cast<int>(nr_problems); i++)
	{
		Vcl::Matrix3f A = ATA.template at<Scalar>(i);

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
		solver.compute(A, Eigen::ComputeEigenvectors);

		U.template at<Scalar>(i) = solver.eigenvectors();
		S.template at<Scalar>(i) = solver.eigenvalues();
	}
}

template<typename WideScalar>
void jacobiEig
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	int avg_nr_iter = 0;
	for (size_t i = 0; i < nr_problems / width; i++)
	{
		// Map data
		auto U = resU.at<real_t>(i);
		auto S = resS.at<real_t>(i);

		// Compute SVD using 2-sided Jacobi iterations (Brent)
		matrix3_t SV = F.at<real_t>(i);
		matrix3_t matU = matrix3_t::Identity();

		avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigen(SV, matU);

		// Store results
		U = matU;
		S = SV.diagonal();
	}
}

template<typename WideScalar>
void jacobiEigQuat
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	int avg_nr_iter = 0;
	for (size_t i = 0; i < nr_problems / width; i++)
	{
		// Map data
		auto U = resU.at<real_t>(i);
		auto S = resS.at<real_t>(i);

		// Compute SVD using Jacobi iterations and QR decomposition
		matrix3_t SV = F.at<real_t>(i);
		matrix3_t matU = matrix3_t::Identity();

		avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigenQuat(SV, matU);

		// Store results
		U = matU;
		S = SV.diagonal();
	}
}

template<typename REAL>
void SortEigenvalues(Eigen::Matrix<REAL, 3, 1>& A, Eigen::Matrix<REAL, 3, 3>& B)
{
	// Bubble sort
	bool swapped = true;
	int j = 0;

	while (swapped)
	{
		swapped = false;
		j++;

		for (int i = 0; i < 3 - j; i++)
		{
			if (A(i) > A(i + 1))
			{
				std::swap(A(i), A(i + 1));
				B.col(i).swap(B.col(i + 1));

				swapped = true;
			}
		}
	}
}

template<typename Scalar>
void checkSolution
(
	const char* Name,
	const char* file,
	size_t nr_problems,
	Scalar tol,
	const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& refUa,
	const Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& refSa,
	const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& resUa,
	const Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& resSa
)
{
	using scalar_t = Scalar;

	int wrong_computations, wrong_u_computations;
	scalar_t accum_error, accum_u_error;
	std::ofstream fout;

	wrong_computations = 0;
	wrong_u_computations = 0;
	accum_error = 0;
	accum_u_error = 0;
	fout.open(file);

	for (int j = 0; j < (int) nr_problems; j++)
	{
		Vcl::Matrix3f refU = refUa.template at<scalar_t>(j);
		Vcl::Vector3f refS = refSa.template at<scalar_t>(j);
		Vcl::Matrix3f cU = resUa.template at<scalar_t>(j);
		Vcl::Vector3f cS = resSa.template at<scalar_t>(j);

		SortEigenvalues(cS, cU);

		bool eqU = refU.array().abs().isApprox(cU.array().abs(), tol);
		bool eqS = refS.array().abs().isApprox(cS.array().abs(), tol);

		if (!eqS || !eqU)
			fout << j;

		if (!eqS)
		{
			wrong_computations++;
			scalar_t err = abs((refS.array().abs() - cS.array().abs()).sum() / scalar_t(3));
			accum_error += err;
			fout << ", E: " << err;
		}
		if (!eqU)
		{
			wrong_u_computations++;
			scalar_t err = abs((refU.array().abs() - cU.array().abs()).sum() / scalar_t(9));
			accum_u_error += err;
			fout << ", U: " << err;
		}
		if (!eqS || !eqU)
			fout << std::endl;
	}

	fout.close();
	std::cout << Name << " - Errors: (" << wrong_computations << ", " << wrong_u_computations << "), "
			  << "Avg. Singular value error: " << accum_error / std::max(wrong_computations, 1) << ", "
			  << "Avg. U error: " << accum_u_error / std::max(wrong_u_computations, 1) << std::endl;
}

int main(int, char**)
{
	size_t nr_problems = 1024*1024;

	using scalar_t = float;

	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> resU(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 1, -1> resS(nr_problems);

	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> refU(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 1, -1> refS(nr_problems);

	auto F = createProblems<scalar_t>(nr_problems);
	computeReferenceSolution(nr_problems, F, refU, refS);

	// Test correctness: Two-sided Jacobi SVD (Brent)
	jacobiEig<float>(nr_problems, F, resU, resS);        checkSolution("JacobiEigen - float",   "jacobi_eigen_float_errors.txt",   nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEig<Vcl::float4>(nr_problems, F, resU, resS);  checkSolution("JacobiEigen - float4",  "jacobi_eigen_float4_errors.txt",  nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEig<Vcl::float8>(nr_problems, F, resU, resS);  checkSolution("JacobiEigen - float8",  "jacobi_eigen_float8_errors.txt",  nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEig<Vcl::float16>(nr_problems, F, resU, resS); checkSolution("JacobiEigen - float16", "jacobi_eigen_float16_errors.txt", nr_problems, 1e-5f, refU, refS, resU, resS);

	// Test correctness: Jacobi SVD with symmetric EV computation and QR decomposition
	jacobiEigQuat<float>(nr_problems, F, resU, resS);        checkSolution("JacobiEigenQuat - float",   "jacobi_eigen_quat_float_errors.txt",   nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEigQuat<Vcl::float8>(nr_problems, F, resU, resS);	 checkSolution("JacobiEigenQuat - float8",  "jacobi_eigen_quat_float8_errors.txt",  nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEigQuat<Vcl::float4>(nr_problems, F, resU, resS);	 checkSolution("JacobiEigenQuat - float4",  "jacobi_eigen_quat_float4_errors.txt",  nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEigQuat<Vcl::float16>(nr_problems, F, resU, resS); checkSolution("JacobiEigenQuat - float16", "jacobi_eigen_quat_float16_errors.txt", nr_problems, 1e-5f, refU, refS, resU, resS);
}


//
// Created by krab1k on 27.08.20.
//

#include <vector>
#include <cmath>
#include <Eigen/LU>

#include "sqeq0.h"
#include "../parameters.h"
#include "../geometry.h"

CHARGEFW2_METHOD(SQEq0)

std::vector<double> SQEq0::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();
    size_t m = molecule.bonds().size();

    Eigen::VectorXd q0 = Eigen::VectorXd::Zero(n);
    Eigen::VectorXd hardness = Eigen::VectorXd::Zero(n);

    for (size_t i = 0; i < n; i++) {
        const auto &atom = molecule.atoms()[i];
        q0(i) = atom.formal_charge();
        hardness(i) = parameters_->atom()->parameter(atom::hardness)(atom);
    }

    Eigen::MatrixXd T = Eigen::MatrixXd::Zero(m, n);
    for (size_t i = 0; i < molecule.bonds().size(); i++) {
        const auto &bond = molecule.bonds()[i];
        auto i1 = bond.first().index();
        auto i2 = bond.second().index();
        T(i, i1) = 1;
        T(i, i2) = -1;
    }

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n, n);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n);

    /* Setup EEM part */
    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = molecule.atoms()[i];
        A(i, i) = hardness(i);
        b(i) = -parameters_->atom()->parameter(atom::electronegativity)(atom_i);
        for (size_t j = i + 1; j < n; j++) {
            const auto &atom_j = molecule.atoms()[j];
            auto d = distance(atom_i, atom_j);
            auto wi = parameters_->atom()->parameter(atom::width)(atom_i);
            auto wj = parameters_->atom()->parameter(atom::width)(atom_j);
            auto d0 = sqrt(2 * wi * wi + 2 * wj * wj);
            auto x = erf(d / d0) / d;
            A(i, j) = x;
            A(j, i) = x;
        }
    }

    b = b - A * q0;
    b = b + hardness.cwiseProduct(q0);

    Eigen::MatrixXd split_A = T * A * T.transpose();
    Eigen::VectorXd split_b = T * b;

    for (size_t i = 0; i < molecule.bonds().size(); i++) {
        const auto &bond = molecule.bonds()[i];
        split_A(i, i) += parameters_->bond()->parameter(bond::kappa)(bond);
    }

    Eigen::VectorXd split_q = split_A.partialPivLu().solve(split_b);
    Eigen::VectorXd q = T.transpose() * split_q + q0;

    return std::vector<double>(q.data(), q.data() + n);
}


#include "localizationkf.h"
#include "Storage/logController.h"

#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Sparse>

#include <math.h>

LocalizationKF::LocalizationKF(LogController& logcontroller):
_logcontroller(logcontroller)
{};

void LocalizationKF::setup(){
    //this current intializaiton method means the filter will take time to converge to the solution
    X = Eigen::Vector<float,6>::Zero();
    P = Eigen::Matrix<float,6,6>::Identity(); // initialize as identity matrix for ease -> can update this later
};

void LocalizationKF::predict(const Eigen::Vector3f& linear_acceleration,float dt){
    const Eigen::Matrix<float,6,6> A{{1, dt, 0, 0, 0, 0},
                                     {0, 1, 0, 0, 0, 0},
                                     {0, 0, 1, dt, 0, 0},
                                     {0, 0, 0, 1, 0, 0},
                                     {0, 0, 0, 0, 1, dt},
                                     {0, 0, 0, 0, 0, 1}}; // construct state transition sub matrix

    const float dt2 = dt*dt;//temporary for readability
    const float dt3 = dt2*dt;
    const Eigen::Matrix<float,6,3> B{{dt2/2, 0, 0},
                                     {dt, 0, 0},
                                     {0, dt2/2, 0},
                                     {0, dt, 0},
                                     {0, 0, dt2/2},
                                     {0, 0,dt}}; // construct control sub matrix

    //dt^4 terms are considered negligible so have been estimated as zero
    const Eigen::Matrix<float,6,6> Q = accelVariance * Eigen::Matrix<float,6,6> {{0, dt3/2, 0, 0, 0, 0},
                                                                                {dt3/2, dt2, 0, 0, 0, 0},
                                                                                {0, 0, 0, dt3/2, 0, 0},
                                                                                {0, 0, dt3/2, dt2, 0, 0},
                                                                                {0, 0, 0, 0, 0, dt3/2},
                                                                                {0, 0, 0, 0, dt3/2, dt2}}; // construct state transition sub matrix

    //update State
    X = (A*X) + (B*linear_acceleration);
    //update Covariance estimate
    P = A*P*A.transpose() + Q;
}

void LocalizationKF::gpsUpdate(const float lat, const float lng, const long alt,const long vn, const long ve, const long vd){
    //measurement matrix
    Eigen::Vector3f positionNED = GPStoNED(lat,lng,alt);
    const Eigen::Vector<float,6> z{{positionNED(0),
                                    ((float)vn)/1000.0f, //conversion to m/s
                                    positionNED(1),
                                    ((float)ve)/1000.0f,
                                    positionNED(2),
                                    ((float)vd)/1000.0f}};

    //System Uncertainty
    Eigen::Matrix<float,6,6> S_GPS = P+R_GPS.toDenseMatrix();
    //Kalman Gain
    Eigen::Matrix<float,6,6> K_GPS = P*(S_GPS).inverse();
    //Reisdual
    Eigen::Vector<float,6> Y_GPS = z-X;
    //State update
    X = X + (K_GPS*Y_GPS);
    //Covariance update
    P = ( (Eigen::Matrix<float,6,6>::Identity() - K_GPS) * P * ((Eigen::Matrix<float,6,6>::Identity() - K_GPS).transpose()) ) + (K_GPS*R_GPS*K_GPS.transpose());

}

void LocalizationKF::updateGPSReference(const float lat, const float lng, const long alt){
    _gpsReferenceECEF = GPStoECEF(lat,lng,alt);
    _gpsReferenceSLat = sin(lat * degtorad);
    _gpsReferenceCLat = cos(lat * degtorad);
    _gpsReferenceSLng = sin(lng * degtorad);
    _gpsReferenceCLng = cos(lng * degtorad);
}

Eigen::Vector3f LocalizationKF::GPStoECEF(const float lat, const float lng,const long alt){
    const float slat = sin(lat * degtorad);
    const float clat = cos(lat * degtorad);
    const float slng = sin(lng * degtorad);
    const float clng = cos(lng * degtorad);

    const float ecefN = earthMajorAxis/sqrt(1-(earthEccentricity2*slat*slat));
    const float h = alt/1000.0f; //conversion from mm to m

    float ecefX = (ecefN + h)*clat*clng;
    float ecefY = (ecefN + h)*clat*slng;
    float ecefZ = (((1-earthEccentricity2)*ecefN) + h)*slat;

    return Eigen::Vector3f{ecefX,ecefY,ecefZ};
}

Eigen::Vector3f LocalizationKF::GPStoNED(const float lat, const float lng, const long alt){
    Eigen::Matrix3f  _tangentPlane{{-_gpsReferenceSLng, _gpsReferenceCLng, 0},
                                   {-_gpsReferenceSLat*_gpsReferenceCLng, -_gpsReferenceSLat*_gpsReferenceSLng, _gpsReferenceCLat},
                                   {_gpsReferenceCLat*_gpsReferenceCLng, _gpsReferenceCLat*_gpsReferenceSLng, _gpsReferenceSLat}};
    Eigen::Vector3f ENU = _tangentPlane * (GPStoECEF(lat,lng,alt) - _gpsReferenceECEF);
    Eigen::Vector3f NED({ENU(1),ENU(0),-ENU(2)});
    return NED;
}


/*!
*   \file expm.hpp
*
*   Implement matrix exponential using pade approximation.
*
*  Copyright (c) 2007
*  \author Tsai, Dung-Bang
*
*/
//  Department of Physics,
//  National Taiwan University.
//
//  E-Mail : dbtsai [_at_] dbtsai [_dot_] org
//  Begine : 2007/11/20
//  Last modify : 2007/11/26
//  Version : v0.4
//
//  expm_pad computes the matrix exponential exp(H) for general matrixs,
//  including complex and real matrixs using the irreducible (p,p) degree
//  rational Pade approximation to the exponential
//  exp(z) = r(z) = (+/-)( I+2*(Q(z)/P(z))).
//
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt).
//

#ifndef _BOOST_UBLAS_EXPM_
#define _BOOST_UBLAS_EXPM_
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/traits.hpp>

namespace boost { namespace numeric { namespace ublas {

template<typename MATRIX> MATRIX expm_pad(const MATRIX &H, typename type_traits<typename MATRIX::value_type>::real_type t = 1.0, const int p = 6){
	typedef typename MATRIX::value_type value_type;
        typedef typename MATRIX::size_type size_type;
	typedef typename type_traits<value_type>::real_type real_value_type;
	assert(H.size1() == H.size2());
	assert(p >= 1);
	const size_type n = H.size1();
	const identity_matrix<value_type> I(n);
	matrix<value_type> U(n,n),H2(n,n),P(n,n),Q(n,n);
	real_value_type norm = 0.0;
// Calcuate Pade coefficients
	vector<real_value_type> c(p+1);
	c(0)=1;
	for(size_type i = 0; i < (size_type) p; ++i)
		c(i+1) = c(i) * ((p - i)/((i + 1.0) * (2.0 * p - i)));
// Calcuate the infinty norm of H, which is defined as the largest row sum of a matrix
	for(size_type i=0; i<n; ++i) {
		real_value_type temp = 0.0;
		for(size_type j = 0; j < n; j++)
			temp += std::abs(H(i, j));
		norm = t * std::max<real_value_type>(norm, temp);
	}
// If norm = 0, and all H elements are not NaN or infinity but zero,
// then U should be identity.
	if (norm == 0.0) {
		bool all_H_are_zero = true;
		for(size_type i = 0; i < n; i++)
			for(size_type j = 0; j < n; j++)
				if( H(i,j) != value_type(0.0) )
					all_H_are_zero = false;
		if( all_H_are_zero == true ) return I;
// Some error happens, H has elements which are NaN or infinity.
		std::cerr<<"Null input error in the template expm_pad.\n";
		//		std::cout << "Null INPUT : " << H <<"\n";
		exit(0);
	}
// Scaling, seek s such that || H*2^(-s) || < 1/2, and set scale = 2^(-s)
 	int s = 0;
	real_value_type scale = 1.0;
	if(norm > 0.5) {
		s = std::max<int>(0, static_cast<int>((log(norm) / log(2.0) + 2.0)));
		scale /= real_value_type(std::pow(2.0, s));
		U.assign((scale * t) * H); // Here U is used as temp value due to that H is const
	}
	else
		U.assign(H);

// Horner evaluation of the irreducible fraction, see the following ref above.
// Initialise P (numerator) and Q (denominator)
	H2.assign( prod(U, U) );
	Q.assign( c(p)*I );
	P.assign( c(p-1)*I );
	size_type odd = 1;
	for( size_type k = p - 1; k > 0; --k) {
		( odd == 1 ) ?
			( Q = ( prod(Q, H2) + c(k-1) * I ) ) :
			( P = ( prod(P, H2) + c(k-1) * I ) ) ;
		odd = 1 - odd;
	}
	( odd == 1 ) ? ( Q = prod(Q, U) ) : ( P = prod(P, U) );
	Q -= P;
// In origine expokit package, they use lapack ZGESV to obtain inverse matrix,
// and in that ZGESV routine, it uses LU decomposition for obtaing inverse matrix.
// Since in ublas, there is no matrix inversion template, I simply use the build-in
// LU decompostion package in ublas, and back substitute by myself.

// Implement Matrix Inversion
	permutation_matrix<size_type> pm(n);
	int res = lu_factorize(Q, pm);
	if( res != 0) {
		std::cerr << "Matrix inversion error in the template expm_pad.\n";
		exit(0);
	}
// H2 is not needed anymore, so it is temporary used as identity matrix for substituting.
	H2.assign(I);
	lu_substitute(Q, pm, H2);
	(odd == 1) ?
		( U.assign( -(I + real_value_type(2.0) * prod(H2, P))) ):
		( U.assign(   I + real_value_type(2.0) * prod(H2, P) ) );
// Squaring
	for(size_type i = 0; i < (size_type) s; ++i)
		U = (prod(U,U));
	return U;
}

}}}

#endif


// two_dim_rand.cpp
//
#include <iostream>

#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>

double on_circle_prob(double x, double y)
{
    static const double k_delta = 0.01;
    if (std::abs(x*x + y*y - 1) <= k_delta)
        return 1.0;
    else
        return 0;
}

int main(int argc, char* argv[])
{
    using boost::lexical_cast;
    using boost::random::uniform_real_distribution;
    using boost::random::discrete_distribution;

    std::size_t samples = argc > 1 ? lexical_cast<std::size_t>(argv[1]) : 100;
    std::size_t n = argc > 2 ? lexical_cast<std::size_t>(argv[2]) : 100;
    double xmin = -1.0;
    double xmax = 1.0;
    double ymin = -1.0;
    double ymax = 1.0;

    boost::mt19937 gen;
    uniform_real_distribution<> x_dist(xmin, xmax);
    for (int i = 0; i < samples; ) {
        double x = x_dist(gen);
        discrete_distribution<> y_dist(n, ymin, ymax, boost::bind(on_circle_prob, x, _1));
        int rand_n = y_dist(gen);
        double y = (rand_n * 1.0 / n) * (ymax - ymin) + ymin;
        if (true || on_circle_prob(x, y) != 0) {
            std::cout << x << ' ' << y << std::endl;
            i++;
        }
    }
}


// eig1.cpp

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

int main(int argc, char const *argv[])
{
    // 2x2 matrix, of type float
    Eigen::Matrix<float, 2, 2> A, B;
    A << 2, -1, -1, 3;
    B << 1, 2, 3, 1;

    std::cout << "A:     " << A <<  '\n';
    std::cout << "B:     " << B << '\n';
    std::cout << "A+B:   " << A+B << '\n';
    std::cout << "A-B:   " << A-B << '\n';
    std::cout << "1.6*A: " << 1.6*A << '\n';

    return 0;
}


#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include "covariance.h"

CovObj::CovObj()
{
    clearCov();
}

CovObj::CovObj(const Vector3d &v1, const Vector3d &v2, const Vector3d &v3)
{
    area_ = 0.5 * (v2 - v1).cross(v3 - v1).norm();
    center_ = 1 / 3.0 * (v1 + v2 + v3);
    Matrix3d vm, c;
    vm(0, 0) = v1(0);
    vm(1, 0) = v1(1);
    vm(2, 0) = v1(2);
    vm(0, 1) = v2(0);
    vm(1, 1) = v2(1);
    vm(2, 1) = v2(2);
    vm(0, 2) = v3(0);
    vm(1, 2) = v3(1);
    vm(2, 2) = v3(2);
    c(0, 0) = c(1, 1) = c(2, 2) = 2;
    c(0, 1) = c(0, 2) = c(1, 0) = c(1, 2) = c(2, 0) = c(2, 1) = -1;
    cov_ = area_ / 36.0 * vm * c * vm.transpose();
    size_ = 1;
}

void CovObj::clearCov()
{
    cov_.setZero();
    area_ = 0;
    size_ = 0;
    center_ = normal_ = Vector3d::Zero();
}

CovObj &CovObj::operator+=(const CovObj &Q)
{
    // some models may contain corrupted null faces, ignore them
    if (Q.area_ < 1e-18)
        return *this;
    CovObj Q_old = *this;
    area_ = Q_old.area_ + Q.area_;
    center_ = (Q_old.area_ * Q_old.center_ + Q.area_ * Q.center_) / (area_);
    Vector3d ktoi, ktoj;
    ktoi = Q_old.center_ - center_;
    ktoj = Q.center_ - center_;
    cov_ = Q_old.cov_ + Q.cov_ + Q_old.area_ * ktoi * ktoi.transpose() + Q.area_ * ktoj * ktoj.transpose();
    size_ += Q.size_;
    return *this;
}

CovObj &CovObj::operator-=(const CovObj &Q)
{
    // some models may contain corrupted null faces, ignore them
    if (Q.area_ < 1e-18)
        return *this;
    CovObj Q_old = *this;
    area_ = Q_old.area_ - Q.area_;
    center_ = (Q_old.area_ * Q_old.center_ - Q.area_ * Q.center_) / (area_);
    Vector3d ktoi, ktoj;
    ktoi = Q_old.center_ - center_;
    ktoj = Q_old.center_ - Q.center_;
    cov_ = Q_old.cov_ - Q.cov_ - area_ * ktoi * ktoi.transpose() - Q.area_ * ktoj * ktoj.transpose();
    size_ -= Q.size_;
    return *this;
}

bool CovObj::operator==(const CovObj &Q)
{
    return cov_ == Q.cov_ && normal_ == Q.normal_ && center_ == Q.center_ && area_ == Q.area_ && size_ == Q.size_;
}

CovObj &CovObj::operator=(const CovObj &Q)
{
    // A good habit to check for self-assignment
    if (this == &Q)
        return *this;

    cov_ = Q.cov_;
    normal_ = Q.normal_;
    center_ = Q.center_;
    area_ = Q.area_;
    size_ = Q.size_;
    return *this;
}

double CovObj::energy()
{
    if (area_ < 1e-10 || size_ <= 1) // 1 triangle has 0 energy
        return 0;
    if (cov_.determinant() / pow(area_, 5) < 1e-15)
        return cov_.trace() * area_ * 1e-20;
    else
        return cov_.determinant() / pow(area_, 4);
}

void CovObj::computePlaneNormal()
{
    Eigen::SelfAdjointEigenSolver<Matrix3d> es(cov_);
    int smallest = 0;

    for (int i = 1; i < 3; i++)
    {
        if (abs(es.eigenvalues()[i]) < abs(es.eigenvalues()[smallest]))
            smallest = i;
    }
    // Plane normal is the eigenvector corresponding to the smallest eigenvalue
    normal_ = es.eigenvectors().col(smallest);
    normal_.normalize();
}


//
// Created by Amir Masoud Abdol on 2019-01-24.
//

#include <spdlog/spdlog.h>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>

#include "TestStrategy.h"

using namespace sam;

using boost::math::students_t;

TestStrategy::~TestStrategy(){
    // Pure destructors
};

std::unique_ptr<TestStrategy> TestStrategy::build(json &test_strategy_config) {

  if (test_strategy_config["name"] == "TTest") {

    auto params = test_strategy_config.get<TTest::Parameters>();
    return std::make_unique<TTest>(params);

  } else if (test_strategy_config["name"] == "YuenTest") {

    auto params = test_strategy_config.get<YuenTest::Parameters>();
    return std::make_unique<YuenTest>(params);

  } else if (test_strategy_config["name"] == "WilcoxonTest") {
    auto params = test_strategy_config.get<WilcoxonTest::Parameters>();
    return std::make_unique<WilcoxonTest>(params);
  } else {
    spdlog::critical("Unknown Test Strategy.");
    exit(1);
  }
}

namespace sam {

///
/// Calculate confidence intervals for the mean. For example if we set the
/// confidence limit to 0.95, we know that if we repeat the sampling 100 times,
/// then we expect that the true mean will be between out limits on 95 occasions.
/// Note: this is not the same as saying a 95% confidence interval means that there
/// is a 95% probability that the interval contains the true mean. The interval
/// computed from a given sample either contains the true mean or it does not.
///
/// @note       Obtained from [Boost Library
///             Example](https://www.boost.org/doc/libs/1_69_0/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/paired_st.html).
///
/// @param      Sm    Sample Mean.
/// @param      Sd    Sample Standard Deviation.
/// @param      Sn    Sample Size.
///
std::pair<float, float>
confidence_limits_on_mean(float Sm, float Sd, unsigned Sn, float alpha,
                          TestStrategy::TestAlternative alternative) {

  using namespace sam;

  students_t dist(Sn - 1);

  // calculate T
  float T =
      quantile(complement(dist, alpha / 2)); // TODO: Implement the side!./sa

  // Calculate width of interval (one sided):
  float w = T * Sd / sqrt(float(Sn));

  // Calculate and return the interval
  return std::make_pair(Sm - w, Sm + w);
}

///
/// Caculate the degress of freedom to achieve a significance result with the given
/// alpha
///
/// @param      M     True Mean.
/// @param      Sm    Sample Mean.
/// @param      Sd    Sample Standard Deviation.
///
float single_sample_find_df(float M, float Sm, float Sd, float alpha,
                             TestStrategy::TestAlternative alternative) {
  using namespace sam;
  using boost::math::students_t;

  // calculate df for one-sided or two-sided test:
  float df = students_t::find_degrees_of_freedom(
      fabs(M - Sm),
      (alternative == TestStrategy::TestAlternative::Greater) ? alpha
                                                              : alpha / 2.,
      alpha, Sd);

  // convert to sample size, always one more than the degrees of freedom:
  return ceil(df) + 1;
}

float win_var(const arma::Row<float> &x, const float trim) {
  return arma::var(win_val(x, trim));
}

std::pair<float, float> win_cor_cov(const arma::Row<float> &x,
                                      const arma::Row<float> &y,
                                      const float trim) {

  arma::Row<float> xvec{win_val(x, trim)};
  arma::Row<float> yvec{win_val(y, trim)};

  arma::Mat<float> wcor{arma::cor(xvec, yvec)};
  float vwcor{static_cast<float>(wcor.at(0, 0))};

  arma::Mat<float> wcov{arma::cov(xvec, yvec)};
  float vwcov{static_cast<float>(wcov.at(0, 0))};

  return std::make_pair(vwcor, vwcov);
}

arma::Row<float> win_val(const arma::Row<float> &x, float trim) {

  arma::Row<float> y{arma::sort(x)};

  auto ibot = floor(trim * x.n_elem) + 1;
  auto itop = x.n_elem - ibot + 1;

  float xbot{y.at(ibot - 1)};
  float xtop{y.at(itop - 1)};

  return arma::clamp(x, xbot, xtop);
}

// TODO: this can be an extention to arma, something like I did for
// nlohmann::json I should basically put it into arma's namespace
float trim_mean(const arma::Row<float> &x, float trim) {
  arma::Row<float> y{arma::sort(x)};

  auto ibot = floor(trim * x.n_elem) + 1;
  auto itop = x.n_elem - ibot + 1;

  return arma::mean(y.subvec(ibot - 1, itop - 1));
}

float tie_correct(const arma::Col<float> &rankvals) {

  arma::Col<float> arr = arma::sort(rankvals);

  arma::uvec vindx = arma::join_cols(
      arma::uvec({1}), arr.tail(arr.n_elem - 1) != arr.head(arr.n_elem - 1));

  arma::uvec vvindx = arma::join_cols(vindx, arma::uvec({1}));

  arma::uvec indx = nonzeros_index(vvindx);

  arma::uvec cnt = arma::diff(indx);

  auto size = arr.n_elem;

  if (size < 2) {
    return 1.0;
  }

  return 1.0 - arma::accu(arma::pow(cnt, 3) - cnt) / (std::pow(size, 3) - size);
}

arma::Col<float> rankdata(const arma::Row<float> &arr,
                   const std::string method = "average") {

  // if method not in ('average', 'min', 'max', 'dense', 'ordinal'):
  //     raise ValueError('unknown method "{0}"'.format(method))

  // arr = np.ravel(np.asarray(a))
  // > This is always true, for now

  // algo = 'mergesort' if method == 'ordinal' else 'quicksort'
  arma::uvec sorter = arma::stable_sort_index(arr);

  // inv = np.empty(sorter.size, dtype=np.intp)
  // inv[sorter] = np.arange(sorter.size, dtype=np.intp)

  arma::uvec inv(sorter.n_elem);

  inv.elem(sorter) = arma::regspace<arma::uvec>(0, sorter.n_elem - 1);

  //        if (method == "ordinal")
  //            return inv + 1;

  arma::Col<float> arr_sorted(arr.n_elem);
  arr_sorted = arr(sorter);

  // obs = np.r_[True, arr[1:] != arr[:-1]]
  arma::uvec obs = arma::join_cols(arma::uvec({1}),
                                   arr_sorted.tail(arr_sorted.n_elem - 1) !=
                                       arr_sorted.head(arr_sorted.n_elem - 1));

  // dense = obs.cumsum()[inv]
  arma::uvec dense = arma::cumsum(obs); //.elem(inv);
  arma::uvec dense_sorted(dense.n_elem);
  dense_sorted = dense(inv);

  if (method == "dense")
    return arma::conv_to<arma::Col<float>>::from(dense_sorted);

  // cumulative counts of each unique value
  // count = [np.r_[np.nonzero(obs)[0], len(obs)]]
  arma::uvec count =
      arma::join_cols(nonzeros_index(obs), arma::uvec({obs.n_elem}));

  //        if (method == "max")
  //            return count(dense_sorted);
  //
  //        if (method == "min")
  //            return count(dense_sorted - 1) + 1;

  // average method
  return .5 * arma::conv_to<arma::Col<float>>::from(
                  (count(dense_sorted) + count(dense_sorted - 1) + 1));
}

} // namespace sam


/*
 * math.cpp
 *
 *  Created on: Apr 30, 2021
 *      Author: jelavice
 */

#include "icp_localization/common/math.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace icp_loco {

Eigen::Quaterniond fromRPY(const double roll, const double pitch, const double yaw)
{

  const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
  return yaw_angle * pitch_angle * roll_angle;
}

Eigen::Vector3d toRPY(const Eigen::Quaterniond &_q)
{
  Eigen::Quaterniond q(_q);
  q.normalize();
  const double r = getRollFromQuat(q.w(), q.x(), q.y(), q.z());
  const double p = getPitchFromQuat(q.w(), q.x(), q.y(), q.z());
  const double y = getYawFromQuat(q.w(), q.x(), q.y(), q.z());
  return Eigen::Vector3d(r,p,y);
}

Eigen::Quaterniond fromRPY(const Eigen::Vector3d &rpy){
	return fromRPY(rpy.x(), rpy.y(), rpy.z());
}

}  // namespace icp_loco


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, long>>>>>
    graph; // new! weightmap corresponds to costs
typedef boost::graph_traits<graph>::edge_descriptor edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator out_edge_it; // Iterator

using namespace std;

// Custom edge adder class
class edge_adder
{
    graph &G;

public:
    explicit edge_adder(graph &G) : G(G) {}
    void add_edge(int from, int to, long capacity, long cost)
    {
        auto c_map = boost::get(boost::edge_capacity, G);
        auto r_map = boost::get(boost::edge_reverse, G);
        auto w_map = boost::get(boost::edge_weight, G); // new!
        const edge_desc e = boost::add_edge(from, to, G).first;
        const edge_desc rev_e = boost::add_edge(to, from, G).first;
        c_map[e] = capacity;
        c_map[rev_e] = 0; // reverse edge has no capacity!
        r_map[e] = rev_e;
        r_map[rev_e] = e;
        w_map[e] = cost;      // new assign cost
        w_map[rev_e] = -cost; // new negative cost
    }
};

void solve() {
    int b, s, p;
    cin >> b >> s >> p;

    graph G(b + s);
    edge_adder adder(G);

    auto source = boost::add_vertex(G);
    auto target = boost::add_vertex(G);

    int maxValue = 50;

    for (int i = 0; i < b; ++i) {
      adder.add_edge(source, i, 1, 0);
      if (b <= s) {
        adder.add_edge(i, target, 1, maxValue);
      }
    }
    for (int j = 0; j < s; ++j) {
      adder.add_edge(b + j, target, 1, 0);
      if (b > s) {
        adder.add_edge(source, b + j, 1, maxValue);
      }
    }
    int bi, si, ci;
    for (int i = 0; i < p; ++i) {
      cin >> bi >> si >> ci;
      adder.add_edge(bi, b + si, 1, maxValue - ci);
    }

    boost::successive_shortest_path_nonnegative_weights(G, source, target);
    auto cost = boost::find_flow_cost(G);
    cost = min(b, s) * maxValue - cost;
    cout << cost << endl;
}

int main() {
    ios_base::sync_with_stdio(false);
    int t; cin >> t;
    for (int i = 0; i < t; ++i) {
        solve();
    }
    return 0;
}


#include <iostream>
#include <fstream>
#include <cmath>
#include <ctime>
#include <vector>
#include <string>

#include <sys/resource.h> /* give extra space to stack `rlimit` */

#include <Eigen/Eigen> /* Sparse Matrices */

#include <omp.h> /* Parallel */

using namespace std;

int get_grid_size(int b, int level, int c) {
    return (c+1) * pow(b, level) + 1;
}

void get_grid_layout(int b, int l, int level, int c, long** grid_layout, int grid_size) {
    // Checks if the parameters given are valid
    if ((l!=0 && b%2 != l%2) || (b==l)) {
        cout << "Invalid Input!" << endl;
    } else {
        // Fill Matrix Initially
        for (int y = 0; y<grid_size; y++) {
            for (int x = 0; x<grid_size; x++) {
                if (x%(c+1) == 0 && y%(c+1) == 0) {
                    grid_layout[y][x] = -1;
                } else {
                    grid_layout[y][x] = -2;
                }
            }
        }

        // Fills in the holes in the grid
        for (int current_level = 1; current_level<=level; current_level++) {
            int current_grid_size = get_grid_size(b, current_level, c);
            int prev_grid_size = get_grid_size(b, current_level-1, c);
            int hole_size = l*(prev_grid_size-1)-1;
            //#pragma omp parallel collapse(4)
            for (int j = (b-l)/2*(prev_grid_size-1)+1; j<grid_size; j+=(current_grid_size-1)) {
                for (int i = (b-l)/2*(prev_grid_size-1)+1; i<grid_size; i+=(current_grid_size-1)) {
                    for (int y = 0; y<hole_size; y++) {
                        for (int x = 0; x<hole_size; x++) {
                            grid_layout[j+y][i+x] = -1;
                        }
                    }
                }
            }
        }
    }
}

void display_grid_layout(long** grid_layout, int grid_size) {
    for (int y = 0; y<grid_size; y++) {
        for (int x = 0; x<grid_size; x++) {
            if (grid_layout[y][x] != -1) {
                cout <<  "\u2588 ";
            } else {
                cout << "  ";
            }

        }
        cout << endl;
    }
}

struct point {
    int y;
    int x;
};

void print_point(point p) {
    cout << "{" << p.y << ", " << p.x << "}";
}

// -3 is the offset (starts -3, -4, -5 and converts to 0, 1, 2, ...)
// Needs to be confirmed
void get_adj_list(long** grid_layout, int grid_size, int crosswires, vector< vector<long>> & adjacency_list, vector<point> & coordinates) {
    long next_available_index = -3;

    // Left Boundary Assignment
    for (int y = 0; y<grid_size; y++) {
        if (grid_layout[y][0] == -2) {
            grid_layout[y][0] = next_available_index;
            coordinates.push_back({y, 0});
            next_available_index--;
        }
    }

    // Right Boundary Assignment
    for (int y = 0; y<grid_size; y++) {
        if (grid_layout[y][grid_size-1] == -2) {
            grid_layout[y][grid_size-1] = next_available_index;
            coordinates.push_back({y, grid_size-1});
            next_available_index--;
        }
    }

    // Beginning of Flood Fill Algorithm
    vector<point> stack;
    stack.push_back({1, 0});

    while (stack.size() > 0) {

        // Fetch Last element in stack
        point p = stack[stack.size()-1];
        stack.pop_back();
        if (grid_layout[p.y][p.x] < 0) {
            // Marks as visited
            grid_layout[p.y][p.x] = -grid_layout[p.y][p.x]-3;
            vector<long> adj_row;

            // Checks Left
            if (p.x > 0 && p.y % (crosswires+1) != 0 && grid_layout[p.y][p.x-1] != -1) {
                if (grid_layout[p.y][p.x-1] >= 0) {
                    adj_row.push_back(grid_layout[p.y][p.x-1]);
                } else {
                    if (grid_layout[p.y][p.x-1] == -2) {
                        grid_layout[p.y][p.x-1] = next_available_index;
                        coordinates.push_back({p.y, p.x-1});
                        next_available_index--;
                    }
                    adj_row.push_back(-grid_layout[p.y][p.x-1]-3);
                    stack.push_back({p.y, p.x-1});
                }
            }

            // Checks Right
            if (p.x < grid_size-1 && p.y % (crosswires+1) != 0 && grid_layout[p.y][p.x+1] != -1) {
                if (grid_layout[p.y][p.x+1] >= 0) {
                    adj_row.push_back(grid_layout[p.y][p.x+1]);
                } else {
                    if (grid_layout[p.y][p.x+1] == -2) {
                        grid_layout[p.y][p.x+1] = next_available_index;
                        coordinates.push_back({p.y, p.x+1});
                        next_available_index--;
                    }
                    adj_row.push_back(-grid_layout[p.y][p.x+1]-3);
                    stack.push_back({p.y, p.x+1});
                }
            }

            // Checks Up
            if (p.y > 0 && p.x % (crosswires+1) != 0 && grid_layout[p.y-1][p.x] != -1) {
                if (grid_layout[p.y-1][p.x] >= 0) {
                    adj_row.push_back(grid_layout[p.y-1][p.x]);
                } else {
                    if (grid_layout[p.y-1][p.x] == -2) {
                        grid_layout[p.y-1][p.x] = next_available_index;
                        coordinates.push_back({p.y-1, p.x});
                        next_available_index--;
                    }
                    adj_row.push_back(-grid_layout[p.y-1][p.x]-3);
                    stack.push_back({p.y-1, p.x});
                }
            }

            // Checks Down
            if (p.y < grid_size-1 && p.x % (crosswires+1) != 0 && grid_layout[p.y+1][p.x] != -1) {
                if (grid_layout[p.y+1][p.x] >= 0) {
                    adj_row.push_back(grid_layout[p.y+1][p.x]);
                } else {
                    if (grid_layout[p.y+1][p.x] == -2) {
                        grid_layout[p.y+1][p.x] = next_available_index;
                        coordinates.push_back({p.y+1, p.x});
                        next_available_index--;
                    }
                    adj_row.push_back(-grid_layout[p.y+1][p.x]-3);
                    stack.push_back({p.y+1, p.x});
                }
            }

            while (adjacency_list.size() <= grid_layout[p.y][p.x]) {
                vector<long> blank_row;
                adjacency_list.push_back(blank_row);
            }
            adjacency_list[grid_layout[p.y][p.x]] = adj_row;
        }
    }
}

void get_laplacian(Eigen::SparseMatrix<short> & laplacian, vector< vector<long>> & adjacency_list) {
    for (int i = 0; i<adjacency_list.size(); i++) {
        vector<long> row = adjacency_list[i];
        for (int j = 0; j<row.size(); j++) {
            laplacian.coeffRef(i, row[j]) = 1;
        }
        laplacian.coeffRef(i, i) = -row.size();
    }
}

void print_laplacian(Eigen::SparseMatrix<short> & laplacian) {
    cout << "Laplacian Matrix: " << endl;
    long num_coordinates = laplacian.cols();
    for (int i = 0; i<num_coordinates; i++) {
        for (int j = 0; j<num_coordinates; j++) {
            cout << laplacian.coeff(i, j) << " ";
        }
        cout << endl;
    }
}

void print_mtrx(Eigen::SparseMatrix<double> & mtrx) {
    cout << "Matrix: " << endl;
    long num_coordinates = mtrx.cols();
    for (int i = 0; i<mtrx.rows(); i++) {
        for (int j = 0; j<mtrx.cols(); j++) {
            cout << mtrx.coeff(i, j) << " ";
        }
        cout << endl;
    }
}

template<typename Derived>
void find_potentials(Eigen::MatrixBase<Derived>& potentials, vector< vector<long>> & adjacency_list) {
    /*struct rlimit lim;
    getrlimit(RLIMIT_STACK, &lim);
    cout << lim.rlim_cur << endl;*/

    long num_coordinates = adjacency_list.size();
    int num_computed_points = potentials.rows();
    int num_boundary_points = num_coordinates-num_computed_points;
    cout << "Total Boundary Points: " << num_boundary_points << endl;
    cout << "Computed Points: " << num_computed_points << endl;

    cout << "1) Initailizing Matrices ..." << endl;
    Eigen::SparseMatrix<double> r(num_computed_points, num_boundary_points);
    Eigen::SparseMatrix<double> a(num_computed_points, num_computed_points);

    cout << "2) Reserving Space ..." << endl;
    r.reserve(Eigen::VectorXi::Constant(num_boundary_points, 5));
    a.reserve(Eigen::VectorXi::Constant(num_computed_points, 5));

    cout << "3) Inserting Elements ..." << endl;
    for (int i = 0; i<adjacency_list.size(); i++) {
        if (i >= num_boundary_points) {
            vector<long> row = adjacency_list[i];

            // Adds elements not on diagonal
            for (int j = 0; j<row.size(); j++) {
                if (row[j] < num_boundary_points) {
                    r.insert(i-num_boundary_points, row[j]) = 1;
                } else {
                    a.insert(i-num_boundary_points, row[j]-num_boundary_points) = 1;
                }
            }

            // Add elements on the diagonal
            if (i < num_boundary_points) {
                r.insert(i-num_boundary_points, i) = -(double)row.size();
            } else {
                a.insert(i-num_boundary_points, i-num_boundary_points) = -(double)row.size();
            }
        }
    }
    //print_mtrx(a);
    //print_mtrx(r);

    cout << "4) Setting Boundary Conditions ..." << endl;
    Eigen::SparseMatrix<double> dirichlet(num_boundary_points, 1);
    for (int i = 0; i<num_boundary_points; i++) {
        if (i < num_boundary_points / 2) {
            dirichlet.coeffRef(i, 0) = 0;
        } else {
            dirichlet.coeffRef(i, 0) = 1;
        }
    }

    cout << "5) Solving ..." << endl;
    Eigen::VectorXd b = -r*dirichlet;
    //Eigen::SparseLU<Eigen::SparseMatrix<double> > solver;
    //solver.analyzePattern(a);
    //solver.factorize(a);
    //potentials = solver.solve(b);

    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::Lower|Eigen::Upper> cg;

    cg.compute(a);
    cg.setMaxIterations(1);
    potentials = cg.solve(b);

    //Eigen::Matrix<double> potentials(2, 1);

    for (int i = 0; i<500; i++) {
        potentials = cg.solveWithGuess(b, potentials);
        cout << 1*(i+1) << endl;
        cout << cg.error() << endl;
    }

    //print_mtrx(potentials);
}

// TO BE CLEANED UP
template<typename Derived>
double max_difference(vector< vector<long>> & adjacency_list, Eigen::MatrixBase<Derived>& potentials) {
    long num_coordinates = adjacency_list.size();
    int num_computed_points = potentials.rows();
    int num_boundary_points = num_coordinates-num_computed_points;

    double max_diff = 0;

    for (int i = 0; i<adjacency_list.size(); i++) {
        // figure out potential
        double potential_a = 1;
        if (i < num_boundary_points/2) {
            potential_a = 0;
        } else if (i >= num_boundary_points) {
            potential_a = potentials.coeff(i-num_boundary_points, 0);
        }

        vector<long> row = adjacency_list[i];
        for (int j = 0; j<row.size(); j++) {
            long index = row[j];

            // figure out potential
            double potential_b = 1;
            if (index < num_boundary_points/2) {
                potential_b = 0;
            } else if (index >= num_boundary_points) {
                potential_b = potentials.coeff(index-num_boundary_points, 0);
            }

            double diff = abs(potential_a-potential_b);

            if (diff-max_diff > 0.0000001) {
                //cout << diff << endl;
                max_diff = diff;
            }
        }
    }

    return max_diff;
}
// THIS SECTION ABOVE

int harmonic_function(int b, int l, int level, int crosswires, string filename) {
    struct rlimit lim;
    getrlimit(RLIMIT_STACK, &lim);
    cout << lim.rlim_cur << endl;

    const rlimit stack_size = {10*1024*1024, 10*1024*1024};
    if (setrlimit(RLIMIT_STACK, &stack_size) == -1) {
        return 1;
    }
    // Parameters
    /*const int b = 3;
    const int l = 1;
    const int level = 1;
    const int crosswires = 1;*/

    // Making Grid Layout
    cout << "Making Grid Layout ..." << endl;
    int grid_size = get_grid_size(b, level, crosswires);
    cout << "Grid Size: " << grid_size << endl;
    long ** grid_layout = new long*[grid_size];
    for (int i = 0; i<grid_size; i++) {
        grid_layout[i] = new long[grid_size];
    }
    get_grid_layout(b, l, level, crosswires, grid_layout, grid_size);

    // View Grid Layout in Terminal
    //display_grid_layout(grid_layout, grid_size);

    // Adjacency List Calculation
    cout << "Making Adjacency List ..." << endl;
    vector< vector<long>> adjacency_list;
    vector<point> coordinates;
    get_adj_list(grid_layout, grid_size, crosswires, adjacency_list, coordinates);

    // FREE GRID LAYOUT
    for(int i =0 ; i<grid_size; i++) {
        delete[] grid_layout[i];
    }
    delete[] grid_layout;

    // Computes Laplacian
    long num_coordinates = adjacency_list.size();
    cout << "num_coordinates: " << num_coordinates << endl;
    //Eigen::SparseMatrix<short> laplacian(num_coordinates, num_coordinates);
    //get_laplacian(laplacian, adjacency_list);

    // Computes Potentials
    cout << "Compute Potentials ..." << endl;
    int num_boundary_points = 2*crosswires*pow(b,level);
    int num_computed_points = num_coordinates-num_boundary_points;
    Eigen::VectorXd potentials(num_computed_points);
    find_potentials(potentials, adjacency_list);

    // To be fixed up
    double max_diff = max_difference(adjacency_list, potentials);
    cout << max_diff << endl;

    // Below is the Printing code (needs to)
    // Output to file
    /*ofstream fout(filename);

    time_t now = time(0);
    string dt = ctime(&now);
    fout << "Produced by \"The Resistance\", " << dt;
    fout << "---------------------------------------------------------" << endl;
    fout << "Parameters: b = " << b << ", l = " << l << ", level = " << level << ", crosswires = " << crosswires << endl;
    fout << "---------------------------------------------------------" << endl;
    fout << "y\tx\tpotential" << endl;
    for (int i = 0; i<num_coordinates; i++) {
        if (i < num_boundary_points) {
            if (i < num_boundary_points/2) {
                fout << coordinates[i].y << "\t" << coordinates[i].x << "\t" << 0 << endl;
            } else {
                fout << coordinates[i].y << "\t" << coordinates[i].x << "\t" << 1 << endl;
            }
        } else {
            fout << coordinates[i].y << "\t" << coordinates[i].x << "\t" << potentials.coeff(i-num_boundary_points, 0) << endl;
        }
    }
    fout.close();*/
    return 0;
}

int main() {
    int b = 3;
    int l = 1;
    int level = 8;
    int crosswires = 1;
    omp_set_num_threads(4);
    /*for (int level = 0; level<8; level++) {
        cout << "----------------------\n";
        string filename = "../data/plus/plusdata_"+to_string(b)+"_"+to_string(l)+"_"+to_string(crosswires)+"_level"+to_string(level)+".dat";
        //cout << filename;
        harmonic_function(b, l, level, crosswires, filename);
    }*/

    harmonic_function(b, l, level, crosswires, "asdf");

}


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>

#include <boost/range/adaptor/reversed.hpp>
#include <boost/math/constants/constants.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

namespace {
    const long double epsilon = 1e-9L;

    struct comparedouble {
        bool operator()(const long double &lhs, const long double &rhs) const {
            return std::abs(lhs - rhs) < epsilon ? false : lhs < rhs;
        }
    };
}

ENREGISTRER_PROBLEME(177, "Integer angled Quadrilaterals") {
    // Let ABCD be a convex quadrilateral, with diagonals AC and BD. At each vertex the diagonal makes an angle with
    // each of the two sides, creating eight corner angles.
    //
    // For example, at vertex A, the two angles are CAD, CAB.
    //
    // We call such a quadrilateral for which all eight corner angles have integer values when measured in degrees an
    // "integer angled quadrilateral". An example of an integer angled quadrilateral is a square, where all eight corner
    // angles are 45°. Another example is given by DAC = 20°, BAC = 60°, ABD = 50°, CBD = 30°, BCA = 40°, DCA = 30°,
    // CDB = 80°, ADB = 50°.
    //
    // What is the total number of non-similar integer angled quadrilaterals?
    //
    // Note: In your calculations you may assume that a calculated angle is integral if it is within a tolerance of 10-9
    // of an integer value.
    std::vector<long double> sinus(180, 0.0L);
    std::vector<long double> cosinus(180, 0.0L);
    std::vector<long double> rad(180, 0.0L);
    std::vector<bool> prevalue(10000 + 1, false);

    std::vector<nombre> solution(8 + 1, 0);

    std::set<long double, comparedouble> fs;
    for (nombre i = 1; i < 180; i++) {
        rad[i] = static_cast<long double>(i) * M_PIl / 180.0L;
        sinus[i] = std::sin(rad[i]);
        cosinus[i] = std::cos(rad[i]);
        auto j = static_cast<nombre>(sinus[i] * 10000);
        prevalue[j] = true;
        fs.insert(sinus[i]);
    }
    prevalue[10000] = true; // sin90
    prevalue[10000 - 1] = true; // sin90
    prevalue[5000] = true; // sin30
    prevalue[5000 - 1] = true; //sin30

    for (nombre a = 1; a <= 45; a++)
        for (nombre b = a; a + b < 180 - 1; b++)
            for (nombre c = 1; a + b + c < 180; c++) {
                nombre d = 180 - a - b - c;
                for (nombre e = 1; e < b + d; e++) {
                    nombre f = b + d - e;
                    long double m = (sinus[b] * sinus[c] * sinus[f]) / (sinus[a] * sinus[e] * sinus[d]);
                    long double n = cosinus[a + c];
                    long double siny = std::sqrt((1 - n * n) / (m * m + 2 * m * n + 1));
                    auto j = static_cast<nombre>(siny * 10000);
                    if (!prevalue[j]) continue;
                    if (fs.find(siny) != fs.end()) {
                        if (siny > 1) siny = 1;

                        auto y = static_cast<nombre>(std::asin(siny) * 180 / M_PIl + 0.01L);
                        long double sinx = m * siny;
                        long double xangle = (std::abs(sinx - 1) < epsilon) ? 90 : std::asin(sinx) * 180 / M_PIl;
                        auto x = static_cast<nombre>(xangle + 0.01L);
                        if (xangle < x + epsilon && xangle > x - epsilon) {
                            x = (180 - x + y == a + c) ? 180 - x : x;
                            y = (180 + x - y == a + c) ? 180 - y : y;

                            std::vector<vecteur> quadrilateres
                                    {
                                            {a, b, d, y, x, f, e, c},
                                            {d, y, x, f, e, c, a, b},
                                            {x, f, e, c, a, b, d, y},
                                            {e, c, a, b, d, y, x, f},
                                            {b, a, c, e, f, x, y, d},
                                            {c, e, f, x, y, d, b, a},
                                            {f, x, y, d, b, a, c, e},
                                            {y, d, b, a, c, e, f, x}
                                    };

                            std::set<vecteur> s;
                            for (auto quadrilatere: quadrilateres) {
                                if (quadrilatere.front() <= quadrilatere.at(1) && quadrilatere.front() <= 45)
                                    s.insert(quadrilatere);
                            }

                            solution[s.size()]++;
                        }
                    }
                }
            }

    nombre resultat = 0;
    for (nombre i = 1; i <= 8; i++)
        resultat += solution[i] / i;

    return std::to_string(resultat);
}


//---------------------------------------------------------------------------

#pragma hdrstop

#include <System.SysUtils.hpp>

#include <random>
#include <future>
#include <vector>
#include <numeric>
#include <iterator>
#include <utility>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include "IModel.h"

#include "StochasticMTAreaCalc.h"

using std::mt19937;
using std::uniform_real_distribution;
using std::accumulate;
using std::begin;
using std::end;

using boost::geometry::envelope;

using boost::geometry::model::box;
using boost::geometry::area;

//---------------------------------------------------------------------------

#pragma package(smart_init)

namespace AreaPrj {

String StochasticMTAreaCalc::DoGetDescription() const
{
    return Format(
        _D( "\'%s\' con %.0n di punti casuali suddiviso in %u task" ),
        ARRAYOFCONST((
            GetName(),
            static_cast<long double>( pointCount_ ),
            taskCount_
        ))
    );
}

double StochasticMTAreaCalc::DoCompute( IModel const & Model ) const
{
    box<IModel::PointType> BoundingBox;
    auto const & Polygons = Model.GetPolygons();
    envelope( Polygons, BoundingBox );
    std::vector<std::future<size_t>> Tasks;
    for ( size_t n = 0 ; n < taskCount_ ; ++n ) {
        Tasks.push_back(
            std::async(
                std::launch::async,
                [&]( std::random_device::result_type seed, size_t PtCnt ) -> size_t {
                    auto const & Polygons = Model.GetPolygons();
                    mt19937 Generator( seed );
                    uniform_real_distribution<> DisX(
                        BoundingBox.min_corner().x(),
                        BoundingBox.max_corner().x()
                    );

                    uniform_real_distribution<> DisY(
                        BoundingBox.min_corner().y(),
                        BoundingBox.max_corner().y()
                    );

                    size_t HitCnt {};

                    for ( size_t n = 0 ; n < PtCnt ; ++n ) {
                        if ( Model.HitTest( DisX( Generator ), DisY( Generator ) ) ) {
                            ++HitCnt;
                        }
                    }
                    return HitCnt;
                },
                rd_(),
                pointCount_ / taskCount_
            )
        );
    }
    return
        static_cast<double>(
            accumulate<decltype( begin( Tasks ) ), size_t>(
                begin( Tasks ), end( Tasks ), {},
                []( size_t Val, auto& fut ) {
                    return Val + fut.get();
                }
            )
        ) /
        pointCount_ * area( BoundingBox );
}

} // End of namespace AreaPrj


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * 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.
 */

///
/// \file self_adjoint_coneigensolver.hpp
///
#ifndef MXPFIT_SELF_ADJOINT_CONEIGENSOLVER_HPP
#define MXPFIT_SELF_ADJOINT_CONEIGENSOLVER_HPP

#include <cassert>
#include <type_traits>

#include <Eigen/Core>
#include <Eigen/QR>

#include <mxpfit/jacobi_svd.hpp>

namespace mxpfit
{
///
/// ### SelfAdjointConeigenSolver
///
/// \brief Compute con-eigenvalue decomposition of self-adjoint matrix having a
///        rank-revealing decomposition
///
/// \tparam T  Scalar type of matrix to be decomposed.
///
/// Let \f$A\f$ be an \f$n \times n\f$ self-adjoint matrix having a rank
/// revealing decomposition of the form
///
/// \f[ A = X D^2 X^{\ast},\f]
///
/// where \f$X\f$ is a \f$n \times k \, (n \geq k)\f$ matrix and \f$D\f$ is a
/// \f$k \times k\f$ diagonal matrix with non-negative entries. This class
/// computes a con-eigenvalue decomposition of matrix \f$A\f$ defined as
///
/// \f[
///   A = \overline{U} \Sigma U^{\ast}
/// \f]
///
/// where \f$U\f$ is a \f$n \times k\f$ matrix satisfying \f$U^{-1}=U^{T},\f$
/// and overline denotes the element-wise complex conjugate of a matrix.
/// \f$\Sigma=\mathrm{diag}(\sigma_{1},\dots,\sigma_{k})\f$ is a \f$k \times
/// k\f$ diagonal matrix with \f$\sigma_{1}\geq\sigma_{2}\geq \cdots \geq
/// \sigma_{k} > 0.\f$ The con-eigenvalue decomposition defined above is a
/// special case of singular value decomposition (SVD) \f$A=W\Sigma V^{\ast}\f$,
/// where \f$ W=\overline{U} \f$ and \f$V=U.\f$
///
/// The con-eigenvalue decomposition is computed in high-relative accuracy using
/// the algorithm developed by Haut and Beylkin. The algorithm can also be
/// regarded as modification of Demmel's algorithm for high accuracy SVD of a
/// matrix with rank-revealing decomposition. See the references listed below
/// for the details.
///
///
/// #### References
///
/// 1. T. S. Haut and G. Beylkin, "FAST AND ACCURATE CON-EIGENVALUE ALGORITHM
///    FOR OPTIMAL RATIONAL APPROXIMATIONS", SIAM J. Matrix Anal. Appl. **33**
///    (2012) 1101-1125. [DOI: https://doi.org/10.1137/110821901]
/// 2. J. Demmel, "ACCURATE SINGULAR VALUE DECOMPOSITIONS OF STRUCTURED
///    MATRICES", SIAM J. Matrix Anal. Appl. **21** (1999) 562-580.
///    [DOI: https://doi.org/10.1137/S0895479897328716]
/// 3. J. Demmel, M. Gu, S. Eisenstat, I. Slapnicar, K. Veselic, and Z. Drmac,
///    "ACCURATE SINGULAR VALUE DECOMPOSITIONS OF STRUCTURED MATRICES", SIAM J.
///    Linear Algebra Appl. **299** (1999) 21-80. [DOI:
///    https://doi.org/10.1016/S0024-3795(99)00134-2]
///

enum DecompositionOption
{
    ConeigenvaluesOnly,
    ComputeConeigenvectors
};

template <typename T>
class SelfAdjointConeigenSolver
{
public:
    using Index         = Eigen::Index;
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<Scalar>::Real;
    using ComplexScalar = std::complex<RealScalar>;

    using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVectorType = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;

    using ConeigenvalueType = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;

protected:
    enum
    {
        IsComplex  = Eigen::NumTraits<Scalar>::IsComplex,
        PacketSize = Eigen::internal::packet_traits<Scalar>::size,
        Alignment  = Eigen::internal::traits<MatrixType>::Alignment,
        // Alignment  = Eigen::internal::unpacket_traits<PacketType>::alignment
    };

    using MappedMatrix = Eigen::Map<MatrixType, Alignment>;
    using PacketType   = typename Eigen::internal::packet_traits<Scalar>::type;

    MatrixType m_ceigvecs;     // m x n (m >= n)
    RealVectorType m_ceigvals; // n
    MatrixType m_mat_work1;    // n x n
    MatrixType m_mat_work2;    // n x n

public:
    SelfAdjointConeigenSolver()                                 = default;
    SelfAdjointConeigenSolver(const SelfAdjointConeigenSolver&) = default;

    explicit SelfAdjointConeigenSolver(Index size, Index rank)
        : m_ceigvecs(size, rank),
          m_ceigvals(rank),
          m_mat_work1(rank, rank),
          m_mat_work2(rank, rank)
    {
        assert(size >= rank);
    }

    ~SelfAdjointConeigenSolver() = default;

    template <typename InputMatrix, typename InputVector>
    void compute(const Eigen::MatrixBase<InputMatrix>& matX,
                 const Eigen::MatrixBase<InputVector>& vecD,
                 DecompositionOption option = ComputeConeigenvectors);

    const MatrixType& coneigenvectors() const
    {
        return m_ceigvecs;
    }

    const RealVectorType& coneigenvalues() const
    {
        return m_ceigvals;
    }

protected:
    void resize(Index m, Index n)
    {
        m_ceigvecs.resize(m, n);
        m_ceigvals.resize(n);
        m_mat_work1.resize(n, n);
        m_mat_work2.resize(n, n);
    }
};

template <typename T>
template <typename InputMatrix, typename InputVector>
void SelfAdjointConeigenSolver<T>::compute(
    const Eigen::MatrixBase<InputMatrix>& matX,
    const Eigen::MatrixBase<InputVector>& vecD, DecompositionOption option)
{
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(InputVector);

    assert(matX.rows() >= matX.cols());
    assert(vecD.size() == matX.cols());

    const Index m = matX.rows();
    const Index n = matX.cols();
    resize(m, n);

    MappedMatrix matG(m_ceigvecs.data(), n, n);

    //
    // Form G = D * (X.st() * X) * D
    //
    matG.noalias() = matX.transpose() * matX;
    for (Index j = 0; j < n; ++j)
    {
        for (Index i = 0; i < n; ++i)
        {
            matG(i, j) *= vecD(i) * vecD(j);
        }
    }
    //
    // Compute G = Q * R by Householder QR factorization. G is overwritten by QR
    // factors.
    //
    Eigen::HouseholderQR<Eigen::Ref<MatrixType>> qr(matG);

    //
    // Compute SVD of `R = U S V^H` with high relative accuracy using the
    // one-sided Jacobi SVD algorithm. Only the singular values and left
    // singular vectors U are computed. The obtained singular values coincide
    // with coneigenvalues of input matrix
    //
    // Applying the one-sided Jacobi SVD to the matrix X = R^H is much faster
    // than applying the algorithm to R directly. This is because `R R^H` is
    // more diagonal than `R^H R`. Thus, `U` is computed as right singular
    // vectors of R^H.
    //
    MatrixType& matRt = m_mat_work1;
    matRt.setZero();
    matRt            = matG.template triangularView<Eigen::Upper>().adjoint();
    MatrixType& matU = m_mat_work2;
    const RealScalar tol_svd = Eigen::NumTraits<RealScalar>::epsilon() *
                               Eigen::numext::sqrt(RealScalar(n));
    one_sided_jacobi_svd(matRt, m_ceigvals, matU, tol_svd);

    //
    // Quick return if no con-eigenvector is required.
    //
    if (option == ConeigenvaluesOnly)
    {
        return;
    }

    //-------------------------------------------------------------------------
    //
    // The eigenvectors of A * conj(A) are given as
    //
    //   conj(X') = X * D * V * S^{-1/2}.
    //
    // However, direct evaluation of eigenvectors with this formula might be
    // inaccurate since D is ill-conditioned. The following formula are used
    // instead.
    //
    //   conj(X') = X * D * R.inv() * U * S^{1/2}
    //            = X * (D.inv() * R * D.inv()).inv() * (D.inv() * U *
    //            S^{1/2})
    //            = X * R1.inv() * X1
    //
    //-------------------------------------------------------------------------
    // Matrix R is stored on upper triangular part of G
    auto matR1 = matG.template triangularView<Eigen::Upper>();
    //
    // Compute R1 = D^(-1) * R * D^(-1). The upper triangular part of `matG` is
    // overwritten by R1.
    //
    for (Index j = 0; j < n; ++j)
    {
        const auto dj = vecD(j);
        for (Index i = 0; i <= j; ++i)
        {
            const auto di = vecD(i);
            matR1(i, j)   = matR1(i, j) / (di * dj);
        }
    }
    //
    // Compute X1 = D^{-1} * U * S^{1/2}, and then solve R1 * Y1 = X1 in-place.
    // Matrix `matU` is overwritten by X1 and then overwritten by Y1.
    //
    MatrixType& matY1 = matU;
    for (Index j = 0; j < n; ++j)
    {
        const auto sj = Eigen::numext::sqrt(m_ceigvals(j));
        for (Index i = 0; i < n; ++i)
        {
            matY1(i, j) *= sj / vecD(i);
        }
    }

    matR1.solveInPlace(matY1);
    //
    // Compute con-eigenvectors U = conj(X) * conj(Y).
    //
    m_ceigvecs.noalias() = matX.conjugate() * matY1.conjugate();

    if (IsComplex)
    {
        //
        // Adjust phase factor of each con-eigenvectors, so that U^{T} * U = I
        //
        for (Index j = 0; j < n; ++j)
        {
            auto xj          = m_ceigvecs.col(j);
            const auto t     = (xj.transpose() * xj).value();
            const auto phase = t / std::abs(t);
            const auto scale = std::sqrt(Eigen::numext::conj(phase));
            xj *= scale;
        }
    }
}

} // namespace: mxpfit

#endif /* MXPFIT_SELF_ADJOINT_CONEIGENSOLVER_HPP */