#include <cmath>
#include <memory>
#include <fstream>
#include <set>
#include <numeric>
#include <random>
#include <iostream>

#include <CandyPretty/CandyPretty.h>

#include <boost/exception/all.hpp>
#include <boost/variant.hpp>

#if 0

/*
                D(t)V(t) = E~(D(T)V(T)|F(T))
 */

/*
        This is the analytic solution to geometric brownian motion with drift

                dS = a S dt + b S dw

 */

#include <ql/pricingengines/blackcalculator.hpp>

struct Differential{
        virtual ~Differential()=default;
        /*
                dx = f(x,dt,dw)
                x(t + dt ) = x(t) + dx(t)
         */
        virtual double Eval(double x, double dt, double std_norm)const=0;
};

struct ProcessContext;

struct ProcessIntegral{
        ProcessIntegral()=default;
        ProcessIntegral(ProcessContext& ctx, double x, std::shared_ptr<Differential> dx);
        void SmallChange(double dt, double std_norm){
                x_ += dx_->Eval( x_, dt, std_norm );
        }
        double Value()const{ return x_; }
private:
        double x_;
        std::shared_ptr<Differential> dx_;
};

struct ProcessContext{
        void Register(ProcessIntegral* ptr){
                procs_.push_back(ptr);
        }
        void Step(double dt){
                auto std_norm = [&](){ return D_(G_); };
                for(auto ptr : procs_){
                        ptr->SmallChange(dt, std_norm());
                }
        }
private:
        #if 0
        std::default_random_engine G_;
        #else
        std::random_device G_;
        #endif
        std::normal_distribution<double> D_{0.0, 1.0};
        std::vector<ProcessIntegral*> procs_;
};

inline ProcessIntegral::ProcessIntegral(ProcessContext& ctx, double x, std::shared_ptr<Differential> dx)
        :x_(x),
        dx_(dx)
{
        ctx.Register(this);
}

struct ProcessView{
        struct Impl{
                virtual ~Impl()=default;
                virtual double Value()const=0;
        };
        struct SptrImpl : Impl{
                explicit SptrImpl(std::shared_ptr<ProcessIntegral const> q_) :q(q_) {}
                virtual double Value()const{ return q->Value(); }
                std::shared_ptr<ProcessIntegral const> q;
        };

        ProcessView()=default;
        ProcessView(std::shared_ptr<ProcessIntegral> p){
                impl_ = std::make_shared<SptrImpl>(p);
        }
        // assumes objects lifetime exists for as long as it'self
        ProcessView(ProcessIntegral const& p){
                std::shared_ptr<ProcessIntegral const> aux(&p, [](auto*){});
                impl_ = std::make_shared<SptrImpl>(aux);
        }

        ProcessView& operator=(std::shared_ptr<ProcessIntegral> p){
                impl_ = std::make_shared<SptrImpl>(p);
                return *this;
        }
        ProcessView& operator=(ProcessIntegral const& p){
                std::shared_ptr<ProcessIntegral const> aux(&p, [](auto*){});
                impl_ = std::make_shared<SptrImpl>(aux);
                return *this;
        }

        double Value()const{ return impl_->Value(); }

        // for printing to csv etc
        // this is how it is set, p.Name()  = "Discount ProcessIntegral()"
        std::string& Name(){ return name_; }
        std::string const& Name()const{ return name_; }
protected:
        std::shared_ptr<Impl> impl_;
        std::string name_{"ProcessIntegral"};
};

struct IdentityDifferential : Differential{
        virtual double Eval(double x, double dt, double std_norm)const override{
                return dt;
        }
};

struct BankAccountDifferential : Differential{
        explicit BankAccountDifferential(ProcessView interest_rate)
                :interest_rate_(interest_rate)
        {}
        virtual double Eval(double x, double dt, double std_norm)const override{
                return x * interest_rate_.Value() * dt;
        }
private:
        ProcessView interest_rate_;
};

struct GeometricBrownianMotionWithDriftDifferential : Differential{
        GeometricBrownianMotionWithDriftDifferential(double S0, double r, double sigma)
                :S0_(S0),
                r_(r),
                sigma_(sigma)
        {}
        virtual double Eval(double x, double dt, double std_norm)const override{
                double a = r_ * dt + sigma_ * std_norm * std::sqrt(dt);
                double b = a * x;
                return b;
        }
private:
        double S0_;
        double r_;
        double sigma_;
};

struct VasicekDifferential : Differential{
        VasicekDifferential(double alpha, double beta, double sigma)
                :alpha_(alpha),
                beta_(beta),
                sigma_(sigma)
        {}
        virtual double Eval(double x, double dt, double std_norm)const override{
                double a = ( alpha_ - beta_ * x ) * dt;
                double b = sigma_ * std_norm * std::sqrt(dt);
                double c = a + b;
                return c;
        }
private:
        double alpha_;
        double beta_;
        double sigma_;
};
struct CoxIngersollRos : Differential{
        CoxIngersollRos(double alpha, double beta, double sigma)
                :alpha_(alpha),
                beta_(beta),
                sigma_(sigma)
        {}
        virtual double Eval(double x, double dt, double std_norm)const override{
                double a = ( alpha_ - beta_ * x ) * dt;
                double b = sigma_ * std::sqrt(x) *  std_norm * std::sqrt(dt);
                double c = a + b;
                return c;
        }
private:
        double alpha_;
        double beta_;
        double sigma_;
};

struct AnaBlack : ProcessView{
        AnaBlack(ProcessView t){
                struct AnaBlackImpl : Impl{
                        AnaBlackImpl(ProcessView t)
                                :t_(t)
                        {}
                        virtual double Value()const override{
                                double r = 0.02;
                                double vol = 0.1;
                                double s0 = 10.0;
                                double k = 1.5 * s0;

                                double T = t_.Value();
                                auto discount = std::exp( -r * T );
                                auto fwd = s0 / discount;
                                auto std_dev = std::sqrt( vol * vol * T);
                                QuantLib::BlackCalculator bc(QuantLib::Option::Call,
                                                             k,
                                                             fwd,
                                                             std_dev,
                                                             discount);
                                return bc.value();
                        }
                private:
                        ProcessView t_;
                };
                impl_ = std::make_shared<AnaBlackImpl>(t);
        }
};

struct DiscountProcess : ProcessView{
        DiscountProcess(ProcessView t, double r){
                struct DPImpl : Impl{
                        DPImpl(ProcessView t_, double r_)
                                :t(t_),
                                r(r_)
                        {}
                        virtual double Value()const override{
                                return std::exp( - t.Value() * r );
                        }
                private:
                        ProcessView t;
                        double r;
                };
                impl_ = std::make_shared<DPImpl>(t,r);
        }
};

struct AverageView : ProcessView{
        struct Final : Impl{
                virtual double Value()const override{
                        size_t n = v_.size();
                        double sigma = 0.0;
                        for(size_t idx=0;idx!=n;++idx){
                                sigma +=  v_[idx].Value();
                        }
                        return sigma / n;
                }
        private:
                friend struct AverageView;
                std::vector<ProcessView> v_;
        };
        AverageView(){
                impl_ = std::make_shared<Final>();
        }
        AverageView& Add(ProcessView view){
                auto casted = dynamic_cast<Final*>(impl_.get());
                casted->v_.push_back(view);
                return *this;
        }
        template<class Iter>
        AverageView(Iter first, Iter last){
                impl_ = std::make_shared<Final>();
                for(;first!=last;++first){
                        Add(*first);
                }
        }
};

struct Option : ProcessView{
        Option(ProcessView process, double strike){
                struct OptionImpl : Impl{
                        OptionImpl(ProcessView process, double strike)
                                :process_(process),
                                strike_(strike)
                        {}
                        virtual double Value()const override{
                                return (std::max)(process_.Value() - strike_, 0.0);
                        }
                private:
                        ProcessView process_;
                        double strike_;
                };
                impl_ = std::make_shared<OptionImpl>(process, strike);
        }
};

struct ProcessViewRenderer{
        ProcessViewRenderer(std::ostream& out, std::vector<ProcessView> const& views)
                :out_{std::shared_ptr<std::ostream>(&out, [](auto*){})}, views_(views)
        {
                EmitHeader_();
        }
        ProcessViewRenderer(std::shared_ptr<std::ostream> out, std::vector<ProcessView> const& views)
                :out_{out}, views_(views)
        {
                EmitHeader_();
        }
        void RenderLine(){
                std::vector<std::string> line;
                for(auto const& view : views_){
                        line.push_back(boost::lexical_cast<std::string>(view.Value()));
                }
                lines_.push_back(std::move(line));
        }
        void Emit(){
                CandyPretty::RenderTablePretty(*out_, lines_, opts_);
        }
private:
        void EmitHeader_(){
                std::vector<std::string> line;
                for(auto const& view : views_){
                        line.push_back(boost::lexical_cast<std::string>(view.Name()));
                }
                lines_.push_back(std::move(line));
        }
        CandyPretty::RenderOptions opts_{CandyPretty::RenderOptions::CsvOptions()};
        std::shared_ptr<std::ostream> out_;
        std::vector<ProcessView> views_;
        std::vector<CandyPretty::LineItem> lines_;
};

void example_0(){
        using namespace CandyPretty;

        double r = 0.02;
        double vol = 0.1;
        double T = 20;
        double s0 = 10.0;

        auto gbm = std::make_shared<GeometricBrownianMotionWithDriftDifferential>(s0, r, vol);

        enum{ SampleSize = 4000 };
        ProcessContext ctx;
        auto t = std::make_shared<ProcessIntegral>(ctx, 0, std::make_shared<IdentityDifferential>() );

        std::vector<std::shared_ptr<ProcessIntegral> > gbm_sample(SampleSize);
        for(size_t idx=0;idx!=SampleSize;++idx){
                gbm_sample[idx] = std::make_shared<ProcessIntegral>(ctx, s0, gbm);
        }

        DiscountProcess disc(t, r);

        std::vector<AverageView> avg_vec;
        auto first = &gbm_sample[0];
        avg_vec.emplace_back( first, first + 10);
        avg_vec.back().Name() = "Avg_{10}";
        avg_vec.emplace_back( first, first + 100);
        avg_vec.back().Name() = "Avg_{100}";
        avg_vec.emplace_back( first, first + 1000);
        avg_vec.back().Name() = "Avg_{1000}";
        avg_vec.emplace_back( first, first + 2000);
        avg_vec.back().Name() = "Avg_{2000}";
        avg_vec.emplace_back( first, first + 4000);
        avg_vec.back().Name() = "Avg_{4000}";

        std::vector<ProcessView> views;
        views.push_back(t);
        views.back().Name() = "t";
        views.push_back(disc);
        views.back().Name() = "D(t)";
        for(auto const& _ : avg_vec){
                views.push_back(_);
        }
        enum{ GbmViews = 20 };
        for(size_t idx=0;idx < gbm_sample.size() && idx < GbmViews;++idx){
                views.push_back(gbm_sample[idx]);
                std::stringstream sstr;
                sstr << "W_{" << idx << "}(t)";
                views.back().Name() = sstr.str();
        }

        std::vector<ProcessView>* render_view = &views;

        #if 0

        AnaBlack black( t);

        std::vector<ProcessView> call_view;
        call_view.push_back(t);
        call_view.push_back(disc);
        call_view.push_back(black);

        std::vector<ProcessView> call_options(SampleSize);
        std::vector<std::pair<AverageView, size_t> > call_avg;
        call_avg.emplace_back(AverageView(), 10);
        call_avg.emplace_back(AverageView(), 100);
        call_avg.emplace_back(AverageView(), 1000);
        call_avg.emplace_back(AverageView(), 2000);
        call_avg.emplace_back(AverageView(), 4000);
        for(auto& _ : call_avg){
                call_view.push_back(_.first);
        }
        for(size_t idx=0;idx!=SampleSize;++idx){
                auto& p = gbm_sample[idx];
                call_options[idx] = Option(p, k);
                for(auto& _ : call_avg){
                        if( idx < _.second ){
                                _.first.Add(call_options[idx]);
                        }
                }
        }

        double ir_0 = 0.05;
        auto f = 10.0;
        auto ir_diff = std::make_shared<VasicekDifferential>(1/f, 20/f, 0.1);
        std::vector<ProcessIntegral> bank_acct_sample(SampleSize);
        std::vector<ProcessView> ir_view;
        AverageView bank_acct_avg;
        AverageView ir_avg;

        ir_view.push_back(t);
        ir_view.push_back(ir_avg);
        ir_view.push_back(bank_acct_avg);

        for(size_t idx=0;idx!=SampleSize;++idx){
                auto irp_p = std::make_shared<ProcessIntegral>(ir_0, ir_diff);
                ProcessView irp(irp_p);
                procs.push_back(irp_p.get());
                auto bank_acct_diff = std::make_shared<BankAccountDifferential>(irp);
                auto bank_acct_p = std::make_shared<ProcessIntegral>(1.0, bank_acct_diff);
                ProcessView bank_acct(bank_acct_p);
                procs.push_back(bank_acct_p.get());

                if( idx < GbmViews ){
                        ir_view.push_back(irp);
                        ir_view.push_back( bank_acct);
                }
                ir_avg.Add(irp);
                bank_acct_avg.Add(bank_acct);
        }
        #endif

        size_t N = 1000;
        double dt = T / N;

        std::ofstream of{"RiskNeutralBrownianMotion.csv"};
        if( ! of.is_open() )
                BOOST_THROW_EXCEPTION(std::domain_error("unable to open RiskNeutralBrownianMotion.csv"));
        ProcessViewRenderer renderer{of, *render_view};

        for(size_t idx=0;idx!=N;++idx){
                ctx.Step(dt);
                renderer.RenderLine();
        }
        renderer.Emit();
}

