/*
 *            Copyright 2016 The MUSCET Development Team
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/xtp/gyration.h>
#include <boost/format.hpp>
#include <votca/xtp/numerical_integrations.h>
#include <votca/xtp/orbitals.h>
//#include <votca/xtp/units.h>
#include <votca/tools/linalg.h>

namespace votca { namespace xtp {

void Density2Gyration::Initialize(Property* options) {
    string key = Identify();

    _state    = options->get(key + ".state").as<string> ();
    _state_no = options->get(key + ".statenumber").as<int> ();
    _spin     = options->get(key + ".spin").as<string> ();
    if ( options->exists(key+".ecp")) {
       _use_ecp=options->get(key + ".ecp").as<bool> ();
    }

    _integrationmethod     = options->get(key + ".integrationmethod").as<string> ();

    if (!(_integrationmethod=="numeric" || _integrationmethod=="analytic")){
        std::runtime_error("Method not recognized. Only numeric and analytic available");
    }
    if ( options->exists(key+".gridsize")) {
         _gridsize = options->get(key+".gridsize").as<string>();
         }
    else _gridsize="medium";
    if ( options->exists(key+".openmp")) {
         _openmp_threads = options->get(key+".openmp").as<int>();
         }
    else _openmp_threads=0;

    // get the path to the shared folders with xml files
    char *votca_share = getenv("VOTCASHARE");
    if(votca_share == NULL) throw std::runtime_error("VOTCASHARE not set, cannot open help files.");

}

void Density2Gyration::AnalyzeDensity( Orbitals & _orbitals ){
    int threads=1;
#ifdef _OPENMP
            if ( _openmp_threads > 0 ) omp_set_num_threads(_openmp_threads);
            threads=omp_get_max_threads();
#endif
   CTP_LOG(ctp::logDEBUG, *_log) << "===== Running on "<< threads << " threads ===== " << flush;

        vector< ctp::QMAtom* > Atomlist =_orbitals.QMAtoms();
        std::vector< ctp::QMAtom* >::iterator at;
        for (at=Atomlist.begin();at<Atomlist.end();++at){
            ctp::QMAtom * atom=new ctp::QMAtom(*(*at));
            _Atomlist.push_back(atom);
        }
        ub::matrix<double> DMAT_tot;
        BasisSet bs;
        bs.LoadBasisSet(_orbitals.getDFTbasis());
        AOBasis basis;
        basis.AOBasisFill(&bs, _Atomlist );

        // Analyze geometry
        AnalyzeGeometry( _Atomlist );

        std::vector<ub::matrix<double> > DMAT;

        //basis.ReorderMOs(_orbitals.MOCoefficients(), _orbitals.getQMpackage(), "votca" );

        if(_state=="transition"){
                DMAT_tot=_orbitals.TransitionDensityMatrix(_spin, _state_no-1);
        }
        else if (_state=="ground" || _state=="excited" || _state=="exciton" ){
             CTP_LOG(ctp::logDEBUG, *_log) << "Calculating density matrix:        " << _state << " No. " << _state_no << flush;

            ub::matrix<double> DMATGS=_orbitals.DensityMatrixGroundState();
            DMAT_tot=DMATGS;
            if ( _state_no > 0 && ( _state=="excited" || _state=="exciton" ) ){

                DMAT = _orbitals.DensityMatrixExcitedState( _spin, _state_no-1);

                if (_state == "excited" ){
                    DMAT_tot=DMAT_tot-DMAT[0]+DMAT[1];
                }

            }
	   // Ground state + hole_contribution + electron contribution
	}
        else throw std::runtime_error("State entry not recognized");

        if (_integrationmethod=="numeric")  {

            // setup numerical integration grid
            NumericalIntegration numway;
            numway.GridSetup(_gridsize,&bs,_Atomlist,&basis);

            if ( _state=="ground" || _state=="excited") {
                //LOG(logDEBUG, *_log) << TimeStamp() << " Calculate Densities at Numerical Grid with gridsize "<< _gridsize  << flush;
                ub::vector<double> _analysis=numway.IntegrateGyrationTensor(DMAT_tot);

                // convert to eigenframe
                ub::vector<double> _gyration_tensor_diagonal;
                ub::matrix<double> _gyration_tensor_eigenframe;
                CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Converting to Eigenframe " << flush;
                Convert2Eigenframe( _analysis, _gyration_tensor_diagonal, _gyration_tensor_eigenframe );

                // determine quaternion for rotation of xyz to EF
                CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating Quaternion " << flush;
                ub::vector<double> _quaternion = get_quaternion( _gyration_tensor_eigenframe );

                // report results
                CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Reporting " << flush;
                ReportAnalysis( _state, _analysis, _gyration_tensor_diagonal, _gyration_tensor_eigenframe  );

            } else if ( _state == "exciton" ){
                // hole density first
                ub::vector<double> _analysis_hole=numway.IntegrateGyrationTensor(DMAT[0]);

                // convert to eigenframe
                ub::vector<double> _gyration_tensor_diagonal_hole;
                ub::matrix<double> _gyration_tensor_eigenframe_hole;
                Convert2Eigenframe( _analysis_hole, _gyration_tensor_diagonal_hole, _gyration_tensor_eigenframe_hole );

                // determine quaternion for rotation of xyz to EF
                ub::vector<double> _quaternion_hole = get_quaternion( _gyration_tensor_eigenframe_hole );

                // report results
                ReportAnalysis( "hole", _analysis_hole, _gyration_tensor_diagonal_hole, _gyration_tensor_eigenframe_hole  );

                // electron density
                ub::vector<double> _analysis_electron=numway.IntegrateGyrationTensor(DMAT[1]);

                // convert to eigenframe
                ub::vector<double> _gyration_tensor_diagonal_electron;
                ub::matrix<double> _gyration_tensor_eigenframe_electron;
                Convert2Eigenframe( _analysis_electron, _gyration_tensor_diagonal_electron, _gyration_tensor_eigenframe_electron );

                // determine quaternion for rotation of xyz to EF
                 ub::vector<double> _quaternion_electron = get_quaternion( _gyration_tensor_eigenframe_electron );

                // report results
                ReportAnalysis( "electron", _analysis_electron, _gyration_tensor_diagonal_electron,  _gyration_tensor_eigenframe_electron  );

            }

        }
          else if (_integrationmethod=="analytic") {
              //esp.Fit2Density_analytic(_Atomlist,DMAT_tot,basis);
        }
        }

    void Density2Gyration::AnalyzeGeometry(vector<ctp::QMAtom*> _atoms){

        Elements _elements;
        ub::vector<double> _analysis = ub::zero_vector<double>(10);
        std::vector< ctp::QMAtom* >::iterator at;
        for (at=_atoms.begin();at<_atoms.end();++at){

            double m = _elements.getMass( (*at)->type );
            double x = (*at)->x ;
            double y = (*at)->y ;
            double z = (*at)->z ;
            _analysis(0) += m   ;
            _analysis(1) += m*x ;
            _analysis(2) += m*y ;
            _analysis(3) += m*z ;

            _analysis(4) += m*x*x;
            _analysis(5) += m*x*y;
            _analysis(6) += m*x*z;
            _analysis(7) += m*y*y;
            _analysis(8) += m*y*z;
            _analysis(9) += m*z*z;

        }

        // normalize
        for ( unsigned i =1 ; i < 4; i++){
            _analysis(i) = _analysis(i)/_analysis(0)/tools::conv::bohr2ang;
        }
                // normalize
        for ( unsigned i =4 ; i < _analysis.size(); i++){
            _analysis(i) = _analysis(i)/_analysis(0)/tools::conv::bohr2ang/tools::conv::bohr2ang;
        }

        // gyration tensor
        _analysis(4) -= _analysis(1)*_analysis(1);
        _analysis(5) -= _analysis(1)*_analysis(2);
        _analysis(6) -= _analysis(1)*_analysis(3);
        _analysis(7) -= _analysis(2)*_analysis(2);
        _analysis(8) -= _analysis(2)*_analysis(3);
        _analysis(9) -= _analysis(3)*_analysis(3);

        // convert to eigenframe
        ub::vector<double> _gyration_tensor_diagonal;
        ub::matrix<double> _gyration_tensor_eigenframe;
        Convert2Eigenframe( _analysis, _gyration_tensor_diagonal, _gyration_tensor_eigenframe );

        cout << "\n";

        for ( int i =0 ; i< 3; i++){

            cout << _gyration_tensor_eigenframe(0,i) << " " << _gyration_tensor_eigenframe(1,i) << "  " <<  _gyration_tensor_eigenframe(2,i) << "\n" << endl;

        }

        /*
        // test for right-handedness
        double check_x = _gyration_tensor_eigenframe(1,0)* _gyration_tensor_eigenframe(2,1) -  _gyration_tensor_eigenframe(2,0)* _gyration_tensor_eigenframe(1,1) ;
        double check_y = _gyration_tensor_eigenframe(2,0)* _gyration_tensor_eigenframe(0,1) -  _gyration_tensor_eigenframe(0,0)* _gyration_tensor_eigenframe(2,1) ;
        double check_z = _gyration_tensor_eigenframe(0,0)* _gyration_tensor_eigenframe(1,1) -  _gyration_tensor_eigenframe(1,0)* _gyration_tensor_eigenframe(0,1) ;

        cout << " check x" << check_x << " vs " << _gyration_tensor_eigenframe(0,2) << "\n" <<endl;
        cout << " check y" << check_y << " vs " << _gyration_tensor_eigenframe(1,2) << "\n" <<endl;
        cout << " check z" << check_z << " vs " << _gyration_tensor_eigenframe(2,2) << "\n" <<endl;
        */
        // determine quaternion for rotation of xyz to EF
        ub::vector<double> _quaternion = get_quaternion( _gyration_tensor_eigenframe );

        // report results
        ReportAnalysis( "geometry", _analysis, _gyration_tensor_diagonal, _gyration_tensor_eigenframe  );

    }

    void Density2Gyration::ReportAnalysis(string label, ub::vector<double> _tensor_elements, ub::vector<double> _tensor_diagonal, ub::matrix<double> _tensor_frame){

            CTP_LOG(ctp::logINFO, *_log) << "---------------- " << label << " ----------------" << flush;
            CTP_LOG(ctp::logINFO, *_log) << (boost::format("  Norm               = %1$9.4f ") % (_tensor_elements(0)) ) << flush;

            //LOG(logINFO,*_pLog) << (format("  Level = %1$4d DFT = %2$+1.4f VXC = %3$+1.4f S-X = %4$+1.4f S-C = %5$+1.4f GWA = %6$+1.4f") % (_i+_qpmin+1) % _dft_energies( _i + _qpmin ) % _vxc(_i,_i) % _sigma_x(_i,_i) % _sigma_c(_i,_i) % _qp_energies(_i + _qpmin ) ).str() << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid x         = %1$9.4f Ang") % (_tensor_elements(1)*tools::conv::bohr2ang) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid y         = %1$9.4f Ang") % (_tensor_elements(2)*tools::conv::bohr2ang) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid y         = %1$9.4f Ang") % (_tensor_elements(3)*tools::conv::bohr2ang) ) << flush;

            double RA2 = tools::conv::bohr2ang  *tools::conv::bohr2ang;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xx = %1$9.4f Ang^2") % (_tensor_elements(4)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xy = %1$9.4f Ang^2") % (_tensor_elements(5)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xz = %1$9.4f Ang^2") % (_tensor_elements(6)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor yy = %1$9.4f Ang^2") % (_tensor_elements(7)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor yz = %1$9.4f Ang^2") % (_tensor_elements(8)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor zz = %1$9.4f Ang^2") % (_tensor_elements(9)*RA2) ) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D1 = %1$9.4f Ang^2") % (_tensor_diagonal(0)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D2 = %1$9.4f Ang^2") % (_tensor_diagonal(1)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D3 = %1$9.4f Ang^2") % (_tensor_diagonal(2)*RA2) ) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Radius of Gyration = %1$9.4f Ang") % (std::sqrt(_tensor_diagonal(0) + _tensor_diagonal(1) + _tensor_diagonal(2))*tools::conv::bohr2ang )) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 1 = %1$9.4f ") % (_tensor_frame(0,0)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 2 = %1$9.4f ") % (_tensor_frame(1,0)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 3 = %1$9.4f ") % (_tensor_frame(2,0)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 1 = %1$9.4f ") % (_tensor_frame(0,1)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 2 = %1$9.4f ") % (_tensor_frame(1,1)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 3 = %1$9.4f ") % (_tensor_frame(2,1)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 1 = %1$9.4f ") % (_tensor_frame(0,2)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 2 = %1$9.4f ") % (_tensor_frame(1,2)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 3 = %1$9.4f ") % (_tensor_frame(2,2)) ) << flush;

    }

    ub::vector<double> Density2Gyration::get_quaternion(ub::matrix<double>& eigenframe){

        // second coordinate system is assumed to be Cartesian

        ub::matrix<double> M=ub::zero_matrix<double>(3,3);

        // add all the outer products, if second coordinate system was not Cartesian
        // is anyways, so M=eigenframe
        //M=ub::trans(eigenframe);
        M=eigenframe;

        ub::matrix<double> N = ub::zero_matrix<double>(4,4);

        N(0,0) =  M(0,0) + M(1,1) + M(2,2); // N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2]);
        N(1,1) =  M(0,0) - M(1,1) - M(2,2); // N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2])
        N(2,2) = -M(0,0) + M(1,1) - M(2,2); // N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2])
        N(3,3) = -M(0,0) - M(1,1) + M(2,2); // N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2])
        N(0,1) =  M(1,2) - M(2,1);          // N12=float(M[1][:,2]-M[2][:,1])
        N(0,2) =  M(2,0) - M(0,2);          // N13=float(M[2][:,0]-M[0][:,2])
        N(0,3) =  M(0,1) - M(1,0);          // N14=float(M[0][:,1]-M[1][:,0])
        N(1,0) =  N(0,1);                   // N21=float(N12)
        N(1,2) =  M(0,1) + M(1,0);          // N23=float(M[0][:,1]+M[1][:,0])
        N(1,3) =  M(2,0) + M(0,2);          // N24=float(M[2][:,0]+M[0][:,2])
        N(2,0) =  N(0,2);                   // N31=float(N13)
        N(2,1) =  N(1,2);                   // N32=float(N23)
        N(2,3) =  M(1,2) + M(2,1);          // N34=float(M[1][:,2]+M[2][:,1])
        N(3,0) =  N(0,3);                   // N41=float(N14)
        N(3,1) =  N(1,3);                   // N42=float(N24)
        N(3,2) =  N(2,3);                   // N43=float(N34)

        ub::vector<double> eigenvalues;
        ub::matrix<double> eigenvectors;
        linalg_eigenvalues( N, eigenvalues, eigenvectors );

        // find max eigenvalue
        int index=0;
        double maxev = eigenvalues(0);

        for (unsigned i = 1; i < eigenvalues.size(); i++ ) {
            if ( eigenvalues(i) > maxev ) {
                maxev = eigenvalues(i);
                index = i;
            }
        }

        // return this vector
        ub::vector<double> quaternion = ub::zero_vector<double>(4);
        quaternion(0)= eigenvectors(0,index);
        quaternion(1)= eigenvectors(1,index);
        quaternion(2)= eigenvectors(2,index);
        quaternion(3)= eigenvectors(3,index);

        return quaternion;

    }

        void Density2Gyration::Convert2Eigenframe(ub::vector<double> V, ub::vector<double> &_gyration_tensor_diagonal, ub::matrix<double> &_gyration_tensor_eigenframe   ){

                 ub::matrix<double> _gyration_tensor = ub::zero_matrix<double>(3,3);
                _gyration_tensor(0,0) = V(4);
                _gyration_tensor(1,0) = V(5);
                _gyration_tensor(2,0) = V(6);
                _gyration_tensor(0,1) = V(5);
                _gyration_tensor(1,1) = V(7);
                _gyration_tensor(2,1) = V(8);
                _gyration_tensor(0,2) = V(6);
                _gyration_tensor(1,2) = V(8);
                _gyration_tensor(2,2) = V(9);

                linalg_eigenvalues( _gyration_tensor, _gyration_tensor_diagonal, _gyration_tensor_eigenframe );

        }

}}


