/* Copyright (c) 2012, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See LICENSE.txt or
 * http://www.opensource.org/licenses/mit-license.php */

#pragma once

#include "random.hpp"
#include "baseMeasure.hpp"
#include "hdp_base.hpp"
#include "probabilityHelpers.hpp"

#include <stddef.h>
#include <stdint.h>
#include <typeinfo>

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

using namespace std;
using namespace arma;

template <class U>
class HDP_gibbs : public HDP<U>
{
  public:
    HDP_gibbs(const BaseMeasure<U>& base, double alpha, double omega)
      : HDP<U>(base, alpha, omega)
    {
      //    cout<<"Creating "<<typeid(this).name()<<endl;
    };

    ~HDP_gibbs()
    {	};

    // method for "one shot" computation without storing data in this class
    vector<Row<uint32_t> > densityEst(const vector<Mat<U> >& x, uint32_t Nw, uint32_t K0, uint32_t T0, uint32_t It)
    {
      mNw = Nw;

      RandDisc rndDisc;
      // x is a list of numpy arrays: one array per document
      uint32_t J=x.size(); // number of documents
      vector<uint32_t> N(J,0);
      for (uint32_t j=0; j<J; ++j)
        N.at(j)=int(x[j].n_cols); // number of datapoints in each document

      uint32_t K=K0; // number of clusters/dishes in the franchise
      vector<uint32_t> T(J,0);   // number of tables in each restaurant
      vector<Col<uint32_t> > t_ji(J); // assignment of a table in restaurant j to customer i -> stores table number for each customer (per restaurant)
      vector<Col<uint32_t> > k_jt(J); // assignment of a dish in restaurant j at table t -> stores dish number for each table (per restaurant)
      RandInt rndT(0,T0);
      RandInt rndK(0,K0);
      for (uint32_t j=0; j<J; ++j)
      {
        T[j]=T0;
        t_ji[j] = rndT.draw(N[j]);
        k_jt[j] = rndK.draw(T[j]);
      }

      vector<Row<uint32_t> > z_ji(J);
      vector<uint32_t> Tprev=T;   // number of tables in each restaurant
      uint32_t Kprev=K;
      for (uint32_t tt=0; tt<It; ++tt)
      {
        cout<<"---------------- Iteration "<<tt<<" K="<<K<<" -------------------"<<endl;
        // gibbs update for the customer assignments to tables
        for (uint32_t j=0; j<J; ++j)
        {
          cout<<"@j="<<j<<"; N_j="<<N[j]<<"; T_j="<<T[j]<<endl;
          uint32_t n_j=t_ji[j].n_rows;
          for (uint32_t i=0; i<N[j]; ++i)
          {
            colvec l=zeros<colvec>(T[j]+K+1,1);
            colvec f=zeros<colvec>(T[j]+K+1,1);
            for (uint32_t t=0; t<T[j]; ++t)
            {
              uint32_t n_jt=sum(t_ji[j]==t);
              if (t_ji[j](i) == t) n_jt--;
              if (n_jt == 0){
                l[t]=math::nan();
                continue;
              }
              Mat<U> x_k_ji = getXinK(x,j,i,k_jt[j](t),k_jt,t_ji);
              //HDP hdp_x_ji = posterior(x_k_ji); // compute posterior hdp given the data
              //cout<<"x_j="<<x[j].col(i)<<endl;
              //cout<<"x_k_ji="<<x_k_ji<<endl;
              double f_k_jt = this->mH0.predictiveProb(x[j].col(i),x_k_ji);
              //cout<<"f_k_jt="<<f_k_jt<<endl;
              //logGaus(x[j].col(i), hdp_x_ji.mVtheta, hdp_x_ji.mmCov()); // marginal probability of x_ji in cluster k/dish k given all other data in that cluster
              f(t) = f_k_jt;
              l(t) = log(n_jt/(n_j + this->mAlpha)) + f_k_jt;
              //          if(x_k_ji.n_rows <3)
              //            cout<<"x_k_ji="<<x_k_ji<<"size="<<x_k_ji.n_rows<<" x "<<x_k_ji.n_cols<<"\t mu="<<hdp_x_ji.mVtheta(0)<<" \t cov="<<hdp_x_ji.mmCov()(0)<<"\t f="<<f_k_jt<<endl;
            }
            uint32_t m_=K; // number of dishes
            for (uint32_t k=0; k<K; ++k)
            {// handle cases where x_ji is seated at a new table with a existing dish
              //Col<uint32_t> m_k = zeros<Col<uint32_t> >(1,1); //number of tables serving dish k
              uint32_t m_k = 0; //number of tables serving dish k
              for (uint32_t jj=0; jj<J; ++jj)
                m_k += sum(k_jt[jj] ==k);
              if(m_k ==0){
                l[T[j]+k] = math::nan();
                continue;
              }
              Mat<U> x_k_ji = getXinK(x,j,i,k,k_jt,t_ji,tt==It-1);
              //HDP hdp_x_ji = posterior(x_k_ji); // compute posterior hdp given the data in
              double f_k =  this->mH0.predictiveProb(x[j].col(i),x_k_ji);
              //logGaus(x[j].col(i), hdp_x_ji.mVtheta, hdp_x_ji.mmCov());  // marginal probability of x_ji in cluster k/dish k given all other data in that cluster
              f(T[j]+k) = f_k;
              l(T[j]+k) = log(this->mAlpha*m_k/((n_j+this->mAlpha)*(m_ + HDP<U>::mOmega))) + f_k; // TODO: shouldnt this be mAlpha of the posterior hdp?
            }
            // handle the case where x_ji sits at a new table with a new dish
            //        cout<<"ji=:"<<j<<" " <<i<<endl;
            //        cout<<"x_ji=:"<<x[j].col(i)<<endl;
            //        cout<<"mVtheta=:"<<mVtheta<<endl;
            //        cout<<"Cov=:"<<mmCov()<<endl;
            //
            double f_knew = this->mH0.predictiveProb(x[j].col(i));
            //logGaus(x[j].col(i), mVtheta, mmCov());
            f[T[j]+K] = f_knew;
            l[T[j]+K] = log(this->mAlpha*HDP<U>::mOmega/((n_j+this->mAlpha)*(m_+HDP<U>::mOmega))) + f_knew;

            //        cout<<"l.n_elem= "<<l.n_elem<<" l="<<l.t()<<endl;
            uint32_t z_i = sampleDiscLogProb(rndDisc,l);

#ifndef NDEBUG
            cout<<endl<<"l="<<l.t()<<" |l|="<<l.n_elem<<endl;
            cout<<"T_j="<<T[j]<<"; K="<<K<<"; z_i="<<z_i<<endl;
#endif
            if (z_i < T[j])
            { // customer sits at existing table
              t_ji[j](i)=z_i; // update table information of customer i in restaurant j
#ifndef NDEBUG
              cout<<"customer sits at existing table "<<z_i<<endl;
#endif
            }else if (z_i-T[j] < K)
            { // customer sits at new table with a already existing dish
              t_ji[j](i)=T[j]; // update table information of customer i in restaurant j
              k_jt[j].resize(k_jt[j].n_elem+1);
              k_jt[j](k_jt[j].n_elem-1) = z_i-T[j]; // add a new table with the sampled dish
#ifndef NDEBUG
              cout<<"customer sits at new table with a already existing dish "<<z_i-T[j]<<" z_i="<<z_i<<" T_j="<<T[j]<<endl;
#endif
              T[j]++;
            }else if (z_i == T[j]+K)
            { // customer sits at a new table with a new dish
              t_ji[j](i)=T[j]; // update table information of customer i in restaurant j
              k_jt[j].resize(k_jt[j].n_elem+1);
              k_jt[j](k_jt[j].n_elem-1)=K; // add a new table with a new dish
              T[j]++;
              K++;
#ifndef NDEBUG
              cout<<"customer sits at a new table with a new dish"<<endl;
#endif
            }
          }
        }
        //remove unused tables
        for (uint32_t j=0; j<J; ++j) // find unused dishes
          for (int32_t t=T[j]-1; t>-1; --t)
          {
            //Col<uint32_t> contained = (t_ji[j]==t);
            //contained=sum(contained);
            uint32_t contained = sum(t_ji[j]==t);
            if (contained == 0)
            {
              t_ji[j].elem(find(t_ji[j] >= t)) -=1;
              T[j]--;
              //cout<<"shed "<<t<<endl<<k_jt[j].t();
              k_jt[j].shed_row(t);
              //cout<<k_jt[j].t()<<endl;
            }
          }

        for (uint32_t j=0; j<J; ++j)
        {
          cout<<"-- T["<<j<<"]="<<T[j]<<"; Tprev["<<j<<"]="<<Tprev[j]<<" deltaT["<<j<<"]="<<int32_t(T[j])-int32_t(Tprev[j])<<endl;
        }
        Tprev=T;

        cout<<" Gibbs update for k_jt"<<endl;
        for (uint32_t j=0; j<J; ++j)
        {
          for (uint32_t t=0; t<T[j]; ++t)
          {
            colvec l=zeros<colvec>(K+1,1);
            colvec f=zeros<colvec>(K+1,1);
            uint32_t m_ = 0; // number of tables
            for (uint32_t jj=0; jj<J; ++jj)
              m_ += T[jj];
            uvec i_jt=find(t == t_ji[j]);
            Mat<U> x_jt = zeros<Mat<U> >(x[j].n_rows,i_jt.n_elem);
            for (uint32_t i=0; i<i_jt.n_elem; ++i)
              x_jt.col(i) = x[j].col(i_jt(i)); //all datapoints which are sitting at table t
            for (uint32_t k=0; k<K; ++k)
            {
              uint32_t m_k = 0; // number of tables serving dish k
              for (uint32_t jj=0; jj<J; ++jj)
                m_k += sum(k_jt[jj] == k);
              if (m_k == 0){
                l(k) = math::nan();
                continue;
              }
              double f_k=0.0;
              for (uint32_t i=0; i<x_jt.n_cols; ++i)
              { // product over independent x_ji_t
                Mat<U> x_k_ji = getXinK(x,j,i_jt(i),k,k_jt,t_ji); // for posterior computation
                //HDP hdp_x_ji = posterior(x_k_ji); // compute posterior hdp given the data in
                f_k += this->mH0.predictiveProb(x_jt.col(i),x_k_ji);
                //logGaus(x_jt.col(i), hdp_x_ji.mVtheta, hdp_x_ji.mmCov());
              }
              f(k)=f_k;
              l(k)=log(m_k/(m_ + HDP<U>::mOmega)) + f_k;
            }
            double f_knew=0.0;
            for (uint32_t i=0; i<x_jt.n_cols; ++i) // product over independent x_ji_t
              f_knew += this->mH0.predictiveProb(x_jt.col(i));
            //logGaus(x_jt.col(i), mVtheta, mmCov());
            f(K)=f_knew;
            l(K)=log(HDP<U>::mOmega/(m_ + HDP<U>::mOmega)) + f_knew; // update dish at table t in restaurant j
            uint32_t z_jt = sampleDiscLogProb(rndDisc, l);
#ifndef NDEBUG
            cout<<endl<<"l="<<l.t()<<" |l|="<<l.n_elem<<endl;
            cout<<"T_j="<<T[j]<<"; K="<<K<<"; z_jt="<<z_jt<<endl;
#endif
            if (z_jt < K){
              k_jt[j](t)=z_jt;
#ifndef NDEBUG
              cout<<"Table "<<t<<" gets already existing meal "<<z_jt<<endl;
#endif
            }else if ( z_jt == K){
              k_jt[j](t)=K;
              K++;
#ifndef NDEBUG
              cout<<"Table "<<t<<" gets new meal "<<K-1<<endl;
#endif
            }
          }
        }

        //    for (uint32_t j=0; j<J; ++j)
        //      cout<<"k_jt="<<k_jt[j].t()<<endl;
        // remove unused dishes
        Col<uint32_t> k_used=k_jt[0];
        Col<uint32_t> k_unused=zeros<Col<uint32_t> >(K,1);
        for (uint32_t j=0; j<J; ++j)
        {
          k_used.insert_rows(k_used.n_elem,k_jt[j]);
        }
        uint32_t k_sum;
        for (uint32_t k=0; k<K; ++k)
        {// find unused dishes
          k_sum = sum(k_used==k);
          k_unused(k) = k_sum>0?0:1;
        }
        for (int32_t k=K-1; k>-1; --k) // iterate from large to small so that the indices work out when deleting
          if (k_unused(k)==1){
            for (uint32_t j=0; j<J; ++j)
            {
              Col<uint32_t> ids=find(k_jt[j]>=k);
              for(uint32_t i=0; i<ids.n_elem; ++i)
                k_jt[j](ids(i)) -= 1;
              //cout<<k_jt[j].elem(find(k_jt[j]>=k))<<endl;
            }
            K--;
          }

        cout<<"-- K="<<K<<"; Kprev="<<Kprev<<" deltaK="<<int32_t(K)-int32_t(Kprev)<<endl;
        //    for (uint32_t j=0; j<J; ++j)
        //      cout<<"k_jt="<<k_jt[j].t()<<endl;
        for (uint32_t j=0; j<J; ++j)
        {
          z_ji[j].set_size(N[j]);
          z_ji[j].zeros();
          for (uint32_t i=0; i<N[j]; ++i)
            z_ji[j](i)=k_jt[j](t_ji[j](i));
          //cout<<"z_ji["<<j<<"]="<<z_ji[j].t()<<" |.|="<<z_ji[j].n_elem<<endl;
        }

      }
      mZ_ji = z_ji;
      mK = K;
      mT = T;

      computeTopics(); // compute the corpus level topic distributions from the labels z_ji

      return z_ji;
    };
    // compute density estimate based on data previously fed into the class using addDoc
    bool densityEst(uint32_t Nw, uint32_t K0, uint32_t T0, uint32_t It)
    {
      if(HDP<U>::mX.size() > 0)
      {
        if(HDP<U>::mX_te.size() > 0){
          // put the test docs mX_te into th normal docs and record their index
          mX_id_test.resize(HDP<U>::mX_te.size());
          for(uint32_t i=0; i<HDP<U>::mX_te.size(); ++i){
            mX_id_test[i]= HDP<U>::mX.size(); // helps locate the documents wich are trained in order to get a topic model
            HDP<U>::mX.push_back(HDP<U>::mX_te[i]);
          }
        }
        mZ_ji = densityEst(HDP<U>::mX,Nw,K0,T0,It);
        return true;
      }else{
        return false;
      }
    };

