#include"main.hpp"
#include"Option.hpp"
#include"Mcmc.hpp"
#include"Pre.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/gamma_distribution.hpp>

boost::mt19937 r;

const double sigma2_y0 = 10000;

double quantile(const vector<double>& data,const double f)
{
    vector<double> sorted_data(data);
    sort(sorted_data.begin(),sorted_data.end());
    const double index = f * (data.size() - 1) ;
    const size_t lhs = static_cast<size_t>(index) ;
    const double delta = index - lhs ;
    double result;
    if (data.size() == 0) throw runtime_error("quantile");
    if (lhs == data.size() - 1) {
        result = sorted_data.at(lhs) ;
    } else {
        result = (1 - delta) * sorted_data.at(lhs) + delta * sorted_data.at(lhs + 1) ;
    }
    return result ;
}

bool Ey_fn(const int p, const int j, const vector<double>& y0i,const vector<double>& Ri, vector<double>& Ey_ij,const Mcmc& mc)
{
    Ey_ij.at(0) = y0i.at(j);
    for (int t=1;t<mc.op().nt();t++) {
        Ey_ij.at(t) = Ey_ij.at(t-1)
            + (mc.op().timep().at(t)-mc.op().timep().at(t-1))*(exp(mc.xx().at(p).at(j*mc.op().nt()+t-1))*Ri.at(t-1)-Ey_ij.at(t-1)*(1-Ri.at(t-1)));
        if (Ey_ij.at(t)<=0) return true;
    }
    return false;
}

double sum_sq(const int p, const int j, const vector<double>& y0i, const vector<double>& Ri,const Mcmc& mc)
{
    double sum2=0;
    vector<double> Ey_ij(mc.op().nt());
    if (Ey_fn(p,j,y0i,Ri,Ey_ij,mc)) return NAN;
    for (int t=0; t<mc.op().nt(); t++) for (unsigned q=0;q<mc.yy().at(p).size();q++) {
        sum2 += pow(mc.yy().at(p).at(q).at(j*mc.op().nt()+t)-log(Ey_ij.at(t)),2);
    }
    return sum2;
}

double sum_sq(const int p, const vector<double>& y0i, const vector<double>& Ri,const Mcmc& mc,const int s=0)
{
    double sum2=0;
    for (int j=0; j<mc.op().nrep(); j++) {
        bool flag=false;
        for (unsigned q=0;q<mc.yy().at(p).size();q++) if (obs(mc.yy().at(p).at(q).at(j*mc.op().nt()))) {
            flag=true;
            break;
        }
        if (flag) {
            vector<double> Ey_ij(mc.op().nt());
            if (Ey_fn(p,j,y0i,Ri,Ey_ij,mc)) return NAN;
            for (int t=s; t<mc.op().nt(); t++) for (unsigned q=0;q<mc.yy().at(p).size();q++) {
                sum2 += pow(mc.yy().at(p).at(q).at(j*mc.op().nt()+t)-log(Ey_ij.at(t)),2);
            }
        }
    }
    return sum2;
}

boost::random::normal_distribution<> ugaussian(0,1);
boost::uniform_real<> uniform(0,1);
void Mcmc::update_y0(const int p)
{
    for (int j=0;j<op().nrep();j++) {
        bool flag=false;
        for (unsigned q=0;q<yy().at(p).size();q++) if (obs(yy().at(p).at(q).at(j*op().nt()))) {
            flag=true;
            break;
        }
        if (flag) {
            vector<double> newy0p(y0().at(p));
            newy0p.at(j) *= exp(ugaussian(r));
            double logr = -sum_sq(p,j,newy0p,RR().at(p),*this)/tau2().at(p)/2;
            logr -= -sum_sq(p,j,y0().at(p),RR().at(p),*this)/tau2().at(p)/2;
            logr += -pow(log(newy0p.at(j)),2)/2/sigma2_y0;
            logr -= -pow(log(y0().at(p).at(j)),2)/2/sigma2_y0;
            if (logr>0 or uniform(r)<exp(logr)) {
                d_y0.at(p).at(j) = newy0p.at(j);
            }
        }
    }
}

void Mcmc::update_R(const int p)
{
    vector<double> newRp;
    int s=-1;
    for (int t=0;t<op().nt()-1;t++) {
        if (CPS().at(p).at(t)==1) {
            s = t+1;
            newRp = RR().at(p);
            newRp.at(t) = inv_logit(logit(RR().at(p).at(t))+ugaussian(r));
        } else {
            newRp.at(t) = newRp.at(t-1);
        }
        if (CPS().at(p).at(t+1)==0) continue;
        double logr = -sum_sq(p,y0().at(p),newRp,*this,s)/tau2().at(p)/2;
        logr -= -sum_sq(p,y0().at(p),RR().at(p),*this,s)/tau2().at(p)/2;
        logr += log(pq(newRp.at(t)))-log(pq(RR().at(p).at(t)));
        if (logr>0 or uniform(r)<exp(logr)) {
            d_RR.at(p) = newRp;
        }
    }
}

void Mcmc::update_CPS(const int p)
{
    const vector<double>& Rp=RR().at(p);
    for (int cp=1;cp<op().nt()-1;cp++) {
        vector<double> newRp(Rp);
        int l_time_index=-1, r_time_index=-1;
        for (int t=cp-1;l_time_index<0;t--) if (CPS().at(p).at(t)==1) l_time_index=t;
        for (int t=cp+1;r_time_index<0;t++) if (CPS().at(p).at(t)==1) r_time_index=t;
        const double l_time = op().timep().at(cp)-op().timep().at(l_time_index);
        const double r_time = op().timep().at(r_time_index)-op().timep().at(cp);
        const double t_time = op().timep().at(r_time_index)-op().timep().at(l_time_index);
        double logr=0;
        if (CPS().at(p).at(cp)==0) {
            const double u = uniform(r);
            const double newRleft = inv_logit(logit(Rp.at(cp))+r_time/t_time*logit(u));
            const double newRright = inv_logit(logit(Rp.at(cp))-l_time/t_time*logit(u));
            for (int t=l_time_index;t<cp;t++) newRp.at(t) = newRleft;
            for (int t=cp;t<r_time_index;t++) newRp.at(t) = newRright;
            if (d_prior) logr += d_varphi.at(p).at(cp);
            //if (d_prior) logr += varphi_fn(p,cp,*this,d_mo);
            logr += 2*log(newRleft*(1-newRright)+newRright*(1-newRleft))-log(pq(Rp.at(cp)));
        } else {
            const double mergedR = inv_logit((l_time*logit(Rp.at(cp-1))+r_time*logit(Rp.at(cp)))/t_time);
            for (int t=l_time_index;t<r_time_index;t++) newRp.at(t)=mergedR;
            if (d_prior) logr -= d_varphi.at(p).at(cp);
            //if (d_prior) logr -= varphi_fn(p,cp,*this,d_mo);
            logr -= 2*log(Rp.at(cp-1)*(1-Rp.at(cp))+Rp.at(cp)*(1-Rp.at(cp-1)))-log(pq(mergedR));
        }
        logr += -sum_sq(p,y0().at(p),newRp,*this,l_time_index+1)/tau2().at(p)/2;
        logr -= -sum_sq(p,y0().at(p),Rp,*this,l_time_index+1)/tau2().at(p)/2;
        if (logr>0 or uniform(r)<exp(logr)) {
            d_RR.at(p) = newRp;
            d_CPS.at(p).at(cp)=1-CPS().at(p).at(cp);
        }
    }
}

void Mcmc::tau_hyperparams()
{

        d_tau2.assign(yy().size(),0);
            for (unsigned p=0;p<yy().size();p++) {
                for (unsigned q=0; q<yy().at(p).size(); q++) {
                    for (int j=0;j<op().nrep();j++) {
                        vector<double> mu(op().nt()-1);
                        for (int t=0;t<op().nt()-1;t++) {
                            mu.at(t)=yy().at(p).at(q).at(j*op().nt()+t)+yy().at(p).at(q).at(j*op().nt()+t+1);
                            mu.at(t)/=2;
                        }
                        for (int t=1;t<op().nt()-1;t++) {
                            d_tau2.at(p)+=.5*pow(yy().at(p).at(q).at(j*op().nt()+t)-mu.at(t-1),2);
                            d_tau2.at(p)+=.5*pow(yy().at(p).at(q).at(j*op().nt()+t)-mu.at(t),2);
                        }
                        d_tau2.at(p)+=pow(yy().at(p).at(q).at(j*op().nt()+0)-mu.at(0),2);
                        const int t=op().nt()-2;
                        d_tau2.at(p)+=pow(yy().at(p).at(q).at(j*op().nt()+t)-mu.at(t),2);
                    }
                }
                d_tau2.at(p) /= op().nc()*yy().at(p).size();
            }
            const double m=quantile(d_tau2,.1);
            for (unsigned p=0;p<yy().size();p++) d_tau2.at(p) += m;
        ofstream var_ofs("varc.txt");
        copy(tau2().begin(),tau2().end(),ostream_iterator<double>(var_ofs,"\n"));
        const double mom1=accumulate(tau2().begin(),tau2().end(),0.)/tau2().size();
        double mom2=0;
        for (vector<double>::const_iterator it=tau2().begin();it!=tau2().end();it++) mom2 += (*it)*(*it);
        mom2 /= tau2().size();
        d_a_tau=(2*mom2-mom1*mom1)/(mom2-mom1*mom1);//MOM estimates
        d_b_tau=mom1*mom2/(mom2-mom1*mom1);         //MOM estimates
}

Mcmc::Mcmc(const Pre& pr) : d_op(pr.op()),d_prior(false),d_varphi(vector<vector<double> >()),d_xx(pr.xx()),d_yy(pr.yy())//,d_nobs(pr.nobs())
{
    Construct();
}

Mcmc::Mcmc(const Pre& pr,const vector<vector<double> >& varphi) : d_op(pr.op()),d_prior(true),d_varphi(varphi),d_xx(pr.xx()),d_yy(pr.yy())//,d_nobs(pr.nobs())
{
    Construct();
}