#include "tree.h"
#include <cmath>
#include "utilities.h"
#include <boost/foreach.hpp>

Tree::Tree(const HyperParameters &hp, const int n) : m_hp( hp )
{
    if (n>-1){
        _rand(n);
    }
    if (hp.useRandProj)
    {
        m_rootNode = Node::Ptr(new NodeHyperPlane(hp,0,0));
    }
    else
    {
        if (hp.useInfoGain)
        {
            m_rootNode = Node::Ptr(new NodeInfoGain(hp,0,0));
        }
        else
        {
            m_rootNode = Node::Ptr(new NodeGini(hp,0,0));
        }
    }
}

Tree::Tree(const HyperParameters &hp, const xmlNodePtr treeNode,const int n) : m_hp( hp )
{
    if (n>-1){
        _rand(n);
    }
    xmlNodePtr cur = treeNode->xmlChildrenNode;
    while ( cur != 0 )
    {
        if ( xmlStrcmp( cur->name, reinterpret_cast<const xmlChar*>( "node" ) ) == 0 )
        {
            std::string nodeType = readStringProp( cur, "type" );
            if (nodeType == NODE_GINI)
            {
                m_rootNode = Node::Ptr( new NodeGini( m_hp, 0, cur ) );
            }
            else if (nodeType == NODE_INFO_GAIN)
            {
                m_rootNode = Node::Ptr( new NodeInfoGain( hp, 0, cur ) );
            }
        }
        else if ( xmlStrcmp( cur->name, reinterpret_cast<const xmlChar*>( "constants" ) ) == 0 )
        {
            //Configurator::conf()->loadConstants( cur );
        }
        cur = cur->next;
    }
}

Tree::~Tree()
{
}

xmlNodePtr Tree::save() const
{
    xmlNodePtr node = xmlNewNode( NULL, reinterpret_cast<const xmlChar*>( "tree" ) );
    xmlAddChild( node, m_rootNode->save() );
    return node;
}

void Tree::finalize(const matrix<float>& data,
                    matrix<float>& forestConfidences, matrix<float>& forestOutOfBagConfidences,
                    std::vector<int>& forestOutOfBagVoteNum)
{
    // Fill the confidence of the forest
    for (long int nSamp = 0; nSamp < m_hp.numLabeled; nSamp++)
    {
        if (m_hp.useSoftVoting)
        {
            for (int nClass = 0; nClass < m_hp.numClasses; nClass++)
            {
                forestConfidences(nSamp,nClass) += m_confidences(nSamp,nClass);
            }
        }
        else
        {
            forestConfidences(nSamp, m_predictions[nSamp])++;
        }
    }

    // Fill the out of bag confidences and vote count
    BOOST_FOREACH(long int n, m_outOfBagSamples)
    {
        forestOutOfBagVoteNum[n]++;

        if (m_hp.useSoftVoting)
        {
            for (int nClass = 0; nClass < m_hp.numClasses; nClass++)
            {
                forestOutOfBagConfidences(n,nClass) += m_confidences(n,nClass);
            }
        }
        else
        {
            forestOutOfBagConfidences(n, m_predictions[n])++;
        }
    }

    // Clean up
    clean();
}

void Tree::clean()
{
    m_confidences.clear();
}

// This
void Tree::train(const matrix<float>& data, const std::vector<int>& labels,
                 matrix<float>& forestConfidences, matrix<float>& forestOutOfBagConfidences,
                 std::vector<int>& forestOutOfBagVoteNum)
{
    // Initialize
    initialize(m_hp.numLabeled);

    // Random Subsamples data according to bagratio
    subSample(m_hp.numLabeled);

    // Train the root Node
    m_rootNode->train(data, labels, m_inBagSamples, m_confidences, m_predictions);

    std::vector<int> allSamples;
    allSamples.insert(allSamples.begin(),m_inBagSamples.begin(),m_inBagSamples.end());
    allSamples.insert(allSamples.begin(),m_outOfBagSamples.begin(),m_outOfBagSamples.end());

//    m_rootNode->refine(data,labels,allSamples,m_confidences,m_predictions);

    evalOutOfBagSamples(data);
    finalize(data, forestConfidences, forestOutOfBagConfidences, forestOutOfBagVoteNum);
    if (m_hp.verbose)
    {
        cout << "Trained a tree with " << m_rootNode->numNodes() << " nodes." << endl;
        cout << "Training error = " << computeError(labels) << ", in bag = ";
        cout << computeError(labels, m_inBagSamples) << ", out of bag = ";
        cout << computeError(labels, m_outOfBagSamples) <<  endl;
    }
}