void example_1(){
        using namespace CandyPretty;

        double r = 0.02;
        double vol = 0.1;
        double T = 40;
        double s0 = 10.0;
        double k = 1.5 * s0;

        auto gbm = std::make_shared<GeometricBrownianMotionWithDriftDifferential>(s0, r, vol);

        enum{ SampleSize = 4000 };
        ProcessContext ctx;
        auto t = std::make_shared<ProcessIntegral>(ctx, 0, std::make_shared<IdentityDifferential>() );

        std::vector<std::shared_ptr<ProcessIntegral> > gbm_sample(SampleSize);
        std::vector<ProcessView> call_options(SampleSize);
        for(size_t idx=0;idx!=SampleSize;++idx){
                gbm_sample[idx] = std::make_shared<ProcessIntegral>(ctx, s0, gbm);
                call_options[idx] = Option(gbm_sample[idx], k);
        }

        DiscountProcess disc(t, r);

        AnaBlack black( t);

        std::vector<AverageView> avg_vec;
        auto first = &call_options[0];
        avg_vec.emplace_back( first, first + 10);
        avg_vec.back().Name() = "Avg_{10}";
        avg_vec.emplace_back( first, first + 100);
        avg_vec.back().Name() = "Avg_{100}";
        avg_vec.emplace_back( first, first + 1000);
        avg_vec.back().Name() = "Avg_{1000}";
        avg_vec.emplace_back( first, first + 2000);
        avg_vec.back().Name() = "Avg_{2000}";
        avg_vec.emplace_back( first, first + 4000);
        avg_vec.back().Name() = "Avg_{4000}";

        std::vector<ProcessView> views;
        views.push_back(t);
        views.back().Name() = "t";
        views.push_back(disc);
        views.back().Name() = "D(t)";
        views.push_back(black);
        views.back().Name() = "BS(.)";
        for(auto const& _ : avg_vec){
                views.push_back(_);
        }
        enum{ GbmViews = 20 };
        for(size_t idx=0;idx < call_options.size() && idx < GbmViews;++idx){
                views.push_back(call_options[idx]);
                std::stringstream sstr;
                sstr << "Call_{" << idx << "}(t)";
                views.back().Name() = sstr.str();
        }

        std::vector<ProcessView>* render_view = &views;

        #if 0

        std::vector<ProcessView> call_view;
        call_view.push_back(t);
        call_view.push_back(disc);
        call_view.push_back(black);

        std::vector<ProcessView> call_options(SampleSize);
        std::vector<std::pair<AverageView, size_t> > call_avg;
        call_avg.emplace_back(AverageView(), 10);
        call_avg.emplace_back(AverageView(), 100);
        call_avg.emplace_back(AverageView(), 1000);
        call_avg.emplace_back(AverageView(), 2000);
        call_avg.emplace_back(AverageView(), 4000);
        for(auto& _ : call_avg){
                call_view.push_back(_.first);
        }
        for(size_t idx=0;idx!=SampleSize;++idx){
                auto& p = gbm_sample[idx];
                call_options[idx] = Option(p, k);
                for(auto& _ : call_avg){
                        if( idx < _.second ){
                                _.first.Add(call_options[idx]);
                        }
                }
        }

        double ir_0 = 0.05;
        auto f = 10.0;
        auto ir_diff = std::make_shared<VasicekDifferential>(1/f, 20/f, 0.1);
        std::vector<ProcessIntegral> bank_acct_sample(SampleSize);
        std::vector<ProcessView> ir_view;
        AverageView bank_acct_avg;
        AverageView ir_avg;

        ir_view.push_back(t);
        ir_view.push_back(ir_avg);
        ir_view.push_back(bank_acct_avg);

        for(size_t idx=0;idx!=SampleSize;++idx){
                auto irp_p = std::make_shared<ProcessIntegral>(ir_0, ir_diff);
                ProcessView irp(irp_p);
                procs.push_back(irp_p.get());
                auto bank_acct_diff = std::make_shared<BankAccountDifferential>(irp);
                auto bank_acct_p = std::make_shared<ProcessIntegral>(1.0, bank_acct_diff);
                ProcessView bank_acct(bank_acct_p);
                procs.push_back(bank_acct_p.get());

                if( idx < GbmViews ){
                        ir_view.push_back(irp);
                        ir_view.push_back( bank_acct);
                }
                ir_avg.Add(irp);
                bank_acct_avg.Add(bank_acct);
        }
        #endif

        size_t N = 1000;
        double dt = T / N;

        std::ofstream of{"CallOption.csv"};
        if( ! of.is_open() )
                BOOST_THROW_EXCEPTION(std::domain_error("unable to open CallOption.csv"));
        ProcessViewRenderer renderer{of, *render_view};

        for(size_t idx=0;idx!=N;++idx){
                ctx.Step(dt);
                renderer.RenderLine();
        }
        renderer.Emit();
}

void example_2(){
        using namespace CandyPretty;

        double r = 0.02;
        double vol = 0.1;
        double T = 40;
        double s0 = 10.0;

        enum{ SampleSize = 4000 };

        ProcessContext ctx;

        auto t = std::make_shared<ProcessIntegral>(ctx, 0, std::make_shared<IdentityDifferential>() );

        std::vector<ProcessView> interest_rate_samples(SampleSize);
        std::vector<ProcessView> bank_account_samples(SampleSize);

        double ir_0 = 0.05;
        auto f = 10.0;
        auto ir_diff = std::make_shared<VasicekDifferential>(1/f, 20/f, 0.1);

        for(size_t idx=0;idx!=SampleSize;++idx){
                interest_rate_samples[idx] = std::make_shared<ProcessIntegral>(ctx, ir_0, ir_diff);
                auto bank_acct_diff = std::make_shared<BankAccountDifferential>(interest_rate_samples[idx]);
                bank_account_samples[idx]  = std::make_shared<ProcessIntegral>(ctx, 1.0, bank_acct_diff);
        }

        std::vector<AverageView> avg_vec;
        auto first = &bank_account_samples[0];
        avg_vec.emplace_back( first, first + 10);
        avg_vec.back().Name() = "Avg_{10}";
        avg_vec.emplace_back( first, first + 100);
        avg_vec.back().Name() = "Avg_{100}";
        avg_vec.emplace_back( first, first + 1000);
        avg_vec.back().Name() = "Avg_{1000}";
        avg_vec.emplace_back( first, first + 2000);
        avg_vec.back().Name() = "Avg_{2000}";
        avg_vec.emplace_back( first, first + 4000);
        avg_vec.back().Name() = "Avg_{4000}";

        std::vector<ProcessView> views;
        views.push_back(t);
        views.back().Name() = "t";
        for(auto const& _ : avg_vec){
                views.push_back(_);
        }
        enum{ GbmViews = 20 };
        for(size_t idx=0;idx < SampleSize && idx < GbmViews;++idx){
                std::stringstream sstr;
                views.push_back(interest_rate_samples[idx]);
                sstr << "R_{" << idx << "}(t)";
                views.back().Name() = sstr.str();
                views.push_back(bank_account_samples[idx]);
                sstr.str("");
                sstr << "B_{" << idx << "}(t)";
                views.back().Name() = sstr.str();
        }

        std::vector<ProcessView>* render_view = &views;

        size_t N = 1000;
        double dt = T / N;

        std::ofstream of{"BankAccount.csv"};
        if( ! of.is_open() )
                BOOST_THROW_EXCEPTION(std::domain_error("unable to open BankAccount.csv"));
        ProcessViewRenderer renderer{of, *render_view};

        for(size_t idx=0;idx!=N;++idx){
                ctx.Step(dt);
                renderer.RenderLine();
        }
        renderer.Emit();
}

#endif

struct Omega{};
struct Nul{};
struct Not;
struct Union;
struct Intersection;
struct Interval;

using BorelSet = boost::variant<
        Omega,
        Nul,
        boost::recursive_wrapper<Not>,
        boost::recursive_wrapper<Union>,
        boost::recursive_wrapper<Intersection>,
        boost::recursive_wrapper<Interval>
>;

struct Not{
        BorelSet child;
};
struct Union{
        template<class... Args>
        Union(Args&&... args):children{args...}{}

        void Add(Union const& that){
                for(auto const& _ : that.children)
                        children.push_back(_);
        }

        std::vector<BorelSet> children;
};
struct Intersection{
        template<class... Args>
        Intersection(Args&&... args):children{args...}{}

        void Add(Intersection const& that){
                for(auto const& _ : that.children)
                        children.push_back(_);
        }

        std::vector<BorelSet> children;
};
struct IntervalEndPoint{
        friend std::ostream& operator<<(std::ostream& ostr, IntervalEndPoint const& self){
                ostr << "IntervalEndPoint{is_open = " << self.is_open;
                ostr << ", point = " << self.point << "}";
                return ostr;
        }
        bool operator<(IntervalEndPoint const& that)const{
                if( point != that.point )
                        return point < that.point;
                return is_open < that.is_open;
        }
        bool operator==(IntervalEndPoint const& that)const{
                return point == that.point && is_open == that.is_open;
        }
        bool operator!=(IntervalEndPoint const& that)const{
                return ! operator==(that);
        }
        bool is_open;
        double point;

        IntervalEndPoint Switch()const{ return IntervalEndPoint{ ! is_open, point }; }
};
IntervalEndPoint Open(double x){
        return IntervalEndPoint{true, x};
}
IntervalEndPoint Closed(double x){
        return IntervalEndPoint{false, x};
}

struct IntervalUnion{
        template<class... Args>
        IntervalUnion(Args&&... args):children{args...}{}

        void Add(IntervalUnion const& that){
                for(auto const& _ : that.children)
                        children.push_back(_);
        }

        std::vector<Interval> children;

        operator Union()const{
                return AsUnion();
        }
        Union AsUnion()const{
                Union tmp;
                for(auto const& _ : children )
                        tmp.children.push_back(_);
                return tmp;
        }

        #if 0
        bool operator==(IntervalUnion const& that)const{
                return children == that.children;
        }
        bool operator!=(IntervalUnion const& that)const{
                return ! operator==(that);
        }
        #endif
        bool operator<(IntervalUnion const& that)const{
                if( children.size() != that.children.size() ){
                        return  children.size() < that.children.size();
                }
                return children < that.children;
        }
};

struct Interval{
        IntervalEndPoint left;
        IntervalEndPoint right;

        static Interval Closed(double x, double y){
                return Interval{ IntervalEndPoint{ false, x},
                                 IntervalEndPoint{ false, y} };
        }
        static Interval Open(double x, double y){
                return Interval{ IntervalEndPoint{ false, x},
                                 IntervalEndPoint{ false, y} };
        }

        bool operator<(Interval const& that)const{
                if( left != that.left )
                        return left < that.left;
                return right < that.right;
        }

        // homogenous operations are here
        IntervalUnion Not()const{
                IntervalUnion result;

                /*
                        A) this  \subset world => this
                        B) world \subset this => that

                 */

                if( left.point > right.point )
                        return result;

                if( 0.0 == left.point && left.is_open ){
                        result.children.push_back(Interval::Closed(0.0, 0.0));
                } else if( 0.0 < left.point ){
                        result.children.push_back(Interval{ IntervalEndPoint{false, 0.0},
                                                   left.Switch() } );
                }

                if( 1.0 == right.point && right.is_open ){
                        result.children.push_back(Interval::Closed(1.0, 1.0));
                } else if( right.point < 1.0 ){
                        result.children.push_back(Interval{ right.Switch(),
                                                   IntervalEndPoint{false, 1.0} } );
                }

                return result;
        }

        bool IsSubsetOf(Interval const& that)const{
                if( that.left.point > left.point )
                        return false;
                if( that.left.point ==left.point ){
                        if( that.left.is_open != left.is_open && that.left.is_open )
                                return false;
                }
                if( that.right.point < right.point )
                        return false;
                if( that.right.point ==right.point ){
                        if( that.right.is_open != right.is_open && that.right.is_open )
                                return false;
                }
                return true;
        }

};

