//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::example::random2.cpp                               //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#include <boost/typeof/typeof.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/poisson/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/include.hpp>
#include <boost/statistics/detail/distribution_common/distributions/reference/include.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/generator.hpp>
#include <boost/statistics/detail/non_parametric/kolmogorov_smirnov/check_convergence.hpp>

#include <libs/statistics/detail/distribution_toolkit/example/random.h>

void example_random(std::ostream& os)
{
    // Checks convergence of sample generated by make_random_generator(urng,d)
    // to distribution b, by the kolmogorov-smirnov

    os << "-> test_random2 " << std::endl;

	namespace ks = boost::statistics::detail::kolmogorov_smirnov;

    using namespace boost;
    using namespace boost::statistics;
    namespace stat = statistics::detail;
	namespace dist = stat::distribution;
	typedef int int_;
    typedef double val_;
	typedef boost::mt19937 urng_;
	typedef ks::check_convergence<val_> check_;

    const unsigned n_loops = 6;
    const unsigned n1 = 1e1;
    const unsigned n2 = 1e1;

    urng_ urng;
    check_ check;

    {
        typedef math::chi_squared_distribution<val_> dist_;
        const val_ df = 10;
        dist_ d( df );
        os << d << std::endl;
        BOOST_AUTO(
        	vg,
            dist::make_random_generator(urng,d)
        );
        check(n_loops,n1,n2,d,vg,os);
    }
    {
        typedef math::exponential_distribution<val_>     dist_;
        const val_ lambda = 1.0;
        dist_ d( lambda );
        os << d << std::endl;
        BOOST_AUTO(
        	vg,
            dist::make_random_generator(urng,d)
        );
        check(n_loops,n1,n2,d,vg,os);
    }
    {
        typedef math::gamma_distribution<val_>            dist_;
        const val_ shape = 1.0;
        const val_ scale = 0.5;
        dist_ d( shape, scale );
        dist_ d1( shape + 0.01, scale );
        os << d << std::endl;

        BOOST_AUTO(
        	vg,
            dist::make_random_generator(urng,d)
        );
        check(n_loops,n1,n2,d1,vg,os);
    }
    {
        typedef math::normal_distribution<val_>            dist_;
        const val_ m = 1.0;
        const val_ s = 2.0;
        dist_ d( m, s );
        os << d << std::endl;

        BOOST_AUTO(
        	vg,
            dist::make_random_generator(urng,d)
        );
        check(n_loops,n1,n2,d,vg,os);

    }
    {
        typedef boost::math::poisson_distribution<val_>            dist_;
        const val_ m = 10.0;
        dist_ d( m );
        os << d << std::endl;

        BOOST_AUTO(
        	vg,
            dist::make_random_generator(urng,d)
        );
        check(n_loops,n1,n2,d,vg,os);

    }
    {
        typedef math::students_t_distribution<val_>        dist_;
        const val_ df = 4.0;
        dist_ d( df );
        os << d << std::endl;

        BOOST_AUTO(
        	vg,
            dist::make_random_generator(urng,d)
        );
        check(n_loops,n1,n2,d,vg,os);
    }
    {
        typedef math::students_t_distribution<val_> dist_z_;
        typedef dist::toolkit::location_scale_distribution<dist_z_> dist_;
        const val_ df = 4.0;
        const val_ m = 1.0;
        const val_ s = 2.0;
        dist_z_ dist_z( df );
        dist_ d(m, s, dist_z);
        os << d << std::endl;

        BOOST_AUTO(
        	vg,
            dist::make_random_generator(urng,d)
        );
        check(n_loops,n1,n2,d,vg,os);
    }
    {
        typedef math::students_t_distribution<val_>         d0_;
        typedef dist::reference_wrapper<d0_>                dist_;
        const val_ df = 4.0;
        d0_ d0( df );
        dist_ d(d0);
        os << d << std::endl;

        BOOST_AUTO(
        	vg,
            dist::make_random_generator(urng,d)
        );
        check(n_loops,n1,n2,d,vg,os);
    }

    os << "<-" << std::endl;
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_COMPUTEMASSES_HPP
#define MCL_COMPUTEMASSES_HPP 1

#include <Eigen/Dense>

namespace mcl
{

// V are vertices (n x 2 or 3)
// P are primitives (m x 3 or 4)
// M is n x 1 of per-vertex masses
// Computes volume (or area) weighted masses with unit-volume density.
// If negative, defaults are used: 1100 for volumetric, 0.4 for cloth/2D
// See: https://www.engineeringtoolbox.com/density-solids-d_1265.html.
// Masses between the min and max indices in P are set.
// If unreferenced, they are set to zero.
// Returns true if all masses in span are positive.
template <typename DerivedV, typename DerivedP, typename Scalar>
static inline bool compute_masses(
	const Eigen::MatrixBase<DerivedV> &V,
	const Eigen::MatrixBase<DerivedP> &P,
	Eigen::Matrix<Scalar,Eigen::Dynamic,1> &M,
	double density_kgd = -1)
{
	using namespace Eigen;
	int V_dim = V.cols();
	int P_dim = P.cols();
	if (V_dim < 2 || V_dim > 3) { return false; }
	if (P_dim < 3 || P_dim > 4) { return false; }

	// Use 3D vec for calculation even if 2D
	auto Vi = [&](int idx)
	{
		Vector3d vi = Vector3d::Zero();
		vi.head(V_dim) = V.row(idx).template cast<double>();
		return vi;
	};

	// Default densities
	if (density_kgd < 0)
	{
		if (V_dim == 2 || P_dim == 3) {
			density_kgd = 0.4;
		}
		else if (V_dim == 3 && P_dim == 4) {
			density_kgd = 1100;
		}
	}

	// Resize masses if needed and set span to zero
	int min_Pi = P.minCoeff();
	int max_Pi = P.maxCoeff();
	if (M.rows() < max_Pi) {
		M.conservativeResize(max_Pi+1);
	}
	M.segment(min_Pi, max_Pi-min_Pi+1).array() = 0;

	// Compute mass contrib from each element
	int np = P.rows();
	for (int i=0; i<np; ++i)
	{
		if (P_dim == 4)
		{
			Vector3d p_verts[4] = {
				Vi(P(i,0)),
				Vi(P(i,1)),
				Vi(P(i,2)),
				Vi(P(i,3)) };
			Matrix<double,3,3> E;
			E.col(0) = p_verts[1] - p_verts[0];
			E.col(1) = p_verts[2] - p_verts[0];
			E.col(2) = p_verts[3] - p_verts[0];
			double vol = std::abs(E.determinant()/6.0);
			double tet_mass = density_kgd * vol;
			M[P(i,0)] += Scalar(tet_mass / 4.0);
			M[P(i,1)] += Scalar(tet_mass / 4.0);
			M[P(i,2)] += Scalar(tet_mass / 4.0);
			M[P(i,3)] += Scalar(tet_mass / 4.0);
		}
		else if (P_dim == 3)
		{
			Vector3d p_verts[3] = {
				Vi(P(i,0)),
				Vi(P(i,1)),
				Vi(P(i,2)) };
			Vector3d e0 = p_verts[1] - p_verts[0];
			Vector3d e1 = p_verts[2] - p_verts[0];
			double area = 0.5 * (e0.cross(e1)).norm();
			double tri_mass = density_kgd * area;
			M[P(i,0)] += Scalar(tri_mass / 3.0);
			M[P(i,1)] += Scalar(tri_mass / 3.0);
			M[P(i,2)] += Scalar(tri_mass / 3.0);
		}
	}

	double min_mass = M.segment(min_Pi, max_Pi-min_Pi+1).minCoeff();
	return min_mass > 0;
}

} // ns mcl

#endif


// -----------------------------------------------------------------------
// 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.
//
//------------------------------------------------------------------------

// this file defines the rttbCoreTests for the test driver
// and all it expects is that you have a function called RegisterTests

#include <boost/shared_ptr.hpp>

#include "litCheckMacros.h"
#include "rttbBioModel.h"
#include "rttbDVH.h"
#include "rttbTCPLQModel.h"
#include "rttbNTCPLKBModel.h"
#include "rttbNTCPRSModel.h"
#include "rttbBioModelScatterPlots.h"
#include "rttbBioModelCurve.h"
#include "rttbDvhBasedModels.h"
#include "../models/rttbScatterTester.h"
#include "rttbInvalidParameterException.h"
#include "rttbDVHXMLFileReader.h"

namespace rttb
{
	namespace testing
	{

		/*! @brief RTBioModelScatterPlotExampleTest.
		calculating Curves and Scatterplots for TCP and NTCP models.
		The values on curve and scatterplot need to be similar for similar dose values.
		The range of difference is given by the variance used to generate the scatter.

		WARNING: The values for comparison need to be adjusted if the input files are changed!
		*/
		int RTBioModelScatterPlotExampleTest(int argc, char* argv[])
		{
			PREPARE_DEFAULT_TEST_REPORTING;

			typedef rttb::models::CurveDataType CurveDataType;
			typedef rttb::models::ScatterPlotType ScatterPlotType;
			typedef core::DVH::Pointer DVHPointer;

			//increased accuracy requires double values in the calculation (rttbBaseType.h)
			double toleranceEUD = 1e-5;

			//ARGUMENTS: 1: ptv dvh file name
			//           2: normal tissue 1 dvh file name
			//           3: TV dvh file name

			std::string DVH_FILENAME_PTV;
			std::string DVH_FILENAME_NT1;
			std::string DVH_FILENAME_TV_TEST;

			if (argc > 1)
			{
				DVH_FILENAME_PTV = argv[1];
			}

			if (argc > 2)
			{
				DVH_FILENAME_NT1 = argv[2];
			}

			if (argc > 3)
			{
				DVH_FILENAME_TV_TEST = argv[3];
			}

			//DVH PTV
			rttb::io::other::DVHXMLFileReader dvhReader = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_PTV);
			DVHPointer dvhPtr = dvhReader.generateDVH();

			rttb::io::other::DVHXMLFileReader dvhReader_test_tv = rttb::io::other::DVHXMLFileReader(
			            DVH_FILENAME_TV_TEST);
			DVHPointer dvh_test_tv = dvhReader_test_tv.generateDVH();

			//test TCP LQ Model
			models::BioModelParamType alpha = 0.35;
			models::BioModelParamType beta = 0.023333333333333;
			models::BioModelParamType roh = 10000000;
			int numFractions = 2;

			DoseTypeGy normalizationDose = 68;

			rttb::models::TCPLQModel tcplq = rttb::models::TCPLQModel(dvhPtr, alpha, beta, roh, numFractions);

			CHECK_NO_THROW(tcplq.init());

			CurveDataType curve = models::getCurveDoseVSBioModel(tcplq, normalizationDose);

			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(tcplq, 0, alpha, 0, normalizationDose, 100));
			ScatterPlotType tcpScatter = models::getScatterPlotVary1Parameter(tcplq, 0, alpha, 0,
			                             normalizationDose, 100);
			CHECK_EQUAL(100, tcpScatter.size());

			ScatterTester scatterCompare(curve, tcpScatter);
			CHECK_TESTER(scatterCompare);

			//test also with other parameter
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq, 3, roh, 0, normalizationDose, 100);

			scatterCompare.setCompareScatter(tcpScatter);
			CHECK_TESTER(scatterCompare);

			std::vector<int> paramIdVec;
			models::BioModel::ParamVectorType meanVec;
			models::BioModel::ParamVectorType meanVecTest;
			meanVecTest.push_back(alpha);
			models::BioModel::ParamVectorType varianceVec;
			//"alphaMean":0,"alphaVariance":1,"alpha_beta":2, "rho":3
			paramIdVec.push_back(0);
			meanVec.push_back(tcplq.getAlphaMean());
			varianceVec.push_back(0);
			//setting parameter 1 will change the resulting scatter plot dramatically - is it meant to?
			//this is unexpected since the value was taken from the original calculation
			//paramIdVec.push_back(1); meanVec.push_back(tcplq.getAlphaVariance()); varianceVec.push_back(0);
			paramIdVec.push_back(2);
			meanVec.push_back(tcplq.getAlphaBeta());
			varianceVec.push_back(0);
			paramIdVec.push_back(3);
			meanVec.push_back(tcplq.getRho());
			varianceVec.push_back(0);

			CHECK_THROW_EXPLICIT(models::getScatterPlotVaryParameters(tcplq, paramIdVec, meanVecTest,
			                     varianceVec, normalizationDose, 50), core::InvalidParameterException);
			ScatterPlotType scatterVary = models::getScatterPlotVaryParameters(tcplq, paramIdVec, meanVec,
			                              varianceVec, normalizationDose, 50);
			CHECK_EQUAL(50, scatterVary.size());

			scatterCompare.setCompareScatter(scatterVary);
			CHECK_TESTER(scatterCompare);

			models::BioModelParamType variance = 0.00015;
			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(tcplq, 0, alpha, variance, normalizationDose,
			               100));
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq, 0, alpha, variance, normalizationDose,
			             100);

			scatterCompare.setVariance(variance);
			scatterCompare.setCompareScatter(tcpScatter);
			//allow 5% of the points to deviate more
			scatterCompare.setAllowExceptions(true);
			CHECK_TESTER(scatterCompare);

			//test also with other parameter
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq, 3, roh, variance, normalizationDose, 100);
			scatterCompare.setCompareScatter(tcpScatter);
			//allow 5% of the points to deviate more
			scatterCompare.setAllowExceptions(true);
			CHECK_TESTER(scatterCompare);

			paramIdVec.clear();
			meanVec.clear();
			varianceVec.clear();
			paramIdVec.push_back(0);
			meanVec.push_back(tcplq.getAlphaMean());
			varianceVec.push_back(variance);
			//paramIdVec.push_back(1); meanVec.push_back(tcplq.getAlphaVariance()); varianceVec.push_back(variance);
			paramIdVec.push_back(2);
			meanVec.push_back(tcplq.getAlphaBeta());
			varianceVec.push_back(variance);
			paramIdVec.push_back(3);
			meanVec.push_back(tcplq.getRho());
			varianceVec.push_back(variance);

			scatterVary = models::getScatterPlotVaryParameters(tcplq, paramIdVec, meanVec, varianceVec,
			              normalizationDose, 50);

			scatterCompare.setCompareScatter(scatterVary);
			//allow 5% of the points to deviate more
			scatterCompare.setAllowExceptions(true);
			CHECK_TESTER(scatterCompare);

			models::BioModelParamType alphaBeta = 10;
			tcplq.setParameters(alpha, alphaBeta, roh, 0.08);
			tcplq.init();

			normalizationDose = 40;
			curve = models::getCurveDoseVSBioModel(tcplq, normalizationDose);

			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(tcplq, 0, alpha, 0, normalizationDose, 100));
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq, 0, alpha, 0, normalizationDose, 100);

			scatterCompare.setReferenceCurve(curve);
			scatterCompare.setVariance(0);
			//do not allow larger deviations
			scatterCompare.setAllowExceptions(false);
			scatterCompare.setCompareScatter(tcpScatter);
			CHECK_TESTER(scatterCompare);

			variance = 0.25;
			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(tcplq, 0, alpha, variance, normalizationDose,
			               100));
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq, 0, alpha, variance, normalizationDose,
			             100);

			scatterCompare.setCompareScatter(tcpScatter);
			scatterCompare.setVariance(variance);
			//allow 5% of the points to deviate more
			scatterCompare.setAllowExceptions(true);
			CHECK_TESTER(scatterCompare);

			/*TCP LQ Test*/
			alpha = 0.3;
			beta = 0.03;
			roh = 10000000;
			numFractions = 20;
			rttb::models::TCPLQModel tcplq_test = rttb::models::TCPLQModel(dvh_test_tv, alpha, beta, roh,
			                                      numFractions);

			CHECK_NO_THROW(tcplq_test.init());
			normalizationDose = 60;
			curve = models::getCurveDoseVSBioModel(tcplq_test, normalizationDose);

			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(tcplq_test, 0, alpha, 0, normalizationDose,
			               100));
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq_test, 0, alpha, 0, normalizationDose, 100);

			scatterCompare.setReferenceCurve(curve);
			scatterCompare.setVariance(0);
			scatterCompare.setCompareScatter(tcpScatter);
			CHECK_TESTER(scatterCompare);

			//test also with other parameter
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq_test, 3, roh, 0, normalizationDose, 100);
			scatterCompare.setCompareScatter(tcpScatter);
			CHECK_TESTER(scatterCompare);

			paramIdVec.clear();
			meanVec.clear();
			varianceVec.clear();
			paramIdVec.push_back(0);
			meanVec.push_back(tcplq_test.getAlphaMean());
			varianceVec.push_back(0);
			//paramIdVec.push_back(1); meanVec.push_back(tcplq_test.getAlphaVariance()); varianceVec.push_back(0);
			paramIdVec.push_back(2);
			meanVec.push_back(tcplq_test.getAlphaBeta());
			varianceVec.push_back(0);
			paramIdVec.push_back(3);
			meanVec.push_back(tcplq_test.getRho());
			varianceVec.push_back(0);

			scatterVary = models::getScatterPlotVaryParameters(tcplq_test, paramIdVec, meanVec, varianceVec,
			              normalizationDose, 50);

			scatterCompare.setCompareScatter(scatterVary);
			CHECK_TESTER(scatterCompare);

			variance = 0.00025;
			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(tcplq_test, 0, alpha, variance,
			               normalizationDose, 100));
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq_test, 0, alpha, variance, normalizationDose,
			             100);

			scatterCompare.setCompareScatter(tcpScatter);
			scatterCompare.setVariance(variance);
			CHECK_TESTER(scatterCompare);

			//test also with other parameter
			tcpScatter = models::getScatterPlotVary1Parameter(tcplq_test, 3, roh, variance, normalizationDose,
			             100);
			scatterCompare.setCompareScatter(tcpScatter);
			scatterCompare.setAllowExceptions(true);
			CHECK_TESTER(scatterCompare);
			scatterCompare.setAllowExceptions(false);

			paramIdVec.clear();
			meanVec.clear();
			varianceVec.clear();
			paramIdVec.push_back(0);
			meanVec.push_back(tcplq_test.getAlphaMean());
			varianceVec.push_back(variance);
			//paramIdVec.push_back(1); meanVec.push_back(tcplq_test.getAlphaVariance()); varianceVec.push_back(variance);
			paramIdVec.push_back(2);
			meanVec.push_back(tcplq_test.getAlphaBeta());
			varianceVec.push_back(variance);
			paramIdVec.push_back(3);
			meanVec.push_back(tcplq_test.getRho());
			varianceVec.push_back(variance);

			scatterVary = models::getScatterPlotVaryParameters(tcplq_test, paramIdVec, meanVec, varianceVec,
			              normalizationDose, 50);

			scatterCompare.setCompareScatter(scatterVary);
			//allow 5% of the points to deviate more
			scatterCompare.setAllowExceptions(true);
			CHECK_TESTER(scatterCompare);

			//DVH HT 1
			rttb::io::other::DVHXMLFileReader dvhReader2 = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_NT1);
			DVHPointer dvhPtr2 = dvhReader2.generateDVH();

			CHECK_CLOSE(1.07920836034015810000e+001, models::getEUD(dvhPtr2, 10), toleranceEUD);

			//test RTNTCPLKBModel
			rttb::models::NTCPLKBModel lkb = rttb::models::NTCPLKBModel();
			models::BioModelParamType aVal = 10;
			models::BioModelParamType mVal = 0.16;
			models::BioModelParamType d50Val = 55;
			lkb.setDVH(dvhPtr2);
			lkb.setA(aVal);
			lkb.setM(mVal);
			lkb.setD50(d50Val);
			CHECK_NO_THROW(lkb.init());

			normalizationDose = 60;
			curve = models::getCurveDoseVSBioModel(lkb, normalizationDose);

			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(lkb, 2, aVal, 0, normalizationDose, 100));
			ScatterPlotType scatter = models::getScatterPlotVary1Parameter(lkb, 2, aVal, 0, normalizationDose,
			                          100);

			scatterCompare.setReferenceCurve(curve);
			scatterCompare.setVariance(0);
			scatterCompare.setCompareScatter(scatter);
			CHECK_TESTER(scatterCompare);

			//"d50":0,"m":1,"a":2
			//test also with other parameter
			scatter = models::getScatterPlotVary1Parameter(lkb, 0, d50Val, 0, normalizationDose, 100);
			scatterCompare.setCompareScatter(scatter);
			CHECK_TESTER(scatterCompare);

			paramIdVec.clear();
			meanVec.clear();
			varianceVec.clear();
			paramIdVec.push_back(0);
			meanVec.push_back(lkb.getD50());
			varianceVec.push_back(0);
			paramIdVec.push_back(1);
			meanVec.push_back(lkb.getM());
			varianceVec.push_back(0);
			paramIdVec.push_back(2);
			meanVec.push_back(lkb.getA());
			varianceVec.push_back(0);

			scatterVary = models::getScatterPlotVaryParameters(lkb, paramIdVec, meanVec, varianceVec,
			              normalizationDose, 50);

			scatterCompare.setCompareScatter(scatterVary);
			CHECK_TESTER(scatterCompare);

			variance = 0.00025;
			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(lkb, 2, aVal, variance, normalizationDose,
			               100));
			scatter = models::getScatterPlotVary1Parameter(lkb, 2, aVal, variance, normalizationDose, 100);

			scatterCompare.setCompareScatter(scatter);
			scatterCompare.setVariance(variance);
			CHECK_TESTER(scatterCompare);

			//test also with other parameter
			scatter = models::getScatterPlotVary1Parameter(lkb, 0, d50Val, variance, normalizationDose, 100);
			scatterCompare.setCompareScatter(scatter);
			CHECK_TESTER(scatterCompare);

			paramIdVec.clear();
			meanVec.clear();
			varianceVec.clear();
			paramIdVec.push_back(0);
			meanVec.push_back(lkb.getD50());
			varianceVec.push_back(variance);
			paramIdVec.push_back(1);
			meanVec.push_back(lkb.getM());
			varianceVec.push_back(variance);
			paramIdVec.push_back(2);
			meanVec.push_back(lkb.getA());
			varianceVec.push_back(variance);

			scatterVary = models::getScatterPlotVaryParameters(lkb, paramIdVec, meanVec, varianceVec,
			              normalizationDose, 50);

			scatterCompare.setCompareScatter(scatterVary);
			CHECK_TESTER(scatterCompare);

			//test RTNTCPRSModel
			rttb::models::NTCPRSModel rs = rttb::models::NTCPRSModel();
			models::BioModelParamType gammaVal = 1.7;
			models::BioModelParamType sVal = 1;
			rs.setDVH(dvhPtr2);
			rs.setD50(d50Val);
			rs.setGamma(gammaVal);
			rs.setS(sVal);
			CHECK_NO_THROW(rs.init());

			normalizationDose = 60;
			curve = models::getCurveDoseVSBioModel(rs, normalizationDose);

			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(rs, 0, d50Val, 0, normalizationDose, 100));
			scatter = models::getScatterPlotVary1Parameter(rs, 0, d50Val, 0, normalizationDose, 100);

			scatterCompare.setReferenceCurve(curve);
			scatterCompare.setVariance(0);
			scatterCompare.setCompareScatter(scatter);
			CHECK_TESTER(scatterCompare);

			//"d50":0,"gamma":1,"s":2
			//test also with other parameter
			scatter = models::getScatterPlotVary1Parameter(rs, 1, gammaVal, 0, normalizationDose, 100);
			scatterCompare.setCompareScatter(scatter);
			CHECK_TESTER(scatterCompare);

			paramIdVec.clear();
			meanVec.clear();
			varianceVec.clear();
			paramIdVec.push_back(0);
			meanVec.push_back(rs.getD50());
			varianceVec.push_back(0);
			paramIdVec.push_back(1);
			meanVec.push_back(rs.getGamma());
			varianceVec.push_back(0);
			paramIdVec.push_back(2);
			meanVec.push_back(rs.getS());
			varianceVec.push_back(0);

			scatterVary = models::getScatterPlotVaryParameters(rs, paramIdVec, meanVec, varianceVec,
			              normalizationDose, 50);

			scatterCompare.setCompareScatter(scatterVary);
			CHECK_TESTER(scatterCompare);

			variance = 0.0075;
			CHECK_NO_THROW(models::getScatterPlotVary1Parameter(rs, 0, d50Val, variance, normalizationDose,
			               100));
			scatter = models::getScatterPlotVary1Parameter(rs, 0, d50Val, variance, normalizationDose, 100);

			scatterCompare.setCompareScatter(scatter);
			scatterCompare.setVariance(variance);
			CHECK_TESTER(scatterCompare);

			//test also with other parameter
			scatter = models::getScatterPlotVary1Parameter(rs, 2, sVal, variance, normalizationDose, 100);
			scatterCompare.setCompareScatter(scatter);
			CHECK_TESTER(scatterCompare);

			paramIdVec.clear();
			meanVec.clear();
			varianceVec.clear();
			paramIdVec.push_back(0);
			meanVec.push_back(rs.getD50());
			varianceVec.push_back(variance);
			paramIdVec.push_back(1);
			meanVec.push_back(rs.getGamma());
			varianceVec.push_back(variance);
			paramIdVec.push_back(2);
			meanVec.push_back(rs.getS());
			varianceVec.push_back(variance);

			scatterVary = models::getScatterPlotVaryParameters(rs, paramIdVec, meanVec, varianceVec,
			              normalizationDose, 50);

			scatterCompare.setCompareScatter(scatterVary);
			CHECK_TESTER(scatterCompare);

			RETURN_AND_REPORT_TEST_SUCCESS;

		}

	}//testing
}//rttb