void Tree::train(const matrix<float>& data, const std::vector<int>& labels, const std::vector<double>& weights,
                 matrix<float>& forestConfidences, matrix<float>& forestOutOfBagConfidences,
                 std::vector<int>& forestOutOfBagVoteNum)
{

    // Initialize
    initialize(m_hp.numLabeled);

    // Random Subsamples data according to bagratio
    subSample(m_hp.numLabeled);
    //subSample(m_hp.numLabeled, weights);

    // Train the root Node
    m_rootNode->train(data, labels, weights, m_inBagSamples, m_confidences, m_predictions);

    std::vector<int> allSamples;
    allSamples.insert(allSamples.begin(),m_inBagSamples.begin(),m_inBagSamples.end());
    allSamples.insert(allSamples.begin(),m_outOfBagSamples.begin(),m_outOfBagSamples.end());

//    m_rootNode->refine(data,labels,weights,allSamples,m_confidences,m_predictions);

    evalOutOfBagSamples(data);

    // Fill out the confidences of the forest
    finalize(data,forestConfidences,forestOutOfBagConfidences,forestOutOfBagVoteNum);

    if (m_hp.verbose)
    {
        cout << "Trained a tree with " << m_rootNode->numNodes() << " nodes." << endl;
        cout << "Training error = " << computeError(labels) << ", in bag = ";
        cout << computeError(labels, m_inBagSamples) << ", out of bag = ";
        cout << computeError(labels, m_outOfBagSamples) <<  endl;
    }
}

void Tree::train(const matrix<float>& data, const std::vector<int>& labels)
{
    // Initialize
    initialize(m_hp.numLabeled);

    // Random Subsamples data according to bagratio
    subSample(m_hp.numLabeled);

    // Train the root Node
    m_rootNode->train(data, labels, m_inBagSamples, m_confidences, m_predictions);

    evalOutOfBagSamples(data);

    if (m_hp.verbose)
    {
        cout << "Trained a tree with " << m_rootNode->numNodes() << " nodes." << endl;
        cout << "Training error = " << computeError(labels) << ", in bag = ";
        cout << computeError(labels, m_inBagSamples) << ", out of bag = ";
        cout << computeError(labels, m_outOfBagSamples) <<  endl;
    }
}

void Tree::trainLU(const matrix<float>& data, const std::vector<int>& labels)
{
    // Initialize
    initialize(m_hp.numLabeled);

    // Random Subsamples data according to bagratio
    subSample(m_hp.numLabeled);

    // Train the root Node
    m_rootNode->trainLU(data, labels, m_inBagSamples, m_confidences, m_predictions);
    evalOutOfBagSamples(data);

    if (m_hp.verbose)
    {
        cout << "Trained a tree with " << m_rootNode->numNodes() << " nodes." << endl;
        cout << "Training error = " << computeError(labels) << ", in bag = ";
        cout << computeError(labels, m_inBagSamples) << ", out of bag = ";
        cout << computeError(labels, m_outOfBagSamples) <<  endl;
    }
}

void Tree::train(const matrix<float>& data, const std::vector<int>& labels, const std::vector<double>& weights)
{
    // Initialize
    initialize(m_hp.numLabeled);

    // Random Subsamples data according to bagratio
    subSample(m_hp.numLabeled);

    // Train the root Node
    m_rootNode->train(data, labels, weights, m_inBagSamples, m_confidences, m_predictions);

    bool refine = false;
    if (refine)
    {
        cout << "refine tree... ";
        std::vector<int> samples(m_hp.numLabeled,0);
        std::vector<int>::iterator sampleIt(samples.begin());
        std::vector<int>::iterator sampleEnd(samples.end());
        long int counter = 0;
        while ( sampleIt != sampleEnd )
        {
            *sampleIt = counter;
            ++counter;
            ++sampleIt;
        }
        m_rootNode->refine(data,labels,weights,samples,m_confidences,m_predictions);
        cout << "done." << endl;
    }
    evalOutOfBagSamples(data);

    if (m_hp.verbose)
    {
        cout << "Trained a tree with " << m_rootNode->numNodes() << " nodes." << endl;
        cout << "Training error = " << computeError(labels) << ", in bag = ";
        cout << computeError(labels, m_inBagSamples) << ", out of bag = ";
        cout << computeError(labels, m_outOfBagSamples) <<  endl;
    }
}

void Tree::evalOutOfBagSamples(const matrix<float>& data)
{
    m_rootNode->eval(data, m_outOfBagSamples, m_confidences, m_predictions);
}

void Tree::eval(const matrix<float>& data, const std::vector<int>& labels, matrix<float>& forestConfidences)
{
    // Initialize
    m_confidences.resize(data.size1(), m_hp.numClasses);
    m_predictions.resize(data.size1());

    std::vector<int> sampleIndeces(data.size1());
//    sampleIndeces.reserve(data.size1());
    for (unsigned int i = 0; i < data.size1(); i++)
    {
//        sampleIndeces.push_back(i);
    	sampleIndeces[i]=i;
    }
    m_rootNode->eval(data, sampleIndeces, m_confidences, m_predictions);

    // Fill the forest confidences
    for (unsigned long int nSamp = 0; nSamp < data.size1(); nSamp++)
    {
        if (m_hp.useSoftVoting)
        {
            for (int nClass = 0; nClass < m_hp.numClasses; nClass++)
            {
                forestConfidences(nSamp,nClass) += m_confidences(nSamp,nClass);
            }
        }
        else
        {
            forestConfidences(nSamp, m_predictions[nSamp])++;
        }
    }

    if (m_hp.verbose)
    {
        cout << "Tree test error: " << computeError(labels) << endl;
    }
}

void Tree::eval(const matrix<float>& data, const std::vector<int>& labels)
{
    // Initialize
    m_confidences.resize(data.size1(), m_hp.numClasses);
    m_predictions.resize(data.size1());

    std::vector<int> sampleIndeces(data.size1());
//    std::vector<int> sampleIndeces;
//    sampleIndeces.reserve(data.size1());
    for (unsigned long int i = 0; i < data.size1(); i++)
    {
    	sampleIndeces[i]=i;
//        sampleIndeces.push_back(i);
    }

    m_rootNode->eval(data, sampleIndeces, m_confidences, m_predictions);

    if (m_hp.verbose)
    {
        cout << "Test error: " << computeError(labels) << endl;
    }
}

void Tree::initialize(const long int numSamples)
{
    m_confidences.resize(numSamples, m_hp.numClasses);
    m_predictions.resize(numSamples);

    m_inBagSamples.clear();
    m_outOfBagSamples.clear();
}

void Tree::reInitialize(const long int numSamples)
{
    m_confidences.resize(numSamples, m_hp.numClasses);
    m_predictions.resize(numSamples);
}

void Tree::subSample(const long int numSamples)
{
    if (m_hp.useSubSamplingWithReplacement)
    {
        m_inBagSamples = subSampleWithReplacement(numSamples);
        m_outOfBagSamples = setDiff(m_inBagSamples, numSamples);
    }
    else
    {
        subSampleWithoutReplacement(numSamples, static_cast<int>(floor(numSamples * m_hp.bagRatio)),
                                    m_inBagSamples, m_outOfBagSamples);
    }
}

void Tree::subSample(const long int numSamples, const std::vector<double>& weights)
{
    std::vector<int> tmpInBag;
    if (m_hp.useSubSamplingWithReplacement)
    {
        tmpInBag = subSampleWithReplacement(numSamples);
    }
    else
    {
        subSampleWithoutReplacement(numSamples, static_cast<int>(floor(numSamples * m_hp.bagRatio)),
                                    tmpInBag, m_outOfBagSamples);
    }

    // Weight Trimming
    BOOST_FOREACH(int n, tmpInBag)
    {
        if (weights[n] > 1e-4)
        {
            m_inBagSamples.push_back(n);
        }
    }
    m_outOfBagSamples = setDiff(m_inBagSamples, numSamples);
}

double Tree::computeError(const std::vector<int>& labels, const std::vector<int>& sampleIndeces)
{
    double error = 0.0;
    long int sampleCount = 0;
    BOOST_FOREACH(long int n, sampleIndeces)
    {
        if (sampleCount < (int) labels.size())
        {
            error += (m_predictions[n] != labels[n]) ? 1.0 : 0.0;
        }
        else
        {
            break;
        }

        sampleCount++;
    }
    return error/(double) labels.size();
}

double Tree::computeError(const std::vector<int>& labels)
{
    double error = 0.0;
    std::vector<int>::const_iterator itr(m_predictions.begin());
    std::vector<int>::const_iterator labelItr(labels.begin()), labelEnd(labels.end());
    for (; labelItr != labelEnd; itr++, labelItr++)
    {
        error += (*itr != *labelItr) ? 1.0 : 0.0;
    }
    return error/(double) labels.size();
}

void Tree::getTreeAsMatrix(boost::numeric::ublas::matrix<float> *data, const int tree_index)
{
    getTreeAsMatrixRecursive(m_rootNode, data, tree_index, 0);
}