struct SortIntervalUnions{
        bool operator()(Union const& a, Union const& b)const{
                if( a.children.size() != b.children.size() )
                        return a.children.size() < b.children.size();
                for(size_t idx=0;idx!=a.children.size();++idx){
                        auto a_interval = boost::get<Interval>(&a.children[idx]);
                        auto b_interval = boost::get<Interval>(&b.children[idx]);
                        BOOST_ASSERT( a_interval );
                        BOOST_ASSERT( b_interval );
                        if( a_interval->left != b_interval->left )
                                return  a_interval->left < b_interval->left;
                        if( a_interval->right != b_interval->right )
                                return  a_interval->right < b_interval->right;
                }
                return false;
        }
};

std::string ToString(BorelSet const& b){
        struct ToStringImpl{
                void operator()(Omega const&){
                        ostr_ << "Omega";
                }
                void operator()(Nul const&){
                        ostr_ << "Nul";
                }
                void operator()(Not const& obj){
                        ostr_ << "Not{";
                        boost::apply_visitor(*this, obj.child);
                        ostr_ << "}";
                }
                void operator()(Union const& obj){
                        ostr_ << "Union{";
                        for(size_t idx=0;idx!=obj.children.size();++idx){
                                if( idx != 0 )
                                        ostr_ << ", ";
                                boost::apply_visitor(*this, obj.children[idx]);
                        }
                        ostr_ << "}";
                }
                void operator()(Intersection const& obj){
                        ostr_ << "Intersection{";
                        for(size_t idx=0;idx!=obj.children.size();++idx){
                                if( idx != 0 )
                                        ostr_ << ", ";
                                boost::apply_visitor(*this, obj.children[idx]);
                        }
                        ostr_ << "}";
                }
                void operator()(Interval const& i){
                        ostr_ << ( i.left.is_open ? "(" : "[" );
                        ostr_ << i.left.point;
                        ostr_ << ",";
                        ostr_ << i.right.point;
                        ostr_ << ( i.right.is_open ? ")" : "]" );
                }
                std::stringstream ostr_;
        };
        ToStringImpl impl;
        boost::apply_visitor(impl, b);
        return impl.ostr_.str();
}

void Display(BorelSet const& b){
        std::cout << ToString(b) << "\n";
}

IntervalUnion ToIntervals(BorelSet const& b){
        struct ToIntervalsImpl : boost::static_visitor<IntervalUnion>{
                IntervalUnion operator()(Omega const&)const{
                        return IntervalUnion{Interval::Closed(0,1)};
                }
                IntervalUnion operator()(Nul const&)const{
                        return IntervalUnion{};
                }
                IntervalUnion operator()(Not const& obj)const{
                        Union result;
                        for(auto const& i : boost::apply_visitor(*this, obj.child).children ){
                                result.Add( i.Not().AsUnion() );
                        }
                        return operator()(result);
                }
                IntervalUnion operator()(Union const& obj)const{
                        IntervalUnion mapped;
                        for(auto const& _ : obj.children ){
                                for( auto const& inner : boost::apply_visitor(*this,_).children ){
                                        mapped.children.push_back(inner);
                                }
                        }
                        std::vector<Interval const*> subs;
                        for(auto const& _ : mapped.children ){
                                subs.push_back(&_);
                        }
                        boost::sort( subs, [](auto const& l, auto const& r){
                                if( l->left.point != r->left.point )
                                        return l->left.point < r->left.point;
                                // prefer closed
                                return l->left.is_open < r->left.is_open;
                        });

                        IntervalUnion result;
                        size_t iter = 0;
                        for(;iter!=subs.size();++iter){
                                if( subs[iter] == 0 )
                                        continue;

                                // for debugging
                                std::vector<Interval const*> dbg_path;
                                dbg_path.push_back(subs[iter]);

                                IntervalEndPoint left  = subs[iter]->left;
                                IntervalEndPoint right = subs[iter]->right;
                                // what is the largest path we can construct
                                subs[iter] = 0;
                                for(size_t j=iter+1;j!=subs.size();){
                                        if( subs[j] == 0 ){
                                                ++j;
                                                continue;
                                        }
                                        auto head = subs[j];

                                        // do these overlap?
                                        if( head->left.point < right.point ||
                                           (head->left.point ==right.point && ! (head->left.is_open && right.is_open) ) ){

                                                // now can we extend right?
                                                if( right.point < head->right.point ||
                                                   (right.point ==head->right.point && right.is_open && !head->right.is_open ) ){
                                                        right = head->right;
                                                        dbg_path.push_back(head);
                                                        // restart nice and slow
                                                        j = iter+1;
                                                        continue;
                                                }
                                        }
                                        ++j;
                                }
                                Interval i{left, right};

                                for(size_t j=iter+1;j!=subs.size();++j){
                                        if( subs[j] == 0 )
                                                continue;
                                        if( subs[j]->IsSubsetOf(i) ){
                                                subs[j] = 0;
                                        }
                                }

                                result.children.push_back(i);

                        }

                        return result;
                }
                IntervalUnion operator()(Intersection const& obj)const{
                        if( obj.children.empty() )
                                return IntervalUnion{};
                        IntervalUnion mapped;
                        for(auto const& _ : obj.children ){
                                for(auto const& inner : boost::apply_visitor(*this,_).children ){
                                        mapped.children.push_back(inner);
                                }
                        }

                        auto first = &mapped.children.at(0);

                        IntervalEndPoint const* upper_left  = &first->left;
                        IntervalEndPoint const* lower_right = &first->right;

                        for(auto const& i : mapped.children ){
                                auto ptr = &i;

                                if( upper_left->point < ptr->left.point )
                                        upper_left = &ptr->left;
                                else if(  upper_left->point == ptr->left.point &&
                                          ! upper_left->is_open &&
                                          ptr->left.is_open )
                                        upper_left = &ptr->left;

                                if( lower_right->point > ptr->right.point )
                                        lower_right = &ptr->right;
                                else if(  lower_right->point == ptr->right.point &&
                                          ! lower_right->is_open &&
                                          ptr->right.is_open )
                                        lower_right = &ptr->right;

                        }

                        std::cout << "*upper_left = " << *upper_left << "\n";
                        std::cout << "*lower_right = " << *lower_right << "\n";

                        if( upper_left->point <  lower_right->point ||
                            ( upper_left->point == lower_right->point
                              && ! upper_left->is_open && ! lower_right->is_open ) ){
                                return IntervalUnion{ Interval{ *upper_left, *lower_right} };
                        }

                        return IntervalUnion{};

                }
                IntervalUnion operator()(Interval const& i)const{
                        return IntervalUnion{i};
                }
        };

        auto tmp = boost::apply_visitor(ToIntervalsImpl(), b);
        boost::sort(tmp.children);
        return tmp;
}

struct BorelFamily : std::vector<BorelSet>{
        using impl_type = std::vector<BorelSet>;
        template<class... Args>
        BorelFamily(Args&&... args):impl_type{args...}{}
        friend std::ostream& operator<<(std::ostream& ostr, BorelFamily const& self){
                ostr << "{";
                for(size_t idx=0;idx!=self.size();++idx){
                        ostr << ( idx == 0 ? "" : ", " ) << ToString(self[idx]);
                }
                return ostr << "}";
        }
        void Display(std::ostream& out)const{
                for(size_t idx=0;idx!=size();++idx){
                        out << "    " << std::setw(2) << idx << " : " << ToString(at(idx)) << "\n";
                }
        }
};

#if 1
BorelFamily GenerateSigmaAlgebra(BorelFamily const& family){
        BorelFamily head = family;
        std::vector<BorelSet> to_add;

        std::set<IntervalUnion> interval_set;
        for(auto const& _ : family ){
                interval_set.insert( ToIntervals(_) );
        }

        auto test = [&](BorelSet const& b){
                auto iu = ToIntervals(b);
                if( ! interval_set.count( iu ) ){
                        std::cout << "====== found new ======\n";
                        std::cout << "    b  = " << ToString(b) << "\n";
                        std::cout << "    iu = " << ToString(iu.AsUnion()) << "\n";

                        to_add.push_back(b);
                        interval_set.insert( iu );
                        return 1;
                }
                return 0;
        };

        for(;;){
                int changes = 0;
                for(auto const& _ : head ){
                        auto complement = Not{_};
                        changes += test(complement);
                }
                for(size_t i=0;i+1<head.size();++i){
                        for(size_t j=i+1;j<head.size();++j){
                                auto u = Union{ head[i], head[j] };
                                changes += test(u);
                        }
                }
                if( changes == 0 )
                        break;
                for(auto const& _ : to_add )
                        head.push_back(_);
                //break;
        }

        BorelFamily result;
        for(auto const& _ : interval_set ){
                result.push_back( _.AsUnion() );
        }
        return result;

        return head;

}
#endif

int main(){

        BorelSet b = Intersection{ Interval{ Closed(0.0 ), Closed(0.25) },
                            Interval{ Open(0.25), Open(0.50) },
                            Interval{ Closed(0.1), Closed(0.6) } };
        Display(b);

        Display(ToIntervals(b).AsUnion());

        BorelFamily f0{ Omega(), Nul() };
        std::cout << "f0 = " << f0 << "\n";
        BorelFamily f1{ Omega(), Nul(),
                        Interval{ Closed(0.0), Open(0.5) },
                        Interval{ Closed(0.5), Closed(1.0) } };
        std::cout << "f1 = " << f1 << "\n";
        BorelFamily f2{ Omega(), Nul(),
                        Interval{ Closed(0.0), Open(0.25) },
                        Interval{ Closed(0.25), Open(0.50) },
                        Interval{ Closed(0.50), Open(0.75) },
                        Interval{ Closed(0.75), Closed(1.0) } };
        std::cout << "f2 = " << f2 << "\n";

        std::cout << "GenerateSigmaAlgebra(f2):\n";
        GenerateSigmaAlgebra(f2).Display(std::cout);

        //example_0();
        //example_1();
        //example_2();

}


//==================================================================================================
/*!
  @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_ACOSH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOSH_HPP_INCLUDED

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

 /*!

    @ingroup group-hyperbolic
    Function object implementing acosh capabilities

    Returns the hyperbolic cosine argument: \f$\log(x+\sqrt{x^2-1})\f$.

    @par Semantic:

    For every parameter of floating type @c T

    @code
    T r = acosh(x);
    @endcode

    @see log, sqrt, cosh, dec

  **/
  Value acosh(Value const & v0);
} }
#endif

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

#endif


#ifndef icvPrimitiveTensorData_hxx
#define icvPrimitiveTensorData_hxx

#include "OpenICV/Core/icvDataObject.h"

#include <vector>
#include <cstdarg>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_pod.hpp>

namespace icv { namespace data
{
    enum icvTensorIndexOrder
    {
        FromFirst, // Index from leftmost, i.e. the last index is contiguous, i.e. row major, i.e. lexicographical
        FromLast,  // Index from rightmost, i.e. the first index is contiguous, i.e column major, i.e. colexicographical
    };

    template<typename ElemT, icvTensorIndexOrder IndexOrder = FromFirst>
    class icvPrimitiveTensorData : public icv::core::icvDataObject
    {
    private:
        BOOST_STATIC_ASSERT_MSG(boost::is_pod<ElemT>::value, "icvPrimitiveTensorData only support POD types");

    public:
        icvPrimitiveTensorData(const std::vector<IndexType>& shape) :_shape(shape) {}

        IndexType GetSize() const
        {
            IndexType total = 1;
            for (auto count : _shape)
                total *= count;
            return total;
        }
        const std::vector<IndexType>& GetShape() const { return _shape; }

        virtual void Reserve() ICV_OVERRIDE { if (!_data) _data = new ElemT[GetSize()]; }
        virtual void Dispose() ICV_OVERRIDE { if (_data) delete[] _data; _data = ICV_NULLPTR; }

        void Reshape(const std::vector<IndexType>& shape) { throw "Not Implemented yet!"; }

        virtual Uint64 GetActualMemorySize() ICV_OVERRIDE
        {
            if (_data) return sizeof(ElemT) * GetSize();
            else return 0;
        }

        // TODO: Not implemented yet
        virtual void Serialize(std::stringstream& out, const uint32_t& version) const { throw "Not implemented yet!"; }
        virtual void Deserialize(std::stringstream& in, const uint32_t& version) { throw "Not implemented yet!"; }

        virtual icv::core::icvDataObject* DeepCopy() ICV_OVERRIDE
        {
            icvPrimitiveTensorData<ElemT, IndexOrder>* copy = new icvPrimitiveTensorData<ElemT, IndexOrder>(_shape);
            copy->_sourceTime = _sourceTime;
            copy->Reserve(); std::copy(_data, _data + GetSize(), copy->_data);
            return copy;
        }