void Mcmc::Construct()
{
    d_y0.assign(yy().size(),vector<double>(op().nrep()));
    for (unsigned p=0;p<yy().size();p++) for (int j=0;j<op().nrep();j++) {
        double sum=0;
        for (unsigned q=0;q<yy().at(p).size();q++) sum+=yy().at(p).at(q).at(j*op().nt());
        d_y0.at(p).at(j)=exp(sum/yy().at(p).size());
    }
    d_RR.assign(yy().size(),vector<double> (op().nt(),.9));
    d_CPS.assign(yy().size(),vector<int> (op().nt(),1));
    d_CPSum.assign(yy().size(),vector<int> (op().nt()));

    tau_hyperparams();
    ofstream y0_ofs("s_y0");
    ofstream RR_ofs("s_RR");
    ofstream tau2_ofs("s_tau2");
    if (not tau2_ofs) throw runtime_error("can't open s_tau2");
    ofstream CPS_ofs("s_CPS");
    ofstream xRyD_ofs("s_xRyD");
    ofstream loglike_ofs("s_loglike");

    for (int iter=0,ns=0; ns<op().ns(); iter++) {

        if (iter%100==0) cout<<'\r'<<iter<<flush;
        for (unsigned p=0; p<yy().size(); p++) {
            update_y0(p);
            update_R(p);
            update_CPS(p);
        }

        if (iter>op().nburn() and iter%op().nthin()==0) {
            ns++;

            for (unsigned p=0;p<yy().size();p++) {
                tau2_ofs<<d_tau2.at(p)<<'\t';
                for (int j=0;j<op().nrep();j++) y0_ofs<<d_y0.at(p).at(j)<<'\t';
                for (int t=0;t<op().nt()-1;t++) {
                    RR_ofs<<d_RR.at(p).at(t)<<'\t';
                }
                for (int t=1;t<op().nt()-1;t++) {
                    CPS_ofs<<d_CPS.at(p).at(t)<<'\t';
                    d_CPSum.at(p).at(t)+=d_CPS.at(p).at(t);
                }

            }

            y0_ofs<<'\n';
            tau2_ofs<<'\n';
            RR_ofs<<'\n';
            CPS_ofs<<'\n';

            double loglike = 0;
            for (unsigned p=0;p<yy().size();p++) for (unsigned q=0;q<yy().at(p).size();q++) for (int j=0;j<op().nrep();j++) {
                if (obs(yy().at(p).at(q).at(j*op().nt()))) {
                    vector<double> Ey_ij(op().nt());
                    if (Ey_fn(p,j,d_y0.at(p),d_RR.at(p),Ey_ij,*this)) throw runtime_error("fit");
                    for (int t=0;t<op().nt();t++) {
                        xRyD_ofs << log(Ey_ij.at(t)) << '\t';
                        //likelihood
                        loglike += -log(d_tau2.at(p))-pow(yy().at(p).at(q).at(j*op().nt()+t)-log(Ey_ij.at(t)),2)/d_tau2.at(p)/2;
                    }
                } else for (int t=0;t<op().nt();t++) xRyD_ofs << "NA\t";
            }
            xRyD_ofs<<'\n';
            loglike_ofs << loglike << '\n';
        }
    }
    cout<<'\n';
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_MANTISSAMASK_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_MANTISSAMASK_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Mantissamask generic tag

     Represents the Mantissamask constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Mantissamask,double
                                , 0,0x807FFFFFUL,0x800FFFFFFFFFFFFFULL
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Mantissamask, Site> dispatching_Mantissamask(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Mantissamask, Site>();
   }
   template<class... Args>
   struct impl_Mantissamask;
  }
  /*!
    Generates a mask used to compute the mantissa of a floating point value

    @par Semantic:

    @code
    as_integer<T> r = Mantissamask<T>();
    @endcode

    @code
    if T is double
      r =  -2.225073858507200889e-308;
    else if T is float
      r =  -1.1754942106924410755e-38;
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Mantissamask, Mantissamask)
} }

#include <boost/simd/constant/common.hpp>

#endif


// Copyright (C) 1996-2016 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef MATH_POWER_HPP
#define MATH_POWER_HPP

#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/utility/enable_if.hpp>
#include <complex>

namespace math {

using boost::is_arithmetic;
using boost::enable_if;
using boost::disable_if;

namespace detail {

template<typename T>
struct power_traits {
  typedef T power_type;
};

template<typename T>
struct power_traits<std::complex<T> > {
  typedef T power_type;
};

} // end namespace detail

//
// function power2 and p2
//

#ifndef BOOST_NO_SFINAE

template<typename T>
typename detail::power_traits<T>::power_type
power2(T t, typename enable_if<is_arithmetic<T> >::type* = 0) { return t * t; }

template<typename T>
typename detail::power_traits<T>::power_type
power2(T const& t, typename disable_if<is_arithmetic<T> >::type* = 0) { return t * t; }

template<typename T>
typename detail::power_traits<T>::power_type
p2(T t, typename enable_if<is_arithmetic<T> >::type* = 0) { return power2(t); }

template<typename T>
typename detail::power_traits<T>::power_type
p2(T const& t, typename disable_if<is_arithmetic<T> >::type* = 0) { return power2(t); }

#else

template<typename T>
typename detail::power_traits<T>::power_type
power2(T const& t) { return t * t; }

template<typename T>
typename detail::power_traits<T>::power_type
p2(T const& t) { return power2(t); }

#endif

template<typename T>
typename detail::power_traits<std::complex<T> >::power_type
power2(std::complex<T> const& t) { return power2(real(t)) + power2(imag(t)); }

//
// function power3 and p3
//

#ifndef BOOST_NO_SFINAE

template<typename T>
typename detail::power_traits<T>::power_type
power3(T t, typename enable_if<is_arithmetic<T> >::type* = 0) { return t * t * t; }

template<typename T>
typename detail::power_traits<T>::power_type
power3(T const& t, typename disable_if<is_arithmetic<T> >::type* = 0) { return t * t * t; }

template<typename T>
typename detail::power_traits<T>::power_type
p3(T t, typename enable_if<is_arithmetic<T> >::type* = 0) { return power3(t); }

template<typename T>
typename detail::power_traits<T>::power_type
p3(T const& t, typename disable_if<is_arithmetic<T> >::type* = 0) { return power3(t); }

#else

template<typename T>
typename detail::power_traits<T>::power_type
power3(T const& t) { return t * t * t; }

template<typename T>
typename detail::power_traits<T>::power_type
p3(T const& t) { return power3(t); }

#endif

//
// function power4 and p4
//

#ifndef BOOST_NO_SFINAE

template<typename T>
typename detail::power_traits<T>::power_type
power4(T t, typename enable_if<is_arithmetic<T> >::type* = 0) { return power2(power2(t)); }

template<typename T>
typename detail::power_traits<T>::power_type
power4(T const& t, typename disable_if<is_arithmetic<T> >::type* = 0) { return power2(power2(t)); }

template<typename T>
typename detail::power_traits<T>::power_type
p4(T t, typename enable_if<is_arithmetic<T> >::type* = 0) { return power4(t); }

template<typename T>
typename detail::power_traits<T>::power_type
p4(T const& t, typename disable_if<is_arithmetic<T> >::type* = 0) { return power4(t); }

#else

template<typename T>
typename detail::power_traits<T>::power_type
power4(T const& t) { return power2(power2(t)); }

template<typename T>
typename detail::power_traits<T>::power_type
p4(T const& t) { return power4(t); }

#endif

//
// function power6 and p6
//

#ifndef BOOST_NO_SFINAE

template<typename T>
typename detail::power_traits<T>::power_type
power6(T t, typename enable_if<is_arithmetic<T> >::type* = 0) { return power3(power2(t)); }

template<typename T>
typename detail::power_traits<T>::power_type
power6(T const& t, typename disable_if<is_arithmetic<T> >::type* = 0) { return power3(power2(t)); }

template<typename T>
typename detail::power_traits<T>::power_type
p6(T t, typename enable_if<is_arithmetic<T> >::type* = 0) { return power6(t); }

template<typename T>
typename detail::power_traits<T>::power_type
p6(T const& t, typename disable_if<is_arithmetic<T> >::type* = 0) { return power6(t); }

#else

template<typename T>
typename detail::power_traits<T>::power_type
power6(T const& t) { return power3(power2(t)); }

template<typename T>
typename detail::power_traits<T>::power_type
p6(T const& t) { return power6(t); }

#endif

} // end namespace math

#endif // MATH_POWER_HPP


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_DIGEST_HPP
#define BOOST_HASH_DIGEST_HPP

#include <boost/array.hpp>
#include <boost/integer.hpp>
#include <boost/static_assert.hpp>

#include <string>

#include <cstring>

namespace boost {
namespace hashes {

unsigned const octet_bits = 8;
typedef uint_t<octet_bits>::least octet_type;

// Always stored internally as a sequence of octets in display order.
// This allows digests from different algorithms to have the same type,
// allowing them to be more easily stored and compared.
template <unsigned digest_bits_>
class digest : public array<octet_type, digest_bits_/octet_bits> {
  public:

    static unsigned const digest_bits = digest_bits_;
    BOOST_STATIC_ASSERT(digest_bits % octet_bits == 0);
    static unsigned const digest_octets = digest_bits/octet_bits;
    typedef array<octet_type, digest_octets> base_array_type;

    static unsigned const cstring_size = digest_bits/4 + 1;
    typedef array<char, cstring_size> cstring_type;

    digest() : base_array_type() {}
    digest(base_array_type const &a) : base_array_type(a) {}

    template <typename oit_T>
    oit_T to_ascii(oit_T it) const {
        for (unsigned j = 0; j < digest_octets; ++j) {
            octet_type b = base_array()[j];
            *it++ = "0123456789abcdef"[(b >> 4) & 0xF];
            *it++ = "0123456789abcdef"[(b >> 0) & 0xF];
        }
        return it;
    }

    std::string
    str() const {
        cstring_type cstr = cstring();
        return std::string(cstr.data(), cstr.size()-1);
    }

    cstring_type
    cstring() const {
        cstring_type s;
        char *p = to_ascii(s.data());
        *p++ = '\0';
        return s;
    }

    base_array_type const &base_array() const { return *this; }
};

template <unsigned NDB, unsigned ODB>
digest<NDB>
resize(digest<ODB> const &od) {
    digest<NDB> nd;
    unsigned bytes = sizeof(octet_type)*(NDB < ODB ? NDB : ODB)/octet_bits;
    std::memcpy(nd.data(), od.data(), bytes);
    return nd;
}

template <unsigned NDB, unsigned ODB>
digest<NDB>
truncate(digest<ODB> const &od) {
    BOOST_STATIC_ASSERT(NDB <= ODB);
    return resize<NDB>(od);
}

template <unsigned DB1, unsigned DB2>
bool operator==(digest<DB1> const &a, digest<DB2> const &b) {
    unsigned const DB = DB1 < DB2 ? DB2 : DB1;
    return resize<DB>(a).base_array() == resize<DB>(b).base_array();
}
template <unsigned DB1, unsigned DB2>
bool operator!=(digest<DB1> const &a, digest<DB2> const &b) {
    return !(a == b);
}

template <unsigned DB1, unsigned DB2>
bool operator<(digest<DB1> const &a, digest<DB2> const &b) {
    unsigned const DB = DB1 < DB2 ? DB2 : DB1;
    return resize<DB>(a).base_array() < resize<DB>(b).base_array();
}
template <unsigned DB1, unsigned DB2>
bool operator>(digest<DB1> const &a, digest<DB2> const &b) {
    return b < a;
}
template <unsigned DB1, unsigned DB2>
bool operator<=(digest<DB1> const &a, digest<DB2> const &b) {
    return !(b < a);
}
template <unsigned DB1, unsigned DB2>
bool operator>=(digest<DB1> const &a, digest<DB2> const &b) {
    return !(b > a);
}

template <unsigned DB>
bool operator!=(digest<DB> const &a, char const *b) {
    BOOST_ASSERT(std::strlen(b) == DB/4);
    return std::strcmp(a.cstring().data(), b);
}
template <unsigned DB>
bool operator==(digest<DB> const &a, char const *b) {
    return !(a != b);
}
template <unsigned DB>
bool operator!=(char const *b, digest<DB> const &a) {
    return a != b;
}
template <unsigned DB>
bool operator==(char const *b, digest<DB> const &a) {
    return a == b;
}

} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_DIGEST_HPP


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/math/unimodular_generator.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

namespace scitbx { namespace math {

namespace {

  struct unimodular_generator_wrappers
  {
    typedef unimodular_generator<int> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("unimodular_generator", no_init)
        .def(init<int const&>((arg("range"))))
        .def("at_end", &w_t::at_end)
        .def("next", &w_t::next)
        .def("count", &w_t::count)
      ;
    }
  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_unimodular_generator()
  {
    unimodular_generator_wrappers::wrap();
  }

}}} // namespace scitbx::math::boost_python


#ifndef STASTISTICS_H_
#define STASTISTICS_H_

#include <armadillo>

void moment_function_4th(const arma::vec &seq,
                         const int width,
                         arma::vec &momfunc4);

void cumulants_2nd(const arma::vec &seq,
                   const int width,
                   arma::vec &cum2);

void cumulants_4th(const arma::vec &seq,
                   const int n2,
                   const int n4,
                   arma::vec &cum4);

#endif


#include <boost/units/systems/si/solid_angle.hpp>


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_GTZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_GTZ_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-predicates
   This function object returns @ref True or @ref False according x is greater than @ref Zero or not.

    @par Header <boost/simd/function/is_gtz.hpp>

    @par Note

     Using `is_gtz(x)` is similar to: `x > 0`

    @par Example:

      @snippet is_gtz.cpp is_gtz

    @par Possible output:

      @snippet is_gtz.txt is_gtz

  **/
  as_logical_t<Value> is_gtz(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_gtz.hpp>
#include <boost/simd/function/simd/is_gtz.hpp>

#endif


#define BOOST_TEST_MODULE LocationCheckerTest

#include "LocationChecker.hpp"

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string.hpp>

BOOST_AUTO_TEST_SUITE(LocationCheckerSuite)

using namespace com::github::nimelo;
using namespace std;

BOOST_AUTO_TEST_CASE(PositiveTest)
{
    // Arrange
    LocationType x1 = 0, x2 = 4;
    MoveRatioType v1 = 3, v2 = 2;
    bool expected_will_meet = true;

    LocationChecker lc(x1, v1, x2, v2);
    // Act
    auto actual_will_meet = lc.willMeet();

    // Assert
    BOOST_CHECK_EQUAL(expected_will_meet, actual_will_meet);
}

BOOST_AUTO_TEST_CASE(NegativeTest)
{
    // Arrange
    LocationType x1 = 0, x2 = 5;
    MoveRatioType v1 = 2, v2 = 3;
    bool expected_will_meet = false;

    LocationChecker lc(x1, v1, x2, v2);
    // Act
    auto actual_will_meet = lc.willMeet();

    // Assert
    BOOST_CHECK_EQUAL(expected_will_meet, actual_will_meet);
}

BOOST_AUTO_TEST_CASE(NegativeTestTwo)
{
    // Arrange
    LocationType x1 = 28, x2 = 96;
    MoveRatioType v1 = 8, v2 = 2;
    bool expected_will_meet = false;

    LocationChecker lc(x1, v1, x2, v2);
    // Act
    auto actual_will_meet = lc.willMeet();

    // Assert
    BOOST_CHECK_EQUAL(expected_will_meet, actual_will_meet);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * Unit tests for random-graph generation utilities that are used for actual
 * testing.
 */

#include <vector>
#include <tuple>
#include <unordered_set>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/test/unit_test.hpp>

#include "utils.h"
#include "random_graph.h"

#define REPEAT(n) for (unsigned i__ = 0; i__ < (n); i__++)

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;
typedef VertexDesc<Graph> Vertex;
typedef VertexSizeT<Graph> VertexSz;

BOOST_AUTO_TEST_SUITE(RandomGraphGeneration)

/**
 * Ensure that graphs generated by gen_random_connected_graph() are simple i.e.
 * they don't have multiple parallel edges between any pair of vertices.
 */
BOOST_AUTO_TEST_CASE(random_connected_graph_is_simple) {
	REPEAT(10) {
		Graph g = gen_random_connected_graph<Graph>(100, 1);

		for (auto v : iter_vertices(g)) {
			auto neighbors = std::make_from_tuple<std::unordered_set<Vertex>>(boost::adjacent_vertices(v, g));
			BOOST_CHECK_EQUAL(neighbors.size(), boost::degree(v, g));
		}
	}
}

/**
 * Ensure that graphs generated by gen_random_connected_graph() are actually
 * connected.
 */
BOOST_AUTO_TEST_CASE(random_connected_graph_is_connected) {
	REPEAT(100) {
		Graph g = gen_random_connected_graph<Graph>(100, 0);

		std::unordered_map<Vertex, VertexSz> component(boost::num_vertices(g));
		auto map = boost::make_assoc_property_map(component);
		auto n_components = boost::connected_components(g, map);

		BOOST_CHECK_EQUAL(n_components, 1);
	}
}

/**
 * Ensure that graphs generated by gen_random_connected_graph() are a single
 * chain of nodes when the given edge probability is 0. This assumes that the
 * graph is connected (which is checked by the above test).
 */
BOOST_AUTO_TEST_CASE(random_connected_graph_can_be_single_chain) {
	REPEAT(10) {
		Graph g = gen_random_connected_graph<Graph>(500, 0);
		VertexSz degree_two = 0;
		VertexSz degree_one = 0;

		for (const auto v : iter_vertices(g)) {
			const auto deg = boost::degree(v, g);

			if (deg == 2)
				degree_two++;
			else if (deg == 1)
				degree_one++;
		}

		BOOST_CHECK_EQUAL(degree_one, 2);
		BOOST_CHECK_EQUAL(degree_two, boost::num_vertices(g) - 2);
	}
}

/**
 * Ensure that graphs generated by gen_random_chordal_graph() respect the
 * maximum number of edges specified as second argument.
 */
BOOST_AUTO_TEST_CASE(random_chordal_graph_respects_max_edges) {
	const unsigned max_e = 100 * 99 / 4;

	REPEAT(100) {
		Graph g = gen_random_chordal_graph<Graph>(100, max_e);
		BOOST_CHECK_LE(boost::num_edges(g), max_e);
	}
}

/**
 * Ensure that graphs generated by gen_random_connected_graph() are complete
 * when the given edge probability is 1.
 */
BOOST_AUTO_TEST_CASE(random_connected_graph_can_be_complete) {
	REPEAT(10) {
		Graph g = gen_random_connected_graph<Graph>(500, 1);
		const auto n_vertices = boost::num_vertices(g);

		for (auto v : iter_vertices(g))
			BOOST_CHECK_EQUAL(boost::degree(v, g), n_vertices - 1);
	}
}

/**
 * Ensure that graphs generated by gen_random_chordal_graph() are simple i.e.
 * they don't have multiple parallel edges between any pair of vertices.
 */
BOOST_AUTO_TEST_CASE(random_chordal_graph_is_simple) {
	REPEAT(10) {
		Graph g = gen_random_chordal_graph<Graph>(100, 3000);

		for (auto v : iter_vertices(g)) {
			auto neighbors = std::make_from_tuple<std::unordered_set<Vertex>>(boost::adjacent_vertices(v, g));
			BOOST_CHECK_EQUAL(neighbors.size(), boost::degree(v, g));
		}
	}
}

/**
 * Ensure that graphs generated by gen_random_chordal_graph() are connected.
 */
BOOST_AUTO_TEST_CASE(random_chordal_graph_is_connected) {
	REPEAT(100) {
		Graph g = gen_random_chordal_graph<Graph>(100, 3000);

		std::unordered_map<Vertex, VertexSz> component(boost::num_vertices(g));
		auto map = boost::make_assoc_property_map(component);
		auto n_components = boost::connected_components(g, map);

		BOOST_CHECK_EQUAL(n_components, 1);
	}
}

/**
 * Ensure that graphs generated by gen_random_chordal_graph() are actually
 * chordal. This is a brute force test of the all elimination orders.
 */
BOOST_AUTO_TEST_CASE(random_chordal_graph_is_chordal) {
	REPEAT(10) {
		Graph g = gen_random_chordal_graph<Graph>(8, 40);
		auto ordered = std::make_from_tuple<std::vector<Vertex>>(boost::vertices(g));
		bool ok;

		do {
			std::unordered_set<Vertex> removed;
			ok = true;

			for (const auto v : ordered) {
				for (const auto a : iter_neighbors(g, v)) {
					for (const auto b : iter_neighbors(g, v)) {
						if (removed.find(a) != removed.end() || removed.find(b) != removed.end())
							continue;

						if (a != b && !boost::edge(a, b, g).second)
							goto bad_order;
					}
				}

				removed.insert(v);
			}

			break;

		bad_order:
			ok = false;
		} while (boost::range::next_permutation(ordered)); // 8! = 40320

		BOOST_CHECK_MESSAGE(ok, "no order w/ empty fill-in => graph is not cordal");
	}
}

/**
 * Ensure that the order of vertices generated by gen_random_order() is a
 * bijection vertex<->index with index in [0, n_vertices - 1].
 */
BOOST_AUTO_TEST_CASE(random_vertex_order_is_bijection) {
	Graph g;

	for (unsigned i = 0; i < 100000; i++)
		boost::add_vertex(g);

	auto order = gen_random_order(g);
	std::unordered_set<Vertex> vertices(boost::num_vertices(g));
	std::unordered_set<VertexSz> indices(boost::num_vertices(g));

	for (size_t i = 0; i < order.size(); i++) {
		indices.insert(i);
		vertices.insert(order[i]);
	}

	BOOST_CHECK_EQUAL(vertices.size(), boost::num_vertices(g));
	BOOST_CHECK_EQUAL(indices.size(), boost::num_vertices(g));
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef EXAMPLES_UTILS_H_
#define EXAMPLES_UTILS_H_

#include <fstream>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>

#include <Eigen/Dense>

class Point3f{
public:
    Point3f(float x, float y, float z) : x(x), y(y), z(z){
    }

    float x, y, z;
};

template <typename PointT, typename ContainerT>
void readPointsDat(const std::string& filename, ContainerT& points)
{
    std::ifstream in(filename.c_str());
    std::string line;
    boost::char_separator<char> sep(" ");
    // read point cloud from "freiburg format"
    while (!in.eof())
    {
        std::getline(in, line);
        in.peek();

        boost::tokenizer<boost::char_separator<char> > tokenizer(line, sep);
        std::vector<std::string> tokens(tokenizer.begin(), tokenizer.end());

        if (tokens.size() != 6) continue;
        float x = boost::lexical_cast<float>(tokens[3]);
        float y = boost::lexical_cast<float>(tokens[4]);
        float z = boost::lexical_cast<float>(tokens[5]);

        points.push_back(PointT(x, y, z));
    }

    in.close();
}

template <typename PointT, typename ContainerT>
void readPointsTxt(const std::string& filename, ContainerT& points)
{
    std::ifstream in(filename.c_str());
    std::string line;
    boost::char_separator<char> sep(" ");
    // read point cloud from "freiburg format"
    while (!in.eof())
    {
        std::getline(in, line);
        in.peek();

        boost::tokenizer<boost::char_separator<char> > tokenizer(line, sep);
        std::vector<std::string> tokens(tokenizer.begin(), tokenizer.end());

        if (tokens.size() != 14) continue;
        float x = boost::lexical_cast<float>(tokens[0]);
        float y = boost::lexical_cast<float>(tokens[1]);
        float z = boost::lexical_cast<float>(tokens[2]);

        points.push_back(PointT(x, y, z));
    }

    in.close();
}

template <typename PointT, typename ContainerT>
void readPoints(const std::string& filename, ContainerT& points)
{
    std::string surfix = filename.substr(filename.size()-3, filename.size());
    if(std::strcmp(surfix.c_str(), "dat")==0){
        readPointsDat<PointT>(filename, points);
        return;
    }
    if(std::strcmp(surfix.c_str(), "txt")==0) {
        readPointsTxt<PointT>(filename, points);
        return;
    }

    std::cout<<"wrong file!!!!!!!!!!!!!!!!!!"<<std::endl;
}

void printPoints(const Eigen::MatrixXf points){
    std::cout<<"Point number: "<<points.rows()<<std::endl;
    for(size_t i=0;i<points.rows();++i){
        std::cout<<points(i, 0)<<", "<<points(i, 1)<<", "<<points(i, 2)<<std::endl;
    }

}

#endif /* EXAMPLES_UTILS_H_ */


/* RevKit: A Toolkit for Reversible Circuit Design (www.revkit.org)
 * Copyright (C) 2009-2011  The RevKit Developers <revkit@informatik.uni-bremen.de>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file fully_specified.hpp
 *
 * @brief Determines whether a truth_table is fully specified
 */

#ifndef FULLY_SPECIFIED_HPP
#define FULLY_SPECIFIED_HPP

#include <boost/foreach.hpp>

#include <core/truth_table.hpp>

/** @cond */
#define foreach BOOST_FOREACH
/** @endcond */

namespace revkit
{

  /**
   * @brief Returns whether a truth table is fully specified
   *
   * This function checks whether a truth table \p tt is full specified,
   * that is input dimension is equal to output dimension and there is
   * no don't care value \p dc_value in any of the assigments.
   *
   * Further, the truth table cannot be empty.
   *
   * @note A fully specified truth table does not imply that it is reversible
   *
   * @param tt       Truth table
   * @param dc_value The don't care value for the truth table
   * @param is_reversible If true, number of outputs has to match number of inputs. Default value is \b true.
   * @return true, if it is fully speciefied, otherwise false
   *
   * @author RevKit
   * @since  1.0
   */
  template<typename T>
  bool fully_specified( const truth_table<T>& tt, const typename truth_table<T>::value_type& dc_value, bool is_reversible = true )
  {
    if ( tt.num_inputs() == 0 )
    {
      return false;
    }

    if ( is_reversible && tt.num_inputs() != tt.num_outputs() )
    {
      return false;
    }

    if ( ( 1u << tt.num_inputs() ) != (unsigned)std::distance( tt.begin(), tt.end() ) )
    {
      return false;
    }

    for ( typename truth_table<T>::const_iterator it = tt.begin(); it != tt.end(); ++it )
    {
      if ( std::find( it->first.first, it->first.second, dc_value ) != it->first.second )
      {
        return false;
      }

      if ( std::find( it->second.first, it->second.second, dc_value ) != it->second.second )
      {
        return false;
      }
    }

    return true;
  }

  /**
   * @brief Returns whether a truth table is fully specified
   *
   * This function is specialized for a reversible_truth_table,
   * but calls the generic version with the don't care value.
   *
   * @param tt       Truth table
   * @param is_reversible If true, number of outputs has to match number of inputs. Default value is \b true.
   * @return true, if it is fully speciefied, otherwise false
   *
   * @author RevKit
   * @since  1.0
   */
  bool fully_specified( const binary_truth_table& tt, bool is_reversible = true );

}

#endif /* FULLY_SPECIFIED_HPP */


//==================================================================================================
/*!
  @file

  Copyright 2016 NumScale SAS
  Copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/is_real.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;
  using pl_t = bs::pack<bs::logical<T>, N>;

  T a1[N];
  bs::logical<T> b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = (i%2) ? T(i) : T(-i);
     b[i] = bs::is_real(a1[i]);
   }
  p_t aa1(&a1[0], &a1[N]);
  pl_t bb(&b[0], &b[N]);
  STF_IEEE_EQUAL(bs::is_real(aa1), bb);
}

STF_CASE_TPL("Check is_real on pack" , STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}


/*
	@author herumi

	tiny calculator 2
	This program generates a function to calc the value of
	polynomial given by user in run-time.
	use boost::spirit::qi
*/
#ifdef _WIN32
	#pragma warning(disable : 4127) // for boost(constant condition)
	#pragma warning(disable : 4512) // for boost
	#pragma warning(disable : 4819)
#endif
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_container.hpp>
#include <boost/spirit/include/phoenix_bind.hpp>
#include <boost/timer.hpp>

#include <stdio.h>
#include <assert.h>
#include <string>
#include <vector>
#define XBYAK_NO_OP_NAMES
#include "xbyak/xbyak.h"

enum Operand {
	OpAdd,
	OpSub,
	OpMul,
	OpDiv,
	OpNeg,
	OpImm,
	OpVarX
};

struct Code {
	Operand op_;
	double val_;
	Code(Operand op)
		: op_(op)
		, val_(0)
	{
	}
	Code(double val)
		: op_(OpImm)
		, val_(val)
	{
	}
};

typedef std::vector<Code> CodeSet;

struct Vm {
	CodeSet code_;
	double operator()(double x) const
	{
		const size_t maxStack = 16;
		double stack[maxStack];
		double *p = stack;
		CodeSet::const_iterator pc = code_.begin();

		while (pc != code_.end()) {
			switch (pc->op_) {
			case OpVarX:
				*p++ = x;
				break;
			case OpImm:
				*p++ = pc->val_;
				break;
			case OpNeg:
				p[-1] = -p[-1];
				break;
			case OpAdd:
				--p;
				p[-1] += p[0];
				break;
			case OpSub:
				--p;
				p[-1] -= p[0];
				break;
			case OpMul:
				--p;
				p[-1] *= p[0];
				break;
			case OpDiv:
				--p;
				p[-1] /= p[0];
				break;
			}
			++pc;
			assert(p < stack + maxStack);
		}
		return p[-1];
	}
};

class Jit : public Xbyak::CodeGenerator {
private:
	enum {
		MAX_CONST_NUM = 32
	};
	MIE_ALIGN(16) double constTbl_[MAX_CONST_NUM];
	Xbyak::uint64_t negConst_;
	size_t constTblPos_;
#ifdef XBYAK32
	const Xbyak::Reg32& varTbl_;
	const Xbyak::Reg32& tbl_;
#else
	const Xbyak::Reg64& tbl_;
#endif
	int regIdx_;
public:
	/*
		double jit(double x);
		@note 32bit: x : [esp+4], return fp0
		      64bit: x [rcx](win), xmm0(gcc), return xmm0
	*/
	Jit()
		: negConst_(Xbyak::uint64_t(1) << 63)
		, constTblPos_(0)
#ifdef XBYAK32
		, varTbl_(eax)
		, tbl_(edx)
#elif defined(XBYAK64_WIN)
		, tbl_(rcx)
#else
		, tbl_(rdi)
#endif
		, regIdx_(-1)
	{
#ifdef XBYAK32
		lea(varTbl_, ptr [esp+4]);
#else
#ifdef XBYAK64_WIN
		movaps(ptr [rsp + 8], xm6); // save xm6, xm7
		movaps(ptr [rsp + 8 + 16], xm7);
#endif
		movaps(xm7, xm0); // save xm0
#endif
		mov(tbl_, (size_t)constTbl_);
	}
	void genPush(double n)
	{
		if (constTblPos_ >= MAX_CONST_NUM) throw;
		constTbl_[constTblPos_] = n;
		if (regIdx_ == 7) throw;
		movsd(Xbyak::Xmm(++regIdx_), ptr[tbl_ + constTblPos_ * sizeof(double)]);
		constTblPos_++;
	}
	void genVarX()
	{
#ifdef XBYAK32
		if (regIdx_ == 7) throw;
		movsd(Xbyak::Xmm(++regIdx_), ptr[varTbl_]);
#else
		if (regIdx_ == 6) throw;
		movsd(Xbyak::Xmm(++regIdx_), xm7);
#endif
	}
	void genAdd()
	{
		addsd(Xbyak::Xmm(regIdx_ - 1), Xbyak::Xmm(regIdx_)); regIdx_--;
	}
	void genSub()
	{
		subsd(Xbyak::Xmm(regIdx_ - 1), Xbyak::Xmm(regIdx_)); regIdx_--;
	}
	void genMul()
	{
		mulsd(Xbyak::Xmm(regIdx_ - 1), Xbyak::Xmm(regIdx_)); regIdx_--;
	}
	void genDiv()
	{
		divsd(Xbyak::Xmm(regIdx_ - 1), Xbyak::Xmm(regIdx_)); regIdx_--;
	}
	void genNeg()
	{
		xorpd(Xbyak::Xmm(regIdx_), ptr [tbl_ + MAX_CONST_NUM * sizeof(double)]);
	}
	void complete()
	{
#ifdef XBYAK32
		sub(esp, 8);
		movsd(ptr [esp], xm0);
		fld(qword [esp]);
		add(esp, 8);
#else
#ifdef XBYAK64_WIN
		movaps(xm6, ptr [rsp + 8]);
		movaps(xm7, ptr [rsp + 8 + 16]);
#endif
#endif
		ret();
	}
};

template<typename Iterator>
struct Parser : boost::spirit::qi::grammar<Iterator, boost::spirit::ascii::space_type> {
	boost::spirit::qi::rule<Iterator, boost::spirit::ascii::space_type> expression, term, factor;
	CodeSet& code_;
	Parser(CodeSet& code)
		: Parser::base_type(expression)
		, code_(code)
	{
		namespace qi = boost::spirit::qi;
		using namespace qi::labels;

		using boost::phoenix::ref;
		using boost::phoenix::push_back;

		expression = term >> *(('+' > term[push_back(ref(code_), OpAdd)])
							 | ('-' > term[push_back(ref(code_), OpSub)]));

		term = factor >> *(('*' > factor[push_back(ref(code_), OpMul)])
						 | ('/' > factor[push_back(ref(code_), OpDiv)]));

		factor = qi::double_[push_back(ref(code_), _1)]
				| qi::lit('x')[push_back(ref(code_), OpVarX)]
				| ('(' > expression > ')')
				| ('-' > factor[push_back(ref(code_), OpNeg)])
				| ('+' > factor);
	}
};

template<typename Iterator>
struct ParserJit : boost::spirit::qi::grammar<Iterator, boost::spirit::ascii::space_type> {
	boost::spirit::qi::rule<Iterator, boost::spirit::ascii::space_type> expression, term, factor;
	Jit code_;
	ParserJit()
		: ParserJit::base_type(expression)
	{
		namespace qi = boost::spirit::qi;
		using namespace qi::labels;

		using boost::phoenix::ref;
		using boost::phoenix::push_back;
		using boost::phoenix::bind;

		expression = term >> *(('+' > term[bind(&Jit::genAdd, ref(code_))])
							 | ('-' > term[bind(&Jit::genSub, ref(code_))]));

		term = factor >> *(('*' > factor[bind(&Jit::genMul, ref(code_))])
						 | ('/' > factor[bind(&Jit::genDiv, ref(code_))]));

		factor = qi::double_[bind(&Jit::genPush, ref(code_), _1)]
				| qi::lit('x')[bind(&Jit::genVarX, ref(code_))]
				| ('(' > expression > ')')
				| ('-' > factor[bind(&Jit::genNeg, ref(code_))])
				| ('+' > factor);
	}
};

template<class Func>
void Test(const char *msg, const Func& f)
{
	printf("%s:", msg);
	boost::timer t;
	double sum = 0;
	for (double x = 0; x < 1000; x += 0.0001) {
		sum += f(x);
	}
	printf("sum=%f, %fsec\n", sum, t.elapsed());
}

int main(int argc, char *argv[])
{
	if (argc < 2) {
		fprintf(stderr, "input formula\n");
		return 1;
	}
	const std::string str(argv[1]);

	try {
		Vm vm;
		Parser<std::string::const_iterator> parser(vm.code_);
		ParserJit<std::string::const_iterator> parserJit;

		const std::string::const_iterator end = str.end();

		std::string::const_iterator i = str.begin();
		if (!phrase_parse(i, end, parser, boost::spirit::ascii::space) || i != end) {
			puts("err 1");
			return 1;
		}
		printf("ret=%f\n", vm(2.3));

		i = str.begin();
		if (!phrase_parse(i, end, parserJit, boost::spirit::ascii::space) || i != end) {
			puts("err 2");
			return 1;
		}
		parserJit.code_.complete();
		double (*jit)(double) = parserJit.code_.getCode<double (*)(double)>();

		Test("VM ", vm);
		Test("JIT", jit);
	} catch (...) {
		fprintf(stderr, "err\n");
	}
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_CORE_TOPOLOGICAL_DIMENSION_HPP
#define BOOST_GEOMETRY_CORE_TOPOLOGICAL_DIMENSION_HPP

#include <boost/mpl/int.hpp>

#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace core_dispatch
{

template <typename GeometryTag>
struct top_dim {};

template <>
struct top_dim<point_tag>      : boost::mpl::int_<0> {};

template <>
struct top_dim<linestring_tag> : boost::mpl::int_<1> {};

template <>
struct top_dim<segment_tag>    : boost::mpl::int_<1> {};

// ring: topological dimension of two, but some people say: 1 !!
template <>
struct top_dim<ring_tag>       : boost::mpl::int_<2> {};

template <>
struct top_dim<box_tag>        : boost::mpl::int_<2> {};

template <>
struct top_dim<polygon_tag>    : boost::mpl::int_<2> {};

} // namespace core_dispatch
#endif

/*!
    \brief Meta-function returning the topological dimension of a geometry
    \details The topological dimension defines a point as 0-dimensional,
        a linestring as 1-dimensional,
        and a ring or polygon as 2-dimensional.
    \see http://www.math.okstate.edu/mathdept/dynamics/lecnotes/node36.html
    \ingroup core
*/
template <typename Geometry>
struct topological_dimension
    : core_dispatch::top_dim<typename tag<Geometry>::type> {};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_CORE_TOPOLOGICAL_DIMENSION_HPP


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

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

 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file    timeIncremental.cpp
 * @brief   Overall timing tests for incremental solving
 * @author  Richard Roberts
 */

#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/base/timing.h>

#include <fstream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
#include <boost/range/adaptor/reversed.hpp>

using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;

typedef Pose2 Pose;

typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR;

//GTSAM_VALUE_EXPORT(Value);
//GTSAM_VALUE_EXPORT(Pose);
//GTSAM_VALUE_EXPORT(NonlinearFactor);
//GTSAM_VALUE_EXPORT(NoiseModelFactor);
//GTSAM_VALUE_EXPORT(NM1);
//GTSAM_VALUE_EXPORT(NM2);
//GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
//GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
//GTSAM_VALUE_EXPORT(BR);
//GTSAM_VALUE_EXPORT(noiseModel::Base);
//GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
//GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
//GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
//GTSAM_VALUE_EXPORT(noiseModel::Unit);

double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
  // Compute degrees of freedom (observations - variables)
  // In ocaml, +1 was added to the observations to account for the prior, but
  // the factor graph already includes a factor for the prior/equality constraint.
  //  double dof = graph.size() - config.size();
  int graph_dim = 0;
  for(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
    graph_dim += nlf->dim();
  }
  double dof = graph_dim - config.dim(); // kaess: changed to dim
  return 2. * graph.error(config) / dof; // kaess: added factor 2, graph.error returns half of actual error
}

int main(int argc, char *argv[]) {

  cout << "Loading data..." << endl;

  gttic_(Find_datafile);
  //string datasetFile = findExampleDataFile("w10000-odom");
  string datasetFile = findExampleDataFile("victoria_park");
  std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
    load2D(datasetFile);
  gttoc_(Find_datafile);

  NonlinearFactorGraph measurements = *data.first;
  Values initial = *data.second;

  cout << "Playing forward time steps..." << endl;

  ISAM2 isam2;

  size_t nextMeasurement = 0;
  for(size_t step=1; nextMeasurement < measurements.size(); ++step) {

    Values newVariables;
    NonlinearFactorGraph newFactors;

    // Collect measurements and new variables for the current step
    gttic_(Collect_measurements);
    if(step == 1) {
      //      cout << "Initializing " << 0 << endl;
      newVariables.insert(0, Pose());
      // Add prior
      newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(3)));
    }
    while(nextMeasurement < measurements.size()) {

      NonlinearFactor::shared_ptr measurementf = measurements[nextMeasurement];

      if(BetweenFactor<Pose>::shared_ptr measurement =
        boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
      {
        // Stop collecting measurements that are for future steps
        if(measurement->key1() > step || measurement->key2() > step)
          break;

        // Require that one of the nodes is the current one
        if(measurement->key1() != step && measurement->key2() != step)
          throw runtime_error("Problem in data file, out-of-sequence measurements");

        // Add a new factor
        newFactors.push_back(measurement);

        // Initialize the new variable
        if(measurement->key1() == step && measurement->key2() == step-1) {
          if(step == 1)
            newVariables.insert(step, measurement->measured().inverse());
          else {
            Pose prevPose = isam2.calculateEstimate<Pose>(step-1);
            newVariables.insert(step, prevPose * measurement->measured().inverse());
          }
          //        cout << "Initializing " << step << endl;
        } else if(measurement->key2() == step && measurement->key1() == step-1) {
          if(step == 1)
            newVariables.insert(step, measurement->measured());
          else {
            Pose prevPose = isam2.calculateEstimate<Pose>(step-1);
            newVariables.insert(step, prevPose * measurement->measured());
          }
          //        cout << "Initializing " << step << endl;
        }
      }
      else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
        boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
      {
        Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];

        // Stop collecting measurements that are for future steps
        if(poseKey > step)
          throw runtime_error("Problem in data file, out-of-sequence measurements");

        // Add new factor
        newFactors.push_back(measurement);

        // Initialize new landmark
        if(!isam2.getLinearizationPoint().exists(lmKey))
        {
          Pose pose = isam2.calculateEstimate<Pose>(poseKey);
          Rot2 measuredBearing = measurement->measured().first;
          double measuredRange = measurement->measured().second;
          newVariables.insert(lmKey,
            pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
        }
      }
      else
      {
        throw std::runtime_error("Unknown factor type read from data file");
      }
      ++ nextMeasurement;
    }
    gttoc_(Collect_measurements);

    // Update iSAM2
    gttic_(Update_ISAM2);
    isam2.update(newFactors, newVariables);
    gttoc_(Update_ISAM2);

    if(step % 100 == 0) {
      gttic_(chi2);
      Values estimate(isam2.calculateEstimate());
      double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate);
      cout << "chi2 = " << chi2 << endl;
      gttoc_(chi2);
    }

    tictoc_finishedIteration_();

    if(step % 1000 == 0) {
      cout << "Step " << step << endl;
      tictoc_print_();
    }
  }

  //try {
  //  {
  //    std::ofstream writerStream("incremental_init", ios::binary);
  //    boost::archive::binary_oarchive writer(writerStream);
  //    writer << isam2.calculateEstimate();
  //    writerStream.close();
  //  }
  //  {
  //    std::ofstream writerStream("incremental_graph", ios::binary);
  //    boost::archive::binary_oarchive writer(writerStream);
  //    writer << isam2.getFactorsUnsafe();
  //    writerStream.close();
  //  }
  //} catch(std::exception& e) {
  //  cout << e.what() << endl;
  //}

  NonlinearFactorGraph graph;
  Values values;

  //{
  //  std::ifstream readerStream("incremental_init", ios::binary);
  //  boost::archive::binary_iarchive reader(readerStream);
  //  reader >> values;
  //}
  //{
  //  std::ifstream readerStream("incremental_graph", ios::binary);
  //  boost::archive::binary_iarchive reader(readerStream);
  //  reader >> graph;
  //}

  graph = isam2.getFactorsUnsafe();
  values = isam2.calculateEstimate();

  // Compute marginals
  try {
    Marginals marginals(graph, values);
    int i=0;
    for (Key key1: boost::adaptors::reverse(values.keys())) {
      int j=0;
      for (Key key2: boost::adaptors::reverse(values.keys())) {
        if(i != j) {
          gttic_(jointMarginalInformation);
          std::vector<Key> keys(2);
          keys[0] = key1;
          keys[1] = key2;
          JointMarginal info = marginals.jointMarginalInformation(keys);
          gttoc_(jointMarginalInformation);
          tictoc_finishedIteration_();
        }
        ++j;
        if(j >= 50)
          break;
      }
      ++i;
      if(i >= 50)
        break;
    }
    tictoc_print_();
    for(Key key: values.keys()) {
      gttic_(marginalInformation);
      Matrix info = marginals.marginalInformation(key);
      gttoc_(marginalInformation);
      tictoc_finishedIteration_();
      ++i;
    }
  } catch(std::exception& e) {
    cout << e.what() << endl;
  }
  tictoc_print_();

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
#define CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_

#include <boost/function.hpp>
#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/core/residual-base.hpp"

namespace crocoddyl {

/**
 * @brief This class computes the numerical differentiation of a residual model.
 *
 * It computes Jacobian of the residual model via numerical differentiation, i.e., \f$\mathbf{R_x}\f$
 * and \f$\mathbf{R_u}\f$ which denote the Jacobians of the residual function
 * \f$\mathbf{r}(\mathbf{x},\mathbf{u})\f$.
 *
 * \sa `ResidualModelAbstractTpl()`, `calcDiff()`
 */
template <typename _Scalar>
class ResidualModelNumDiffTpl : public ResidualModelAbstractTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
  typedef ResidualModelAbstractTpl<Scalar> Base;
  typedef ResidualDataNumDiffTpl<Scalar> Data;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
  typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;

  /**
   * @brief Initialize the numdiff residual model
   *
   * @param model  Residual model that we want to apply the numerical differentiation
   */
  explicit ResidualModelNumDiffTpl(const boost::shared_ptr<Base>& model);

  /**
   * @brief Initialize the numdiff residual model
   */
  virtual ~ResidualModelNumDiffTpl();

  /**
   * @brief @copydoc Base::calc()
   */
  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief @copydoc Base::calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>&
   * x)
   */
  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief @copydoc Base::calcDiff()
   */
  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief @copydoc Base::calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
   * VectorXs>& x)
   */
  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief @copydoc Base::createData()
   */
  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);

  /**
   * @brief Return the original residual model
   */
  const boost::shared_ptr<Base>& get_model() const;

  /**
   * @brief Return the disturbance value used by the numdiff routine
   */
  const Scalar get_disturbance() const;

  /**
   * @brief Modify the disturbance value used by the numdiff routine
   */
  void set_disturbance(const Scalar disturbance);

  /**
   * @brief Register functions that updates the shared data computed for a system rollout
   * The updated data is used to evaluate of the gradient and hessian.
   *
   * @param reevals are the registered functions.
   */
  void set_reevals(const std::vector<ReevaluationFunction>& reevals);

 protected:
  using Base::nu_;
  using Base::state_;
  using Base::unone_;

 private:
  /**
   * @brief Make sure that when we finite difference the residual model, the user
   * does not face unknown behaviour because of the finite differencing of a
   * quaternion around pi. This behaviour might occur if ResidualModelState and
   * FloatingInContact differential model are used together.
   *
   * For full discussions see issue
   * https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
   *
   * @param x is the state at which the check is performed.
   */
  void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);

  boost::shared_ptr<Base> model_;              //!< Residual model hat we want to apply the numerical differentiation
  Scalar disturbance_;                         //!< Disturbance used in the numerical differentiation routine
  std::vector<ReevaluationFunction> reevals_;  //!< Functions that needs execution before calc or calcDiff
};