void Tree::getTreeAsMatrixRecursive(Node::Ptr current_node, boost::numeric::ublas::matrix<float> *data,
                                    const int tree_index, const int node_index)
{

    // fill matrix row with current node data
    long int last_index = 0;

    // treeIndex
    (*data)(node_index, last_index++) = (float)tree_index;
    // nodeIndex
    (*data)(node_index, last_index++) = (float)node_index;
    // isTerminal
    if (current_node->isLeaf())
        (*data)(node_index, last_index++) = 1.0f;
    else
        (*data)(node_index, last_index++) = 0.0f;
    // feature indices and weights
    if (!current_node->isLeaf())
    {
        if (m_hp.useRandProj)
        {
            for (int i = 0; i < m_hp.numProjFeatures; i++)
            {
                (*data)(node_index, last_index++) = (float)current_node->bestFeature()[i];
                (*data)(node_index, last_index++) = (float)current_node->bestWeight()[i];
            }
        }
        else
        {
            (*data)(node_index, last_index++) = (float)current_node->bestFeature()[0];
            (*data)(node_index, last_index++) = (float)current_node->bestWeight()[0];
        }
    }
    else
    {
        if (m_hp.useRandProj)
        {
            for (int i = 0; i < m_hp.numProjFeatures; i++)
            {
                (*data)(node_index, last_index++) = -1.0f;
                (*data)(node_index, last_index++) = -1.0f;
            }
        }
        else
        {
            (*data)(node_index, last_index++) = -1.0f;
            (*data)(node_index, last_index++) = -1.0f;
        }
    }
    // threshold
    (*data)(node_index, last_index++) = current_node->bestThreshold();
    // confidences
    if (current_node->isLeaf())
    {
        std::vector<float> confidences = current_node->nodeConf();
        if (confidences.size() != (unsigned int) m_hp.numClasses)
            throw("the number of confidences stored doesn't equal the number of classes");
        for (int i = 0; i < m_hp.numClasses; i++)
            (*data)(node_index, last_index++) = confidences[i];
        // prediction
        (*data)(node_index, last_index++) = (float) current_node->nodeLabel();
    }
    else
    {
        for (int i = 0; i < m_hp.numClasses; i++)
            (*data)(node_index, last_index++) = -1.0f;
    }

    // if necessary, reinvoke function for the child nodes
    if (!current_node->isLeaf())
    {
        getTreeAsMatrixRecursive(current_node->leftChildNode(), data, tree_index, node_index * 2 + 1);
        getTreeAsMatrixRecursive(current_node->rightChildNode(), data, tree_index, node_index * 2 + 2);
    }
}

std::vector<std::vector<int> > Tree::getPath(const matrix<float>& data)
{
    std::vector<std::vector<int> > path(data.size1());
    std::vector<int> sampleIndeces;
    sampleIndeces.reserve(data.size1());
    for (unsigned long int i = 0; i < data.size1(); i++)
    {
        sampleIndeces.push_back(i);
    }
    m_rootNode->getPath(data, sampleIndeces, path);

    return path;
}

int Tree::getDepth()
{
    int treeDepth = m_rootNode->getDepth(0);
    return ++treeDepth;
}

std::vector<int> Tree::getNodeLabels()
{
    std::set<int> nodeLabels;
    m_rootNode->getLabels(nodeLabels);

    std::vector<int> out;
    std::set<int>::const_iterator itr = nodeLabels.begin(), end = nodeLabels.end();
    for (; itr != end; itr++)
    {
        out.push_back(*itr);
    }

    return out;
}


#include <iostream>
#include <string>
#include <boost/functional/hash.hpp>

#include <stdint.h>
#include <boost/format.hpp>
#include <boost/crc.hpp>

#include "consistent_hash_map.hpp"

struct crc32_hasher {
    uint32_t operator()(const std::string& node) {
        boost::crc_32_type ret;
        ret.process_bytes(node.c_str(),node.size());
        return ret.checksum();
    }
    typedef uint32_t result_type;
};

int main(int argc, char const *argv[])
{
    typedef consistent_hash_map<std::string,crc32_hasher> consistent_hash_t;
    consistent_hash_t consistent_hash_;
    boost::format node_fmt("192.168.1.%1%");

    for(std::size_t i=0;i<3;++i) {
        std::string node = boost::str(node_fmt % i);
        consistent_hash_.insert(node);
        std::cout<<boost::format("add node: %1%") % node << std::endl;
    }

{
    std::cout<<"========================================================="<<std::endl;
    for(consistent_hash_t::iterator it = consistent_hash_.begin();it != consistent_hash_.end(); ++it) {
        std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
    }
}

{
    consistent_hash_t::iterator it;
    it = consistent_hash_.find(290235110);
    std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
}

{
    consistent_hash_t::iterator it;
    it = consistent_hash_.find(2286285664);
    std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
}

{
    consistent_hash_t::iterator it;
    it = consistent_hash_.find(4282565578);
    std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
}

  std::cout<<"========================================================="<<std::endl;
{
    std::string node = boost::str(node_fmt % 1);
    consistent_hash_.erase(node);
    for(consistent_hash_t::iterator it = consistent_hash_.begin();it != consistent_hash_.end(); ++it) {
        std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
    }
}

  std::cout<<"========================================================="<<std::endl;
{
    consistent_hash_t::iterator it;
    it = consistent_hash_.find(4282565578);
    std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
    std::cout<<"-------------------------------------------"<<std::endl;
    consistent_hash_.erase(it);
    for(consistent_hash_t::iterator it = consistent_hash_.begin();it != consistent_hash_.end(); ++it) {
        std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
    }
}

  std::cout<<"========================================================="<<std::endl;
{
    std::cout<<"-------------------------------------------"<<std::endl;
    consistent_hash_t::iterator it;
    it = consistent_hash_.find(4282565578);
    std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
    std::cout<<"-------------------------------------------"<<std::endl;

    it = consistent_hash_.find(4282565576);
    std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
    std::cout<<"-------------------------------------------"<<std::endl;

    consistent_hash_.erase(it);

    for(consistent_hash_t::iterator it = consistent_hash_.begin();it != consistent_hash_.end(); ++it) {
        std::cout<<boost::format("node:%1%,%2%") % it->second % it->first << std::endl;
    }
    std::cout<<"-------------------------------------------"<<std::endl;
}

  std::cout<<"========================================================="<<std::endl;
{
    std::cout<<"-------------------------------------------"<<std::endl;
    consistent_hash_t::iterator it;
    it = consistent_hash_.find(4282565578);
    if(it == consistent_hash_.end()) {
        std::cout<<"not found, consistent_hash is empty"<<std::endl;
    }
}

    return 0;
}


/* Structs defined by the RTPS spec
 *
 */

#ifndef CMBML__TYPES__HPP_
#define CMBML__TYPES__HPP_

#include <boost/hana/define_struct.hpp>
#include <chrono>

#include <array>
#include <cstdint>
#include <vector>

namespace cmbml {
  // TODO this should be selectable at compile time
  static const uint32_t cmbml_test_domain_id = 1337;

  enum class ReliabilityKind_t {
    best_effort = 1,
    reliable = 3
  };
  enum class TopicKind_t {
    no_key = 1,
    with_key = 2
  };

  using Octet = uint8_t;
  // 12 octets
  using GuidPrefix_t = std::array<Octet, 12>;
  static const GuidPrefix_t guid_prefix_unknown = {0};
  // 4 octets
  using EntityId_t = std::array<Octet, 4>;
  static const EntityId_t entity_id_unknown = {0x00, 0x00, 0x00, 0x00};
  using VendorId_t = std::array<Octet, 2>;
  static const VendorId_t cmbml_vendor_id = {0xf0, 0x9f};
  static const VendorId_t vendor_id_unknown = {0x0, 0x0};

  using ProtocolId_t = std::array<char, 4>;
  static const ProtocolId_t rtps_protocol_id = {'R', 'T', 'P', 'S'};

  // TODO: Figure out a better embedded-friendly sequence implementation.
  // are we going to pass around an allocator std vector?
  template<typename T, typename Allocator = std::allocator<T>>
  using List = std::vector<T, Allocator>;

  using IPAddress = std::array<Octet, 16>;

  //using Duration_t = std::chrono::nanoseconds;
  struct Duration_t {
    constexpr Duration_t(uint32_t s, uint32_t ns) : sec(s), nsec(ns) {}

