/*
   Implementation of "Highly efficient incremental estimation of Gaussian mixture models for online data stream clustering."

   Used to merge multiple mixture models

   Author: Ryan Watson

 */

#include <omp.h>
#include <vector>
#include <numeric>
#include <stdexcept>
#include <iostream>
#include <Eigen/Dense>

#include "merge.h"
#include "libcluster.h"
#include "probutils.h"
#include "comutils.h"
#include "distributions.h"

#include <boost/math/distributions/fisher_f.hpp>
#include <boost/math/distributions/chi_squared.hpp>

using namespace std;
using namespace Eigen;
using namespace boost;
using namespace comutils;
using namespace probutils;
using namespace libcluster;
using namespace boost::math;
using namespace distributions;

using boost::math::chi_squared;
using boost::math::quantile;
using boost::math::complement;

//
// /*! \brief Merge similar mixture components as specified in the reference provided below.
//
//    REF ::
//    [1] Song, Mingzhou, and Hongbin Wang. "Highly efficient incremental estimation of Gaussian mixture models for online data stream clustering." Intelligent Computing: Theory and Applications III. Vol. 5803. International Society for Optics and Photonics, 2005.
//  */
//
//
// Author: Ryan Watson
//

bool merge::checkCov(Eigen::MatrixXd data, Eigen::MatrixXd qZ, mixtureComponents priorModel, GaussWish currModel, double currWeight, int currModelIdx, double alpha)
{
        Eigen::MatrixXd transObs, chol, chol_inv, tmp;

        chol = (priorModel.get<4>()).llt().matrixL();
        chol_inv = chol.inverse();

        int n = currModel.getN() + 1;
        int d = (currModel.getmean()).size() + 1;

        chi_squared dist((d*(d+1))/2);
        double ucv = quantile(complement(dist, alpha));
        double ucv2 = quantile(complement(dist, alpha/2.0));
        double lcv = quantile(dist, alpha);
        double lcv2 = quantile(dist, alpha/2.0);

        ArrayXi mapidx = partobs(data, (qZ.col(currModelIdx).array()>0.5), tmp);
        transObs.setZero(tmp.rows(),d);
        for (int k = 0; k < tmp.rows(); k++)
        {
                transObs.row(k) = chol_inv * tmp.row(k);
        }

        if (transObs.rows() < 3) {return false; }
        auto transCov = probutils::cov(transObs);
        auto covDiff = transCov - Eigen::MatrixXd::Identity(d,d);
        auto covDiff_sq = covDiff * covDiff;
        double dd = static_cast<double>(d);
        double nn = static_cast<double>(n);
        double f = (1.0/dd) * ( (covDiff_sq).trace() );
        double s = (dd/nn) * pow( (1.0/dd)*(transCov.trace()),2 );
        double W = f - s + dd/nn;
        double t_stat = ((nn*W*dd)/2.0);
        if ( t_stat < ucv2 || t_stat > lcv2 ) { return true; }
        return false;
}

bool merge::checkCovGMM(mixtureComponents prior, mixtureComponents test, double alpha)
{
        Eigen::MatrixXd test_cov, chol, chol_inv;

        chol = (test.get<4>()).llt().matrixL();
        chol_inv = chol.inverse();
        test_cov = chol_inv * test.get<4>() * chol_inv.transpose();

        int n = prior.get<1>() + 1;
        int d = (prior.get<3>()).size() + 1;

        chi_squared dist((d*(d+1))/2);
        double ucv = quantile(complement(dist, alpha));
        double ucv2 = quantile(complement(dist, alpha/2.0));
        double lcv = quantile(dist, alpha);
        double lcv2 = quantile(dist, alpha/2.0);

        auto covDiff = test_cov - Eigen::MatrixXd::Identity(d,d);
        auto covDiff_sq = covDiff * covDiff;
        double dd = static_cast<double>(d);
        double nn = static_cast<double>(n);
        double f = (1.0/dd) * ( (covDiff_sq).trace() );
        double s = (dd/nn) * pow( (1.0/dd)*(test_cov.trace()),2 );
        double W = f - s + dd/nn;
        double t_stat = ((nn*W*dd)/2.0);
        if ( t_stat < ucv2 || t_stat > lcv2 ) { return true; }
        return false;
}

bool merge::checkMean(mixtureComponents priorModel, GaussWish currModel, double alpha)
{

        int n = currModel.getN() + 1;
        int d = (currModel.getmean()).size() + 1;
        if (n-d <= 1) {return false; }
        fisher_f dist(d, n-d);

        double ucv = quantile(complement(dist, alpha));
        double ucv2 = quantile(complement(dist, alpha/2.0));
        double lcv = quantile(dist, alpha);
        double lcv2 = quantile(dist, alpha/2.0);

        Eigen::RowVectorXd meanDiff = currModel.getmean() - priorModel.get<3>();
        double t = (meanDiff * currModel.getcov() * meanDiff.transpose());
        double t_stat = (double)(n-d)/(double)(d*(n-1)) * pow(t,2.0);
        if (t_stat < ucv) {  return true; }
        return false;
}

bool merge::checkMeanGMM(mixtureComponents prior, mixtureComponents test, double alpha)
{

        int n = test.get<1>() + 1;
        int d = (test.get<3>()).size() + 1;
        if (n-d <= 1) {return false; }

        fisher_f dist(d, n-d);

        double ucv = quantile(complement(dist, alpha));
        double ucv2 = quantile(complement(dist, alpha/2.0));
        double lcv = quantile(dist, alpha);
        double lcv2 = quantile(dist, alpha/2.0);

        Eigen::RowVectorXd meanDiff = prior.get<3>() - test.get<3>();
        double t = (meanDiff * test.get<4>() * meanDiff.transpose());
        double t_stat = (double)(n-d)/(double)(d*(n-1)) * pow(t,2.0);
        if (t_stat < ucv) {  return true; }
        return false;
}

vector<double> merge::getPriorWeights(vector<mixtureComponents> gmm)
{
        vector<double> weights;
        for (unsigned int i=0; i<gmm.size(); i++)
        {
                weights.push_back(gmm[i].get<2>());
        }
        return weights;
}

bool merge::checkComponent(Eigen::MatrixXd data, Eigen::MatrixXd qZ, mixtureComponents priorModel, GaussWish currModel, double currWeight, int currModelIdx, double alpha)
{
        if (checkCov(data, qZ, priorModel, currModel, currWeight, currModelIdx, alpha))
        {
                if (checkMean(priorModel, currModel, alpha))
                        return true;
                else
                        return false;
        }
        return false;
}

bool merge::checkComponentGMM(mixtureComponents prior, mixtureComponents test, double alpha)
{
        if (checkCovGMM(prior, test, alpha))
        {
                if (checkMeanGMM(prior, test, alpha))
                        return true;
                else
                        return false;
        }
        return false;
}

bool merge::sortbyobs(merge::mixtureComponents a, merge::mixtureComponents b)
{
        return (a.get<1>() < b.get<1>());
}

vector<merge::mixtureComponents> merge::updateObs(vector<merge::mixtureComponents> gmm, vector<int> numObs)
{
        unsigned int N(0);
        for (unsigned int i=0; i<gmm.size(); i++)
        {
                N += numObs.at(i);
        }

        for (unsigned int i=0; i<gmm.size(); i++)
        {
                // update total num of obs.
                gmm[i].get<0>() += N;
                if (numObs.at(i) != 0 )
                {
                        // update num of obs in component
                        gmm[i].get<1>() += numObs.at(i);
                        // update components weight.
                        gmm[i].get<2>() = gmm[i].get<1>() / double(gmm[i].get<0>());
                }
        }
        return gmm;
}

vector<merge::mixtureComponents> merge::pruneMixtureModel(vector<merge::mixtureComponents> gmm, int truncLevel)
{
        if (gmm.size() < truncLevel)
                return gmm;

        merge::mixtureComponents currComponent;
        sort(gmm.begin(), gmm.end(), sortbyobs);
        while (gmm.size() > truncLevel)
        {
                currComponent = gmm[0];
                int n = currComponent.get<1>();
                for (unsigned int i=1; i<gmm.size(); i++)
                {
                        // update number of obs. for each component
                        gmm[i].get<0>() = gmm[i].get<0>() - n;
                        // update weight for each component
                        gmm[i].get<2>() = (double)gmm[i].get<1>() / (double)gmm[i].get<0>();
                }
                gmm.erase(gmm.begin());
        }

        return gmm;
}

vector<merge::mixtureComponents> merge::mergeMixtureModel(Eigen::MatrixXd data, Eigen::MatrixXd qZ, vector<merge::mixtureComponents> priorModel, vector<GaussWish> currModel, StickBreak currWeight, double alpha, int truncLevel)
{
        vector<merge::mixtureComponents> gmm;
        int dataCard = data.rows();

        auto pWeights =  getPriorWeights(priorModel);
        auto cWeights =  currWeight.Elogweight().exp().transpose();
        int cc = 0;
        vector<int> pModelMatches;
        vector<int> cModelMatches;

        if ( priorModel.size() == 0)
        {
                for (vector<GaussWish>::iterator j = currModel.begin(); j < currModel.end(); ++j)
                {
                        // num obs.
                        int n = j->getN();
                        // cluster weights
                        double w = cWeights[cc];
                        // cluster mean
                        auto m = j->getmean();
                        // cluster cov
                        auto c = j->getcov();
                        // add new component to mixture model
                        gmm.push_back(boost::make_tuple(dataCard, n, w, m, c));
                        cc+=1;
                }
                return gmm;
        }

        // remove cluster without sufficient obs.
        int m_inc = 0;
        for (vector<GaussWish>::iterator j = currModel.begin(); j < currModel.end(); ++j)
        {
                if (j->getN() < 2.0) {
                        currModel.erase(currModel.begin() + m_inc);
                }
                m_inc++;
        }

        for (unsigned int i=0; i<priorModel.size(); i++)
        {

                for (vector<GaussWish>::iterator j = currModel.begin(); j < currModel.end(); ++j)
                {
                        if( checkComponent(data, qZ, priorModel[i], *j, cWeights[cc], cc, alpha)
                            && !(std::find(pModelMatches.begin(), pModelMatches.end(), i) != pModelMatches.end())
                            && !(std::find(cModelMatches.begin(), cModelMatches.end(), cc) != cModelMatches.end()) )
                        {
                                // merge components
                                mixtureComponents pModel = priorModel[i];
                                // update mean and cov based on eq.

                                int NP = pModel.get<0>();
                                int n = pModel.get<1>() +j->getN();
                                double WP = pModel.get<2>();
                                Eigen::RowVectorXd MP = pModel.get<3>();
                                Eigen::MatrixXd CM = pModel.get<4>();
                                double NPWP = NP*WP;

                                Eigen::RowVectorXd MC = j->getmean();
                                Eigen::MatrixXd CC = j->getcov();
                                int NC = j->getN();

                                auto denom = NPWP + NC;

                                /// update mean --> REF:: [1] Eq. 6
                                Eigen::RowVectorXd n_mean = (NPWP*MP + NC*MC )*(1/denom);

                                Eigen::MatrixXd A = (NPWP*CM + NC*CC)*(1/denom);

                                Eigen::MatrixXd cov_p = MP.transpose()*MP;
                                Eigen::MatrixXd cov_c = MC.transpose()*MC;

                                Eigen::MatrixXd B = ( NPWP*cov_p + NC*cov_c )*(1/denom);

                                Eigen::MatrixXd C = n_mean.transpose() * n_mean;

                                /// update cov --> REF:: [1] Eq. 7
                                Eigen::MatrixXd n_cov = A + B - C;

                                double w =  (NPWP + NC) /(double) (NP + dataCard);

                                gmm.push_back(boost::make_tuple(NP+dataCard, n, w, n_mean, n_cov));
                                pModelMatches.push_back(i);
                                cModelMatches.push_back(cc);
                        }
                        cc+=1;
                }
                cc=0;
        }

        // Add all unmerged components from new GMM
        int idx = 0;
        for (vector<GaussWish>::iterator j = currModel.begin(); j < currModel.end(); ++j)
        {
                if (!(std::find(cModelMatches.begin(), cModelMatches.end(), idx) != cModelMatches.end()))
                {
                        Eigen::RowVectorXd MC = j->getmean();
                        Eigen::MatrixXd CC = j->getcov();
                        int NC = j->getN();
                        int n = priorModel[0].get<0>() + dataCard;

                        double w = NC/(double)n;

                        gmm.push_back(boost::make_tuple(n, NC, w, MC, CC));
                }

                idx+=1;
        }

        // Add all unmerged components from previous GMM
        idx = 0;
        for (unsigned int i=0; i<priorModel.size(); i++)
        {
                mixtureComponents pModel;
                pModel = priorModel[i];
                if (!(std::find(pModelMatches.begin(), pModelMatches.end(), idx) != pModelMatches.end()))
                {
                        int NP = pModel.get<1>();
                        int n = pModel.get<0>() + dataCard;
                        double w = NP/(double)n;
                        Eigen::RowVectorXd MP = pModel.get<3>();
                        Eigen::MatrixXd CM = pModel.get<4>();

                        gmm.push_back(boost::make_tuple(n, NP, w, MP, CM));
                }

                idx+=1;
        }

        // Merge equilavent components in new GMM
        for (unsigned int i=0; i<gmm.size(); i++)
        {
                for (unsigned int j=0; j<gmm.size(); j++)
                {
                        if (i==j) {continue; }
                        if (checkComponentGMM(gmm[i], gmm[j], alpha))
                        {

                                int NP = gmm[i].get<0>();
                                int n = gmm[i].get<1>();
                                double WP = gmm[i].get<2>();
                                Eigen::RowVectorXd MP = gmm[i].get<3>();
                                Eigen::MatrixXd CM = gmm[i].get<4>();
                                double NPWP = NP*WP;

                                int NH = gmm[j].get<0>();
                                int nh = gmm[j].get<1>();
                                double WH = gmm[j].get<2>();
                                Eigen::RowVectorXd MH = gmm[j].get<3>();
                                Eigen::MatrixXd CH = gmm[j].get<4>();
                                double NHWH = NH*WH;

                                auto denom = NPWP + NHWH;

                                /// update mean --> REF:: [1] Eq. 6
                                Eigen::RowVectorXd n_mean = (NPWP*MP + NHWH*MH )*(1/denom);

                                Eigen::MatrixXd A = (NPWP*CM + NHWH*CH)*(1/denom);

                                Eigen::MatrixXd cov_p = MP.transpose()*MP;
                                Eigen::MatrixXd cov_c = MH.transpose()*MH;

                                Eigen::MatrixXd B = ( NPWP*cov_p + NHWH*cov_c )*(1/denom);

                                Eigen::MatrixXd C = n_mean.transpose() * n_mean;

                                gmm[i].get<1>() = (n + nh);
                                gmm[i].get<2>() = gmm[i].get<1>()/(double) (NP);
                                gmm[i].get<3>() = n_mean;
                                /// update cov --> REF:: [1] Eq. 7
                                gmm[i].get<4>() = A + B - C;

                                // remove merged component from global GMM
                                gmm.erase(gmm.begin() + j);
                        }
                }

        }

        // prune the model to specified truncation level
        gmm = pruneMixtureModel(gmm, truncLevel);

        return gmm;
}