#include <CGAL/Cartesian.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Periodic_4_hyperbolic_triangulation_2/internal/Periodic_4_hyperbolic_triangulation_dummy_14.h>
#include <CGAL/Hyperbolic_octagon_translation.h>
#include <CGAL/Algebraic_kernel_for_circles_2_2.h>
#include <CGAL/Circular_kernel_2.h>

#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

#include <iostream>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>               Traits;
typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_2<Traits>                Triangulation;
typedef Triangulation::Face_handle                                                  Face_handle;
typedef Triangulation::Vertex_handle                                                Vertex_handle;
typedef Triangulation::Locate_type                                                  Locate_type;
typedef Triangulation::Hyperbolic_translation                                       Hyperbolic_translation;
typedef Triangulation::Point                                                        Point;

std::ostream& operator<<(std::ostream& s, const Locate_type& lt)
{
  switch(lt)
  {
    case Triangulation::VERTEX:
      s << "VERTEX";
      break;
    case Triangulation::FACE:
      s << "FACE";
      break;
    case Triangulation::EDGE:
      s << "EDGE";
      break;
  }

  return s;
}

int main(int, char**)
{
  Triangulation tr;

  assert(tr.is_valid());

  Locate_type lt;
  int li;
  Face_handle fh;

  std::cout << "---- locating dummy points (all should be vertices) ----" << std::endl;
  for(int j=0; j<14; ++j) {
    Point query = tr.get_dummy_point(j);
    fh = tr.hyperbolic_locate(query, lt, li);
    assert(lt == Triangulation::VERTEX);
    std::cout << "   dummy point " << j << ": OK " << std::endl;
  }

  std::cout << "---- locating the midpoint of a Euclidean segment ----" << std::endl;
  Point p1 = tr.get_dummy_point(0), p2 = tr.get_dummy_point(1);
  Point query = midpoint(p1, p2);
  fh = tr.hyperbolic_locate(query, lt, li);
  assert(lt == Triangulation::EDGE);
  std::cout << "   located as edge OK" << std::endl;

  std::cout << "---- inserting a single point and locating it ----" << std::endl;
  Vertex_handle v = tr.insert(Point(-0.4, -0.1));
  fh = tr.hyperbolic_locate(v->point(), lt, li);
  assert(lt == Triangulation::VERTEX);
  std::cout << "   located as vertex OK" << std::endl;

  // TODO: add a test case for a circular edge!

  return EXIT_SUCCESS;
}