    uint32_t sec;
    uint32_t nsec;
    std::chrono::nanoseconds to_ns() const {
      return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::seconds(sec)) +
        std::chrono::nanoseconds(nsec);
    };
  };

  // Interesting, a fixed-point time representation (IETF RFC 1305)
  struct Time_t {
    BOOST_HANA_DEFINE_STRUCT(Time_t,
    (int32_t, seconds),
    (uint32_t, fraction));  // represents sec/2^32
    Time_t() : seconds(0), fraction(0) {};

    constexpr Time_t(int32_t sec, uint32_t frac) : seconds(sec), fraction(frac) {};
  };

  // TODO These constants should have better names (namespaced, or different case)
  constexpr static Time_t time_zero = Time_t(0, 0);
  // woah. Actually, time_t should be 64-bit ints then?
  constexpr static Time_t time_invalid = Time_t(-1, 0xffffffff);
  constexpr static Time_t time_infinite = Time_t(0x7fffffff, 0xffffffff);

  struct SequenceNumber_t {
    BOOST_HANA_DEFINE_STRUCT(SequenceNumber_t,
    (int32_t, high),
    (uint32_t, low));

    constexpr uint64_t value() const {
      return high*multiplicand + low;
    }

    constexpr static bool less_than(const SequenceNumber_t & a, const SequenceNumber_t & b) {
      return a.value() < b.value();
    }
    bool operator<(const SequenceNumber_t & a) {
      return this->value() < a.value();
    }
    bool operator>=(const SequenceNumber_t & a) const {
      return this->value() >= a.value();
    }
    bool operator<=(const SequenceNumber_t & a) const {
      return this->value() <= a.value();
    }
    bool operator>(const SequenceNumber_t & a) const {
      return this->value() > a.value();
    }
    constexpr static bool equal(const SequenceNumber_t & a, const SequenceNumber_t & b) {
      return a.value() == b.value();
    }
    template<typename T, typename std::enable_if<std::is_integral<T>::value>::type * = nullptr>
    constexpr SequenceNumber_t operator+(const T i) const {
      // TODO Overflow
      return SequenceNumber_t({this->high, this->low + static_cast<uint32_t>(i)});
    }

    template<typename T, typename std::enable_if<std::is_integral<T>::value>::type * = nullptr>
    constexpr SequenceNumber_t operator-(const T i) const {
      // TODO Overflow
      return SequenceNumber_t({this->high, this->low - static_cast<uint32_t>(i)});
    }
    static const uint64_t multiplicand = static_cast<uint64_t>(1) << 32;
  };

  constexpr static SequenceNumber_t minimum_sequence_number = {INT32_MAX, UINT32_MAX};
  constexpr static SequenceNumber_t maximum_sequence_number = {INT32_MIN, 0};

  struct SequenceNumberCompare {
    constexpr bool operator()(const SequenceNumber_t & a, const SequenceNumber_t & b) {
      return SequenceNumber_t::less_than(a, b);
    }
  };

  // 16-byte (128 bit) GUID
  struct GUID_t {
    // TODO Prefix always sets the first 2 bytes to vendor ID
    // prefix: Uniquely identifies the Participant within the Domain.
    // entity_id: Uniquely identifies the Entity within the Participant.
    BOOST_HANA_DEFINE_STRUCT(GUID_t,
      (GuidPrefix_t, prefix),
      (EntityId_t, entity_id)
    );

    bool operator==(const GUID_t & b) const {
      for (size_t i = 0; i < entity_id.size(); ++i) {
        if (entity_id[i] != b.entity_id[i]) {
          return false;
        }
      }
      for (size_t i = 0; i < prefix.size(); ++i) {
        if (prefix[i] != b.prefix[i]) {
          return false;
        }
      }
      return true;
    }
  };

  // TODO: collisions with endpoints outside of participant
  struct GUIDCompare {
    constexpr bool operator()(const GUID_t & a, const GUID_t & b) const {
      bool less = true;
      for (size_t i = 0; i < 4; ++i) {
        if (a.entity_id[i] > b.entity_id[i]) {
          return false;
        }
      }
      for (size_t i = 0; i < 12; ++i) {
        if (a.prefix[i] > b.prefix[i]) {
          return false;
        }
      }

      return true;
    }
  };

  struct Entity {
    // Globally and uniquely identifies the
    // RTPS Entity within the DDS domain.
    GUID_t guid;
  };

  struct Locator_t {
    BOOST_HANA_DEFINE_STRUCT(Locator_t,
    (int32_t, kind),
    (uint32_t, port),
    (IPAddress, address));
  };

  struct ProtocolVersion_t {
    BOOST_HANA_DEFINE_STRUCT(ProtocolVersion_t,
    (Octet, major),
    (Octet, minor));
  };
  // lower protocol versions are not supported
  static const ProtocolVersion_t rtps_protocol_version = {2, 2};

  struct Participant : Entity {
    // a participant "contains" Endpoints
    // Lists of endpoints.
    List<Locator_t> default_unicast_locator_list;
    List<Locator_t> default_multicast_locator_list;
    uint32_t participant_id;
    //static constexpr ProtocolVersion_t protocol_version = rtps_protocol_version;
    //static constexpr VendorId_t vendor_id = cmbml_vendor_id;
  };

  template<ReliabilityKind_t reliabilityLevel, TopicKind_t topicKind>
  struct EndpointParams {
    static const ReliabilityKind_t reliability_level = reliabilityLevel;
    static const TopicKind_t topic_kind = topicKind;
  };

  // TODO Template error checking
  // On initialization, initialize Transport-specific data with this Endpoint.
  template<typename EndpointParams>
  struct Endpoint : Entity {
    List<Locator_t> unicast_locator_list;
    List<Locator_t> multicast_locator_list;
    static const ReliabilityKind_t reliability_level = EndpointParams::reliability_level;
    static const TopicKind_t topic_kind = EndpointParams::topic_kind;
  };

}

#endif  // CMBML__TYPES__HPP_


#define BOOST_TEST_MODULE TestGCellGrid
#include <boost/test/included/unit_test.hpp>
#include "db.h"
#include "helper.cpp"

using namespace odb;
using namespace std;

BOOST_AUTO_TEST_SUITE( test_suite )

BOOST_AUTO_TEST_CASE( test_default )
{
    dbDatabase*  db;
    db        = createSimpleDB();
    auto block     = db->getChip()->getBlock();
    auto l1 = db->getTech()->findLayer("L1");
    dbGCellGrid* grid = dbGCellGrid::create(block);
    grid->addGridPatternX(0, 20, 10);// 0 10 20 30 40 50 ... 190
    grid->addGridPatternY(0, 20, 10);// 0 10 20 30 40 50 ... 190

    BOOST_TEST(grid->getXIdx(-1) == 0);
    BOOST_TEST(grid->getXIdx(0) == 0);
    BOOST_TEST(grid->getXIdx(5) == 0);
    BOOST_TEST(grid->getXIdx(15) == 1);
    BOOST_TEST(grid->getXIdx(210) == 19);
    BOOST_TEST(grid->getHorizontalCapacity(l1, 0, 0) == 0);
    grid->setHorizontalCapacity(l1, 0, 0, 20);
    BOOST_TEST(grid->getHorizontalCapacity(l1, 0, 0) == 20);
    grid->addGridPatternX(30, 20, 10);
    BOOST_TEST(grid->getHorizontalCapacity(l1, 0, 0) == 0);

}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE "Features"
#include <boost/test/unit_test.hpp>
#include <seq2map/sequence.hpp>

using namespace seq2map;

BOOST_AUTO_TEST_CASE(matching)
{
    Sequence seq;
    ImageFeatureSet f0, f1;

    if (!seq.Restore("KITTI_ODOMETRY_00"))
    {
        BOOST_FAIL("error reading test sequence");
    }

    if (!seq.GetFeatureStore(0))
    {
        BOOST_FAIL("error getting feature store 0");
    }

    if (!seq.GetFeatureStore(0)->Retrieve(0, f0) ||
        !seq.GetFeatureStore(0)->Retrieve(1, f1))
    {
        BOOST_FAIL("error retrieving feature set");
    }

    FeatureMatcher matcher(true, false, false, 0.8f, true);
    const ImageStore& im = seq.GetFeatureStore(0)->GetCamera()->GetImageStore();

    E_INFO << "feature set 0: " << f0.GetSize() << " feature(s)";
    E_INFO << "feature set 1: " << f1.GetSize() << " feature(s)";

    matcher.SetUniqueness(false);
    ImageFeatureMap map0 = matcher(f0, f1);
    //cv::imshow("DMatching", map0.Draw(im[0].im, im[1].im));
    E_INFO << "without uniqueness and symmetric checks, we got " << map0.Select(FeatureMatch::INLIER).size() << " match(es)";

    matcher.SetUniqueness(true);
    ImageFeatureMap map1 = matcher(f0, f1);
    //cv::imshow("DMatching + Uniqueness", map1.Draw(im[0].im, im[1].im));
    E_INFO << "by enforcing uniqueness check, we got " << map1.Select(FeatureMatch::INLIER).size() << " match(es)";

    matcher.SetSymmetric(true);
    ImageFeatureMap map2 = matcher(f0, f1);
    //cv::imshow("DMatching + Uniqueness + Symmetry", map2.Draw(im[0].im, im[1].im));
    E_INFO << "by enforcing uniqueness and symmetric checks, we got " << map2.Select(FeatureMatch::INLIER).size() << " match(es)";

    //cv::waitKey(0);
}


#include <stan/math/rev/core.hpp>
#include <stan/math/prim.hpp>
#include <boost/typeof/typeof.hpp>
#include <gtest/gtest.h>
#include <type_traits>
#include <vector>

using Eigen::Matrix;
using stan::math::promote_elements;
using stan::math::var;
using std::vector;

TEST(MathFunctionsScalPromote_Elements, double2var) {
  double from;
  promote_elements<var, double> p;
  typedef BOOST_TYPEOF(p.promote(from)) result_t;
  bool same = std::is_same<var, result_t>::value;
  EXPECT_TRUE(same);
}

TEST(MathFunctionsArrPromote_Elements, doubleVec2varVec) {
  vector<double> from;
  from.push_back(1);
  from.push_back(2);
  from.push_back(3);
  promote_elements<vector<var>, vector<double> > p;
  typedef BOOST_TYPEOF(p.promote(from)) result_t;
  bool same = std::is_same<vector<var>, result_t>::value;
  EXPECT_TRUE(same);
}

TEST(MathFunctionsMatPromote_Elements, doubleMat2varMat) {
  stan::math::matrix_d m1(2, 3);
  m1 << 1, 2, 3, 4, 5, 6;
  promote_elements<Matrix<var, 2, 3>, Matrix<double, 2, 3> > p;
  typedef BOOST_TYPEOF(p.promote(m1)) result_t;
  bool same = std::is_same<Matrix<var, 2, 3>, result_t>::value;
  EXPECT_TRUE(same);
}


//
// test-generator.cc
//
//  Created by Peter Gusev on 19 March 2016.
//  Copyright 2013-2016 Regents of the University of California
//

#include <stdlib.h>
#include <boost/asio.hpp>
#include <boost/thread.hpp>

#include "gtest/gtest.h"
#include "tests-helpers.hpp"
#include "precise-generator.hpp"

using namespace std;
using namespace ::testing;
using namespace boost::chrono;

class GeneratorTest : public ::testing::Test {
 protected:
  virtual void SetUp() {}
  virtual void TearDown() {}

  void testRate(double rate, unsigned int durationSec,
  	const PreciseGenerator::Task& task)
  {
  	boost::asio::io_service io;
  	unsigned int count = 0;

  	PreciseGenerator g(io, rate, task);

  	boost::shared_ptr<boost::asio::io_service::work> work;
  	boost::asio::deadline_timer runTimer(io);
  	high_resolution_clock::time_point t1 = high_resolution_clock::now();
  	unsigned int d = durationSec;

  	runTimer.expires_from_now(boost::posix_time::milliseconds(d*1000+10));
  	runTimer.async_wait([&](const boost::system::error_code& ){
  		work.reset();
  		g.stop();
  	});

  	g.start();

  	work.reset(new boost::asio::io_service::work(io));
  	io.run();
  	io.stop();

  	// cout << "mean processing overhead " << fixed << setprecision(2) << g.getMeanProcessingOverheadNs() << " ns" << endl;
  	// cout << "mean task time " << fixed << setprecision(2) << g.getMeanTaskTimeNs() << " ns" << endl;

  	high_resolution_clock::time_point t2 = high_resolution_clock::now();
  	actualDuration_ = duration_cast<seconds>( t2 - t1 ).count();
  	actualTaskDurationNs_ = g.getMeanTaskTimeNs();
  }

  long long actualDuration_;
  double actualTaskDurationNs_;
};

TEST_F(GeneratorTest, Test30FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 30.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test29FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 29.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test28_5FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 28.5;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_GE(count, floor((unsigned int)(actualDuration_)*rate));
	EXPECT_LE(count, ceil((unsigned int)(actualDuration_)*rate));
}