        ElemT & At(const std::vector<IndexType>& index)
        {
            if (index.size() != _shape.size())
                ICV_THROW_MESSAGE("Input indices for At() should have the same length with the shape of tensor");
                // TODO: return an iterator if length of index is smaller than shape

            IndexType tidx = 0;
            if (IndexOrder == FromFirst)
                for (IndexType i = 0; i < index.size(); i++)
                    tidx = tidx * _shape[i] + index[i];
            else
            {
                for (IndexType i = index.size() - 1; i > 0; i--) // Avoid overflow of i
                    tidx = tidx * _shape[i] + index[i];
                tidx = tidx * _shape[0] + index[0];
            }

            return _data[tidx];
        }

        const ElemT & At(const std::vector<IndexType>& index) const
        {
            return const_cast<icvPrimitiveTensorData&>(*this).At(index);
        }

        // Note: use 'u' literal when calling this function
        template<typename... T>
        ElemT & At(T... indices)
        {
            std::vector<IndexType> ids{ indices... };
            return At(ids);
        }

        template<typename... T>
        const ElemT & At(T... indices) const
        {
            return const_cast<icvPrimitiveTensorData&>(*this).At(indices...);
        }

        virtual std::string Print() ICV_OVERRIDE
        {
            throw "Not implemented yet!";
        }

    protected:
        std::vector<IndexType> _shape;
        ElemT* _data = ICV_NULLPTR;
    };

    #define _ICV_DECLARE_TENSOR_DATA(type) typedef icvPrimitiveTensorData<type> icv##type##TensorData;
    ICV_BASIC_TYPES_TEMPLATE(_ICV_DECLARE_TENSOR_DATA);
    #undef _ICV_DECLARE_TENSOR_DATA

    // Common uses
    typedef icvPrimitiveTensorData<IndexType> icvIndexTensorData;
    typedef icvPrimitiveTensorData<int>       icvIntTensorData;
    typedef icvPrimitiveTensorData<double>    icvDoubleTensorData;
}}

#endif // icvPrimitiveTensorData_hxx


/*
(***********************************************************************)
(*                                                                     *)
(* The ACME project                                                    *)
(*                                                                     *)
(* Copyright (c) 2020-2021, Davide Stocco and Enrico Bertolazzi.       *)
(*                                                                     *)
(* The ACME project and its components are supplied under the terms of *)
(* the open source BSD 2-Clause License. The contents of the ACME      *)
(* project and its components may not be copied or disclosed except in *)
(* accordance with the terms of the BSD 2-Clause License.              *)
(*                                                                     *)
(* URL: https://opensource.org/licenses/BSD-2-Clause                   *)
(*                                                                     *)
(*    Davide Stocco                                                    *)
(*    Department of Industrial Engineering                             *)
(*    University of Trento                                             *)
(*    e-mail: davide.stocco@unitn.it                                   *)
(*                                                                     *)
(*    Enrico Bertolazzi                                                *)
(*    Department of Industrial Engineering                             *)
(*    University of Trento                                             *)
(*    e-mail: enrico.bertolazzi@unitn.it                               *)
(*                                                                     *)
(***********************************************************************)
*/

// TEST 8 - POINT/VECTOR TRANSFORMATION

#include <fstream>
#include <iostream>
#include <string>

#include <Eigen/Geometry>

#include "acme.hh"
#include "acme_aabb.hh"
#include "acme_intersection.hh"
#include "acme_triangle.hh"
#include "acme_utils.hh"

using namespace acme;

// Main function
int main()
{
  // Initialize point and vector
  point point_tmp(0.0, 0.0, 1.0);
  vec3 vector_tmp(0.0, 0.0, 1.0);

  // Initialize rotation matrix
  affine transformation = translate(0.0, 0.0, -1.0) * angleaxis(PI / 2, UNITX_VEC3);

  // Transform objects
  point mov_point(point_tmp);
  vec3 mov_vector(vector_tmp);
  mov_point.transform(transformation);
  acme::transform(mov_vector, transformation);

  // Transform objects back
  point ground_point(mov_point);
  vec3 ground_vector(mov_vector);
  ground_point.transform(transformation.inverse());
  acme::transform(ground_vector, transformation.inverse());

  // Display results
  std::cout
      << std::endl
      << "TEST 8 - POINT/VECTOR TRANSFORMATION" << std::endl
      << std::endl
      << "Point  (ground) = " << point_tmp << std::endl
      << "Vector (ground) = " << vector_tmp << std::endl
      << std::endl
      << "Point  (moving frame) = " << mov_point << std::endl
      << "Vector (moving frame) = " << mov_vector << std::endl
      << std::endl
      << "Point  (back to ground) = " << ground_point << std::endl
      << "Vector (back to ground) = " << ground_vector << std::endl
      << std::endl
      << "TEST 8: Completed" << std::endl;

  // Exit the program
  return 0;
}


#include "thin_lq.hpp"

#include <armadillo>
#include <stdexcept>

template <class Real>
void ThinLq(const arma::Mat<Real> &A, arma::Mat<Real> &L, arma::Mat<Real> &Q) {
    // A = Q * R  =>  A^T = R^T * Q^T

    arma::Mat<Real> L_temp;
    arma::Mat<Real> Q_temp;
    bool status = arma::qr_econ(Q_temp, L_temp, A.t());

    if (!status) {
        throw std::runtime_error("RQ decomposition failed");
    }

    Q = Q_temp.t();
    L = L_temp.t();
}

template void ThinLq<float>(const arma::Mat<float> &A, arma::Mat<float> &L,
                            arma::Mat<float> &Q);
template void ThinLq<double>(const arma::Mat<double> &A, arma::Mat<double> &L,
                             arma::Mat<double> &Q);


/**
 * @file generateData.cpp
 * @brief Create 3D simulation data for ICRA 2012 paper.
 * @author Michael Kaess
 * @author David Rosen
 * @version $Id: generateSpheresICRA2012.cpp 6377 2012-03-30 20:06:44Z kaess $
 */

#include <stdio.h>
#include <string>
#include <fstream>
#include <sstream>

#include <isam/Pose3d.h>

using namespace std;
using namespace isam;
using namespace Eigen;

//Number of samples to generate
const int num_samples = 1000;

//Covariances
const double sigmas[6] = {0.1, 0.1, 0.1, 0.04, 0.04, 0.04};

string directory_name = "sphere_data/";
string base_filename = "sample_";
string file_extension = ".txt";

char* write_string = new char[500];

const bool ADD_NOISE = true;
const bool LOOP_CLOSING = true;
// vehicle on surface of sphere, instead of upright (pitch=roll=0)
const bool PERPENDICULAR = true;

// sample from a normal distribution
#include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
static boost::minstd_rand generator(27u);
double sample_normal(double sigma = 1.0)
{
  typedef boost::normal_distribution<double> Normal;
  Normal dist(0.0, sigma);
  boost::variate_generator<boost::minstd_rand&, Normal> norm(generator, dist);
  return(norm());
}

void write_constraint(ofstream& outfile, int id0, int id1, const Pose3d& delta_) {
  Pose3d delta = delta_;
  if (ADD_NOISE) {
    // corrupt measurement with Gaussian noise
    VectorXd v = delta.vector();
    for (int i=0; i<6; i++) {
      v(i) += sample_normal(sigmas[i]);
    }
    delta.set(v);
  }

  // X Y Z roll pitch yaw
  sprintf(write_string, "EDGE3 %i %i %g %g %g %g %g %g",
          id0, id1, delta.x(), delta.y(), delta.z(), delta.roll(), delta.pitch(), delta.yaw());
  outfile << write_string;
  for (int i=0; i<6; i++) {
    for (int j=i; j<6; j++) {
      double sqrtinf = 0.;
      // only diagonal entries populated
      if (i==j) {
        // roll,pitch,yaw order
        if (i<3) {
          sqrtinf = 1./sigmas[i];
        } else {
          sqrtinf = 1./sigmas[8-i];
        }
      }
      sprintf(write_string, " %g", sqrtinf);
      outfile << write_string;
    }
  }
  outfile << "\n";
}

void add_constraint(ofstream& outfile, Pose3d* poses, int id0, int id1) {
  Pose3d delta = poses[id1].ominus(poses[id0]);
  write_constraint(outfile, id0, id1, delta);
}

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

  // Make sure that we have a 'test_data' directory.
  int ret = system(("mkdir " + directory_name).c_str());
  require(ret!=-1, "Failed to created directory.");

  for (int s = 1; s <= num_samples; s++) {
    // Form the filename that will be used to hold the output of this sample.

    stringstream current_filename;

    current_filename << directory_name;
    current_filename << base_filename;
    current_filename << s;
    current_filename << file_extension;

    // Use this filename to open up a new file for writing.
    ofstream outfile( (current_filename.str()).c_str());

    // generate poses on the surface of a sphere
    const int steps = 50; // number of poses for one turn around the sphere
    const int slices = 50; // number of rounds around the sphere
    const double radius0 = 2.; // starting radius
    const double radius = 50.; // sphere radius

    int n = steps*slices;
    Pose3d poses[n];
    int pose_id = 0;
    // current vertical angle for elevation/slice (starting angle and increment)
    double alpha = atan(radius0/radius);
    double d_alpha = (M_PI-2*alpha) / (double)(n);
    // angle in horizontal plane (starting angle and increment)
    double phi = 0.;
    double d_phi = 2*M_PI / (double)steps;
    for (int i=0; i<steps; i++) {
      for (int j=0; j<slices; j++) {
        // calculate position
        double r = radius * sin(alpha);
        double h = sqrt(radius*radius - r*r);
        // bottom half of sphere?
        if (i<(slices/2)) h = -h;
        // calculate pose
        double y = r * sin(phi);
        double x = r * cos(phi);
        double z = -(radius + h);
        double yaw = phi + M_PI/2.0;
        double roll = 0.;
        if (PERPENDICULAR) {
          roll = alpha;
        }
        // generate measurements
        poses[pose_id] = Pose3d(x,y,z, yaw,0.,roll);
        if (pose_id>0) {

          add_constraint(outfile, poses, pose_id-1, pose_id);
          if (LOOP_CLOSING) {
            if (pose_id >= steps) {
              add_constraint(outfile, poses, pose_id-steps, pose_id);
            }
          }
        }
        // update for next step
        alpha += d_alpha;
        phi += d_phi;
        pose_id++;
      }
    }
  }
}


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

#include "ssmkit/distribution/gaussian.hpp"

#define STR_EXPAND(tok) #tok
#define STR(tok) STR_EXPAND(tok)

using namespace ssmkit;

BOOST_AUTO_TEST_SUITE(distribution_gausian);

// Number of Monte-Carlo runs
constexpr int mc_n = 500000;

BOOST_AUTO_TEST_CASE(mc_test_random_default_pdf) {
  // testing distribution with default constructor

  constexpr int dimension = 8;
  // 8-dimensional Gaussian distribution with zero mean identity covariance
  distribution::Gaussian pdf(dimension);

  // set random seed
  random::setRandomSeed();

  // sampling large number of random variables from distribution
  arma::mat samples(dimension, mc_n);
  samples.each_col([&pdf](arma::vec &col) { col = pdf.random(); });
  // Calculating sample mean
  auto mc_mean = arma::sum(samples, 1) / mc_n;
  // Calculating sample covariance
  auto mc_covariance = arma::cov(samples.t());

  // check if sample mean is close to zero
  BOOST_CHECK(arma::approx_equal(mc_mean, arma::zeros<arma::vec>(dimension),
                                 "absdiff", 0.01));
  // check if sample covariance is close to identity
  BOOST_CHECK(arma::approx_equal(mc_covariance,
                                 arma::eye<arma::mat>(dimension, dimension),
                                 "absdiff", 0.01));
}

BOOST_AUTO_TEST_CASE(mc_test_random_arbitary_pdf) {
  // testing distribution with given mean and covariance

  // construction distribution
  constexpr int dimension = 2;
  arma::vec mean{89, 16};
  arma::mat chol{{10, 1}, {0, 2}};
  arma::mat covariance = chol.t() * chol;
  distribution::Gaussian pdf(mean, covariance);

  // set random seed
  random::setRandomSeed();

  // sampling large number of random variables from distribution
  arma::mat samples(dimension, mc_n);
  samples.each_col([&pdf](arma::vec &col) { col = pdf.random(); });
  // Calculating sample mean
  auto mc_mean = arma::sum(samples, 1) / mc_n;
  // Calculating sample covariance
  auto mc_covariance = arma::cov(samples.t());

  // check if sample mean is close to the given mean
  BOOST_CHECK(arma::approx_equal(mean, mc_mean, "absdiff", 0.1));
  // check if sample covariance is close to given covariance
  BOOST_CHECK(arma::approx_equal(covariance, mc_covariance, "absdiff", 0.5));
}