#ifndef INTEGRALS_HPP_
#define INTEGRALS_HPP_
/**
 * @file integrals.hpp
 * @author Adam Lamson
 * @brief Probility Density Function integrals for KMC algorithm. Used to create
 * lookup tables
 * @version 0.1
 * @date 2019-04-15
 *
 * @copyright Copyright (c) 2019
 *
 */

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <cassert>
#include <cstdio>
#include <iostream>
#include <string>

/*! \brief Integrate exponential factor with the form of
 * e^{-M * (\sqrt{ s^2 + lm^2} - ell0)^2}
 * from sbound0 to sbound1  with respect to the variable s.
 *
 * \param lm Physically, this is the perpendicular distance above rod
 * \param sbound lowerr limit of integral
 * \param sbound Upper limit of integral
 * \param M exponential constant factor. Physically, this is the product of
 (1-load_sensitivity)*spring_const/(k_B * Temperature)
 * \param ell0 Shift of the integrands mean. Physically, protein rest length
 * \return result The value of the integration

 */
inline double integral(double lm, double sbound0, double sbound1, double M,
                       double ell0) {
    if (sbound0 >= sbound1) {
        return 0;
    }
    auto integrand = [&](double s) {
        // lambda capture variabls ell0 and M
        const double exponent = sqrt(s * s + lm * lm) - ell0;
        return exp(-M * exponent * exponent);
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, sbound0, sbound1, 10, 1e-6, &error);
    return result;
}

inline double bind_vol_integral(double sbound, double M, double ell0) {
    assert(sbound > 0);
    auto integrand = [&](double s) {
        // lambda capture variabls ell0 and M
        const double exponent = s - ell0;
        return s * s * exp(-M * exponent * exponent);
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, 0, sbound, 10, 1e-6, &error);
    return 4. * M_PI * result;
}

/*! \brief Integrate exponential factor with the form of
 * e^{-M * [ (1-e_fact)\sqrt{s^2 + lm^2} - ell0)^2 -
 *          fdep_length * (\sqrt{ s^2 + lm^2} - ell0) ] }
 * from sbound0 to sbound1  with respect to the variable s.
 *
 * \param lm Physically, this is the perpendicular distance above rod
 * \param sbound lowerr limit of integral
 * \param sbound Upper limit of integral
 * \param M exponential constant factor. Physically, this is the product of
 spring_const/(k_B * Temperature)
 * \param e_fact energy(load) sensitivity to unbinding.
 * \param fdep_length Characteristic length for force dependent unbinding.
 * \param ell0 Shift of the integrands mean. Physically, protein rest length
 * \return result The value of the integration

 */
inline double fdep_integral(double lm, double sbound0, double sbound1, double M,
                            double e_fact, double fdep_length, double ell0) {
    if (sbound0 >= sbound1) {
        return 0;
    }
    auto integrand = [&](double s) {
        const double rprime = sqrt(s * s + lm * lm) - ell0;
        const double energy_term = .5 * (1. - e_fact) * rprime * rprime;
        const double force_term = fdep_length * rprime;
        return exp(-M * (energy_term - force_term));
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, sbound0, sbound1, 10, 1e-6, &error);
    return result;
}

inline double fdep_bind_vol_integral(double sbound, double M, double e_fact,
                                     double fdep_length, double ell0) {
    assert(sbound > 0);
    auto integrand = [&](double s) {
        const double rprime = s - ell0;
        const double energy_term = .5 * (1. - e_fact) * rprime * rprime;
        const double force_term = fdep_length * rprime;
        return exp(-M * (energy_term - force_term));
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, 0, sbound, 10, 1e-6, &error);
    return 4. * M_PI * result;
}

/*! \brief Integrate exponential factor with the form of
 * e^{-M * [ (1-e_fact)\sqrt{s^2 + lm^2} - ell0)^2 -
 *          fdep_length * (\sqrt{ s^2 + lm^2} - ell0) ] }
 * from sbound0 to sbound1  with respect to the variable s.
 *
 * \param lm Physically, this is the perpendicular distance above rod
 * \param sbound lower limit of integral
 * \param sbound Upper limit of integral
 * \param M1 exponential constant factor when spring is compressed.
 *    Physically, this is the product of spring_const_1/(k_B * Temperature)
 * \param M2 exponential constant factor when spring is stretched.
 *    Physically, this is the product of spring_const_2/(k_B * Temperature)
 * \param e_fact energy(load) sensitivity to unbinding.
 * \param fdep_length Characteristic length for force dependent unbinding.
 * \param ell0 Shift of the integrands mean. Physically, protein rest length
 * \return result The value of the integration

 */
inline double asym_integral(double lm, double sbound0, double sbound1,
                            double M1, double M2, double e_fact,
                            double fdep_length, double ell0) {
    if (sbound0 >= sbound1) {
        return 0;
    }
    auto integrand = [&](double s) {
        const double rprime = sqrt(s * s + lm * lm) - ell0;
        const double energy_term = .5 * (1. - e_fact) * rprime * rprime;
        const double force_term = fdep_length * rprime;
        const double M = rprime < 0. ? M1 : M2;
        return exp(-M * (energy_term - force_term));
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, sbound0, sbound1, 10, 1e-6, &error);
    return result;
}

inline double asym_bind_vol_integral(double sbound, double M1, double M2,
                                     double e_fact, double fdep_length,
                                     double ell0) {
    assert(sbound > 0);
    auto integrand = [&](double s) {
        const double rprime = s - ell0;
        const double energy_term = .5 * (1. - e_fact) * rprime * rprime;
        const double force_term = fdep_length * rprime;
        const double M = rprime < 0. ? M1 : M2;
        return exp(-M * (energy_term - force_term));
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, 0, sbound, 10, 1e-6, &error);
    return 4. * M_PI * result;
}

#endif /* INTEGRALS_HPP_ */


/*
 * Software License Agreement (BSD License)
 *
 *  Beta-SLAM - Simultaneous localization and grid mapping with beta distributions
 *  Copyright (c) 2013-2019, Joachim Clemens, Thomas Reineking, Tobias Kluth
 *  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 BSLAM nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <cmath>
#include <assert.h>

#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

#include "bslam/utils/Factorial.h"

namespace bslam {

BetaDistribution::BetaDistribution( float alpha, float beta ) :
		m_alpha( alpha ),
		m_beta( beta )
{
	// Nothing else to do here
}

bool
BetaDistribution::operator==( const BetaDistribution &other ) const {
	return m_alpha == other.m_alpha && m_beta == other.m_beta;
}

double
BetaDistribution::B( double alpha, double beta ) {
	//return gamma( alpha ) * gamma( beta ) / gamma( alpha + beta );
	return boost::math::beta( alpha, beta );
}

double
BetaDistribution::Binv( double alpha, double beta ) {
	//return gamma( alpha + beta ) / gamma( alpha ) * gamma( beta );
	return 1.0 / B( alpha, beta );
}

uint32_t
BetaDistribution::choose( uint32_t n, uint32_t k ) {
	// iterative
	uint32_t res = 1;
	for( uint32_t i = 1; i <= k; i++ )
		res *= (n + 1 - i) / i;
	return res;

	// recursive
	//return k == 0 ? 1 : (n * choose( n - 1, k - 1 )) / k;
}

double
BetaDistribution::gamma( double x ) {
	//return std::tgamma( x );
	return boost::math::tgamma( x );
}

double
BetaDistribution::digamma( double x ) {
	/*
	// According to http://web.science.mq.edu.au/~mjohnson/code/digamma.c
	assert(x > 0);

	double 	result = 0,
			xx, xx2, xx4;

	for ( ; x < 7; ++x)
		result -= 1/x;

	x	-= 1.0/2.0;
	xx	= 1.0/x;
	xx2	= xx*xx;
	xx4	= xx2*xx2;

	result += log( x ) + (1. / 24.) * xx2 - (7.0 / 960.0) * xx4 + (31.0 / 8064.0) * xx4 * xx2 - (127.0 / 30720.0) * xx4 * xx4;

	return result;
	*/

	return boost::math::digamma( x );
}

double
BetaDistribution::pdf( double x ) const {
	return pdf( x, m_alpha, m_beta );
}

double
BetaDistribution::pdf( double x, double alpha, double beta ) {
	return Binv( alpha, beta ) * pow( x, alpha - 1 ) * pow( 1 - x, beta - 1 );
}

double
BetaDistribution::pmf( uint32_t k, uint32_t n, double alpha, double beta ) {
	return choose( n, k ) * B( k + alpha, n - k + beta ) * Binv( alpha, beta );
}

double
BetaDistribution::cdf( double x ) const {
	return cdf( x, m_alpha, m_beta );
}

double
BetaDistribution::cdf( double x, double alpha, double beta ) {
	return boost::math::ibeta( alpha, beta, x ); // regularized incomplete beta function
}

double
BetaDistribution::cdfComp( double x, double alpha, double beta ) {
	return boost::math::ibetac( alpha, beta, x ); // regularized incomplete beta function
}

double
BetaDistribution::cdf( uint32_t k, uint32_t n, double alpha, double beta ) {
	throw std::runtime_error( "Not implemented yet" );
	//return 1 - B( beta + n - k - 1, alpha + k + 1 ) * hyp3F2( 1, alpha + k + 1, -n + k + 1; k + 2, -beta - n + k + 2; 1 ) / (B( alpha, beta ) * B( n - k, k + 2 ) * (n + 1));
	return 0.0;
}