    // after computing the labels we can use this to get them.
    bool getClassLabels(Col<uint32_t>& z_i, uint32_t i)
    {
      if(mZ_ji.size() > 0 && i < mZ_ji.size())
      {
        z_i=mZ_ji[i];
        return true;
      }else{
        return false;
      }
    };

    Row<double> logP_w(uint32_t d) const
    {
      uint32_t N = mZ_ji[d].n_elem;
      Row<double> logP(N);
      logP.zeros();
      for (uint32_t i=0; i<N; ++i){
        logP[ i ] += mBeta[mZ_ji[d](i)]->mode()->logP(HDP<U>::mX[d].col(i));
      }
      return logP;
    };

    Mixture<U> docMixture(uint32_t d) const
    {
//      uint32_t N = mZ_ji[d].n_elem;
      Col<uint32_t> z_u = unique(mZ_ji[d]);
      Row<double> ps(z_u.n_elem);

      DistriContainer<U> beta(z_u.n_elem);
      for (uint32_t i=0; i < z_u.n_elem; ++i){
        ps[i] = sum(mZ_ji[d]==z_u(i));
        beta[i] = mBeta[z_u(i)]->mode()->getCopy(); // TODO only works for Dir Base Measure I guess
      }
      return Mixture<U>(beta,ps);

    }

    /* compute the perplexity of all test docs after gibbs sampling is done
     */
    Row<double> perplexity()
    {
      mPerp.set_size(HDP<U>::mX_ho.size());
      for (uint32_t i=0; i<mX_id_test.size(); ++i){
        // iterate over all held out data and compute the perplexity
        uint32_t d=mX_id_test[i];
        Mixture<U> mix = docMixture(d);
        mPerp(i) = HDP<U>::perplexity(HDP<U>::mX_ho[i],mix);
      }
      return mPerp;
    };

  protected:

    Row<uint32_t> mX_id_test; //  the id of the half of the test data in mX

    vector<Row<uint32_t> > mZ_ji;
    uint32_t mNw; // number of different words
    uint32_t mK;
    vector<uint32_t> mT;
    DistriContainer<U> mBeta; // corpus level topics
    Row<double> mPerp; // perplexities of all test docs after sampling is finished

  private:

    //TODO: compute topics from the labeling
    void computeTopics(void)
    {
      mBeta.init(HDP<U>::mH0,mK);

      for (uint32_t k=0; k<mK; ++k){
        Mat<U> x_k(HDP<U>::mX[0].n_rows,0);

        uint32_t D=HDP<U>::mX.size(); // D=J -> number of documents in corpus; document d/j
        for (uint32_t d=0; d<D; ++d){
//          uint32_t N = mZ_ji[d].n_elem;
          x_k = join_rows(x_k, HDP<U>::mX[d].cols(find(mZ_ji[d] == k )));

//          for (uint32_t i=0; i<N; ++i){
//            assert(mZ_ji[d](i)<mK);
//            assert(HDP<U>::mX[d](i)<mNw);
//            mBeta[
//              mBeta(mZ_ji[d](i), HDP<U>::mX[d](i)) ++; // increment count for word x_di in topic z_di
//          }
        }
        mBeta[k]->posterior(x_k);
      }

//      //normalize
//      for (uint32_t k=0; k<mK; ++k){
//        mBeta.row(k) /= sum(mBeta.row(k));
//      }
    };

    Mat<U> getXinK(const vector<Mat<U> >& x, uint32_t j_x, uint32_t i_x, uint32_t k,
        const vector<Col<uint32_t> >& k_jt, const vector<Col<uint32_t> >& t_ji, bool disp=false) const
    {
      uint32_t J = k_jt.size();
      Mat<U> x_k; // datapoints in cluster k
#ifndef NDEBUG
      if(disp) printf("----------- J=%d; j_x=%d; i_x=%d; k=%d; ----------- \n",J,j_x,i_x,k);
#endif
      for (uint32_t j=0; j<J; ++j)
      {
        Col<uint32_t> T_k = find(k_jt[j] == k);
#ifndef NDEBUG
        if(disp) cout<<"k="<<k<<" k_jt[j]="<<k_jt[j].t();
        if(disp) cout<<"@j="<<j<<" T_k="<<T_k.t();
#endif
        for (uint32_t i=0; i < T_k.n_elem; ++i)
        { // for all tables with the dish k_jt
          uvec id=find(t_ji[j] == T_k(i));
          //uint32_t inThere=sum(id==i_x)
          if(j_x==j)
          {
            Col<uint32_t> i_x_id=find(id==i_x);
#ifndef NDEBUG
            if(disp) cout<<"T_k(i)="<<T_k(i)<<"; i_x_id="<<i_x_id.t();
#endif
            if(i_x_id.n_elem > 0)
              id.shed_row(i_x_id(0)); // make sure, that that j_x and i_x are not in the x_k
          }
#ifndef NDEBUG
          if(disp) cout<<"t_ji[j]="<<t_ji[j].t();
          if(disp) cout<<"id="<<id.t();
          if(disp) cout<<"x_k_before="<<x_k<<endl;
#endif
          uint32_t offset=x_k.n_cols;
          x_k.resize(x[j].n_rows,x_k.n_cols+id.n_elem);
          for (uint32_t i=0; i<id.n_elem; ++i)
            x_k.col(offset+i) = x[j].col(id(i)); // append all datapoints which are sitting at a table with dish k
#ifndef NDEBUG
          if(disp) cout<<"x_k_after="<<x_k<<endl;
#endif
        }
      }
      //x_k.shed_col(0); // remove first row of zeros
#ifndef NDEBUG
      if(disp) cout<<x_k<<endl;
#endif
      return x_k;
    };
};


//
// latency-control.cpp
//
//  Created by Peter Gusev on 07 June 2016.
//  Copyright 2013-2016 Regents of the University of California
//

#include "latency-control.hpp"
#include <boost/make_shared.hpp>
#include <boost/thread/lock_guard.hpp>

#include "estimators.hpp"
#include "clock.hpp"
#include "statistics.hpp"
#include "playout-control.hpp"

using namespace ndnrtc;
using namespace ndnrtc::statistics;
using namespace estimators;

#define DEFAULT_TARGET_QUEUE_SIZE 150

#define STABILITY_ESTIMATOR_LOW_SENSITIVITY 0.3
#define STABILITY_ESTIMATOR_MID_SENSITIVITY 0.18
#define STABILITY_ESTIMATOR_HI_SENSITIVITY  0.1

#define STABILITY_ESTIMATOR_LOW (boost::make_shared<StabilityEstimator>(10, 4, \
                                 STABILITY_ESTIMATOR_LOW_SENSITIVITY, \
                                 (1-STABILITY_ESTIMATOR_LOW_SENSITIVITY)))
#define STABILITY_ESTIMATOR_MID (boost::make_shared<StabilityEstimator>(10, 4, \
                                 STABILITY_ESTIMATOR_MID_SENSITIVITY, \
                                 (1-STABILITY_ESTIMATOR_MID_SENSITIVITY)))
#define STABILITY_ESTIMATOR_HI (boost::make_shared<StabilityEstimator>(10, 4, \
                                 STABILITY_ESTIMATOR_HI_SENSITIVITY, \
                                 (1-STABILITY_ESTIMATOR_HI_SENSITIVITY)))

//******************************************************************************
namespace ndnrtc
{
class StabilityEstimator : public ndnlog::new_api::ILoggingObject
{
  public:
    StabilityEstimator(unsigned int sampleSize, unsigned int minStableOccurrences,
                       double threshold, double rateSimilarityLevel);
    virtual ~StabilityEstimator() {}

    void newDataArrived(double currentRate);
    bool isStable() const { return isStable_; }
    void flush();

    Average getDarrAverage() const { return m1_; }

  protected:
    unsigned int sampleSize_, minOccurrences_;
    double threshold_, rateSimilarityLevel_;
    Average m1_, m2_;
    unsigned int nStableOccurrences_, nUnstableOccurrences_;
    bool isStable_;
    uint64_t lastTimestamp_;
};

class DrdChangeEstimator : public ndnlog::new_api::ILoggingObject
{
  public:
    DrdChangeEstimator(unsigned int sampleSize, unsigned int minStableOccurrences,
                       double threshold);
    ~DrdChangeEstimator() {}

    void newDrdValue(const Average &drdEstimator);
    bool hasChange();
    void flush();

  private:
    unsigned int sampleSize_, minOccurrences_;
    double threshold_;
    unsigned int nStableOccurrences_;
    bool isStable_;

    unsigned int nChanges_;
    unsigned int nMinorConsecutiveChanges_;
    unsigned int lastCheckedChangeNumber_;
};
}

//******************************************************************************
StabilityEstimator::StabilityEstimator(unsigned int sampleSize, unsigned int minStableOccurrences,
                                       double threshold, double rateSimilarityLevel)
    : sampleSize_(sampleSize), minOccurrences_(minStableOccurrences),
      threshold_(threshold), rateSimilarityLevel_(rateSimilarityLevel),
      m1_(Average(boost::make_shared<SampleWindow>(sampleSize_))),
      m2_(Average(boost::make_shared<SampleWindow>(sampleSize_)))
{
    flush();
    description_ = "stability-estimator";
}

