//=======================================================================
// Copyright 2008
// Author: Matyas W Egyhazy
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <iostream>
#include <vector>
#include <fstream>
#include <set>
#include <ctime>

#include <boost/assert.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#include <boost/timer.hpp>
#include <boost/integer_traits.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/simple_point.hpp>
#include <boost/graph/metric_tsp_approx.hpp>
#include <boost/graph/graphviz.hpp>

// TODO: Integrate this into the test system a little better. We need to run
// the test with some kind of input file.

template<typename PointType>
struct cmpPnt
{
    bool operator()(const boost::simple_point<PointType>& l,
                    const boost::simple_point<PointType>& r) const
    { return (l.x > r.x); }
};

//add edges to the graph (for each node connect it to all other nodes)
template<typename VertexListGraph, typename PointContainer,
    typename WeightMap, typename VertexIndexMap>
void connectAllEuclidean(VertexListGraph& g,
                        const PointContainer& points,
                        WeightMap wmap,            // Property maps passed by value
                        VertexIndexMap vmap,       // Property maps passed by value
                        int /*sz*/)
{
    using namespace boost;
    using namespace std;
    typedef typename graph_traits<VertexListGraph>::edge_descriptor Edge;
    typedef typename graph_traits<VertexListGraph>::vertex_iterator VItr;

    Edge e;
    bool inserted;

    pair<VItr, VItr> verts(vertices(g));
    for (VItr src(verts.first); src != verts.second; src++)
    {
        for (VItr dest(src); dest != verts.second; dest++)
        {
            if (dest != src)
            {
                double weight(sqrt(pow(
                    static_cast<double>(points[vmap[*src]].x -
                        points[vmap[*dest]].x), 2.0) +
                    pow(static_cast<double>(points[vmap[*dest]].y -
                        points[vmap[*src]].y), 2.0)));

                boost::tie(e, inserted) = add_edge(*src, *dest, g);

                wmap[e] = weight;
            }

        }

    }
}

// Create a randomly generated point
// scatter time execution
void testScalability(unsigned numpts)
{
    using namespace boost;
    using namespace std;

    typedef adjacency_matrix<undirectedS, no_property,
        property <edge_weight_t, double,
        property<edge_index_t, int> > > Graph;
    typedef graph_traits<Graph>::vertex_descriptor Vertex;
    typedef graph_traits <Graph>::edge_descriptor Edge;
    typedef property_map<Graph, edge_weight_t>::type WeightMap;
    typedef set<simple_point<double>, cmpPnt<double> > PointSet;
    typedef vector< Vertex > Container;

    boost::mt19937 rng(time(0));
    uniform_real<> range(0.01, (numpts * 2));
    variate_generator<boost::mt19937&, uniform_real<> >
        pnt_gen(rng, range);

    PointSet points;
    simple_point<double> pnt;

    while (points.size() < numpts)
    {
        pnt.x = pnt_gen();
        pnt.y = pnt_gen();
        points.insert(pnt);
    }

    Graph g(numpts);
    WeightMap weight_map(get(edge_weight, g));
    vector<simple_point<double> > point_vec(points.begin(), points.end());

    connectAllEuclidean(g, point_vec, weight_map, get(vertex_index, g), numpts);

    Container c;
    timer t;
    double len = 0.0;

    // Run the TSP approx, creating the visitor on the fly.
    metric_tsp_approx(g, make_tsp_tour_len_visitor(g, back_inserter(c), len, weight_map));

    cout << "Number of points: " << num_vertices(g) << endl;
    cout << "Number of edges: " << num_edges(g) << endl;
    cout << "Length of tour: " << len << endl;
    cout << "Elapsed: " << t.elapsed() << endl;
}

template <typename PositionVec>
void checkAdjList(PositionVec v)
{
    using namespace std;
    using namespace boost;

    typedef adjacency_list<listS, listS, undirectedS> Graph;
    typedef graph_traits<Graph>::vertex_descriptor Vertex;
    typedef graph_traits <Graph>::edge_descriptor Edge;
    typedef vector<Vertex> Container;
    typedef map<Vertex, std::size_t> VertexIndexMap;
    typedef map<Edge, double> EdgeWeightMap;
    typedef associative_property_map<VertexIndexMap> VPropertyMap;
    typedef associative_property_map<EdgeWeightMap> EWeightPropertyMap;
    typedef graph_traits<Graph>::vertex_iterator VItr;

    Container c;
    EdgeWeightMap w_map;
    VertexIndexMap v_map;
    VPropertyMap v_pmap(v_map);
    EWeightPropertyMap w_pmap(w_map);

    Graph g(v.size());

    //create vertex index map
    VItr vi, ve;
    int idx(0);
    for (boost::tie(vi, ve) = vertices(g); vi != ve; ++vi)
    {
        Vertex v(*vi);
        v_pmap[v] = idx;
        idx++;
    }

    connectAllEuclidean(g, v, w_pmap,
        v_pmap, v.size());

    metric_tsp_approx_from_vertex(g,
        *vertices(g).first,
        w_pmap,
        v_pmap,
        tsp_tour_visitor<back_insert_iterator<Container > >
        (back_inserter(c)));

    cout << "adj_list" << endl;
    for (Container::iterator itr = c.begin(); itr != c.end(); ++itr) {
        cout << v_map[*itr] << " ";
    }
    cout << endl << endl;

    c.clear();
}

static void usage()
{
    using namespace std;
    cerr << "To run this program properly please place a "
         << "file called graph.txt"
         << endl << "into the current working directory." << endl
         << "Each line of this file should be a coordinate specifying the"
         << endl << "location of a vertex" << endl
         << "For example: " << endl << "1,2" << endl << "20,4" << endl
         << "15,7" << endl << endl;
}

int main(int argc, char* argv[])
{
   using namespace boost;
   using namespace std;

    typedef vector<simple_point<double> > PositionVec;
    typedef adjacency_matrix<undirectedS, no_property,
        property <edge_weight_t, double> > Graph;
    typedef graph_traits<Graph>::vertex_descriptor Vertex;
    typedef graph_traits <Graph>::edge_descriptor Edge;
    typedef vector<Vertex> Container;
    typedef property_map<Graph, edge_weight_t>::type WeightMap;
    typedef property_map<Graph, vertex_index_t>::type VertexMap;

    // Make sure that the the we can parse the given file.
    if(argc < 2) {
        usage();
        // return -1;
        return 0;
    }

    // Open the graph file, failing if one isn't given on the command line.
    ifstream fin(argv[1]);
    if (!fin)
    {
        usage();
        // return -1;
        return 0;
    }

   string line;
   PositionVec position_vec;

   int n(0);
   while (getline(fin, line))
   {
       simple_point<double> vertex;

       size_t idx(line.find(","));
       string xStr(line.substr(0, idx));
       string yStr(line.substr(idx + 1, line.size() - idx));

       vertex.x = lexical_cast<double>(xStr);
       vertex.y = lexical_cast<double>(yStr);

       position_vec.push_back(vertex);
       n++;
   }

   fin.close();

   Container c;
   Graph g(position_vec.size());
   WeightMap weight_map(get(edge_weight, g));
   VertexMap v_map = get(vertex_index, g);

   connectAllEuclidean(g, position_vec, weight_map, v_map, n);

   metric_tsp_approx_tour(g, back_inserter(c));

   for (vector<Vertex>::iterator itr = c.begin(); itr != c.end(); ++itr)
   {
       cout << *itr << " ";
   }
   cout << endl << endl;

   c.clear();

   checkAdjList(position_vec);

   metric_tsp_approx_from_vertex(g, *vertices(g).first,
       get(edge_weight, g), get(vertex_index, g),
       tsp_tour_visitor<back_insert_iterator<vector<Vertex> > >
       (back_inserter(c)));

   for (vector<Vertex>::iterator itr = c.begin(); itr != c.end(); ++itr)
   {
       cout << *itr << " ";
   }
   cout << endl << endl;

   c.clear();

   double len(0.0);
   try {
       metric_tsp_approx(g, make_tsp_tour_len_visitor(g, back_inserter(c), len, weight_map));
   }
   catch (const bad_graph& e) {
       cerr << "bad_graph: " << e.what() << endl;
       return -1;
   }

   cout << "Number of points: " << num_vertices(g) << endl;
   cout << "Number of edges: " << num_edges(g) << endl;
   cout << "Length of Tour: " << len << endl;

   int cnt(0);
   pair<Vertex,Vertex> triangleEdge;
   for (vector<Vertex>::iterator itr = c.begin(); itr != c.end();
       ++itr, ++cnt)
   {
       cout << *itr << " ";

       if (cnt == 2)
       {
           triangleEdge.first = *itr;
       }
       if (cnt == 3)
       {
           triangleEdge.second = *itr;
       }
   }
   cout << endl << endl;
   c.clear();

   testScalability(1000);

   // if the graph is not fully connected then some of the
   // assumed triangle-inequality edges may not exist
   remove_edge(edge(triangleEdge.first, triangleEdge.second, g).first, g);

    // Make sure that we can actually trap incomplete graphs.
    bool caught = false;
    try {
        double len = 0.0;
        metric_tsp_approx(g, make_tsp_tour_len_visitor(g, back_inserter(c), len, weight_map));
    }
    catch (const bad_graph& e) { caught = true; }
    BOOST_ASSERT(caught);

   return 0;
}


#pragma once

#include "GaussianDistribution.hpp"

#include <Eigen/Dense>
#include <Eigen/Cholesky>

#include <array>

namespace icarus {
    template<typename T, size_t N>
    struct MerweScaledSigmaPoints
    {
        explicit MerweScaledSigmaPoints(T alpha, T beta = 2, T kappa = 3 - T(N))
        {
            mLambda = alpha * alpha * (T(N) + kappa) - T(N);
            mFirstMeanWeight = mLambda / (T(N) + mLambda);
            mCovarianceWeight = mFirstMeanWeight + 1 - alpha * alpha + beta;
            mNextWeights = T(1) / (2 * (T(N) + mLambda));
        }

        static constexpr size_t size()
        {
            return 2 * N + 1;
        }

        std::array<Eigen::Matrix<T, N, 1>, size()> operator()(GaussianDistribution<T, N> const & distribution) const
        {
            Eigen::Matrix<T, N, N> offsets = ((T(N) + mLambda) * distribution.covariance).llt().matrixL();

            std::array<Eigen::Matrix<T, N, 1>, size()> ret;
            ret[0] = distribution.mean;

            for (int i = 0; i < N; ++i) {
                ret[1 + 2 * i + 0] = distribution.mean + offsets.col(i);
                ret[1 + 2 * i + 1] = distribution.mean - offsets.col(i);
            }

            return ret;
        }

        template<int M>
        GaussianDistribution<T, M> unscentedTransform(std::array<Eigen::Matrix<T, M, 1>, 2 * N + 1> const & points) const
        {
            GaussianDistribution<T, M> ret;

            ret.mean = mFirstMeanWeight * points[0];

            for (int i = 1; i < 2 * N + 1; ++i) {
                ret.mean += mNextWeights * points[i];
            }

            auto difference = (points[0] - ret.mean).eval();
            ret.covariance.template triangularView<Eigen::Lower>() = mCovarianceWeight * difference * difference.transpose();

            for (int i = 1; i < 2 * N + 1; ++i) {
                difference = (points[i] - ret.mean).eval();
                ret.covariance.template selfadjointView<Eigen::Lower>().rankUpdate(difference, mNextWeights);
            }

            return ret;
        }

        T covarianceWeight(size_t index) const
        {
            if (index == 0) {
                return mCovarianceWeight;
            } else {
                return mNextWeights;
            }
        }
    private:
        T mLambda;
        T mFirstMeanWeight;
        T mCovarianceWeight;
        T mNextWeights;
    };
}


#include <random>
#include <limits>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "algorithms/math/least_common_multiple.hpp"

BOOST_AUTO_TEST_SUITE(TestLeastCommonMultiple)