double
BetaDistribution::mean() const {
	return mean( m_alpha, m_beta );
}

constexpr double
BetaDistribution::mean( double alpha, double beta ) {
	return alpha / (alpha + beta);
}

constexpr double
BetaDistribution::mean( uint32_t n, double alpha, double beta ) {
	return n * alpha / (alpha + beta);
}

constexpr double
BetaDistribution::mean( uint32_t k, uint32_t n, double alpha, double beta ) {
	return (alpha + k) / (alpha + beta + n);
}

double
BetaDistribution::mode() const {
	return mode( m_alpha, m_beta );
}

constexpr double
BetaDistribution::mode( double alpha, double beta ) {
	/*
	assert( alpha >= 1 );
	assert( alpha + beta > 2 );
	*/
	return (alpha - 1) / (alpha + beta - 2);
}

double
BetaDistribution::var() const {
	return var( m_alpha, m_beta );
}

constexpr double
BetaDistribution::var( double alpha, double beta ) {
	return alpha * beta / ((alpha + beta + 1) * (alpha + beta) * (alpha + beta));
}

double
BetaDistribution::entropy() const {
	return entropy( m_alpha, m_beta );
}

double
BetaDistribution::entropy( double alpha, double beta ) {
	return log( B( alpha, beta ) ) - (alpha - 1)*digamma( alpha ) - (beta - 1)*digamma( beta ) + (alpha + beta - 2)*digamma( alpha + beta );
}

constexpr double
BetaDistribution::var( uint32_t n, double alpha, double beta ) {
	return n * alpha * beta * (alpha + beta + n) / ((alpha + beta + 1) * (alpha + beta) * (alpha + beta));
}

constexpr double
BetaDistribution::var( uint32_t k, uint32_t n, double alpha, double beta ) {
	return var( alpha + k, beta + n - k );
}

/*
double
BetaDistribution::pdf( double x, int alpha, int beta ) {
	assert( alpha > 0 );
	assert( beta > 0 );
	return Binv( alpha, beta ) * pow( x, alpha - 1 ) * pow( 1 - x, beta - 1 );
}

double
BetaDistribution::B( int alpha, int beta ) {
	return gamma( alpha ) * gamma( beta ) / gamma( alpha + beta );
}

double
BetaDistribution::Binv( int alpha, int beta ) {
	return gamma( alpha + beta ) / gamma( alpha ) * gamma( beta );
}

double
BetaDistribution::gamma( int x ) {
	return Factorial::value( x - 1 );
}
*/

double
BetaDistribution::ignorance( double priorAlpha, double priorBeta ) const {
	return ( priorAlpha + priorBeta + 1 ) / ( m_alpha + m_beta + 1 );
}

double
BetaDistribution::dissonance() const {
	double	curMean = mean();

	// Shannon entropy
	return -curMean * log( curMean ) - (1.0 - curMean) * log( 1.0 - curMean );
}

double
BetaDistribution::conflict( double epsilon ) const {
	return cdf( 0.5 + epsilon ) - cdf( 0.5 - epsilon );
}

} /* namespace bslam */


/**
 * @file bigbatch_sgd_test.cpp
 * @author Marcus Edel
 *
 * Test file for big-batch SGD.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/bigbatch_sgd/bigbatch_sgd.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace arma;
using namespace mlpack;
using namespace mlpack::optimization;

using namespace mlpack::distribution;
using namespace mlpack::regression;

BOOST_AUTO_TEST_SUITE(BigBatchSGDTest);

/**
 * Create the data for the logistic regression test case.
 */
void CreateLogisticRegressionTestData(arma::mat& data,
                                      arma::mat& testData,
                                      arma::mat& shuffledData,
                                      arma::Row<size_t>& responses,
                                      arma::Row<size_t>& testResponses,
                                      arma::Row<size_t>& shuffledResponses)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  data = arma::mat(3, 1000);
  responses = arma::Row<size_t>(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0,
      data.n_cols - 1, data.n_cols));
  shuffledData = arma::mat(3, 1000);
  shuffledResponses = arma::Row<size_t>(1000);
  for (size_t i = 0; i < data.n_cols; ++i)
  {
    shuffledData.col(i) = data.col(indices[i]);
    shuffledResponses[i] = responses[indices[i]];
  }

  // Create a test set.
  testData = arma::mat(3, 1000);
  testResponses = arma::Row<size_t>(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    testData.col(i) = g1.Random();
    testResponses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    testData.col(i) = g2.Random();
    testResponses[i] = 1;
  }
}

/**
 * Run big-batch SGD using BBS_BB on logistic regression and make sure the
 * results are acceptable.
 */
BOOST_AUTO_TEST_CASE(BBSBBLogisticRegressionTest)
{
  arma::mat data, testData, shuffledData;
  arma::Row<size_t> responses, testResponses, shuffledResponses;

  CreateLogisticRegressionTestData(data, testData, shuffledData,
      responses, testResponses, shuffledResponses);

  // Now run big-batch SGD with a couple of batch sizes.
  for (size_t batchSize = 30; batchSize < 40; batchSize += 5)
  {
    BBS_BB bbsgd(batchSize, 0.01, 0.1, 6000, 1e-3);
    LogisticRegression<> lr(shuffledData, shuffledResponses, bbsgd, 0.5);

    // Ensure that the error is close to zero.
    const double acc = lr.ComputeAccuracy(data, responses);
    BOOST_REQUIRE_CLOSE(acc, 100.0, 0.3); // 0.3% error tolerance.

    const double testAcc = lr.ComputeAccuracy(testData, testResponses);
    BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.6); // 0.6% error tolerance.
  }
}

/**
 * Run big-batch SGD using BBS_Armijo on logistic regression and make sure the
 * results are acceptable.
 */