BOOST_AUTO_TEST_CASE(likelihood_test) {
  // check the likelihood function using precomputed data
  arma::mat rvs;
  arma::mat ln;
  arma::mat ln5;
  arma::mat lnm;
  arma::mat lnc;

  BOOST_REQUIRE(rvs.load(STR(SOURCE_DIR) "/test/data/gaussian/rvs.csv"));
  BOOST_REQUIRE(ln.load(STR(SOURCE_DIR) "/test/data/gaussian/ln.csv"));
  BOOST_REQUIRE(ln5.load(STR(SOURCE_DIR) "/test/data/gaussian/ln5.csv"));
  BOOST_REQUIRE(lnm.load(STR(SOURCE_DIR) "/test/data/gaussian/lnm.csv"));
  BOOST_REQUIRE(lnc.load(STR(SOURCE_DIR) "/test/data/gaussian/lnc.csv"));

  distribution::Gaussian pdf(3);
  arma::mat ln_test(size(ln));
  auto it_ln_test = ln_test.begin();
  rvs.each_col([&pdf, &it_ln_test](arma::vec &col) {
    *it_ln_test++ = pdf.likelihood(col);
  });
  BOOST_CHECK(arma::approx_equal(ln, ln_test, "absdiff", 0.001));

  pdf.parameterize(arma::zeros<arma::vec>(3), arma::eye<arma::mat>(3, 3) * 5.0);
  arma::mat ln5_test(size(ln5));
  auto it_ln5_test = ln5_test.begin();
  rvs.each_col([&pdf, &it_ln5_test](arma::vec &col) {
    *it_ln5_test++ = pdf.likelihood(col);
  });
  BOOST_CHECK(arma::approx_equal(ln5, ln5_test, "absdiff", 0.001));

  pdf.parameterize({1, 2, 3}, arma::eye<arma::mat>(3, 3) * 5.0);
  arma::mat lnm_test(size(lnm));
  auto it_lnm_test = lnm_test.begin();
  rvs.each_col([&pdf, &it_lnm_test](arma::vec &col) {
    *it_lnm_test++ = pdf.likelihood(col);
  });
  BOOST_CHECK(arma::approx_equal(lnm, lnm_test, "absdiff", 0.001));

  arma::mat chol{{1, 1, 1}, {0, 1, 1}, {0, 0, 1}};
  arma::mat covariance = chol.t() * chol;
  pdf.parameterize(arma::zeros<arma::vec>(3), covariance);
  arma::mat lnc_test(size(lnc));
  auto it_lnc_test = lnc_test.begin();
  rvs.each_col([&pdf, &it_lnc_test](arma::vec &col) {
    *it_lnc_test++ = pdf.likelihood(col);
  });
  BOOST_CHECK(arma::approx_equal(lnc, lnc_test, "absdiff", 0.001));
}

BOOST_AUTO_TEST_SUITE_END();


//#include <iostream>
//#include <ostream>
//#include <fstream>
//#include <armadillo>
//#include "master.hpp"
//
//Rzxy(,)
//F_A: functions for the Agisoft conventions
//double yaw_rotZXY(mat Rzxy,bool giveRadians=false)
//{
//
//    double c = (180/datum::pi);
//
//    if(giveRadians)
//    {
//        c = 1.0f;
//    }
//    F_A
//    return atan2(Rzxy(0,1),Rzxy(1,1)) * c;
//}
//
//double pitch_rotZXY(mat Rzxy,bool giveRadians=false)
//{
//    double c = (180/datum::pi);
//
//    if(giveRadians)
//    {
//        c = 1.0f;
//    }
//    F_A
//    return -asin(Rzxy(2,1)) * c;
//}
//
//double roll_rotZXY(mat Rzxy,bool giveRadians=false)
//{
//    double c = (180/datum::pi);
//
//    if(giveRadians)
//    {
//        c = 1.0f;
//    }
//    F_A
//    return atan2(Rzxy(2,0),Rzxy(2,2)) * c;
//}
//
//mat one_axis_rot(double a,int axis,bool giveInverseT=false,bool isRadians=false)
//{
//
//    these will give you the rotation in a CLOCKWISE way
//
//    mat res = eye(3,3);
//    double c = (datum::pi/180);
//
//    if (isRadians)
//    {
//        c = 1.0f;
//    }
//
//    double ca = cos(a*c);
//    double sa = sin(a*c);
//
//    if(axis == 1)
//    {
//        res(1,1) =  ca;
//        res(1,2) =  sa;
//        res(2,1) = -sa;
//        res(2,2) =  ca;
//    }
//    if(axis == 2)
//    {
//        res(0,0) =  ca;
//        res(0,2) = -sa;
//        res(2,0) =  sa;
//        res(2,2) =  ca;
//    }
//    if (axis == 3)
//    {
//        res(0,0) =  ca;
//        res(0,1) =  sa;
//        res(1,0) = -sa;
//        res(1,1) =  ca;
//    }
//
//    if (giveInverseT)
//    {res = res.t();}
//
//    return res;
//}
//
//mat novatel_DCM(double roll,double pitch,double azimuth,bool isRadians=false,bool giveInverseT=false)
//{
//    matrix that gives the transformation
//    FROM   BODY FRAME
//    TO     LOCAL LEVEL FRAME
//
//    accordingly to the manual, is a combination of the following order:
//    Rz * Rx * Ry  or R3 * R1 * R2 , all of them TRANSPOSED
//
//    NOVATEL definitions:
//    roll   is around the Y axis
//    pitch  is around the X axis
//    yaw    is around the Z axis (and also Azimuth)
//
//    there is the fourth argument that can be used to give the inverse transformation
//
//    mat res = zeros(3,3);
//
//    double c = (datum::pi/180);
//
//    if (isRadians)
//    {
//        c = 1.0f;
//    }
//
//    conversion between azimuth and yaw
//    double yaw = -azimuth;
//
//    double cr = cos(roll * c);
//    double sr = sin(roll * c);
//
//    double cp = cos(pitch * c);
//    double sp = sin(pitch * c);
//
//    double cy = cos(yaw * c);
//    double sy = sin(yaw * c);
//
//    linewise splitted
//    res(0,0)= cy*cr-sy*sp*sr;
//    res(0,1)= -sy*cp;
//    res(0,2)= cy*sr + sy*sp*cr;
//
//    res(1,0)=  sy*cr+cy*sp*sr;
//    res(1,1)=  cy*cp;
//    res(1,2)=  sy*sr-cy*sp*cr;
//
//    res(2,0)=-cp*sr;
//    res(2,1)= sp;
//    res(2,2)= cp*cr;
//
//    if (giveInverseT)
//    {res = res.t();}
//
//    return res;
//}
//
//
//struct outputPoseToPhotoscan
//{
//    struct for individual data
//    string img_name;
//    double lat,lgt,h,yaw,pitch,roll;
//    double lat0,lgt0,h0; //coordinates of the origin
//
//    vec3 v0ECEF;
//
//    mat attMat;
//    outputPoseToPhotoscan(vec3 vecBF,mat bsight,unsigned int ind);
//};
//
//outputPoseToPhotoscan::outputPoseToPhotoscan(vec3 vBF,mat bsight,unsigned int ind)
//{
//    constants for multiplication and transformation
//    double to_deg = 180/datum::pi;
//    double to_rad = datum::pi/180;
//
/// First: position of the camera CP
//    filling the origin
//    lat0 = job.finalObsWcovs.at(ind).observation.lat;
//    lgt0 = job.finalObsWcovs.at(ind).observation.lgt;
//    h0   = job.finalObsWcovs.at(ind).observation.h;
//
//    converting the origin to XYZ
//    GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
//    GEODESY_REFERENCE_ELLIPSE_WGS84,
//    lat0*to_rad,lgt0*to_rad,h0,&v0ECEF(0),&v0ECEF(1),&v0ECEF(2));
//
//    compute the rotation matrix
//    mat R = novatel_DCM(
//    job.finalObsWcovs.at(ind).observation.roll,
//    job.finalObsWcovs.at(ind).observation.pitch,
//    job.finalObsWcovs.at(ind).observation.azimuth);
//
//    transform the vector to the LLF
//    vec3 vLLF = R * vBF;
//    transform to the ECEF system
//    vec3 vecECEF = ENU_to_ECEF(vLLF,v0ECEF,lat0,lgt0,h0);
//
//    transform to lat, long, h
//    GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
//    GEODESY_REFERENCE_ELLIPSE_WGS84,vecECEF(0),vecECEF(1),vecECEF(2),&lat,&lgt,&h);
//
//    converting to degrees:
//    lat *= to_deg;lgt *= to_deg;
//
//    / second: orientation
//
//         R is from IMU BF to IMU CN LLF
//
//        matrix from ECEF to IMU CN LLF
//        mat Rel1 = R_ecef_enu(lat0,lgt0);
//
//        matrix from ECEF to camera CP LLF
//        mat Rel2 = R_ecef_enu(lat,lgt);
//
//        matrix from IMU CN LLF to camera CP LLF
//        mat Rl1l2 = Rel1 * Rel2.t();
//
//        matrix from the camera CP LLF to IMU BF
//        mat Rl2bf1 = R.t() * Rl1l2.t();
//
//        finally, the camera BF to camera CP LLF
//        the bsight needs to be Rbf2bf1, aka from Camera BF to IMU
//        attMat = Rl2bf1.t()*bsight.t();
//
//        now the yaw pitch roll to photoscan
//        yaw     = yaw_rotZXY(attMat);
//        pitch   = pitch_rotZXY(attMat);
//        roll    = roll_rotZXY(attMat);
//    }
//
//struct outputterToPhotoscan
//{
// vector<outputPoseToPhotoscan> individuals;
//
//
//
// outputterToPhotoscan(void);
//};
//
//outputterToPhotoscan::outputterToPhotoscan(void)
//{
// ofstream outL("to_photoscan_left.txt");
// ofstream outR("to_photoscan_right.txt");
//
// string separator = "   ";
//
//    for (unsigned int i=0;i<job.finalObsWcovs.size();i++)
//    {
//        outputPoseToPhotoscan  Left(rCalib.LcamLA,rCalib.Rimu_LC.t(),i);
//        outputPoseToPhotoscan Right(rCalib.RcamLA,rCalib.Rimu_RC.t(),i);
//
//        outL<<Left.img_name<<separator<<Left.lat<<separator<<Left.lgt<<separator<<Left.h;
//        outL<<separator<<Left.yaw<<separator<<Left.pitch<<separator<<Left.roll<<endl;
//
//        outR<<Right.img_name<<separator<<Right.lat<<separator<<Right.lgt<<separator<<Left.h;
//        outR<<separator<<Right.yaw<<separator<<Right.pitch<<separator<<Right.roll<<endl;
//    }
//}
//


#include "stdafx.h"
#include "qmath.h"
#include "config.h"

// each test module could contain no more then one 'main' file with init function defined
// alternatively you could define init function yourself
#include <boost/test/unit_test.hpp>

// These are sample tests that show the different features of the framework

using namespace math;