void StabilityEstimator::newDataArrived(double currentRate)
{
    int64_t now = clock::millisecondTimestamp();

    if (lastTimestamp_ != 0)
    {
        unsigned int delta = now - lastTimestamp_;

        m2_.newValue(m1_.oldestValue());
        m1_.newValue(delta);

        double mean = m1_.value();
        double mean2 = m2_.value();

        if (mean != 0 && mean2 != 0)
        {
            double ratio = (mean2 / mean);
            double targetDelay = 1000. / currentRate;
            double similarityLevel = 1 - fabs(mean - targetDelay) / targetDelay;

            if (fabs(ratio - 1) <= threshold_ &&
                similarityLevel >= rateSimilarityLevel_)
            {
                nUnstableOccurrences_ = 0;
                nStableOccurrences_++;

                if (!isStable_)
                    LogTraceC << "stable occurrence #" << nStableOccurrences_ << std::endl;
            }
            else if (++nUnstableOccurrences_ >= minOccurrences_)
                nStableOccurrences_ = 0;

            isStable_ = (nStableOccurrences_ >= minOccurrences_);

            LogTraceC
                << "delta\t" << delta
                << "\tmean\t" << mean
                << "\tmean2\t" << mean2
                << "\tratio\t" << ratio
                << "\trate\t" << currentRate
                << "\ttarget delay\t" << targetDelay
                << "\tsim level\t" << similarityLevel
                << "\tstable\t" << (isStable_ ? "YES" : "NO")
                << std::endl;
        }
        else
            LogTraceC
                << "delta\t" << delta
                << "\tmean\t" << mean
                << "\tmean2\t" << mean2
                << "\tstable\t" << (isStable_ ? "YES" : "NO")
                << std::endl;
    }

    lastTimestamp_ = now;
}

void StabilityEstimator::flush()
{
    isStable_ = false;
    nStableOccurrences_ = 0;
    nUnstableOccurrences_ = 0;
    lastTimestamp_ = 0;
    m1_ = Average(boost::make_shared<SampleWindow>(sampleSize_));
    m2_ = Average(boost::make_shared<SampleWindow>(sampleSize_));
}

//******************************************************************************
DrdChangeEstimator::DrdChangeEstimator(unsigned int sampleSize,
                                       unsigned int minStableOccurrences, double threshold)
{
    description_ = "drd-change-est";
}

void DrdChangeEstimator::newDrdValue(const Average &drdEstimator)
{
    double mean = drdEstimator.value();

    if (mean != 0)
    {
        double deviationPercentage = drdEstimator.deviation() / mean;

        if (deviationPercentage <= threshold_)
            nStableOccurrences_++;
        else
        {
            isStable_ = false;
            nChanges_ = 0;
            nMinorConsecutiveChanges_ = 0;
            nStableOccurrences_ = 0;
        }

        isStable_ = (nStableOccurrences_ >= minOccurrences_);

        if (isStable_)
        {
            double changePercentage = fabs(drdEstimator.latestValue() - mean) / mean;
            if (changePercentage >= 0.08)
            {
                if (changePercentage <= 0.2)
                {
                    nMinorConsecutiveChanges_++;
                    if (nMinorConsecutiveChanges_ >= sampleSize_)
                    {
                        nMinorConsecutiveChanges_ = 0;
                        nChanges_++;
                    }
                }
                else
                    nChanges_++;
            }
            else
                nMinorConsecutiveChanges_ = 0;
        }

        LogTraceC
            << "rtt\t" << drdEstimator.latestValue()
            << "\tmean\t" << mean
            << "\tdeviation\t" << deviationPercentage
            << "\tn changes\t" << nChanges_
            << "\tn minor\t" << nMinorConsecutiveChanges_
            << "\tstable\t" << (isStable_ ? "YES" : "NO")
            << std::endl;
    }
}

bool DrdChangeEstimator::hasChange()
{
    bool command = false;

    if (nChanges_ > lastCheckedChangeNumber_)
        command = true;

    lastCheckedChangeNumber_ = nChanges_;
    return command;
}

void DrdChangeEstimator::flush()
{
    isStable_ = false;
    nStableOccurrences_ = 0;
    nChanges_ = 0;
    nMinorConsecutiveChanges_ = 0;
    lastCheckedChangeNumber_ = 0;
}

//******************************************************************************
unsigned int
LatencyControl::DefaultStrategy::getTargetPlayoutSize(const estimators::Average &drd,
                                                      const unsigned int &lowerLimit)
{
    double d = drd.value() + alpha_ * drd.deviation();
    return (d > lowerLimit ? (unsigned int)d : lowerLimit);
}

//******************************************************************************
LatencyControl::LatencyControl(unsigned int timeoutWindowMs,
                               const boost::shared_ptr<const DrdEstimator> &drd,
                               const boost::shared_ptr<statistics::StatisticsStorage> &storage)
    :
    stabilityEstimator_(STABILITY_ESTIMATOR_LOW),
    queueSizeStrategy_(boost::make_shared<DefaultStrategy>()), // default
    // queueSizeStrategy_(boos::make_shared<DefaultStrategy>(10)), // conservative
    drdChangeEstimator_(boost::make_shared<DrdChangeEstimator>(7, 3, 0.12)),
    timestamp_(0),
    waitForChange_(false), waitForStability_(false),
    timeoutWindowMs_(timeoutWindowMs),
    drd_(drd),
    sstorage_(storage),
    interArrival_(Average(boost::make_shared<estimators::SampleWindow>(10))),
    targetRate_(30.),
    observer_(nullptr),
    currentCommand_(KeepPipeline)
{
    description_ = "latency-control";
    (*sstorage_)[Indicator::BufferTargetSize] = DEFAULT_TARGET_QUEUE_SIZE;
}

LatencyControl::~LatencyControl()
{
}

void LatencyControl::onDrdUpdate()
{
    drdChangeEstimator_->newDrdValue(drd_->getLatestUpdatedAverage());

    if (playoutControl_.get())
    {
        unsigned int targetSize = queueSizeStrategy_->getTargetPlayoutSize(drd_->getOriginalAverage(),
                                                                           DEFAULT_TARGET_QUEUE_SIZE);

        if (targetSize != playoutControl_->getThreshold())
        {
            LogDebugC << "updating target playback queue size to " << targetSize << std::endl;
            playoutControl_->setThreshold(targetSize);

            (*sstorage_)[Indicator::BufferTargetSize] = targetSize;
        }
    }
}

void LatencyControl::sampleArrived(const PacketNumber &playbackNo)
{
    LogTraceC << "sample " << playbackNo << ". target rate " << targetRate_ << std::endl;

    PipelineAdjust command = KeepPipeline;
    int64_t now = clock::millisecondTimestamp();

    if (timestamp_ == 0)
        timestamp_ = now;

    bool timeoutFired = (now - timestamp_ > timeoutWindowMs_);
    stabilityEstimator_->newDataArrived(targetRate_);

    if (stabilityEstimator_->isStable())
    {
        if (waitForChange_)
        {
            if (drdChangeEstimator_->hasChange())
            {
                LogTraceC << "latest data. DRD changed ("
                          << drd_->getLatestUpdatedAverage().value() << "ms)"
                          << " wait for stabilization" << std::endl;

                timestamp_ = now;
                waitForStability_ = true;
                waitForChange_ = false;
            }
            else if (timeoutFired)
            {
                LogTraceC << "latest data. DRD change timed out. cmd: decrease" << std::endl;

                command = DecreasePipeline;
            }
        }
        else
        {
            LogTraceC << "latest data. cmd: decrease" << std::endl;

            waitForStability_ = false;
            // this needs to be set only if pipeline was actually changed. @see pipelineChanged()
            // waitForChange_ = true;
            command = DecreasePipeline;
        }
    }
    else // if unstable and we're not waiting for anything - increase
        if (timeoutFired)
        {
            LogTraceC << "stale data. cmd: increase" << std::endl;

            timestamp_ = now;
            waitForStability_ = true;
            waitForChange_ = false;
            command = IncreasePipeline;
        }

    currentCommand_ = command;

    {
        boost::lock_guard<boost::mutex> scopedLock(mutex_);
        if (observer_ && observer_->needPipelineAdjustment(command))
            pipelineChanged(now);
    }

    (*sstorage_)[Indicator::Darr] = stabilityEstimator_->getDarrAverage().latestValue();
    (*sstorage_)[Indicator::LatencyControlStable] = stabilityEstimator_->isStable();
    (*sstorage_)[Indicator::LatencyControlCommand] = command;
}

void LatencyControl::reset()
{
    stabilityEstimator_->flush();
    drdChangeEstimator_->flush();
    timestamp_ = 0;
    waitForChange_ = false;
    waitForStability_ = false;
    interArrival_ = Average(boost::make_shared<estimators::SampleWindow>(10));
    targetRate_ = 30.;
    currentCommand_ = KeepPipeline;
}

void LatencyControl::registerObserver(ILatencyControlObserver *o)
{
    boost::lock_guard<boost::mutex> scopedLock(mutex_);
    observer_ = o;
}

void LatencyControl::unregisterObserver()
{
    boost::lock_guard<boost::mutex> scopedLock(mutex_);
    observer_ = nullptr;
}

void LatencyControl::setLogger(boost::shared_ptr<ndnlog::new_api::Logger> logger)
{
    NdnRtcComponent::setLogger(logger);
    stabilityEstimator_->setLogger(logger);
    drdChangeEstimator_->setLogger(logger);
}

#pragma mark - private
void LatencyControl::pipelineChanged(int64_t now)
{
    if (currentCommand_ == DecreasePipeline)
    {
        timestamp_ = now;
        waitForChange_ = (currentCommand_ == DecreasePipeline);
    }
    drdChangeEstimator_->flush();

    LogDebugC << "pipeline changed. cmd: "
              << (currentCommand_ == DecreasePipeline ? "decrease" :
                                    (currentCommand_ == IncreasePipeline ? "increase" : "keep"))
              << " wait for DRD change: " << (waitForChange_ ? "YES" : "NO")
              << " wait for stabilization: " << (waitForStability_ ? "YES" : "NO")
              << std::endl;
}


#ifndef GRG_HPP
#define GRG_HPP

#include "CImg.h"

using namespace cimg_library;

#include <algorithm>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
using namespace boost::numeric::ublas;

#endif


// Copyright 2014 Irfan Hamid
//
// 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 <cmath>
#include <boost/test/unit_test.hpp>
#include "ProcessorCaps.hpp"
#include "AvxInternals.hpp"

#define CHECK_DELTA(ref, actual, e) BOOST_CHECK_LE(abs(ref - actual), e)

#define EPSILON 0.001
#define TEST_VECTOR_LENGTH 517

extern khyber::ProcessorCaps caps;

BOOST_AUTO_TEST_SUITE(AvxInternalsTestSuite)

using namespace khyber;