template <typename _Scalar>
struct ResidualDataNumDiffTpl : public ResidualDataAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ResidualDataAbstractTpl<Scalar> Base;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;

  /**
   * @brief Initialize the numdiff residual data
   *
   * @tparam Model is the type of the `ResidualModelAbstractTpl`.
   * @param model is the object to compute the numerical differentiation from.
   */
  template <template <typename Scalar> class Model>
  explicit ResidualDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
      : Base(model, shared_data),
        dx(model->get_state()->get_ndx()),
        xp(model->get_state()->get_nx()),
        du(model->get_nu()),
        up(model->get_nu()) {
    dx.setZero();
    xp.setZero();
    du.setZero();
    up.setZero();

    const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
    const std::size_t& nu = model->get_model()->get_nu();
    data_0 = model->get_model()->createData(shared_data);
    for (std::size_t i = 0; i < ndx; ++i) {
      data_x.push_back(model->get_model()->createData(shared_data));
    }
    for (std::size_t i = 0; i < nu; ++i) {
      data_u.push_back(model->get_model()->createData(shared_data));
    }
  }

  virtual ~ResidualDataNumDiffTpl() {}

  using Base::r;
  using Base::Ru;
  using Base::Rx;
  using Base::shared;

  VectorXs dx;  //!< State disturbance.
  VectorXs xp;  //!< The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
  VectorXs du;  //!< Control disturbance.
  VectorXs up;  //!< The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
  boost::shared_ptr<Base> data_0;                //!< The data at the approximation point.
  std::vector<boost::shared_ptr<Base> > data_x;  //!< The temporary data associated with the state variation.
  std::vector<boost::shared_ptr<Base> > data_u;  //!< The temporary data associated with the control variation.
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/numdiff/residual.hxx"