namespace {

BOOST_AUTO_TEST_CASE(SQRT)
{
	{
		float a = 4.f;
		float b = sqrt(a);

		BOOST_CHECK(equals(b, 2.f));
	}

	{
		double a = 4.0;
		double b = sqrt(a);

		BOOST_CHECK(equals(b, 2.0));
	}

	{
		vec2f a(4.f, 16.f);
		vec2f b = sqrt(a);

		BOOST_CHECK(equals(b, vec2f(2.f, 4.f)));
	}

	{
		vec3f a(4.f, 16.f, 25.f);
		vec3f b = sqrt(a);

		BOOST_CHECK(equals(b, vec3f(2.f, 4.f, 5.f)));
	}

	{
		vec4f a(4.f, 16.f, 25.f, 36.f);
		vec4f b = sqrt(a);

		BOOST_CHECK(equals(b, vec4f(2.f, 4.f, 5.f ,6.f)));
	}
}

BOOST_AUTO_TEST_CASE(SQRT_FAST)
{
	{
		float a = 4.f;
		float b = sqrt<float, fast>(a);

		BOOST_CHECK(equals(b, 2.f, 0.005f));
	}

	{
		double a = 4.0;
		double b = sqrt<double, fast>(a);

		BOOST_CHECK(equals(b, 2.0, 0.005));
	}

	{
		vec2f a(4.f, 16.f);
		vec2f b = sqrt<float, fast>(a);

		BOOST_CHECK(equals(b, vec2f(2.f, 4.f), 0.005f));
	}

	{
		vec3f a(4.f, 16.f, 25.f);
		vec3f b = sqrt<float, fast>(a);

		BOOST_CHECK(equals(b, vec3f(2.f, 4.f, 5.f), 0.005f));
	}

	{
		vec4f a(4.f, 16.f, 25.f, 36.f);
		vec4f b = sqrt<float, fast>(a);

		BOOST_CHECK(equals(b, vec4f(2.f, 4.f, 5.f ,6.f), 0.005f));
	}
}

BOOST_AUTO_TEST_CASE(INV_SQRT)
{
	{
		float a = 4.f;
		float b = inv_sqrt(a);

		BOOST_CHECK(equals(b, 0.5f));
	}

	{
		double a = 4.0;
		double b = inv_sqrt(a);

		BOOST_CHECK(equals(b, 0.5));
	}

	{
		vec2f a(4.f, 16.f);
		vec2f b = inv_sqrt(a);

		BOOST_CHECK(equals(b, vec2f(0.5f, 0.25f)));
	}

	{
		vec3f a(4.f, 16.f, 25.f);
		vec3f b = inv_sqrt(a);

		BOOST_CHECK(equals(b, vec3f(0.5f, 0.25f, 0.2f)));
	}

	{
		vec4f a(4.f, 16.f, 25.f, 36.f);
		vec4f b = inv_sqrt(a);

		BOOST_CHECK(equals(b, vec4f(0.5f, 0.25f, 0.2f, 1.f / 6.f)));
	}
}

BOOST_AUTO_TEST_CASE(INV_SQRT_FAST)
{
	{
		float a = 4.f;
		float b = inv_sqrt<float, fast>(a);

		BOOST_CHECK(equals(b, 0.5f, 0.001f));
	}

	{
		double a = 4.0;
		double b = inv_sqrt<double, fast>(a);

		BOOST_CHECK(equals(b, 0.5, 0.001));
	}

	{
		vec2f a(4.f, 16.f);
		vec2f b = inv_sqrt<float, fast>(a);

		BOOST_CHECK(equals(b, vec2f(0.5f, 0.25f), 0.001f));
	}

	{
		vec3f a(4.f, 16.f, 25.f);
		vec3f b = inv_sqrt<float, fast>(a);

		BOOST_CHECK(equals(b, vec3f(0.5f, 0.25f, 0.2f), 0.001f));
	}

	{
		vec4f a(4.f, 16.f, 25.f, 36.f);
		vec4f b = inv_sqrt<float, fast>(a);

		BOOST_CHECK(equals(b, vec4f(0.5f, 0.25f, 0.2f, 1.f / 6.f), 0.001f));
	}
}

BOOST_AUTO_TEST_CASE(POSITIVE_ZERO)
{
	{
		{
			std::stringstream str;
			str << -0.f;
			BOOST_CHECK(str.str() == "-0");
		}
		{
			std::stringstream str;
			str << positive_zero(-0.f);
			BOOST_CHECK(str.str() == "0");
		}
		{
			std::stringstream str;
			str << positive_zero(0.f);
			BOOST_CHECK(str.str() == "0");
		}
		{
			std::stringstream str;
			str << positive_zero(-7.f);
			BOOST_CHECK(str.str() == "-7");
		}
		{
			std::stringstream str;
			str << positive_zero(7.f);
			BOOST_CHECK(str.str() == "7");
		}
	}
}

BOOST_AUTO_TEST_CASE(ABS_SGN)
{
	float a = -1.f;
	float b = math::abs(a);

	BOOST_CHECK(equals(b, -a));
	BOOST_CHECK(sgn(a) == a);
	BOOST_CHECK(sgn(b) == b);

	double c = -12123412.187246;
	double d = abs(c);

	BOOST_CHECK(equals(d, -c));
	BOOST_CHECK(sgn(d) == 1);
	BOOST_CHECK(sgn(c) == -1);

	int e = -10;
	int f = abs(e);

	BOOST_CHECK(equals(f, -e));
	BOOST_CHECK(sgn(e) == -1);
	BOOST_CHECK(sgn(f) == 1);

	int64_t g = -129381094710288L;
	int64_t h = math::abs(g);

	BOOST_CHECK(equals(h, -g));
	BOOST_CHECK(sgn(g) == -1);
	BOOST_CHECK(sgn(h) == 1);

	vec2f a2(0.f, -1.f);
	vec2f b2 = abs(a2);

	BOOST_CHECK(equals(b2, -a2));
	BOOST_CHECK(sgn(a2) == a2);
	BOOST_CHECK(sgn(b2) == b2);

	vec3f a3(0.f, -1.f, -1.f);
	vec3f b3 = abs(a3);

	BOOST_CHECK(equals(b3, -a3));
	BOOST_CHECK(sgn(a3) == a3);
	BOOST_CHECK(sgn(b3) == b3);

	vec4f a4(0.f, -1.f, 0.0f, -1.f);
	vec4f b4 = abs(a4);

	BOOST_CHECK(equals(b4, -a4));
	BOOST_CHECK(sgn(a4) == a4);
	BOOST_CHECK(sgn(b4) == b4);
}

BOOST_AUTO_TEST_CASE(INVERSE_TRANSPOSE)
{
	{
		float a = 10.f;
		float b = inverse(a);
		BOOST_CHECK(equals(b, 0.1f, math::epsilon<float>()));
	}

	{
		double a = 10.f;
		double b = inverse(a);
		double c = 0.1;
		BOOST_CHECK(equals(b, c));
	}

	{
		mat3f a;
		a.set_row(0, vec3f(1.f, 1.f, 1.f));
		a.set_row(1, vec3f(2.f, 3.f, 2.f));
		a.set_row(2, vec3f(4.f, 5.f, 6.f));

		mat3f b = inverse(a);

		BOOST_CHECK(equals(b.get_row(0), vec3f(4.f, -0.5f, -0.5f), math::epsilon<float>()));
		BOOST_CHECK(equals(b.get_row(1), vec3f(-2.f, 1.f, 0.f), math::epsilon<float>()));
		BOOST_CHECK(equals(b.get_row(2), vec3f(-1.f, -0.5f, 0.5f), math::epsilon<float>()));

		mat3f c = transposed(a);

		BOOST_CHECK(equals(c.get_row(0), vec3f(1.f, 2.f, 4.f), math::epsilon<float>()));
		BOOST_CHECK(equals(c.get_row(1), vec3f(1.f, 3.f, 5.f), math::epsilon<float>()));
		BOOST_CHECK(equals(c.get_row(2), vec3f(1.f, 2.f, 6.f), math::epsilon<float>()));

	}

	{
		mat4f a;
		a.set_row(0, vec4f(1.f, 2.f, 2.f, 1.f));
		a.set_row(1, vec4f(2.f, 1.f, 2.f, 1.f));
		a.set_row(2, vec4f(2.f, 2.f, 1.f, 2.f));
		a.set_row(3, vec4f(1.f, 1.f, 2.f, 1.f));

		mat4f b = inverse(a);

		BOOST_CHECK(equals(b.get_row(0), vec4f(0.f, 1.f, 0.f, -1.f), math::epsilon<float>()));
		BOOST_CHECK(equals(b.get_row(1), vec4f(1.f, 0.f, 0.f, -1.f), math::epsilon<float>()));
		BOOST_CHECK(equals(b.get_row(2), vec4f(0.f, 0.f, -0.33333333f, 0.66666666f), math::epsilon<float>()));
		BOOST_CHECK(equals(b.get_row(3), vec4f(-1.f, -1.f, 0.66666666f, 1.66666666f), math::epsilon<float>()));

		mat4f c = transposed(a);

		BOOST_CHECK(equals(c.get_row(0), vec4f(1.f, 2.f, 2.f, 1.f), math::epsilon<float>()));
		BOOST_CHECK(equals(c.get_row(1), vec4f(2.f, 1.f, 2.f, 1.f), math::epsilon<float>()));
		BOOST_CHECK(equals(c.get_row(2), vec4f(2.f, 2.f, 1.f, 2.f), math::epsilon<float>()));
		BOOST_CHECK(equals(c.get_row(3), vec4f(1.f, 1.f, 2.f, 1.f), math::epsilon<float>()));
	}

	{
		quatf a(0,1,1,1);
		quatf b = inverse(a);
		quatf c(0,-1,-1,1);
		BOOST_CHECK(equals(b, c));
	}
}

BOOST_AUTO_TEST_CASE(NORMALIZE)
{
	{
		 vec2f c(3.f, 4.f);
		 vec2f d = normalized(c);
		 BOOST_CHECK(equals(d, vec2f(0.6f, 0.8f)));
	}
	{
		 vec3f c(0.f, 3.f, 4.f);
		 vec3f d = normalized(c);
		 BOOST_CHECK(equals(d, vec3f(0.0f, 0.6f, 0.8f)));
	}
	{
		 vec4f c(0.f, 3.f, 4.f, 0.f);
		 vec4f d = normalized(c);
		 BOOST_CHECK(equals(d, vec4f(0.0f, 0.6f, 0.8f, 0.0f)));
	}
	{
		 quatf a;
		 quatf b = normalized(a);
		 BOOST_CHECK(equals(b, quatf()));

		 //quatf c(kPI, kPI, kPI, 1.f);
		 //quatf d = normalized(c);
		 //BOOST_CHECK(equals(d, quatf()));
	}
}

BOOST_AUTO_TEST_CASE(NORMALIZE_FAST)
{
	{
// 		vec2f a;
// 		vec2f b = normalized<float, fast>(a);
// 		BOOST_CHECK(equals(b, vec2f(), 0.001f));

		vec2f c(3.f, 4.f);
		vec2f d = normalized<float, fast>(c);
		BOOST_CHECK(equals(d, vec2f(0.6f, 0.8f), 0.005f));
	}
	{
		vec3f c(0.f, 3.f, 4.f);
		vec3f d = normalized<float, fast>(c);
		BOOST_CHECK(equals(d, vec3f(0.0f, 0.6f, 0.8f), 0.005f));
	}
	{
		vec4f c(0.f, 3.f, 4.f, 0.f);
		vec4f d = normalized<float, fast>(c);
		BOOST_CHECK(equals(d, vec4f(0.0f, 0.6f, 0.8f, 0.0f), 0.005f));
	}
	{
		quatf a;
		quatf b = normalized<float, fast>(a);
		BOOST_CHECK(equals(b, quatf(), 0.005f));

		//quatf c(kPI, kPI, kPI, 1.f);
		//quatf d = normalized(c);
		//BOOST_CHECK(equals(d, quatf()));
	}
}

BOOST_AUTO_TEST_CASE(DOT)
{
	{
		 vec2f a(1.f, 2.f);
		 vec2f b(3.f, 4.f);
		 float c = dot(a, b);
		 BOOST_CHECK(equals(c, 11.f));
	}
	{
		 vec3f a(1.f, 2.f, 3.f);
		 vec3f b(4.f, 5.f, 6.f);
		 float c = dot(a, b);
		 BOOST_CHECK(equals(c, 32.f));
	}
	{
		 vec4f a(1.f, 2.f, 3.f, 4.f);
		 vec4f b(5.f, 6.f, 7.f, 8.f);
		 float c = dot(a, b);
		 BOOST_CHECK(equals(c, 70.f));
	}
}
BOOST_AUTO_TEST_CASE(CROSS)
{
	{
		 vec2f a;
		 vec2f b;
		 float c = cross(a, b);
		 BOOST_CHECK(equals(c, 0.f));
	}
	{
		 vec3f a;
		 vec3f b;
		 vec3f c = cross(a, b);
		 BOOST_CHECK(equals(b, vec3f()));
	}
	{
		 vec4f a;
		 vec4f b;
		 vec4f c;
		 vec4f d = cross(a, b, c);
		 BOOST_CHECK(equals(d, vec4f()));
	}
}

BOOST_AUTO_TEST_CASE(DISTANCE)
{
	{
		 vec2f a;
		 vec2f b;
		 float c = distance(a, b);
		 float d = distance_sq(a, b);
		 BOOST_CHECK(equals(c, 0.f, math::epsilon<float>()));
		 BOOST_CHECK(equals(d, 0.f, math::epsilon<float>()));
	}
	{
		vec2f a(0.32f, 0);
		vec2f b(0, 0.12f);
		float c = distance(a, b);
		float d = distance_sq(a, b);
		BOOST_CHECK(equals(c, 0.341760129f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.116799995f, math::epsilon<float>()));
	}
	{
		 vec3f a;
		 vec3f b;
		 float c = distance(a, b);
		 float d = distance_sq(a, b);
		 BOOST_CHECK(equals(c, 0.f, math::epsilon<float>()));
		 BOOST_CHECK(equals(d, 0.f, math::epsilon<float>()));
	}
	{
		vec3f a(0.32f, 0, 0);
		vec3f b(0, 0.12f, 0);
		float c = distance(a, b);
		float d = distance_sq(a, b);
		BOOST_CHECK(equals(c, 0.341760129f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.116799995f, math::epsilon<float>()));
	}
	{
		 vec4f a;
		 vec4f b;
		 float c = distance(a, b);
		 float d = distance_sq(a, b);
		 BOOST_CHECK(equals(c, 0.f, math::epsilon<float>()));
		 BOOST_CHECK(equals(d, 0.f, math::epsilon<float>()));
	}
	{
		vec4f a(0.32f, 0, 0, 0);
		vec4f b(0, 0.12f, 0, 0);
		float c = distance(a, b);
		float d = distance_sq(a, b);
		BOOST_CHECK(equals(c, 0.341760129f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.116799995f, math::epsilon<float>()));
	}
}

BOOST_AUTO_TEST_CASE(LENGTH)
{
	{
		vec2f a;
		float c = length(a);
		float d = length_sq(a);
		BOOST_CHECK(equals(c, 0.f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.f, math::epsilon<float>()));
	}
	{
		vec2f a(0.32f, 0);
		float c = length(a);
		float d = length_sq(a);
		BOOST_CHECK(equals(c, 0.32f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.1024f, math::epsilon<float>()));
	}
	{
		vec3f a;
		float c = length(a);
		float d = length_sq(a);
		BOOST_CHECK(equals(c, 0.f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.f, math::epsilon<float>()));
	}
	{
		vec3f a(0.32f, 0, 0);
		float c = length(a);
		float d = length_sq(a);
		BOOST_CHECK(equals(c, 0.32f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.1024f, math::epsilon<float>()));
	}
	{
		vec4f a;
		float c = length(a);
		float d = length_sq(a);
		BOOST_CHECK(equals(c, 0.f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.f, math::epsilon<float>()));
	}
	{
		vec4f a(0.32f, 0, 0, 0);
		float c = length(a);
		float d = length_sq(a);
		BOOST_CHECK(equals(c, 0.32f, math::epsilon<float>()));
		BOOST_CHECK(equals(d, 0.1024f, math::epsilon<float>()));
	}
}

BOOST_AUTO_TEST_CASE(DISTANCE_FAST)
{
	{
		vec2f a;
		vec2f b;
		float c = distance<float, fast>(a, b);
		BOOST_CHECK(equals(c, 0.f, 0.005f));
	}
	{
		vec2f a(0.32f, 0);
		vec2f b(0, 0.12f);
		float c = distance<float, fast>(a, b);
		BOOST_CHECK(equals(c, 0.341760129f, 0.005f));
	}
	{
		vec3f a;
		vec3f b;
		float c = distance<float, fast>(a, b);
		BOOST_CHECK(equals(c, 0.f, 0.005f));
	}
	{
		vec3f a(0.32f, 0, 0);
		vec3f b(0, 0.12f, 0);
		float c = distance<float, fast>(a, b);
		BOOST_CHECK(equals(c, 0.341760129f, 0.005f));
	}
	{
		vec4f a;
		vec4f b;
		float c = distance<float, fast>(a, b);
		BOOST_CHECK(equals(c, 0.f, 0.005f));
	}
	{
		vec4f a(0.32f, 0, 0, 0);
		vec4f b(0, 0.12f, 0, 0);
		float c = distance<float, fast>(a, b);
		BOOST_CHECK(equals(c, 0.341760129f, 0.005f));
	}
}

BOOST_AUTO_TEST_CASE(LENGTH_FAST)
{
	{
		vec2f a;
		float c = length<float, fast>(a);
		BOOST_CHECK(equals(c, 0.f, 0.005f));
	}
	{
		vec2f a(0.32f, 0);
		float c = length<float, fast>(a);
		BOOST_CHECK(equals(c, 0.32f, 0.005f));
	}
	{
		vec3f a;
		float c = length<float, fast>(a);
		BOOST_CHECK(equals(c, 0.f, 0.005f));
	}
	{
		vec3f a(0.32f, 0, 0);
		float c = length<float, fast>(a);
		BOOST_CHECK(equals(c, 0.32f, 0.005f));
	}
	{
		vec4f a;
		float c = length<float, fast>(a);
		BOOST_CHECK(equals(c, 0.f, 0.005f));
	}
	{
		vec4f a(0.32f, 0, 0, 0);
		float c = length<float, fast>(a);
		BOOST_CHECK(equals(c, 0.32f, 0.005f));
	}
}

}