BOOST_AUTO_TEST_CASE(BBSArmijoLogisticRegressionTest)
{
  arma::mat data, testData, shuffledData;
  arma::Row<size_t> responses, testResponses, shuffledResponses;

  CreateLogisticRegressionTestData(data, testData, shuffledData,
      responses, testResponses, shuffledResponses);

  // Now run big-batch SGD with a couple of batch sizes.
  for (size_t batchSize = 30; batchSize < 60; batchSize += 1)
  {
    BBS_Armijo bbsgd(batchSize, 0.01, 0.1, 6000, 1e-3);
    LogisticRegression<> lr(shuffledData, shuffledResponses, bbsgd, 0.5);

    // Ensure that the error is close to zero.
    const double acc = lr.ComputeAccuracy(data, responses);
    BOOST_REQUIRE_CLOSE(acc, 100.0, 0.3); // 0.3% error tolerance.

    const double testAcc = lr.ComputeAccuracy(testData, testResponses);
    BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.6); // 0.6% error tolerance.
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include "utils/data_generator.cpp"
#include "../src/numerical/gradient_descent/gd_solver.h"
#include "../src/logging/easylogging++.h"
#include <armadillo>

using arma::mat;

INITIALIZE_EASYLOGGINGPP

mat WEIGHTS = {100, -20, 50, -0.5};

int main(int argc, char *argv[])
{
    el::Configurations conf("./logging-config.conf");
    el::Loggers::reconfigureLogger("default", conf);

    auto data_generator = DataGenerator();
    auto L = data_generator.generate_library();
    auto s = data_generator.generate_signal(WEIGHTS);

    LOG(INFO) << "True: " << WEIGHTS;

    GDSolver solver = GDSolver(L, 1, 1000);
    solver.find_optimal_lr(s);
    auto result = solver.solve(s);
    LOG(INFO) << "Result: " << result;

    return 0;
}


/*
 *  VSCSound.cpp
 *  SynthStation
 *
 *  Created by Jonathan Thorpe on 22/10/2011.
 *  Copyright 2011 NXP. All rights reserved.
 *
 */

#include "VSCSound.h"

#include <boost/assert.hpp>

#include <cmath>
#include <cassert>

VSC::Sound::Pitch::Pitch() : mReferenceAFrequency(440.0)
{
    computeMidiNoteFrequencies();
}

VSC::Float VSC::Sound::Pitch::logFrequencyToFrequency(Float logFreq)
{
	return std::pow(10.0, logFreq);
}

VSC::Float VSC::Sound::Pitch::frequencyToLogFrequency(Float freq)
{
	return std::log10(freq);
}

VSC::Float VSC::Sound::Pitch::frequencyForMidiNote(Float midiNote)
{
	return frequencyForMidiNote((unsigned int)midiNote);
}

VSC::Float VSC::Sound::Pitch::frequencyForMidiNote(unsigned int midiNote)
{
	BOOST_ASSERT_MSG(midiNote >= 0 && midiNote < 127, "MIDI note should be in range [0-127]");
	return mMIDINoteFrequencies[midiNote];
}

void VSC::Sound::Pitch::setReferenceAFrequency(Float f)
{
	mReferenceAFrequency = f;
    computeMidiNoteFrequencies();
}

VSC::Float VSC::Sound::Pitch::getReferenceAFrequency(void)
{
	return mReferenceAFrequency;
}

void VSC::Sound::Pitch::computeMidiNoteFrequencies(void)
{
	mMIDINoteFrequencies.resize(128);
	for (int x = 0; x < 127; ++x)
    {
		Float freq = (mReferenceAFrequency / 32.0) * (std::pow(2.0, ((x - 9.0) / 12.0)));
		mMIDINoteFrequencies[x] = freq;
	}
}


#include "aux/eigen2hdf.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "post_processing/macroscopic_quantities.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"
#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/spectral_elem.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/spectral_function/hermite_polynomial.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/utility/mass_matrix.hpp"

#include "spectral/polar_to_hermite.hpp"
#include "spectral/shift_hermite_2d.hpp"

#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>

template <typename T>
struct show_name
{
};

#define PI 3.141592653589793238462643383279502884197

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

#ifdef EXTENDED_PRECISION
typedef long double numeric_t;
#else
typedef double numeric_t;
#endif

int main(int argc, char *argv[])
{
  Timer<> timer;
  po::options_description options("options");
  options.add_options()
      ("help", "show help message");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  // read polar basis from file
  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, "spectral_basis.desc");
  //  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  int max_deg = spectral::get_max_k(polar_basis);
  const unsigned int K = max_deg + 1;
  // create corresponding Hermite basis
  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, max_deg + 1, 2);
  SpectralBasisFactoryHN::write_basis_descriptor(hermite_basis, "hermite_basis.desc");

  if (hermite_basis.n_dofs() != polar_basis.n_dofs()) {
    throw runtime_error("Hermite basis does not match!");
    return 1;
  }

  cout << "size(polar basis) = " << polar_basis.n_dofs() << endl
       << "size(hermite basis) = " << hermite_basis.n_dofs();

  cout << "\n--------------------\n";
  cout << "Test 2: (P->H) -> (H->P) show coefficients\n";

  /*
   * load coefficients (polar basis) from HDF5
   */
  const unsigned int N = polar_basis.n_dofs();
  Eigen::VectorXd coeffs(N);
  hid_t h5_init = H5Fopen("init.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  eigen2hdf::load(h5_init, "coeffs", coeffs);
  H5Fclose(h5_init);
  coeffs.setZero();
  coeffs[0] = 1;
  {
    cout << "input: ||cp||^2: " << coeffs.cwiseAbs2().sum() << endl;
    ofstream fout("cp.dat");
    fout << coeffs;
    fout.close();
  }

  // compute bulk velocity
  Mass mass;
  mass.init(polar_basis);
  Momentum momentum;
  momentum.init(polar_basis);

  {
    auto entries = momentum.entries();
    for (auto entry : entries) {
      cout << entry.first << " " << entry.second << "\n";
    }
  }

  MQEval mqtsc(polar_basis);
  auto mq_eval = mqtsc.evaluator();
  mq_eval(coeffs.data(), N);
  cout << "correct mass: " << mq_eval.m << endl;
  cout << "correct momentum: " << mq_eval.v.transpose() << endl;

  const double m = mass.compute(coeffs.data());
  Eigen::Vector2d u = momentum.compute(coeffs.data()) / m;
  cout << "\n----- input -----\n"
       << "\n";
  cout << scientific << setprecision(8) << "mass: " << m << endl
       << "momentum: " << u(0) << ", " << u(1) << endl;

  // compute hermite coefficients
  Polar2Hermite<polar_basis_t, hermite_basis_t> P2H(polar_basis, hermite_basis);
  // print_timer(timer.stop(), "init P2H");

  Eigen::VectorXd buf(N);
  P2H.to_hermite(buf, coeffs);

  if (sizeof(numeric_t) == 16) {
    cout << "Using *extended precision*  in ShiftHermite\n";
  } else if (sizeof(numeric_t) == 8) {
    cout << "Using double precision in ShiftHermite\n";
  }
  std::vector<numeric_t> cH(buf.data(), buf.data() + N);
  typedef Eigen::Array<numeric_t, Eigen::Dynamic, 1> array_t;
  Eigen::Map<const array_t> vec_cH(cH.data(), cH.size());
  cout << "||c_H||^2: " << vec_cH.cwiseAbs2().sum() << "\n";
  ShiftHermite2D<hermite_basis_t, numeric_t> shift_hermite(hermite_basis);
  shift_hermite.init();
  timer.start();
  shift_hermite.shift(cH.data(), u(0), u(1));
  //  print_timer(timer.stop(), "shift Hermite coefficients");

  // convert to double
  std::transform(cH.begin(), cH.end(), buf.data(), [](numeric_t x) { return double(x); });

  // -> Polar coordinates
  Eigen::VectorXd Cc(N);
  P2H.to_polar(Cc, buf);

  const double mc = mass.compute(Cc.data());
  Eigen::Vector2d uc = momentum.compute(Cc.data()) / mc;
  cout << "\n----- centered -----\n";
  cout << "mass: " << scientific << setprecision(8) << mc << "\t(diff = " << std::abs(m - mc) << ")"
       << endl
       << "momentum: " << uc(0) << ", " << uc(1) << endl;

  // write new coefficients to disk
  hid_t h5_shifted = H5Fcreate("shifted.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  Eigen::Map<Eigen::VectorXd> Cc_eigen(Cc.data(), Cc.size());
  eigen2hdf::save(h5_shifted, "coeffs", Cc_eigen);
  // export hermite coefficients
  Eigen::Map<Eigen::VectorXd> cH_eigen(buf.data(), buf.size());
  eigen2hdf::save(h5_shifted, "coeffs_hermite", cH_eigen);
  H5Fclose(h5_shifted);

  // do some cheap scattering
  // ...

  // Move back to original position
  timer.start();
  shift_hermite.shift(cH.data(), -u(0), -u(1));
  //  print_timer(timer.stop(), "shift Hermite coefficients (back)");

  // go back to polar coordinates
  std::transform(cH.begin(), cH.end(), buf.data(), [](numeric_t x) { return double(x); });
  Eigen::VectorXd Cc2(N);
  P2H.to_polar(Cc2, buf);

  const double m1 = mass.compute(Cc2.data());
  Eigen::Vector2d u1 = momentum.compute(Cc2.data()) / m1;
  // stop here

  auto M = make_mass_matrix(polar_basis, polar_basis);

  cout << "----- move to original pos. -----\n";
  cout << "mass: " << scientific << setprecision(8) << m1 << "\t(diff = " << std::abs(m - m1) << ")"
       << endl
       << "momentum: " << scientific << setprecision(8) << u1(0) << ", " << u1(1)
       << "\t(diff = " << (u - u1).squaredNorm() << ")" << endl;

  Eigen::Map<Eigen::VectorXd> coeffs2(Cc2.data(), Cc2.size());
  Eigen::VectorXd tmp = (coeffs - coeffs2).array().square();
  double shift_error = sqrt((M * tmp).sum());

  cout << "shift_error: " << scientific << setprecision(8) << shift_error << endl;

  return 0;
}


// Boost.Geometry

// Copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>

namespace boost { namespace geometry { namespace detail
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with first order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD703541
*/
template <typename CT>
class andoyer_inverse
{
public:
    template <typename T1, typename T2, typename Spheroid>
    andoyer_inverse(T1 const& lon1,
                    T1 const& lat1,
                    T2 const& lon2,
                    T2 const& lat2,
                    Spheroid const& spheroid)
        : m_a(get_radius<0>(spheroid))
        , m_b(get_radius<2>(spheroid))
        , m_f(detail::flattening<CT>(spheroid))
        , m_is_result_zero(false)
    {
        // coordinates in radians

        if ( math::equals(lon1, lon2)
          && math::equals(lat1, lat2) )
        {
            m_is_result_zero = true;
            return;
        }

        CT const pi_half = math::pi<CT>() / CT(2);

        if ( math::equals(math::abs(lat1), pi_half)
          && math::equals(math::abs(lat2), pi_half) )
        {
            m_is_result_zero = true;
            return;
        }

        CT const dlon = lon2 - lon1;
        m_sin_dlon = sin(dlon);
        m_cos_dlon = cos(dlon);
        m_sin_lat1 = sin(lat1);
        m_cos_lat1 = cos(lat1);
        m_sin_lat2 = sin(lat2);
        m_cos_lat2 = cos(lat2);

        // H,G,T = infinity if cos_d = 1 or cos_d = -1
        // lat1 == +-90 && lat2 == +-90
        // lat1 == lat2 && lon1 == lon2
        m_cos_d = m_sin_lat1*m_sin_lat2 + m_cos_lat1*m_cos_lat2*m_cos_dlon;
        m_d = acos(m_cos_d);
        m_sin_d = sin(m_d);

        // just in case since above lat1 and lat2 is checked
        // the check below is equal to cos_d == 1 || cos_d == -1 || d == 0
        if ( math::equals(m_sin_d, CT(0)) )
        {
            m_is_result_zero = true;
            return;
        }
    }

    inline CT distance() const
    {
        if ( m_is_result_zero )
        {
            // TODO return some approximated value
            return CT(0);
        }

        CT const K = math::sqr(m_sin_lat1-m_sin_lat2);
        CT const L = math::sqr(m_sin_lat1+m_sin_lat2);
        CT const three_sin_d = CT(3) * m_sin_d;
        // H or G = infinity if cos_d = 1 or cos_d = -1
        CT const H = (m_d+three_sin_d)/(CT(1)-m_cos_d);
        CT const G = (m_d-three_sin_d)/(CT(1)+m_cos_d);

        // for e.g. lat1=-90 && lat2=90 here we have G*L=INF*0
        CT const dd = -(m_f/CT(4))*(H*K+G*L);

        return m_a * (m_d + dd);
    }

    inline CT azimuth() const
    {
        // it's a situation when the endpoints are on the poles +-90 deg
        // in this case the azimuth could either be 0 or +-pi
        if ( m_is_result_zero )
        {
            return CT(0);
        }

        CT A = CT(0);
        CT U = CT(0);
        if ( ! math::equals(m_cos_lat2, CT(0)) )
        {
            CT const tan_lat2 = m_sin_lat2/m_cos_lat2;
            CT const M = m_cos_lat1*tan_lat2-m_sin_lat1*m_cos_dlon;
            A = atan2(m_sin_dlon, M);
            CT const sin_2A = sin(CT(2)*A);
            U = (m_f/CT(2))*math::sqr(m_cos_lat1)*sin_2A;
        }

        CT V = CT(0);
        if ( ! math::equals(m_cos_lat1, CT(0)) )
        {
            CT const tan_lat1 = m_sin_lat1/m_cos_lat1;
            CT const N = m_cos_lat2*tan_lat1-m_sin_lat2*m_cos_dlon;
            CT const B = atan2(m_sin_dlon, N);
            CT const sin_2B = sin(CT(2)*B);
            V = (m_f/CT(2))*math::sqr(m_cos_lat2)*sin_2B;
        }

        // infinity if sin_d = 0, so cos_d = 1 or cos_d = -1
        CT const T = m_d / m_sin_d;
        CT const dA = V*T-U;

        return A - dA;
    }

private:
    CT const m_a;
    CT const m_b;
    CT const m_f;

    CT m_sin_dlon;
    CT m_cos_dlon;
    CT m_sin_lat1;
    CT m_cos_lat1;
    CT m_sin_lat2;
    CT m_cos_lat2;

    CT m_cos_d;
    CT m_d;
    CT m_sin_d;

    bool m_is_result_zero;
};

}}} // namespace boost::geometry::detail

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP


#include "C_DL1D.h"
#include <vector>
#include <MatrixOper.h>
#include "IM_IO.h"
#include "C_OMP.h"
using namespace std;
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>
#include <fcntl.h>
// #include <gsl/gsl_matrix.h>
// #include <gsl/gsl_linalg.h>
#include <armadillo>
using namespace arma;
dblarray TabTrain;

C_DL1D::C_DL1D(dblarray &training_set)
{
	TabTrain = training_set; // training set, with samples as column
	Npix = TabTrain.nx(); // atom/training sample length
	eps = 2.220446049250313e-16;
}
C_DL1D::~C_DL1D()
{
}