#endif  // CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * 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 <iostream>
#include <sstream>
#include <iomanip>
#include <cassert>

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

#include "NGLMExt.hpp"
#include "NTris.hpp"

void NTris::add( const glm::vec3& a, const glm::vec3& b, const glm::vec3& c)
{
    unsigned i = verts.size(); verts.push_back(a);
    unsigned j = verts.size(); verts.push_back(b);
    unsigned k = verts.size(); verts.push_back(c);
    glm::uvec3 tri(i,j,k);
    tris.push_back(tri);
}

unsigned NTris::get_num_tri() const
{
    return tris.size();
}

unsigned NTris::get_num_vert() const
{
    return verts.size();
}

void NTris::get_vert( unsigned i, glm::vec3& v ) const
{
    v = verts[i] ;
}
void NTris::get_normal( unsigned /*i*/, glm::vec3& n ) const
{
    n.x = 0  ;
    n.y = 0  ;
    n.z = 0  ;
}
void NTris::get_uv( unsigned /*i*/, glm::vec3& uv ) const
{
    uv.x = 0  ;
    uv.y = 0  ;
    uv.z = 0  ;
}

void NTris::get_tri(unsigned i, glm::uvec3& t) const
{
    t = tris[i] ;
}
void NTris::get_tri(unsigned i, glm::uvec3& t, glm::vec3& a, glm::vec3& b, glm::vec3& c ) const
{
    t = tris[i] ;
    a = verts[t.x];
    b = verts[t.y];
    c = verts[t.z];
}