#include <random>
#include <map>
#include <tuple>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <gtest/gtest.h>
#include <mpi.h>

#include "tasktorrent/tasktorrent.hpp"

using namespace std;
using namespace ttor;
using namespace Eigen;

typedef SparseMatrix<double> SpMat;

int VERB = 0;

SpMat random_SpMat(int n, double p, int seed)
{
    default_random_engine gen;
    gen.seed(seed);
    uniform_real_distribution<double> dist(0.0, 1.0);
    vector<Triplet<double>> triplets;
    for (int i = 0; i < n; ++i)
    {
        for (int j = 0; j < n; ++j)
        {
            auto v_ij = dist(gen);
            if (v_ij < p)
            {
                triplets.push_back(Triplet<double>(i, j, v_ij));
            }
        }
    }
    SpMat A(n, n);
    A.setFromTriplets(triplets.begin(), triplets.end());
    return A;
}

PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> random_perm(int n, int seed)
{
    PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> perm(n);
    perm.setIdentity();
    default_random_engine gen;
    gen.seed(seed);
    std::shuffle(perm.indices().data(), perm.indices().data() + perm.indices().size(), gen);
    return perm;
}

SpMat random_dag(int n, double p, int seed)
{
    SpMat A = random_SpMat(n, p, seed).triangularView<StrictlyLower>();
    auto P = random_perm(n, seed);
    return P.transpose() * A * P;
}

void test(int n_threads, int n, double p) {

    // Create a random dag
    SpMat G = random_dag(n, p, n * p + 2020);

    const int rank = comm_rank();
    const int n_ranks = comm_size();
    default_random_engine gen;
    gen.seed(n * p + 2021);

    // Assign tasks to ranks
    vector<int> task_2_rank(n, 0);
    std::uniform_int_distribution<> dist1(0, n_ranks-1);
    for(int i = 0; i < n; i++) task_2_rank[i] = dist1(gen);

    // Decide if header or header+body
    vector<int> msg_kind(n, 0);
    std::uniform_int_distribution<> dist2(0, 1);
    for(int i = 0; i < n; i++) msg_kind[i] = dist2(gen);

    // Count in_deps
    vector<int> in_degree(n, 0);
    for (int k = 0; k < G.outerSize(); ++k) {
        for (SpMat::InnerIterator it(G, k); it; ++it) {
            in_degree[it.row()]++;
        }
    }

    Communicator comm(MPI_COMM_WORLD, VERB);
    Threadpool tp(n_threads, &comm, VERB);
    Taskflow<int> tf(&tp, VERB);

    vector<int> buff_recv(n, 0);
    vector<int> task_ran(n, 0);
    vector<int> large_order_check(n * n, 0);

    auto am = comm.make_large_active_msg(
                [&](int& source, int& dest) {
                    EXPECT_NE(task_2_rank.at(source), rank);
                    EXPECT_EQ(task_2_rank.at(dest), rank);
                    EXPECT_EQ(large_order_check.at(source * n + dest), 1);
                    tf.fulfill_promise(dest);
                },
                [&](int& source, int& dest) {
                    EXPECT_NE(task_2_rank.at(source), rank);
                    EXPECT_EQ(task_2_rank.at(dest), rank);
                    EXPECT_EQ(large_order_check.at(source * n + dest), 0);
                    large_order_check[source * n + dest] = 1;
                    return &buff_recv[source]; // We're receiving 0 elements
                },
                [&](int& source, int& dest){
                    EXPECT_EQ(task_2_rank.at(source), rank);
                    EXPECT_NE(task_2_rank.at(dest), rank);
                });

    auto am_large = comm.make_active_msg(
                [&](int& source, int& dest) {
                    EXPECT_NE(task_2_rank.at(source), rank);
                    EXPECT_EQ(task_2_rank.at(dest), rank);
                    tf.fulfill_promise(dest);
                });

    tf.set_mapping([&](int k) {
        EXPECT_EQ(task_2_rank.at(k), rank);
        return (k % n_threads);
    })
    .set_indegree([&](int k) {
        EXPECT_EQ(task_2_rank.at(k), rank);
        return std::max(1, in_degree[k]);
    })
    .set_task([&](int k) {
        EXPECT_EQ(task_2_rank.at(k), rank);
        EXPECT_EQ(task_ran[k], 0);
        task_ran[k] ++;
        for (SpMat::InnerIterator it(G, k); it; ++it)
        {
            int other = it.row();
            int dest = task_2_rank.at(other);
            if(dest == rank) {
                tf.fulfill_promise(other);
            } else {
                if(msg_kind.at(k) == 0) {
                    am->send(dest, k, other);
                } else {
                    auto v = view<int>();
                    am_large->send_large(dest, v, k, other);
                }
            }
        }
    });

    for (int i = 0; i < n; i++) {
        if(in_degree[i] == 0 && task_2_rank.at(i) == rank) {
            tf.fulfill_promise(i);
        }
    }

    tp.join();

    MPI_Barrier(MPI_COMM_WORLD);

    for (int i = 0; i < n; i++) {
        if(task_2_rank.at(i) == rank) {
            EXPECT_EQ(task_ran[i], 1);
        }
    }
}

class DagProblemTest : public ::testing::Test, public ::testing::WithParamInterface<tuple<int, int, double>> {};

TEST_P(DagProblemTest, MixedTwoSteps) {
    int n_threads;
    int n;
    double p;
    std::tie(n_threads, n, p) = GetParam();
    test(n_threads, n, p);
}

INSTANTIATE_TEST_SUITE_P(
    DagProblem, DagProblemTest,
    ::testing::Combine(
        ::testing::Values(1, 2, 4),
        ::testing::Values(1, 3, 5, 7, 9, 15, 20, 100, 1000),
        ::testing::Values(0.0, 0.001, 0.01, 0.1, 0.2)
    )
);

int main(int argc, char **argv)
{
    ::testing::InitGoogleTest(&argc, argv);

    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

    MPI_Init_thread(NULL, NULL, req, &prov);

    assert(prov == req);

    if (argc >= 2)
    {
        VERB = atoi(argv[1]);
    }

    if (VERB > 0)
        printf("VERB = %d\n", VERB);

    const int return_flag = RUN_ALL_TESTS();

    MPI_Finalize();

    return return_flag;
}


#define CATCH_CONFIG_ENABLE_BENCHMARKING
#include "catch.hpp"

#include <BoostUnitDefinitions/Units.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/type_traits/function_traits.hpp>

#include <libUncertainty/correlation.hpp>
#include <libUncertainty/propagate.hpp>
#include <libUncertainty/uncertain.hpp>
#include <libUncertainty/utils.hpp>
// clang-format off
#include <uncertainties/ureal.hpp>
#include <uncertainties/impl.hpp>
#include <uncertainties/io.hpp>
#include <uncertainties/math.hpp>
// clang-format on
/**
 * This file is used for developement. As new classes are created, small tests
 * are written here so that we can try to compile and use them.
 */

using namespace boost::units;
using namespace libUncertainty;

TEST_CASE("Memory Usage")
{
  CHECK( sizeof( uncertain<double> ) == 2*sizeof(double) );
  CHECK( sizeof( add_id<uncertain<double>> ) == 2*sizeof(double)+sizeof(size_t) );
  CHECK( sizeof( uncertainties::udouble ) > sizeof(uncertain<double>));
  CHECK( sizeof( uncertainties::udouble ) > sizeof(add_id<uncertain<double>>));
}

TEST_CASE("Bencharmks", "[.][benchmarks]")
{
  SECTION("Error Propagation")
  {
    auto              f = [](double x, double y, double z) { return sin(x) * cos(y) * tan(z); };
    uncertain<double> x(M_PI / 2, 0.01), y(M_PI, 0.01), z(M_PI / 4, 0.01);
    BENCHMARK("Normal Call")
    {
      return f(x.nominal(), y.nominal(), z.nominal());
    };
    BENCHMARK("Error Propagation w/o Correlations")
    {
      return basic_error_propagator::propagate_error(f, x, y, z);
    };

    auto u = basic_error_propagator::propagate_error(f, x, y, z);
    CHECK(u.nominal() == Approx(-1));
    CHECK(u.uncertainty() == Approx(0.0202028));
  }

  SECTION("uncertainties-cpp comparison")
  {
    SECTION("uncertainties-cpp calculations")
    {
      uncertainties::udouble alpha(0.200, 0.003), gamma(0.100, 0.001);
      BENCHMARK("Solid angle")
      {
        return 2 * M_PI * (1 - uncertainties::cos(alpha / 2));
      };
      uncertainties::udouble Omega = 2 * M_PI * (1 - uncertainties::cos(alpha / 2));
      std::stringstream      out;
      out << Omega;

      CHECK(out.str() == "(3.14 ± 0.09)e-2");
    }
    SECTION("libUncertainty calculations")
    {
      uncertain<double> alpha(.200, 0.003), gamma(0.100, 0.001);
      BENCHMARK("Solid angle")
      {
        return basic_error_propagator::propagate_error([](double alpha) { return 2 * M_PI * (1 - cos(alpha / 2)); }, alpha);
      };
      auto              Omega = basic_error_propagator::propagate_error([](double alpha) { return 2 * M_PI * (1 - cos(alpha / 2)); }, alpha);
      std::stringstream out;
      out << Omega;
      CHECK(out.str() == "0.0313898 +/- 0.000947941");
      out.str("");
      out << Omega.normalize();
      CHECK(out.str() == "0.0314 +/- 0.0009");
    }
  }
}