dblarray C_DL1D::dl1d(dblarray &training_set, dblarray &initD, int IterationNumber,int SparsityTarget,double ErrorTarget,bool Verb)
{
	Na = initD.ny(); // number of atoms in dictionary
	int Ntrain = TabTrain.ny(); // number of training samples
	dblarray sample(Npix); // training sample
	double Amean,sample_mean; // atom and training sample mean
	dblarray D = initD; // learned dictionary initialized with initD
	dblarray X(Na,Ntrain);	 // sparse coding coefficients of TabTrain
	dblarray Xt(Ntrain,Na); // transpose of X
	dblarray Xtpinv(Na,Ntrain); // pseudoinverse of Xt
	dblarray Xpinv(Ntrain,Na); // X pseudo-inverse
	dblarray Scurrent(Npix); // Current sample
	dblarray Acurrent(Npix); // Current atom, during normalization
	double Anorm; // Current atom norm
	double minT; // eigenvalue threshold used to compute pseudo inverse
	int atom_usage; // number of sample using a given atom
	int new_sample_ind; // index of the new sample used in place of an useless atom
	int atoms_replaced; // number of unused atoms replaced with training samples
	MatOper terminator;
	double average_sparsity = 0;
	double average_error = 0;
	dblarray sparse_approx(Npix,Ntrain); // sparse approximation of TabTrain in the current dictionary
	dblarray sparse_sample(Npix); // sparse approximation of current training sample
	mat armaXt(Ntrain,Na);
	dblarray At(Na,Ntrain);
	dblarray Vt(Na,Na);
	dblarray V(Na,Na);
	dblarray S(Na,Na);
	mat armaInv;
	gsl_rng *rng;
	const gsl_rng_type * T;
	T = gsl_rng_default;
	rng = gsl_rng_alloc (T);
	/*gsl_matrix * gslA = gsl_matrix_alloc(Ntrain,Na);
		gsl_matrix * gslV = gsl_matrix_alloc(Na,Na);
		gsl_vector * gslS = gsl_vector_alloc(Na);
		gsl_vector * work = gsl_vector_alloc(Na);*/

	//Removing atoms mean and normalizing atoms with norm > 1
	for (int m=0;m<Na;m++)
	{
		for (int p=0;p<Npix;p++)
			Acurrent(p) = D(p,m); // reading atom p
		Amean = Acurrent.mean();
		for (int p=0;p<Npix;p++) // computing atom mean
			Acurrent(p) = Acurrent(p) - Amean; // removing atom mean
		Anorm = sqrt(Acurrent.energy()); // computing 0-mean atom norm
		if (Anorm > 1)
		{
			for (int p=0;p<Npix;p++)
				D(p,m) = (D(p,m) - Amean) / Anorm; // removing mean and normalizing
		}
		else 	for (int p=0;p<Npix;p++) D(p,m) = D(p,m) - Amean; // only removing mean
	}
	C_OMP coder(D); // building OMP sparse coder
	// Removing training sample mean
	for (int k=0; k<Ntrain; k++)
	{
		for (int i=0; i<Npix; i++) // reading sample k
			sample(i) = TabTrain(i,k);
		sample_mean = sample.mean(); // computing sample mean
		for (int i=0; i<Npix; i++)
			TabTrain(i,k) = sample(i) - sample_mean;  // subtracting mean
	}
	// iterating sparse coding and dictionary update steps
	if (Verb == true)
	{
		cout << "Learning dictionary of " << Na << " atoms of " << Npix << " pixels" << endl;
		cout << "Starting Dictionary Learning for " << IterationNumber << " iterations" << endl;
		cout << "Using OMP with SparsityTarget = "<< SparsityTarget << " and ErrorTarget = " << ErrorTarget << endl;
	}
	for (int i=0;i<IterationNumber;i++)
	{
		if (Verb == true)
		{
			if (IterationNumber > 1000)
			{
				if ((i+1)%(IterationNumber/10) == 1)
					cout << "DL iteration " << i+1 << " / " << IterationNumber << ", average sparsity " << average_sparsity << ", average error " << average_error << endl;
			}
			else cout << "DL iteration " << i+1 << " / " << IterationNumber << ", average sparsity " << average_sparsity << ", average error " << average_error << endl;
		}
		// sparse coding training sample in dictionary
		X = coder.omp(TabTrain,SparsityTarget,ErrorTarget,False);
		if (Verb == true)
		{
			// Computing average sparsity given sparse encoding coefficients
			average_sparsity = 0;
			for (int i=0;i<X.nx();i++)
				for (int j=0;j<X.ny();j++)
					if (X(i,j) !=0)
						average_sparsity++;
			average_sparsity = average_sparsity / (Ntrain);
		}

		// Computing sparse coefficients matrix pseudo inverse for dictionary update
		if (Verb == True)
		{
			cout << "Sparse coding complete, average sparsity " << average_sparsity << endl;
			cout << "Updating dictionary ..." << endl;
		}
		//		terminator.inv_mat_svd(X,Xpinv,minT);  previous method, too slow for large data
		// Transposing matrix X before computing its pseudoinverse
		for (int p=0;p<X.nx();p++)
			for (int q=0;q<X.ny();q++)
				Xt(q,p) = X(p,q);
		// Filling matrix armaXt with coefficients from Xt
		for (int p=0;p<Ntrain;p++)
			for (int q=0;q<Na;q++)
				armaXt(p,q) = Xt(p,q);
		//Chosing eigenvalue threhsold value
		minT = Ntrain*eps * sqrt(Xt.energy());
		// Computing pseudoinverse
		armaInv = pinv(armaXt,minT);
		for (int p=0;p<Na;p++)
			for (int q=0;q<Ntrain;q++)
				Xtpinv(p,q) = armaInv(p,q);

		// gsl_matrix_set(gslA,p,q,Xt(p,q));
		// gsl_matrix_free(gslA);
		// gslA = gsl_matrix_alloc(Ntrain,Na);
		// for (int p=0;p<Ntrain;p++)
		// 	for (int q=0;q<Na;q++)
		// 		gsl_matrix_set(gslA,p,q,Xt(p,q));
		// Computing SVD from A
		// gsl_linalg_SV_decomp (gslA,gslV,gslS,work);
		// gsl_linalg_SV_decomp_jacobi (gslA,gslV,gslS);
		// Thresholding/inverting eigenvalues
		// S.init(0);
		/*for (int q=0;q<Na;q++)
			if (gsl_vector_get(gslS,q)<minT)
				S(q,q) = 0;
			else
				S(q,q) = 1/(gsl_vector_get(gslS,q));
		// Reading remaining gsl matrices
		for (int p=0;p<Ntrain;p++)
			for (int q=0;q<Na;q++)
				At(q,p) = gsl_matrix_get(gslA,p,q);
		for (int p=0;p<Na;p++)
			for (int q=0;q<Na;q++)
				V(p,q) = gsl_matrix_get(gslV,p,q);
		// Computing pseudo inverse by multiplying matrices
		Xtpinv = mult(V,mult(S,At));*/

		// Applying MOD dictionary update
		for (int p=0;p<D.nx();p++)
			for (int q=0;q<D.ny();q++)
			{
				D(p,q) = 0;
				for (int k=0;k<Ntrain;k++)
					D(p,q) += TabTrain(p,k)*Xtpinv(q,k);
			}
		// Computing sparse approximation and average quadratic error
		average_error = 0;
		sparse_approx = mult(D,X);
		for (int k=0;k<Ntrain;k++)
		{
			for (int np=0;np<Npix;np++)
				sparse_sample(np) = sparse_approx(np,k) - TabTrain(np,k);
			average_error =+ sqrt(sparse_sample.energy())/Ntrain;
		}
		// Throwing away unused atoms and replacing them by random training samples
		atoms_replaced = 0;
		for (int m=0;m<Na;m++)
		{
			atom_usage = 0;
			for (int p=0;p<Ntrain;p++)
				if (X(m,p)!=0) atom_usage++;
			if (atom_usage == 0)
			{
				new_sample_ind	= gsl_rng_uniform_int(rng, Ntrain-1);
				for (int k=0;k<Npix;k++)
					Acurrent(k) = TabTrain(k,new_sample_ind);
				Amean = Acurrent.mean();
				for (int k=0;k<Npix;k++)
					Acurrent(k) = Acurrent(k) - Amean;
				Anorm = sqrt(Acurrent.energy()); // computing 0-mean atom norm
				if (Anorm > 1)
				{
					for (int k=0;k<Npix;k++)
						D(k,m) = Acurrent(k) / Anorm; // removing mean and normalizing
				}
				else
					for (int k=0;k<Npix;k++)
						D(k,m) = Acurrent(k); // only removing mean
				atoms_replaced++;
			}
		}
		if (Verb == True && atoms_replaced !=0)
			if (IterationNumber > 0)
				if ((i+1)%(IterationNumber/10) == 1) cout << "Replaced " << atoms_replaced << " unused atoms with random training samples" << endl;
				else
					cout << "Replaced " << atoms_replaced << " unused atoms with random training samples" << endl;

		// Normalizing atoms with norm > 1
		for (int m=0;m<Na;m++)
		{
			for (int p=0;p<Npix;p++)
				Acurrent(p) = D(p,m);
			Anorm = sqrt(Acurrent.energy());
			if (Anorm > 1)
				for (int p=0;p<Npix;p++)
					D(p,m) = D(p,m) / Anorm;
		}

		// Updating sparse coder with new version of dictionary
		coder.update_dictionary(D);
	}
	return D;
}