std::string NTris::brief() const
{
    std::stringstream ss ;

    ss << "NTris"
       << " nf " << std::setw(5) << tris.size()
       << " nv " << std::setw(5) << verts.size()
       ;

    return ss.str();
}

void NTris::dump(const char* msg) const
{
    std::cout << msg << " " << brief() << std::endl ;

    glm::uvec3 t ;
    glm::vec3 a ;
    glm::vec3 b ;
    glm::vec3 c ;

    unsigned ntri = get_num_tri();
    for(unsigned i=0 ; i < ntri ; i++)
    {
        get_tri(i, t, a,b,c );

        std::cout << " t " << std::setw(20) << glm::to_string(t)
                  << " a " << std::setw(20) << glm::to_string(a)
                  << " b " << std::setw(20) << glm::to_string(b)
                  << " c " << std::setw(20) << glm::to_string(c)
                  << std::endl
                  ;

    }

}

NTris* NTris::make_sphere( unsigned n_polar, unsigned n_azimuthal, float ctmin, float ctmax )
{

    /*

                                  t0             t1            ct0

            t = 0                  0             1/n_polar      +1

            t = n_polar - 1      1 - 1/n_polar     1            -1

                 -  ct0
                    max_exclude:  ct1 > zmax
                 -  ct1               - ct0
    zmax     +-----------------+        max_straddle
            /    -              \     - ct1
           /                     \
          /      -                \   - ct0
    zmin +-------------------------+    min_straddle
                 - ct0                - ct1
                   min_exclude:  ct0 < zmin
                 - ct1

    */

    assert(ctmax > ctmin && ctmax <= 1.f && ctmin >= -1.f);

    NTris* tris = new NTris ;

    float pi = boost::math::constants::pi<float>() ;

    for(unsigned t=0 ; t < n_polar ; t++)
    {
        double t0 = 1.0f*pi*float(t)/n_polar ;
        double t1 = 1.0f*pi*float(t+1)/n_polar ;

        double st0,st1,ct0,ct1 ;
        sincos_<double>(t0, st0, ct0 );
        sincos_<double>(t1, st1, ct1 );
        assert( ct0 > ct1 );

        bool max_exclude  = ct1 > ctmax ;
        bool max_straddle = ctmax <= ct0 && ctmax > ct1 ;
        bool min_straddle = ctmin <= ct0 && ctmin > ct1 ;
        bool min_exclude  = ct0 < ctmin ;

        if(max_exclude || min_exclude )
        {
            continue ;
        }
        else if(max_straddle)
        {
            sincos_<double>(acos(ctmax), st0, ct0 );
        }
        else if(min_straddle)
        {
            sincos_<double>(acos(ctmin), st1, ct1 );
        }

        for(unsigned p=0 ; p < n_azimuthal ; p++)
        {
            float p0 = 2.0f*pi*float(p)/n_azimuthal ;
            float p1 = 2.0f*pi*float(p+1)/n_azimuthal ;

            double sp0,sp1,cp0,cp1 ;
            sincos_<double>(p0, sp0, cp0 );
            sincos_<double>(p1, sp1, cp1 );

            glm::vec3 x00( st0*cp0, st0*sp0, ct0 );
            glm::vec3 x10( st0*cp1, st0*sp1, ct0 );

            glm::vec3 x01( st1*cp0, st1*sp0, ct1 );
            glm::vec3 x11( st1*cp1, st1*sp1, ct1 );

            if( t == 0 || t == n_polar - 1)
            {
                tris->add(x00,x01,x11);
            }
            else
            {
                tris->add(x00,x01,x10);
                tris->add(x10,x01,x11);
            }
        }
    }
    return tris ;
}