BOOST_AUTO_TEST_CASE(TestAvxAddSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t sum[TEST_VECTOR_LENGTH];
  sp_t addend0[TEST_VECTOR_LENGTH];
  sp_t addend1[TEST_VECTOR_LENGTH];
  for ( size_t i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    addend0[i] = i;
    addend1[i] = 2 * i;
  }

  avx::InternalAdd(TEST_VECTOR_LENGTH,
                           sum,
                           addend0,
                           addend1);
  for ( size_t i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( sum[i] != (addend0[i] + addend1[i]) ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxSubSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t difference[TEST_VECTOR_LENGTH];
  sp_t minuend[TEST_VECTOR_LENGTH];
  sp_t subtrahend[TEST_VECTOR_LENGTH];
  for ( size_t i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    minuend[i] = i * i;
    subtrahend[i] = (i - 1) * (i - 1);
  }

  avx::InternalSub(TEST_VECTOR_LENGTH,
                    difference,
                    minuend,
                    subtrahend);
  for ( size_t i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( difference[i] != (minuend[i] - subtrahend[i]) ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxMulSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t product[TEST_VECTOR_LENGTH];
  sp_t multiplier[TEST_VECTOR_LENGTH];
  sp_t multiplicand[TEST_VECTOR_LENGTH];
  for ( size_t i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    multiplier[i] = i;
    multiplicand[i] = (i % 2 ? -1 : 1) * i;
  }

  avx::InternalMul(TEST_VECTOR_LENGTH,
                    product,
                    multiplier,
                    multiplicand);
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( product[i] != multiplier[i] * multiplicand[i] ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxScalarMulSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t product[TEST_VECTOR_LENGTH];
  sp_t src[TEST_VECTOR_LENGTH];
  sp_t multiplier = 3.14159;
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    src[i] = i;
  }

  avx::InternalScalarMul(TEST_VECTOR_LENGTH,
                         multiplier,
                         product,
                         src);
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( product[i] != src[i] * multiplier ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxDivSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t quotient[TEST_VECTOR_LENGTH];
  sp_t dividend[TEST_VECTOR_LENGTH];
  sp_t divisor[TEST_VECTOR_LENGTH];
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    dividend[i] = i;
    divisor[i] = 1 + ((i % 2 ? -1 : 1) * i * i);
  }

  avx::InternalDiv(TEST_VECTOR_LENGTH,
                    quotient,
                    dividend,
                    divisor);
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( quotient[i] != dividend[i] / divisor[i] ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxScalarDivSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t quotient[TEST_VECTOR_LENGTH];
  sp_t src[TEST_VECTOR_LENGTH];
  sp_t divisor = 3.14159;
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    src[i] = i;
  }

  avx::InternalScalarDiv(TEST_VECTOR_LENGTH,
                         divisor,
                         quotient,
                         src);
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( quotient[i] != src[i] / divisor ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxSqrtSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t src[TEST_VECTOR_LENGTH];
  sp_t dst[TEST_VECTOR_LENGTH];
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    src[i] = i * (i + 10);
  }

  avx::InternalSqrt(TEST_VECTOR_LENGTH,
                     dst,
                     src);
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( abs(dst[i] * dst[i] - src[i]) > EPSILON ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxPowersSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t src[TEST_VECTOR_LENGTH];
  sp_t sqr[TEST_VECTOR_LENGTH];
  sp_t cub[TEST_VECTOR_LENGTH];
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    src[i] = i * (i % 2 ? 1 : -1);
  }

  avx::InternalSquare(TEST_VECTOR_LENGTH,
                       sqr,
                       src);
  avx::InternalCube(TEST_VECTOR_LENGTH,
                     cub,
                     src);
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( sqr[i] != src[i] * src[i] ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
    if ( cub[i] != src[i] * src[i] * src[i] ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxArraySum)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  const size_t LEN = 531;
  sp_t refVal = 0;
  sp_t src[LEN];
  for ( auto i = 0; i < LEN; ++i ) {
    src[i] = i;
    refVal += i;
  }

  sp_t sum;
  avx::InternalSummation(LEN,
                          &sum,
                          src);
  BOOST_CHECK_EQUAL(sum, refVal);
}

BOOST_AUTO_TEST_CASE(TestAvxDotProductSinglePrecision)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t product;
  sp_t multiplicand[TEST_VECTOR_LENGTH];
  sp_t multiplier[TEST_VECTOR_LENGTH];

  sp_t refVal = 0;
  for ( size_t i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    multiplicand[i] = 0.1 * i;
    multiplier[i] = i;
    refVal += (multiplicand[i] * multiplier[i]);
  }

  avx::InternalDotProduct(TEST_VECTOR_LENGTH,
                                   &product,
                                   multiplicand,
                                   multiplier);
  BOOST_CHECK_EQUAL(refVal, product);

  avx::InternalDotProductFma(TEST_VECTOR_LENGTH,
                              &product,
                              multiplicand,
                              multiplier);
  BOOST_CHECK_EQUAL(refVal, product);
}

BOOST_AUTO_TEST_CASE(TestAvxNegate)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t negated[TEST_VECTOR_LENGTH];
  sp_t src[TEST_VECTOR_LENGTH];

  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    src[i] = i / 10;
  }

  avx::InternalNegate(TEST_VECTOR_LENGTH,
                      negated,
                      src);
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    if ( negated[i] != -src[i] ) {
      BOOST_CHECK_MESSAGE(false, i);
      break;
    }
  }
}

BOOST_AUTO_TEST_CASE(TestAvxDistance)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t v1[TEST_VECTOR_LENGTH];
  sp_t v2[TEST_VECTOR_LENGTH];

  sp_t refVal = 0;
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    v1[i] = i * 1.1;
    v2[i] = i / 1.1;
    refVal += ((v1[i] - v2[i]) * (v1[i] - v2[i]));
  }
  refVal = sqrt(refVal);

  sp_t distance;
  avx::InternalDistance(TEST_VECTOR_LENGTH,
                        &distance,
                        v1,
                        v2);
  BOOST_CHECK_LT(abs(distance - refVal), EPSILON);
}

BOOST_AUTO_TEST_CASE(TestAvxReciprocate)
{
  if ( !caps.IsAvx() ) {
    return;
  }

  sp_t reciprocated[TEST_VECTOR_LENGTH];
  sp_t src[TEST_VECTOR_LENGTH];
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    src[i] = 1.0 / (sp_t)i;
  }

  avx::InternalReciprocate(TEST_VECTOR_LENGTH,
                           reciprocated,
                           src);
  for ( auto i = 0; i < TEST_VECTOR_LENGTH; ++i ) {
    CHECK_DELTA(reciprocated[i], i, EPSILON);
  }
}

BOOST_AUTO_TEST_SUITE_END()


// =======================================================================
// Copyright 2015 by Ireneusz Szcześniak
// Author: Ireneusz Szcześniak <www.irkos.org>
//
// 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)
// =======================================================================

// =======================================================================
// This is the implementation of the Yen algorithm:
//
// Jin Y. Yen, Finding the k shortest loopless paths in a network,
// Management Science, vol. 17, no. 11, July 1971, pages 712-716
//
// Note 1: in the article there are Q^k Q_k Q^k_k used.  As far as I
// can tell they have the same meaning.  Q simply denotes the number
// of the node previous to the last node in a path (which always
// equals to the number of nodes in the path - 1), while (Q) denotes
// the node number Q in a path.  For instance, in the path a-b-c-d,
// the number of the node previous to the last is Q = 3, while (Q) =
// c.  Q^k refers to the k-th shortest path, and simply means the
// number of the node, which is previous to the last in the k-shortest
// path.
// =======================================================================

#ifndef BOOST_GRAPH_YEN_KSP
#define BOOST_GRAPH_YEN_KSP

#include <list>
#include <optional>
#include <set>
#include <utility>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>

#include "custom_dijkstra_call.hpp"

namespace boost {

  template <typename G>
  using Edge = typename G::edge_descriptor;

  template <typename G>
  using Vertex = typename G::vertex_descriptor;

  template <typename G>
  using Path = std::list<Edge<G>>;

  template <typename W, typename G>
  using Result = std::pair<W, Path<G>>;

  template <typename Graph, typename WeightMap, typename IndexMap>
  bool
  yen_ksp(const Graph& g, Vertex<Graph> s, Vertex<Graph> t,
          WeightMap wm, IndexMap im,
          std::list<Result<typename WeightMap::value_type, Graph>> &A,
          std::set<Result<typename WeightMap::value_type, Graph>> &B)
  {
    using vs_type = std::set<Vertex<Graph>>;
    using es_type = std::set<Edge<Graph>>;
    using kr_type = Result<typename WeightMap::value_type, Graph>;
    using fg_type = filtered_graph<Graph,
                                   is_not_in_subset<es_type>,
                                   is_not_in_subset<vs_type>>;

    std::optional<kr_type> ksp;

    // If A is empty, then we're looking for the first shortest path.
    if (A.empty())
      {
        assert(B.empty());
        // Try to find the (optional) shortest path.
        ksp = custom_dijkstra_call(g, s, t, wm, im);
      }
    else
      {
        // The previous shortest result and path.
        const auto &psr = A.back();
        const auto &psp = psr.second;

        // The set of excluded edges.  It's a set, because the
        // algorithm can try to exclude an edge many times.
        es_type exe;
        // The set of excluded vertexes.
        vs_type exv;

        // The edge predicate.
        is_not_in_subset<es_type> ep(exe);
        // The vertex predicate.
        is_not_in_subset<vs_type> vp(exv);

        // The filtered graph.
        fg_type fg(g, ep, vp);

        // The root result.
        kr_type rr;
        // The root path.
        const Path<Graph> &rp = rr.second;

        // Use the previous shortest path to get tentative paths.  We
        // can go ahead with the loop without checking any condition
        // (the condition in the for-statement is true): the path
        // found must have at least one link, because s != t.
        for(typename Path<Graph>::const_iterator i = psp.begin();
            true;)
          {
            // An edge of the previous shortest path.
            const Edge<Graph> &edge = *i;

            // The spur vertex - we try to deviate at this node.
            const Vertex<Graph> &sv = source(edge, g);

            // Iterate over all previous shortest paths.  An iteration
            // examines j-th shortest path.
            for(const auto &jr: A)
              {
                // The j-th shortest path.
                const Path<Graph> &jp = jr.second;

                // Let's prepare for the comparison.
                typename Path<Graph>::const_iterator jpi = jp.begin();
                typename Path<Graph>::const_iterator rpi = rp.begin();

                // Iterate as long as the edges are equal.
                while(jpi != jp.end() && rpi != rp.end() && *jpi == *rpi)
                  ++jpi, ++rpi;

                // Make sure we didn't reach the end of jp.  If we
                // did, there is no next edge in jp, which we could
                // exclude.  Also, make sure we reached the end of rp,
                // i.e., the jp begins with the complete rp, and not a
                // head of rp.
                if (jpi != jp.end() && rpi == rp.end())
                  exe.insert(*jpi);
              }

            // Optional spur result.
            std::optional<kr_type>
              osr = custom_dijkstra_call(fg, sv, t, wm, im);

            if (osr)
              {
                // The tentative result.
                kr_type tr = std::move(osr.value());
                tr.first += rr.first;
                tr.second.insert(tr.second.begin(), rp.begin(),
                                 rp.end());
                B.insert(std::move(tr));
              }

            // We have the condition to break the look here, and not
            // at the beginning, because we don't want to execute the
            // remainer of the loop in vain.
            if (++i == psp.end())
              break;

            // Remove the vertex that in this iteration is the spur,
            // but in the next iteration it's going to be a vertex
            // that should not be considered in the search for a spur
            // path.
            exv.insert(sv);

            // Add the edge to the back of the root result.
            rr.first += get(wm, edge);
            rr.second.push_back(edge);
          }

        // Take the shortest tentative path and make it the next
        // shortest path.
        if (!B.empty())
          {
            ksp = *B.begin();
            B.erase(B.begin());
          }
      }

    if (ksp)
      A.push_back(std::move(ksp.value()));

    return bool(ksp);
  }

  template <typename Graph, typename WeightMap, typename IndexMap>
  std::list<std::pair<typename WeightMap::value_type,
                      std::list<typename Graph::edge_descriptor>>>
  yen_ksp(const Graph& g, Vertex<Graph> s, Vertex<Graph> t,
          WeightMap wm, IndexMap im, std::optional<unsigned> K)
  {
    using kr_type = Result<typename WeightMap::value_type, Graph>;

    // The shortest paths - these we return.
    std::list<kr_type> A;

    // An empty result if the source and destination are the same.
    if (s != t)
      {
        // The tentative paths - these are candidate paths.  It's a
        // set, because we want to make sure that a given result can
        // show up in the set of tentative results only once.  The
        // problem is that the algorithm can find the same tentative
        // path many times.
        std::set<kr_type> B;

        // In each iteration we produce the k-th shortest path.
        for (int k = 1; !K || k <= K.value(); ++k)
          if (!yen_ksp(g, s, t, wm, im, A, B))
            // We break the loop if no path was found.
            break;
      }

    return A;
  }

  template <typename Graph>
  std::list<std::pair<typename property_map<Graph, edge_weight_t>::value_type,
                      std::list<Edge<Graph>>>>
  yen_ksp(const Graph& g, Vertex<Graph> s, Vertex<Graph> t,
          std::optional<unsigned> K = std::optional<unsigned>())
  {
    return yen_ksp(g, s, t, get(edge_weight_t(), g),
                   get(vertex_index_t(), g), K);
  }

} // boost