merge::observationModel merge::getMixtureComponent(vector<merge::mixtureComponents> gmm, Eigen::VectorXd observation)
{

        int idx = 0;
        double minProb = 0, modelProb;
        Eigen::RowVectorXd mean;
        Eigen::RowVectorXd obs = observation.transpose();
        Eigen::MatrixXd cov;
        double doublePi = M_PI * 2.0;

        for (int i=0; i<gmm.size(); i++)
        {

                mean = gmm[i].get<3>();
                cov = gmm[i].get<4>();
                double err = (obs-mean)*cov.inverse()*(observation-mean.transpose());
                modelProb = -1 * log(err);

                if (modelProb < minProb)
                {
                        minProb = modelProb;
                        idx = i;
                }
        }

        merge::mixtureComponents gmmComp = gmm[idx];
        merge::observationModel obsModel = boost::make_tuple(gmmComp, minProb);

        return obsModel;
}


//==============================================================================
//         Copyright 2015 J.T.Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/euler/include/functions/betaln.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/functions/splat.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/inf.hpp>

NT2_TEST_CASE_TPL ( betaln_real,  NT2_SIMD_REAL_TYPES)
{
  using nt2::betaln;
  using nt2::tag::betaln_;
  using boost::simd::native;
  using nt2::splat;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::call<betaln_(vT,vT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(betaln(nt2::Nan<vT>(), nt2::Nan<vT>()), nt2::Nan<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(betaln(nt2::Inf<vT>(), nt2::Inf<vT>()), nt2::Nan<r_t>(), 0);
#endif
  T r[5] = {T(-1.609437912434100),  T(-2.995732273553991),  T(-3.401197381662155),  T(-2.995732273553991),  T(  -1.609437912434100)};

  for(int i = 1; i <= 5 ; i+= 1)
  {
    vT v1 =  splat<vT>(i);
    vT v2 =  splat<vT>(6-i);
    NT2_TEST_ULP_EQUAL(betaln(v1, v2), splat<vT>(r[i-1]), 0.5);
  }

}


/*! \file hydrogen_fem.cpp
    \brief FEMで水素原子に対するSchrödinger方程式を解くクラスの実装

    Copyright © 2019 @dc1394 All Rights Reserved.
    (but this is originally adapted by sunsetyuhi for fem1d_poisson.py from https://github.com/sunsetyuhi/fem_py/blob/master/fem1d_poisson )
    This software is released under the BSD 2-Clause License.
*/

#include "hydrogen_fem.h"
#include <cmath>                // for std::exp
#include <cstdio>               // for FILE, std::fclose, std::fopen, std::fprintf
#include <memory>               // for std::unique_ptr
#include <boost/assert.hpp>     // for BOOST_ASSERT
#include <Eigen/Eigenvalues>    // for Eigen::GeneralizedSelfAdjointEigenSolver

namespace hydrogen_fem {
    // #region コンストラクタ

    Hydrogen_FEM::Hydrogen_FEM()
        :   hg_(Eigen::MatrixXd::Zero(NODE_TOTAL, NODE_TOTAL)),
            length_(ELE_TOTAL),
            mat_A_ele_(boost::extents[ELE_TOTAL][2][2]),
            mat_B_ele_(boost::extents[ELE_TOTAL][2][2]),
            node_num_seg_(boost::extents[ELE_TOTAL][2]),
            node_r_ele_(boost::extents[ELE_TOTAL][2]),
            node_r_glo_(NODE_TOTAL),
            ug_(Eigen::MatrixXd::Zero(NODE_TOTAL, NODE_TOTAL))
    {
    }

    // #endregion コンストラクタ

    // #region publicメンバ関数

    double Hydrogen_FEM::do_run()
    {
        // 各種データの生成
        make_data();

        // 要素行列の生成
        make_element_matrix();

        // 全体行列を生成
        make_global_matrix();

        // 境界条件処理を行う
        boundary_conditions();

        // 一般化固有値問題を解く
        Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es(hg_, ug_);

        // エネルギー固有値を取得
        eigenval_ = es.eigenvalues();

        // 基底状態の固有関数（波動関数）（波動関数）を取得
        phi_ = es.eigenvectors().col(0);

        // 固有ベクトル（波動関数）のN要素目を追加
        phi_.resize(NODE_TOTAL);

        // 固有ベクトル（波動関数）を規格化
        normalize();

        return eigenval_[0];
    }

    void Hydrogen_FEM::save_result() const
    {
        std::unique_ptr<FILE, decltype(&std::fclose)> fp_eigenfunc(std::fopen(Hydrogen_FEM::EIGENFUNC_FILENAME, "w"), std::fclose);

        for (auto i = 0; i < NODE_TOTAL; i++) {
            auto const r = node_r_glo_[i];
            // 厳密な結果と比較
            std::fprintf(fp_eigenfunc.get(), "%.14f, %.14f, %.14f\n", r, phi_[i], 2.0 * std::exp(-r));
        }

        std::unique_ptr<FILE, decltype(&std::fclose)> fp_eigenval(std::fopen(Hydrogen_FEM::EIGENVAL_FILENAME, "w"), std::fclose);

        for (auto i = 0; i < NODE_TOTAL - 1; i++) {
            // 厳密な結果と比較
            std::fprintf(fp_eigenval.get(), "%d, %.14f, %.14f\n", i + 1, eigenval_[i], - 0.5 / static_cast<double>((i + 1) * (i + 1)));
        }

    }

    // #endregion publicメンバ関数

    // #region privateメンバ関数

    void Hydrogen_FEM::boundary_conditions()
    {
        // 左辺の全体行列のN + 1行とN + 1列を削る
        hg_.conservativeResize(hg_.rows() - 1, hg_.cols() - 1);

        // 右辺の全体行列のN + 1行とN + 1列を削る
        ug_.conservativeResize(ug_.rows() - 1, ug_.cols() - 1);
    }

    double Hydrogen_FEM::get_A_matrix_element(std::int32_t e, double le, std::int32_t p, std::int32_t q) const
    {
        auto const ed = static_cast<double>(e);
        switch (p) {
        case 0:
            switch (q) {
            case 0:
                return 0.5 * le * (ed * ed + ed + 1.0 / 3.0) - le * le * (ed / 3.0 + 1.0 / 12.0);

            case 1:
                return -0.5 * le * (ed * ed + ed + 1.0 / 3.0) - le * le * (ed / 6.0 + 1.0 / 12.0);

            default:
                BOOST_ASSERT(!"heの添字が2以上！");
                return 0.0;
            }

        case 1:
            switch (q) {
            case 0:
                return -0.5 * le * (ed * ed + ed + 1.0 / 3.0) - le * le * (ed / 6.0 + 1.0 / 12.0);

            case 1:
                return 0.5 * le * (ed * ed + ed + 1.0 / 3.0) - le * le * (ed / 3.0 + 0.25);

            default:
                BOOST_ASSERT(!"heの添字が2以上！");
                return 0.0;
            }

        default:
            BOOST_ASSERT(!"heの添字が2以上！");
            return 0.0;
        }
    }

    double Hydrogen_FEM::get_B_matrix_element(std::int32_t e, double le, std::int32_t p, std::int32_t q) const
    {
        auto const ed = static_cast<double>(e);
        switch (p) {
        case 0:
            switch (q) {
            case 0:
                return le * le * le * (ed * ed / 3.0 + ed / 6.0 + 1.0 / 30.0);

            case 1:
                return le * le * le * (ed * ed / 6.0 + ed / 6.0 + 0.05);

            default:
                BOOST_ASSERT(!"ueの添字が2以上！");
                return 0.0;
            }

        case 1:
            switch (q) {
            case 0:
                return le * le * le * (ed * ed / 6.0 + ed / 6.0 + 0.05);

            case 1:
                return le * le * le * (ed * ed / 3.0 + ed / 2.0 + 0.2);

            default:
                BOOST_ASSERT(!"ueの添字が2以上！");
                return 0.0;
            }

        default:
            BOOST_ASSERT(!"ueの添字が2以上！");
            return 0.0;
        }
    }

    void Hydrogen_FEM::make_element_matrix()
    {
        // 各線分要素の長さを計算
        for (auto e = 0; e < ELE_TOTAL; e++) {
            length_[e] = std::fabs(node_r_ele_[e][1] - node_r_ele_[e][0]);
        }

        // 要素行列の各成分を計算
        for (auto e = 0; e < ELE_TOTAL; e++) {
            auto const le = length_[e];
            for (auto i = 0; i < 2; i++) {
                for (auto j = 0; j < 2; j++) {
                    mat_A_ele_[e][i][j] = get_A_matrix_element(e, le, i, j);
                    mat_B_ele_[e][i][j] = get_B_matrix_element(e, le, i, j);
                }
            }
        }
    }

    void Hydrogen_FEM::make_data()
    {
        // Global節点のx座標を定義(R_MIN～R_MAX）
        auto const dr = (R_MAX - R_MIN) / static_cast<double>(ELE_TOTAL);
        for (auto i = 0; i <= ELE_TOTAL; i++) {
            // 計算領域を等分割
            node_r_glo_[i] = R_MIN + static_cast<double>(i) * dr;
        }

        for (auto e = 0; e < ELE_TOTAL; e++) {
            node_num_seg_[e][0] = e;
            node_num_seg_[e][1] = e + 1;
        }

        for (auto e = 0; e < ELE_TOTAL; e++) {
            for (auto i = 0; i < 2; i++) {
                node_r_ele_[e][i] = node_r_glo_[node_num_seg_[e][i]];
            }
        }
    }

    void Hydrogen_FEM::make_global_matrix()
    {
        for (auto e = 0; e < ELE_TOTAL; e++) {
            for (auto i = 0; i < 2; i++) {
                for (auto j = 0; j < 2; j++) {
                    hg_(node_num_seg_[e][i], node_num_seg_[e][j]) += mat_A_ele_[e][i][j];
                    ug_(node_num_seg_[e][i], node_num_seg_[e][j]) += mat_B_ele_[e][i][j];
                }
            }
        }
    }

    void Hydrogen_FEM::normalize()
    {
        auto sum = 0.0;
        auto const size = phi_.size();
        auto const max = size - 2;

        // Simpsonの公式によって数値積分する
        for (auto i = 0; i < max; i += 2) {
            auto const f0 = phi_[i] * phi_[i] * node_r_glo_[i] * node_r_glo_[i];
            auto const f1 = phi_[i + 1] * phi_[i + 1] * node_r_glo_[i + 1] * node_r_glo_[i + 1];
            auto const f2 = phi_[i + 2] * phi_[i + 2] * node_r_glo_[i + 2] * node_r_glo_[i + 2];
            sum += (f0 + 4.0 * f1 + f2);
        }

        auto const a_1 = 1.0 / std::sqrt(sum * length_[0] / 3.0);

        for (auto i = 0; i < size; i++) {
            phi_[i] *= -a_1;
        }
    }

    // #endregion privateメンバ関数
}


/*

// Struct_definition.h defines different types of sturctures being used in the implementation of MSCKF

*/

#ifndef STRUCT_DEF_H

#define STRUCT_DEF_H

#include <Eigen/Dense>

#include <map>

#include <set>

#include "Parameters.hpp"

#include <deque>

#define PI 3.1415926535

#define NaN -1

typedef Eigen::Matrix<double, 15, 15> Matrix15;

typedef Eigen::Matrix<double, 16, 16> Matrix16;

typedef Eigen::Matrix<double,4,1> quaternion;

struct Feature

{

	/* ----- Structure to store Camera Feature Points ----- */

	double u;

	double v;

};

struct IMURawData

{

	Eigen::Vector3d imuData;

	double timeStamp;

};

struct IMUdata

{

	/* --------- Structure to store Raw IMU Data ---------- */

	Eigen::Vector3d a;

	Eigen::Vector3d g;

	/* -------- Operator to copy structure members -------- */

	IMUdata& operator=(const IMUdata& old)

	{

		a = old.a;

		g = old.g;

		return *this;

	}

};

struct StampedIMUData

{

	double timestamp;

	IMUdata imudata;

};

struct StampedAccData{

	double timestamp;

	Eigen::Vector3d accdata;

};

struct StampedGyroData{

	double timestamp;

	Eigen::Vector3d gyrodata;

};

struct CalibrationParameter{

	float  Td;

	CalibrationParameter()

	{

		 Td = 0;

	}

};

struct IMUstate

{

	/* ------ Structure of IMU Part of State Vector ------- */

	Eigen::Vector3d p_b_G;

	Eigen::Vector3d v_b_G;

	quaternion q_B_G;

	Eigen::Vector3d ba;

	Eigen::Vector3d bg;

	CalibrationParameter calibParam;

	Eigen::Vector3d w_b;

	/* ------------------- Constructor -------------------- */

	IMUstate()

	{

		p_b_G.setZero();

		v_b_G.setZero();

		q_B_G.setIdentity();

		ba.setZero();

		bg.setZero();

		calibParam. Td = 0;

		w_b.setZero();

	}

	/* -------- Operator to copy structure members -------- */

	IMUstate& operator=(const IMUstate& old)

	{

		p_b_G = old.p_b_G;

		v_b_G = old.v_b_G;

		q_B_G = old.q_B_G;

		ba = old.ba;

		bg = old.bg;

		w_b = old.w_b;

		calibParam = old.calibParam;

		return *this;

	}

};

struct IMUGroundTruth

{

	/* ------ Structure of IMU Part of State Vector ------- */

	double timeStamp;

	Eigen::Vector3d p_b_G;

	Eigen::Vector3d v_b_G;

	quaternion q_B_G;

	Eigen::Vector3d bias_w;

	Eigen::Vector3d bias_a;

	/* ------------------- Constructor -------------------- */

	IMUGroundTruth()

	{

		timeStamp = 0.0;

		p_b_G.setZero();

		v_b_G.setZero();

		q_B_G.setIdentity();

		bias_a.setZero();

		bias_w.setZero();

	}

	/* -------- Operator to copy structure members -------- */

	IMUGroundTruth& operator=(const IMUGroundTruth& old)

	{

		p_b_G = old.p_b_G;

		v_b_G = old.v_b_G;

		q_B_G = old.q_B_G;

		bias_a = old.bias_a;

		bias_w = old.bias_w;

		timeStamp = old.timeStamp;

		return *this;

	}

};

struct Bodystate

{

	/* --- Structure of Camera/Body Part of State Vector -- */

	int state_k;

	Eigen::Vector3d p_b_G;

	Eigen::Vector3d v_b_G;

	quaternion q_B_G;

	Eigen::Vector3d w_b;

	std::set<int> trackedFeatureID;   // TODO:: need to be modified as a set

	/* -------- Operator to copy structure members -------- */

	Bodystate& operator=(const Bodystate& old)

	{

		state_k = old.state_k;

		p_b_G = old.p_b_G;

		v_b_G = old.v_b_G;

		q_B_G = old.q_B_G;

		trackedFeatureID = old.trackedFeatureID;

		return *this;

	}

	Bodystate()

	{

		state_k = 0;

		p_b_G.setZero();

		v_b_G.setZero();

		q_B_G.setIdentity();

		trackedFeatureID.clear();

	}

};

struct Camstate{

	Eigen::Vector3d p_c_G;

	quaternion q_C_G;

	Camstate(){

		p_c_G.setZero();

		q_C_G.setIdentity();

	}

};

struct FeatureTrack

{

	/* - Structure of Features and corresponding Body States - */

	int featureID;

	std::vector<Feature> observations;

	std::map<int, std::vector<Feature>, std::less<int>, Eigen::aligned_allocator<std::pair<int, std::vector<Feature>>>> featureTracks;

	std::vector<int> bodyStateID;

	std::vector<Bodystate, Eigen::aligned_allocator<Bodystate>> bodyStateBuffer;

	/* -------- Operator to copy structure members -------- */

	FeatureTrack& operator=(const FeatureTrack& old)

	{

		featureID = old.featureID;

		observations = old.observations;

		featureTracks = old.featureTracks;

		bodyStateID = old.bodyStateID;

		bodyStateBuffer = old.bodyStateBuffer;

		return *this;

	}

	FeatureTrack(){

		featureID = 0;

		observations.clear();

		featureTracks.clear();

		bodyStateID.clear();

		bodyStateBuffer.clear();

	}

};

struct StateVector

{
	/* -------- Structure of Complete State Vector -------- */

	IMUstate imuState;

	Eigen::Matrix<double, 16, 16> imuCovariance; // imu + calib

	Eigen::MatrixXd bodyCovariance;

	Eigen::MatrixXd imuBodyCovariance;

	std::vector<Bodystate, Eigen::aligned_allocator<Bodystate>> bodyStateBuffer;

	//CalibrationParameter camCalibState;

	//Eigen::MatrixXd camCalibCovariance;

	/* -------- Operator to copy structure members -------- */

	StateVector& operator=(const StateVector& old)

	{

		imuState = old.imuState;

		imuCovariance = old.imuCovariance;

		bodyCovariance = old.bodyCovariance;

		imuBodyCovariance = old.imuBodyCovariance;

		bodyStateBuffer = old.bodyStateBuffer;

		//camCalibState = old.camCalibState;

		//camCalibCovariance = old.camCalibCovariance;

		return *this;

	}

	StateVector(){

		imuCovariance.setZero();

		bodyCovariance.setZero();

		imuBodyCovariance.setZero();

		//bodyStateBuffer.clear();

		//camCalibCovariance.setZero();

	}

};

struct ImageNameStruct{

	std::string imageName;

	double imageTimeStamp;

};
typedef struct IMUMeanData_{

	double gx_mean;
	double gy_mean;
	double gz_mean;

	double ax_mean;
	double ay_mean;
	double az_mean;

}IMUMeanData;

void getParameter(std::string cameParam,std::string imuParam,cameraParameters & camParam,	noiseParameters & initNoiseParam);

std::vector<ImageNameStruct> getImageList( std::ifstream& imageListFile);

std::vector<StampedIMUData> getIMUReading(std::ifstream& IMURecordFile);
std::vector<StampedIMUData> getIMUReadingEuroc(std::ifstream& IMURecordFile);

std::vector<StampedIMUData> getIMUReadingDvt(std::ifstream& IMURecordFile);

std::vector<ImageNameStruct> getImageListDvt( std::ifstream& imageListFile);

std::vector<StampedAccData> getAccReading(std::ifstream & AccRecordFile);

std::vector<StampedGyroData> getGyroReading(std::ifstream & gyroRecordFile);
std::vector<IMUGroundTruth> getGroundTruthEuroc(std::ifstream & groundTruthFile);

#endif


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include "DatasetAR.h"
#include "GaussSeq.h"
#include "ARSeq.h"
#include "utils.h"

using utils::my_float;

int main() {
    using boost::multiprecision::cpp_bin_float_50;
    using boost::random::uniform_real_distribution;

    boost::random::mt19937 gen {};
    uniform_real_distribution<my_float> u (0, 4);

    // prepare target
    std::map<int, my_float> coeff {
        {3, 0.2},
        {20, 0.5},
    };
    std::vector<my_float> v;
    for (auto i = 0; i < 20; i++) {
        v.push_back(u(gen));
    }

    // AR process
    ARSeq targ_seq(coeff, 0);
    targ_seq.seed_prev_vals(v);

    // Dataset write
    DatasetAR dat_0("testing_zero_background", targ_seq, 0);
    DatasetAR dat_1("testing_one_background", targ_seq, 1);
    DatasetAR dat_2("testing_two_background", targ_seq, 2);
    DatasetAR dat_3("testing_three_background", targ_seq, 3);
    DatasetAR dat_4("testing_four_background", targ_seq, 4);

    dat_0.write_csv();
    dat_1.write_csv();
    dat_2.write_csv();
    dat_3.write_csv();
    dat_4.write_csv();

    return 0;
}


/**
 * \file boost/numeric/ublasx/operation/svd.hpp
 *
 * \brief Singular Value Decomposition problem.
 *
 * The singular value decomposition (SVD) of a m-by-n real/complex
 * matrix \f$A\f$ is:
 * \f[
 *   A = U \Sigma V^{H}
 * \f]
 * where \f$\Sigma\f$ is an m-by-n matrix which is zero except for its
 * \f$\min(m,n)\f$ diagonal elements, \f$U\f$ is an m-by-m unitary matrix, and
 * \f$V\f$ is an n-by-n unitary matrix.
 * The diagonal elements of \f$\Sigma\f$ are the singular values of
 * \f$A\f$; they are real and non-negative, and are returned in descending order.
 * The first \f$\min(m,n)\f$ columns of \f$U\f$ and \f$V\f$ are the left and
 * right singular vectors of \f$A\f$.
 *
 * When an economy-size SVD is requested, if \f$k=\min(m.n)\f$, it results that
 * \f$\Sigma\f$ is a k-by-k diagonal matrix, \f$U\f$ is an m-by-k unitary matrix
 * and \f$V\f$ is an n-by-k unitary matrix.
 * In this case the original matrix \f$A\f$ cannot be reconstructed.
 *
 * \note
 *  For real m-by-n matrix \f$A\f$ the associated SVD is:
 * \f[
 *   A = U \Sigma V^{T}
 * \f]
 *
 *

 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_SVD_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_SVD_HPP

#include <algorithm>
#include <boost/numeric/bindings/lapack/driver/gesvd.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/detail/lapack.hpp>
#include <boost/numeric/ublasx/operation/diag.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/traits/layout_type.hpp>
#include <boost/type_traits/is_complex.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail { namespace /*<unnamed>*/ {

template <
	typename AMatrixT,
	typename SVectorT,
	typename UMatrixT,
	typename VTMatrixT
>
void svd_impl(AMatrixT const& A, SVectorT& s, bool want_U, bool full_U, UMatrixT& U, bool want_VT, bool full_VT, VTMatrixT& VT, column_major_tag)
{
	typedef typename matrix_traits<AMatrixT>::value_type value_type;
	typedef typename matrix_traits<AMatrixT>::size_type size_type;
	typedef matrix<value_type, column_major> work_matrix_type;

	char jobu = 'N';
	char jobvt = 'N';
	size_type m = num_rows(A);
	size_type n = num_columns(A);
	size_type k = ::std::min(m, n);
	size_type U_nr = detail::lapack::min_array_size;
	size_type U_nc = detail::lapack::min_array_size;
	size_type VT_nr = detail::lapack::min_array_size;
	size_type VT_nc = detail::lapack::min_array_size;

	if (want_U)
	{
		U_nr = m;
		if (full_U)
		{
			jobu = 'A';
			U_nc = m;
		}
		else
		{
			jobu = 'S';
			U_nc = k;
		}
	}
	if (want_VT)
	{
		VT_nc = n;
		if (full_VT)
		{
			jobvt = 'A';
			VT_nr = n;
		}
		else
		{
			jobvt = 'S';
			VT_nr = k;
		}
	}

	if (size(s) != k)
	{
		s.resize(k, false);
	}
	if (num_rows(U) != U_nr || num_columns(U) != U_nc)
	{
		U.resize(U_nr, U_nc, false);
	}
	if (num_rows(VT) != VT_nr || num_columns(VT) != VT_nc)
	{
		VT.resize(VT_nr, VT_nc, false);
	}

	work_matrix_type tmp_A(A);

	::boost::numeric::bindings::lapack::gesvd(
		jobu,
		jobvt,
		tmp_A,
		s,
		U,
		VT
	);
}

template <
	typename AMatrixT,
	typename SVectorT,
	typename UMatrixT,
	typename VTMatrixT
>
void svd_impl(AMatrixT const& A, SVectorT& s, bool want_U, bool full_U, UMatrixT& U, bool want_VT, bool full_VT, VTMatrixT& VT, row_major_tag)
{
	typedef typename matrix_traits<AMatrixT>::value_type value_type;
	typedef matrix<value_type, column_major> colmaj_matrix_type;

	colmaj_matrix_type tmp_A(A);
	colmaj_matrix_type tmp_U;
	colmaj_matrix_type tmp_VT;

	svd_impl(tmp_A, s, want_U, full_U, tmp_U, want_VT, full_VT, tmp_VT, column_major_tag());

	if (want_U)
	{
		U = tmp_U;
	}
	if (want_VT)
	{
		VT = tmp_VT;
	}
}

template <typename MatrixT>
typename ::boost::enable_if<
	::boost::is_complex<typename matrix_traits<MatrixT>::value_type>,
	MatrixT
>::type make_V(MatrixT const& VH)
{
	return herm(VH);
}

template <typename MatrixT>
typename ::boost::disable_if<
	::boost::is_complex<typename matrix_traits<MatrixT>::value_type>,
	MatrixT
>::type make_V(MatrixT const& VT)
{
	return trans(VT);
}

}} // Namespace detail::<unnamed>

/**
 * \brief Computes the singular value decomposition (SVD) of a matrix.
 *
 * Computes the singular value decomposition (SVD) of a m-by-n matrix,
 * optionally computing the left and/or right singular vectors.
 * The SVD is written as:
 * \f[
 *   A = U \Sigma V^{H}
 * \f]
 * where \f$\Sigma\f$ is an m-by-n matrix which is zero except for its
 * \f$\min(m,n)\f$ diagonal elements, \f$U\f$ is an m-by-m unitary matrix, and
 * \f$V\f$ is an n-by-n unitary matrix.
 * The diagonal elements of \f$\Sigma\f$ are the singular values of
 * \f$A\f$; they are real and non-negative, and are returned in descending order.
 * The first \f$\min(m,n)\f$ columns of \f$U\f$ and \f$V\f$ are the left and
 * right singular vectors of \f$A\f$.
 *
 * When full mode is disabled, an economy-size SVD is computed, such
 * that, if \f$k=\min(m.n)\f$, \f$\Sigma\f$ is a k-by-k diagonal matrix,
 * \f$U\f$ is an m-by-k unitary matrix and \f$V\f$ is an n-by-k unitary matrix.
 *
 * \author Marco Guazzone, marco.guazzoe@gmail.com
 */
template <typename ValueT>
class svd_decomposition
{
	public: typedef ValueT value_type;
	public: typedef typename type_traits<value_type>::real_type real_type;
	public: typedef vector<real_type> vector_type;
	public: typedef matrix<value_type, column_major> matrix_type;
	public: typedef matrix<real_type, column_major> real_matrix_type;
	private: typedef typename matrix_traits<matrix_type>::size_type size_type;

	/// Default constructor
	public: svd_decomposition()
		: full_(false),
		  m_(0),
		  n_(0),
		  k_(0)
	{
	}

	/// A constructor.
	public: template <typename MatrixExprT>
		svd_decomposition(matrix_expression<MatrixExprT> const& A, bool full = true)
	{
		decompose(A, full);
	}

	/// Compute the SVD \f$A=U \Sigma V^H\f$
	public: template <typename MatrixExprT>
		void decompose(matrix_expression<MatrixExprT> const& A, bool full = true)
	{
		// Cache some values (useful for later info retrieval)
		full_ = full;
		m_ = num_rows(A);
		n_ = num_columns(A);
		k_ = ::std::min(m_, n_);

		detail::svd_impl(A(), s_, true, full, U_, true, full, VH_, column_major_tag());
	}

	/// Return the U matrix of the SVD \f$U \Sigma V^H\f$.
	public: matrix_type const& U() const
	{
		return U_;
	}

	/// Return the \f$\operatorname{diag}(\Sigma)\f$ vector of the SVD \f$U \Sigma V^H\f$.
	public: vector_type const& s() const
	{
		return s_;
	}

	/// Return the \f$\Sigma\f$ matrix of the SVD \f$U \Sigma V^H\f$.
	public: real_matrix_type S() const
	{
		if (full_)
		{
			//return diag(s_, m_, n_);
			return diag<vector_type,column_major>(s_, m_, n_);
		}
		//return diag(s_, k_, k_);
		return diag<vector_type,column_major>(s_, k_, k_);
	}

	/// Return the \f$V^H\f$ matrix (\f$V^T\f$ for real types) of the SVD
	/// \f$U \Sigma V^H\f$.
	public: matrix_type const& VH() const
	{
		return VH_;
	}

	/// Return the \f$V\f$ matrix of the SVD \f$U \Sigma V^H\f$.
	public: matrix_type V() const
	{
		return detail::make_V(VH_);
	}

//	TODO: interesting but not urgent.
//	/**
//	 * \brief  Compute the approximate error bound for the computed singular
//	 *  values.
//	 * \note
//	 *  For the 2-norm, \f$\Sigma(1,1) = \operatorname{norm}(A)\f$.
//	 */
//	public: real_type s_error() const
//	{
//		return ::std::numeric_limits<real_type>::epsilon()*s_(0);
//	}

//	TODO: interesting but not urgent.
//	/**
//	 * \brief  Compute the approximate error bound for the computed singular
//	 *  values.
//	 * \note
//	 *  For the 2-norm, \f$\Sigma(1,1) = \operatorname{norm}(A)\f$.
//	 */
//	public: vector_type U_error() const
//	{
//		// Estimate reciprocal condition numbers for the singular vectors
//		vector_type rcond(n);
//		DDISNA('Left',M,N,s_,rcond)
//		// Compute the error estimates for the singular vectors
//		real_type s_err = s_error();
//		vector_type err(n);
//		for (size_type i = 0; i < n; ++i)
//		{
//			err(i) = s_err/rcond(i)
//		}
//
//		return err;
//	}

//	TODO: interesting but not urgent.
//	/**
//	 * \brief  Compute the approximate error bound for the computed singular
//	 *  values.
//	 * \note
//	 *  For the 2-norm, \f$\Sigma(1,1) = \operatorname{norm}(A)\f$.
//	 */
//	public: vector_type VH_error() const
//	{
//		// Estimate reciprocal condition numbers for the singular vectors
//		vector_type rcond(n);
//		DDISNA('Right',M,N,s_,rcond)
//		// Compute the error estimates for the singular vectors
//		real_type s_err = s_error();
//		vector_type err(n);
//		for (size_type i = 0; i < n; ++i)
//		{
//			err(i) = s_err/rcond(i)
//		}
//
//		return err;
//	}

	/// Tell if the current SVD is in full or economy mode.
	bool full_;
	/// The number of rows of the original decomposed matrix.
	size_type m_;
	/// The number of columns of the original decomposed matrix.
	size_type n_;
	/// The minimum between the number of rows and columns of the original
	/// decomposed matrix.
	size_type k_;
	/// The vector of singular values.
	private: vector_type s_;
	/// The matrix containing the left singular vectors.
	private: matrix_type U_;
	/// The matrix containing the right singular vectors.
	private: matrix_type VH_;
};

/// Compute the singular values of matrix \a A.
template <typename MatrixExprT>
vector<
	typename type_traits<
		typename matrix_traits<MatrixExprT>::value_type
	>::real_type
> svd_values(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type real_type;
	typedef vector<real_type> vector_type;
	typedef typename layout_type<MatrixExprT>::type layout_type;
	typedef matrix<value_type, layout_type> work_matrix_type;

	vector_type s;
	work_matrix_type dummy_U;
	work_matrix_type dummy_VT;

	detail::svd_impl(A(), s, false, false, dummy_U, false, false, dummy_VT, orientation_category());

	return s;
}

/// Compute the singular value decomposition of matrix \a A.
template <typename MatrixExprT>
svd_decomposition<typename matrix_traits<MatrixExprT>::value_type> svd_decompose(matrix_expression<MatrixExprT> const& A, bool full = true)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	return svd_decomposition<value_type>(A, full);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_SVD_HPP


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_random.h>
#include <OpenTissue/core/math/optimization/optimization_projected_gauss_seidel.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

template <typename vector_type>
class BoundFunction
{
public:

  typedef typename vector_type::size_type          size_type;
  typedef typename vector_type::value_type         real_type;
  typedef OpenTissue::math::ValueTraits<real_type> value_traits;

  bool m_is_lower;

public:

  BoundFunction(bool const & is_lower)
    : m_is_lower(is_lower)
  {}

  real_type operator()(vector_type const & x, size_type const & i) const
  {
    size_type r = (i%3);

    if(r==0)
      return m_is_lower ? value_traits::zero() : value_traits::infinity();

    real_type mu_i = value_traits::one();
    size_type j = i - r;
    return m_is_lower ?  -mu_i*x(j) : mu_i*x(j);
  }

};

template<typename matrix_type,typename vector_type>
void test(matrix_type const & A, vector_type  & x, vector_type const & b, vector_type const & y)
{
  typedef typename matrix_type::value_type real_type;
  typedef typename matrix_type::size_type  size_type;

  BoundFunction<vector_type> l(true);
  BoundFunction<vector_type> u(false);

  size_type max_iterations       = 1000;
  real_type absolute_tolerance   = boost::numeric_cast<real_type>(0.025);
  real_type relative_tolerance   = boost::numeric_cast<real_type>(0.00001);
  real_type stagnation_tolerance = boost::numeric_cast<real_type>(0.00001);
  size_t status = 0;
  size_type iteration = 0;
  real_type accuracy = boost::numeric_cast<real_type>(0.0);
  real_type relative_accuracy = boost::numeric_cast<real_type>(0.0);

  OpenTissue::math::optimization::projected_gauss_seidel(
    A, b, l , u, x
    ,  max_iterations
    ,  absolute_tolerance
    ,  relative_tolerance
    ,  stagnation_tolerance
    ,  status
    ,  iteration
    ,  accuracy
    ,  relative_accuracy
    );

  if(status==OpenTissue::math::optimization::ABSOLUTE_CONVERGENCE)
  {
    BOOST_CHECK( accuracy < absolute_tolerance );
    BOOST_CHECK( iteration <= max_iterations );
  }
  else
  {
    std::cout << std::endl;
    std::cout << "absolute " << accuracy << " iter " << iteration << " status " << status << " relative " << relative_accuracy << std::endl;
    std::cout << std::endl;
  }
}

BOOST_AUTO_TEST_SUITE(opentissue_math_big_projected_gauss_seidel);

BOOST_AUTO_TEST_CASE(random_test_case)
{

  typedef ublas::compressed_matrix<double> matrix_type;
  typedef ublas::vector<double>            vector_type;
  typedef vector_type::size_type           size_type;

  for(size_type tst=0;tst<1000;++tst)
  {
    size_type N = 20;

    matrix_type A;
    A.resize(N,N,false);

    vector_type x;
    x.resize(N,false);

    vector_type b;
    b.resize(N,false);

    vector_type y;
    y.resize(N,false);

    matrix_type R;
    R.resize(N,N,false);

    OpenTissue::math::Random<double> value(0.0,1.0);
    for(size_t i=0;i<R.size1();++i)
    {
      b(i) = -value();

      x(i) = value();
      y(i) = value();
      for(size_t j=0;j<R.size2();++j)
        R(i,j) = value();
    }
    ublas::noalias(A) = ublas::sparse_prod<matrix_type>( ublas::trans(R), R );
    // forcing A to become PD matrix (it should be non-singular at all times)
    for(size_t i=0;i<R.size1();++i)
      A(i,i) += 0.5;

    x.clear();
    test(A,x,b,y);
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include "Vector3d.hpp"
#include "utils.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

namespace fragdock {
  void getNormalizeRot(const Vector3d& v1, const Vector3d& _v2, fltype& theta, fltype& phi, fltype& psi) {
    theta = 0;
    phi = 0;
    psi = 0;
    Vector3d vec1 = v1;
    Vector3d vec2 = v1.cross(_v2);
    psi = Vector3d(0, 1, 0).getAngle(Vector3d(vec2.x, vec2.y, 0));
    phi = Vector3d(0, 0, 1).getAngle(Vector3d(0, sqrt(vec2.x * vec2.x + vec2.y * vec2.y), vec2.z));
    if (vec2.x < 0) psi = -psi;
    vec1.rotate(0, phi, psi);
    theta = -Vector3d(1, 0, 0).getAngle(vec1);
    if (vec1.y < 0) theta = -theta;
    // rotate(theta, phi, psi);
  }

  Vector3d getRot(const Vector3d& v1, const Vector3d& _v2) {
    fltype theta, phi, psi;
    getNormalizeRot(v1, _v2, theta, phi, psi);
    return Vector3d(-psi, -phi, -theta);
  }

  void Vector3d::axisRotate(const Vector3d &axis, double th) {
    Vector3d n = axis/axis.abs();
    fltype B = cos(th), C = sin(th);
    fltype A = 1-B;
    fltype ox = x, oy = y, oz = z;
    x = (n.x*n.x*A +     B)*ox + (n.x*n.y*A - n.z*C)*oy + (n.z*n.x*A + n.y*C)*oz;
    y = (n.x*n.y*A + n.z*C)*ox + (n.y*n.y*A +     B)*oy + (n.y*n.z*A - n.x*C)*oz;
    z = (n.z*n.x*A - n.y*C)*ox + (n.y*n.z*A + n.x*C)*oy + (n.z*n.z*A +     B)*oz;
  }

  // zxz rotation
  void Vector3d::rotate(fltype theta, fltype phi, fltype psi) {
    const fltype cosT = cos(theta), sinT = sin(theta), cosH = cos(phi), sinH = sin(phi), cosS = cos(psi), sinS = sin(psi);
    fltype ox = x, oy = y, oz = z;

    x = (cosT*cosS  - sinT*cosH*sinS) * ox + (-cosT*sinS - sinT*cosH*cosS) * oy + (sinT*sinH ) * oz;
    y = (sinT*cosS  + cosT*cosH*sinS) * ox + (-sinT*sinS + cosT*cosH*cosS) * oy + (-cosT*sinH) * oz;
    z = (sinH*sinS                  ) * ox + (sinH*cosS                  ) * oy + (cosH      ) * oz;
  }
  void Vector3d::rotate(const Vector3d& v) {
    rotate(v.x, v.y, v.z);
  }

  const std::vector<Vector3d> makeRotations60() {
    const fltype phi = (1.0 + sqrt(5.0)) / 2.0;
    const fltype pi = acos(-1.0);

    fragdock::Vector3d poleA(0, phi*phi*phi, phi*phi);
    fragdock::Vector3d poleB(0, 1.0, phi*phi);
    poleA = poleA.unit();
    poleB = poleB.unit();
    fltype p = poleA.getAngle(poleB);
    poleA = Vector3d(1, 0, 0);
    poleB = Vector3d(cos(p), sin(p), 0);

    const int ORDER_NUM = 12;
    const int order[ORDER_NUM] = {0, 0, 1, 1, 1, 2, 1, 2, 2, 2, 3, -1};

    std::vector<Vector3d> rots;
    for(int ord_id = 0; ord_id < ORDER_NUM; ord_id++) {
      int o = order[ord_id];
      for(int i = 0; i < 5; i++) {
        // fltype theta = getAxisAngle(poleA, poleB);
        rots.push_back(getRot(poleA, poleB));
        poleB.axisRotate(poleA, 2.0*pi/5.0);
      }
      poleB.axisRotate(poleA, 2.0*o*pi/5.0);
      poleA.axisRotate(poleB, 4.0*pi/3.0);
    }
    return rots;
  }

  const std::vector<Vector3d> readRotations(const std::string& filename) {
    std::ifstream ifs(filename);
    if (ifs.fail()){
      std::cerr << "opening grid file failed:" << filename << std::endl;
      abort();
    }
    std::vector<Vector3d> rots;
    while (!ifs.eof()) {
      std::string str;
      std::getline(ifs, str);
      std::vector<std::string> vals;
      boost::algorithm::split(vals, str, boost::algorithm::is_any_of(","));
      if ((int)vals.size() < 3) continue;
      boost::algorithm::trim(vals[0]);
      boost::algorithm::trim(vals[1]);
      boost::algorithm::trim(vals[2]);
      rots.push_back(Vector3d(boost::lexical_cast<fltype>(vals[0]),
                              boost::lexical_cast<fltype>(vals[1]),
                              boost::lexical_cast<fltype>(vals[2])));
    }
    return rots;
  }
}


#pragma once

/// @file

#define EIGEN_SPARSEMATRIX_PLUGIN "numeric/thread_safe_plugin.hpp"

#include <Eigen/Sparse>

#include <complex>

namespace neon
{
/// Type alias for row major real-valued sparse matrix
using sparse_matrix = Eigen::SparseMatrix<double, Eigen::RowMajor>;
/// Type alias for row major complex-valued sparse matrix
using complex_sparse_matrix = Eigen::SparseMatrix<std::complex<double>, Eigen::RowMajor>;
/// Type alias for the permutation matrix
using permutation_matrix = Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, sparse_matrix::StorageIndex>;
}


#include "test_ntl.hpp"
#include <NTL/GF2X.h>
#define LINEARITY_CHECK
#include "tinymt32.h"
#include <string>
#include <stdio.h>
#include <inttypes.h>
#include <ctype.h>

using namespace NTL;
using namespace std;

tinymt32_t tiny32;

unsigned int dummy()
{
    return tinymt32_generate_uint32(&tiny32);
}

void ntl_exeuclid2(GF2X& a, GF2X& c, GF2X& x, GF2X& y, long m)
{
    PUTS("f2p_exeuclid2 start\n");
    assert(!IsZero(x));
    assert(!IsZero(y));
    GF2X q1;
    GF2X r0(x);
    GF2X r1(y);
    GF2X r2;
    GF2X a0;
    GF2X a1;
    GF2X a2;
    GF2X tmp;
    PRT("x= ", x);
    PRT("y= ", y);
    a0 = 1;
    a1 = 0;
    PRT("r0 = ", r0);
    PRT("r1 = ", r1);
    PRT("a0 = ", a0);
    PRT("a1 = ", a1);
    long dr = deg(r0);
    while (dr >= m) {
        DivRem(q1, r2, r0, r1);
        tmp = q1 * a1;
        a2 = a0 + tmp;
        PRT("q1 = ", q1);
        PRT("tmp = ", tmp);
        PRT("r0 = ", r0);
        PRT("r1 = ", r1);
        PRT("r2 = ", r2);
        PRT("a0 = ", a0);
        PRT("a1 = ", a1);
        PRT("a2 = ", a2);
        r0 = r1;
        r1 = r2;
        a0 = a1;
        a1 = a2;
        dr = deg(r0);
        PUTS("loop last\n");
    }
    PUTS("loop end\n");
    PRT("a0 = ", a0);
    a = a0;
    PRT("r0 = ", r0);
    c = r0;
    PUTS("f2p_exeuclid2 end\n");
}

#if 0
void ntl_minpoly(GF2X& poly, f2rng gen, int mexp)
{
    Vec<GF2> v;
    v.SetLength(2 * mexp);
    for (int i = 0; i < 2 * mexp; i++) {
        v[i] = gen() & 1;
    }
    MinPolySeq(poly, v, mexp);
}
#endif

void ntl_minpoly(GF2X& poly, tinymt32_t * tiny32, int mexp)
{
    GF2X v;
    v.SetLength(2 * mexp);
    //for (int i = 0; i < 2 * mexp; i++) {
    for (int i = 2 * mexp - 1; i >= 0; i--) {
        v[i] = tinymt32_generate_uint32(tiny32) & 1;
    }
    PRT("seq = ", v);
    //MinPolySeq(poly, v, mexp);
    GF2X x2m;
    GF2X c;
    SetCoeff(x2m, 2 * mexp, 1);
    ntl_exeuclid2(poly, c, v, x2m, mexp);
    printf("deg(c) = %ld\n", deg(c));
}

void ntl_minpoly(char * minpoly, tinymt32_t * tiny32, int mexp)
{
    GF2X poly;
    ntl_minpoly(poly, tiny32, mexp);
    string str;
    to_hexstring(str, poly);
    strcpy(minpoly, str.c_str());
}

int ntl_annihilate(const GF2X& poly, tinymt32_t * tiny32, int mexp)
{
    int bit = 0;
    for (int i = 0; i <= mexp; i++) {
        if (IsOne(poly[i])) {
            bit ^= tinymt32_generate_uint32(tiny32) & 1;
        }
    }
    if (bit == 0) {
        return 1;
    } else {
        return 0;
    }
}

int test_minpoly(tinymt32_t * tiny32)
{
    GF2X poly;
    ntl_minpoly(poly, tiny32, 127);
    int ok = 1;
    for (int i = 0; i < 100; i++) {
        int r = ntl_annihilate(poly, tiny32, 127);
        ok &= r;
        dummy();
        if (r == 1) {
            printf("o");
        } else {
            printf("x");
        }
    }
    printf("\n");
    if (ok) {
        printf("annihilate OK\n");
    } else {
        printf("annihilate NG\n");
    }
    //char polystr[200];
    string str;
    to_hexstring(str, poly);
    printf("poly = %ld,%s\n", deg(poly), str.c_str());
    hexto_poly(poly, str);
    to_hexstring(str, poly);
    printf("poly = %s\n", str.c_str());
    printf("minpoly end\n");
    //int deg = f2p_degree(poly);
    //printf("deg(poly) = %d\n", deg);
    return 0;
}

int main(int argc, char * argv[])
{
    int verbose = 0;
    int r = 0;
    if (argc > 1 && argv[1][0] == 'v') {
        verbose = 1;
    }
    tiny32.mat1 = 0x8f7011ee;
    tiny32.mat2 = 0xfc78ff1f;
    tiny32.tmat = 0x3793fdff;

    tinymt32_init(&tiny32, 1);
    r += test_minpoly(&tiny32);
    if (r == 0) {
        return 0;
    } else {
        return -1;
    }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @brief wraps Rot3 class to python
 * @author Andrew Melim
 * @author Ellon Paiva Mendes (LAAS-CNRS)
 **/

#include <boost/python.hpp>

#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>

#include "gtsam/geometry/Rot3.h"

using namespace boost::python;
using namespace gtsam;

static Rot3 Quaternion_0(const Vector4& q)
{
    return Rot3::Quaternion(q[0],q[1],q[2],q[3]);
}

static Rot3 Quaternion_1(double w, double x, double y, double z)
{
    return Rot3::Quaternion(w,x,y,z);
}

// Prototypes used to perform overloading
// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html
gtsam::Rot3  (*AxisAngle_0)(const gtsam::Point3&, double) = &Rot3::AxisAngle;
gtsam::Rot3  (*AxisAngle_1)(const gtsam::Unit3&, double) = &Rot3::AxisAngle;
gtsam::Rot3  (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues;
gtsam::Rot3  (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues;
gtsam::Rot3  (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx;
gtsam::Rot3  (*RzRyRx_1)(const Vector&) = &Rot3::RzRyRx;
Vector (Rot3::*quaternion_0)() const = &Rot3::quaternion;

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot3::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot3::equals, 1, 2)

void exportRot3(){

  class_<Rot3>("Rot3")
    .def(init<Point3,Point3,Point3>())
    .def(init<double,double,double,double,double,double,double,double,double>())
    .def(init<double,double,double,double>())
    .def(init<const Quaternion &>())
    .def(init<const Matrix3 &>())
    .def(init<const Matrix &>())
    .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion")
    .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) )
    .staticmethod("Quaternion")
    .def("Expmap", &Rot3::Expmap)
    .staticmethod("Expmap")
    .def("ExpmapDerivative", &Rot3::ExpmapDerivative)
    .staticmethod("ExpmapDerivative")
    .def("Logmap", &Rot3::Logmap)
    .staticmethod("Logmap")
    .def("LogmapDerivative", &Rot3::LogmapDerivative)
    .staticmethod("LogmapDerivative")
    .def("AxisAngle", AxisAngle_0)
    .def("AxisAngle", AxisAngle_1)
    .staticmethod("AxisAngle")
    .def("Rodrigues", Rodrigues_0)
    .def("Rodrigues", Rodrigues_1)
    .staticmethod("Rodrigues")
    .def("Rx", &Rot3::Rx)
    .staticmethod("Rx")
    .def("Ry", &Rot3::Ry)
    .staticmethod("Ry")
    .def("Rz", &Rot3::Rz)
    .staticmethod("Rz")
    .def("RzRyRx", RzRyRx_0, (arg("x"),arg("y"),arg("z")), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" )
    .def("RzRyRx", RzRyRx_1, arg("xyz"), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" )
    .staticmethod("RzRyRx")
    .def("Ypr", &Rot3::Ypr)
    .staticmethod("Ypr")
    .def("identity", &Rot3::identity)
    .staticmethod("identity")
    .def("AdjointMap", &Rot3::AdjointMap)
    .def("column", &Rot3::column)
    .def("conjugate", &Rot3::conjugate)
    .def("equals", &Rot3::equals, equals_overloads(args("q","tol")))
#ifndef GTSAM_USE_QUATERNIONS
    .def("localCayley", &Rot3::localCayley)
    .def("retractCayley", &Rot3::retractCayley)
#endif
    .def("matrix", &Rot3::matrix)
    .def("print", &Rot3::print, print_overloads(args("s")))
    .def("r1", &Rot3::r1)
    .def("r2", &Rot3::r2)
    .def("r3", &Rot3::r3)
    .def("rpy", &Rot3::rpy)
    .def("slerp", &Rot3::slerp)
    .def("transpose", &Rot3::transpose)
    .def("xyz", &Rot3::xyz)
    .def("quaternion", quaternion_0)
    .def(self * self)
    .def(self * other<Point3>())
    .def(self * other<Unit3>())
    .def(self_ns::str(self)) // __str__
    .def(repr(self))         // __repr__
  ;

}


// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/statistics.h>
#include <dlib/sparse_vector.h>
#include <map>

#include "tester.h"

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.cca");

    dlib::rand rnd;
// ----------------------------------------------------------------------------------------

    /*
    std::vector<std::map<unsigned long, double> > make_really_big_test_matrix (
    )
    {
        std::vector<std::map<unsigned long,double> > temp(30000);
        for (unsigned long i = 0; i < temp.size(); ++i)
        {
            for (int k = 0; k < 30; ++k)
                temp[i][rnd.get_random_32bit_number()%10000] = 1;
        }
        return temp;
    }
    */

    template <typename T>
    std::vector<std::map<unsigned long, T> > mat_to_sparse (
        const matrix<T>& A
    )
    {
        std::vector<std::map<unsigned long,T> > temp(A.nr());
        for (long r = 0; r < A.nr(); ++r)
        {
            for (long c = 0; c < A.nc(); ++c)
            {
                temp[r][c] = A(r,c);
            }
        }
        return temp;
    }

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

    template <typename EXP>
    matrix<typename EXP::type> rm_zeros (
        const matrix_exp<EXP>& m
    )
    {
        // Do this to avoid trying to correlate super small numbers that are really just
        // zero.  Doing this avoids some potential false alarms in the unit tests below.
        return round_zeros(m, max(abs(m))*1e-14);
    }

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

    /*
    void check_correlation (
        matrix<double> L,
        matrix<double> R,
        const matrix<double>& Ltrans,
        const matrix<double>& Rtrans,
        const matrix<double,0,1>& correlations
    )
    {
        // apply the transforms
        L = L*Ltrans;
        R = R*Rtrans;

        // compute the real correlation values. Store them in A.
        matrix<double> A = compute_correlations(L, R);

        for (long i = 0; i < correlations.size(); ++i)
        {
            // compare what the measured correlation values are (in A) to the
            // predicted values.
            cout << "error: "<< A(i) - correlations(i);
        }
    }
    */

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

    void test_cca3()
    {
        print_spinner();
        const unsigned long rank = rnd.get_random_32bit_number()%10 + 1;
        const unsigned long m = rank + rnd.get_random_32bit_number()%15;
        const unsigned long n = rank + rnd.get_random_32bit_number()%15;
        const unsigned long n2 = rank + rnd.get_random_32bit_number()%15;
        const unsigned long rank2 = rank + rnd.get_random_32bit_number()%5;

        dlog << LINFO << "m:  " << m;
        dlog << LINFO << "n:  " << n;
        dlog << LINFO << "n2: " << n2;
        dlog << LINFO << "rank:  " << rank;
        dlog << LINFO << "rank2: " << rank2;

        matrix<double> L = randm(m,rank, rnd)*randm(rank,n, rnd);
        //matrix<double> R = randm(m,rank, rnd)*randm(rank,n2, rnd);
        matrix<double> R = L*randm(n,n2);
        //matrix<double> L = randm(m,n, rnd);
        //matrix<double> R = randm(m,n2, rnd);

        matrix<double> Ltrans, Rtrans;
        matrix<double,0,1> correlations;

        {
            correlations = cca(L, R, Ltrans, Rtrans, min(m,n), max(n,n2));
            DLIB_TEST(Ltrans.nc() == Rtrans.nc());
            dlog << LINFO << "correlations: "<< trans(correlations);

            const double corr_error = max(abs(compute_correlations(rm_zeros(L*Ltrans), rm_zeros(R*Rtrans)) - correlations));
            dlog << LINFO << "correlation error: "<< corr_error;
            DLIB_TEST_MSG(corr_error < 1e-13, Ltrans << "\n\n" << Rtrans);

            const double trans_error = max(abs(L*Ltrans - R*Rtrans));
            dlog << LINFO << "trans_error: "<< trans_error;
            DLIB_TEST(trans_error < 1e-9);
        }
        {
            correlations = cca(mat_to_sparse(L), mat_to_sparse(R), Ltrans, Rtrans, min(m,n), max(n,n2)+6, 4);
            DLIB_TEST(Ltrans.nc() == Rtrans.nc());
            dlog << LINFO << "correlations: "<< trans(correlations);
            dlog << LINFO << "computed cors: " << trans(compute_correlations(rm_zeros(L*Ltrans), rm_zeros(R*Rtrans)));

            const double trans_error = max(abs(L*Ltrans - R*Rtrans));
            dlog << LINFO << "trans_error: "<< trans_error;
            const double corr_error = max(abs(compute_correlations(rm_zeros(L*Ltrans), rm_zeros(R*Rtrans)) - correlations));
            dlog << LINFO << "correlation error: "<< corr_error;
            DLIB_TEST_MSG(corr_error < 1e-13, Ltrans << "\n\n" << Rtrans);

            DLIB_TEST(trans_error < 1e-9);
        }

        dlog << LINFO << "*****************************************************";
    }

    void test_cca2()
    {
        print_spinner();
        const unsigned long rank = rnd.get_random_32bit_number()%10 + 1;
        const unsigned long m = rank + rnd.get_random_32bit_number()%15;
        const unsigned long n = rank + rnd.get_random_32bit_number()%15;
        const unsigned long n2 = rank + rnd.get_random_32bit_number()%15;

        dlog << LINFO << "m:  " << m;
        dlog << LINFO << "n:  " << n;
        dlog << LINFO << "n2: " << n2;
        dlog << LINFO << "rank:  " << rank;

        matrix<double> L = randm(m,n, rnd);
        matrix<double> R = randm(m,n2, rnd);

        matrix<double> Ltrans, Rtrans;
        matrix<double,0,1> correlations;

        {
            correlations = cca(L, R, Ltrans, Rtrans, min(n,n2), max(n,n2)-min(n,n2));
            DLIB_TEST(Ltrans.nc() == Rtrans.nc());
            dlog << LINFO << "correlations: "<< trans(correlations);

            if (Ltrans.nc() > 1)
            {
                // The CCA projection directions are supposed to be uncorrelated for
                // non-matching pairs of projections.
                const double corr_rot1_error = max(abs(compute_correlations(rm_zeros(L*rotate<0,1>(Ltrans)), rm_zeros(R*Rtrans))));
                dlog << LINFO << "corr_rot1_error: "<< corr_rot1_error;
                DLIB_TEST(std::abs(corr_rot1_error) < 1e-10);
            }
            // Matching projection directions should be correlated with the amount of
            // correlation indicated by the return value of cca().
            const double corr_error = max(abs(compute_correlations(rm_zeros(L*Ltrans), rm_zeros(R*Rtrans)) - correlations));
            dlog << LINFO << "correlation error: "<< corr_error;
            DLIB_TEST(corr_error < 1e-13);
        }
        {
            correlations = cca(mat_to_sparse(L), mat_to_sparse(R), Ltrans, Rtrans, min(n,n2), max(n,n2)-min(n,n2));
            DLIB_TEST(Ltrans.nc() == Rtrans.nc());
            dlog << LINFO << "correlations: "<< trans(correlations);

            if (Ltrans.nc() > 1)
            {
                // The CCA projection directions are supposed to be uncorrelated for
                // non-matching pairs of projections.
                const double corr_rot1_error = max(abs(compute_correlations(rm_zeros(L*rotate<0,1>(Ltrans)), rm_zeros(R*Rtrans))));
                dlog << LINFO << "corr_rot1_error: "<< corr_rot1_error;
                DLIB_TEST(std::abs(corr_rot1_error) < 1e-10);
            }
            // Matching projection directions should be correlated with the amount of
            // correlation indicated by the return value of cca().
            const double corr_error = max(abs(compute_correlations(rm_zeros(L*Ltrans), rm_zeros(R*Rtrans)) - correlations));
            dlog << LINFO << "correlation error: "<< corr_error;
            DLIB_TEST(corr_error < 1e-13);
        }

        dlog << LINFO << "*****************************************************";
    }

    void test_cca1()
    {
        print_spinner();
        const unsigned long rank = rnd.get_random_32bit_number()%10 + 1;
        const unsigned long m = rank + rnd.get_random_32bit_number()%15;
        const unsigned long n = rank + rnd.get_random_32bit_number()%15;

        dlog << LINFO << "m: " << m;
        dlog << LINFO << "n: " << n;
        dlog << LINFO << "rank: " << rank;

        matrix<double> T = randm(n,n, rnd);

        matrix<double> L = randm(m,rank, rnd)*randm(rank,n, rnd);
        //matrix<double> L = randm(m,n, rnd);
        matrix<double> R = L*T;

        matrix<double> Ltrans, Rtrans;
        matrix<double,0,1> correlations;

        {
            correlations = cca(L, R, Ltrans, Rtrans, rank);
            DLIB_TEST(Ltrans.nc() == Rtrans.nc());
            if (Ltrans.nc() > 1)
            {
                // The CCA projection directions are supposed to be uncorrelated for
                // non-matching pairs of projections.
                const double corr_rot1_error = max(abs(compute_correlations(rm_zeros(L*rotate<0,1>(Ltrans)), rm_zeros(R*Rtrans))));
                dlog << LINFO << "corr_rot1_error: "<< corr_rot1_error;
                DLIB_TEST(std::abs(corr_rot1_error) < 1e-10);
            }
            // Matching projection directions should be correlated with the amount of
            // correlation indicated by the return value of cca().
            const double corr_error = max(abs(compute_correlations(rm_zeros(L*Ltrans), rm_zeros(R*Rtrans)) - correlations));
            dlog << LINFO << "correlation error: "<< corr_error;
            DLIB_TEST(corr_error < 1e-13);

            const double trans_error = max(abs(L*Ltrans - R*Rtrans));
            dlog << LINFO << "trans_error: "<< trans_error;
            DLIB_TEST(trans_error < 1e-10);

            dlog << LINFO << "correlations: "<< trans(correlations);
        }
        {
            correlations = cca(mat_to_sparse(L), mat_to_sparse(R), Ltrans, Rtrans, rank);
            DLIB_TEST(Ltrans.nc() == Rtrans.nc());
            if (Ltrans.nc() > 1)
            {
                // The CCA projection directions are supposed to be uncorrelated for
                // non-matching pairs of projections.
                const double corr_rot1_error = max(abs(compute_correlations(rm_zeros(L*rotate<0,1>(Ltrans)), rm_zeros(R*Rtrans))));
                dlog << LINFO << "corr_rot1_error: "<< corr_rot1_error;
                DLIB_TEST(std::abs(corr_rot1_error) < 1e-10);
            }
            // Matching projection directions should be correlated with the amount of
            // correlation indicated by the return value of cca().
            const double corr_error = max(abs(compute_correlations(rm_zeros(L*Ltrans), rm_zeros(R*Rtrans)) - correlations));
            dlog << LINFO << "correlation error: "<< corr_error;
            DLIB_TEST(corr_error < 1e-13);

            const double trans_error = max(abs(L*Ltrans - R*Rtrans));
            dlog << LINFO << "trans_error: "<< trans_error;
            DLIB_TEST(trans_error < 1e-9);

            dlog << LINFO << "correlations: "<< trans(correlations);
        }

        dlog << LINFO << "*****************************************************";
    }

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

    void test_svd_fast(
        long rank,
        long m,
        long n
    )
    {
        print_spinner();
        matrix<double> A = randm(m,rank,rnd)*randm(rank,n,rnd);
        matrix<double> u,v;
        matrix<double,0,1> w;

        dlog << LINFO << "rank: "<< rank;
        dlog << LINFO << "m: "<< m;
        dlog << LINFO << "n: "<< n;

        svd_fast(A, u, w, v, rank, 2);
        DLIB_TEST(u.nr() == m);
        DLIB_TEST(u.nc() == rank);
        DLIB_TEST(w.nr() == rank);
        DLIB_TEST(w.nc() == 1);
        DLIB_TEST(v.nr() == n);
        DLIB_TEST(v.nc() == rank);
        DLIB_TEST(max(abs(trans(u)*u - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(trans(v)*v - identity_matrix<double>(u.nc()))) < 1e-13);

        DLIB_TEST(max(abs(tmp(A - u*diagm(w)*trans(v)))) < 1e-13);
        svd_fast(mat_to_sparse(A), u, w, v, rank, 2);
        DLIB_TEST(u.nr() == m);
        DLIB_TEST(u.nc() == rank);
        DLIB_TEST(w.nr() == rank);
        DLIB_TEST(w.nc() == 1);
        DLIB_TEST(v.nr() == n);
        DLIB_TEST(v.nc() == rank);
        DLIB_TEST(max(abs(trans(u)*u - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(trans(v)*v - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(tmp(A - u*diagm(w)*trans(v)))) < 1e-13);

        svd_fast(A, u, w, v, rank, 0);
        DLIB_TEST(u.nr() == m);
        DLIB_TEST(u.nc() == rank);
        DLIB_TEST(w.nr() == rank);
        DLIB_TEST(w.nc() == 1);
        DLIB_TEST(v.nr() == n);
        DLIB_TEST(v.nc() == rank);
        DLIB_TEST(max(abs(trans(u)*u - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(trans(v)*v - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST_MSG(max(abs(tmp(A - u*diagm(w)*trans(v)))) < 1e-9,max(abs(tmp(A - u*diagm(w)*trans(v)))));
        svd_fast(mat_to_sparse(A), u, w, v, rank, 0);
        DLIB_TEST(u.nr() == m);
        DLIB_TEST(u.nc() == rank);
        DLIB_TEST(w.nr() == rank);
        DLIB_TEST(w.nc() == 1);
        DLIB_TEST(v.nr() == n);
        DLIB_TEST(v.nc() == rank);
        DLIB_TEST(max(abs(trans(u)*u - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(trans(v)*v - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(tmp(A - u*diagm(w)*trans(v)))) < 1e-10);

        svd_fast(A, u, w, v, rank+5, 0);
        DLIB_TEST(max(abs(trans(u)*u - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(trans(v)*v - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(tmp(A - u*diagm(w)*trans(v)))) < 1e-11);
        svd_fast(mat_to_sparse(A), u, w, v, rank+5, 0);
        DLIB_TEST(max(abs(trans(u)*u - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(trans(v)*v - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(tmp(A - u*diagm(w)*trans(v)))) < 1e-11);
        svd_fast(A, u, w, v, rank+5, 1);
        DLIB_TEST(max(abs(trans(u)*u - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(trans(v)*v - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(tmp(A - u*diagm(w)*trans(v)))) < 1e-12);
        svd_fast(mat_to_sparse(A), u, w, v, rank+5, 1);
        DLIB_TEST(max(abs(trans(u)*u - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(trans(v)*v - identity_matrix<double>(u.nc()))) < 1e-13);
        DLIB_TEST(max(abs(tmp(A - u*diagm(w)*trans(v)))) < 1e-12);
    }

    void test_svd_fast()
    {
        for (int iter = 0; iter < 1000; ++iter)
        {
            const unsigned long rank = rnd.get_random_32bit_number()%10 + 1;
            const unsigned long m = rank + rnd.get_random_32bit_number()%10;
            const unsigned long n = rank + rnd.get_random_32bit_number()%10;

            test_svd_fast(rank, m, n);

        }
        test_svd_fast(1, 1, 1);
        test_svd_fast(1, 2, 2);
        test_svd_fast(1, 1, 2);
        test_svd_fast(1, 2, 1);
    }

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

    class test_cca : public tester
    {
    public:
        test_cca (
        ) :
            tester ("test_cca",
                "Runs tests on the cca() and svd_fast() routines.")
        {}

        void perform_test (
        )
        {
            for (int i = 0; i < 200; ++i)
            {
                test_cca1();
                test_cca2();
                test_cca3();
            }
            test_svd_fast();
        }
    } a;

}


#include "caffe/util/prediction.hpp"

#include <vector>
#include <Eigen/Core>

#include <opencv2/core/core.hpp>

// #include <cv.h>
// #include <highgui.h>

namespace caffe {

template <typename Dtype>
Grid<Dtype> rotate_voxels_prediction(const Grid<Dtype> &vox,
								   const Eigen::Matrix<Dtype,4,4> &model1,
								   const Eigen::Matrix<Dtype,4,4> &view1,
								   const Eigen::Matrix<Dtype,4,4> &model2,
								   const Eigen::Matrix<Dtype,4,4> &view2,
								   const Eigen::Matrix<Dtype,4,4> &proj)
{
int size = vox[0].rows();
//std::cout<<size<<std::endl;
	Grid<Dtype> rot_vox(size);
	for(int c = 0; c < size; c++)
	{
		rot_vox[c] = Slice<Dtype>(size, size);
		for(int i = 0; i <size; i++)
		{
			for(int j = 0; j < size; j++)
			{
				rot_vox[c](i,j) = rotated_proba_value<Dtype>(vox, model1, view1,model2, view2, proj, c, i, j);
			}
		}
	}
	return rot_vox;
}

	template <typename Dtype>
Dtype rotated_proba_value(const Grid<Dtype> &vox,
						  const Eigen::Matrix<Dtype,4,4> &model1,
						  const Eigen::Matrix<Dtype,4,4> &view1,
						  const Eigen::Matrix<Dtype,4,4> &model2,
						  const Eigen::Matrix<Dtype,4,4> &view2,
						  const Eigen::Matrix<Dtype,4,4> &proj,
						  Dtype z,Dtype x, Dtype y)
{
	int size = vox[0].rows();
	//pass coord in the unit cube and rotate it
	// std::cout<<x<<" "<<y<<" "<<z<<" -> ";
	Dtype depth = z_to_depth(z, size, proj);;
	// std::cout<<depth<<std::endl;
	Eigen::Matrix<Dtype,3,1> rotated =
		rotate_coords<Dtype>((y+0.5)/(size), 1-(x+0.5)/(size),
					  depth, model1, view1,model2, view2, proj);
	// std::cout<<rotated<<std::endl;

	int new_i = (int)std::floor((1-rotated.y())*(size));
	int new_j = (int)std::floor(rotated.x()*(size));
	int new_z = depth_to_z(rotated.z(), size, proj);
	new_z = std::min<int>(size - 1,std::max<int>(0,new_z));
	// std::cout<<new_i<<" "<<new_j<<" "<<new_z<<std::endl;
	Dtype v0 = vox[new_z](new_i, new_j);
	return v0;
}

	template <typename Dtype>
	Dtype z_to_depth(int z, int size, const Eigen::Matrix<Dtype,4,4> &proj)
{
	Dtype zf=z*0.8+(size/16);
	Dtype depth = -(zf+0.5)/size*5.5-2.5;
	depth = -proj(2,3)/depth-proj(2,2);
	depth=depth/2+0.5;
	return depth;
}

	template <typename Dtype>
int depth_to_z(Dtype depth, int size, const Eigen::Matrix<Dtype,4,4> &proj)
{
	depth = depth * 2 - 1;
	depth = -proj(2,3)/(depth+proj(2,2));
	Dtype z = (-(depth+2.5)/5.5)*size-0.5;
	return (int)std::round((z-(size/16))/0.8);
}

	template <typename Dtype>
Eigen::Matrix<Dtype,3,1> rotate_coords(Dtype x, Dtype y, Dtype z,
						  const Eigen::Matrix<Dtype,4,4> &model1,
						  const Eigen::Matrix<Dtype,4,4> &view1,
						  const Eigen::Matrix<Dtype,4,4> &model2,
						  const Eigen::Matrix<Dtype,4,4> &view2,
							  const Eigen::Matrix<Dtype,4,4> &proj)
{
	Eigen::Matrix<Dtype,4,1> NDC(x*2-1, y*2-1, z*2-1, 1.0);
	Eigen::Matrix<Dtype,4,1> position = (proj*view2*model2).inverse()*NDC;
	//rotate
	Eigen::Matrix<Dtype,4,1> coords = proj * view1 * model1 * position;
	coords /= coords.w();
	//goes back into (0,1)
	coords = coords.array()/2+0.5;
	//make sure
	coords.x() = std::min<Dtype>(0.99,std::max<Dtype>(0.01,coords.x()));
	coords.y() = std::min<Dtype>(0.99,std::max<Dtype>(0.01,coords.y()));
	coords.z() = std::min<Dtype>(0.99,std::max<Dtype>(0.01,coords.z()));
	return coords.head(3);
}

float interpolate( float val, float y0, float x0, float y1, float x1 ) {
    return (val-x0)*(y1-y0)/(x1-x0) + y0;
}

float base( float val ) {
    if ( val <= -0.75 ) return 0;
    else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 );
    else if ( val <= 0.25 ) return 1.0;
    else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 );
    else return 0.0;
}

cv::Vec3b jetColor(float gray)
{
	return cv::Vec3b(base( gray + 0.5 )*255,base( gray )*255, base( gray - 0.5 )*255);
}

	template <typename Dtype>
cv::Mat iso_surface(const  Grid<Dtype> &vox,
					float threshold)
{
	int size = vox[0].rows();
	cv::Mat img(size, size,CV_8UC3);
	for(int i = 0; i <size; i++)
	{
		for(int j = 0; j < size; j++)
		{
			int c_depth = -1;
			for(int c = 0; c < size; c++)
			{
float val_depth= vox[c](i,j);
				if (val_depth > threshold)
				{
					c_depth = c;
					break;
				}
			}
			float depth = (c_depth+0.5)/(size);
			cv::Vec3b color = jetColor(depth*2-1);
			img.at<cv::Vec3b>(i, j)=color;
		}
	}

	return img;
}

//takes only blobs and do the work (go to eigen and rotate) AVOID COPYING!
	template <typename Dtype>
	void rotate_blobs(const Blob<Dtype> * pred,
					  const Dtype* model1,
					  const Dtype* view_mat1,
					  const Dtype* model2,
					  const Dtype* view_mat2,
					  const Dtype* proj_mat,
					  Dtype * output)  //WARNING a voir
	{
		//go from pred to a Grid<Dtype>
		int output_channels=pred->channels();
		int output_width = pred->width();
		int output_height = pred->height();
		int size = output_width;

		Grid<Dtype> grid(size);
		const Dtype* output_data=pred->cpu_data();
		//chenger methode selon taille du blob  (unfold ou 3d)
		if (output_height == size*size) //if pred from a net, skip first classif layer
			output_data+=output_width * output_height;

		for(int c = 0; c < size; c++)
		{
			Slice<Dtype> channel(size, size);
			std::memcpy(channel.data(), output_data, size*size* sizeof(Dtype));
			output_data += size*size;
			grid[c] = channel;
		}
		//transform viewpoint, view_mat, proj_mat into eigen matrices
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > view1(view_mat1);
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > view2(view_mat2);
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > proj(proj_mat);
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > mv1(model1);
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > mv2(model2);
		// std::cout<<view1<<std::endl<<std::endl;
		// std::cout<<view2<<std::endl<<std::endl;
		// std::cout<<mv1<<std::endl<<std::endl;
		// std::cout<<mv2<<std::endl<<std::endl;
		//compute model
		// Eigen::Matrix<Dtype,4,4> model;
		// model = new_view*model_old.inverse();
		// model=model.inverse();

		//rotate
		Grid<Dtype> gt12 = rotate_voxels_prediction<Dtype>(grid, mv1, view1, mv2, view2, proj);
		// std::cout<<"rotate ok"<<std::endl;
		// cv::Mat pred1 = iso_surface(grid, 0.3);
		// cv::namedWindow( "pred1", CV_WINDOW_NORMAL );
		// cv::imshow("pred1",pred1);
		// cv::Mat pred2 = iso_surface(gt12, 0.3);
		// cv::namedWindow( "pred2", CV_WINDOW_NORMAL );
		// cv::imshow("pred2",pred2);
		// cv::waitKey(0);
		//put into output

		for(int c = 0; c < size; c++)
		{
			std::memcpy(output, gt12[c].data() ,size * size * sizeof(Dtype));
			output += size*size;
		}
	}

	template <typename Dtype>
	Grid<Dtype> unpack_pred_in_image( cv::Mat &image, int grid_rows, int grid_cols)
{
	//assert(grid_rows*grid_cols == grid.size()/4);
	int nrows = image.rows/grid_rows;
	int ncols = image.cols/grid_cols;
	int nchannels = nrows/4;//std::min<int>(grid.size(), grid_rows*grid_cols);
	//cv::Mat img(nrows*grid_rows, ncols*grid_cols,CV_8UC4);
	//std::cout<<"start to unpack\n";
	std::vector<cv::Mat> imgs;
	int from_to[4*2] = {0,2,1,1,2,0,3,3};
	std::vector<cv::Mat> img;
	img.push_back(image);
	// std::cout<<img[0].type()<<std::endl;
	//std::cout<<img[0].channels()<<std::endl;
	for(int i = 0; i<4; i++)
	{
		imgs.push_back(cv::Mat(image.size(),CV_8UC1));
	}
	cv::mixChannels(img,imgs,from_to,4);
	// for(int i = 0; i<4; i++)
	// 	imShow::show(imgs[i],"img"+std::to_string(i));
	// imShow::wait();
	// cv::namedWindow("grid");

	Grid<Dtype> grid(nrows);
	for(int c = 0; c<nchannels; c++)
	{
		int i_cell = c/grid_cols*nrows;
		int j_cell = c%grid_cols*ncols;
		//std::cout<<"c = "<<c<<", icell = "<<i_cell<<", jcell = "<<j_cell<<std::endl;
		for(int i = 0; i<4; i++)
		{
			int channel = c*4 + i;
			//std::cout<<"copying channel "<<channel<<std::endl;
			cv::Mat part = imgs[i](cv::Rect(j_cell,i_cell,nrows,ncols)),partF;
			part.convertTo(partF, CV_type<Dtype>(), 1.0/255);

			Eigen::Map<Slice<Dtype> > chan(reinterpret_cast<Dtype*>(partF.data),nrows,ncols);
			grid[channel] =chan;

		}
	}
	// cv::imshow("grid",pred_to_grid(grid,8,8));
	// cv::waitKey(0);
	return grid;
}

template <typename Dtype>
int CV_type()
{
	return 0;
}
	template <>
	int CV_type<float>()
	{
		return CV_32F;
	}
	template <>
	int CV_type<double>()
	{
		return CV_64F;
	}

	template 	Grid<float> unpack_pred_in_image( cv::Mat &image, int grid_rows, int grid_cols);
	template 	Grid<double> unpack_pred_in_image( cv::Mat &image, int grid_rows, int grid_cols);

	template void rotate_blobs(const Blob<double> * pred, const double* model1, const double* model2, const double* view_mat1, const double* view_mat2,  const double* proj_mat, double * output);
	template void rotate_blobs(const Blob<float> * pred, const float* model1, const float* model2, const float* view_mat1, const float* view_mat2,  const float* proj_mat, float * output);
}


#include <CGAL/Surface_mesh_segmentation/internal/Expectation_maximization.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
/**
 * Generates sample points using a few gauissians.
 * Then applies gmm fitting on these generated points.
 * Provides a heuristic score for each gmm fitting result.
 *
 */
int main(void)
{
    boost::mt19937 engine;
    engine.seed(1340818006);

    // generate random data using gauissians below
    std::vector< boost::normal_distribution<double> > distributions;
    distributions.push_back(boost::normal_distribution<double>(0.1, 0.05));
    distributions.push_back(boost::normal_distribution<double>(0.4, 0.1));
    distributions.push_back(boost::normal_distribution<double>(0.55, 0.05));
    distributions.push_back(boost::normal_distribution<double>(0.7, 0.1));
    distributions.push_back(boost::normal_distribution<double>(0.9, 0.05));
    distributions.push_back(boost::normal_distribution<double>(1.0, 0.05));

    std::vector<double> data;
    for(std::vector< boost::normal_distribution<double> >::iterator it = distributions.begin();
      it != distributions.end(); ++it)
    {
        boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > var_nor(engine, *it);

        for(std::size_t i = 0; i < 300; ++i) { data.push_back(var_nor()); }
    }

    // calculate closest center (using above gauissians) for each generated points
    // we will compare it with gmm fitting results
    // also we might want to compute mixing coef for each center and select centers according to mixing_coef * prob(data)
    std::vector<std::size_t> data_centers;
    for(std::vector<double>::iterator it = data.begin(); it != data.end(); ++it)
    {
        std::size_t center_id = (std::numeric_limits<std::size_t>::max)(), center_counter = 0;;
        double min_distance = (std::numeric_limits<double>::max)();
        for(std::vector< boost::normal_distribution<double> >::iterator dis_it = distributions.begin();
          dis_it != distributions.end(); ++dis_it, ++center_counter)
        {
            double distance = std::abs(*it - dis_it->mean());
            if(min_distance > distance)
            {
                min_distance = distance;
                center_id = center_counter;
            }
        }
        data_centers.push_back(center_id);
    }

    // apply gmm fitting clustering
    typedef CGAL::internal::Expectation_maximization E_M;
    std::vector<E_M> gmm_fitters;
    gmm_fitters.push_back(E_M(distributions.size(), data, E_M::PLUS_INITIALIZATION));
    gmm_fitters.push_back(E_M(distributions.size(), data, E_M::RANDOM_INITIALIZATION));
    gmm_fitters.push_back(E_M(distributions.size(), data, E_M::K_MEANS_INITIALIZATION));

    std::vector< std::vector<std::size_t> > calculated_centers(gmm_fitters.size());
    std::vector< std::vector<std::size_t> >::iterator calc_centers_it = calculated_centers.begin();
    for(std::vector<E_M>::iterator it = gmm_fitters.begin(); it != gmm_fitters.end(); ++it, ++calc_centers_it)
    {
        it->fill_with_center_ids(*calc_centers_it);
    }

    std::cout << "Compare results of EM with 'expected' (but be aware, it is not optimal result in terms of likelihood)" << std::endl;
    std::cout << "Another words a clustering which has higher likelihood can result in worse score in here" << std::endl;
    for(std::vector< std::vector<std::size_t> >::iterator calc_centers_it = calculated_centers.begin();
        calc_centers_it != calculated_centers.end(); ++calc_centers_it)
    {
        std::size_t true_count = 0;
        std::vector<std::size_t>::iterator calculated_it = calc_centers_it->begin();
        for(std::vector<std::size_t>::iterator it = data_centers.begin(); it != data_centers.end(); ++it, ++calculated_it)
        {
            if( (*it) == (*calculated_it) ) { ++true_count; }
        }
        double app_fit = static_cast<double>(true_count) / data_centers.size();
        std::cout << "[0,1]: " << app_fit << std::endl;
        if(app_fit < 0.7) {
            std::cerr << "There might be a problem if above printed comparison is too low." << std::endl;
            return EXIT_FAILURE;
        }
    }
}