#include "WSPRLog.hxx"

#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <list>
#include <math.h>
#include <set>

class myWSPRLog : public WSPRLog {
public:
  myWSPRLog(WSPRLogEntry::Field _sel) : WSPRLog() {
    sel = _sel;
  }

  bool processEntry(WSPRLogEntry * ent) {
    int el;

    ent->getField(sel, el);

    if(histogram.find(el) == histogram.end()) {
      histogram[el] = 1;
    }
    else {
      histogram[el] += 1;
    }

    return false;
  }

  void printHistogram(std::ostream & os) {
    // sum the samples..
    int sum = 0;
    for(auto he: histogram) {
      sum += he.second;
    }

    double fsum = ((double) sum);
    double cdf = 0.0;

    for(auto he: histogram) {
      double v =((double) he.second);
      double pdf = v / fsum;
      cdf += pdf;
      os << boost::format("%d %d %f %f\n")
	% he.first % he.second % pdf % cdf;
    }
  }
private:
  std::map<int, int> histogram;
  WSPRLogEntry::Field sel;
};

int main(int argc, char * argv[])
{
  std::string in_name, out_name, field_selector;
  bool input_gzipped;
  namespace po = boost::program_options;

  po::options_description desc("Options:");

  desc.add_options()
    ("help", "help message")
    ("log", po::value<std::string>(&in_name)->required(), "Input log file (csv) in WSPR log format")
    ("out", po::value<std::string>(&out_name)->required(), "Histogram table suitable for gnuplot")
    ("field", po::value<std::string>(&field_selector)->required(), "Numeric field to use for histogram buckets")
    ("igz", po::value<bool>(&input_gzipped)->default_value(false), "if true, input file is gzip compressed");

  po::positional_options_description pos_opts ;
  pos_opts.add("log", 1);
  pos_opts.add("out", 1);
  pos_opts.add("multi", 1);

  po::variables_map vm;

  std::string what_am_i("Build histogram, pdf, cdf, from log file and selected field\n");

  try {
    po::store(po::command_line_parser(argc, argv).options(desc)
	      .positional(pos_opts).run(), vm);

    if(vm.count("help")) {
      std::cout << what_am_i
		<< desc << std::endl;
      exit(-1);
    }

    po::notify(vm);
  }
  catch(po::required_option & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }
  catch(po::error & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }

  WSPRLogEntry::Field sel;
  sel = WSPRLogEntry::str2Field(field_selector);
  if(sel == WSPRLogEntry::UNDEFINED) {
    std::cerr << "Bad field selected.\n";
    WSPRLogEntry::printFieldChoices(std::cerr);
    exit(-1);
  }

  myWSPRLog wlog(sel);

  wlog.readLog(in_name, input_gzipped);

  std::ofstream ofs(out_name);
  wlog.printHistogram(ofs);
}


#include <algorithm>
#include <fstream>
#include <string>
#include <cstdint>
#include <algorithm>
#include <mutex>          // std::call_once, std::once_flag
#include <cmath> // round()
#include <boost/function_output_iterator.hpp>

#include "LZJD.h"
#include "MurmurHash3.h"

using namespace std;

#ifdef __cplusplus
extern "C" {
#endif

LZJD::LZJD() {
}

LZJD::~LZJD() {
}

std::vector<int32_t> getAllHashes(std::vector<char>& bytes)
{
    std::vector<int32_t> ints;

    std::unordered_set<int32_t> x_set;
    MurmurHash3 running_hash = MurmurHash3();

    for(char b : bytes)
    {
        int8_t some_byte = b; //wherever you want to get this
        int32_t hash = running_hash.pushByte(some_byte);

        if (x_set.insert(hash).second)
        {
            //was successfully added, so never seen it before. Put it in!
            ints.push_back(hash);
            running_hash.reset();
        }

    }

    return ints;
}

std::vector<int32_t> digest(uint64_t k, std::vector<char>& bytes)
{
    std::vector<int32_t> ints = getAllHashes(bytes);

    if(ints.size() > k)
    {
        std::nth_element (ints.begin(), ints.begin()+k, ints.end());
        ints.resize(k);
        std::sort(ints.begin(), ints.end());
    }
    else
    {
        std::sort(ints.begin(), ints.end());
        ints.resize(k);
    }

    return ints;
}

int32_t similarity(const std::vector<int32_t>& x_minset, const std::vector<int32_t>& y_minset)
{
    int32_t same = 0;
    vector<int32_t> v3;
    set_intersection(x_minset.begin(),x_minset.end(),
                     y_minset.begin(),y_minset.end(),
                     back_inserter(v3));
    same = v3.size();
    double sim = same / (double) (x_minset.size() + y_minset.size() - same);
    return (int) (round(100*sim));
}

#ifdef __cplusplus
}
#endif


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

#include <boost/hana/orderable/less_mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/orderable.hpp>
using namespace boost::hana;