#endif /* BOOST_GRAPH_YEN_KSP */


#ifndef CONWAY_GAME_HPP_INCLUDED
#define CONWAY_GAME_HPP_INCLUDED

#include <glad/glad.h>
#include <glm/glm.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "../abstract_game.hpp"
#include "renderer.hpp"

using namespace boost::numeric;

namespace conway {

enum State : char {dead = 0, alive = 1};

class ConwaysGame: public AbstractGame
{
  public:
    ConwaysGame(uint size_x, uint size_y, const glm::mat4 &projection);
    ConwaysGame(const ConwaysGame&) = delete;
    ConwaysGame(ConwaysGame&&) = default;
    ConwaysGame& operator=(const ConwaysGame&) = delete;
    ConwaysGame& operator=(ConwaysGame&&) = default;
    ~ConwaysGame();

    glm::uvec2 getSize() const;
    void draw(GLuint vertex_array);
    void nextGeneration();

  private:
    const uint size_x_;
    const uint size_y_;

    ublas::matrix<State> current_grid_;
    ublas::matrix<State> alternate_grid_;

    Renderer renderer_;
    uint texture_;
    bool is_dirty_;

    static State nextState(State state, int neighbours);
    int countNeighbours(int x, int y) const;
    void writeToTexture(GLuint texture);
};

}

#endif  // CONWAY_GAME_HPP_INCLUDED


/*
 *  The MIT License (MIT)
 *
 *  Copyright (c) 2015 MRL-SPL RoboCup Team
 *
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in all
 *  copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *  SOFTWARE.
 *
 */

/*
 * File:   AxisAngle.hpp
 * Author: Mohammad Ali Sharpasand
 *
 * Created on February 22, 2017
 */

#ifndef BIPED_LIBRARY_AXIS_ANGLE_H
#define BIPED_LIBRARY_AXIS_ANGLE_H

#include "Angle.hpp"
#include <armadillo>

namespace BipedLibrary
{
    using namespace arma;

    class AxisAngle
    {
    public:
        AxisAngle(vec3 axis = vec3(), Angle angle = Angle());

        mat33 rotationMatrix() const;

        vec3 axis() const;
        Angle angle() const;
        vec3 asAVector() const;

        AxisAngle& setAxis(vec3 const& axis);
        AxisAngle& setAngle(Angle const& angle);

        vec3& mutableAxis();
        Angle& mutableAngle();

    private:
        vec3 axis_;
        Angle angle_;

    };

}

#endif


#ifndef IGL_CUT_TO_DISK_H
#define IGL_CUT_TO_DISK_H

#include <igl/igl_inline.h>

#include <Eigen/Core>

#include <vector>

namespace igl
{
    // Given a triangle mesh, computes a set of edge cuts sufficient to carve the
    // mesh into a topological disk, without disconnecting any connected components.
    // Nothing else about the cuts (including number, total length, or smoothness)
    // is guaranteed to be optimal.
    //
    // Simply-connected components without boundary (topological spheres) are left
    // untouched (delete any edge if you really want a disk).
    // All other connected components are cut into disks. Meshes with boundary are
    // supported; boundary edges will be included as cuts.
    //
    // The cut mesh itself can be materialized using cut_mesh().
    //
    // Implements the triangle-deletion approach described by Gu et al's
    // "Geometry Images."
    //
    // Template Parameters:
    //   Index  Integrable type large enough to represent the total number of faces
    //     and edges in the surface represented by F, and all entries of F.
    //
    // Inputs:
    //   F  #F by 3 list of the faces (must be triangles)
    //
    // Outputs:
    //   cuts  List of cuts. Each cut is a sequence of vertex indices (where
    //     pairs of consecutive vertices share a face), is simple, and is either
    //     a closed loop (in which the first and last indices are identical) or
    //     an open curve. Cuts are edge-disjoint.
    //

    template <
    typename DerivedF,
    typename Index>
    IGL_INLINE void cut_to_disk(
                                const Eigen::MatrixBase<DerivedF> &F,
                                std::vector<std::vector<Index> > &cuts);
};

#ifndef IGL_STATIC_LIBRARY
#include "cut_to_disk.cpp"
#endif

#endif


/******************************************************************************
Copyright (c) 2020, Johannes Pankert. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

 * Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

#include <gtest/gtest.h>
#include <ocs2_core/automatic_differentiation/CppAdInterface.h>
#include <ocs2_core/automatic_differentiation/Types.h>
#include <ocs2_robotic_tools/common/RotationTransforms.h>

#include <Eigen/Dense>

TEST(RotationTransforms, quaternionDifferenceJacobian) {
  using ad_vector_t = ocs2::ad_vector_t;
  using ad_scalar_t = ocs2::ad_scalar_t;

  auto adFunction = [](const ad_vector_t& x, const ad_vector_t& p, ad_vector_t& y) {
    Eigen::Quaternion<ad_scalar_t> q(x.head<4>());
    Eigen::Quaternion<ad_scalar_t> qRef(p.head<4>());
    y.resize(3);
    y = ocs2::quaternionDistance(q, qRef);
  };

  ocs2::CppAdInterface adInterface(adFunction, 4, 4, "quaternion_error");
  adInterface.createModels();

  for (int i = 0; i < 1000; i++) {
    Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom();
    Eigen::Quaterniond qRef = Eigen::Quaterniond::UnitRandom();

    Eigen::VectorXd qDyn = q.coeffs();
    Eigen::VectorXd qRefDyn = qRef.coeffs();
    ASSERT_NEAR((ocs2::quaternionDistance(q, qRef) - adInterface.getFunctionValue(qDyn, qRefDyn)).norm(), 0, 1e-5);

    Eigen::MatrixXd jacManual = ocs2::quaternionDistanceJacobian(q, qRef);
    Eigen::MatrixXd jacAuto = adInterface.getJacobian(qDyn, qRefDyn);
    ASSERT_NEAR((jacManual - jacAuto).sum(), 0, 1e-5);
  }

  ASSERT_TRUE(true);
}

// Asserts that the quaterion performs the same rotation as the rotation matrix
void testQuaternionAgainstRotationMatrix(const Eigen::Quaterniond& quat, const Eigen::Matrix3d& rotMatrix){
  for (int i = 0; i < 10; i++){
    const Eigen::Vector3d preRotatePoint = Eigen::Vector3d::Random();
    const Eigen::Vector3d postRotatePointMatrix = rotMatrix*preRotatePoint;
    const Eigen::Vector3d postRotatePointQuat = quat*preRotatePoint;
    ASSERT_NEAR((postRotatePointMatrix - postRotatePointQuat).norm(), 0, 1e-5)<<"rotMatrix = \n"<<rotMatrix<<"\n quat = "<<quat.coeffs().transpose();
  }
}

//Attempts to test all the branches in the cpp ad function
TEST(RotationTransforms, matrixToQuaternionCppAdSelected) {
  using ad_matrix_t = ocs2::ad_matrix_t;
  using ad_vector_t = ocs2::ad_vector_t;
  using ad_scalar_t = ocs2::ad_scalar_t;

  auto adFunction = [](const ad_vector_t& x,  ad_vector_t& y) {
    ad_matrix_t rotMatrix = ad_matrix_t::Zero(3,3);
    rotMatrix << x;
    y.resize(4);
    y = ocs2::matrixToQuaternion(rotMatrix).coeffs();
  };

  ocs2::CppAdInterface adInterface(adFunction, 9, "matrix_to_quaternion");
  adInterface.createModels();

  {
    Eigen::Matrix3d rotMatrix;
    // clang-format off
    rotMatrix << 1, 0, 0,
                 0, 1, 0,
                 0, 0, 1;
    // clang format on
    Eigen::VectorXd serializedRotMatrix(9);
    serializedRotMatrix << rotMatrix;
    const Eigen::VectorXd result = adInterface.getFunctionValue(serializedRotMatrix);
    const Eigen::Quaterniond resultQuaternion(result[3],result[0],result[1],result[2]);
    testQuaternionAgainstRotationMatrix(resultQuaternion, rotMatrix);
  }

  {
    Eigen::Matrix3d rotMatrix;
    // clang-format off
    rotMatrix << 0, 0, 1,
                 0, 1, 0,
                -1, 0, 0;
    // clang format on
    Eigen::VectorXd serializedRotMatrix(9);
    serializedRotMatrix << rotMatrix;
    const Eigen::VectorXd result = adInterface.getFunctionValue(serializedRotMatrix);
    const Eigen::Quaterniond resultQuaternion(result[3],result[0],result[1],result[2]);
    testQuaternionAgainstRotationMatrix(resultQuaternion, rotMatrix);
  }

  {
    Eigen::Matrix3d rotMatrix;
    // clang-format off
    rotMatrix << 0, 0, 1,
                 0, 1, 0,
                -1, 0, 0;
    // clang format on
    Eigen::VectorXd serializedRotMatrix(9);
    serializedRotMatrix << rotMatrix;
    const Eigen::VectorXd result = adInterface.getFunctionValue(serializedRotMatrix);
    const Eigen::Quaterniond resultQuaternion(result[3],result[0],result[1],result[2]);
    testQuaternionAgainstRotationMatrix(resultQuaternion, rotMatrix);
  }

  {
    Eigen::Matrix3d rotMatrix;
    // clang-format off
    rotMatrix << 1, 0, 0,
                 0, 0, 1,
                 0,-1, 0;
    // clang format on
    Eigen::VectorXd serializedRotMatrix(9);
    serializedRotMatrix << rotMatrix;
    const Eigen::VectorXd result = adInterface.getFunctionValue(serializedRotMatrix);
    const Eigen::Quaterniond resultQuaternion(result[3],result[0],result[1],result[2]);
    testQuaternionAgainstRotationMatrix(resultQuaternion, rotMatrix);
  }

  {
    Eigen::Matrix3d rotMatrix;
    // clang-format off
    rotMatrix << 0, 1, 0,
                -1, 0, 0,
                 0, 0, 1;
    // clang format on
    Eigen::VectorXd serializedRotMatrix(9);
    serializedRotMatrix << rotMatrix;
    const Eigen::VectorXd result = adInterface.getFunctionValue(serializedRotMatrix);
    const Eigen::Quaterniond resultQuaternion(result[3],result[0],result[1],result[2]);
    testQuaternionAgainstRotationMatrix(resultQuaternion, rotMatrix);
  }

  {
    Eigen::Matrix3d rotMatrix;
    // clang-format off
    rotMatrix << 0,-1, 0,
                -1, 0, 0,
                 0, 0,-1;
    // clang format on
    Eigen::VectorXd serializedRotMatrix(9);
    serializedRotMatrix << rotMatrix;
    const Eigen::VectorXd result = adInterface.getFunctionValue(serializedRotMatrix);
    const Eigen::Quaterniond resultQuaternion(result[3],result[0],result[1],result[2]);
    testQuaternionAgainstRotationMatrix(resultQuaternion, rotMatrix);
  }
}

TEST(RotationTransforms, matrixToQuaternionCppAdRandom) {
  using ad_matrix_t = ocs2::ad_matrix_t;
  using ad_vector_t = ocs2::ad_vector_t;
  using ad_scalar_t = ocs2::ad_scalar_t;

  auto adFunction = [](const ad_vector_t& x,  ad_vector_t& y) {
    ad_matrix_t rotMatrix = ad_matrix_t::Zero(3,3);
    rotMatrix << x;
    y.resize(4);
    y = ocs2::matrixToQuaternion(rotMatrix).coeffs();
  };

  ocs2::CppAdInterface adInterface(adFunction, 9, "matrix_to_quaternion");
  adInterface.createModels();

  for (int i = 0; i < 1000; i++) {
    const Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom();

    const auto rotMatrix = q.toRotationMatrix();
    Eigen::VectorXd serializedRotMatrix(9);
    serializedRotMatrix << rotMatrix;

    const Eigen::VectorXd result = adInterface.getFunctionValue(serializedRotMatrix);
    const Eigen::Quaterniond resultQuaternion(result[3],result[0],result[1],result[2]);

    // Assert that our original generating quaternion matches the resulting one
    ASSERT_NEAR(ocs2::quaternionDistance(q, resultQuaternion).norm(), 0, 1e-5)<<"q = "<<q.coeffs().transpose()<<", resultQuaternion = "<<resultQuaternion.coeffs().transpose();

    // Assert that our resulting quaternion performs the same rotation as the rotation matrix
    testQuaternionAgainstRotationMatrix(resultQuaternion, rotMatrix);

  }
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, primitiveValue) {
  using stan::math::primitive_value;
  EXPECT_EQ(5,primitive_value(5));       // int
  EXPECT_EQ(5U, primitive_value(5U));    // uint
  EXPECT_EQ(10000000000L, primitive_value(10000000000L));  // long >> int
  EXPECT_EQ('a', primitive_value('a'));  // char

  EXPECT_EQ(7.3,primitive_value(7.3));  // double
  EXPECT_EQ(7.3f,primitive_value(7.3f));  // float
}

TEST(MathFunctions, primiviteValueNaN) {
  using boost::math::isnan;
  using std::numeric_limits;
  using stan::math::primitive_value;

  EXPECT_TRUE(isnan<double>(primitive_value(numeric_limits<double>::quiet_NaN())));
}


/*!
 * @file
 * Forward declares the @ref Orderable typeclass.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_FWD_ORDERABLE_HPP
#define BOOST_MPL11_FWD_ORDERABLE_HPP

#include <boost/mpl11/fwd/bool.hpp>

namespace boost { namespace mpl11 {
    /*!
     * @ingroup typeclasses
     * @defgroup Orderable Orderable
     *
     * The `Orderable` typeclass is used for totally ordered datatypes.
     *
     *
     * ### Methods
     * `less`, `less_equal`, `greater`, `greater_equal`, `min` and `max`.
     *
     * The `less`, `less_equal`, `greater` and `greater_equal` methods can
     * all be invoked with more than two arguments, in which case the method's
     * behavior conforms to the usual mathematical usage. Specifically,
     * `less<x1, x2, xs...>` is equivalent to
     * `and_<less<x1, x2>, less<x2, xs...>>`.
     *
     * The `min` and `max` methods can also be invoked with more than two
     * arguments. Specifically, `min<x1, x2, xs...>` is equivalent to
     * `min<min<x1, x2>, xs...>`.
     *
     *
     * ### Minimal complete definition
     * `less`
     *
     * @{
     */
    template <typename Left, typename Right = Left, typename = true_>
    struct Orderable;

    /*!
     * Returns whether the given objects are ordered according to the `<`
     * ordering.
     */
    template <typename x1, typename x2, typename ...xs>
    struct less;

    /*!
     * Returns whether the given objects are ordered according to the `<=`
     * ordering.
     */
    template <typename x1, typename x2, typename ...xs>
    struct less_equal;

    /*!
     * Returns whether the given objects are ordered according to the `>`
     * ordering.
     */
    template <typename x1, typename x2, typename ...xs>
    struct greater;

    /*!
     * Returns whether the given objects are ordered according to the `>=`
     * ordering.
     */
    template <typename x1, typename x2, typename ...xs>
    struct greater_equal;

    /*!
     * Returns the smallest of its arguments according to the ordering
     * induced by `less`.
     */
    template <typename x1, typename x2, typename ...xs>
    struct min;

    /*!
     * Returns the largest of its arguments according to the ordering
     * induced by `less`.
     */
    template <typename x1, typename x2, typename ...xs>
    struct max;
    //! @}
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_FWD_ORDERABLE_HPP