BOOST_AUTO_TEST_CASE(numbers) {
    BOOST_CHECK(Algo::Math::LeastCommonMultiple::Find(1, 1) == 1);
    BOOST_CHECK(Algo::Math::LeastCommonMultiple::Find(5, 1) == 5);
    BOOST_CHECK(Algo::Math::LeastCommonMultiple::Find(5, 15) == 15);
    BOOST_CHECK(Algo::Math::LeastCommonMultiple::Find(6, 8) == 24);
    BOOST_CHECK(Algo::Math::LeastCommonMultiple::Find(761457, 614573) == 467970912861);
}

BOOST_AUTO_TEST_CASE(random_numbers) {
    std::random_device rd;
    std::mt19937 gen(rd());

    std::uniform_int_distribution<int> valuesDist(1, 100);
    for (int i = 0; i < 100; ++i) {
        const uint64_t first = static_cast<uint64_t>(valuesDist(gen));
        const uint64_t second = static_cast<uint64_t>(valuesDist(gen));

        const auto result = Algo::Math::LeastCommonMultiple::Find(first, second);
        const auto resultNaive =
                Algo::Math::LeastCommonMultiple::FindNaive(first, second);

        if (result != resultNaive)
        {
            std::cout << first << " and " << second << ": " <<
                         result << " vs " << resultNaive << std::endl;
            BOOST_FAIL("Different results!");
            return;
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Sebastian Schlenkrich

*/

/*! \file integratorsT.hpp
    \brief provide template functions for numerical integration
*/

#ifndef quantlib_templateintegrators_hpp
#define quantlib_templateintegrators_hpp

#include <boost/function.hpp>
//#include <boost/math/special_functions/erf.hpp>
//#include <ql/experimental/template/auxilliaries/MinimADVariable2.hpp>

namespace TemplateAuxilliaries {

    // evaluate \int_a^b v(t) f(t) dt = \sum v_i [F(t_i) - F(t_i-1)] with
    // v(t) piece-wise left-constant,
    // F'(t) = f(t)
    template <typename PassiveType, typename ActiveType, typename FuncType>
    class PieceWiseConstantIntegral {
    private:
        std::vector<PassiveType> t_;
        std::vector<ActiveType> v_;
        FuncType F_;
    public:
        PieceWiseConstantIntegral(const std::vector<PassiveType>& t, const std::vector<ActiveType>& v, const FuncType& F) : t_(t), v_(v), F_(F) {}
        ActiveType operator()(PassiveType startTime, PassiveType endTime) {
            int sgn = 1;
            if (startTime>endTime) {  // we want to ensure startTime <= endTime
                PassiveType t = startTime;
                startTime = endTime;
                endTime = t;
                sgn = -1;
            }
            // organising indices
            size_t idx_min  = 0;
            size_t idx_max  = std::min(t_.size(),v_.size())-1;
            size_t idx_last = idx_max;
            // enforce a < t_min <= t_max < b or special treatment
            while ((startTime>=t_[idx_min])&&(idx_min<idx_last)) ++idx_min;
            while ((endTime  <=t_[idx_max])&&(idx_max>0       )) --idx_max;
            ActiveType tmp = sgn * ( F_(endTime) - F_(startTime) );
            if (endTime<=t_[0])    return v_[0]        * tmp;  // short end
            if (idx_min==idx_last) return v_[idx_last] * tmp;  // long end
            if (idx_min> idx_max)  return v_[idx_min]  * tmp;  // integration within grid interval
            // integral a ... x_min
            tmp = v_[idx_min] * ( F_(t_[idx_min]) - F_(startTime) );
            // integral x_min ... x_max
            for (size_t i=idx_min; i<idx_max; ++i) tmp += v_[i+1] * ( F_(t_[i+1]) - F_(t_[i]) );
            // integral x_max ... b
            if (idx_max<idx_last) tmp += v_[idx_max+1] * ( F_(endTime) - F_(t_[idx_max]) );
            else                  tmp += v_[idx_max]   * ( F_(endTime) - F_(t_[idx_max]) );
            // finished
            return sgn * tmp;
        }
    };

    // evaluate \int_x(0)^x(n) v(x) f(x) dx via trapezoidal rule
    // v(x) interpolated values on variable x-grid
    // f(x) scalar function as functor
    class TrapezoidalIntegral {
    public:
        template <typename PassiveType, typename ActiveType, typename FuncType>
        ActiveType operator()(const std::vector<PassiveType>& x, const std::vector<ActiveType>& v, const FuncType& f) {
            size_t n=std::min(x.size(),v.size());
            if (n<2) return (ActiveType)0.0;
            ActiveType sum=0;
            for (size_t i=0; i<n-1; ++i) sum += 0.5*(v[i]*f(x[i]) + v[i+1]*f(x[i+1]))*(x[i+1] - x[i]);
            return sum;
        }
    };

    // evaluate \int_x[0]^x[n-1] v(x) f(x) dx via Gau�-Tschebyschow-Integration
    // x[0] left boundary, x[n-1] right boundary
    // x[1], ..., x[n-2] Gauss-Tschebyschow grid points
    // v(x) interpolated values on x-grid
    // f(x) scalar function as functor
    class GaussTschebyschowIntegral {
    public:
        template <typename PassiveType>
        std::vector<PassiveType> getGrid(PassiveType a, PassiveType b, size_t n) {
            std::vector<PassiveType> x(n);
            if (n==0) return x;
            if (n==1) { x[0] = 0.5*(a+b); return x; }
            x[0] = a;
            x[n-1] = b;
            if (n==2) return x;
            // n>2
            for (size_t k=1; k<n-1; ++k) {
                x[k] = -cos( (2.0*k-1.0) / (2.0*(n-2.0)) * M_PI );      // x \in (-1, 1)
                x[k] = 0.5*(b-a)*(x[k]+1) + a;                 // x \in ( a, b)
            }
            return x;
        }
        template <typename PassiveType, typename ActiveType, typename FuncType>
        ActiveType operator()(const std::vector<PassiveType>& x, const std::vector<ActiveType>& v, const FuncType& f) {
            size_t n=std::min(x.size(),v.size());
            if (n<3) return TrapezoidalIntegral()(x,v,f);
            ActiveType sum=0;
            for (size_t k=1; k<n-1; ++k) {
                sum += v[k] * f(x[k]) * sin( (2*k-1)/(2*(n-2))*M_PI );
            }
            sum *= M_PI / (n-2);
            return sum;
        }
    };

    // 4th order Runge Kutta step for y' = f(t,y)
    // via y1   = y0 + b^T k dt
    //     k_i  = f(t+c_i dt, y0 + a_i^T k dt)
    template <typename DateType, typename ActiveType>
    void rungeKuttaStep( const std::vector<ActiveType>&                                                                      y0,
                         const DateType                                                                                t,
                         const boost::function< void (const DateType, const std::vector<ActiveType>&, std::vector<ActiveType>&) >& f,
                         const DateType                                                                                dt,
                         std::vector<ActiveType>&                                                                      y1  ) {
        std::vector<ActiveType> k1(y0.size()), k2(y0.size()), k3(y0.size()), k4(y0.size());
        y1 = y0;
        // we add an epsilon in case we are at a boundary of piecewise constant parameters
        DateType eps = 1.0e-8*dt;
        // k1 = f(t,y0)
        f( t+eps,    y0, k1);
        for (size_t i=0; i<y0.size(); ++i) y1[i] += 1.0/6.0 * dt * k1[i];
        // k2 = f(t + 0.5dt, y0 + 0.5 k1)
        for (size_t i=0; i<y0.size(); ++i) k1[i] = y0[i] + 0.5 * dt * k1[i];
        f( t+0.5*dt, k1, k2);
        for (size_t i=0; i<y0.size(); ++i) y1[i] += 1.0/3.0 * dt * k2[i];
        // k3 = f(t + 0.5dt, y0 + 0.5 k2)
        for (size_t i=0; i<y0.size(); ++i) k2[i] = y0[i] + 0.5 * dt * k2[i];
        f( t+0.5*dt, k2, k3);
        for (size_t i=0; i<y0.size(); ++i) y1[i] += 1.0/3.0 * dt * k3[i];
        // k4 = f(t + dt, y0 + k3)
        for (size_t i=0; i<y0.size(); ++i) k3[i] = y0[i] + dt * k3[i];
        f( t+1.0*dt-eps, k3, k4);
        for (size_t i=0; i<y0.size(); ++i) y1[i] += 1.0/6.0 * dt * k4[i];
        return;
    }

}

#endif  /* quantlib_templateintegrators_hpp */


/*
 *  mgbvtd.cpp
 *  vortrac
 *
 *  Created by Xiaowen Tang on 5/28/13.
 *  Copyright 2013 University Corporation for Atmospheric Research.
 *  All rights reserved.
 *
 */
#include <vector>
#include <algorithm>
#include <cmath>
#include <armadillo>
#include "mgbvtd.h"

MGBVTD::MGBVTD(float x0, float y0, float hgt, float rmw, GriddedData& cappi):
m_cappi(cappi)
{
	m_centerx = x0;
	m_centery = y0;
	m_centerz = hgt;
	m_rmw     = rmw;
}

float MGBVTD::computeCrossBeamWind(float guessMax, QString& velField, GBVTD* gbvtd, Hvvp* hvvp)
{
	const float Rt = sqrt(m_centerx*m_centerx+m_centery*m_centery);

	//1. calculate Vt profile first
	std::vector<float> vt;
	std::vector<float> vt_rng;
	//1. compute the radial profile of symmetric tangential wind
	for(float rng=m_rmw*1.2; rng<=.6*Rt; rng+=1.){
		m_cappi.setCartesianReferencePoint(m_centerx, m_centery, m_centerz);
		int numData = m_cappi.getCylindricalAzimuthLength(rng, m_centerz);
		float* ringData = new float[numData];
		float* ringAzi  = new float[numData];
		m_cappi.getCylindricalAzimuthData(velField, numData, rng, m_centerz, ringData);
        m_cappi.getCylindricalAzimuthPosition(numData, rng, m_centerz, ringAzi);
		Coefficient* coeff = new Coefficient[20];
		float vtdDev;
		if(gbvtd->analyzeRing(m_centerx, m_centery, rng, m_centerz, numData, ringData, ringAzi, coeff, vtdDev)){
			if(coeff[0].getParameter()=="VTC0"){
				vt.push_back(coeff[0].getValue());
				vt_rng.push_back(rng);
			}
		}
		delete[] ringAzi;
		delete[] ringData;
		delete[] coeff;
	}
	if(vt.size()<15) {
		// std::cout<<std::endl;
		return 0.f;
	}

	//Compute hvvp first
	float cc0, cc6, vt_std, hvvp_std;
	if(!hvvp->computeCrossBeamWind(m_centerz, cc0, cc6, hvvp_std)) {
		// std::cout<<std::endl;
		return 0.f;
	}
	// printf("cc0=%5.2f, cc6=%5.2f, ", cc0, cc6);

	//Iterate through all possible values of cross-beam wind
	arma::fmat A(vt.size(), 2);
	for(int ii=0; ii<vt.size(); ++ii){
		A(ii,0) = log(Rt/vt_rng[ii]);
		A(ii,1) = 1;
	}

	std::vector<float> guessWinds;
	for(float currentWind=-fabs(guessMax); currentWind<fabs(guessMax)+1.; currentWind+=1.)
		guessWinds.push_back(currentWind);

	std::vector<bool> flag(guessWinds.size(), false);
	arma::fmat B(vt.size(), guessWinds.size());
	B.fill(0.f);
	arma::fvec b(vt.size());
	for(std::vector<float>::iterator it=guessWinds.begin(); it!=guessWinds.end(); ++it){
		int idx = std::distance(guessWinds.begin(), it);
		for(int ii=0; ii<vt.size(); ++ii)
			b(ii) = vt[ii]-*it*vt_rng[ii]/Rt;
		if( b.min()>0.f){
			B.col(idx) = arma::log(b);
			flag[idx]  = true;
		}
	}
	arma::fmat X=arma::solve(A, B);
	arma::fmat E=(A*X-B);
	vt_std = sqrt(arma::accu(arma::square(E))/E.size());
	// printf("min_Xt=%5.2f, max_Xt=%5.2f, ", arma::min(X.row(0)), arma::max(X.row(0)));

	//Compare results and find the best one
	float curDev=999., tmpBest=0., tmpXt=999.;
	for(std::vector<float>::iterator it=guessWinds.begin(); it!=guessWinds.end(); ++it){
		int idx = std::distance(guessWinds.begin(), it);
		if(!flag[idx] || X(0,idx)<=0.f ) continue;
		float hvvp_vm = cc0-Rt*cc6/(X(0,idx)+1.);
		if(fabs(*it-hvvp_vm)<curDev){
			curDev  = fabs(*it-hvvp_vm);
			tmpBest = *it;
			tmpXt   = X(0,idx);
		}
	}

	// printf("num_vt_fit=%3d, vt_std=%5.2f, hvvp_std=%5.2f, tmpXt=%5.2f, vm=%5.2f, dev=%5.2f\n", vt.size(), vt_std, hvvp_std, tmpXt, tmpBest, curDev);
	if(curDev>4.0f)
		return 0.0f;
	else
		return tmpBest;
}


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"

#include <qle/models/exactbachelierimpliedvolatility.hpp>

#include <ql/pricingengines/blackformula.hpp>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace boost::unit_test_framework;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(ExactBachelierImpliedVolatilityTest)

BOOST_AUTO_TEST_CASE(testExactBachelierImpliedVolatility) {

    BOOST_TEST_MESSAGE("Testing exact Bachelier implied volatility...");

    Real tolerance = 1E-4; // percent, i.e. we test for 1E-6 relative error
    Real forward = 0.05;   // fix the forward, only the difference forward - strike matters

    for (Real strikeSpread = -0.10; strikeSpread < 0.10 + 1E-5; strikeSpread += 0.001) {
        Real strike = forward + strikeSpread;
        for (Real vol = 0.0; vol < 0.02 + 1E-5; vol += 0.001) {
            for (Real tte = 0.001; tte < 51.0; tte += 0.1) {
                Real stdDev = std::sqrt(tte) * vol;
                Real call = bachelierBlackFormula(Option::Call, strike, forward, stdDev);
                Real put = bachelierBlackFormula(Option::Put, strike, forward, stdDev);
                if (std::abs(call) < 1E-12 || std::abs(put) < 1E-12)
                    continue;
                Real impliedVolCall = exactBachelierImpliedVolatility(Option::Call, strike, forward, tte, call);
                Real impliedVolPut = exactBachelierImpliedVolatility(Option::Put, strike, forward, tte, put);
                BOOST_CHECK_CLOSE(vol, impliedVolCall, tolerance);
                BOOST_CHECK_CLOSE(vol, impliedVolPut, tolerance);
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//
// Created by kellerberrin on 13/12/19.
//

#include "kpl_asrv.h"

#include "kel_distribution.h"

#include <boost/math/distributions/gamma.hpp>

namespace kel = kellerberrin;

namespace kpl = kellerberrin::phylogenetic;

void kpl::ASRV::clear() {

  // Rate homogeneity is the default
  _invar_model = false;
  _ratevar_fixed = false;
  _pinvar_fixed = false;
  _ratevar_ptr = std::make_shared<double>(1.0);
  _pinvar_ptr = std::make_shared<double>(0.0);
  _num_categ = 1;
  recalcASRV();

}

void kpl::ASRV::recalcASRV() {
  // This implementation assumes discrete gamma among-site rate heterogeneity
  // using a _num_categ category discrete gamma distribution with equal category
  // probabilities and Gamma density with mean 1.0 and variance _rate_var.
  // If _invar_model is true, then rate probs will sum to 1 - _pinvar rather than 1
  // and the mean rate will be 1/(1 - _pinvar) rather than 1; the rest of the invariable
  // sites component of the model is handled outside the ASRV class.

  // _num_categ, _rate_var, and _pinvar must all have been assigned in order to compute rates and probs
  if ((not _ratevar_ptr) || (_num_categ == 0) || (not _pinvar_ptr) ) {

    return;

  }

  double pinvar = *_pinvar_ptr;
  assert(pinvar >= 0.0);
  assert(pinvar <  1.0);

  assert(_num_categ > 0);

  double equal_prob = 1.0 /_num_categ;
  double mean_rate_variable_sites = 1.0;

  if (_invar_model) {

    mean_rate_variable_sites = 1.0 / (1.0 - pinvar);

  }

  _rates.assign(_num_categ, mean_rate_variable_sites);
  _probs.assign(_num_categ, equal_prob);

  double rate_variance = *_ratevar_ptr;
  assert(rate_variance >= 0.0);

  if (_num_categ == 1 || rate_variance == 0.0)
    return;

  double alpha = 1.0/rate_variance;
  double beta = rate_variance;

// #define USE_BOOST  1 // Check the timing between the two gamma distribution functions.
#ifdef USE_BOOST
  boost::math::gamma_distribution<> my_gamma(alpha, beta);
  boost::math::gamma_distribution<> my_gamma_plus(alpha + 1.0, beta);
#else
  kel::GammaDistribution k_gamma(alpha, beta);
  kel::GammaDistribution k_gamma_plus(alpha + 1.0, beta);
#endif

  double cum_upper        = 0.0;
  double cum_upper_plus   = 0.0;
  double upper            = 0.0;
  double cum_prob         = 0.0;

  for (unsigned i = 1; i <= _num_categ; ++i) {

    double cum_lower_plus       = cum_upper_plus;
    double cum_lower            = cum_upper;
    cum_prob                    += equal_prob;

    if (i < _num_categ) {

#ifdef USE_BOOST
      upper                   = boost::math::quantile(my_gamma, cum_prob);
      cum_upper_plus          = boost::math::cdf(my_gamma_plus, upper);
      cum_upper               = boost::math::cdf(my_gamma, upper);
#else
      upper                   = k_gamma.quantile(cum_prob);
      cum_upper_plus          = k_gamma_plus.cdf(upper);
      cum_upper               = k_gamma.cdf(upper);
#endif
    }
    else {
      cum_upper_plus          = 1.0;
      cum_upper               = 1.0;
    }

    double numer                = cum_upper_plus - cum_lower_plus;
    double denom                = cum_upper - cum_lower;
    double r_mean               = (denom > 0.0 ? (alpha*beta*numer/denom) : 0.0);
    _rates[i-1]        = r_mean*mean_rate_variable_sites;
  }

}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
GeneralizedEigenSolver<MatrixXf> ges;
MatrixXf A = MatrixXf::Random(4,4);
MatrixXf B = MatrixXf::Random(4,4);
ges.compute(A, B);
cout << "The (complex) numerators of the generalzied eigenvalues are: " << ges.alphas().transpose() << endl;
cout << "The (real) denominatore of the generalzied eigenvalues are: " << ges.betas().transpose() << endl;
cout << "The (complex) generalzied eigenvalues are (alphas./beta): " << ges.eigenvalues().transpose() << endl;

}
  return 0;
}


#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

#include <iostream>
#include <string>
#include <complex>

namespace client{
  template <typename Iterator>
    bool parse_complex(Iterator first, Iterator last, std::complex<double>& c){
      using boost::spirit::qi::double_;
      using boost::spirit::qi::_1;
      using boost::spirit::qi::phrase_parse;
      using boost::spirit::ascii::space;
      using boost::phoenix::ref;

      double rN = 0.0;
      double iN = 0.0;
      bool r = phrase_parse(first, last,
          (
            '(' >> double_[ref(rN) = _1] >> -(',' >> double_[ref(iN) = _1]) >> ')'
            | double_[ref(rN) = _1]
          ),
          space);

      if(!r || first != last) // fail if we did not get a full match
        return false;
      c = std::complex<double>(rN, iN);
      return r;
    }
}

int main(){
  std::string str;
  while(getline(std::cin, str)){
    if(str.empty() || str[0] == 'q' || str[0] == 'Q')
      break;

    std::complex<double> c;
    if(client::parse_complex(str.begin(), str.end(), c)){
      std::cout << "Parsing succeeded\n" << "got: " << c << std::endl;
    }
    else{
      std::cout << "Parsing failed\n";
    }
  }

  return 0;
}


// This source file should check the imported headers and do the simple things,
// like querying library version. It doesn't intended to do some lengthy work
// or running library tests - just only check that package is properly created.
// See docs for details:
// https://docs.conan.io/en/latest/creating_packages/getting_started.html#the-test-package-folder
#include <boost/simd/pack.hpp>
#include <boost/date_time.hpp>
#include <iostream>

namespace bs = boost::simd;

int main()
{
    using namespace boost::date_time;
    bs::pack<float,4> p{1.f,2.f,3.f,4.f};
    std::cout << "Boost.SIMD test from README.md : " << p + 10*p << "\n";
    std::cout << "Boost.date_time interop test: " << boost::gregorian::date(2021, Aug, 4) << "\n";
    return 0;
}


#pragma once

#include "utils.hpp"
#include <Eigen/Core>

template <typename MI, typename Tree>
std::vector<Eigen::VectorXd> FD(const MI& info, Tree& tree, const std::vector<Eigen::VectorXd>& Tau)
{
    constexpr int ord = Tree::order;
    const auto& mb = info.model.mb;
    const auto& pred = mb.predecessors();
    const auto& succ = mb.successors();

    std::vector<Eigen::MatrixXd> C(mb.nrBodies());
    std::vector<Eigen::MatrixXd> CD(mb.nrBodies());
    std::vector<Eigen::VectorXd> PA(mb.nrBodies());
    std::vector<Eigen::MatrixXd> IA(mb.nrBodies());
    std::vector<Eigen::MatrixXd> G(mb.nrBodies());
    std::vector<Eigen::MatrixXd> U(mb.nrBodies());
    std::vector<Eigen::MatrixXd> UD(mb.nrBodies());
    std::vector<Eigen::MatrixXd> D(mb.nrBodies());
    std::vector<Eigen::VectorXd> T(mb.nrBodies());
    std::vector<Eigen::VectorXd> y(mb.nrBodies());

    for (int i = 0; i < mb.nrBodies(); ++i) {
        C[i] = tree.joints[i].inverse().template matrix<ord>();
        CD[i] = tree.joints[i].template dualMatrix<ord>();
        G[i] = makeDiag<ord>(mb.joint(i).motionSubspace());
        IA[i].setZero(6 * ord, 6 * ord);
        PA[i].setZero(6 * ord);
    }

    for (int i = mb.nrBodies() - 1; i >= 0; --i) {
        IA[i] += makeDiag<ord>(mb.body(i).inertia().matrix());
        U[i] = IA[i] * G[i];
        UD[i] = G[i].transpose() * IA[i];
        D[i] = G[i].transpose() * U[i];

        y[i] = D[i].inverse() * (Tau[i] - G[i].transpose() * PA[i]);
        if (pred[i] != -1) {
            auto tmp1 = IA[i] - U[i] * D[i].inverse() * UD[i];
            IA[pred[i]] += CD[i] * tmp1 * C[i];
            auto tmp2 = PA[i] + U[i] * y[i];
            PA[pred[i]] += CD[i] * tmp2;
        }
    }

    for (int i = 0; i < mb.nrJoints(); ++i) {
        int dof = mb.joint(i).dof();
        if (pred[i] != -1) {
            y[i] -= D[i].inverse() * UD[i] * C[i] * T[pred[i]];
        }
        T[i] = G[i] * y[i];
        if (pred[i] != -1) {
            T[i] += C[i] * T[pred[i]];
        }
    }

    return y;
}

template <typename MI, typename Tree>
Eigen::VectorXd standard_FD(const MI& info, Tree& tree, const Eigen::VectorXd& tau)
{
    constexpr int ord = Tree::order;
    const auto& mb = info.model.mb;
    const auto& pred = mb.predecessors();
    const auto& succ = mb.successors();
    const auto& jpd = mb.jointsPosInDof();

    std::vector<Eigen::Vector6d> PA(mb.nrBodies());
    std::vector<Eigen::Matrix6d> IA(mb.nrBodies());
    std::vector<Eigen::Matrix6d> X(mb.nrBodies());
    std::vector<Eigen::Vector6d> T(mb.nrBodies());
    std::vector<Eigen::MatrixXd> U(mb.nrBodies());
    std::vector<Eigen::MatrixXd> D(mb.nrBodies());
    Eigen::VectorXd y(tau.size());

    for (int i = 0; i < mb.nrBodies(); ++i) {
        IA[i].setZero();
        PA[i].setZero();
        X[i] = tree.joints[i].transform().inverse().matrix();
    }

    for (int i = mb.nrBodies() - 1; i >= 0; --i) {
        const auto& S = mb.joint(i).motionSubspace();
        int dof = mb.joint(i).dof();
        IA[i] += mb.body(i).inertia().matrix();
        U[i] = IA[i] * S;
        D[i] = S.transpose() * U[i];

        y.segment(jpd[i], dof) = D[i].inverse() * (tau.segment(jpd[i], dof) - S.transpose() * PA[i]);
        if (pred[i] != -1) {
            auto tmp1 = IA[i] - U[i] * D[i].inverse() * U[i].transpose();
            IA[pred[i]] += X[i].transpose() * tmp1 * X[i];
            auto tmp2 = PA[i] + U[i] * y.segment(jpd[i], dof);
            PA[pred[i]] += X[i].transpose() * tmp2;
        }
    }

    for (int i = 0; i < mb.nrJoints(); ++i) {
        int dof = mb.joint(i).dof();
        if (pred[i] != -1) {
            y.segment(jpd[i], dof) -= D[i].inverse() * U[i].transpose() * X[i] * T[pred[i]];
        }
        T[i] = mb.joint(i).motionSubspace() * y.segment(jpd[i], dof);
        if (pred[i] != -1) {
            T[i] += X[i] * T[pred[i]];
        }
    }

    return y;
}


#pragma once

#include <Eigen/Dense>

namespace polyfem {
namespace autogen {
void q_nodes_2d(const int q, Eigen::MatrixXd &val);

void q_basis_value_2d(const int q, const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val);

void q_grad_basis_value_2d(const int q, const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val);

void q_nodes_3d(const int q, Eigen::MatrixXd &val);

void q_basis_value_3d(const int q, const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val);

void q_grad_basis_value_3d(const int q, const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val);

static const int MAX_Q_BASES = 3;

}}


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. 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 ANU 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 ANU OR THE 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 <opengv/triangulation/methods.hpp>
#include <Eigen/Eigenvalues>

opengv::point_t
opengv::triangulation::triangulate(
    const relative_pose::RelativeAdapterBase & adapter,
    size_t index )
{
  translation_t t12 = adapter.gett12();
  rotation_t R12 = adapter.getR12();
  Eigen::Matrix<double,3,4> P1 = Eigen::Matrix<double,3,4>::Zero();
  P1.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  Eigen::Matrix<double,3,4> P2 = Eigen::Matrix<double,3,4>::Zero();
  P2.block<3,3>(0,0) = R12.transpose();
  P2.block<3,1>(0,3) = -R12.transpose()*t12;
  bearingVector_t f1 = adapter.getBearingVector1(index);
  bearingVector_t f2 = adapter.getBearingVector2(index);

  Eigen::MatrixXd A(4,4);
  A.row(0) = f1[0] * P1.row(2) - f1[2] * P1.row(0);
  A.row(1) = f1[1] * P1.row(2) - f1[2] * P1.row(1);
  A.row(2) = f2[0] * P2.row(2) - f2[2] * P2.row(0);
  A.row(3) = f2[1] * P2.row(2) - f2[2] * P2.row(1);

  Eigen::JacobiSVD< Eigen::MatrixXd > mySVD(A, Eigen::ComputeFullV );
  point_t worldPoint;
  worldPoint[0] = mySVD.matrixV()(0,3);
  worldPoint[1] = mySVD.matrixV()(1,3);
  worldPoint[2] = mySVD.matrixV()(2,3);
  worldPoint = worldPoint / mySVD.matrixV()(3,3);

  return worldPoint;
};

opengv::point_t
opengv::triangulation::triangulate(bearingVector_t const & f1, bearingVector_t const &  f2,
    translation_t const &  t12, rotation_t const &  R12)
{
  Eigen::Matrix<double,3,4> P1 = Eigen::Matrix<double,3,4>::Zero();
  P1.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  Eigen::Matrix<double,3,4> P2 = Eigen::Matrix<double,3,4>::Zero();
  P2.block<3,3>(0,0) = R12.transpose();
  P2.block<3,1>(0,3) = -R12.transpose()*t12;

  Eigen::MatrixXd A(4,4);
  A.row(0) = f1[0] * P1.row(2) - f1[2] * P1.row(0);
  A.row(1) = f1[1] * P1.row(2) - f1[2] * P1.row(1);
  A.row(2) = f2[0] * P2.row(2) - f2[2] * P2.row(0);
  A.row(3) = f2[1] * P2.row(2) - f2[2] * P2.row(1);

  Eigen::JacobiSVD< Eigen::MatrixXd > mySVD(A, Eigen::ComputeFullV );
  point_t worldPoint;
  worldPoint[0] = mySVD.matrixV()(0,3);
  worldPoint[1] = mySVD.matrixV()(1,3);
  worldPoint[2] = mySVD.matrixV()(2,3);
  worldPoint = worldPoint / mySVD.matrixV()(3,3);

  return worldPoint;
};

opengv::point_t
opengv::triangulation::triangulate2(
    const relative_pose::RelativeAdapterBase & adapter,
    size_t index )
{
  translation_t t12 = adapter.gett12();
  rotation_t R12 = adapter.getR12();
  bearingVector_t f1 = adapter.getBearingVector1(index);
  bearingVector_t f2 = adapter.getBearingVector2(index);

  bearingVector_t f2_unrotated = R12 * f2;
  Eigen::Vector2d b;
  b[0] = t12.dot(f1);
  b[1] = t12.dot(f2_unrotated);
  Eigen::Matrix2d A;
  A(0,0) = f1.dot(f1);
  A(1,0) = f1.dot(f2_unrotated);
  A(0,1) = -A(1,0);
  A(1,1) = -f2_unrotated.dot(f2_unrotated);
  Eigen::Vector2d lambda = A.inverse() * b;
  Eigen::Vector3d xm = lambda[0] * f1;
  Eigen::Vector3d xn = t12 + lambda[1] * f2_unrotated;
  point_t point = ( xm + xn )/2;
  return point;
};


// Copyright 2014, Max Planck Society.
// Distributed under the BSD 3-Clause license.
// (See accompanying file LICENSE.txt or copy at
// http://opensource.org/licenses/BSD-3-Clause)

#ifndef GRASSMANN_AVERAGES_PCA_TRIMMED_HPP__
#define GRASSMANN_AVERAGES_PCA_TRIMMED_HPP__

/*!@file
 * Grassmann averages for robust PCA functions, following the paper of Soren Hauberg.
 *
 * This file contains the implementation of the trimmed version.
 */

// for the thread pools
#include <boost/asio/io_service.hpp>
#include <boost/bind.hpp>
#include <boost/thread/thread.hpp>

#include <boost/scoped_array.hpp>
#include <boost/function.hpp>

// utilities
#include <include/private/utilities.hpp>

namespace grassmann_averages_pca
{

  namespace details
  {

    //!@internal
    //!Helper object for the updates
    template <class scalar_t>
    struct s_dimension_update
    {
      size_t dimension;
      scalar_t value;
    };

    /*!@internal
     * @brief Adaptation of merger_addition concept for accumulator and count at the same time.
     *
     * The trimmed version of the grassmann pca algorithm may strip some element along each dimension.
     * In order to compute the @f$\mu@f$ properly, the count should also be transfered.
     */
    template <class data_t>
    struct merger_update_specific_dimension
    {
      typedef data_t input_t;
      bool operator()(input_t &current_state, s_dimension_update<typename data_t::value_type> const& update_value) const
      {
        current_state(update_value.dimension) += update_value.value;
        return true;
      }
    };

  }

  /*!@brief Grassmann Average algorithm for robust PCA computation, with trimming of outliers.
   *
   * This class implements the Grassmann average for computing the robust PCA, which also includes the trimming of "outliers".
   * Its purpose is to compute the PCA of a dataset @f$\mathbf{X} = \{X_i\}@f$, where each @f$X_i@f$ is a vector of dimension
   * D, and also by being less sensitive to the outliers of the original data.
   *
   * The algorithm is the following:
   * - pick a random or a given @f$\mu_{k, 0}@f$, where @f$k@f$ is the current basis vector being computed and @f$0@f$ is the current iteration number (0).
   * - ensure this @f$\mu_{k, 0}@f$ is orthogonal to the previous detected @f$\mu_{k', 0},\, \forall k' \in [0, k)@f$
   * - until the sequence @f$(\mu_{k, t})_t@f$ converges, do:
   *   - computes the sign @f$s_{j, t}@f$ of the projection of the input vectors @f$X_j@f$ onto @f$\mu_{i, t}@f$. We have @f[s_{j, t} = X_j \cdot \mu_{k, t} \geq 0@f]
   *   - for each dimension @f$0 \lt d \leq D@f$, do:
   *     - For all @f$j@f$, consider the multiplied data set projected onto dimension @f$d@f$:
   *       @f[\mathbf{X_\mu^{(d)}} = \left\{proj_d \left(s_{j, t} \cdot X_j\right) \right\} = \left\{s_{j, t} \cdot X_j^{(d)}\right\}@f]
   *       which is a 1-D sequence
   *     - compute the indexes @f$J_d@f$ of the @f$\frac{K}{2}@f$ lowest and biggest points of this 1-D sequence @f$\mathbf{X_\mu^{(d)}}@f$
   *     - compute the update of @f$\mu_{k, .}@f$ for dimension @f$d@f$ :
   *       @f[\mu_{k, t+1}^{(d)} = \frac{\sum_{j \notin J_d} proj_d \left(s_{j, t} \cdot X_j \right)}{\# J - \# J_d} @f]
   *   - normalize @f[\mu_{k, t+1} = \frac{\left(\mu_{k, t+1}^{(1)}, \ldots \mu_{k, t+1}^{(D)}\right)^t}{\left\|\left(\mu_{k, t+1}^{(1)}, \ldots \mu_{k, t+1}^{(D)}\right)^t\right\|}@f]
   * - project the @f$X_j@f$'s onto the orthogonal subspace of @f$\mu_{k} = \lim_{t \rightarrow +\infty} \mu_{k, t}@f$: @f[\forall j, X_{j} = X_{j} - X_{j}\cdot\mu_{k} @f]
   *
   * The range taken by @f$k@f$ is a parameter of the algorithm: @c max_dimension_to_compute (see grassmann_pca::batch_process).
   * The range taken by @f$t@f$ is also a parameter of the algorithm: @c max_iterations (see grassmann_pca::batch_process).
   * The test for convergence is delegated to the class details::convergence_check.
   *
   * The computation is distributed among several threads. The multithreading strategy is
   * - to split the computation of @f$\sum_j s_{j, t} X_j@f$ among several independant chunks. This computation involves the inner product and the sign. Each chunk addresses
   *   a subset of the data @f$\{X_j\}@f$ without any overlap with other chunks. The maximal size of a chunk can be configured through the function grassmann_pca::set_max_chunk_size.
   *   By default, the size of the chunk would be the size of the data divided by the number of threads.
   * - to compute the @f$\frac{K}{2}@f$ extremal points along each dimension in parallel
   * - to split the computation of the projection onto the orthogonal subspace of @f$\mu_{k}@f$.
   * - to split the computation of the regular PCA algorithm (if any) into several independant chunks.
   *
   * The number of threads can be configured through the function grassmann_pca::set_nb_processors.
   *
   * @note
   * The algorithm may also perform a few "regular PCA" steps, which is the computation of the basis vector with highest "eigen-value". This can be configured through the function
   * grassmann_pca_with_trimming::set_nb_steps_pca. Also, the data can be centered before applying any computation (see grassmann_pca_with_trimming::set_centering). This is
   * convenient especially when the input data iterator is just a stub that provides data read from the disk for instance (see the application of Grassmann applied on videos).
   *
   * @tparam data_t type of vectors used for the computation.
   * @tparam norm_mu_t norm used to normalize the basis vectors and project them onto the unit circle.
   * @tparam observer_t an observer type following the signature of the class grassmann_trivial_callback
   *
   * @author Soren Hauberg, Raffi Enficiaud
   */
  template <class data_t,
            class observer_t = grassmann_trivial_callback<data_t>,
            class norm_mu_t = details::norm2 >
  struct grassmann_pca_with_trimming
  {
  private:
    //! Random generator for initialising @f$\mu@f$ at each dimension.
    details::random_data_generator<data_t> random_init_op;

    //! Norm used for normalizing @f$\mu@f$.
    norm_mu_t norm_op;

    //! The percentage of the data that should be trimmed.
    //! The trimming is performed symmetrically in the upper and lower distribution of the data, hence
    //! each side is trimmed by trimming_percentage/2.
    double trimming_percentage;

    //! Number of parallel tasks that will be used for computing.
    size_t nb_processors;

    //! Maximal size of a chunk (infinity by default).
    size_t max_chunk_size;

    //! Number of steps for the initial PCA like algorithm (defaults to 3).
    size_t nb_steps_pca;

    //! Type of the element of data_t.
    typedef typename data_t::value_type scalar_t;

    //! Type of the vector used for counting the element falling into the non-trimmed range.
    typedef boost::numeric::ublas::vector<size_t> count_vector_t;

    //! Indicates that the incoming data is not centered and a centering should be performed prior
    //! to the computation of the PCA or the trimmed grassmann average.
    bool need_centering;

    //! An instance observing the steps of the algorithm
    observer_t *observer;

    //!@internal
    //!@brief Contains the logic for processing part of the accumulator
    struct s_grassmann_averages_trimmed_processor_inner_products
    {
    private:
      //! Scalar type of the data
      typedef typename data_t::value_type scalar_t;

      typedef details::s_dimension_update<scalar_t> accumulator_element_t;

      size_t nb_elements;                 //!< The size of the current dataset
      size_t data_dimension;              //!< The dimension of the data
      size_t k_first_last;                //!< The number of elements to remove from the lower and upper distributions.

      typedef boost::function<void ()> connector_counter_t;
      connector_counter_t signal_counter;

      typedef boost::function<void (accumulator_element_t const*)> connector_accumulator_dimension_t;
      connector_accumulator_dimension_t signal_acc_dimension;

      std::vector<accumulator_element_t> v_accumulated_per_dimension;

      //! The matrix containing a copy of the data
      scalar_t *p_c_matrix;

      //! The result of the inner products
      std::vector<scalar_t> inner_prod_results;

      void compute_inner_products(data_t const &mu)
      {
        scalar_t *out = &inner_prod_results[0];
        scalar_t mu_element = mu(0);
        scalar_t const * current_line = p_c_matrix;

        for(int column = 0; column < nb_elements; column++)
        {
          out[column] = mu_element * current_line[column];
        }
        current_line += nb_elements;

        for(int line = 1; line < data_dimension; line ++, current_line += nb_elements)
        {
          mu_element = mu(line);
          for(int column = 0; column < nb_elements; column++)
          {
            out[column] += mu_element * current_line[column];
          }
        }

      }

    public:
      s_grassmann_averages_trimmed_processor_inner_products() :
        nb_elements(0),
        data_dimension(0),
        k_first_last(0),
        p_c_matrix(0)
      {
      }

      ~s_grassmann_averages_trimmed_processor_inner_products()
      {
        delete [] p_c_matrix;
      }

      //! Returns the connected object that will receive the notification of the end of the current process.
      connector_counter_t& connector_counter()
      {
        return signal_counter;
      }

      connector_accumulator_dimension_t& connector_accumulator()
      {
        return signal_acc_dimension;
      }

      //! Sets the data range
      template <class container_iterator_t>
      bool set_data_range(container_iterator_t const &b, container_iterator_t const& e)
      {
        if(data_dimension <= 0)
        {
          return false;
        }

        nb_elements = std::distance(b, e);

        assert(data_dimension > 0);
        assert(nb_elements > 0);

        delete [] p_c_matrix;
        p_c_matrix = new scalar_t[nb_elements*data_dimension];

        container_iterator_t bb(b);

        for(int column = 0; column < nb_elements; column++, ++bb)
        {
          scalar_t* current_line = p_c_matrix + column;
          for(int line = 0; line < data_dimension; line ++, current_line += nb_elements)
          {
            *current_line = (*bb)(line);
          }

        }

        inner_prod_results.resize(nb_elements);

        signal_counter();
        return true;
      }

      //! Sets the dimension of the data vectors
      //! @pre data_dimensions_ is strictly positive
      void set_data_dimensions(size_t data_dimensions_)
      {
        assert(data_dimensions_ > 0);
        data_dimension = data_dimensions_;
        v_accumulated_per_dimension.resize(data_dimension);
      }

      //! Sets the number of element to remove in the upper and lower distributions.
      void set_nb_elements_to_remove(int k_first_last_)
      {
        assert(k_first_last_ >= 0);
        k_first_last = k_first_last_;
      }

      //! Centering the data in case it was not possible to do it beforehand
      void data_centering_first_phase(size_t full_dataset_size)
      {
        scalar_t const * current_line = p_c_matrix;
        for(size_t dimension = 0; dimension < data_dimension; dimension++)
        {
          scalar_t acc = 0;
          for(size_t s = 0; s < nb_elements; s++)
          {
            acc += *current_line++;
          }

          // posts the new value to the listeners for the current dimension
          accumulator_element_t &result = v_accumulated_per_dimension[dimension];
          result.dimension = dimension;
          result.value = acc / full_dataset_size;
          signal_acc_dimension(&result);
        }

        signal_counter();

      }

      //! Project the data onto the orthogonal subspace of the provided vector
      void data_centering_second_phase(data_t const &mean_value)
      {
        scalar_t *current_element_ptr = p_c_matrix;

        for(int line = 0; line < data_dimension; line++)
        {
          const scalar_t scalar = mean_value(line);
          scalar_t * const current_line_end = current_element_ptr + nb_elements;
          for(; current_element_ptr < current_line_end; current_element_ptr++)
          {
            *current_element_ptr -= scalar;
          }
        }

        signal_counter();
      }

      //! PCA steps
      void pca_accumulation(data_t const &mu)
      {
        compute_inner_products(mu);

        scalar_t const * const p_inner_product = &inner_prod_results[0];

        for(size_t dimension = 0; dimension < data_dimension; dimension++)
        {
          scalar_t const * const current_line = p_c_matrix + dimension*nb_elements;
          scalar_t acc = 0;
          for(size_t s = 0; s < nb_elements; s++)
          {
            acc += p_inner_product[s] * current_line[s];
          }

          // posts the new value to the listeners for the current dimension
          accumulator_element_t &result = v_accumulated_per_dimension[dimension];
          result.dimension = dimension;
          result.value = acc;
          signal_acc_dimension(&result);
        }

        signal_counter();
      }

      //! Computes the inner products and stores the signed result where appropriate.
      void compute_data_matrix(data_t const &mu, scalar_t* p_out, size_t padding)
      {
        // updates the internal inner products
        compute_inner_products(mu);

        // the current line is spans a particular dimension
        scalar_t *current_line = p_c_matrix;

        // this spans the inner product results for all dimensions

        std::vector<int> v_mult(nb_elements);
        for(size_t element(0); element < nb_elements; element++)
        {
          v_mult[element] = inner_prod_results[element] >= 0 ? 1 : -1;
        }
        int const * const out = &v_mult[0];

        for(size_t current_dimension = 0;
            current_dimension < data_dimension;
            current_dimension++, current_line += nb_elements, p_out+= padding)
        {
          for(size_t element(0); element < nb_elements; element++)
          {
            p_out[element] = out[element] * current_line[element];
          }
        }

        // signals the main merger
        signal_counter();
      }

      //! Computes the mean on the subset of the data where the k first and last elements are removed.
      void compute_bounded_accumulation(size_t dimension, size_t nb_total_elements, scalar_t* p_data)
      {
        accumulator_element_t &result = v_accumulated_per_dimension[dimension];
        result.dimension = dimension;
        result.value = details::compute_mean_within_bounds(p_data, nb_total_elements, k_first_last);

        // signals the update
        signal_acc_dimension(&result);
        // signals the main merger
        signal_counter();
      }

      //! Project the data onto the orthogonal subspace of the provided vector
	    template <class vector_t>
      void project_onto_orthogonal_subspace(vector_t const &mu)
      {
        compute_inner_products(mu);
        scalar_t *current_line = p_c_matrix;

        for(int line = 0; line < data_dimension; line ++, current_line += nb_elements)
        {
          scalar_t mu_element = mu(line);
          for(int column = 0; column < nb_elements; column++)
          {
            current_line[column] -= mu_element * inner_prod_results[column];
          }
        }

        signal_counter();
      }

    };

    /*!@internal
     * @brief Merges the result of all workers and signals the results to the main thread.
     *
     * The purpose of this class is to add the computed accumulator of each thread to the final result
     * which contains the sum of all accumulators.
     *
     */
    struct asynchronous_results_merger :
      details::threading::asynchronous_results_merger<
        data_t,
        details::merger_update_specific_dimension<data_t>,
        details::threading::initialisation_vector_specific_dimension<data_t>,
        details::s_dimension_update<typename data_t::value_type>
      >
    {
    public:
      typedef data_t result_t;

    private:
      typedef details::threading::initialisation_vector_specific_dimension<data_t> data_init_type;
      typedef details::merger_update_specific_dimension<data_t> merger_type;

      const size_t data_dimension;

    public:
      typedef details::threading::asynchronous_results_merger<
        result_t,
        merger_type,
        data_init_type,
        details::s_dimension_update<typename data_t::value_type>
      > parent_type;
      typedef typename parent_type::lock_t lock_t;

      /*!Constructor
       *
       * @param dimension_ the number of dimensions of the vector to accumulate
       */
      asynchronous_results_merger(size_t data_dimension_) :
        parent_type(data_init_type(data_dimension_)),
        data_dimension(data_dimension_)
      {}

    };

  public:
    /*!@brief Constructor
     *
     * Constructs an instance of the RobustPCA with trimming with the provided percentage of trimming.
     *
     * @param[in] trimming_percentage_ the percentage of data that should be trimmed from the lower and upper distributions.
     * @note By default the number of processors used for computation is set to 1.
     * The maximum size of the chunks is "infinite": each chunk will receive in that case the size of the data
     * divided by the number of running threads.
     */
    grassmann_pca_with_trimming(double trimming_percentage_ = 0) :
      random_init_op(details::fVerySmallButStillComputable, details::fVeryBigButStillComputable),
      trimming_percentage(trimming_percentage_),
      nb_processors(1),
      max_chunk_size(std::numeric_limits<size_t>::max()),
      nb_steps_pca(3),
      need_centering(false),
      observer(0)
    {
      assert(trimming_percentage_ >= 0 && trimming_percentage_ <= 1);
    }

    //! Sets the observer of the algorithm.
    //!
    //! The lifetime of the observer is not managed by this class. Set to 0 to disable
    //! observation.
    bool set_observer(observer_t* observer_)
    {
      observer = observer_;
      return true;
    }

    //! Sets the number of parallel tasks used for computing.
    bool set_nb_processors(size_t nb_processors_)
    {
      assert(nb_processors_ >= 1);
      nb_processors = nb_processors_;
      return true;
    }

    /*!@brief Sets the maximum chunk size.
     *
     * By default, the chunk size is the size of the data divided by the number of processing threads.
     * Lowering the chunk size should provid better granularity in the overall processing time at the end
     * of the processing.
     */
    bool set_max_chunk_size(size_t chunk_size)
    {
      if(chunk_size == 0)
      {
        return false;
      }
      max_chunk_size = chunk_size;
      return true;
    }

    //! Sets the number of iterations for the initial PCA like algorithm.
    bool set_nb_steps_pca(size_t nb_steps)
    {
      nb_steps_pca = nb_steps;
      return true;
    }

    //! Sets the centering flags.
    //!
    //! If set to true, a centering will be performed before applying any computation (PCA and Grassmann averages).
    bool set_centering(bool need_centering_)
    {
      need_centering = need_centering_;
      return true;
    }

    /*!@brief Performs the computation of the current subspace on the elements given by the two iterators.
     *
     * @tparam it_t an input forward iterator to input vectors points. Each element pointed by the underlying iterator should be iterable and
     *   should provide a vector point.
     * @tparam it_o_basisvectors_t an output iterator for storing the computed basis vectors. This iterator should model a forward output iterator.
     *
     * @param[in] max_iterations the maximum number of iterations at each dimension.
     * @param[in] max_dimension_to_compute the maximum number of data_dimension to compute in the PCA (only the first max_dimension_to_compute will be
     *            computed).
     * @param[in] it input iterator at the beginning of the data
     * @param[in] ite input iterator at the end of the data
     * @param[in] initial_guess if provided, the initial vectors will be initialized to this value.
     * @param[out] it_basisvectors an iterator on the beginning of the area where the detected basis vectors will be stored. The space should be at least max_dimension_to_compute.
     *
     * @returns true on success, false otherwise
     * @pre
     * - @c !(it >= ite)
     * - all the vectors given by the iterators pair should be of the same size (no check is performed).
     */
    template <class it_t, class it_o_basisvectors_t>
    bool batch_process(
      const size_t max_iterations,
      size_t max_dimension_to_compute,
      it_t const it,
      it_t const ite,
      it_o_basisvectors_t it_basisvectors,
      std::vector<data_t> const * initial_guess = 0)
    {
      // add some log information
      if(it >= ite)
      {
        return false;
      }

      // preparing the thread pool, to avoid individual thread creation/deletion at each step.
      // we perform the init here because it might take some time for the thread to really start.
      boost::asio::io_service ioService;
      boost::thread_group threadpool;

      // in case of non clean exit (or even in case of clean one).
      details::threading::safe_stop worker_lock_guard(ioService, threadpool);

      // this is exactly the number of processors
      boost::asio::io_service::work work(ioService);
      for(int i = 0; i < nb_processors; i++)
      {
        threadpool.create_thread(boost::bind(&boost::asio::io_service::run, &ioService));
      }

      // contains the number of elements. In case the iterator is random access, could be deduced simply
      // by a call to distance.
      const size_t size_data(std::distance(it, ite));

      // size of the chunks.
      const size_t chunks_size = std::min(max_chunk_size, static_cast<size_t>(ceil(double(size_data)/nb_processors)));
      const size_t nb_chunks = (size_data + chunks_size - 1) / chunks_size;

      // number of dimensions of the data vectors
      const size_t number_of_dimensions = it->size();
      max_dimension_to_compute = std::min(max_dimension_to_compute, number_of_dimensions);

      // initial iterator on the output basis vectors
      it_o_basisvectors_t const it_output_basis_vector_beginning(it_basisvectors);
      it_o_basisvectors_t it_output_basis_vector_end(it_output_basis_vector_beginning);
      std::advance(it_output_basis_vector_end, max_dimension_to_compute);

      // the initialisation of mus
      {
        it_o_basisvectors_t it_basis(it_output_basis_vector_beginning);
        for(int i = 0; it_basis != it_output_basis_vector_end; ++it_basis, ++i)
        {
          *it_basis = initial_guess != 0 ? (*initial_guess)[i] : random_init_op(*it);
        }
      }
      if(!details::gram_schmidt_orthonormalisation(it_output_basis_vector_beginning, it_output_basis_vector_end, it_output_basis_vector_beginning, norm_op))
      {
        return false;
      }

      // preparing mu
      data_t mu(*it_basisvectors);
      assert(mu.size() == number_of_dimensions);

      // number of elements to remove, lower bound
      const int K_elements = static_cast<int>(trimming_percentage*size_data/2);

      // preparing the ranges on which each processing thread will run.
      // the number of objects can be much more than the current number of processors, in order to
      // avoid waiting too long for a thread (better granularity) but involving a slight overhead in memory and
      // processing at the synchronization point.
      typedef s_grassmann_averages_trimmed_processor_inner_products async_processor_t;
      std::vector<async_processor_t> v_individual_accumulators(nb_chunks);

      asynchronous_results_merger async_merger(number_of_dimensions);

      {
        it_t it_current_begin(it);
        for(int i = 0; i < nb_chunks; i++)
        {
          // setting the range
          it_t it_current_end;
          if(i == nb_chunks - 1)
          {
            // just in case the division giving the chunk has some rounding (the parenthesis are important
            // otherwise it is a + followed by a -, which can be out of range after the first +)
            it_current_end = it_current_begin + (size_data - chunks_size*(nb_chunks - 1));
          }
          else
          {
            it_current_end = it_current_begin + chunks_size;
          }

          // the processor object for this new range
          async_processor_t &current_acc_object = v_individual_accumulators[i];

          // updating the dimension of the problem
          current_acc_object.set_data_dimensions(number_of_dimensions);

          // setting the number of elements to remove
          current_acc_object.set_nb_elements_to_remove(K_elements);

          // attaching the update object callbacks
          current_acc_object.connector_accumulator() = boost::bind(
              &asynchronous_results_merger::update,
              &async_merger,
              _1);

          current_acc_object.connector_counter() = boost::bind(&asynchronous_results_merger::notify, &async_merger);

          //bool b_result = current_acc_object.set_data_range(it_current_begin, it_current_end);
          //if(!b_result)
          //{
          //  return b_result;
          //}

          // pushing the asynchronous copy, which saves a lot of time when loading
          // data lazily from disk
          ioService.post(
            boost::bind(
              &async_processor_t::template set_data_range<it_t>,
              boost::ref(v_individual_accumulators[i]),
              it_current_begin, it_current_end));

          // updating the next
          it_current_begin = it_current_end;
        }

        // waiting for completion (barrier)
        async_merger.wait_notifications(v_individual_accumulators.size());
      }

      // this matrix is a copy of the data. It is a convenient structure for storing the
      // flipped vectors that will then be trimmed and accumulated. But this is a copy of the initial data:
      // if the memory pressure is too big, then something else should be found (like a vector of vector, being
      // potentially scattered in memory).
      boost::scoped_array<scalar_t> matrix_temp(new scalar_t[number_of_dimensions*size_data]);

      // Centering the data if needed:
      // - first run the accumulation and gather all results in a multithreaded manner
      // - second center the data with the collected mean
      if(need_centering)
      {
        // Computing the accumulation
        async_merger.init();

        for(int i = 0; i < v_individual_accumulators.size(); i++)
        {
          ioService.post(
            boost::bind(
              &async_processor_t::data_centering_first_phase,
              boost::ref(v_individual_accumulators[i]),
              size_data)); // size of the dataset to perform division and avoid doing accumulation over big numerical values
        }

        // waiting for completion (barrier)
        async_merger.wait_notifications(v_individual_accumulators.size());

        // gathering the accumulated, already divided by the size
        data_t mean_vector = async_merger.get_merged_result();

        // sending result to observer
        if(observer)
        {
          observer->signal_mean(mean_vector);
        }

        // centering the data
        async_merger.init();

        for(int i = 0; i < v_individual_accumulators.size(); i++)
        {
          ioService.post(
            boost::bind(
              &async_processor_t::data_centering_second_phase,
              boost::ref(v_individual_accumulators[i]),
              boost::cref(mean_vector)
              ));
        }

        // waiting for completion (barrier)
        async_merger.wait_notifications(v_individual_accumulators.size());

      }

      // for each requested subspace
      for(int current_subspace_index = 0; current_subspace_index < max_dimension_to_compute; current_subspace_index++, ++it_basisvectors)
      {

        // PCA like initial steps
        if(nb_steps_pca)
        {
          for(size_t pca_it = 0; pca_it < nb_steps_pca; pca_it++)
          {
            // reseting the final accumulator
            async_merger.init();

            // pushing the initialisation of the mu and sign vectors to the pool
            for(int i = 0; i < v_individual_accumulators.size(); i++)
            {
              ioService.post(
                boost::bind(
                  &async_processor_t::pca_accumulation,
                  boost::ref(v_individual_accumulators[i]),
                  boost::cref(mu)));
            }

            // waiting for completion (barrier)
            async_merger.wait_notifications(v_individual_accumulators.size());

            // gathering the first mu
            mu = async_merger.get_merged_result();
            double norm_mu = norm_op(mu);
            if(norm_mu < 1E-12)
            {
              if(observer)
              {
                std::ostringstream o;
                o << "The result of the PCA is null for subspace " << current_subspace_index;
                observer->log_error_message(o.str().c_str());
              }
              return false;
            }
            mu *= typename data_t::value_type(1./norm_mu);
          }

          // sending result to observer
          if(observer)
          {
            observer->signal_pca(mu, current_subspace_index);
          }
        }

        details::convergence_check<data_t> convergence_op(mu);

        int iterations = 0;
        for(; (!convergence_op(mu) && iterations < max_iterations) || iterations == 0; iterations++)
        {

          // reseting the merger object
          async_merger.init();

          // pushing the computation of the bounds
          for(int i = 0; i < v_individual_accumulators.size(); i++)
          {
            ioService.post(
              boost::bind(
                &async_processor_t::compute_data_matrix,
                boost::ref(v_individual_accumulators[i]),
                boost::cref(mu),
                matrix_temp.get() + i*chunks_size,
                size_data));
          }

          // waiting for completion (barrier)
          async_merger.wait_notifications(v_individual_accumulators.size());

          // clearing the notifications
          async_merger.init_notifications();

          // pushing the computation of the trimmed accumulation on each dimension
          for(int dim_to_compute = 0; dim_to_compute < number_of_dimensions; dim_to_compute++)
          {
            ioService.post(
              boost::bind(
                &async_processor_t::compute_bounded_accumulation,
                boost::ref(v_individual_accumulators[0]),
                dim_to_compute,
                size_data,
                matrix_temp.get() + dim_to_compute*size_data));
          }

          // waiting for completion (barrier)
          async_merger.wait_notifications(number_of_dimensions);

          // gathering the mus
          mu = async_merger.get_merged_result();

          // normalize mu on the sphere
          mu *= typename data_t::value_type(1./norm_op(mu));

          // sending result to observer
          if(observer)
          {
            observer->signal_intermediate_result(mu, current_subspace_index, iterations);
          }

        }

        // orthogonalisation against previous basis vectors
        bool renormalise(false);
        for(it_o_basisvectors_t it_mus(it_output_basis_vector_beginning); it_mus < it_basisvectors; ++it_mus)
        {
          mu -= boost::numeric::ublas::inner_prod(mu, *it_mus) * (*it_mus);
          renormalise = true;
        }
        if(renormalise)
        {
          double norm_mu = norm_op(mu);
          if(norm_mu < 1E-12)
          {
            if(observer)
            {
              std::ostringstream o;
              o << "The result of the subspace computation is null (subspace " << current_subspace_index << ")";
              observer->log_error_message(o.str().c_str());
            }
            return false;
          }

          mu *= typename data_t::value_type(1./norm_mu);
        }

        // mu is the basis vector of the current dimension, we store it in the output vector
        *it_basisvectors = mu;

        // sending result to observer
        if(observer)
        {
          observer->signal_eigenvector(*it_basisvectors, current_subspace_index);
        }

        // projection onto the orthogonal subspace
        if(current_subspace_index < max_dimension_to_compute - 1)
        {
          async_merger.init_notifications();

          // pushing the update of the mu (and signs)
          for(int i = 0; i < v_individual_accumulators.size(); i++)
          {

			      ioService.post(
				      boost::bind(
					      &async_processor_t::template project_onto_orthogonal_subspace<typename it_o_basisvectors_t::value_type>,
					      boost::ref(v_individual_accumulators[i]),
					      *it_basisvectors)); // this is not mu, since we are changing it before the process ends here
          }

          // this is to follow the matlab implementation, but the idea is the following:
          // each time we pick a new candidate vector, we project it to the orthogonal subspace of the previously computed
          // basis vectors. This can be done in two ways:
          // 1. compute the projection on the orthogonal subspace of the current (or next) candidate
          // 2. compute the projection on the orthogonal subspace of the remainder elements
          //
          // in order to be consistent with the matlab implementation, the second choice is implemented here
          if(current_subspace_index+1 < max_dimension_to_compute)
          {
            it_o_basisvectors_t remainder(it_basisvectors);
            ++remainder;

            if(!details::gram_schmidt_orthonormalisation(it_output_basis_vector_beginning, it_output_basis_vector_end, remainder, norm_op))
            {
              return false;
            }

            mu = *remainder;
          }

          // wait for the workers
          async_merger.wait_notifications(v_individual_accumulators.size());

        }

      }

      return true;
    }

  };

}

#endif /* GRASSMANN_AVERAGES_PCA_TRIMMED_HPP__ */


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

#ifndef GSPARSE_UNDIRECTEDGRAPH_HPP
#define GSPARSE_UNDIRECTEDGRAPH_HPP

#include <Eigen/Sparse>
#include <Eigen/Dense>

#include <exception>  // For exception handling
#include <sstream>    // For exception messages
#include <string>
#include <vector>
#include <map>
#include <cstddef>   // size_t definition
#include <cmath>
#include <memory>    // Shared_ptr

#include "Config.hpp"
#include "Interface/Graph.hpp"
#include "Interface/GraphReader.hpp"

/* TODO:
    // Support move operator. Review Eigen documentation.
*/

namespace gSparse
{
    //! An Undirected Graph class
    /*!
        This class provides a multiple representation of an Undirected, Simple graph.
    */
	class UndirectedGraph : public IGraph
	{
	public:
		//! Graph data type should not be copied by value
		UndirectedGraph(const UndirectedGraph & graph)  = delete;
        /*{
            // Call assignment operator
            *this = graph;
        }*/
        //! Graph data type should not be copied by value / reassigne due to Eigen data type
		UndirectedGraph & operator=(const UndirectedGraph & rhs) = delete;
        /*{
            if (this != &rhs)
            {
                // Perform expensive copy on these matrix
                this->_adjMatrix = rhs._adjMatrix;
                this->_degMatrix = rhs._degMatrix;
                this->_incidentMatrix = rhs._incidentMatrix;
                this->_weightMatrix = rhs._weightMatrix;
                this->_laplacianMatrix = rhs._laplacianMatrix;
                this->_edges = rhs._edges;
                this->_weights = rhs._weights;
                this->_edgeCount = rhs._edgeCount;
                this->_nodeCount = rhs._nodeCount;
            }
            return *this;
        }*/

		//! A constructor to initialize graph based on GraphReader
        /*!
        \param DataReader: A subclass of IGraphReader which provides an interface to read external data.
        */
		UndirectedGraph(const gSparse::GraphReader & DataReader)
		{
			if (DataReader == nullptr)
			{
				throw std::invalid_argument("UndirectedGraph: DataReader must not be NULL");
			}
			DataReader->Read(_edges, _weights);
			//Initialize the graph system.
			_initializeSystem();
		}

		//! A constructor to initialize graph based from Edge data. Weight sets to one.
        /*!
        \param Edges: An Eigen Matrix containing Edge List.
        */
		UndirectedGraph(const gSparse::EdgeMatrix & Edges) :_edges(Edges)
		{
			_weights = gSparse::PrecisionMatrix::Ones(Edges.rows(), 1);
			_initializeSystem();
		}
        //! A constructor to initialize graph based from Edge data. Weight sets to one.
        /*!
        \param Edges: An Eigen Matrix containing Edge List.
        \param Weights: An Eigen Matrix containing associated Weights.
        */
		UndirectedGraph(const gSparse::EdgeMatrix & Edges,
			const gSparse::PrecisionRowMatrix & Weights) :
			_edges(Edges),
			_weights(Weights)
		{
			_initializeSystem();
		}
        //! Return Graph's Adjancency Matrix
		virtual inline const gSparse::SparsePrecisionMatrix & GetAdjacentMatrix() const { return _adjMatrix; }
		//! Return Graph's Incident Matrix
        virtual inline const gSparse::SparsePrecisionMatrix & GetIncidentMatrix() const { return _incidentMatrix; }
		//! Return Graph's Degree Matrix
        virtual inline const gSparse::SparsePrecisionMatrix & GetDegreeMatrix() const { return _degMatrix; }
		//! Return Graph's Weight Matrix
        virtual inline const gSparse::SparsePrecisionMatrix & GetWeightMatrix() const { return _weightMatrix; }
		//! Return Graph's Laplacian Matrix
        virtual inline const gSparse::SparsePrecisionMatrix &  GetLaplacianMatrix() const { return _laplacianMatrix; }
		//! Return Graph's Edge List Matrix
        virtual inline const gSparse::EdgeMatrix & GetEdgeList() const { return _edges; }
		//! Return Graph's Weight List Matrix
        virtual inline const gSparse::PrecisionRowMatrix & GetWeightList() const { return _weights; }
        //! Return the number of Edges in the Graph
		virtual inline std::size_t GetEdgeCount() const { return _edgeCount; }
		//! Return the number of Nodes in the Graph
        virtual inline std::size_t GetNodeCount() const { return _nodeCount; }
        // A destructor
		virtual ~UndirectedGraph() = default;
	protected:
		gSparse::SparsePrecisionMatrix _adjMatrix;        //!< adjacency matrix representation
		gSparse::SparsePrecisionMatrix _degMatrix;        //!< degree matrix representation
		gSparse::SparsePrecisionMatrix _incidentMatrix;   //!< incident matrix representation
		gSparse::SparsePrecisionMatrix _weightMatrix;     //!< weight matrix representation
        gSparse::SparsePrecisionMatrix _laplacianMatrix;  //!< laplacian matrix representation

		gSparse::EdgeMatrix _edges;                        //!< edge list
		gSparse::PrecisionRowMatrix _weights;                 //!< weight list

		std::size_t _edgeCount;                            //!< count of edges
		std::size_t _nodeCount;                            //!< number of vertices
	private:
        //! Private function to perform validate and build graph representations
		virtual inline void _initializeSystem()
		{
            #ifndef NDEBUG
                // Enable input check only in DEBUG build
                // Some of the checks are expensive such as checking minCoeff with O(n) runtime.
			    _validateInput();
            #endif
			_initializeMatrixSystem();
		}
        //! validate preconditions
		void inline _validateInput()
		{
			//There are more edges than weights
			if (_edges.rows() != _weights.rows())
			{
				std::stringstream ss;
				ss << "UndirectedGraph: Edges.rows(): " << _edges.rows() << " =/= Weights.rows() " << _weights.rows() << std::endl;
				throw std::invalid_argument(ss.str());
			}
			if (_edges.cols() != 2)
			{
				std::stringstream ss;
				ss << "UndirectedGraph: Edges.cols(): must equal to two" << std::endl;
				throw std::invalid_argument(ss.str());
			}
			if (_weights.minCoeff() < 0)
			{
				std::stringstream ss;
				ss << "UndirectedGraph: Weights must be greater than zero" << std::endl;
				throw std::invalid_argument(ss.str());
			}
		}
		//! Private function to create graph representation from edge and weight list
		void inline _initializeMatrixSystem()
		{
			//Calculate counts
			_nodeCount = static_cast<std::size_t>(std::max(_edges.leftCols(1).maxCoeff(), _edges.rightCols(1).maxCoeff()) + 1);
			_edgeCount = _edges.rows();

			// Building Sparse Symmetric Adjacency Metric
			std::vector<Eigen::Triplet<gSparse::PRECISION>> adjacentList;
			std::vector<Eigen::Triplet<gSparse::PRECISION>> incidentList;
			std::vector<Eigen::Triplet<gSparse::PRECISION>> weightList;
			adjacentList.reserve(_edgeCount * 2);
			incidentList.reserve(_edgeCount * 2);
			weightList.reserve(_edgeCount);

            // Vectorized Zero
			Eigen::VectorXd degVector = Eigen::VectorXd::Zero(_nodeCount);

			for (std::size_t i = 0; i != _edgeCount; ++i)
			{
				std::size_t r = static_cast<std::size_t>(_edges(i, 0));
				std::size_t c = static_cast<std::size_t>(_edges(i, 1));

				//adjacent matrix
				adjacentList.push_back(Eigen::Triplet<gSparse::PRECISION>(r, c, _weights(i, 0)));
				adjacentList.push_back(Eigen::Triplet<gSparse::PRECISION>(c, r, _weights(i, 0)));

				//degree matrix
				degVector(r) += _weights(i, 0);
				degVector(c) += _weights(i, 0);

				//incident matrix
				if (r != c)
				{
					incidentList.push_back(Eigen::Triplet<gSparse::PRECISION>(i, r, 1));
					incidentList.push_back(Eigen::Triplet<gSparse::PRECISION>(i, c, -1));
				}
				//Weight matrix
				weightList.push_back(Eigen::Triplet<gSparse::PRECISION>(i, i, _weights(i)));
			}
			// Create adj matrix
			_adjMatrix = gSparse::SparsePrecisionMatrix(_nodeCount, _nodeCount);
			_adjMatrix.setFromTriplets(adjacentList.begin(), adjacentList.end());

			std::vector<Eigen::Triplet<gSparse::PRECISION>> degreeList;
			degreeList.reserve(degVector.size());
			for (int i = 0; i != degVector.size(); ++i)
			{
				degreeList.push_back(Eigen::Triplet<gSparse::PRECISION>(i, i, degVector(i)));
			}
			//Create degree matrix
			_degMatrix = gSparse::SparsePrecisionMatrix(_nodeCount, _nodeCount);
			_degMatrix.setFromTriplets(degreeList.begin(), degreeList.end());

			//Create incident matrix
			_incidentMatrix = gSparse::SparsePrecisionMatrix(_edgeCount, _nodeCount);
			_incidentMatrix.setFromTriplets(incidentList.begin(), incidentList.end());
			//Create weight matrix
			_weightMatrix = gSparse::SparsePrecisionMatrix(_edgeCount, _edgeCount);
			_weightMatrix.setFromTriplets(weightList.begin(), weightList.end());
            //Create Laplacian matrix
            _laplacianMatrix = _degMatrix - _adjMatrix;

		}
	};
}

#endif


#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>

#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <ros/callback_queue.h>
#include <ros/subscribe_options.h>

#include <ignition/math.hh>
#include <ignition/math/Vector3.hh>

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Wrench.h>

#include <ros/ros.h>
#include "std_msgs/String.h"

#include "fdcl/common_types.hpp"
#include "fdcl/ros_utils.hpp"
#include "fdcl/matrix_utils.hpp"

namespace igm = ignition::math;

namespace gazebo
{

class UavControlPlugin : public ModelPlugin
{

public:
    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
        this->world = _model->GetWorld();
        this->model = _model;

        this->link_name = _sdf->GetElement("bodyName")->Get<std::string>();
        this->link = _model->GetLink(this->link_name);

        // Listen to the update event. This event is broadcast every
        // simulation iteration.
        this->update_connection = event::Events::ConnectWorldUpdateBegin( \
            std::bind(&UavControlPlugin::update, this));

        // Start the ROS subscriber.
        this->topic_name = _sdf->GetElement("topicName")->Get<std::string>();
        this->sub_fm = this->n.subscribe(this->topic_name, 1, \
            UavControlPlugin::update_fm);
    }

    void update(void)
    {
        no_msg_counter++;
        if (no_msg_counter > 100)
        {
            this->reset_uav();

            if (!print_reset_message)
            {
                std::cout << ros::Time::now()
                    << ": no new force messages, resetting UAV .."
                    << std::endl;
                print_reset_message = true;
            }
            return;
        }

        // Both must be in world frame.
        this->calculate_force();
        this->link->SetForce(this->force);
        this->link->SetTorque(this->M_out);
    }

    void calculate_force(void)
    {
        this->update_uav_rotation();

        fdcl::Vector3 force_body;
        this->ignition_to_eigen(this->f, force_body);

        fdcl::Vector3 force_world = this->R * force_body;
        this->eigen_to_ignition(force_world, this->force);

        fdcl::Vector3 M_body;
        this->ignition_to_eigen(this->M, M_body);

        fdcl::Vector3 M_world = this->R * M_body;
        this->eigen_to_ignition(M_world, this->M_out);
    }

    void update_uav_rotation(void)
    {
        ignition::math::Pose3d pose = this->link->WorldPose();
        ignition::math::Quaterniond q = pose.Rot();

        fdcl::Vector3 q13(q.X(), q.Y(), q.Z());
        double q4 = q.W();

        fdcl::Matrix3 hat_q = fdcl::hat(q13);
        this->R = eye3 + 2 * q4 * hat_q + 2 * hat_q * hat_q;
    }

    static void update_fm(const geometry_msgs::Wrench::ConstPtr& msg)
    {

        f[0] = msg->force.x;
        f[1] = msg->force.y;
        f[2] = msg->force.z;

        M[0] = msg->torque.x;
        M[1] = msg->torque.y;
        M[2] = msg->torque.z;

        no_msg_counter = 0;
        print_reset_message = false;
    }

    void reset_uav(void)
    {
        this->link->SetForce(zero_fM);
        this->link->SetTorque(zero_fM);
    }

    void ignition_to_eigen(
        const ignition::math::Vector3d input, fdcl::Vector3 &output
    )
    {
        output(0) = input[0];
        output(1) = input[1];
        output(2) = input[2];
    }

    void eigen_to_ignition(
        const fdcl::Vector3 input, ignition::math::Vector3d &output
    )
    {
        output[0] = input(0);
        output[1] = input(1);
        output[2] = input(2);
    }

private:
    ros::Time t0 = ros::Time::now();

    physics::ModelPtr model;
    physics::WorldPtr world;
    physics::LinkPtr link;

    std::string link_name;
    std::string topic_name;

    event::ConnectionPtr update_connection;
    ros::Subscriber sub_fm;
    ros::NodeHandle n;

    static igm::Vector3d M;
    igm::Vector3d M_out;
    static igm::Vector3d f;

    fdcl::Matrix3 R = fdcl::Matrix3::Identity();
    igm::Vector3d force = igm::Vector3d::Zero;

    static int no_msg_counter;
    static bool print_reset_message;

    const igm::Vector3d zero_fM = igm::Vector3d::Zero;
    const fdcl::Matrix3 eye3 = fdcl::Matrix3::Identity();
};

// Register this plugin with the simulator.
GZ_REGISTER_MODEL_PLUGIN(UavControlPlugin)

}  // End of namespace gazebo.

igm::Vector3d gazebo::UavControlPlugin::f = igm::Vector3d::Zero;
igm::Vector3d gazebo::UavControlPlugin::M = igm::Vector3d::Zero;
int gazebo::UavControlPlugin::no_msg_counter = 0;
bool gazebo::UavControlPlugin::print_reset_message = false;


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

#include "gradient.hpp"
#include "hllc.hpp"
#include "mesh.hpp"
#include "slope_limiter.hpp"

// Note: this class will compute the rate of change due to the fluxes.
// Note: the reason we made this a class is that it allows you to allocate
//       buffers, once at the beginning of the simulation. Add these buffers
//       as needed.
class FluxRateOfChange {
  public:
    explicit FluxRateOfChange(int n_cells) : n_cells(n_cells) {}

    void operator()(Eigen::MatrixXd &dudt,
                    const Eigen::MatrixXd &u,
                    const Mesh &mesh) const
    {
        // Compute the rate of change of u.
        // Note: Please use the method `computeFlux` to abstract
        // away the details of computing the flux through a
        // given interface.
        // Note: You can use `assert_valid_flux` to check
        // if what `computeFlux` returns makes any sense.
        // Note: Do not assume `dudt` is filled with zeros.

        // sanity check:
        assert ((mesh.getNumberOfTriangles() == n_cells));

        // reset dudt to zero:
        dudt*= 0.0;

        // iterate over cells then over the 3 edges within each cell:
        for (int i= 0; i < n_cells; ++i)
            for (int k= 0; k < 3; ++k)
            {
                EulerState edge_flux= computeFlux(u, i, k, mesh);
                assert_valid_flux(mesh, i, k, edge_flux);
                dudt.row(i)+= edge_flux;
            }
    }

    void assert_valid_flux(const Mesh &mesh,
                           int i,
                           int k,
                           const EulerState &nF) const {
        // This is mostly for debugging (but also important to check in
        // real simulations!): Make sure our flux contribution is not
        // nan (ie. it is not not a number, ie it is a number)
        if (!euler::isValidFlux(nF)) {
            // clang-format off
            throw std::runtime_error(
                "invalid value detected in numerical flux, " + euler::to_string(nF)
                + "\nat triangle: " + std::to_string(i)
                + "\nedge:        " + std::to_string(k)
                + "\nis_boundary: " + std::to_string(!mesh.isValidNeighbour(i, k)));
            // clang-format on
        }
    }

    /// Compute the flux through the k-th interface of cell i.
    EulerState computeFlux(const Eigen::MatrixXd &U,
                           int i,
                           int k,
                           const Mesh &mesh) const
    {
        auto boundary_type = mesh.getBoundaryType(i, k);

        if (boundary_type == Mesh::BoundaryType::INTERIOR_EDGE)
        {
            return computeInteriorFlux(U, i, k, mesh);
        }
        else
        {
            if (boundary_type == Mesh::BoundaryType::OUTFLOW_EDGE)
            {
                return computeOutflowFlux(U, i, k, mesh);
            }
            else /* boundary_type == Mesh::BoundaryType::WING_EDGE */
            {
                return computeReflectiveFlux(U, i, k, mesh);
            }
        }
    }

    /// Compute the outflow flux through the k-th interface of cell i.
    /** Note: you know that edge k is an outflow edge.
     */
    EulerState computeOutflowFlux(const Eigen::MatrixXd &U,
                                  int i,
                                  int k,
                                  const Mesh &mesh) const
    {
        // Implement the outflow flux boundary condition.
        auto u_aligned= rotate_align(U.row(i), i, k, mesh);

        auto f_aligned= euler::flux(u_aligned);

        auto f= rotate_dealign(f_aligned, i, k, mesh);

        return mesh.getEdgeLength(i, k) * f; // eqn (35)
    }

    /// Compute the reflective boundary flux through the k-th edge of cell i.
    /** Note: you know that edge k is a reflective/wall boundary edge.
     */
    EulerState computeReflectiveFlux(const Eigen::MatrixXd &U,
                                     int i,
                                     int k,
                                     const Mesh &mesh) const
    {
        // Implement the reflective flux boundary condition.
        auto u= U.row(i);

        // get unit outward normal and transverse t:
        auto n= mesh.getUnitNormal(i, k).normalized();
        auto t= Eigen::Vector2d(-n(1), n(0));

        // assemble u_star:
        double rho= u[0];
        Eigen::Vector2d v= u.segment(1, 2);
        double E= u[3];

        EulerState u_star;
        u_star[0]=  rho;
        u_star.segment(1, 2)= -rho * v.dot(n) * n + rho * v.dot(t) * t;
        u_star[3]=  E;

        // rotate:
        auto u_aligned= rotate_align(u, i, k, mesh);
        auto u_star_aligned= - rotate_align(u_star, i, k, mesh); // (-) for reflection

        // flux:
        auto f_aligned= hllc(u_aligned, u_star_aligned); // equation (36)

        // derotate:
        auto f= rotate_dealign(f_aligned, i, k, mesh);

        return mesh.getEdgeLength(i, k) * f;
    }

    /// Compute the flux through the k-th interface of cell i.
    /** Note: This edge is an interior edge, therefore approximate the flux
     * through this edge with the appropriate FVM formulas.
     */
    EulerState computeInteriorFlux(const Eigen::MatrixXd &U,
                                   int i,
                                   int k,
                                   const Mesh &mesh) const
    {
        // Reconstruct the trace values of U and compute
        // the numerical flux through the k-th interface of
        // cell i.

        // figure out neighbour of i at edge k and check validity:
        int j= mesh.getNeighbour(i, k);
        assert(j >= 0);

        // figure out neighbour j's edge l collocated with i's edge k:
        int l= mesh.getNeighbourEdge(i, k);

        // reconstruction:
        auto uL= reconstruction(U.row(i), i, k, mesh);
        auto uR= reconstruction(U.row(j), j, l, mesh);

        // rotate:
        auto uL_aligned=   rotate_align(uL, i, k, mesh);
        auto uR_aligned= - rotate_align(uR, j, l, mesh);

        // flux:
        auto f_aligned= hllc(uL_aligned, uR_aligned); // equation (32)

        // derotate:
        auto f= rotate_dealign(f_aligned, i, k, mesh);

        return mesh.getEdgeLength(i, k) * f;
    }

    private:
        // generate rotation matrix to align edge k of triangle i with cartesian y-axis:
        Eigen::Matrix2d rot(int i, int k, const Mesh& mesh) const
        {
            auto n= mesh.getUnitNormal(i, k).normalized(); // why is this called UnitNormal if it's not unit normalized?

            Eigen::Matrix2d rot;
            rot << n(0),  n(1),
                  -n(1),  n(0);

            return rot;
        }

        // generate inverse rotation matrix to DEalign edge k of triangle i from cartesian y-axis back to original position:
        Eigen::Matrix2d rot_inv(int i, int k, const Mesh& mesh) const
        {
            // our rotation matrix is orthogonal: RR'=I:
            return rot(i, k, mesh).transpose();
        }

        // rotate u to align velocity component [u(1), u(2)] with cartesian x-axis:
        EulerState rotate_align(EulerState u, int i, int k, const Mesh& mesh) const
        {
            u.segment(1, 2)= rot(i, k, mesh) * u.segment(1, 2);
            return u;
        }

        // rotate calculated f back to point in original grid position before rotation:
        EulerState rotate_dealign(EulerState f, int i, int k, const Mesh& mesh) const
        {
            f.segment(1, 2)= rot_inv(i, k, mesh) * f.segment(1, 2);
        }

        // arggh there's probably no time for doing piecewise linear construction in task 2h)
        EulerState reconstruction(const Eigen::MatrixXd& U, int i, int k, const Mesh& mesh) const
        {
            // pass:
            return U.row(i);
        }

        int n_cells;
};


#define BOOST_TEST_MODULE "test_periodic_gaussian_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/local/PeriodicGaussianPotential.hpp>
#include <mjolnir/math/constants.hpp>

BOOST_AUTO_TEST_CASE(PeriodicGaussian_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();
    const real_type e  = 2.0;
    const real_type w  = 1.0;
    const real_type r0 = 3.0;

    mjolnir::PeriodicGaussianPotential<real_type> gaussian(e, w, r0);

    const real_type x_min = -2 * pi;
    const real_type x_max =  2 * pi;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        const real_type pot1 = gaussian.potential(x + h);
        const real_type pot2 = gaussian.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = gaussian.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(PeriodicGaussian_float)
{
    using real_type = float;
    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();
    const real_type e  = 2.0;
    const real_type w  = 1.0;
    const real_type r0 = 3.0;

    mjolnir::PeriodicGaussianPotential<real_type> gaussian(e, w, r0);

    const real_type x_min = -2 * pi;
    const real_type x_max =  2 * pi;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        const real_type pot1 = gaussian.potential(x + h);
        const real_type pot2 = gaussian.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = gaussian.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}