template <typename mcd>
void test() {
    constexpr auto ord = detail::minimal::orderable<mcd>;

    BOOST_HANA_CONSTEXPR_LAMBDA auto ord_ = [=](auto i) {
        return [=] { return ord(i); };
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto less_ = ordering([](auto x) {
        return x();
    });

    BOOST_HANA_CONSTEXPR_ASSERT(less_(ord_(0), ord_(1)) == less(ord(0), ord(1)));
    BOOST_HANA_CONSTEXPR_ASSERT(less_(ord_(0), ord_(0)) == less(ord(0), ord(0)));
    BOOST_HANA_CONSTEXPR_ASSERT(less_(ord_(1), ord_(0)) == less(ord(1), ord(0)));
}

int main() {
    test<Orderable::less_mcd>();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_CORE_CS_HPP
#define BOOST_GEOMETRY_CORE_CS_HPP

#include <cstddef>

#include <boost/mpl/assert.hpp>
#include <boost/type_traits/integral_constant.hpp>

#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/tags.hpp>

namespace boost { namespace geometry
{

/*!
\brief Unit of plane angle: Degrees
\details Tag defining the unit of plane angle for spherical coordinate systems.
    This tag specifies that coordinates are defined in degrees (-180 .. 180).
    It has to be specified for some coordinate systems.
\qbk{[include reference/core/degree_radian.qbk]}
*/
struct degree {};

/*!
\brief Unit of plane angle: Radians
\details Tag defining the unit of plane angle for spherical coordinate systems.
    This tag specifies that coordinates are defined in radians (-PI .. PI).
    It has to be specified for some coordinate systems.
\qbk{[include reference/core/degree_radian.qbk]}
*/
struct radian {};

#ifndef DOXYGEN_NO_DETAIL
namespace core_detail
{

template <typename DegreeOrRadian>
struct coordinate_system_units
{
    BOOST_MPL_ASSERT_MSG
        ((false),
         COORDINATE_SYSTEM_UNITS_MUST_BE_DEGREES_OR_RADIANS,
         (types<DegreeOrRadian>));
};

template <>
struct coordinate_system_units<geometry::degree>
{
    typedef geometry::degree units;
};

template <>
struct coordinate_system_units<geometry::radian>
{
    typedef geometry::radian units;
};

} // namespace core_detail
#endif // DOXYGEN_NO_DETAIL

namespace cs
{

/*!
\brief Cartesian coordinate system
\details Defines the Cartesian or rectangular coordinate system
    where points are defined in 2 or 3 (or more)
dimensions and usually (but not always) known as x,y,z
\see http://en.wikipedia.org/wiki/Cartesian_coordinate_system
\ingroup cs
*/
struct cartesian {};

/*!
\brief Geographic coordinate system, in degree or in radian
\details Defines the geographic coordinate system where points
    are defined in two angles and usually
known as lat,long or lo,la or phi,lambda
\see http://en.wikipedia.org/wiki/Geographic_coordinate_system
\ingroup cs
\note might be moved to extensions/gis/geographic
*/
template<typename DegreeOrRadian>
struct geographic
{
    typedef typename core_detail::coordinate_system_units
        <
            DegreeOrRadian
        >::units units;
};

/*!
\brief Spherical (polar) coordinate system, in degree or in radian
\details Defines the spherical coordinate system where points are
    defined in two angles
    and an optional radius usually known as r, theta, phi
\par Coordinates:
- coordinate 0:
    0 <= phi < 2pi is the angle between the positive x-axis and the
        line from the origin to the P projected onto the xy-plane.
- coordinate 1:
    0 <= theta <= pi is the angle between the positive z-axis and the
        line formed between the origin and P.
- coordinate 2 (if specified):
    r >= 0 is the distance from the origin to a given point P.

\see http://en.wikipedia.org/wiki/Spherical_coordinates
\ingroup cs
*/
template<typename DegreeOrRadian>
struct spherical
{
    typedef typename core_detail::coordinate_system_units
        <
            DegreeOrRadian
        >::units units;
};

/*!
\brief Spherical equatorial coordinate system, in degree or in radian
\details This one resembles the geographic coordinate system, and has latitude
    up from zero at the equator, to 90 at the pole
    (opposite to the spherical(polar) coordinate system).
    Used in astronomy and in GIS (but there is also the geographic)

\see http://en.wikipedia.org/wiki/Spherical_coordinates
\ingroup cs
*/
template<typename DegreeOrRadian>
struct spherical_equatorial
{
    typedef typename core_detail::coordinate_system_units
        <
            DegreeOrRadian
        >::units units;
};

/*!
\brief Polar coordinate system
\details Defines the polar coordinate system "in which each point
    on a plane is determined by an angle and a distance"
\see http://en.wikipedia.org/wiki/Polar_coordinates
\ingroup cs
*/
template<typename DegreeOrRadian>
struct polar
{
    typedef typename core_detail::coordinate_system_units
        <
            DegreeOrRadian
        >::units units;
};

} // namespace cs

namespace traits
{

/*!
\brief Traits class defining coordinate system tag, bound to coordinate system
\ingroup traits
\tparam CoordinateSystem coordinate system
*/
template <typename CoordinateSystem>
struct cs_tag
{
};

#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS

template<typename DegreeOrRadian>
struct cs_tag<cs::geographic<DegreeOrRadian> >
{
    typedef geographic_tag type;
};

template<typename DegreeOrRadian>
struct cs_tag<cs::spherical<DegreeOrRadian> >
{
    typedef spherical_polar_tag type;
};

template<typename DegreeOrRadian>
struct cs_tag<cs::spherical_equatorial<DegreeOrRadian> >
{
    typedef spherical_equatorial_tag type;
};

template<>
struct cs_tag<cs::cartesian>
{
    typedef cartesian_tag type;
};

#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS
} // namespace traits

/*!
\brief Meta-function returning coordinate system tag (cs family) of any geometry
\tparam Geometry \tparam_geometry
\ingroup core
*/
template <typename Geometry>
struct cs_tag
{
    typedef typename traits::cs_tag
        <
            typename geometry::coordinate_system<Geometry>::type
        >::type type;
};

/*!
\brief Meta-function to verify if a coordinate system is radian
\tparam CoordinateSystem Any coordinate system.
\ingroup core
*/
template <typename CoordinateSystem>
struct is_radian : boost::true_type {};

#ifndef DOXYGEN_NO_SPECIALIZATIONS

// Specialization for any degree coordinate systems
template <template<typename> class CoordinateSystem>
struct is_radian< CoordinateSystem<degree> > : boost::false_type
{
};

#endif // DOXYGEN_NO_SPECIALIZATIONS

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_CORE_CS_HPP


#include <boost/test/unit_test.hpp>
#include "algorithms/search/interpolation_search.hpp"
#include "testcases_for_search_algorithms.hpp"

BOOST_AUTO_TEST_SUITE(TestInterpolationSearch)

using namespace TCFSearchingAlgo;

BOOST_AUTO_TEST_CASE(container_one_elem)
{
    std::vector<int> container;
    int value;
    bool expectedIsFound;
    OneElement(container, value, expectedIsFound);

    size_t result = 0;
    InterpolationSearch::Solution solution;
    bool isFound = solution.Search(container, container.size(), value, result);
    BOOST_CHECK(expectedIsFound == isFound);
}

BOOST_AUTO_TEST_CASE(val_in_middle)
{
    std::vector<int> container;
    int value;
    bool expectedIsFound;
    size_t expectedResultIndex;
    ValueInMiddle(container, value, expectedIsFound, expectedResultIndex);

    size_t result = 0;
    InterpolationSearch::Solution solution;
    bool isFound = solution.Search(container, container.size(), value, result);
    BOOST_CHECK(expectedIsFound == isFound);
    BOOST_CHECK(expectedResultIndex == result);
}

BOOST_AUTO_TEST_CASE(val_in_start)
{
    std::vector<int> container;
    int value;
    bool expectedIsFound;
    size_t expectedResultIndex;
    ValueAtStart(container, value, expectedIsFound, expectedResultIndex);

    size_t result = 0;
    InterpolationSearch::Solution solution;
    bool isFound = solution.Search(container, container.size(), value, result);
    BOOST_CHECK(expectedIsFound == isFound);
    BOOST_CHECK(expectedResultIndex == result);
}

BOOST_AUTO_TEST_CASE(val_in_end)
{
    std::vector<int> container;
    int value;
    bool expectedIsFound;
    size_t expectedResultIndex;
    ValueAtEnd(container, value, expectedIsFound, expectedResultIndex);

    size_t result = 0;
    InterpolationSearch::Solution solution;
    bool isFound = solution.Search(container, container.size(), value, result);
    BOOST_CHECK(expectedIsFound == isFound);
    BOOST_CHECK(expectedResultIndex == result);
}

BOOST_AUTO_TEST_CASE(no_val)
{
    std::vector<int> container;
    int value;
    bool expectedIsFound;
    NoValueInContainer(container, value, expectedIsFound);

    size_t result = 0;
    InterpolationSearch::Solution solution;
    bool isFound = solution.Search(container, container.size(), value, result);
    BOOST_CHECK(expectedIsFound == isFound);
}

BOOST_AUTO_TEST_CASE(val_is_less_than_min)
{
    std::vector<int> container;
    int value;
    bool expectedIsFound;
    ValueIsLessThanMinValueInContainer(container, value, expectedIsFound);

    size_t result = 0;
    InterpolationSearch::Solution solution;
    bool isFound = solution.Search(container, container.size(), value, result);
    BOOST_CHECK(expectedIsFound == isFound);
}

BOOST_AUTO_TEST_CASE(several_same_val)
{
    std::vector<int> container;
    int value;
    bool expectedIsFound;
    std::vector<size_t> expectedResultIndexes;
    SeveralSameValuesInContainer(container, value, expectedIsFound,
                                        expectedResultIndexes);

    size_t result = 0;
    InterpolationSearch::Solution solution;
    bool isFound = solution.Search(container, container.size(), value, result);
    BOOST_CHECK(expectedIsFound == isFound);

    auto iter = std::find(expectedResultIndexes.begin(),
                          expectedResultIndexes.end(), result);
    BOOST_CHECK(iter != expectedResultIndexes.end());
}

BOOST_AUTO_TEST_CASE(random_values) {
    std::vector<int> container;
    int value;
    bool expectedIsFound;
    std::vector<size_t> expectedResultIndexes;

    for (size_t i = 0; i < 10; ++i) {
        RandomContainer(container, value, expectedIsFound,
                        expectedResultIndexes, 1000);

        size_t result = 0;
        InterpolationSearch::Solution solution;
        bool isFound = solution.Search(container, container.size(),
                                       value, result);

        BOOST_CHECK(expectedIsFound == isFound);

        if (isFound) {
            auto iter = std::find(expectedResultIndexes.begin(),
                                  expectedResultIndexes.end(), result);
            BOOST_CHECK(iter != expectedResultIndexes.end());
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef __MATALGERA__
#define __MATALGERA__

#define LAPACK_COMPLEX_CUSTOM
#define lapack_complex_double std::complex<double>
#define lapack_complex_float std::complex<float>

#include <cstddef>
#include <string>
#include <utility>
#include <memory>
#include <fstream>

#include <Eigen/Dense>

//#include <boost/python/numpy.hpp>
//namespace p = boost::python;
//namespace np = boost::python::numpy;

template<typename T>
class Matrix {
    public:
    Matrix() = delete;
    Matrix(int m, int n) {
        n_row = m;
        n_col = n;
        data = new T[m*n];
        for(int i = 0; i < m*n; ++i) {
            data[i] = static_cast<T>(0.0);
        }
    }

    Matrix(const Matrix& a) {
        //std::cout << "inside copy constructor" << std::endl;
        n_row = a.GetNRow();
        n_col = a.GetNCol();

        T* a_data = a.GetData();

        data = new T[n_row*n_col];
        for(int i = 0; i < n_row*n_col; ++i) {
            data[i] = a_data[i];
        }
    }

    Matrix& operator=(const Matrix& a) {
        //std::cout << "inside the = operator" << std::endl;
        n_row = a.GetNRow();
        n_col = a.GetNCol();

        T* a_data = a.GetData();

        data = new T[n_row*n_col];
        for(int i = 0; i < n_row*n_col; ++i) {
            data[i] = a_data[i];
        }

        return this;
    }

    ~Matrix() {
        delete[] data;
        data = nullptr;
    }

    T* GetData() const {
        return data;
    }

    int GetNRow() const {
        return n_row;
    }

    int GetNCol() const {
        return n_col;
    }

    T& operator[](const std::pair<int, int> ind) const{
        return data[ind.first*n_col + ind.second];
    }

    T& operator()(const int i, const int j) const {
        return data[i*n_col + j];
    }

    void Reshape(int m, int n) {
        assert(m*n == n_row*n_col);
        n_row = m;
        n_col = n;
    }

    friend Matrix operator-(const Matrix& A, const Matrix& B) {
        double A_M = A.GetNRow();
        double A_N = A.GetNCol();
        double B_M = B.GetNRow();
        double B_N = B.GetNCol();

        assert(A_M == B_M && A_N == B_N);

        Matrix C(A_M, A_N);

        for(int i = 0; i < A_M; ++i) {
            for(int j = 0; j < A_N; ++j) {
                C(i, j) = A[{i, j}] - B[{i, j}];
            }
        }

        return C;
    }

    friend Matrix operator*(const Matrix& A, const Matrix& B) {
        double A_M = A.GetNRow();
        double A_N = A.GetNCol();
        double B_M = B.GetNRow();
        double B_N = B.GetNCol();

        assert( A_N == B_M);

        Matrix C(A_M, B_N);

        for(int i = 0; i < A_M; ++i) {
            for(int j = 0; j < B_N; ++j) {
                for(int k = 0; k < A_N; ++k) {
                    C(i, j) += A[{i, k}] * B[{k, j}];
                }
            }
        }

        return C;
    }

    double GetNorm() {
        double norm = 0.0;
        for(int i = 0; i < n_row*n_col; ++i) {
            norm += std::norm(data[i]);
        }
        return std::sqrt(norm);
    }

    void Print() {
        for(int i = 0; i < n_row; ++i) {
            for(int j = 0; j < n_col; ++j) {
                std::cout << data[i*n_col + j] << " ";
            }
            std::cout << std::endl;
        }
    }

    void WriteToFile(std::string fileName) {
        std::ofstream fileOut(fileName.c_str(), std::ios::out | std::ios::binary);
        assert(fileOut.is_open());
        std::size_t dataSize = n_row * n_col * sizeof(T);
        fileOut.write((char*)(data), dataSize);
        fileOut.close();
    }

    private:
    int n_row;
    int n_col;
    T* data = nullptr;

};

template<typename T>
Matrix<T> ReadMatrixFromFile(std::string fileName) {
    std::ifstream fileIn(fileName.c_str(), std::ios::in | std::ios::binary);
    assert(fileIn.is_open());
    fileIn.seekg(0, fileIn.end);
    std::size_t fileSize = fileIn.tellg();
    assert(fileSize % sizeof(T) == 0);
    int n_col = fileSize / sizeof(T);
    Matrix<T> A(1, n_col);
    T* a_data = A.GetData();
    fileIn.seekg(0, fileIn.beg);
    fileIn.read((char*)(a_data), fileSize);
    fileIn.close();

    return A;
}

#ifdef _LAPACK_IS_IN_USE_
#include "lapacke.h"

Matrix<std::complex<double>> SolveLinear(Matrix<std::complex<double>>& A, Matrix<std::complex<double>>& B,
                                        bool useExpertVersion = false) {

    const int n_row = A.GetNRow();
    const int n_col = A.GetNCol();
    const int n_rhs = B.GetNCol();

    assert(n_rhs == 1);
    assert(n_row == n_col);

    std::complex<double>* a = A.GetData();
    std::complex<double>* b = B.GetData();
    std::unique_ptr<int[]> ipiv(new int[n_row]);

    if(useExpertVersion) {
        std::unique_ptr<double[]> rpivot(new double [n_row]);

        char equed = 'A';

        std::complex<double> af[n_row][n_col];
        std::unique_ptr<double[]> r(new double[n_row]);    // row scale factors
        std::unique_ptr<double[]> c(new double[n_row]);    // col scale factors
        Matrix<std::complex<double>> X(n_row, n_rhs);
        std::complex<double>* x = X.GetData();

        double rcond;
        std::unique_ptr<double[]> ferr(new double[n_rhs]);
        std::unique_ptr<double[]> berr(new double[n_rhs]);

        lapack_int info = LAPACKE_zgesvx(LAPACK_ROW_MAJOR, 'E', 'N',
                               n_row, n_rhs, a, n_col, *af, n_row,
                               ipiv.get(), &equed, r.get(), c.get(),
                               b, n_rhs, x, n_rhs,
                               &rcond, ferr.get(), berr.get(),
                               rpivot.get() );

        std::cout << " info : " << info << std::endl;
        std::cout << " condition : " << rcond << std::endl;
        std::cout << " ferr : " << ferr[0] << std::endl;
        std::cout << " berr : " << berr[0] << std::endl;
        std::cout << " equed : " << equed << std::endl;

        //std::cout << " ipiv : " << std::endl;
        //int* ipiv_p = ipiv.get();
        //for(int i = 0; i < n_row; ++i) {
        //    std::cout << "(" << i + 1 << ", " << ipiv_p[i] << ")  ";
        //}
        //std::cout << std::endl;

        return X;
    } else {
        lapack_int info = LAPACKE_zgesv(LAPACK_ROW_MAJOR, n_row, n_rhs, a, n_col, ipiv.get(), b, n_rhs);

        Matrix<std::complex<double>> X(n_row, n_rhs);
        std::complex<double>* x = X.GetData();
        for(int i = 0; i < n_row*n_rhs; ++i) {
            x[i] = b[i];
        }

        return X;
    }
}
#endif // _LAPACK_IS_IN_USE_

Matrix<std::complex<double>> SolveLinear_Ei(Matrix<std::complex<double>>& A, Matrix<std::complex<double>>& B) {
    const int n_row = A.GetNRow();
    const int n_col = A.GetNCol();
    const int n_rhs = B.GetNCol();

    assert(n_rhs == 1);
    assert(n_row == n_col);

    Eigen::MatrixXcd A_ei(n_row, n_col);
    Eigen::MatrixXcd B_ei(n_row, n_rhs);

    for(int i = 0; i < n_row; ++i) {
        for(int j = 0; j < n_col; ++j) {
            A_ei(i, j) = A(i, j);
        }
        for(int j = 0; j < n_rhs; ++j) {
            B_ei(i, j) = B(i, j);
        }
    }

    //std::cout << A_ei << std::endl;
    Matrix<std::complex<double>> X(n_row, n_rhs);
    Eigen::MatrixXcd X_ei = A_ei.fullPivLu().solve(B_ei);
    std::cout << X_ei << std::endl;

    for(int i = 0; i < n_row; ++i) {
        for(int j = 0; j < n_rhs; ++j) {
            X(i, j) = X_ei(i, j);
        }
    }

    double relative_error = (A_ei*X_ei - B_ei).norm() / B_ei.norm(); // norm() is L2 norm
    std::cout << "The relative error is:" << relative_error << std::endl;
    relative_error = (A*X - B).GetNorm() / B.GetNorm(); // norm() is L2 norm
    std::cout << "The relative error is:" << relative_error << std::endl;

    return X;
}

Matrix<std::complex<double>> SolveLinear_np_file(Matrix<std::complex<double>>& A, Matrix<std::complex<double>>& B) {
    remove("out/x.data");
    A.WriteToFile("out/A.data");
    B.WriteToFile("out/b.data");
    system("python3 PyMatSolver.py");

    auto x = ReadMatrixFromFile<std::complex<double>>("out/x.data");
    int m = x.GetNRow();
    int n = x.GetNCol();
    assert(m == 1);
    x.Reshape(m*n, 1);
    //x.Print();
    return x;
}

#include "suitesparse/umfpack.h"

Matrix<std::complex<double>> SolveLinear_umfpack(Matrix<std::complex<double>>& A, Matrix<std::complex<double>>& B) {
    const int n = A.GetNRow();
    assert(A.GetNCol() == n && B.GetNRow() == n && B.GetNCol() == 1);

    int nnz = 0;
    for(int i = 0; i < n; ++i) {
        for(int j = 0; j < n; ++j) {
            if( A(i, j) != 0.0 ) {
                nnz += 1;
            }
        }
    }

    std::cout << "n: " << n << "  nnz: " << nnz << std::endl;

    auto a_cols = std::make_unique<int[]>(n + 1);
    auto a_rows = std::make_unique<int[]>(nnz);
    auto a_elems = std::make_unique<double[]>(2*nnz);
    auto b_elems = std::make_unique<double[]>(2*n);
    auto x_elems = std::make_unique<double[]>(2*n);

    int* Ap = a_cols.get();
    int* Ai = a_rows.get();
    double* Ax = a_elems.get();
    double* b = b_elems.get();
    double* x = x_elems.get();

    nnz = 0;
    Ap[0] = 0;
    for(int j = 0; j < n; ++j) {
        for(int i = 0; i < n; ++i) {
            if( A(i, j) != 0.0 ) {
                Ai[nnz] = i;
                Ax[2*nnz]     = A(i, j).real();
                Ax[2*nnz + 1] = A(i, j).imag();
                nnz += 1;
            }
        }
        Ap[j + 1] = nnz;
    }

    for(int i = 0; i < n; ++i) {
        b[2*i]     = B(i, 0).real();
        b[2*i + 1] = B(i, 0).imag();
    }

    double *null = (double *) NULL;
    void *Symbolic, *Numeric;
    (void) umfpack_zi_symbolic(n, n, Ap, Ai, Ax, NULL, &Symbolic, null, null);
    (void) umfpack_zi_numeric(Ap, Ai, Ax, NULL, Symbolic, &Numeric, null, null);
    umfpack_zi_free_symbolic(&Symbolic);
    (void) umfpack_zi_solve (UMFPACK_A, Ap, Ai, Ax, NULL, x, NULL, b, NULL, Numeric, null, null);
    umfpack_zi_free_numeric (&Numeric);

    Matrix<std::complex<double>> X(n, 1);
    //for (int i = 0 ; i < n ; i++) std::cout << i << " " <<  x_complex[i] << std::endl;

    for(int i = 0; i < n; ++i) {
        X(i, 0) = std::complex<double>(x[2*i], x[2*i + 1]);
    }

    return X;
}

#endif


/**
 * @file set_cover.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-02-12
 */
#ifndef PAAL_SET_COVER_HPP
#define PAAL_SET_COVER_HPP

#include "paal/greedy/set_cover/budgeted_maximum_coverage.hpp"
#include "paal/utils/accumulate_functors.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"

#include <boost/function_output_iterator.hpp>

#include <iterator>
#include <vector>

namespace paal{
namespace greedy{
namespace detail{
template <typename SetRange,typename GetCostOfSet>
using set_range_cost_t = pure_result_of_t<
        GetCostOfSet(typename boost::range_reference<SetRange>::type)>;

}//!detail
/**
 * @brief this is solve Set Cover problem
 * and return set cover cost
 * example:
 *  \snippet set_cover_example.cpp Set Cover Example
 *
 * complete example is set_cover_example.cpp
 * @param sets
 * @param set_to_cost
 * @param set_to_elements
 * @param result set iterators of chosen sets
 * @param get_el_index
 * @tparam SetRange
 * @tparam GetCostOfSet
 * @tparam GetElementsOfSet
 * @tparam OutputIterator
 * @tparam GetElementIndex
 */
template<typename SetRange, class GetCostOfSet, class GetElementsOfSet, class OutputIterator,class GetElementIndex>
auto set_cover(SetRange && sets,
        GetCostOfSet set_to_cost,
        GetElementsOfSet set_to_elements,
        OutputIterator result,
        GetElementIndex get_el_index
    ) {
    using set_cost=typename detail::set_range_cost_t<SetRange,GetCostOfSet>;
    //TODO use sum functor from r=Robert commit
    auto cost_of_all_sets=accumulate_functor(sets, set_cost{}, set_to_cost);
    set_cost cost_of_solution{};
    budgeted_maximum_coverage(sets,
        set_to_cost,
        set_to_elements,
        boost::make_function_output_iterator([&](int set){
            cost_of_solution += set_to_cost(set);
            *result = set;
            ++result;
            }),
        get_el_index,
        cost_of_all_sets,
        paal::utils::return_one_functor(),
        0
    );
    return cost_of_solution;
};
}//!greedy
}//!paal

#endif /* PAAL_SET_COVER_HPP */