/*
 * Clustering.hpp
 *
 *  Created on: Apr 4, 2012
 *      Author: david
 */

#ifndef DASP_CLUSTERING_HPP_
#define DASP_CLUSTERING_HPP_

#include "../Point.hpp"
#include "../Parameters.hpp"
#include "../Metric.hpp"
#include <Slimage/Slimage.hpp>
#include <Eigen/Dense>
#include <iostream>

namespace dasp
{

	template<typename METRIC>
	slimage::Image1f ComputeEdges(const ImagePoints& points, const METRIC& mf)
	{
		const unsigned int width = points.width();
		const unsigned int height = points.height();
		slimage::Image1f edges(width, height, slimage::Pixel1f{1e6f});
		// compute edges strength
		for(unsigned int y=1; y<height-1; y++) {
			for(unsigned int x=1; x<width-1; x++) {
				float v;
				const Point& p0 = points(x, y);
				const Point& px1 = points(x-1, y);
				const Point& px2 = points(x+1, y);
				const Point& py1 = points(x, y-1);
				const Point& py2 = points(x, y+1);
				if(!p0.is_valid || !px1.is_valid || !px2.is_valid || !py1.is_valid || !py2.is_valid) {
					v = 1e6; // dont want to be here
				}
				else {
					float dx = mf(px1, px2);
					float dy = mf(py1, py2);
					v = dx + dy;
				}
				edges(x,y) = v;
			}
		}
		return edges;
	}

	template<typename METRIC>
	slimage::Image1i IterateClusters(const std::vector<Cluster>& clusters, const ImagePoints& points, const Parameters& opt, const METRIC& mf)
	{
		slimage::Image1i labels(points.width(), points.height(), slimage::Pixel1i{-1});
		std::vector<float> v_dist(points.size(), 1e9);
		// for each cluster check possible points
		for(unsigned int j=0; j<clusters.size(); j++) {
			const Cluster& c = clusters[j];
			int cx = c.center.px;
			int cy = c.center.py;
			int R = static_cast<int>(c.center.cluster_radius_px * opt.coverage + 0.5f);
			R = std::max<int>(2,R);
			const unsigned int xmin = std::max<int>(0, cx - R);
			const unsigned int xmax = std::min<int>(static_cast<int>(points.width())-1, cx + R);
			const unsigned int ymin = std::max<int>(0, cy - R);
			const unsigned int ymax = std::min<int>(static_cast<int>(points.height())-1, cy + R);
			for(unsigned int y=ymin; y<=ymax; y++) {
				for(unsigned int x=xmin; x<=xmax; x++/*, pnt_index++*/) {
					unsigned int pnt_index = points.index(x, y);
					const Point& p = points[pnt_index];
					if(!p.is_valid) {
						// omit invalid points
						continue;
					}
					float dist = mf(p, c.center);
					float& v_dist_best = v_dist[pnt_index];
					if(dist < v_dist_best) {
						v_dist_best = dist;
						labels[pnt_index] = j;
					}
				}
			}
		}
		return labels;
	}

}

#endif


/*
	This file is part of solidity.

	solidity is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	solidity is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with solidity.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <tools/yulPhaser/SimulationRNG.h>

// NOTE: The code would work with std::random but the results for a given seed would not be reproducible
// across different STL implementations. Boost does not guarantee this either but at least it has only one
// implementation. Reproducibility is not a hard requirement for yul-phaser but it's nice to have.
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <ctime>
#include <limits>

using namespace std;
using namespace solidity;
using namespace solidity::phaser;

thread_local boost::random::mt19937 SimulationRNG::s_generator(SimulationRNG::generateSeed());

bool SimulationRNG::bernoulliTrial(double _successProbability)
{
	boost::random::bernoulli_distribution<double> distribution(_successProbability);

	return distribution(s_generator);
}

size_t SimulationRNG::uniformInt(size_t _min, size_t _max)
{
	boost::random::uniform_int_distribution<size_t> distribution(_min, _max);
	return distribution(s_generator);
}

size_t SimulationRNG::binomialInt(size_t _numTrials, double _successProbability)
{
	// NOTE: binomial_distribution<size_t> would not work because it internally tries to use abs()
	// and fails to compile due to ambiguous conversion.
	assert(_numTrials <= static_cast<size_t>(numeric_limits<long>::max()));

	boost::random::binomial_distribution<long> distribution(static_cast<long>(_numTrials), _successProbability);
	return static_cast<size_t>(distribution(s_generator));
}

uint32_t SimulationRNG::generateSeed()
{
	// This is not a secure way to seed the generator but it's good enough for simulation purposes.
	// The only thing that matters for us is that the sequence is different on each run and that
	// it fits the expected distribution. It does not have to be 100% unpredictable.
	return time(nullptr);
}


// Copyright (c) 2000, 2001, 2002, 2003 by David Scherer and others.
// Copyright (c) 2003, 2004 by Jonathan Brandmeyer and others.
// See the file license.txt for complete license terms.
// See the file authors.txt for a complete list of contributors.

#include "primitive.hpp"
#include "util/errors.hpp"

#include <boost/python/import.hpp>

#include <typeinfo>
#include <cmath>

//#ifndef _MSC_VER // This was needed when we used GTK2 on Windows
#ifndef _WIN32
#include <cxxabi.h>
#endif

#include <sstream>

namespace cvisual {

tmatrix
primitive::model_world_transform(double world_scale, const vector& object_scale) const
{
	// Performs scale, rotation, translation, and world scale (gcf) transforms in that order.
	// ret = world_scale o translation o rotation o scale
	// Note that with the default parameters, only the rotation transformation is returned!  Typical
	//   usage should be model_world_transform( scene.gcf, my_size );

	tmatrix ret;
	// A unit vector along the z_axis.
	vector z_axis = vector(0,0,1);
	if (std::fabs(axis.dot(up) / std::sqrt( up.mag2() * axis.mag2())) > 0.98) {
		// Then axis and up are in (nearly) the same direction: therefore,
		// try two other possible directions for the up vector.
		if (std::fabs(axis.norm().dot( vector(-1,0,0))) > 0.98)
			z_axis = axis.cross( vector(0,0,1)).norm();
		else
			z_axis = axis.cross( vector(-1,0,0)).norm();
	}
	else {
		z_axis = axis.cross( up).norm();
	}

	vector y_axis = z_axis.cross(axis).norm();
	vector x_axis = axis.norm();
	ret.x_column( x_axis );
	ret.y_column( y_axis );
	ret.z_column( z_axis );
	ret.w_column( pos * world_scale );
	ret.w_row();

	ret.scale( object_scale * world_scale );

	return ret;
}

// Oblong objects (e.g. cylinder) whose center is not at "pos" override primitive::get_center
vector
primitive::get_center() const
{
	return pos;
}

using boost::python::import;
using boost::python::object;

bool startup = true;
boost::python::object trail_update;

primitive::primitive()
	: axis(1,0,0), up(0,1,0), pos(0,0,0), trail_initialized(false), obj_initialized(false)
{
}

primitive::primitive( const primitive& other)
	: renderable( other), axis(other.axis), up(other.up),
		pos(other.pos)
{
}

primitive::~primitive()
{
}

void
primitive::rotate( double angle, const vector& _axis, const vector& origin)
{
	tmatrix R = rotation( angle, _axis, origin);
	vector fake_up = up;
	if (!axis.cross( fake_up)) {
		fake_up = vector( 1,0,0);
		if (!axis.cross( fake_up))
			fake_up = vector( 0,1,0);
	}
    {
    	pos = R * pos;
    	axis = R.times_v(axis);
    	up = R.times_v(fake_up);
    }
}

void
primitive::set_pos( const vector& n_pos)
{
	pos = n_pos;
	if (trail_initialized && make_trail) {
		if (obj_initialized) {
			trail_update(primitive_object);
		}
	}
}

shared_vector&
primitive::get_pos()
{
	return pos;
}

void
primitive::set_x( double x)
{
	pos.set_x( x);
	if (trail_initialized && make_trail) {
		if (obj_initialized) {
			trail_update(primitive_object);
		}
	}
}

double
primitive::get_x()
{
	return pos.x;
}

void
primitive::set_y( double y)
{
	pos.set_y( y);
	if (trail_initialized && make_trail) {
		if (obj_initialized) {
			trail_update(primitive_object);
		}
	}
}

double
primitive::get_y()
{
	return pos.y;
}

void
primitive::set_z( double z)
{
	pos.set_z( z);
	if (trail_initialized && make_trail) {
		if (obj_initialized) {
			trail_update(primitive_object);
		}
	}
}

double
primitive::get_z()
{
	return pos.z;
}

void
primitive::set_axis( const vector& n_axis)
{
	axis = n_axis;
}

shared_vector&
primitive::get_axis()
{
	return axis;
}

void
primitive::set_up( const vector& n_up)
{
	up = n_up;
}

shared_vector&
primitive::get_up()
{
	return up;
}

void
primitive::set_color( const rgb& n_color)
{
	color = n_color;
}

void
primitive::set_red( float r)
{
	color.red = r;
}

double
primitive::get_red()
{
	return color.red;
}

void
primitive::set_green( float g)
{
	color.green = g;
}

double
primitive::get_green()
{
	return color.green;
}

void
primitive::set_blue( float b)
{
	color.blue = b;
}

double
primitive::get_blue()
{
	return color.blue;
}

rgb
primitive::get_color()
{
	return color;
}

void
primitive::set_opacity( float a)
{
	opacity = a;
}

double
primitive::get_opacity()
{
	return opacity;
}

void
primitive::set_make_trail( bool t)
{
	if (t && !obj_initialized)
		throw std::runtime_error( "Can't set make_trail=True unless object was created with make_trail specified");
	if (startup) {
		trail_update = import("visual_common.primitives").attr("trail_update");
		startup = false;
	}
	make_trail = t;
	trail_initialized = true;
}

bool
primitive::get_make_trail()
{
	return make_trail;
}

void
primitive::set_primitive_object( boost::python::object obj)
{
	primitive_object = obj;
	obj_initialized = true;
}

boost::python::object
primitive::get_primitive_object()
{
	return primitive_object;
}

PRIMITIVE_TYPEINFO_IMPL(primitive)

} // !namespace cvisual


// (C) 2014 Arek Olek

#include <iostream>

#include <boost/graph/adjacency_list.hpp>

#include "bfs.hpp"
#include "lost.hpp"

#include "test_suite.hpp"

#include "options.hpp"
#include "timing.hpp"

using namespace std;

typedef boost::adjacency_list<
  boost::hash_setS, boost::vecS, boost::undirectedS
  > list_graph;
boost::adjacency_matrix<boost::undirectedS> typedef graph;

int main(int argc, char** argv) {
  options opt(argc, argv);
  int z = opt.get<int>("-z", 1);
  int N = opt.get<int>("-n", 500);
  float p = opt.get<float>("-p", 1);

  timing timer;
  auto improve = make_improvement<graph, graph>("prieto");

  for(int n = 100; n <= N; n += 100) {
    test_suite<graph> suite("gnp", z, n, p);
    double total = 0;
    for(auto G : suite) {
      auto T = bfs_tree(G);
      //list_graph E;
      //copy_edges(T, E);
      timer.start();
      improve(G, T);
      total += timer.stop();
    }
    cout << "{" << n << "," << total/suite.size() << "}";
    if(n < N) cout << ",";
    cout << endl;
  }

  return 0;
}


// Copyright 2015-2020 The ALMA Project Developers
//
// 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.

/// @file
/// Test if the small-grain thermal conductivity is computed correctly.

#include <iostream>
#include <boost/filesystem.hpp>
#include "gtest/gtest.h"
#include <vasp_io.hpp>
#include <qpoint_grid.hpp>
#include <bulk_properties.hpp>

TEST(kappa_sg_case, kappa_sg_test) {
    boost::mpi::environment test;

    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto data_dir = basedir / boost::filesystem::path("GaN_wurtzite");
    auto poscar_path = data_dir / boost::filesystem::path("POSCAR");
    auto ifc_path = data_dir / boost::filesystem::path("FORCE_CONSTANTS");
    auto born_path = data_dir / boost::filesystem::path("BORN");

    auto poscar = alma::load_POSCAR(poscar_path.string().c_str());
    auto syms = alma::Symmetry_operations(*poscar);
    auto force_constants =
        alma::load_FORCE_CONSTANTS(ifc_path.string().c_str(), *poscar, 5, 5, 5);
    auto born = alma::load_BORN(born_path.string().c_str());

    auto grid = std::make_unique<alma::Gamma_grid>(
        *poscar, syms, *force_constants, *born, 12, 12, 12);

    grid->enforce_asr();

    Eigen::Matrix3d kappa_sg = alma::calc_kappa_sg(*poscar, *grid, 300.);

    EXPECT_TRUE(alma::almost_equal(1.75453, kappa_sg(0, 0), 1e-2));
    EXPECT_TRUE(alma::almost_equal(2.12192, kappa_sg(2, 2), 1e-2));
}


/****************************************************************************
 *
 *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
 *   Author: Anton Babushkin <anton.babushkin@me.com>
 *           Pavel Kirienko <pavel.kirienko@gmail.com>
 *           Lorenz Meier <lm@inf.ethz.ch>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file Matrix.hpp
 *
 * Matrix class
 */