#include <iostream>
#include <fstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#define USE_PERIODIC_FFT
#include <imgproc/derivative_gradient.hpp>
#include <imgproc/susan.hpp>
#include <imgproc/rcmg.hpp>
#include <imgproc/quadratureG2.hpp>
#include <imgproc/quadratureS.hpp>
#include <imgproc/quadratureSF.hpp>
#include <imgproc/quadratureLGF.hpp>
#include <imgproc/pc_sqf.hpp>
#include <imgproc/pc_lgf.hpp>
#include <imgproc/pc_matlab.hpp>
#include <imgproc/laplace.hpp>
#include <imgproc/image_operator.hpp>
#include <utility/matlab_helpers.hpp>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>

using namespace lsfm;
using namespace std;
namespace fs = boost::filesystem;

constexpr int runs = 1;

constexpr int ENTRY_SQR = 1;
constexpr int ENTRY_RGB = 2;
constexpr int ENTRY_NO_3 = 4;
constexpr int ENTRY_NO_5 = 8;

typedef double FT;

constexpr FT mag_th = static_cast<FT>(0.05);

struct Entry {
    Entry() {}

    Entry(const cv::Ptr<QuadratureI<uchar, double, double, double, double>>& fi, const cv::Ptr<QuadratureI<uchar, double, double, double, double>>& g, const cv::Ptr<QuadratureI<uchar, double, double, double, double>>& f, const std::string& b)
        : filter(fi), gt(g), ft(f), name(b) {}

    cv::Ptr<QuadratureI<uchar, double, double, double, double>> filter, gt, ft;
    std::string name;

    inline double phaseError(const cv::Mat src) {
        filter->process(src);
        ft->process(src);
        cv::Mat p = filter->phase().clone();
        p.setTo(0, ft->energy() < ft->energyThreshold(0.05));
        //showMat("phase org", abs(p));
        gt->process(src);
        cv::Mat pgt = gt->phase().clone();
        pgt.setTo(0, ft->energy() < ft->energyThreshold(0.05));
        //showMat("phase diff", abs(p - pgt));
        //cvWaitKey();
        return sum(abs(p - pgt))[0] / p.size().area();
    }

};

void parseFolder(const fs::path &folder, std::vector<fs::path> &files) {
    fs::directory_iterator end_iter;
    for_each(fs::directory_iterator(folder), fs::directory_iterator(), [&files](const fs::path& file) {
        if (fs::is_regular_file(file))
        {
            std::string ext = file.extension().generic_string();
            boost::algorithm::to_lower(ext);
            if (ext == ".jpg" || ext == ".png") {
                files.push_back(file);
            }
        }
        if (fs::is_directory(file))
            parseFolder(file, files);
    });
}

double processError(Entry &e, const fs::path& path) {
    std::cout << "processing " << e.name << "...";
    std::vector<fs::path> files;
    parseFolder(path, files);

    double ret = 0;
    int count = 0;
    std::for_each(files.begin(), files.end(), [&](const fs::path& file) {

        cv::Mat src = cv::imread(file.generic_string());
        cv::cvtColor(src, src, CV_BGR2GRAY);
        if (src.empty())
        {
            cout << "Can not open " << file.generic_string() << endl;
            return;
        }

        ret += e.phaseError(src);
        ++count;

    });
    ret /= count;
    std::cout << ret << std::endl;
    return ret;
}

int main(int argc, char** argv)
{
    fs::path path = "../../images/MDB/MiddEval3-Q";

    cv::Ptr<QuadratureI<uchar, double, double, double, double>> gt(new QuadratureSF<uchar, FT>(1, 2, 1.2));
    cv::Ptr<QuadratureI<uchar, double, double, double, double>> ft(new QuadratureS<uchar, FT, FT>(1, 2, 3, 1.2));

    std::vector<Entry> filter;

    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 3, 1.2), gt, ft, "SQF Po (3x3)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 5, 1.2), gt, ft, "SQF Po (5x5)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 7, 1.2), gt, ft, "SQF Po (7x7)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 9, 1.2), gt, ft, "SQF Po (9x9)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 11, 1.2), gt, ft, "SQF Po (11x11)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 13, 1.2), gt, ft, "SQF Po (13x13)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 15, 1.2), gt, ft, "SQF Po (15x15)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 17, 1.2), gt, ft, "SQF Po (17x17)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 19, 1.2), gt, ft, "SQF Po (19x19)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 31, 1.2), gt, ft, "SQF Po (31x31)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 63, 1.2), gt, ft, "SQF Po (63x63)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 127, 1.2), gt, ft, "SQF Po (127x127)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 255, 1.2), gt, ft, "SQF Po (255x255)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 511, 1.2), gt, ft, "SQF Po (511x511)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 749, 1.2), gt, ft, "SQF Po (749x749)"));

    int rows = filter.size() + 1;
    int cols = 2;
    std::vector<std::vector<std::string>> table;
    table.resize(rows);
    for_each(table.begin(), table.end(), [&](std::vector<std::string> &col) {
        col.resize(cols);
    });

    table[0][0] = "Method";
    table[0][1] = "Error";

    int row = 1;
    for_each(filter.begin(), filter.end(), [&](Entry &e) {
        table[row++][0] = e.name;
    });

    row = 1;
    for_each(filter.begin(), filter.end(), [&](Entry &e) {
        table[row++][1] = boost::str(boost::format("%.3f") % (processError(e, path)));
    });

    std::ofstream ofs;
    ofs.open("phase_error.csv");

    for_each(table.begin(), table.end(), [&](const std::vector<std::string> &col) {
        for_each(col.begin(), col.end(), [&](const std::string &cell) {
            std::cout << cell << "\t";
            ofs << cell << ";";
        });
        std::cout << std::endl;
        ofs << std::endl;
    });

    ofs.close();

    return 0;
}


#pragma once

#include <vector>
#include <boost/numeric/odeint.hpp>
#include <cmath>
#include <ros/ros.h>
# define M_PI           3.14159265358979323846  /* pi */
#define toRadian( x )	( (x) / 180 * M_PI )
#define toDegree( x )	( (x) / M_PI * 180 )

namespace cardsflow_gazebo
{
    using namespace std;
	using namespace boost::numeric::odeint;
	//using namespace gazebo;
/*
    struct tendonType {
		vector<ignition::math::Vector3> MidPoint;
		vector<ignition::math::Vector3> Vector;
		//might need it to calculate length
		vector<ignition::math::Vector3> Orientation;
		vector<double> Pitch;
		vector<double> Roll;
	};
*/
	struct SEE {
		double stiffness = 30680.0; // N/m
		double length = 0.056; //m
		double expansion = 0.0; //m
		double force = 0.0;  //
		double length0 = 0; //m
	};

    class ISee {

		// c1-c3 are constand values, c4 is x0. documentation can be found at ____________________
		double c1 = 0.012, c2 = 0.008, c3 = 0.018, c4 = 0.039; //m
		// the angles alpha_* describe the angle the tendons attach to the see.element
		double alpha_1 = std::atan( c1 / c4 ), alpha_2 = std::atan( c2 /  (c3+c4) ); // radian
		// the angles beta_* describe the third angles of the corresponding triangles
		double beta_1  =  M_PI / 4 - alpha_1, beta_2 = M_PI / 4 - alpha_2; // radian
		// length_* is the tendonlength from the two triangles inside the motor
		double length_1 = sqrt( c1*c1 + c4*c4 ), length_2 = sqrt( c2*c2 + (c3+c4)*(c3+c4) ); //m
		// length_c* are constand tendonlengths inside the motor
		double length_c1 = 0.04, length_c2 = 0.013; //m
		// Tendon stiffness (this is a random high number. A real number still has to be set)
		double tendonStiffness = 1e6; // N/m
		double tendonForce = 0;

       public:
		//deltaX is the displacement of the spring inside the motor
		double deltaX = 0.0; //m
		// the Length of the tendon inside the motor. the internal length changes depending on the displacement of the spring.
		double internalLength = length_c1 + length_1 + length_2 + length_c2; //m
        SEE see;

        ISee();

		///////////////////////////////////////
		/// \brief Calculate elastic force of the series elastic element
		/// \param[in] The tandonLength represents the length of the entire tendon.from the motor to the last viapoint.
		/// \param[in] The muscleLength representes the length of the tendon forn the outside of the motor to the last viapoint.
		void ElasticElementModel(const double &tendonLength, const double &muscleLength);

		///////////////////////////////////////
		/// \brief apply the springForce onto the tendons going to motor and out the muscle. The force depends on the angle the tendons have towards the spring
		/// \parm[in] the force going out the muscle
		/// \parm[in] the force going toward the motor
		void applyTendonForce( double &_muscleForce , double &_actuatorForce );
	};
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are 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)

#include <boost/math/bindings/rr.hpp>
#include <boost/test/included/prg_exec_monitor.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/tools/test.hpp>
#include <fstream>

#include <boost/math/tools/test_data.hpp>

using namespace boost::math::tools;
using namespace std;

boost::math::tuple<boost::math::ntl::RR, boost::math::ntl::RR>
   tgamma_ratio(const boost::math::ntl::RR& a, const boost::math::ntl::RR& delta)
{
   if(delta > a)
      throw std::domain_error("");
   boost::math::ntl::RR tg = boost::math::tgamma(a);
   boost::math::ntl::RR r1 = tg / boost::math::tgamma(a + delta);
   boost::math::ntl::RR r2 = tg / boost::math::tgamma(a - delta);
   if((r1 > (std::numeric_limits<float>::max)()) || (r2 > (std::numeric_limits<float>::max)()))
      throw std::domain_error("");

   return boost::math::make_tuple(r1, r2);
}

boost::math::ntl::RR tgamma_ratio2(const boost::math::ntl::RR& a, const boost::math::ntl::RR& b)
{
   return boost::math::tgamma(a) / boost::math::tgamma(b);
}

int cpp_main(int argc, char*argv [])
{
   boost::math::ntl::RR::SetPrecision(1000);
   boost::math::ntl::RR::SetOutputPrecision(40);

   parameter_info<boost::math::ntl::RR> arg1, arg2;
   test_data<boost::math::ntl::RR> data;

   bool cont;
   std::string line;

   if((argc >= 2) && (strcmp(argv[1], "--ratio") == 0))
   {
      std::cout << "Welcome.\n"
         "This program will generate spot tests for the function tgamma_ratio(a, b)\n\n";

      do{
         if(0 == get_user_parameter_info(arg1, "a"))
            return 1;
         if(0 == get_user_parameter_info(arg2, "b"))
            return 1;
         data.insert(&tgamma_ratio2, arg1, arg2);

         std::cout << "Any more data [y/n]?";
         std::getline(std::cin, line);
         boost::algorithm::trim(line);
         cont = (line == "y");
      }while(cont);
   }
   else
   {
      std::cout << "Welcome.\n"
         "This program will generate spot tests for the function tgamma_delta_ratio(a, delta)\n\n";

      do{
         if(0 == get_user_parameter_info(arg1, "a"))
            return 1;
         if(0 == get_user_parameter_info(arg2, "delta"))
            return 1;
         data.insert(&tgamma_ratio, arg1, arg2);

         std::cout << "Any more data [y/n]?";
         std::getline(std::cin, line);
         boost::algorithm::trim(line);
         cont = (line == "y");
      }while(cont);
   }

   std::cout << "Enter name of test data file [default=tgamma_ratio_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "tgamma_ratio_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific;
   write_code(ofs, data, "tgamma_ratio_data");

   return 0;
}


/*
 [auto_generated]
 libs/numeric/odeint/examples/bind_member_functions.hpp

 [begin_description]
 tba.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

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

#include <iostream>
#include <array>
#include <type_traits>

#include <boost/numeric/odeint.hpp>

namespace odeint = boost::numeric::odeint;

typedef std::array< double , 3 > state_type;

struct lorenz
{
    void ode( const state_type &x , state_type &dxdt , double t ) const
    {
        const double sigma = 10.0;
        const double R = 28.0;
        const double b = 8.0 / 3.0;

        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = -b * x[2] + x[0] * x[1];
    }
};

int main( int argc , char *argv[] )
{
    using namespace boost::numeric::odeint;
    //[ bind_member_function_cpp11
    namespace pl = std::placeholders;

    state_type x = {{ 10.0 , 10.0 , 10.0 }};
    integrate_const( runge_kutta4< state_type >() ,
                     std::bind( &lorenz::ode , lorenz() , pl::_1 , pl::_2 , pl::_3 ) ,
                     x , 0.0 , 10.0 , 0.01  );
    //]
    return 0;
}