/*
 * <one line to give the library's name and an idea of what it does.>
 * Copyright (C) 2015  Guillaume L. <guillaume.lozenguez@mines-douai.fr>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "float2.h"
#include "tools.h"
#include <boost/concept_check.hpp>

using namespace mia;
using namespace std;

Float2 Float2 :: middle( const Float2 & A, const Float2 & B )
{
  return (A+B)*0.5f;
}

Float2 Float2 :: mean( const std::list<Float2> & lFloat2 )
{
    Float2 mean;
    int size(0);

    for( std::list<Float2>::const_iterator it= lFloat2.begin() ; it != lFloat2.end(); ++it ){
        mean+= *it;
        ++size;
    }
    return mean / (float)size;
}

std::array<Float2, 2> Float2::simpleLinearRegression( const std::list<Float2> & point )
{
    int size(0);
    Float2 mean(0.f, 0.f), origin(0.f, 0.f);

    for( std::list<Float2>::const_iterator it(point.begin()), itEnd(point.end()) ; it != itEnd ; ++it )
    {
        mean+= Float2(it->x, it->y);
        ++size;
    }
    mean/= (float)size;

    float slopeNum(0.f), yxSlopeDenum(0.f), xySlopeDenum(0.f);
    for( std::list<Float2>::const_iterator it(point.begin()), itEnd(point.end()) ; it != itEnd ; ++it )
    {
        float dx= (it->x-mean.x);
        float dy= (it->y-mean.y);

        slopeNum+= dx*dy;

        yxSlopeDenum+= dx*dx;
        xySlopeDenum+= dy*dy;

        ++size;
    }

    if( yxSlopeDenum == 0 && xySlopeDenum == 0 )
    {
        origin.x= mean.x;
        origin.y= 0.f;
    }
    else if( yxSlopeDenum*yxSlopeDenum > xySlopeDenum*xySlopeDenum )
    {
        float slope= slopeNum / yxSlopeDenum;
        origin.x= 0;
        origin.y= mean.y - slope * mean.x;
    }
    else
    {
        float slope= slopeNum / xySlopeDenum;
        origin.y= 0;
        origin.x= mean.x - slope * mean.y;
    }

    std::array<Float2, 2> descriptor= {mean, Float2(origin, mean)};
    descriptor[1].normalize();

    return descriptor;
}

std::array<Float2, 2> Float2::projectionSegment( const std::list<Float2> & point, const Float2 &mean, const Float2 &normDir )
{
    list<float> projection;
    for( list<Float2>::const_iterator it(point.begin()), itEnd(point.end()) ; it!=itEnd ; ++it )
        projection.push_back( dotProduct( (*it) - mean, normDir ) );
    projection.sort();

    array<Float2, 2> segment= { normDir * *(projection.begin()) + mean, normDir * *(projection.rbegin()) + mean };
    return segment;
}

bool Float2::validSegmentRegression( const std::list<Float2> & lFloat2, const std::array<Float2, 2> & normSegment, float treshold )
{
    Float2 normDir= normSegment[1].orthogonal();
    bool valid(true);

    for( list<Float2>::const_iterator it(lFloat2.begin()), itEnd(lFloat2.end()) ; valid && it!=itEnd ; ++it )
        valid= dotProduct( (*it) - normSegment[0], normDir ) < treshold;

    return valid;
}

list<Float2> Float2 :: polarSort(const list<Float2> & lFloat2)
{
    list< valued<Float2> > toSort;
    for(list<Float2>::const_iterator it = lFloat2.begin(); it != lFloat2.end() ; ++it )
        toSort.push_back( valued<Float2>( *it, it->angle() ) );
    toSort.sort();

    list<Float2> ret;
    for(list<valued<Float2>>::const_iterator it = toSort.begin(); it != toSort.end() ; ++it )
        ret.push_back( it->item );

    return ret;
}

Transform Transform::from_match ( std::list<std::pair<Float2, Float2>>::const_iterator itBegin,
                                     std::list<std::pair<Float2, Float2>>::const_iterator itEnd )
{ // Which translation / rotation to transfom second to first ?
    Transform t;
    t.translation= Float2(0.f, 0.f);
    t.center= Float2(0.f, 0.f);
    t.rotation= 0.f;
    int size(0);

    for( std::list<std::pair<Float2, Float2>>::const_iterator it= itBegin ; it != itEnd ; ++it )
    {// Get center :
        t.center+= it->first;
        t.translation+= it->second;
        ++size;
    }
    t.center/= (float)size; // Center first
    t.translation/= (float)size; // Center second
    t.translation= t.center - t.translation; // translation from second to first.

    // Rotation :
    for( std::list<std::pair<Float2, Float2>>::const_iterator it= itBegin ; it != itEnd ; ++it )
    {
        t.rotation+= angle( it->second + t.translation, t.center, it->first );
    }
    t.rotation= reduceRadian( t.rotation/(float)size );

    return t;
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include <rw/math/VectorND.hpp>

#include <Eigen/Core>
#include <gtest/gtest.h>

using namespace rw::math;

template< size_t N > VectorND< N > getVectorND (double value)
{
    VectorND< N > vec;
    for (size_t i = 0; i < vec.size (); i++) {
        vec[i] = value;
    }
    return vec;
}

template< size_t N > VectorND< N > getVectorND (bool countUp, bool Digit=true)
{
    VectorND< N > vec;
    for (size_t i = 0; i < vec.size (); i++) {
        if(countUp){
            vec[i] = i+1+ Digit*(i+1)%10/10.0;
        }else {
            vec[i] = i - vec.size() +Digit*(i+vec.size())%10/10.0 ;
        }
    }
    return vec;
}

TEST(VectorND,Constructor){
    VectorND< 3 > obj1(1,2,3);
    EXPECT_EQ(obj1[0],1);
    EXPECT_EQ(obj1[1],2);
    EXPECT_EQ(obj1[2],3);

    VectorND< 9 > obj2(1,2.1212142,long(3),4.0,float(5),6,double(7),8u,9);
    EXPECT_DOUBLE_EQ(obj2[0],1);
    EXPECT_DOUBLE_EQ(obj2[1],2.1212142);
    EXPECT_DOUBLE_EQ(obj2[2],3);
    EXPECT_DOUBLE_EQ(obj2[3],4);
    EXPECT_DOUBLE_EQ(obj2[4],5);
    EXPECT_DOUBLE_EQ(obj2[5],6);
    EXPECT_DOUBLE_EQ(obj2[6],7);
    EXPECT_DOUBLE_EQ(obj2[7],8);
    EXPECT_DOUBLE_EQ(obj2[8],9);
}

template< typename T > class VectorNDTest : public ::testing::Test
{};

using test_types = ::testing::Types<
    std::integral_constant< std::size_t, 2 >, std::integral_constant< std::size_t, 3 >,
    std::integral_constant< std::size_t, 4 >, std::integral_constant< std::size_t, 5 >,
    std::integral_constant< std::size_t, 6 >, std::integral_constant< std::size_t, 13 > >;

TYPED_TEST_CASE (VectorNDTest, test_types);

TYPED_TEST (VectorNDTest, MiscTest)
{
    static constexpr std::size_t N = TypeParam::value;
    const VectorND< N > v1 =getVectorND<N>(true,false);
    const VectorND< N > v2 (v1);
    const VectorND< N > v3 = v1 + v2;
    const VectorND< N > v4 = getVectorND<N>(true,false)*2;
    EXPECT_EQ ((v3 - v4).normInf (), 0);

    const VectorND< N > v5 = getVectorND<N>(true);
    const VectorND< N > v5_norm = normalize (v5);
    EXPECT_LT (fabs (v5_norm.norm2 () - 1), 1e-15);

    const double len = v5.norm2 ();
    EXPECT_LT (fabs (v5_norm (0) - v5 (0) / len), 1e-15);
    EXPECT_LT (fabs (v5_norm (1) - v5 (1) / len), 1e-15);
    (fabs (v5_norm (2) - v5 (2) / len) < 1e-15);

    VectorND< N > v6;
    v6 (0) = len;
    EXPECT_EQ (v6 (0), len);

    const VectorND< N, double > vd = getVectorND<N>(true);
    const VectorND< N, int > vi = cast< int > (vd);
    for(int i = 1; size_t (i) <= vi.size(); i++){
        EXPECT_EQ(vi(i-1), i);
    }
}

TYPED_TEST (VectorNDTest, scalarOperatorTest)
{
    static constexpr std::size_t N = TypeParam::value;
    VectorND< N > obj = getVectorND<N>(3.0);

    auto test1 = obj * 2;
    auto test2 = 2 * obj;
    auto test3 = obj / 2;
    auto test4 = 2 / obj;
    auto test5 = obj.elemAdd(2);
    auto test7 = obj.elemSubtract(2);
    auto test9 = obj;
    test9 *= 2;
    auto test10 = obj;
    test10 /= 2;
    auto test13 = -obj;

    for (size_t i = 0; i < obj.size (); i++) {
        EXPECT_DOUBLE_EQ (test1[i], 6.0);
        EXPECT_DOUBLE_EQ (test2[i], 6.0);
        EXPECT_DOUBLE_EQ (test3[i], 3.0 / 2.0);
        EXPECT_DOUBLE_EQ (test4[i], 2.0 / 3.0);
        EXPECT_DOUBLE_EQ (test5[i], 5.0);
        EXPECT_DOUBLE_EQ (test7[i], 1.0);
        EXPECT_DOUBLE_EQ (test9[i], 6.0);
        EXPECT_DOUBLE_EQ (test10[i], 3.0 / 2.0);
        EXPECT_DOUBLE_EQ (test13[i], -3.0);
    }
}

TYPED_TEST (VectorNDTest, VectorNDOperatorTest)
{
    static constexpr std::size_t N = TypeParam::value;
    VectorND< N > obj1 = getVectorND<N>(3.0);
    VectorND< N > obj2 = getVectorND<N>(2.0);

    auto test1 = obj1.elemMultiply(obj2);
    auto test2 = obj1.elemDivide(obj2);
    auto test3 = obj1 + obj2;
    auto test4 = obj1 - obj2;
    auto test7 = obj1;
    test7 += obj2;
    auto test8 = obj1;
    test8 -= obj2;

    for (size_t i = 0; i < obj1.size (); i++) {
        EXPECT_DOUBLE_EQ (test1[i], 6.0);
        EXPECT_DOUBLE_EQ (test2[i], 3.0 / 2.0);
        EXPECT_DOUBLE_EQ (test3[i], 5.0);
        EXPECT_DOUBLE_EQ (test4[i], 1.0);
        EXPECT_DOUBLE_EQ (test7[i], 5.0);
        EXPECT_DOUBLE_EQ (test8[i], 1.0);
    }
}

TYPED_TEST (VectorNDTest, EigenOperatorTest)
{
    static constexpr std::size_t N = TypeParam::value;
    VectorND< N > obj1             = getVectorND< N > (3.0);
    Eigen::Matrix< double, N, 1 > obj2;
    for (size_t i = 0; i < N; i++) {
        obj2[i] = 2.0;
    }

    auto test1  = obj1.elemMultiply(obj2);
    auto test2  = obj1.elemDivide(obj2);
    auto test3  = obj1 + obj2;
    auto test3x = obj2 + obj1;
    auto test4  = obj1 - obj2;
    auto test4x = obj2 - obj1;
    auto test7 = obj1;
    test7 += obj2;
    auto test8 = obj1;
    test8 -= obj2;

    for (size_t i = 0; i < obj1.size (); i++) {
        EXPECT_DOUBLE_EQ (test1[i], 6.0);
        EXPECT_DOUBLE_EQ (test2[i], 3.0 / 2.0);
        EXPECT_DOUBLE_EQ (test3[i], 5.0);
        EXPECT_DOUBLE_EQ (test3x[i], 5.0);
        EXPECT_DOUBLE_EQ (test4[i], 1.0);
        EXPECT_DOUBLE_EQ (test4x[i], -1.0);
        EXPECT_DOUBLE_EQ (test7[i], 5.0);
        EXPECT_DOUBLE_EQ (test8[i], 1.0);
    }

    EXPECT_TRUE (obj1 != obj2);
    EXPECT_TRUE (obj2 != obj1);
    EXPECT_FALSE (obj1 == obj2);
    EXPECT_FALSE (obj2 == obj1);
    obj2 = obj1.e();
    EXPECT_TRUE (obj1 == obj2);
    EXPECT_TRUE (obj2 == obj1);
    EXPECT_FALSE (obj1 != obj2);
    EXPECT_FALSE (obj2 != obj1);
}

TYPED_TEST (VectorNDTest, ComparisonTest)
{
    static constexpr std::size_t N = TypeParam::value;
    const VectorND< N, double > comp1 (getVectorND<N>(true));
    auto comp2 = comp1;
    auto comp3 = -comp1;
    EXPECT_TRUE (comp1 == comp2);
    EXPECT_FALSE (comp1 != comp2);
    EXPECT_TRUE (comp1 != comp3);
    EXPECT_FALSE (comp1 == comp3);
}

TYPED_TEST (VectorNDTest, MathOperators)
{
    static constexpr std::size_t N = TypeParam::value;
    VectorND< N > obj1 = getVectorND<N>(true);
    VectorND< N > obj2 = getVectorND<N>(false);

    auto test2 = obj1.dot (obj2);
    auto test3 = obj1.normalize ();

    EXPECT_EQ (obj1.e ().dot (obj2.e ()), test2);
    EXPECT_DOUBLE_EQ (test3.norm2 (), 1.0);
    for(size_t i = 0; i < N; i++){
        EXPECT_DOUBLE_EQ((test3 * obj1.norm2 ())[i],obj1[i]);
    }
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example that shows how you can perform model selection with the
    dlib C++ Library.

    It will create a simple dataset and show you how to use cross validation and
    global optimization to determine good parameters for the purpose of training
    an svm to classify the data.

    The data used in this example will be 2 dimensional data and will come from a
    distribution where points with a distance less than 10 from the origin are
    labeled +1 and all other points are labeled as -1.

    As an side, you should probably read the svm_ex.cpp and matrix_ex.cpp example
    programs before you read this one.
*/

#include <iostream>
#include <dlib/svm.h>
#include <dlib/global_optimization.h>

using namespace std;
using namespace dlib;

int main() try
{
    // The svm functions use column vectors to contain a lot of the data on which they
    // operate. So the first thing we do here is declare a convenient typedef.

    // This typedef declares a matrix with 2 rows and 1 column.  It will be the
    // object that contains each of our 2 dimensional samples.
    typedef matrix<double, 2, 1> sample_type;

    // Now we make objects to contain our samples and their respective labels.
    std::vector<sample_type> samples;
    std::vector<double> labels;

    // Now let's put some data into our samples and labels objects.  We do this
    // by looping over a bunch of points and labeling them according to their
    // distance from the origin.
    for (double r = -20; r <= 20; r += 0.8)
    {
        for (double c = -20; c <= 20; c += 0.8)
        {
            sample_type samp;
            samp(0) = r;
            samp(1) = c;
            samples.push_back(samp);

            // if this point is less than 10 from the origin
            if (sqrt(r*r + c*c) <= 10)
                labels.push_back(+1);
            else
                labels.push_back(-1);
        }
    }

    cout << "Generated " << samples.size() << " points" << endl;

    // Here we normalize all the samples by subtracting their mean and dividing by their
    // standard deviation.  This is generally a good idea since it often heads off
    // numerical stability problems and also prevents one large feature from smothering
    // others.  Doing this doesn't matter much in this example so I'm just doing this here
    // so you can see an easy way to accomplish this with the library.
    vector_normalizer<sample_type> normalizer;
    // let the normalizer learn the mean and standard deviation of the samples
    normalizer.train(samples);
    // now normalize each sample
    for (unsigned long i = 0; i < samples.size(); ++i)
        samples[i] = normalizer(samples[i]);

    // Now that we have some data we want to train on it.  We are going to train a
    // binary SVM with the RBF kernel to classify the data.  However, there are
    // three parameters to the training.  These are the SVM C parameters for each
    // class and the RBF kernel's gamma parameter.  Our choice for these
    // parameters will influence how good the resulting decision function is.  To
    // test how good a particular choice of these parameters is we can use the
    // cross_validate_trainer() function to perform n-fold cross validation on our
    // training data.  However, there is a problem with the way we have sampled
    // our distribution above.  The problem is that there is a definite ordering
    // to the samples.  That is, the first half of the samples look like they are
    // from a different distribution than the second half.  This would screw up
    // the cross validation process, but we can fix it by randomizing the order of
    // the samples with the following function call.
    randomize_samples(samples, labels);

    // And now we get to the important bit.  Here we define a function,
    // cross_validation_score(), that will do the cross-validation we
    // mentioned and return a number indicating how good a particular setting
    // of gamma, c1, and c2 is.
    auto cross_validation_score = [&](const double gamma, const double c1, const double c2)
    {
        // Make a RBF SVM trainer and tell it what the parameters are supposed to be.
        typedef radial_basis_kernel<sample_type> kernel_type;
        svm_c_trainer<kernel_type> trainer;
        trainer.set_kernel(kernel_type(gamma));
        trainer.set_c_class1(c1);
        trainer.set_c_class2(c2);

        // Finally, perform 10-fold cross validation and then print and return the results.
        matrix<double> result = cross_validate_trainer(trainer, samples, labels, 10);
        cout << "gamma: " << setw(11) << gamma << "  c1: " << setw(11) << c1 <<  "  c2: " << setw(11) << c2 <<  "  cross validation accuracy: " << result;

        // Now return a number indicating how good the parameters are.  Bigger is
        // better in this example.  Here I'm returning the harmonic mean between the
        // accuracies of each class.  However, you could do something else.  For
        // example, you might care a lot more about correctly predicting the +1 class,
        // so you could penalize results that didn't obtain a high accuracy on that
        // class.  You might do this by using something like a weighted version of the
        // F1-score (see http://en.wikipedia.org/wiki/F1_score).
        return 2*prod(result)/sum(result);
    };

    // And finally, we call this global optimizer that will search for the best parameters.
    // It will call cross_validation_score() 30 times with different settings and return
    // the best parameter setting it finds.  find_max_global() uses a global optimization
    // method based on a combination of non-parametric global function modeling and
    // quadratic trust region modeling to efficiently find a global maximizer.  It usually
    // does a good job with a relatively small number of calls to cross_validation_score().
    // In this example, you should observe that it finds settings that give perfect binary
    // classification of the data.
    auto result = find_max_global(cross_validation_score,
                                  {1e-5, 1e-5, 1e-5},  // lower bound constraints on gamma, c1, and c2, respectively
                                  {100,  1e6,  1e6},   // upper bound constraints on gamma, c1, and c2, respectively
                                  max_function_calls(30));

    double best_gamma = result.x(0);
    double best_c1    = result.x(1);
    double best_c2    = result.x(2);

    cout << " best cross-validation score: " << result.y << endl;
    cout << " best gamma: " << best_gamma << "   best c1: " << best_c1 << "    best c2: "<< best_c2  << endl;
}
catch (exception& e)
{
    cout << e.what() << endl;
}


#pragma once
#include <Eigen/Dense>

inline void make_first_positive(Eigen::VectorXi& v)
{
	for(uint32_t n = 0; n < v.size(); ++ n)
	{
		if(v(n) == 0)
			continue;
		if(v(n) < 0)
		{
			v *= -1;
			return ;
		}
		else
			return ;
	}
}