#ifndef MATRIX_HPP
#define MATRIX_HPP

#include <stdio.h>
#include <math.h>

#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
#else
#include <platforms/ros/eigen_math.h>
#include <Eigen/Eigen>
#endif
#include <platforms/px4_defines.h>

namespace math
{

template<unsigned int M, unsigned int N>
class __EXPORT Matrix;

// MxN matrix with float elements
template <unsigned int M, unsigned int N>
class __EXPORT MatrixBase
{
public:
	/**
	 * matrix data[row][col]
	 */
	float data[M][N];

	/**
	 * struct for using arm_math functions
	 */
#ifdef CONFIG_ARCH_ARM
	arm_matrix_instance_f32 arm_mat;
#else
	eigen_matrix_instance arm_mat;
#endif

	/**
	 * trivial ctor
	 * Initializes the elements to zero.
	 */
	MatrixBase() :
		data{},
		arm_mat{M, N, &data[0][0]}
	{
	}

	virtual ~MatrixBase() {};

	/**
	 * copyt ctor
	 */
	MatrixBase(const MatrixBase<M, N> &m) :
		arm_mat{M, N, &data[0][0]}
	{
		memcpy(data, m.data, sizeof(data));
	}

	MatrixBase(const float *d) :
		arm_mat{M, N, &data[0][0]}
	{
		memcpy(data, d, sizeof(data));
	}

	MatrixBase(const float d[M][N]) :
		arm_mat{M, N, &data[0][0]}
	{
		memcpy(data, d, sizeof(data));
	}

	/**
	 * set data
	 */
	void set(const float *d) {
		memcpy(data, d, sizeof(data));
	}

	/**
	 * set data
	 */
	void set(const float d[M][N]) {
		memcpy(data, d, sizeof(data));
	}

#if defined(__PX4_ROS)
	/**
	 * set data from boost::array
	 */
	void set(const boost::array<float, 9ul> d) {
	set(static_cast<const float*>(d.data()));
	}
#endif

	/**
	 * access by index
	 */
	float &operator()(const unsigned int row, const unsigned int col) {
		return data[row][col];
	}

	/**
	 * access by index
	 */
	float operator()(const unsigned int row, const unsigned int col) const {
		return data[row][col];
	}

	/**
	 * get rows number
	 */
	unsigned int get_rows() const {
		return M;
	}

	/**
	 * get columns number
	 */
	unsigned int get_cols() const {
		return N;
	}

	/**
	 * test for equality
	 */
	bool operator ==(const Matrix<M, N> &m) const {
		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				if (data[i][j] != m.data[i][j])
					return false;

		return true;
	}

	/**
	 * test for inequality
	 */
	bool operator !=(const Matrix<M, N> &m) const {
		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				if (data[i][j] != m.data[i][j])
					return true;

		return false;
	}

	/**
	 * set to value
	 */
	const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
		memcpy(data, m.data, sizeof(data));
		return *static_cast<Matrix<M, N>*>(this);
	}

	/**
	 * negation
	 */
	Matrix<M, N> operator -(void) const {
		Matrix<M, N> res;

		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				res.data[i][j] = -data[i][j];

		return res;
	}

	/**
	 * addition
	 */
	Matrix<M, N> operator +(const Matrix<M, N> &m) const {
		Matrix<M, N> res;

		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				res.data[i][j] = data[i][j] + m.data[i][j];

		return res;
	}

	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				data[i][j] += m.data[i][j];

		return *static_cast<Matrix<M, N>*>(this);
	}

	/**
	 * subtraction
	 */
	Matrix<M, N> operator -(const Matrix<M, N> &m) const {
		Matrix<M, N> res;

		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				res.data[i][j] = data[i][j] - m.data[i][j];

		return res;
	}

	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				data[i][j] -= m.data[i][j];

		return *static_cast<Matrix<M, N>*>(this);
	}

	/**
	 * uniform scaling
	 */
	Matrix<M, N> operator *(const float num) const {
		Matrix<M, N> res;

		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				res.data[i][j] = data[i][j] * num;

		return res;
	}

	Matrix<M, N> &operator *=(const float num) {
		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				data[i][j] *= num;

		return *static_cast<Matrix<M, N>*>(this);
	}

	Matrix<M, N> operator /(const float num) const {
		Matrix<M, N> res;

		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				res[i][j] = data[i][j] / num;

		return res;
	}

	Matrix<M, N> &operator /=(const float num) {
		for (unsigned int i = 0; i < M; i++)
			for (unsigned int j = 0; j < N; j++)
				data[i][j] /= num;

		return *static_cast<Matrix<M, N>*>(this);
	}

	/**
	 * multiplication by another matrix
	 */
	template <unsigned int P>
	Matrix<M, P> operator *(const Matrix<N, P> &m) const {
#ifdef CONFIG_ARCH_ARM
		Matrix<M, P> res;
		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
		return res;
#else
		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
				(this->arm_mat.pData);
		Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> >
				(m.arm_mat.pData);
		Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him;
		Matrix<M, P> res(Product.data());
		return res;
#endif
	}

	/**
	 * transpose the matrix
	 */
	Matrix<N, M> transposed(void) const {
#ifdef CONFIG_ARCH_ARM
		Matrix<N, M> res;
		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
		return res;
#else
		Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
				(this->arm_mat.pData);
		Me.transposeInPlace();
		Matrix<N, M> res(Me.data());
		return res;
#endif
	}

	/**
	 * invert the matrix
	 */
	Matrix<M, N> inversed(void) const {
#ifdef CONFIG_ARCH_ARM
		Matrix<M, N> res;
		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
		return res;
#else
		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
				(this->arm_mat.pData);
		Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
		Matrix<M, N> res(MyInverse.data());
		return res;
#endif
	}

	/**
	 * set zero matrix
	 */
	void zero(void) {
		memset(data, 0, sizeof(data));
	}

	/**
	 * set identity matrix
	 */
	void identity(void) {
		memset(data, 0, sizeof(data));
		unsigned int n = (M < N) ? M : N;

		for (unsigned int i = 0; i < n; i++)
			data[i][i] = 1;
	}

	void print(void) {
		for (unsigned int i = 0; i < M; i++) {
			printf("[ ");

			for (unsigned int j = 0; j < N; j++)
				printf("%.3f\t", data[i][j]);

			printf(" ]\n");
		}
	}
};

template <unsigned int M, unsigned int N>
class __EXPORT Matrix : public MatrixBase<M, N>
{
public:
	using MatrixBase<M, N>::operator *;

	Matrix() : MatrixBase<M, N>() {}

	Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}

	Matrix(const float *d) : MatrixBase<M, N>(d) {}

	Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {}

	/**
	 * set to value
	 */
	const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
		memcpy(this->data, m.data, sizeof(this->data));
		return *this;
	}

	/**
	 * multiplication by a vector
	 */
	Vector<M> operator *(const Vector<N> &v) const {
#ifdef CONFIG_ARCH_ARM
		Vector<M> res;
		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
#else
		//probably nicer if this could go into a function like "eigen_mat_mult" or so
		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
				(this->arm_mat.pData);
		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
		Eigen::VectorXf Product = Me * Vec;
		Vector<M> res(Product.data());
#endif
		return res;
	}
};