TEST_F(GeneratorTest, Test28FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 28.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test27FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 27.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test25FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 29.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test24_9FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 24.9;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_GE(count, floor((unsigned int)(actualDuration_)*rate));
	EXPECT_LE(count, ceil((unsigned int)(actualDuration_)*rate));
}

TEST_F(GeneratorTest, Test15FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 15.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test10FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 10.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test7FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 7.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test2FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 2.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test1FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 1.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test0FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 1.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test60FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 60.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test300FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 1.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	EXPECT_EQ((unsigned int)(actualDuration_)*rate, count);
}

TEST_F(GeneratorTest, Test500FPS)
{
	unsigned int count = 0;
	unsigned int d = 1;
	double rate = 500.;

	testRate(rate, d, [&count](){
		count++;
		for (int i = 0; i < 1000; ++i); // burn some time
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	// there is 1% positive error when rates are too high
	int error = int(0.01*(d*rate));
	EXPECT_GE(count, floor((unsigned int)(actualDuration_)*rate));
	EXPECT_LE(count, ceil((unsigned int)(actualDuration_)*rate)+error);
}

TEST_F(GeneratorTest, TestLongTask)
{
	unsigned int count = 0;
	unsigned int d = 3;
	double rate = 30.;
	unsigned int taskDurationMs = 40;

	testRate(rate, d, [&count, taskDurationMs](){
		count++;
		boost::this_thread::sleep_for(boost::chrono::milliseconds(taskDurationMs));
	});

	EXPECT_EQ((unsigned int)actualDuration_, d);
	double actualRate = 1000000000./(double)actualTaskDurationNs_;
	int error = int(0.01*(d*actualRate));
	GT_PRINTF("actual count %d (+-%d). actualRate %.4f (vs %,4f requested). task mean time %.4f ms\n",
		count, error, actualRate, rate, actualTaskDurationNs_/1000000.);
	EXPECT_GE(count, floor((unsigned int)(actualDuration_)*actualRate-error));
	EXPECT_LE(count, ceil((unsigned int)(actualDuration_)*actualRate+error));
}

//******************************************************************************
int main(int argc, char **argv) {
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}


/**
 * TODO: it's necessary to build the genomes analysis in the following way
 * The pool must be splited in the test pool and the training pool, it is, the test pool
 * contains the best quoted gens and the test pool contains the gens which we want to achive
 * a better performance.
 * Once a generation overs and some gens achieved a performance better than the ones in the
 * testing pool they must update the testing pool with the new gens. The ones which achieve
 * a better performance are going to substitute the ones with the worst performance. For allow
 * a better performance achievement and less enviesation, we must add to the training pool more
 * gens than the ones which are going to be tested randomly, something like twice the number of
 * gens in every generation, like that we promote a bigger variance of the genome itself and
 * there must exists a game against a stupid gen to verify if the system stopped learning how
 * to win against a stupid player.
 *
 *
 * Problems with the training technique: There may exists an enviesation of the way that the
 * system plays once it cannot play against gens outside of the testing pool which may learn
 * to play in a really specific way.
 */
#ifndef __GENETIC_POOL
#define __GENETIC_POOL

// System includes
#include <iostream>
#include <math.h>
#include <time.h>
#include <cstdlib>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/lambda/bind.hpp>

// OpenNN includes
#include <opennn.h>

#define INF 0x3f3f3f3f

using namespace OpenNN;

typedef struct Gene {
	long id;
	double fitness;
	Vector<double> gen;
} Gene;

class geneticPool {
private:
	double mutationRate; // The mutation rate of the gens
	double crossoverRate; // The amount of every gen in the crossover process, 50% means equal distribution
	std::vector<Gene> train_pool; // The group of gens with the best fitness of all generations
	std::vector<Gene> test_pool; // The group of gens that are being tested in the generation
	Vector<int> structure; // The neural network topology

public:

	/**
	 * @brief      A pool for iterative learning with neural networks.
	 */
	geneticPool();

	/**
	 * @brief      A simple destructor instanciation.
	 */
	~geneticPool();

	/**
	 * @brief      Creates a mutation of a certain gen.
	 *
	 * @param[in]  gen   The gen to be mutated
	 *
	 * @return     The mutated version of the gen.
	 */
	Vector<double> mutate(Vector<double> gen);

	/**
	 * @brief      A procedure to make the crossover of two gens, putting parts of the gen B in gen A.
	 *
	 * @param[in]  a     The first gen.
	 * @param[in]  b     The second gen.
	 *
	 * @return     The crossover version of the gens.
	 */
	Vector<double> crossover(Vector<double> a, Vector<double> b);

	/**
	 * @brief      Gets a random gen in the test pool.
	 *
	 * @return     The gen.
	 */
	Gene get_testgen();

	/**
	 * @brief      Gets a specific gen in the test pool.
	 *
	 * @param[in]  num   The index of the gen.
	 *
	 * @return     The gen.
	 */
	Gene get_testgen(int num);

	/**
	 * @brief      Gets a specific gen in the train pool.
	 *
	 * @param[in]  num   The index of the gen.
	 *
	 * @return     The gen.
	 */
	Gene get_traingen(int num);

	/**
	 * @brief      Gets the train group.
	 *
	 * @return     The train group.
	 */
	std::vector<Gene> get_traingroup();

	/**
	 * @brief      Gets the test group.
	 *
	 * @return     The test group.
	 */
	std::vector<Gene> get_testgroup();

	/**
	 * @brief      Adds a gen to the train pool.
	 *
	 * @param[in]  gen   The params of the gen.
	 */
	void add_traingen(Vector<double>gen);

	/**
	 * @brief      Adds a gen to the test pool.
	 *
	 * @param[in]  gen   The params of the gen.
	 */
	void add_testgen(Vector<double>gen);

	/**
	 * @brief      Sets the train pool from params.
	 *
	 * @param[in]  genlist  The gen list.
	 */
	void set_traingen(std::vector< Vector<double> > genlist);

	/**
	 * @brief      Sets the test pool from params.
	 *
	 * @param[in]  genlist  The gen list.
	 */
	void set_testgen(std::vector< Vector<double> > genlist);

	/**
	 * @brief      Sets the train pool.
	 *
	 * @param[in]  genlist  The gen list.
	 */
	void set_traingen(std::vector<Gene> genlist);

	/**
	 * @brief      Sets the test pool.
	 *
	 * @param[in]  genlist  The gen list.
	 */
	void set_testgen(std::vector<Gene> genlist);

	/**
	 * @brief      Sets the fitness of a gen in the train pool.
	 *
	 * @param[in]  id       The identifier
	 * @param[in]  fitness  The fitness
	 */
	void set_trainfitness(int id, double fitness);

	/**
	 * @brief      Sets the fitness of a gen in the test pool.
	 *
	 * @param[in]  id       The identifier
	 * @param[in]  fitness  The fitness
	 */
	void set_testfitness(int id, double fitness);

	/**
	 * @brief      Build the next generation of genes.
	 */
	void next_gen();

	/**
	 * @brief      Loads all gens from a folder.
	 *
	 * @param[in]  folder         The folder
	 * @param[in]  network_model  The network model
	 */
	void load_folder(std::string folder, Vector<int> network_model);

	/**
	 * @brief      Saves all gens in a folder.
	 *
	 * @param[in]  folder         The folder
	 * @param[in]  network_model  The network model
	 */
	void save_folder(std::string folder, Vector<int> network_model);

	/**
	 * @brief      Loads all gens from a folder.
	 *
	 * @param[in]  folder         The folder
	 * @param[in]  network_model  The network model
	 */
	void load_folder(const char* folder, Vector<int> network_model);

	/**
	 * @brief      Saves all gens in a folder.
	 *
	 * @param[in]  folder         The folder
	 * @param[in]  network_model  The network model
	 */
	void save_folder(const char* folder, Vector<int> network_model);

	/**
	 * @brief      Loads all gens from a folder.
	 *
	 * @param[in]  folder         The folder
	 * @param[in]  network_model  The network model
	 */
	void load_folder(char* folder, Vector<int> network_model);

	/**
	 * @brief      Saves all gens in a folder.
	 *
	 * @param[in]  folder         The folder
	 * @param[in]  network_model  The network model
	 */
	void save_folder(char* folder, Vector<int> network_model);

	/**
	 * @brief      Sets the mutation rate.
	 *
	 * @param[in]  rate  The rate.
	 */
	void set_mutationrate(double rate);

	/**
	 * @brief      Sets the crossover rate.
	 *
	 * @param[in]  rate  The rate.
	 */
	void set_crossoverrate(double rate);

	/**
	 * @brief      Build a list of random gens.
	 *
	 * @param[in]  gen_num        The generate number
	 * @param[in]  network_model  The network model
	 *
	 * @return     The list of gens.
	 */
	std::vector<Gene> random_gens(int gen_num, Vector<int> network_model);

	// helper functions ---------------------------------------//

	/**
	 * @brief      Helper for sort the gens from the Gene Structure.
	 *
	 * @param[in]  a     The first gen.
	 * @param[in]  b     The second gen.
	 *
	 * @return     True if a > b and false otherwise.
	 */
	static bool sortbyfitness(const Gene &a, const Gene &b);

	/**
	 * @brief      Check if a certain folder exists.
	 *
	 * @param[in]  pathname  The pathname.
	 *
	 * @return     True if the folder exists and false otherwise.
	 */
	static bool exist_dir(std::string pathname);

	/**
	 * @brief      Creates a dir.
	 *
	 * @param[in]  pathname  The pathname
	 */
	static void create_dir(std::string pathname);

	/**
	 * @brief      Counts the number of files in a folder.
	 *
	 * @param[in]  pathname  The folder.
	 *
	 * @return     Number of files.
	 */
	static int count_files(std::string pathname);
};

#endif


#include "resmgr_app.hpp"
#include "sprite_scene.hpp"
#include "bsprite.hpp"
#include "../../camera2d.hpp"
#include "../../effect/if.hpp"
#include "../../effect/uniform/matrix2d.hpp"
#include "../../gl/resource.hpp"
#include "../../gl/texture/filter.hpp"
#include "../../systeminfo.hpp"
#include "../../tls_data.hpp"
#include "frea/src/random/vector.hpp"
#include "lubee/src/meta/countof.hpp"
#include <boost/format.hpp>

namespace rev::test {
	struct SpriteScene::St_Default : StateT<St_Default, St_Base> {
		void onEnter(SpriteScene& self, const ObjTypeId_OP id) override {
			St_Base::onEnter(self, id);

			self._camera = Camera2D::NewS();
			const auto scr = mgr_info.getScreenSize();
			self._camera->setAspectRatio(scr.width / scr.height);
			self._camera->refPose().identity();

			auto& mt = tls_mt.get();
			auto rfi = mt.getUniformF<int>();
			auto rf = mt.getUniformF<float>();
			HTex tex[6];
			auto filter = mgr_gl.createTexFilter();
			filter->setFilter(true, true);
			for(uint32_t i=0 ; i<countof(tex) ; i++) {
				auto src = mgr_gl.loadTexture(UserURI((boost::format("spr%1%.png") % i).str()), MipState::MipmapLinear);
				tex[i] = mgr_gl.attachTexFilter(src, filter);
			}
			using frea::random::GenVecUnit;
			using frea::random::GenVec;
			auto dg = self.getDrawGroup();
			auto& ug = self.getUpdGroupRef();
			for(std::size_t i=0 ; i<NSprite ; i++) {
				const auto vDir = GenVecUnit<Vec2>(rf);
				const auto vPos = GenVec<Vec2>(rf, {-1.f, 1.f});
				auto obj = std::make_shared<BoundingSprite>(dg, tex[rfi({0,5})], vPos, vDir*0.01f);
				obj->setScaling(Vec2{0.2,0.2});
				ug.addObj(obj);
				self._sprite[i] = obj;
			}
		}
		void onExit(SpriteScene& self, const ObjTypeId_OP id) override {
			for(std::size_t i=0 ; i<NSprite ; i++) {
				self._sprite[i]->destroy();
			}
			St_Base::onExit(self, id);
		}
		void onDraw(const SpriteScene& self, IEffect& e) const override {
			St_Base::onDraw(self, e);

			auto& e2 = dynamic_cast<U_Matrix2D&>(e);
			e2.setCamera(self._camera);
		}
	};

	SpriteScene::SpriteScene() {
		setStateNew<St_Default>();
		_makeGui();
	}
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 6, 1> test_double_6_1(const Eigen::Matrix<double, 6, 1>& M) { return M; }
void export_double_6_1() { boost::python::def("test_double_6_1", test_double_6_1); }


/**
* Copyright (c) 2015, BMW Car IT GmbH
* Author: Michael Mühlbauer-Prassek
*
* This software is licensed under BSD 3-clause License
* (see http://spdx.org/licenses/BSD-3-Clause).
**/

#include <testEval.h>
#include <boost/filesystem/operations.hpp>
#include <boost/algorithm/string.hpp>
#include <math.h>
#include <testUtil.h>
#include <fstream>
#include <ios>
#include <iomanip>

testEval::testEval(testResultType* testResults, testSetup& TS) {
  Results = testResults;

  TS.initEvaluation(reportFilePath, referFilePath, resultFilePath, minConf, maxDeviation, resultHeader);

  testEval::getReference();
}

void testEval::writeReportFile() {
  testUtil::writeInfo("Invoke writeReportFile");

  int frame_count = 0;
    std::fstream fs_result;
    fs_result.open(reportFilePath.c_str(), std::ios::out);

  fs_result << testUtil::timeStamp() << std::endl;

  for (std::vector<frameResultType>::iterator it = Results->begin(); it != Results->end(); ++it) {
    fs_result << "-- Frame : " << frame_count++ << std::endl;

    fs_result << "---- Time : " <<  testEval::calcTime(it->first) << std::endl;

    int cluster_count = 0;

    for (std::vector<personClusterType>::iterator c_it = it->second->begin(); c_it != it->second->end(); ++c_it) {
      if (c_it->getPersonConfidence() > minConf) {
        fs_result << "---- Cluster : " << cluster_count++  <<std::endl;
        fs_result << "------ B_X   : " <<c_it->getBottom()[0] << std::endl;
        fs_result << "------ B_Y   : " <<c_it->getBottom()[1] << std::endl;
        fs_result << "------ B_Z   : " <<c_it->getBottom()[2] << std::endl;
        fs_result << "------ HOG_C : " <<c_it->getPersonConfidence() << std::endl;
      }
    }
  }

  fs_result.close();

  testUtil::writeInfo("writeReportFile finished");
}

void testEval::getReference() {
  const std::string frameCount_S = "Frame_Count";
  const std::string frameNumber_S = "Frame_Number";

  int frameCount = 0;
  int frameNumber = 0;

  std::fstream fs_refer;
  fs_refer.open(referFilePath.c_str(), std::ios::in);

  std::string refer_line;

  while (!fs_refer.eof()) {
    getline(fs_refer, refer_line);

    if (refer_line.find(frameCount_S) != std::string::npos) {
      refer_line.replace(refer_line.find(frameCount_S), frameCount_S.length(), "");

      boost::trim(refer_line);

      std::istringstream iss(refer_line);

      iss >> frameCount;

      testReference = new referResultType(frameCount);
    } else if (refer_line.find(frameNumber_S) != std::string::npos) {
      refer_line.replace(refer_line.find(frameNumber_S), frameNumber_S.length(), "");

      boost::trim(refer_line);

      std::istringstream iss(refer_line);

      iss >> frameNumber;

    } else if (!refer_line.empty()) {
      std::istringstream iss(refer_line);

      referEntryType entry;

      iss >> entry.first.first;
      iss >> entry.first.second;

      entry.second = false;

      testReference->at(frameNumber - 1).push_back(entry);
    }
  }

  fs_refer.close();
}

float testEval::calcDistance(float X_1, float Z_1, float X_2 , float Z_2) {
  return sqrt(pow((X_2 -X_1), 2) + pow((Z_2 -Z_1), 2));
}

bool testEval::evalMatch(float distance) {
  return (distance <= maxDeviation);
}

float testEval::calcTime(timespec time) {
  float exec_time;

  if (time.tv_nsec > 0) {
    exec_time = float(time.tv_nsec) / 1000000000 + float(time.tv_sec);
  } else {
    exec_time = float(time.tv_nsec + 1000000000) / 1000000000 + float(time.tv_sec -1);
  }

  return exec_time;
}

void testEval::calcTimeCharac(float& meanET, float& stdDevET, float& minET, float& maxET) {
  meanET = 0.0;
  stdDevET = 0.0;
  minET = 100.0;
  maxET = 0.0;

  for (std::vector<frameCompareType>::iterator it = compareResult->begin(); it != compareResult->end(); ++it) {
    meanET += it->execTime;

    if (it->execTime > maxET) {
      maxET = it->execTime;
    }

    if (it->execTime < minET) {
      minET = it->execTime;
    }
  }

  meanET = meanET/compareResult->size();

  for (std::vector<frameCompareType>::iterator it = compareResult->begin(); it != compareResult->end(); ++it) {
    stdDevET += pow((it->execTime - meanET), 2);
  }
  stdDevET = sqrt((stdDevET)/compareResult->size());
}

void testEval::writeResultFile() {
  testUtil::writeInfo("Invoke writeResultFile");

  std::fstream fs_result;
  fs_result.open(resultFilePath.c_str(), std::ios::out);

  fs_result << "# " << testUtil::timeStamp();

  fs_result << resultHeader.str();

  float minTime;
  float maxTime;
  float meanTime;
  float stdDevT;

  testEval::calcTimeCharac(meanTime, stdDevT, minTime, maxTime);

  fs_result << "# -- Characteristics --" << std::endl;
  fs_result << "# meanTime : " << meanTime << std::endl;
  fs_result << "# stdDevT  : " << stdDevT << std::endl;
  fs_result << "# minTime  : " << minTime << std::endl;
  fs_result << "# maxTime  : " << maxTime << std::endl;

  fs_result << "# ---- Detection  ----" << std::endl;
  fs_result << "# detRate : " << std::setprecision(3) << detRate << std::endl;
  fs_result << "# maxFaSe : " << maxFaultSeq << std::endl;
  fs_result << "# meanHOG : " << meanHOG << std::endl;
  fs_result << "# ---------------------" << std::endl;

    fs_result << std::setw(6) << "#FRAME";
    fs_result << std::setw(11) << "EXE_TIME";
    fs_result << std::setw(5) << "FP";
    fs_result << std::setw(5) << "FN";
    fs_result << std::setw(5) << "MA";
    fs_result << std::setw(9) << "MA_DEV" << std::endl;

  for (std::vector<frameCompareType>::iterator it = compareResult->begin(); it != compareResult->end(); ++it) {
        fs_result << std::setw(6) << it->frameNumber;
        fs_result << std::setw(11) << std::setprecision(5) << it->execTime;
        fs_result << std::setw(5) << it->falsePos;
        fs_result << std::setw(5) << it->falseNeg;
        fs_result << std::setw(5) << it->match;
        fs_result << std::setw(9) << std::setprecision(2) << it->matchDev << std::endl;
  }

  fs_result.close();

  testUtil::writeInfo("writeResultFile finished");
}

void testEval::performEvaluation() {
  testUtil::writeInfo("Invoke performEvaluation");

  compareResult = new compareResultType(testReference->size());

  detRate = 0;
  maxFaultSeq = 0;
  meanHOG = 0;
  int tempFaultSeq = 0;

  for (int frame = 0; frame < testReference->size(); frame++) {
    int match_count = 0;
    float match_dev = 0;
    int detection_count = 0;
    float tempMeanHOG = 0;
    frameCompareType frameResult;

    for (std::vector<personClusterType>::iterator res_it = Results->at(frame).second->begin(); res_it != Results->at(frame).second->end(); ++res_it) {
      if (res_it->getPersonConfidence() >= minConf) {
        detection_count++;

        for (std::vector<referEntryType>::iterator ref_it = testReference->at(frame).begin(); ref_it != testReference->at(frame).end(); ++ref_it) {
          if (!(ref_it->second)) {  // no match yet{
            float deviation = testEval::calcDistance(ref_it->first.first, ref_it->first.second, res_it->getBottom()[0], res_it->getBottom()[2]);

            if (testEval::evalMatch(deviation)) {
              ref_it->second = true;
              match_count++;
              match_dev += deviation;
              tempMeanHOG += res_it->getPersonConfidence();
              break;
            }
          }
        }
      }
    }

    frameResult.frameNumber = frame;
    frameResult.execTime = testEval::calcTime(Results->at(frame).first);
    // falsePos = clusters per frame (HOG_conf > minConf) minus matches per frame
    frameResult.falsePos = detection_count - match_count;
    // falseNeg = real objects per frame minus matches per frame
    frameResult.falseNeg = testReference->at(frame).size() - match_count;
    frameResult.match = match_count;
    // matchDev = average deviation of all matches per frame
    if (match_count > 0) {
      frameResult.matchDev = (match_dev / match_count) / maxDeviation;
    } else {
      frameResult.matchDev = -1.0;
    }

    if (match_count == testReference->at(frame).size()) {
      detRate++;

      meanHOG += tempMeanHOG/match_count;

     if (tempFaultSeq > maxFaultSeq) {
       maxFaultSeq = tempFaultSeq;
     }

      tempFaultSeq = 0;

    } else {
      tempFaultSeq++;
    }

    compareResult->at(frame) = frameResult;
  }

  if (tempFaultSeq > maxFaultSeq) {
    maxFaultSeq = tempFaultSeq;
  }

  meanHOG = meanHOG/detRate;
  detRate = detRate/testReference->size();

  testUtil::writeInfo("performEvaluation finished");

  testEval::writeResultFile();
  testEval::writeReportFile();
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Ilias Khairullin <ilias@nil.foundation>
//
// 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
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE poseidon_test

#include <iostream>
#include <string>
#include <vector>
#include <cstdint>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

// #include <nil/crypto3/hash/algorithm/hash.hpp>
// #include <nil/crypto3/hash/adaptor/hashed.hpp>

#include <nil/crypto3/hash/poseidon.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::algebra;
// using namespace nil::crypto3::accumulators;

using poseidon_default_field_t = fields::bls12_fr<381>;

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp<FieldParams> &e) {
    os << e.data << std::endl;
}

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<typename FieldParams>
            struct print_log_value<typename fields::detail::element_fp<FieldParams>> {
                void operator()(std::ostream &os, typename fields::detail::element_fp<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

// if target == check-hash just data/curves.json
const char *test_data = "../../../../libs/hash/test/data/poseidon.json";

boost::property_tree::ptree string_data(const std::string &interface_type, const std::string &strength,
                                        const std::string &arity) {
    boost::property_tree::ptree string_data;
    boost::property_tree::read_json(test_data, string_data);

    return string_data.get_child(interface_type).get_child(strength).get_child(arity);
}

template<typename poseidon_functions_t, typename TestSet>
void hash_test_data(const TestSet &test_set) {
    using element_type = typename poseidon_functions_t::element_type;
    using modulus_type = typename poseidon_functions_t::element_type::modulus_type;

    typename poseidon_functions_t::state_type input;
    typename poseidon_functions_t::state_type input_optimized;

    auto i = 0;
    for (auto &input_value : test_set.second) {
        input[i] = element_type(modulus_type(input_value.second.data()));
        input_optimized[i] = input[i];
        i++;
    }

    auto result_hash = element_type(modulus_type(test_set.first.data()));

    poseidon_functions_t::permute(input);
    poseidon_functions_t::permute_optimized(input_optimized);

    BOOST_CHECK_EQUAL(input[1], result_hash);
    BOOST_CHECK_EQUAL(input_optimized[1], result_hash);
}

BOOST_AUTO_TEST_SUITE(poseidon_manual_tests)

BOOST_DATA_TEST_CASE(poseidon_strengthen_1, string_data("internal", "strengthen", "1"), data_set) {
    using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 1, 69>;

    hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_strengthen_2, string_data("internal", "strengthen", "2"), data_set) {
    using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 2, 69>;

    hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_strengthen_4, string_data("internal", "strengthen", "4"), data_set) {
    using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 4, 70>;

    hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_strengthen_8, string_data("internal", "strengthen", "8"), data_set) {
    using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 8, 72>;

    hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_strengthen_11, string_data("internal", "strengthen", "11"), data_set) {
   using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 11, 72>;

   hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_strengthen_16, string_data("internal", "strengthen", "16"), data_set) {
   using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 16, 74>;

   hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_strengthen_24, string_data("internal", "strengthen", "24"), data_set) {
   using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 24, 74>;

   hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_strengthen_36, string_data("internal", "strengthen", "36"), data_set) {
   using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 36, 75>;

   hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_standart_1, string_data("internal", "standart", "1"), data_set) {
    using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 1, 55>;

    hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_standart_2, string_data("internal", "standart", "2"), data_set) {
    using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 2, 55>;

    hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_standart_4, string_data("internal", "standart", "4"), data_set) {
    using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 4, 56>;

    hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_standart_8, string_data("internal", "standart", "8"), data_set) {
    using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 8, 57>;

    hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_standart_11, string_data("internal", "standart", "11"), data_set) {
   using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 11, 57>;

   hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_standart_16, string_data("internal", "standart", "16"), data_set) {
   using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 16, 59>;

   hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_standart_24, string_data("internal", "standart", "24"), data_set) {
   using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 24, 59>;

   hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_DATA_TEST_CASE(poseidon_standart_36, string_data("internal", "standart", "36"), data_set) {
   using poseidon_functions_t = hashes::detail::poseidon_functions<poseidon_default_field_t, 36, 60>;

   hash_test_data<poseidon_functions_t>(data_set);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef CRADLE_IMAGING_GRADIENT_HPP
#define CRADLE_IMAGING_GRADIENT_HPP

#include <cradle/math/interpolate.hpp>
#include <boost/range/functions.hpp>

namespace cradle {

template<class Color>
void compute_gradient(std::vector<Color>& gradient,
    std::vector<Color> const& colors, std::vector<double> const& positions)
{
    assert(positions.size() == colors.size());
    regular_grid<1,double> grid(0, 1, unsigned(gradient.size()));
    interpolate(&gradient, grid, colors, positions);
}

template<class Color>
void compute_gradient(std::vector<Color>& gradient,
    std::vector<Color> const& colors, std::vector<double> const& positions,
    linear_function<double> const& scale)
{
    assert(positions.size() == colors.size());
    regular_grid<1,double> grid(scale.intercept, scale.slope,
        unsigned(gradient.size()));
    interpolate(&gradient, grid, colors, positions);
}

}

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2019 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
//
// 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
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_HASH_BLOCK_STREAM_PROCESSOR_HPP
#define CRYPTO3_HASH_BLOCK_STREAM_PROCESSOR_HPP

#include <array>
#include <iterator>

#include <boost/crypto3/detail/pack.hpp>

#include <boost/crypto3/hash/accumulators/bits_count.hpp>
#include <boost/crypto3/hash/accumulators/parameters/bits.hpp>

#include <boost/integer.hpp>
#include <boost/static_assert.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost {
    namespace crypto3 {
        namespace hashes {

            /*!
             * @brief This will do the usual Merkle-Damgård-style strengthening,
             * padding with a 1 bit, then 0 bits as needed, then, if requested,
             * the length.
             *
             * @tparam Hash
             * @tparam StateAccumulator
             * @tparam Params
             */
            template<typename Construction, typename StateAccumulator, typename Params>
            class block_stream_processor {
            protected:
                typedef typename Construction::type construction_type;
                typedef StateAccumulator accumulator_type;
                typedef Params params_type;

                constexpr static const std::size_t word_bits = construction_type::word_bits;

                constexpr static const std::size_t block_bits = construction_type::block_bits;
                typedef typename construction_type::block_type block_type;

            public:
                typedef typename params_type::digest_endian endian_type;

                constexpr static const std::size_t value_bits = params_type::value_bits;
                typedef typename boost::uint_t<value_bits>::least value_type;
                BOOST_STATIC_ASSERT(word_bits % value_bits == 0);
                constexpr static const std::size_t block_values = block_bits / value_bits;
                typedef std::array<value_type, block_values> cache_type;

            protected:
                BOOST_STATIC_ASSERT(block_bits % value_bits == 0);

                inline void process_block(std::size_t block_seen = block_bits) {
                    using namespace boost::crypto3::detail;
                    // Convert the input into words
                    block_type block;
                    pack_to<endian_type, value_bits, word_bits>(cache.begin(), cache.end(), block.begin());
                    // Process the block
                    acc(block, accumulators::bits = block_seen);
                }

            public:
                inline void update_one(value_type value) {
                    cache[cache_seen] = value;
                    ++cache_seen;
                    if (cache_seen == block_values) {
                        // Process the completed block
                        process_block();
                        cache_seen = 0;
                    }
                }

                template<typename InputIterator>
                inline void update_n(InputIterator p, size_t n) {
                    for (; n; --n) {
                        update_one(*p++);
                    }
                }

                template<typename InputIterator>
                inline void update_n(InputIterator first, InputIterator last) {
                    std::size_t n = std::distance(first, last);
                    update_n(first, n);
                }

                template<typename InputIterator>
                inline void operator()(InputIterator b, InputIterator e, std::random_access_iterator_tag) {
                    update_n(b, e);
                }

                template<typename InputIterator, typename Category>
                inline void operator()(InputIterator b, InputIterator e, Category) {
                    while (b != e) {
                        update_one(*b++);
                    }
                }

                template<typename InputIterator>
                inline void operator()(InputIterator b, InputIterator e) {
                    typedef typename std::iterator_traits<InputIterator>::iterator_category cat;

                    operator()(b, e, cat());
                }

                template<typename ContainerT>
                inline void operator()(const ContainerT &c) {
                    update_n(c.data(), c.size());
                }

            public:
                block_stream_processor(accumulator_type &acc) : acc(acc), cache(), cache_seen(0) {
                }

                virtual ~block_stream_processor() {
                    if (!cache.empty()) {
                        process_block(cache_seen * value_bits);
                        cache_seen = 0;
                    }
                }

            private:
                accumulator_type &acc;

                cache_type cache;
                std::size_t cache_seen;
            };
        }    // namespace hashes
    }    // namespace crypto3
}    // namespace boost

#endif