struct hash_vector
{
	size_t operator()(const Eigen::VectorXi& v) const
	{
		std::size_t seed = v.size();
		for(uint32_t n = 0; n < v.size(); ++ n) {
			seed ^= abs(v(n)) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
		}
		return seed;
	}
};

int powi(int base, unsigned int exp)
{
    int res = 1;
    while (exp) {
        if (exp & 1)
            res *= base;
        exp >>= 1;
        base *= base;
    }
    return res;
}

template<typename UINT, template<typename> class Basis>
Eigen::MatrixXd basisMatrix(const Basis<UINT>& basis)
{
	Eigen::MatrixXd res = Eigen::MatrixXd::Zero(1u << basis.getN(), basis.getDim());
	for(unsigned int n = 0; n < basis.getDim(); ++n)
	{
		auto bvec = basis.basisVec(n);
		for(const auto p: bvec)
		{
			res(p.first, n) = p.second;
		}
	}
	return res;
}

template<typename Basis>
Eigen::VectorXd flip(Basis&& basis, const Eigen::VectorXd& r)
{
	Eigen::VectorXd res(r.size());
	for(int i = 0; i < r.size(); i++)
	{
		res(basis.flip(i)) = r(i);
	}
	return res;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the krls object
    from the dlib C++ Library.

    The krls object allows you to perform online regression.  This
    example will use the krls object to perform filtering of a signal
    corrupted by uniformly distributed noise.
*/

#include <iostream>

#include <dlib/svm.h>
#include <dlib/rand.h>

using namespace std;
using namespace dlib;

// Here is the function we will be trying to learn with the krls
// object.
double sinc(double x)
{
    if (x == 0)
        return 1;

    // also add in x just to make this function a little more complex
    return sin(x)/x + x;
}

int main()
{
    // Here we declare that our samples will be 1 dimensional column vectors.  The reason for
    // using a matrix here is that in general you can use N dimensional vectors as inputs to the
    // krls object.  But here we only have 1 dimension to make the example simple.
    typedef matrix<double,1,1> sample_type;

    // Now we are making a typedef for the kind of kernel we want to use.  I picked the
    // radial basis kernel because it only has one parameter and generally gives good
    // results without much fiddling.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Here we declare an instance of the krls object.  The first argument to the constructor
    // is the kernel we wish to use.  The second is a parameter that determines the numerical
    // accuracy with which the object will perform part of the regression algorithm.  Generally
    // smaller values give better results but cause the algorithm to run slower (because it tries
    // to use more "dictionary vectors" to represent the function it is learning.
    // You just have to play with it to decide what balance of speed and accuracy is right
    // for your problem.  Here we have set it to 0.001.
    //
    // The last argument is the maximum number of dictionary vectors the algorithm is allowed
    // to use.  The default value for this field is 1,000,000 which is large enough that you
    // won't ever hit it in practice.  However, here we have set it to the much smaller value
    // of 7.  This means that once the krls object accumulates 7 dictionary vectors it will
    // start discarding old ones in favor of new ones as it goes through the training process.
    // In other words, the algorithm "forgets" about old training data and focuses on recent
    // training samples. So the bigger the maximum dictionary size the longer its memory will
    // be.  But in this example program we are doing filtering so we only care about the most
    // recent data.  So using a small value is appropriate here since it will result in much
    // faster filtering and won't introduce much error.
    krls<kernel_type> test(kernel_type(0.05),0.001,7);

    dlib::rand rnd;

    // Now let's loop over a big range of values from the sinc() function.  Each time
    // adding some random noise to the data we send to the krls object for training.
    sample_type m;
    double mse_noise = 0;
    double mse = 0;
    double count = 0;
    for (double x = -20; x <= 20; x += 0.01)
    {
        m(0) = x;
        // get a random number between -0.5 and 0.5
        const double noise = rnd.get_random_double()-0.5;

        // train on this new sample
        test.train(m, sinc(x)+noise);

        // once we have seen a bit of data start measuring the mean squared prediction error.
        // Also measure the mean squared error due to the noise.
        if (x > -19)
        {
            ++count;
            mse += pow(sinc(x) - test(m),2);
            mse_noise += pow(noise,2);
        }
    }

    mse /= count;
    mse_noise /= count;

    // Output the ratio of the error from the noise and the mean squared prediction error.
    cout << "prediction error:                   " << mse << endl;
    cout << "noise:                              " << mse_noise << endl;
    cout << "ratio of noise to prediction error: " << mse_noise/mse << endl;

    // When the program runs it should print the following:
    //    prediction error:                   0.00735201
    //    noise:                              0.0821628
    //    ratio of noise to prediction error: 11.1756

    // And we see that the noise has been significantly reduced by filtering the points
    // through the krls object.

}


#include <ctime>
#include <vector>
#include <math.h>

#include <Eigen/Dense>
#include <Eigen/Core>

#include "mex.h"
#include "nlopt.hpp"

int counter = 0;

using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;

struct mFuncData
{
	int numDims, numSamples;
	MatrixXd X, Y;
	MatrixXd XX, YX;
};

double myfunc(unsigned n, const double *inW, double *grad, void *inData)
{
	++counter;

	mFuncData* data = (mFuncData*)inData;
	MatrixXd W = Map<MatrixXd>((double*)inW, data->numDims, data->numDims);

	// 2 * W*(X*X') - Y*X' - Y*X';
	if (grad)
	{
		clock_t beginM = clock();

		mwSize size[2]; size[0] = data->numDims;  size[1] = data->numDims;
		mxArray* prhs[2];
		prhs[0] = mxCreateNumericArray(2, size, mxDOUBLE_CLASS, mxREAL);
		double* WMatlab = (double*)mxGetData(prhs[0]);
		Map<MatrixXd>(WMatlab, data->numDims, data->numDims) = W;
		prhs[1] = mxCreateNumericArray(2, size, mxDOUBLE_CLASS, mxREAL);
		double* XMatlab = (double*)mxGetData(prhs[1]);
		Map<MatrixXd>(XMatlab, data->numDims, data->numDims) = data->XX;

		mxArray* plhs[1];
		int output = mexCallMATLAB(1, plhs, 2, prhs, "mtimes");
		double* res = (double*)mxGetData(plhs[0]);
		MatrixXd newW = Map<MatrixXd>(res, data->numDims, data->numDims);
		MatrixXd gradient = 2.0*newW - data->YX - data->YX;

		Map<MatrixXd>(grad, data->numDims, data->numDims) = gradient;

		clock_t endM = clock();
		// mexPrintf("time step newgrad: %f\n", difftime(endM, beginM));
	}

	// val = norm(W*X - Y);
	clock_t begin1 = clock();
	MatrixXd res = W*data->X - data->Y;
	clock_t end1 = clock();
	//mexPrintf("time step main: %f\n", difftime(end1, begin1));

	// Matlab call
	clock_t beginM = clock();
	mxArray* prhs[1];
	mwSize size[2]; size[0] = data->numDims;  size[1] = data->numSamples;
	prhs[0] = mxCreateNumericArray(2, size, mxDOUBLE_CLASS, mxREAL);
	double* resMatlab = (double*)mxGetData(prhs[0]);
	Map<MatrixXd>(resMatlab, data->numDims, data->numSamples) = res;
	mxArray* plhs[1];
	mexCallMATLAB(1, plhs, 1, prhs, "norm");
	double norm = (double)mxGetScalar(plhs[0]);
	clock_t endM = clock();
	// mexPrintf("time step norm: %f\n", difftime(endM, beginM));
	mexPrintf("[iter %d] energy: %f\n", counter, norm);

	/*
	clock_t begin2 = clock();
	double energy = res.operatorNorm();
	clock_t end2 = clock();
	mexPrintf("time step norm: %f\n", difftime(end2, begin2));
	mexPrintf("[iter %d] energy: %f\n", counter, energy);
	return energy;
	*/

	return norm;
}

typedef struct
{
	double a, b;
} mConstraintData;

double myconstraint(unsigned n, const double *x, double *grad, void *data)
{
	mConstraintData *d = (mConstraintData *)data;
	double a = d->a, b = d->b;
	if (grad) {
		grad[0] = 3 * a * (a*x[0] + b) * (a*x[0] + b);
		grad[1] = -1.0;
	}
	return ((a*x[0] + b) * (a*x[0] + b) * (a*x[0] + b) - x[1]);
}

void run_optimiser(double* x, double* residual, int numDims, int numSamples, double* srcCentres, double* tgtCentres)
{
	nlopt_opt opt;

	opt = nlopt_create(NLOPT_LD_MMA, numDims*numDims); /* algorithm and dimensionality */
	// opt = nlopt_create(NLOPT_LN_COBYLA, numDims*numDims); /* algorithm and dimensionality */

	mFuncData funcData;
	funcData.numDims = numDims;
	funcData.numSamples = numSamples;
	funcData.X = Map<MatrixXd>(srcCentres, numDims, numSamples);
	funcData.Y = Map<MatrixXd>(tgtCentres, numDims, numSamples);

	// Precomputation
	funcData.XX = funcData.X * funcData.X.transpose();
	funcData.YX = funcData.Y * funcData.X.transpose();

	nlopt_set_min_objective(opt, myfunc, &funcData);
	nlopt_set_xtol_rel(opt, 1e-4);
	nlopt_set_maxeval(opt, 25);

	// Constraints:
	//mConstraintData data[2] = { { 2, 0 }, { -1, 1 } };
	//nlopt_add_inequality_constraint(opt, myconstraint, &data[0], 1e-4);
	//nlopt_add_inequality_constraint(opt, myconstraint, &data[1], 1e-4);

	if (nlopt_optimize(opt, x, residual) < 0)
		printf("nlopt failed!\n");
	else
		printf("found minimum after %d evaluations with residual %f\n", counter, *residual);

	nlopt_destroy(opt);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	/* Macros for the input arguments */
	#define srcCentres_IN prhs[0]
	#define tgtCentres_IN prhs[1]

	/* Macros for the output arguments */
	#define x_OUT plhs[0]
	#define residual_OUT plhs[1]

	/* Check correctness of input/output arguments */
	if (nrhs < 2 || nrhs > 2)
		mexErrMsgTxt("Wrong number of input arguments.");
	else if (nlhs > 2)
		mexErrMsgTxt("Too many output arguments.");

	/* Get input data */
	double* srcCentres = (double*)mxGetData(srcCentres_IN);
	double* tgtCentres = (double*)mxGetData(tgtCentres_IN);
	int numDims = (int)mxGetM(srcCentres_IN);
	int numSamples = (int)mxGetN(srcCentres_IN);

	/* Create output data */
	x_OUT = mxCreateNumericMatrix(numDims, numDims, mxDOUBLE_CLASS, mxREAL);
	double* x = (double*)mxGetData(x_OUT);
	residual_OUT = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL);
	double* residual = (double*)mxGetData(residual_OUT);

	// Initial guess (Id matrix)
	for (int idx = 0; idx < numDims*numDims; ++idx)
	{
		if (idx % (numDims + 1) == 0)
			x[idx] = 1.0;
		else
			x[idx] = 0.0;
	}

	// Reset counter
	counter = 0;

	/* Call method */
	run_optimiser(x, residual, numDims, numSamples, srcCentres, tgtCentres);

	return;
}


#ifndef MLT_UTILS_LINEAR_SOLVERS_HPP
#define MLT_UTILS_LINEAR_SOLVERS_HPP

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/SVD>

#include "../defs.hpp"

namespace mlt {
namespace utils {
namespace linear_solvers {
	template <typename ConcreteSolver, typename SolverImplementation>
	class BaseSolver {
	public:
		ConcreteSolver& compute(MatrixXdRef A) {
			_solver.compute(A);
			return static_cast<ConcreteSolver&>(*this);
		}

		auto solve(MatrixXdRef B) const {
			return MatrixXd{_solver.solve(B)};
		}

	protected:
		BaseSolver() = default;

		SolverImplementation _solver;
	};

	class CGSolver : public BaseSolver<CGSolver, ConjugateGradient<MatrixXd>> {};

	class LLTSolver : public BaseSolver<LLTSolver, LLT<MatrixXd>> {};

	class LDLTSolver : public BaseSolver<LDLTSolver, LDLT<MatrixXd>> {};

	class SVDSolver : public BaseSolver<SVDSolver, JacobiSVD<MatrixXd>>	{
	public:
		SVDSolver& compute(MatrixXdRef A) {
			_solver.compute(A, ComputeThinU | ComputeThinV);
			return *this;
		}
	};
}
}
}
#endif