template <>
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
{
public:
	using MatrixBase<3, 3>::operator *;

	Matrix() : MatrixBase<3, 3>() {}

	Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}

	Matrix(const float *d) : MatrixBase<3, 3>(d) {}

	Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {}

	/**
	 * set to value
	 */
	const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) {
		memcpy(this->data, m.data, sizeof(this->data));
		return *this;
	}

	/**
	 * multiplication by a vector
	 */
	Vector<3> operator *(const Vector<3> &v) const {
		Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
			      data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
			      data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
		return res;
	}

	/**
	 * create a rotation matrix from given euler angles
	 * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
	 */
	void from_euler(float roll, float pitch, float yaw) {
		float cp = cosf(pitch);
		float sp = sinf(pitch);
		float sr = sinf(roll);
		float cr = cosf(roll);
		float sy = sinf(yaw);
		float cy = cosf(yaw);

		data[0][0] = cp * cy;
		data[0][1] = (sr * sp * cy) - (cr * sy);
		data[0][2] = (cr * sp * cy) + (sr * sy);
		data[1][0] = cp * sy;
		data[1][1] = (sr * sp * sy) + (cr * cy);
		data[1][2] = (cr * sp * sy) - (sr * cy);
		data[2][0] = -sp;
		data[2][1] = sr * cp;
		data[2][2] = cr * cp;
	}

	/**
	 * get euler angles from rotation matrix
	 */
	Vector<3> to_euler(void) const {
		Vector<3> euler;
		euler.data[1] = asinf(-data[2][0]);

		if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
			euler.data[0] = 0.0f;
			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];

		} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
			euler.data[0] = 0.0f;
			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];

		} else {
			euler.data[0] = atan2f(data[2][1], data[2][2]);
			euler.data[2] = atan2f(data[1][0], data[0][0]);
		}

		return euler;
	}
};

}

#endif // MATRIX_HPP


/*
 * lorenz.cpp
 * Date: 2016-01-26
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <gpcxx/app.hpp>
#include <gpcxx/eval.hpp>
#include <gpcxx/evolve.hpp>
#include <gpcxx/tree.hpp>
#include <gpcxx/intrusive_nodes.hpp>
#include <gpcxx/operator.hpp>
#include <gpcxx/stat.hpp>
#include <gpcxx/generate.hpp>
#include <gpcxx/io.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/integrate/integrate_const.hpp>

#include <vector>
#include <array>
#include <cmath>

static const size_t dim = 3;
using state_type = std::array< double , dim >;
using stepper_type = boost::numeric::odeint::runge_kutta4< state_type > ;
using state_container = std::vector< state_type >;
using data_type = std::pair< state_container , state_container >;

using context_type = gpcxx::regression_context< double , dim >;
using node_type = gpcxx::intrusive_named_func_node< double , const context_type > ;
using tree_type = gpcxx::intrusive_tree< node_type >;
using individual_type = std::array< tree_type , dim >;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    const double sigma = 10.0;
    const double R = 28.0;
    const double b = 8.0 / 3.0;
    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

struct lorenz_reconstructed
{
    lorenz_reconstructed( individual_type individual ) : m_individual( std::move( individual ) ) {}
    void operator()( state_type const& x , state_type& dxdt , double t ) const
    {
        dxdt[0] = m_individual[0].root()->eval( x );
        dxdt[1] = m_individual[1].root()->eval( x );
        dxdt[2] = m_individual[2].root()->eval( x );
    }
    individual_type m_individual;
};

data_type generate_data( void )
{
    data_type data;
    state_type x {{ 10.0 , 10.0 , 10.0 }};
    state_type dxdt;
    double dt = 0.1;
    stepper_type stepper;
    for( double t = 0.0 ; t<100.0 ; t+= dt )
    {
        lorenz( x , dxdt , t );
        data.first.push_back( x );
        data.second.push_back( dxdt );
        stepper.do_step( lorenz , x , dxdt , t , dt );
    }
    return data;
}

void plot_data( data_type const& data )
{
    for( size_t i=0 ; i<data.first.size() ; ++i )
    {
        state_type const& x = data.first[i];
        state_type const& dxdt = data.second[i];
        std::cout << x[0] << " " << x[1] << " " << x[2] << " " << dxdt[0] << " " << dxdt[1] << " " << dxdt[2] << std::endl;
    }
}

std::array< std::pair< double , double > , dim > normalize_data( state_container& data )
{
    std::array< std::pair< double , double > , dim > means;
    for( size_t j=0 ; j<dim ; ++j )
    {
        means[j].first = means[j].second = 0.0;
    }

    for( size_t i=0 ; i<data.size() ; ++i )
    {
        assert( data[i].size() == dim );
        for( size_t j=0 ; j<dim ; ++j )
        {
            means[j].first += data[i][j];
            means[j].second += data[i][j] * data[i][j];
        }
    }
    double len = double( data.size() );
    for( size_t j=0 ; j<dim ; ++j )
    {
        double mean = means[j].first / len;
        double sqmean = means[j].second / len;
        means[j].first = mean;
        means[j].second = sqrt( sqmean - mean * mean );
    }
    for( size_t i=0 ; i<data.size() ; ++i )
    {
        for( size_t j=0 ; j<dim ; ++j )
        {
            data[i][j] = ( data[i][j] - means[j].first ) / means[j].second;
        }
    }

    return means;
}

template< typename Pop , typename Fitness >
void write_best_individuals( std::ostream &out , const Pop& p , const Fitness &f , size_t num_individuals , bool write_individual = false )
{
    std::vector< size_t > idx;
    gpcxx::sort_indices( f , idx );
    for( size_t i=0 ; i<num_individuals ; ++i )
    {
        if( write_individual )
        {
            out << i << " " << f[ idx[i] ] << " : \n";
            for( auto const& tree : p[ idx[i] ] )
                out << "\t" << gpcxx::simple( tree ) << "\n";
        }
        else
        {
            out << i << " " << f[ idx[i] ] << "\n";
        }
    }
    out << "\n" << std::flush;
}

bool is_number( double t )
{
    return ( ! std::isnan( t ) ) && ( ! std::isinf( t ) );
}

int main( int argc , char** argv )
{
    std::cout.precision( 14 );
    using namespace std;
    auto training_data = generate_data();
    auto xstat = normalize_data( training_data.first );
    auto ystat = normalize_data( training_data.second );
    for( size_t j=0 ; j<dim ; ++j )
        cout << xstat[j].first << " " << xstat[j].second << " " << ystat[j].first << " " << ystat[j].second << "\n";
    cout << endl;
    // plot_data( training_data );

    //[ define_the_tree
    using rng_type = std::mt19937;
    rng_type rng;
    //]

    //[ define_terminal_set
    auto erc_gen = gpcxx::make_intrusive_erc_generator< node_type >( []( auto& rng ) {
        std::normal_distribution<> dist( 0.0 , 1.0 );
        return dist( rng ); } );
    auto terminal_gen = gpcxx::make_uniform_symbol_erc< node_type >(
        std::vector< node_type >{
            node_type { gpcxx::array_terminal< 0 >{}                                 ,      "x" } ,
            node_type { gpcxx::array_terminal< 1 >{}                                 ,      "y" } ,
            node_type { gpcxx::array_terminal< 2 >{}                                 ,      "z" } } ,
        0.5 ,
        erc_gen );
    //]

    //[ define_function_set
//     auto unary_gen = gpcxx::make_uniform_symbol( std::vector< node_type > {
//         node_type { gpcxx::sin_func {}                                               ,      "s" } ,
//         node_type { gpcxx::cos_func {}                                               ,      "c" }
// //         node_type { gpcxx::exp_func {}                                               ,      "e" } ,
// //         node_type { gpcxx::log_func {}                                               ,      "l" }
//     } );

    auto binary_gen = gpcxx::make_uniform_symbol( std::vector< node_type > {
        node_type { gpcxx::plus_func {}                                              ,      "+" } ,
        node_type { gpcxx::minus_func {}                                             ,      "-" } ,
        node_type { gpcxx::multiplies_func {}                                        ,      "*" } ,
        node_type { gpcxx::divides_func {}                                           ,      "/" }
    } );
    //]

    //[ define_node_generator
    auto node_generator = gpcxx::node_generator< node_type , rng_type , 2 > {
        { 1.0 , 0 , terminal_gen } ,
//        { 1.0 , 1 , unary_gen } ,
        { 1.0 , 2 , binary_gen } };
    //]

    //[ define_gp_parameters
    size_t population_size = 512 * 32;
    size_t generation_size = 2000;
    size_t number_elite = 1;
    double mutation_rate = 0.2;
    double crossover_rate = 0.6;
    double reproduction_rate = 0.3;
    size_t min_tree_height = 1 , max_tree_height = 8;
    size_t tournament_size = 15;
    //]

    //[ define_population_and_fitness
    using population_type = std::vector< individual_type >;
    using fitness_type = std::vector< double >;

    fitness_type fitness ( population_size , 0.0 );
    population_type population ( population_size );
    //]

    //[ define_evolution
    using evolver_type = gpcxx::dynamic_pipeline< population_type , fitness_type , rng_type >;
    evolver_type evolver( rng , number_elite );
    //]

    //[define_evaluator
    auto evaluator = []( auto const& individual , auto const& context ) {
        return state_type {{
            individual[0].root()->eval( context ) ,
            individual[1].root()->eval( context ) ,
            individual[2].root()->eval( context ) }}; };
    auto distance = []( auto const& y , auto const& ytrain ) {
        const double weight_x = 1.0;
        const double weight_y = 1.0;
        const double weight_z = 1.0;
        state_type diff {{ y[0] - ytrain[0] , y[1] - ytrain[1] , y[2] - ytrain[2] }};
        return weight_x * diff[0] * diff[0] + weight_y * diff[1] * diff[1] + weight_z * diff[2] * diff[2]; };
    //]

    //[define_genetic_operators
    auto tree_generator = gpcxx::make_ramp( rng , node_generator , min_tree_height , max_tree_height , 0.5 );
    auto fitness_f1 = gpcxx::make_multi_regression_fitness( evaluator , distance );
    auto fitness_f = [ fitness_f1 ]( auto const& individual , auto const& x , auto const& y ) {
        return fitness_f1.get_chi2( individual , x , y ); };
    evolver.add_operator( gpcxx::make_multi_mutation(
            rng ,
            gpcxx::make_point_mutation( rng , tree_generator , max_tree_height , 20 ) ,
            gpcxx::make_tournament_selector( rng , tournament_size ) )
        , mutation_rate );
    evolver.add_operator( gpcxx::make_multi_crossover(
            rng ,
            gpcxx::make_one_point_crossover_strategy( rng , 10 ) ,
            gpcxx::make_tournament_selector( rng , tournament_size ) )
        , crossover_rate );
    evolver.add_operator( gpcxx::make_reproduce(
            gpcxx::make_tournament_selector( rng , tournament_size ) )
        , reproduction_rate );
    //]

    //[init_population
    std::ofstream fout( "evolution.dat" );
    for( size_t i=0 ; i<population.size() ; )
    {
        for( size_t j=0 ; j<dim ; ++j )
        {
            population[i][j].clear();
            tree_generator( population[i][j] );
        }
        fitness[i] = fitness_f( population[i] , training_data.first , training_data.second );
        if( ! is_number( fitness[i] ) )
        {
            continue;
        }
        ++i;
    }

    std::cout << "Initial population" << std::endl;
    write_best_individuals( std::cout , population , fitness , 10 );
    write_best_individuals( fout , population , fitness , 10 , true );
    //]

    //[main_loop
    for( size_t i=0 ; i<generation_size ; ++i )
    {
        evolver.next_generation( population , fitness );
        for( size_t i=0 ; i<population.size() ; ++i )
            fitness[i] = fitness_f( population[i] , training_data.first , training_data.second );

        std::cout << "Iteration " << i << std::endl;
        write_best_individuals( std::cout , population , fitness , 10 );
        write_best_individuals( fout , population , fitness , 10 , true );
    }
    //]

    return 0;
}