#include "Cady/Cady.h"
#include "Cady/Frontend.h"
#include "Cady/CodeGen.h"

#include <map>
#include <iomanip>

using namespace Cady;
using namespace Cady::CodeGen;

void example_0(){

        Function f("f");
        f.AddArgument("x");
        f.AddArgument("y");

        //auto expr_0 = BinaryOperator::Mul(Log::Make(BinaryOperator::Mul(ExogenousSymbol::Make("x"),ExogenousSymbol::Make("x"))),  Exp::Make(ExogenousSymbol::Make("y")));
        //auto expr_0 = BinaryOperator::Pow(ExogenousSymbol::Make("x"), Constant::Make(2));
        auto expr_0 = Phi::Make(BinaryOperator::Pow(ExogenousSymbol::Make("x"), Constant::Make(3)));

        auto stmt_0 = std::make_shared<EndgenousSymbol>("stmt0", expr_0);

        f.AddStatement(stmt_0);

        std::ofstream fstr("prog.cxx");
        fstr << R"(
#include <cstdio>
#include <cmath>
)";

        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(

int main(){
        double x_min = 0.1;
        double x_max = +2.0;
        double y_min = -2.0;
        double y_max = +2.0;

        double epsilon = 1e-10;
        double increment = 0.05;

        for(double x =x_min; x <= x_max + increment /2; x += increment ){
                for(double y =y_min; y <= y_max + increment /2; y += increment ){
                        double d_x = 0.0;
                        double d_y = 0.0;

                        double value = f(x, &d_x, y, &d_y);

                        double dummy;
                        double x_lower = f(x - epsilon /2 , &dummy, y, &dummy);
                        double x_upper = f(x + epsilon /2 , &dummy, y, &dummy);
                        double x_finite_diff = ( x_upper - x_lower ) / epsilon;
                        double x_residue = d_x - x_finite_diff;

                        double y_lower = f(x, &dummy, y - epsilon /2 , &dummy);
                        double y_upper = f(x, &dummy, y + epsilon /2 , &dummy);
                        double y_finite_diff = ( y_upper - y_lower ) / epsilon;
                        double y_residue = d_y - y_finite_diff;

                        //printf("%f,%f,%f,%f,%f,%f\n", x, y, d_x, d_y, x_finite_diff, x_residue);
                        printf("%f,%f,%f => %f,%f,%f => %f,%f,%f\n", x, y,value, d_x, x_finite_diff,x_residue, d_y, y_finite_diff,y_residue);
                }

        }

}
)";
}

void example_1(){

        Function f("f");
        f.AddArgument("a");
        f.AddArgument("b");
        f.AddArgument("x");

        auto expr_0 = BinaryOperator::Mul( ExogenousSymbol::Make("a"), BinaryOperator::Mul(ExogenousSymbol::Make("x"),  ExogenousSymbol::Make("x")));

        auto stmt_0 = std::make_shared<EndgenousSymbol>("stmt0", expr_0);

        auto expr_1 = BinaryOperator::Add( ExogenousSymbol::Make(stmt_0->Name()), ExogenousSymbol::Make("b"));

        auto stmt_1 = std::make_shared<EndgenousSymbol>("stmt1", expr_1);
        f.AddStatement(stmt_0);
        f.AddStatement(stmt_1);

        std::ofstream fstr("prog.c");
        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(
#include <stdio.h>
int main(){
        double a = 2.0;
        double b = 3.0;

        double epsilon = 1e-10;
        double increment = 0.05;

        for(double x =0.0; x <= 2.0 + increment /2; x += increment ){
                double d_a = 0.0;
                double d_b = 0.0;
                double d_x = 0.0;

                double y = f(a, &d_a, b, &d_b, x, &d_x);

                double dummy;
                double lower = f(a, &dummy, b, &dummy, x - epsilon/2, &dummy);
                double upper = f(a, &dummy, b, &dummy, x + epsilon/2, &dummy);
                double finite_diff = ( upper - lower ) / epsilon;
                double residue = d_x - finite_diff;

                printf("%f,%f,%f,%f,%f,%f,%f\n", x, y, d_a, d_b, d_x, finite_diff, residue);

        }

}
)";
}

void black_scholes(){

        Function f("black");
        f.AddArgument("t");
        f.AddArgument("T");
        f.AddArgument("r");
        f.AddArgument("S");
        f.AddArgument("K");
        f.AddArgument("vol");

        auto time_to_expiry = BinaryOperator::Sub(
                ExogenousSymbol::Make("T"),
                ExogenousSymbol::Make("t")
        );

        auto deno = BinaryOperator::Div(
                Constant::Make(1.0),
                BinaryOperator::Mul(
                        ExogenousSymbol::Make("vol"),
                        BinaryOperator::Pow(
                                time_to_expiry,
                                Constant::Make(0.5)
                        )
                )
        );

        auto d1 = BinaryOperator::Mul(
                deno,
                BinaryOperator::Add(
                        Log::Make(
                                BinaryOperator::Div(
                                        ExogenousSymbol::Make("S"),
                                        ExogenousSymbol::Make("K")
                                )
                        ),
                        BinaryOperator::Mul(
                                BinaryOperator::Add(
                                        ExogenousSymbol::Make("r"),
                                        BinaryOperator::Div(
                                                BinaryOperator::Pow(
                                                        ExogenousSymbol::Make("vol"),
                                                        Constant::Make(2.0)
                                                ),
                                                Constant::Make(2.0)
                                        )
                                ),
                                time_to_expiry
                        )
                )
        );

        auto stmt_0 = std::make_shared<EndgenousSymbol>("stmt0", d1);

        auto d2 = BinaryOperator::Sub(
                ExogenousSymbol::Make(stmt_0->Name()),
                BinaryOperator::Mul(
                        ExogenousSymbol::Make("vol"),
                        time_to_expiry
                )
        );

        auto stmt_1 = std::make_shared<EndgenousSymbol>("stmt1", d2);

        auto pv = BinaryOperator::Mul(
                ExogenousSymbol::Make("K"),
                Exp::Make(
                        BinaryOperator::Mul(
                                BinaryOperator::Sub(
                                        Constant::Make(0.0),
                                        ExogenousSymbol::Make("r")
                                ),
                                time_to_expiry
                        )
                )
        );

        auto stmt_2 = std::make_shared<EndgenousSymbol>("stmt2", pv);

        auto black = BinaryOperator::Sub(
                BinaryOperator::Mul(
                        Phi::Make(stmt_0),
                        ExogenousSymbol::Make("S")
                ),
                BinaryOperator::Mul(
                        Phi::Make(stmt_1),
                        stmt_2
                )
        );

        auto stmt_3 = std::make_shared<EndgenousSymbol>("stmt3", black);

        f.AddStatement(stmt_0);
        f.AddStatement(stmt_1);
        f.AddStatement(stmt_2);
        f.AddStatement(stmt_3);

        std::ofstream fstr("prog.cxx");
        fstr << R"(
#include <cstdio>
#include <cmath>
)";

        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(

double black_fd(double epsilon, double t, double d_t, double T, double d_T, double r, double d_r, double S, double d_S, double K, double d_K, double vol, double d_vol){
        double dummy;
        double lower = black( t - d_t*epsilon/2 , &dummy, T - d_T*epsilon/2  , &dummy, r - d_r*epsilon/2  , &dummy, S - d_S*epsilon/2  , &dummy, K - d_K*epsilon/2  , &dummy, vol - d_vol*epsilon/2, &dummy);
        double upper = black( t + d_t*epsilon/2 , &dummy, T + d_T*epsilon/2  , &dummy, r + d_r*epsilon/2  , &dummy, S + d_S*epsilon/2  , &dummy, K + d_K*epsilon/2  , &dummy, vol + d_vol*epsilon/2, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        return finite_diff;
}
int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double epsilon = 1e-10;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        double d1 = 1/ ( vol * std::sqrt(T - t)) *  ( std::log(S/K) + ( r + vol*vol/2)*(T-t));

        double dummy;
        double lower = black( t - epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double upper = black( t + epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        double residue = d_t - finite_diff;

        printf("%f,%f,%f,%f,%f,%f => %f,%f => %f,%f,%f\n", t, T, r, S, K, vol, value, d1, d_t, finite_diff, residue);

        printf("d[t]  ,%f,%f\n", d_t  ,  black_fd(epsilon, t, 1, T  , 0, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[T]  ,%f,%f\n", d_T  ,  black_fd(epsilon, t, 0, T  , 1, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[r]  ,%f,%f\n", d_r  ,  black_fd(epsilon, t, 0, T  , 0, r  , 1, S  , 0, K  , 0, vol, 0));
        printf("d[S]  ,%f,%f\n", d_S  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 1, K  , 0, vol, 0));
        printf("d[K]  ,%f,%f\n", d_K  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 1, vol, 0));
        printf("d[vol],%f,%f\n", d_vol,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 0, vol, 1));

}
)";
}

void black_scholes_frontend(){

        using namespace Frontend;
        using Frontend::Log;
        using Frontend::Exp;
        using Frontend::Phi;

        Function f("black");
        f.AddArgument("t");
        f.AddArgument("T");
        f.AddArgument("r");
        f.AddArgument("S");
        f.AddArgument("K");
        f.AddArgument("vol");

        auto d1    = f.AddStatement(Stmt("d1"   , (1.0 / ( Var("vol") * ((Var("T") - Var("t")) ^ 0.5) )) * ( Log(Var("S") / "K") +   ("r" + ( Var("vol") ^ 2.0 ) / 2 ) * (Var("T") - Var("t")) )));
        auto d2    = f.AddStatement(Stmt("d2"   , d1 - "vol" * (Var("T") - Var("t"))));
        auto pv    = f.AddStatement(Stmt("pv"   , "K" * Exp( -Var("r") * ( Var("T") - Var("t") ) )));
        auto black = f.AddStatement(Stmt("black", Phi(d1) * "S" - Phi(d2) * pv));

        std::ofstream fstr("prog.cxx");
        fstr << R"(
#include <cstdio>
#include <cmath>
)";

        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(

double black_fd(double epsilon, double t, double d_t, double T, double d_T, double r, double d_r, double S, double d_S, double K, double d_K, double vol, double d_vol){
        double dummy;
        double lower = black( t - d_t*epsilon/2 , &dummy, T - d_T*epsilon/2  , &dummy, r - d_r*epsilon/2  , &dummy, S - d_S*epsilon/2  , &dummy, K - d_K*epsilon/2  , &dummy, vol - d_vol*epsilon/2, &dummy);
        double upper = black( t + d_t*epsilon/2 , &dummy, T + d_T*epsilon/2  , &dummy, r + d_r*epsilon/2  , &dummy, S + d_S*epsilon/2  , &dummy, K + d_K*epsilon/2  , &dummy, vol + d_vol*epsilon/2, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        return finite_diff;
}
int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double epsilon = 1e-10;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        double d1 = 1/ ( vol * std::sqrt(T - t)) *  ( std::log(S/K) + ( r + vol*vol/2)*(T-t));

        double dummy;
        double lower = black( t - epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double upper = black( t + epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        double residue = d_t - finite_diff;

        printf("%f,%f,%f,%f,%f,%f => %f,%f => %f,%f,%f\n", t, T, r, S, K, vol, value, d1, d_t, finite_diff, residue);

        printf("d[t]  ,%f,%f\n", d_t  ,  black_fd(epsilon, t, 1, T  , 0, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[T]  ,%f,%f\n", d_T  ,  black_fd(epsilon, t, 0, T  , 1, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[r]  ,%f,%f\n", d_r  ,  black_fd(epsilon, t, 0, T  , 0, r  , 1, S  , 0, K  , 0, vol, 0));
        printf("d[S]  ,%f,%f\n", d_S  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 1, K  , 0, vol, 0));
        printf("d[K]  ,%f,%f\n", d_K  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 1, vol, 0));
        printf("d[vol],%f,%f\n", d_vol,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 0, vol, 1));

}
)";
}

void black_scholes_template(){
        auto black_eval = BlackScholesCallOption::Build<double>{};

        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        std::cout << "black_eval(t,T,r,S,K,vol) => " << black_eval.Evaluate(t,T,r,S,K,vol) << "\n"; // __CandyPrint__(cxx-print-scalar,black_eval(t,T,r,S,K,vol))

        auto ad_kernel = BlackScholesCallOption::Build<DoubleKernel>{};

        auto as_black = ad_kernel.Evaluate(
                DoubleKernel::BuildFromExo("t"),
                DoubleKernel::BuildFromExo("T"),
                DoubleKernel::BuildFromExo("r"),
                DoubleKernel::BuildFromExo("S"),
                DoubleKernel::BuildFromExo("K"),
                DoubleKernel::BuildFromExo("vol")
        );

        Function f("black");
        f.AddArgument("t");
        f.AddArgument("T");
        f.AddArgument("r");
        f.AddArgument("S");
        f.AddArgument("K");
        f.AddArgument("vol");

        using namespace Frontend;

        std::unordered_set< std::shared_ptr<Operator > > seen;
        struct StackFrame{
                explicit StackFrame(std::shared_ptr<EndgenousSymbol > op)
                        : Op{op}
                {
                        auto deps_set = Op->EndgenousDependencies();
                        Deps.assign(deps_set.begin(), deps_set.end());
                }
                std::shared_ptr<EndgenousSymbol > Op;
                std::vector<std::shared_ptr<EndgenousSymbol > > Deps;
        };
        std::vector<StackFrame> stack{StackFrame{std::reinterpret_pointer_cast<EndgenousSymbol>(as_black.as_operator_())}};
        for(size_t ttl=1000;stack.size() && ttl;--ttl){
                auto& frame = stack.back();
                if( frame.Deps.size() == 0 ){
                        if( seen.count(frame.Op) == 0 ){
                                seen.insert(frame.Op);
                                auto black = f.AddStatement(frame.Op);
                                #if 0
                                std::cout << "----------TERMINAL--------------\n";
                                frame.Op->Display();
                                #endif
                        }
                        stack.pop_back();
                        continue;
                }
                auto dep = frame.Deps.back();
                frame.Deps.pop_back();

                stack.push_back(StackFrame{dep});

        }

        std::ofstream fstr("prog.cxx");
        fstr << R"(
#include <cstdio>
#include <cmath>
#include <iostream>
#include <boost/timer/timer.hpp>
)";

        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(

double black_fd(double epsilon, double t, double d_t, double T, double d_T, double r, double d_r, double S, double d_S, double K, double d_K, double vol, double d_vol){
        double dummy;
        double lower = black( t - d_t*epsilon/2 , &dummy, T - d_T*epsilon/2  , &dummy, r - d_r*epsilon/2  , &dummy, S - d_S*epsilon/2  , &dummy, K - d_K*epsilon/2  , &dummy, vol - d_vol*epsilon/2, &dummy);
        double upper = black( t + d_t*epsilon/2 , &dummy, T + d_T*epsilon/2  , &dummy, r + d_r*epsilon/2  , &dummy, S + d_S*epsilon/2  , &dummy, K + d_K*epsilon/2  , &dummy, vol + d_vol*epsilon/2, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        return finite_diff;
}
int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double epsilon = 1e-10;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        double d1 = 1/ ( vol * std::sqrt(T - t)) *  ( std::log(S/K) + ( r + vol*vol/2)*(T-t));

        double dummy;
        double lower = black( t - epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double upper = black( t + epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        double residue = d_t - finite_diff;

        printf("%f,%f,%f,%f,%f,%f => %f,%f => %f,%f,%f\n", t, T, r, S, K, vol, value, d1, d_t, finite_diff, residue);

        printf("d[t]  ,%f,%f\n", d_t  ,  black_fd(epsilon, t, 1, T  , 0, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[T]  ,%f,%f\n", d_T  ,  black_fd(epsilon, t, 0, T  , 1, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[r]  ,%f,%f\n", d_r  ,  black_fd(epsilon, t, 0, T  , 0, r  , 1, S  , 0, K  , 0, vol, 0));
        printf("d[S]  ,%f,%f\n", d_S  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 1, K  , 0, vol, 0));
        printf("d[K]  ,%f,%f\n", d_K  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 1, vol, 0));
        printf("d[vol],%f,%f\n", d_vol,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 0, vol, 1));

        // time profile
        for(volatile size_t N = 100;;N*=2){
                boost::timer::cpu_timer timer;
                for(volatile size_t idx=0;idx!=N;++idx){
                        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);
                }
                std::string ad_time = timer.format(4, "%w");
                timer.start();
                for(volatile size_t idx=0;idx!=N;++idx){
                        black_fd(epsilon, t, 1, T  , 0, r  , 0, S  , 0, K  , 0, vol, 0);
                        black_fd(epsilon, t, 0, T  , 1, r  , 0, S  , 0, K  , 0, vol, 0);
                        black_fd(epsilon, t, 0, T  , 0, r  , 1, S  , 0, K  , 0, vol, 0);
                        black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 1, K  , 0, vol, 0);
                        black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 1, vol, 0);
                        black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 0, vol, 1);
                }
                std::string fd_time = timer.format(4, "%w");
                std::cout << N << "," << fd_time << "," << ad_time << "\n";
        }

}
)";
}

struct RemoveEndgenousFolder{
        std::shared_ptr<Operator> Fold(std::shared_ptr<Operator> root){
                if( root->Kind() == OPKind_EndgenousSymbol ){
                        auto as_endgenous = std::reinterpret_pointer_cast<EndgenousSymbol>(root);
                        return this->Fold(as_endgenous->Expr());
                }

                if( root->IsNonTerminal() ){
                        for(size_t idx=0;idx!=root->Arity();++idx){
                                auto folded = this->Fold(root->At(idx));
                                root->Rebind(idx, folded);
                        }
                }

                return root;
        }
};

struct RemoveEndo : OperatorTransform{
        virtual std::shared_ptr<Operator> Apply(std::shared_ptr<Operator> const& ptr){
                auto candidate = ptr->Clone(shared_from_this());
                if( candidate->Kind() == OPKind_EndgenousSymbol ){
                        if( auto typed = std::dynamic_pointer_cast<EndgenousSymbol>(candidate)){
                                return typed->Expr();
                        }
                }
                return candidate;
        }
};
struct RemapUnique : OperatorTransform{
        explicit RemapUnique(std::string const& prefix = "__symbol_")
                : prefix_{prefix}
        {
                std::cerr << " RemapUnique()\n";
        }
        ~RemapUnique(){
                std::cerr << "~RemapUnique()\n";
        }
        void mutate_prefix(std::string const& prefix){
                prefix_ = prefix;
        }
        virtual std::shared_ptr<Operator> Apply(std::shared_ptr<Operator> const& ptr){

                auto candidate = ptr->Clone(shared_from_this());

                auto key = std::make_tuple(
                        candidate->NameInvariantOfChildren(),
                        candidate->Children()
                        );

                auto iter = ops_.find(key);
                if( iter != ops_.end() )
                        return iter->second;

                if(
                    candidate->Kind() != OPKind_EndgenousSymbol &&
                    #if 0
                    candidate->Kind() != OPKind_ExogenousSymbol &&
                    #endif
                    candidate->Kind() != OPKind_Constant )
                {

                        std::stringstream ss;
                        ss << prefix_ << (ops_.size()+1);
                        auto endogous_sym = EndgenousSymbol::Make(ss.str(), candidate);

                        ops_[key] = endogous_sym;
                        return endogous_sym;
                } else {
                        ops_[key] = candidate;
                        return candidate;
                }
        }
private:
        std::string prefix_;
        std::map<
                std::tuple<
                        std::string,
                        std::vector<std::shared_ptr<Operator> >
                >,
                std::shared_ptr<Operator>
        > ops_;
};

void black_scholes_template_opt(){

        auto ad_kernel = BlackScholesCallOption::Build<DoubleKernel>{};

        auto as_black = ad_kernel.Evaluate(
                DoubleKernel::BuildFromExo("t"),
                DoubleKernel::BuildFromExo("T"),
                DoubleKernel::BuildFromExo("r"),
                DoubleKernel::BuildFromExo("S"),
                DoubleKernel::BuildFromExo("K"),
                DoubleKernel::BuildFromExo("vol")
        );

        SymbolTable ST;
        ST("t"  , 0.0);
        ST("T"  , 10.0);
        ST("r"  , 0.04);
        ST("S"  , 50);
        ST("K"  , 60);
        ST("vol", 0.2);

        RemoveEndgenousFolder remove_endogous;
        Transform::FoldZero constant_fold;

        auto black_expr = as_black.as_operator_();

        std::cout << "--------- black_expr -----------\n";
        //black_expr->Display();
        std::cout << "black_expr->Eval(ST) => " << black_expr->Eval(ST) << "\n"; // __CandyPrint__(cxx-print-scalar,black_expr->Eval(ST))

        auto removed_endo = remove_endogous.Fold(black_expr);

        auto params = std::vector<std::string>{ "t", "T", "r", "S", "K", "vol" };

        std::vector<std::shared_ptr<Operator> > ticker;
        for(auto const& s : params){
                auto raw_diff = removed_endo->Diff(s);
                ticker.push_back(raw_diff);
        }

        auto unique_mapper = std::make_shared<RemapUnique>();

        std::ofstream out("black_better.h");

        out << "double black(";
        for(size_t idx=0;idx!=params.size();++idx){
                if( idx != 0 ){
                        out << ", ";
                }
                out << "double " << params[idx] << ", double* d_" << params[idx];
        }
        out << "){\n";

        std::unordered_set<std::shared_ptr<Operator> > seen;
        std::shared_ptr<EndgenousSymbol> return_;
        for(size_t idx=0;idx!=ticker.size();++idx){
                auto constant_folded = constant_fold.Fold(ticker[idx]);

                auto unique          = constant_folded->Clone(unique_mapper);

                auto dependents = unique->DepthFirstAnySymbolicDependency();

                for(auto const& dep : dependents.DepthFirst){
                        // first emit all the expressions we need
                        if( seen.count(dep) > 0 )
                                continue;
                        seen.insert(dep);

                        out << "    double " << std::left << std::setw(15) << dep->Name() << " = ";
                        dep->Expr()->EmitCode(out);
                        out << ";\n";
                }

                out << "    *d_" << params[idx] << " = ";
                unique->EmitCode(out);
                out << ";\n";

                //unique->Display();
        }

        auto constant_folded = constant_fold.Fold(removed_endo);

        auto unique          = constant_folded->Clone(unique_mapper);

        auto dependents = unique->DepthFirstAnySymbolicDependency();

        for(auto const& dep : dependents.DepthFirst){
                // first emit all the expressions we need
                if( seen.count(dep) > 0 )
                        continue;
                seen.insert(dep);

                out << "    double " << std::left << std::setw(15) << dep->Name() << " = ";
                dep->Expr()->EmitCode(out);
                out << ";\n";
        }
        out << "    return ";
        unique->EmitCode(out);
        out << ";\n";
        out << "}\n";

}

struct DataFlow;

struct DotCompiler{
        DotCompiler(){
                order.emplace_back();
        }
        std::stringstream nodes;
        std::stringstream edges;
        std::vector<std::vector<std::string> > order;
};

struct DataFlowGraph{
        void Add(std::shared_ptr<DataFlow> const& flow);
        void EmitDot(std::ostream& out)const;
        void EmitCppCode(std::ostream& out)const;
        void CollectADFlow(std::vector<std::shared_ptr<EndgenousSymbol> > & computation);
        void Display(std::ostream& out = std::cout)const;
        void EmitInstructions(InstructionBlock& B)const;
        void EmitADInstructions(Operator::DependentsProfile& information,
                                std::shared_ptr<RemapUnique> RU,
                                InstructionBlock& B)const;
private:
        std::vector<std::shared_ptr<DataFlow> > rank_;
        std::unordered_map<std::string,std::shared_ptr<DataFlow> > index_;
};

struct DataFlow{
        DataFlow(std::shared_ptr<EndgenousSymbol> sym)
                :sym_(sym)
        {}
        auto Expr()const{ return sym_; }
        auto Name()const{ return sym_->Name(); }
        void AddParent(DataFlow* ptr){
                parents_.push_back(ptr);
        }
        void AddChild(DataFlow* ptr){
                children_.push_back(ptr);
        }
        #if 0
        void EmitADInstructionsImpl(Operator::DependentsProfile& information, std::shared_ptr<RemapUnique> RU, InstructionBlock& B)const
        {
                auto remapped = sym_->Expr()->Clone(RU);

                auto dependents = remapped->DepthFirstAnySymbolicDependency();
                for(auto const& dep : dependents.DepthFirst ){
                        if( information.Set.count(dep) == 1 )
                                continue;
                        information.Set.insert(dep);
                        B.Add(std::make_shared<InstructionDeclareVariable>(
                                        dep->Name(),
                                        dep->Expr()));
                }

                auto exo = std::reinterpret_pointer_cast<EndgenousSymbol>(remapped);

                B.Add(std::make_shared<InstructionDeclareVariable>(
                                exo->Name(),
                                exo->Expr()));
                if( children_.empty() ){
                        std::vector<std::string> deps;
                        auto dependents = sym_->DepthFirstAnySymbolicDependencyAndThis();
                        for(auto const& ptr : dependents.DepthFirst ){
                                deps.push_back(ptr->Name());
                        }
                        B.Add(std::make_shared<InstructionComment>(deps));
                        B.Add(std::make_shared<InstructionReturn>(sym_->Name()));
                }
        }
        #endif
        void EmitInstructionsImpl(InstructionBlock& B)const{
                B.Add(std::make_shared<InstructionDeclareVariable>(
                                sym_->Name(),
                                sym_->Expr()));
                if( children_.empty() ){
                        std::vector<std::string> deps;
                        auto dependents = sym_->DepthFirstAnySymbolicDependencyAndThis();
                        for(auto const& ptr : dependents.DepthFirst ){
                                deps.push_back(ptr->Name());
                        }
                        B.Add(std::make_shared<InstructionComment>(deps));
                        B.Add(std::make_shared<InstructionReturn>(sym_->Name()));
                }
        }
        void EmitADInstructionsImpl(Operator::DependentsProfile& information, std::shared_ptr<RemapUnique> RU, InstructionBlock& B)const
        {
                auto make_ad_sym = [](auto const& name){
                        return "__rev_ad_" + name;
                };

                if( children_.size() == 0 ){
                        B.Add(std::make_shared<InstructionDeclareVariable>(
                                make_ad_sym(sym_->Name()),
                                Constant::Make(1.0)
                        ));
                } else {
                        static auto removed_end = std::make_shared<RemoveEndo>();
                        static Transform::FoldZero constant_fold;
                        // forward
                        auto make_node_back = [&](auto child){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                child->Expr()->Expr()->Diff(sym_->Name()),
                                                ExogenousSymbol::Make(make_ad_sym(child->Name()))
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_back(children_[0]);
                        for(size_t idx=1;idx<children_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_back(children_[idx])
                                );
                        }

                        #if 0
                        auto remapped = head;
                        #else
                        std::stringstream cpp_expr;
                        head = head->Clone(removed_end);
                        head->EmitCode(cpp_expr);
                        //head = remove_endogous.Fold(head);
                        //auto folded = constant_fold.Fold(head);
                        auto remapped = head->Clone(RU);

                        B.Add(std::make_shared<InstructionComment>(std::vector<std::string>{"BEGIN " + cpp_expr.str()}));
                        auto dependents = remapped->DepthFirstAnySymbolicDependency();
                        for(auto const& dep : dependents.DepthFirst ){
                                if( information.Set.count(dep) == 1 )
                                        continue;
                                information.Set.insert(dep);
                                B.Add(std::make_shared<InstructionDeclareVariable>(
                                                dep->Name(),
                                                dep->Expr()));

                        }
                        B.Add(std::make_shared<InstructionComment>(std::vector<std::string>{"END " + cpp_expr.str()}));
                        #endif

                        B.Add(std::make_shared<InstructionDeclareVariable>(
                                make_ad_sym(sym_->Name()),
                                remapped));
                }

                if( parents_.size() == 0 ){
                        auto make_ptr_sym = [](auto const& name){
                                return "d_" + name;
                        };
                        auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr());
                        B.Add(std::make_shared<InstructionPointerAssignment>(
                                        make_ptr_sym(param_name->Name()),
                                        make_ad_sym(sym_->Name())));
                }

#if 0

                auto make_ptr_sym = [](auto const& name){
                        return "d_" + name;
                };
                if( parents_.empty() ){
                        auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr());
                        out << "*" << make_ptr_sym(param_name->Name()) << " = " << make_ad_sym(sym_->Name()) << ";\n";
                }
#endif
        }
        void EmitEvalCodeImpl(std::ostream& out)const{
                out << "double " << sym_->Name() << " = ";
                sym_->Expr()->EmitCode(out);
                out << ";\n";
                if( children_.size() == 0 ){
                        auto make_ad_sym = [](auto const& name){
                                return "__rev_ad_" + name;
                        };
                        auto make_ptr_sym = [](auto const& name){
                                return "d_" + name;
                        };
                        std::cout << sym_->Name() << "\n";
                        //sym_->Display();
                        if( auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr()) ){
                                out << "*" << make_ptr_sym(param_name->Name()) << " = " << make_ad_sym(sym_->Name()) << ";\n";
                        } else {
                                out << "// unexpcted\n";
                        }
                }

                auto make_ad_sym = [](auto const& name){
                        return "__rev_ad_" + name;
                };
                auto make_ptr_sym = [](auto const& name){
                        return "d_" + name;
                };
                if( parents_.empty() ){
                        auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr());
                        out << "*" << make_ptr_sym(param_name->Name()) << " = " << make_ad_sym(sym_->Name()) << ";\n";
                }
        }
        void EmitADFlowImpl(std::vector<std::shared_ptr<EndgenousSymbol> > & computation){
                auto make_ad_sym = [](auto const& name){
                        return "__rev_ad_" + name;
                };
                if( children_.size() > 0 ){
                        static Transform::FoldZero constant_fold;
                        // forward
                        auto make_node_back = [&](auto child){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                child->Expr()->Expr()->Diff(sym_->Name()),
                                                ExogenousSymbol::Make(make_ad_sym(child->Name()))
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_back(children_[0]);
                        for(size_t idx=1;idx<children_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_back(children_[idx])
                                );
                        }

                        computation.push_back(EndgenousSymbol::Make( make_ad_sym(sym_->Name()), head));

                }
        }
        void EmitReverseADCodeImpl(std::ostream& out)const{
                auto make_ad_sym = [](auto const& name){
                        return "__rev_ad_" + name;
                };
                out << "double " << make_ad_sym(sym_->Name()) << " = ";
                if( children_.size() > 0 ){
                        static Transform::FoldZero constant_fold;
                        // forward
                        auto make_node_back = [&](auto child){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                child->Expr()->Expr()->Diff(sym_->Name()),
                                                ExogenousSymbol::Make(make_ad_sym(child->Name()))
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_back(children_[0]);
                        for(size_t idx=1;idx<children_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_back(children_[idx])
                                );
                        }

                        head->EmitCode(out);
                } else {
                        out << "1.0";
                }
                out << ";\n";

                auto make_ptr_sym = [](auto const& name){
                        return "d_" + name;
                };
                if( parents_.empty() ){
                        auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr());
                        out << "*" << make_ptr_sym(param_name->Name()) << " = " << make_ad_sym(sym_->Name()) << ";\n";
                }

        }

        void EmitDot(DotCompiler& compiler)const{
                if( parents_.size() ){
                        compiler.order.emplace_back(std::vector<std::string>{sym_->Name()});
                } else {
                        compiler.order[0].push_back(sym_->Name());
                }
                compiler.nodes << sym_->Name() << "[shape=record, label=\"<expr>";
                compiler.nodes << sym_->Name() << " = ";
                sym_->Expr()->EmitCode(compiler.nodes);

                #if 0
                for(auto const& ptr : parents_){
                        auto name = ptr->Name();
                        auto diff = sym_->Expr()->Diff(name);
                        Transform::FoldZero constant_fold;
                        auto folded = constant_fold.Fold(diff);
                        compiler.nodes << "|D[" << name << "] = ";
                        folded->EmitCode(compiler.nodes);
                }
                #endif
                Transform::FoldZero constant_fold;
                compiler.nodes << "|<diff>D[" << sym_->Name() << "]";
                if( parents_.size() > 0 ){

                        // forward
                        auto make_node_fwd = [&](auto name){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                sym_->Expr()->Diff(name),
                                                ExogenousSymbol::Make("D[" + name + "]")
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_fwd(parents_[0]->Name());
                        for(size_t idx=1;idx<parents_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_fwd(parents_[idx]->Name())
                                );
                        }

                        compiler.nodes << " = ";
                        head->EmitCode(compiler.nodes);

                }

                compiler.nodes << "|<bdiff>B[" << sym_->Name() << "]";
                if( children_.size() > 0 ){
                        // forward
                        auto make_node_back = [&](auto child){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                child->Expr()->Expr()->Diff(sym_->Name()),
                                                ExogenousSymbol::Make("B[" + child->Name() + "]")
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_back(children_[0]);
                        for(size_t idx=1;idx<children_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_back(children_[idx])
                                );
                        }

                        compiler.nodes << " = ";
                        head->EmitCode(compiler.nodes);
                }
                compiler.nodes << "\"];\n";
                #if 1
                for(auto const& ptr : parents_){

                        auto name = ptr->Name();

                        auto diff = sym_->Expr()->Diff(name);
                        auto folded = constant_fold.Fold(diff);

                        compiler.edges << name << ":diff -> " << sym_->Name() << ":diff [color=blue,label=\"";
                        folded->EmitCode(compiler.edges);
                        compiler.edges << "\"];\n";
                }
                #endif
                for(auto const& ptr : children_ ){

                        auto diff = ptr->Expr()->Expr()->Diff(sym_->Name());
                        auto folded = constant_fold.Fold(diff);

                        compiler.edges << ptr->Name() << ":bdiff -> " << sym_->Name() << ":bdiff [color=red,label=\"";
                        folded->EmitCode(compiler.edges);
                        compiler.edges << "\"];\n";
                }

        }
private:
        std::shared_ptr<EndgenousSymbol> sym_;
        std::vector<DataFlow*> parents_;
        std::vector<DataFlow*> children_;
};
void DataFlowGraph::CollectADFlow(std::vector<std::shared_ptr<EndgenousSymbol> > & computation){
        for(auto const& flow : rank_){
                flow->EmitADFlowImpl(computation);
        }
}

void DataFlowGraph::EmitDot(std::ostream& out)const{
        DotCompiler dc;
        for(auto const& flow : rank_){
                flow->EmitDot(dc);
        }
        out << "digraph{\n";
        out << dc.nodes.str();
        out << dc.edges.str();

        out << "node [shape = none];\n";
        for(size_t idx=0;idx!=dc.order.size();++idx){
                if( idx != 0 )
                        out << "->";
                out << idx;
        }
        out << "[arrowhead=none,shape=none]\n";
        for(size_t idx=0;idx!=dc.order.size();++idx){
                out << "{rank=same;" << idx;
                for(auto const& name : dc.order[idx] ){
                        out << "," << name;
                }
                out << "}\n";
        }
        out << "}\n";
}
void DataFlowGraph::Display(std::ostream& out)const{
        for(auto const& flow : rank_){
                std::cout << "--: " << flow->Name() << "\n";
        }
}
void DataFlowGraph::EmitInstructions(InstructionBlock& B)const{
        for(auto const& flow : rank_){
                flow->EmitInstructionsImpl(B);
        }
}
void DataFlowGraph::EmitADInstructions(Operator::DependentsProfile& information,
                                       std::shared_ptr<RemapUnique> RU,
                                       InstructionBlock& B)const
{
        for(size_t idx=rank_.size();idx;){
                --idx;
                rank_[idx]->EmitADInstructionsImpl(information, RU, B);
        }
}
void DataFlowGraph::EmitCppCode(std::ostream& out)const{
        out << R"(

#include <cstdio>
#include <cmath>
#include <iostream>

double black(double t, double* d_t, double T, double* d_T, double r, double* d_r, double S, double* d_S, double K, double* d_K, double vol, double* d_vol){
)";
        for(auto const& flow : rank_){
                flow->EmitEvalCodeImpl(out);
        }
#if 0
for(size_t idx=rank_.size();idx;){
        --idx;
        rank_[idx]->EmitReverseADCodeImpl(out);
}
#endif
        out << "return " << rank_.back()->Name() << ";\n";
        out << "}";
        out <<
R"(

int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        std::cout << "black = " << value << "\n";
        std::cout << "d_t   = " << d_t << "\n";
        std::cout << "d_T   = " << d_T << "\n";
        std::cout << "d_r   = " << d_r << "\n";
        std::cout << "d_S   = " << d_S << "\n";
        std::cout << "d_K   = " << d_K << "\n";
        std::cout << "d_vol = " << d_vol << "\n";
}
)";
}

void DataFlowGraph::Add(std::shared_ptr<DataFlow> const& flow){
        rank_.push_back(flow);
        index_[flow->Name()] = flow;
        auto dependents = flow->Expr()->DepthFirstAnySymbolicDependencyNoRecurse();

        auto link_dependency = [&](auto& child, auto& parent){
                child->AddParent(parent.get());
                parent->AddChild(child.get());
        };

        for(auto const& ptr : dependents.DepthFirst ){
                auto iter = index_.find(ptr->Name());
                if( iter == index_.end() )
                        throw std::domain_error("cant find node");
                link_dependency(flow, iter->second);
        }
}

void reverse_test(){
        using namespace Frontend;
        using Frontend::Sin;
        #if 0
        auto x1 = Var("x1");
        auto x2 = Var("x2");
        auto expr_ = Break("z", Break("y", x1 * x2 + Sin(x1)));
        auto expr = expr_.as_operator_();
        #else
        auto ad_kernel = BlackScholesCallOption::Build<DoubleKernel>{};

        auto as_black = ad_kernel.Evaluate(
                DoubleKernel::BuildFromExo("t"),
                DoubleKernel::BuildFromExo("T"),
                DoubleKernel::BuildFromExo("r"),
                DoubleKernel::BuildFromExo("S"),
                DoubleKernel::BuildFromExo("K"),
                DoubleKernel::BuildFromExo("vol")
        );
        auto expr_ = as_black.as_operator_();
        RemoveEndgenousFolder remove_endogous;
        auto expr = remove_endogous.Fold(expr_);
        #endif

        auto unique_mapper = std::make_shared<RemapUnique>("w");
        auto unique        = Break("value", expr->Clone(unique_mapper)).as_operator_();
        //unique->Display();

        auto dependents = unique->DepthFirstAnySymbolicDependencyAndThis();
        std::vector<std::shared_ptr<DataFlow> > flow;
        DataFlowGraph graph;
        for(auto const& dep : dependents.DepthFirst){
                auto ptr = std::make_shared<DataFlow>(dep);
                flow.push_back(ptr);
                graph.Add(ptr);
        }
        #if 0
        for(auto const& step : flow){
                step->Expr()->Display();
        }
        #endif

        std::ofstream out("graph.dot");
        graph.EmitDot(out);
        out.close();
        std::system("dot -Tpng graph.dot -o graph.png");

        InstructionBlock B;
        graph.EmitInstructions(B);

        std::cout << "dependents.Set.size() => " << dependents.Set.size() << "\n"; // __CandyPrint__(cxx-print-scalar,dependents.Set.size())
        graph.EmitADInstructions(dependents, unique_mapper, B);

        std::ofstream code("black.cpp");
        code << R"(

#include <cstdio>
#include <cmath>
#include <iostream>

double black(double t, double* d_t, double T, double* d_T, double r, double* d_r, double S, double* d_S, double K, double* d_K, double vol, double* d_vol){
)";

        B.EmitCode(code);
        code <<
R"(
}

int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        std::cout << "black = " << value << "\n";
        std::cout << "d_t   = " << d_t << "\n";
        std::cout << "d_T   = " << d_T << "\n";
        std::cout << "d_r   = " << d_r << "\n";
        std::cout << "d_S   = " << d_S << "\n";
        std::cout << "d_K   = " << d_K << "\n";
        std::cout << "d_vol = " << d_vol << "\n";
}
)";

        #if 0
        std::vector<std::shared_ptr<EndgenousSymbol> > ad_computation;
        graph.Display();
        graph.CollectADFlow(ad_computation);
        std::vector<std::shared_ptr<Operator> > computation;
        computation.push_back(unique);

        unique_mapper->mutate_prefix("__rev_ad_");

        for(auto& ptr : ad_computation ){
                computation.push_back(ptr->Clone(unique_mapper));
                //computation.push_back(ptr);
                //ptr->Display();
        }

        std::cout << "ad_computation.size() => " << ad_computation.size() << "\n"; // __CandyPrint__(cxx-print-scalar,computation.size())
        std::cout << "computation.size() => " << computation.size() << "\n"; // __CandyPrint__(cxx-print-scalar,computation.size())

        do{

                DataFlowGraph ad_graph;
                Operator::DependentsProfile dependents;
                for(auto const& ptr : computation ){
                        ptr->CollectDepthFirstAnySymbolicDependency(dependents, true);
                }
                for(auto const& dep : dependents.DepthFirst){
                        auto ptr = std::make_shared<DataFlow>(dep);
                        flow.push_back(ptr);
                        ad_graph.Add(ptr);
                }
                ad_graph.Display();
                std::cerr << __FILE__ << ":" << __LINE__ << ":A\n"; // __CandyTag__A
                std::ofstream code("black.cpp");
                ad_graph.EmitCppCode(code);
                code.close();
        }while(0);
        #endif

}

int main(){
        //black_scholes();
        //black_scholes_frontend();
        //black_scholes_template();
        //black_scholes_template_opt();

        try{
                reverse_test();
        } catch ( std::exception const& e ){
                std::cerr << "Exception: " << e.what() << "\n";
        }
}


//  (C) Copyright Nick Thompson 2021.
//  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 <random>
#include <array>
#include <vector>
#include <iostream>
#include <benchmark/benchmark.h>
#include <boost/math/tools/cubic_roots.hpp>

using boost::math::tools::cubic_roots;

template<class Real>
void CubicRoots(benchmark::State& state)
{
    std::random_device rd;
    auto seed = rd();
    // This seed generates 3 real roots:
    //uint32_t seed = 416683252;
    std::mt19937_64 mt(seed);
    std::uniform_real_distribution<Real> unif(-10, 10);

    Real a = unif(mt);
    Real b = unif(mt);
    Real c = unif(mt);
    Real d = unif(mt);
    for (auto _ : state)
    {
        auto roots = cubic_roots(a,b,c,d);
        benchmark::DoNotOptimize(roots[0]);
    }
}

BENCHMARK_TEMPLATE(CubicRoots, float);
BENCHMARK_TEMPLATE(CubicRoots, double);
BENCHMARK_TEMPLATE(CubicRoots, long double);

BENCHMARK_MAIN();


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#ifndef BOGUS_POLYNOMIAL_IMPL_HPP
#define BOGUS_POLYNOMIAL_IMPL_HPP

#ifndef BOGUS_WITHOUT_EIGEN
#include <Eigen/Eigenvalues>
#endif

#include "Polynomial.hpp"
#include "NumTraits.hpp"

namespace bogus
{

namespace polynomial {

#ifndef BOGUS_WITHOUT_EIGEN

#ifdef _MSC_VER
#define BOGUS_TLS_SPEC __declspec( thread )
#else
// gcc on MacOS X does not support thread local storage
#if !defined( __GNUC__ ) or !defined( __APPLE__ )
#define BOGUS_TLS_SPEC __thread
#endif
#endif

template< unsigned Dimension, typename Scalar >
struct CompanionMatrix
{
	typedef Eigen::Matrix< Scalar, Dimension, Dimension > BaseType ;

#ifdef BOGUS_TLS_SPEC
	typedef typename BaseType::MapType ReturnType ;

	static ReturnType get()
	{
		static BOGUS_TLS_SPEC double s_matrix_data[ Dimension*Dimension ] ;
		static BOGUS_TLS_SPEC bool s_matrix_initialized = false ;

		ReturnType matrix( s_matrix_data ) ;

		if( !s_matrix_initialized )
		{
			matrix.template block< 1, Dimension-1> ( 0, 0 ).setZero() ;
			matrix.template block< Dimension - 1, Dimension -1 >( 1, 0 ).setIdentity() ;
			s_matrix_initialized = true ;
		}

		return matrix ;
	}
#else
	typedef BaseType ReturnType ;

	static ReturnType get()
	{
		static BaseType s_matrix ;
		static bool s_matrix_initialized = false ;

		if( !s_matrix_initialized )
		{
			s_matrix.template block< 1, Dimension-1> ( 0, 0 ).setZero() ;
			s_matrix.template block< Dimension - 1, Dimension -1 >( 1, 0 ).setIdentity() ;
			s_matrix_initialized = true ;
		}

		return s_matrix ;
	}
#endif
} ;

template< unsigned Dimension, typename Scalar >
unsigned RootsFinder< Dimension, Scalar>::getRealRoots(const Scalar *coeffs, Scalar *realRoots,
		RealRootsFilter filter )
{
	typedef CompanionMatrix< Dimension, Scalar > CM ;
	typename CM::ReturnType matrix = CM::get() ;

	matrix.template block< Dimension, 1 >( 0, Dimension - 1 ) = -Eigen::Matrix< Scalar, Dimension, 1 >::Map( coeffs ) ;
	const typename Eigen::EigenSolver< typename CM::BaseType >::EigenvalueType& ev = matrix.eigenvalues() ;

	unsigned count = 0 ;
	for( unsigned i = 0 ; i < Dimension ; ++i )
	{
		if( NumTraits< Scalar >::isZero( std::imag( ev[i] ) ) )
		{
			const bool discard =
					( filter == StrictlyPositiveRoots && std::real( ev[i] ) <= 0 ) ||
					( filter == StrictlyNegativeRoots && std::real( ev[i] ) >= 0 ) ;
			if( !discard ) realRoots[ count++ ] = std::real( ev[i] ) ;
		}
	}
	return count ;
}

#else
template< unsigned Dimension, typename Scalar >
unsigned RootsFinder< Dimension, Scalar>::getRealRoots(const Scalar *coeffs, Scalar *realRoots,
		RealRootsFilter filter )
{
	assert( 0 && "bogus::Polynomial::RootsFinder::getRealRoots requires Eigen" ) ;
	return 0 ;
}
#endif

template< typename Scalar >
struct PossiblyDegenerateRootsFinder< 0, Scalar >
{
	static unsigned getRealRoots( const Scalar* coeffs,
								  Scalar* realRoots,
								  RealRootsFilter filter = AllRoots )
	{
		realRoots[0] = 0. ;
		return filter == AllRoots && NumTraits< Scalar >::isZero( coeffs[0] ) ;
	}
} ;

template< unsigned Dimension, typename Scalar >
unsigned PossiblyDegenerateRootsFinder< Dimension, Scalar>::getRealRoots( Scalar *coeffs, Scalar *realRoots,
		RealRootsFilter filter )
{
	if( NumTraits< Scalar >::isZero( coeffs[ Dimension ] ) )
	{
		return PossiblyDegenerateRootsFinder< Dimension - 1, Scalar >::getRealRoots( coeffs, realRoots, filter ) ;
	}
	const Scalar inv = 1./coeffs[Dimension] ;
	for( unsigned k = 0 ; k < Dimension ; ++k )
	{
		coeffs[k] *= inv ;
	}
	return RootsFinder< Dimension, Scalar >::getRealRoots( coeffs, realRoots, filter ) ;
}

} //namespace polynomial

} //namespace bogus

#endif


/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial:  Use of the iterative solvers in ViennaCL with Eigen (http://eigen.tuxfamily.org/)
*
*/

//
// include necessary system headers
//
#include <iostream>

#ifndef NDEBUG
 #define NDEBUG
#endif

//
// Include Eigen headers
//
#include <Eigen/Core>
#include <Eigen/Sparse>

// Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on Eigen objects
#define VIENNACL_WITH_EIGEN 1

//
// ViennaCL includes
//
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"
#include "../benchmarks/benchmark-utils.hpp"

int main(int, char *[])
{
  typedef float ScalarType;

  Eigen::SparseMatrix<ScalarType, Eigen::RowMajor> eigen_matrix(65025, 65025);
  Eigen::VectorXf eigen_rhs;
  Eigen::VectorXf eigen_result;
  Eigen::VectorXf ref_result;
  Eigen::VectorXf residual;

  //
  // Read system from file
  //
  std::cout << "Reading matrix..." << std::endl;
  eigen_matrix.reserve(65025 * 7);
  if (!viennacl::io::read_matrix_market_file(eigen_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }
  //eigen_matrix.endFill();
  std::cout << "Done: reading matrix" << std::endl;

  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", eigen_rhs))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ref_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }

  //CG solver:
  std::cout << "----- Running CG -----" << std::endl;
  eigen_result = viennacl::linalg::solve(eigen_matrix, eigen_rhs, viennacl::linalg::cg_tag());

  residual = eigen_matrix * eigen_result - eigen_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(eigen_rhs) << std::endl;

  //BiCGStab solver:
  std::cout << "----- Running BiCGStab -----" << std::endl;
  eigen_result = viennacl::linalg::solve(eigen_matrix, eigen_rhs, viennacl::linalg::bicgstab_tag());

  residual = eigen_matrix * eigen_result - eigen_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(eigen_rhs) << std::endl;

  //GMRES solver:
  std::cout << "----- Running GMRES -----" << std::endl;
  eigen_result = viennacl::linalg::solve(eigen_matrix, eigen_rhs, viennacl::linalg::gmres_tag());

  residual = eigen_matrix * eigen_result - eigen_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(eigen_rhs) << std::endl;

}


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
/**
 * @file target.hpp
 * @author Boston Cleek
 * @date 17 Nov 2020
 * @brief Target distribution
 */
#ifndef TARGET_HPP
#define TARGET_HPP

#include <armadillo>

#include <visualization_msgs/MarkerArray.h>

#include <ergodic_exploration/grid.hpp>
#include <ergodic_exploration/numerics.hpp>

namespace ergodic_exploration
{
using arma::mat;
using arma::vec;
struct Gaussian;
typedef std::vector<Gaussian> GaussianList;

/** @brief 2D gaussian */
struct Gaussian
{
  /** @brief Constructor */
  Gaussian()
  {
  }

  /**
   * @brief Constructor
   * @param mu - mean [mean x, mean y]
   * @param sigmas - standard deviations [sigma x, sigma y]
   */
  Gaussian(const vec& mu, const vec& sigmas)
    : mu(mu), cov(arma::diagmat(square(sigmas))), cov_inv(inv(cov))
  {
  }

  /**
   * @brief Evaluate gaussian
   * @param pt - point [x y]
   * @return evaluated gaussian at pt
   */
  double operator()(const vec& pt) const
  {
    const vec diff = pt - mu;
    return std::exp(-0.5 * dot(diff.t() * cov_inv, diff));
  }

  /**
   * @brief Evaluate gaussian
   * @param pt - point [x y]
   * @param trans - translation from map frame to fourier domain
   * @return evaluated gaussian at pt translated by trans
   * @details the translation is used to translate the mean into the fourier domain
   */
  double operator()(const vec& pt, const vec& trans) const
  {
    // DEBUG
    // if (any(mu - trans) < 0.0)
    // {
    //   std::cout << "WARNING: Targert mean not within fourier domain" << std::endl;
    // }

    // translate mu into frame of fourier domain
    const vec diff = pt - (mu - trans);
    return std::exp(-0.5 * dot(diff.t() * cov_inv, diff));
  }

  vec mu;       // mean
  mat cov;      // covariance
  mat cov_inv;  // inverse of covariance
};

/** @brief Target distribution */
class Target
{
public:
  /** @brief Constructor */
  Target();

  /**
   * @brief Constructor
   * @param gaussians - list of target gaussians
   */
  Target(const GaussianList& gaussians);

  /**
   * @brief Adds gaussian to list
   * @param g - gaussians
   */
  void addGaussian(const Gaussian& g);

  /**
   * @brief Remove gaussian from list
   * @param idx - index of gaussian to remove
   */
  void deleteGaussian(unsigned int idx);

  /**
   * @brief Evaluate the list of gaussians
   * @param pt - point [x y]
   * @param trans - translation from map frame to fourier domain
   * @return value of the list of gaussians evaluated at pt translated by trans
   * @details the translation is used to translate the mean into the fourier domain
   */
  double evaluate(const vec& pt, const vec& trans) const;

  /**
   * @brief Evaluate the target distribution
   * @param trans - translation from map frame to fourier domain
   * @param phi_grid - discretization of fourier domain
   * @return target evaluated at each grid cell in phi_grid
   * @details the translation is used to translate the mean into the fourier domain
   */
  vec fill(const vec& trans, const mat& phi_grid) const;

  /**
   * @brief Visualize target distribution
   * @param frame - target frame
   * @return target is visualized as an ellipse
   */
  visualization_msgs::MarkerArray markers(const std::string& frame) const;

private:
  GaussianList gaussians_;  // list of target gaussians
};
}  // namespace ergodic_exploration
#endif


///////////////////////////////////////////////////////////////
//  Copyright 2015 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <iostream>
#include <iomanip>
#include <vector>
#include <iterator>

// Contains Quickbook snippets in comments.

//[IE2

/*`
Importing or exporting cpp_bin_float is similar, but we must proceed via an intermediate integer:
*/
/*=
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <iostream>
#include <iomanip>
#include <vector>
#include <iterator>
*/

int main()
{
   using boost::multiprecision::cpp_bin_float_100;
   using boost::multiprecision::cpp_int;
   // Create a cpp_bin_float to import/export:
   cpp_bin_float_100 f(1);
   f /= 3;
   // export into 8-bit unsigned values, most significant bit first:
   std::vector<unsigned char> v;
   export_bits(cpp_int(f.backend().bits()), std::back_inserter(v), 8);
   // Grab the exponent as well:
   int e = f.backend().exponent();
   // Import back again, and check for equality, we have to proceed via
   // an intermediate integer:
   cpp_int i;
   import_bits(i, v.begin(), v.end());
   cpp_bin_float_100 g(i);
   g.backend().exponent() = e;
   BOOST_ASSERT(f == g);
}

//]


// A numerically integrated implementation of the circuit described in
// "Rolling Your Own Circuit Simulator with Eigen and Boost.ODEInt"
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <iostream>
#include <array>
#include <boost/numeric/odeint.hpp>

typedef std::array<double, 2> state_t;   // 0 = V_out, 1 = I_L

struct circuit {
    circuit(double r, double l, double c) : r_(r), l_(l), c_(c) {}

    void operator()(state_t const& x, state_t& dxdt, double t) {
        // calculate state derivatives from current state
        dxdt[0] = ((1 - x[0]) / r_ - x[1]) / c_;  // KCL at V_out node
        dxdt[1] = x[0] / l_;                      // from V_out = L * dI_L/dt
    }
private:
    double r_, l_, c_;
};

int main() {
    using namespace boost::numeric::odeint;
    circuit ckt(100.0, 20e-6, 20e-9);
    state_t x{0.0, 0.0};                    // initial conditions
    integrate( ckt, x, 0.0, 10e-6, 0.1e-6,  // time range and increment
               [](state_t const& x, double t) {
                   std::cout << t << " " << x[0] << std::endl;
               });
}


#define CATCH_CONFIG_ENABLE_BENCHMARKING
#include "catch.hpp"
#include <boost/numeric/ublas/matrix.hpp>

#include <libUncertainty/correlation.hpp>
#include <libUncertainty/uncertain.hpp>
#include <libUncertainty/propagate.hpp>
#include <BoostUnitDefinitions/Units.hpp>

using namespace boost::units;
using namespace libUncertainty;

TEST_CASE("Correlations Utilities")
{
  SECTION("Correlation matrix")
  {
    correlation_matrix<double> mat(3);
    CHECK(mat(0, 1) == Approx(0).scale(1));
    CHECK(mat(1, 0) == Approx(0).scale(1));
    CHECK(mat(0, 2) == Approx(0).scale(1));
    CHECK(mat(2, 0) == Approx(0).scale(1));
    CHECK(mat(0, 3) == Approx(0).scale(1));
    CHECK(mat(3, 0) == Approx(0).scale(1));
    CHECK(mat(0, 0) == Approx(1));
    CHECK(mat(1, 1) == Approx(1));
    CHECK(mat(2, 2) == Approx(1));

    mat(0, 1) = 0.1;
    mat(0, 2) = 0.2;
    mat(1, 2) = 0.3;

    CHECK(mat(0, 1) == Approx(0.1));
    CHECK(mat(1, 0) == Approx(0.1));
    CHECK(mat(0, 2) == Approx(0.2));
    CHECK(mat(2, 0) == Approx(0.2));
    CHECK(mat(0, 3) == Approx(0.3));
    CHECK(mat(3, 0) == Approx(0.3));
    CHECK(mat(0, 0) == Approx(1));
    CHECK(mat(1, 1) == Approx(1));
    CHECK(mat(2, 2) == Approx(1));
  }

  SECTION("Error propagation w/ correlation matrix.")
  {
    boost::numeric::ublas::matrix<double> corr(2, 2);
    corr(0, 0) = 1;
    corr(1, 0) = 1;
    corr(0, 1) = 1;
    corr(1, 1) = 1;

    uncertain<double> x(2, 0.1), y(3, 0.1);

    auto z = basic_error_propagator::propagate_error([](double a, double b) { return b - a; }, corr, x, y);

    // a and b are directly correlated, so there is no uncertainy in their difference.
    // the nominal and upper values should be the same.
    CHECK(z.nominal() == Approx(1));
    CHECK(z.upper() == Approx(1));
  }

  SECTION("add_correlation_coefficients Mixin")
  {
    add_correlation_coefficient_array<uncertain<double>> x({2.2, 0.1});
    add_correlation_coefficient_array<uncertain<double>> y = {3.3, 0.2};

    CHECK(x.get_correlation_coefficients().size() == 0);
    x.set_correlation_coefficient_array_size(3);
    x.get_correlation_coefficients()[0] = 1;
    x.get_correlation_coefficients()[1] = 2;
    x.get_correlation_coefficients()[2] = 3;

    auto z = x;

    CHECK(x.get_correlation_coefficients().size() == 3);
    CHECK(x.get_correlation_coefficients()[0] == Approx(1));
    CHECK(x.get_correlation_coefficients()[1] == Approx(2));
    CHECK(x.get_correlation_coefficients()[2] == Approx(3));

    CHECK(z.get_correlation_coefficients().size() == 3);
    CHECK(z.get_correlation_coefficients()[0] == Approx(1));
    CHECK(z.get_correlation_coefficients()[1] == Approx(2));
    CHECK(z.get_correlation_coefficients()[2] == Approx(3));
  }

  SECTION("Correlation store")
  {
    add_id<uncertain<double>> x,y,z;
    correlation_store<double> store;

    store.add(x, y, 0.1);
    store.add(y, z, 0.2);

    CHECK_THROWS(store.add(x, y, 0.1));

    CHECK(store.get(x, y) == Approx(0.1));
    CHECK(store.get(y, z) == Approx(0.2));

    auto& global_store = get_global_correlation_store();

    global_store.add(x, y, 0.1);
    global_store.add(x, z, 0.2);

    CHECK_THROWS(global_store.add(y, x, 0.1));

    CHECK(global_store.get(x, y) == Approx(0.1));
    CHECK(global_store.get(x, z) == Approx(0.2));

    global_store.set(x,y,0.5);
    global_store.set(x,z,-0.5);
    global_store.set(z,y,1);

    CHECK(global_store.get(y,x) == Approx(0.5) );
    CHECK(global_store.get(z,x) == Approx(-0.5) );
    CHECK(global_store.get(z,y) == Approx(1) );

  }

  SECTION("Error propagation w/ correlation")
  {
    SECTION("Doubles")
    {
      uncertain<double> x(1, 0.1), y(2, 0.2);

      SECTION("Independent")
      {
        auto z_and_correlation = basic_error_propagator::propagate_error_and_correlation([](double a, double b) { return a + b; }, x, y);

        double unc = sqrt(0.1 * 0.1 + 0.2 * 0.2);
        CHECK(z_and_correlation.nominal() == Approx(3));
        CHECK(z_and_correlation.upper() == Approx(3 + unc));
        CHECK(z_and_correlation.get_correlation_coefficients().size() == 2);
        CHECK(z_and_correlation.get_correlation_coefficient(0) == Approx(0.1 / unc));
        CHECK(z_and_correlation.get_correlation_coefficient(1) == Approx(0.2 / unc));
      }

      SECTION("with Correlation Matrix")
      {
        correlation_matrix<double> corr(2);
        corr(0, 1) = -1;

        auto z_and_correlation = basic_error_propagator::propagate_error_and_correlation([](double a, double b) { return a + b; }, corr, x, y);

        double unc = 0.1;
        CHECK(z_and_correlation.nominal() == Approx(3));
        CHECK(z_and_correlation.upper() == Approx(3 + unc));
        CHECK(z_and_correlation.get_correlation_coefficients().size() == 2);
        CHECK(z_and_correlation.get_correlation_coefficients().size() == 2);
        CHECK(z_and_correlation.get_correlation_coefficient(0) == Approx((0.1 + 0.2 * -1) / unc));
        CHECK(z_and_correlation.get_correlation_coefficient(1) == Approx((0.2 + 0.1 * -1) / unc));
      }
      SECTION("with Correlation Store")
      {
        correlation_store<double> cstore;
        add_id<uncertain<double>> xx;
        add_id<uncertain<double>> yy;
        xx = x;
        yy = y;

        cstore.add(xx, yy, -1);

        auto z = basic_error_propagator::propagate_error([](double a, double b) { return a + b; }, cstore, xx, yy);

        double unc = 0.1;
        CHECK(z.nominal() == Approx(3));
        CHECK(z.upper() == Approx(3 + unc));
        CHECK(cstore.get(z, xx) == Approx((0.1 + 0.2 * -1) / unc));
        CHECK(cstore.get(z, yy) == Approx((0.2 + 0.1 * -1) / unc));

      }

    }

    SECTION("boost quantities")
    {
      uncertain<quantity<t::cm>> x(1 * i::cm, 0.1 * i::cm), y(2 * i::cm, 0.2 * i::cm);
      SECTION("Independent")
      {
        SECTION("Addition")
        {
          auto z_and_correlation = basic_error_propagator::propagate_error_and_correlation([](auto a, auto b) { return a + b; }, x, y);

          double unc = sqrt(0.1 * 0.1 + 0.2 * 0.2);
          CHECK(z_and_correlation.nominal().value() == Approx(3));
          CHECK(z_and_correlation.upper().value() == Approx(3 + unc));
          CHECK(z_and_correlation.get_correlation_coefficients().size() == 2);
          CHECK(z_and_correlation.get_correlation_coefficients().size() == 2);
          CHECK(z_and_correlation.get_correlation_coefficient(0) == Approx(0.1 / unc));
          CHECK(z_and_correlation.get_correlation_coefficient(1) == Approx(0.2 / unc));
        }

        SECTION("Multiplyication")
        {
          auto z_and_correlation = basic_error_propagator::propagate_error_and_correlation([](auto a, auto b) { return a * b; }, x, y);

          double unc = sqrt(0.2 * 0.2 + 0.2 * 0.2);
          CHECK(z_and_correlation.nominal().value() == Approx(2));
          CHECK(z_and_correlation.upper().value() == Approx(2 + unc));
          CHECK(z_and_correlation.get_correlation_coefficients().size() == 2);
          CHECK(z_and_correlation.get_correlation_coefficient(0) == Approx(0.2 / unc));
          CHECK(z_and_correlation.get_correlation_coefficient(1) == Approx(0.2 / unc));
        }
      }
      SECTION("with Correlation Matrix")
      {
        correlation_matrix<double> corr(2);
        corr(0, 1) = -1;

        auto z_and_correlation = basic_error_propagator::propagate_error_and_correlation([](auto a, auto b) { return a + b; }, corr, x, y);

        double unc = 0.1;
        CHECK(z_and_correlation.nominal().value() == Approx(3));
        CHECK(z_and_correlation.upper().value() == Approx(3 + unc));
        CHECK(z_and_correlation.get_correlation_coefficients().size() == 2);
        CHECK(z_and_correlation.get_correlation_coefficients().size() == 2);
        CHECK(z_and_correlation.get_correlation_coefficient(0) == Approx((0.1 + 0.2 * -1) / unc));
        CHECK(z_and_correlation.get_correlation_coefficient(1) == Approx((0.2 + 0.1 * -1) / unc));
      }
    }
  }
}


#pragma once

#include <Eigen/Core>

using Vec2i = Eigen::Vector2i;
using Vec2f = Eigen::Vector2f;
using Vec2d = Eigen::Vector2d;

using Vec3i = Eigen::Vector3i;
using Vec3f = Eigen::Vector3f;
using Vec3d = Eigen::Vector3d;


//==================================================================================================
/*!
  @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_SQRT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SQRT_HPP_INCLUDED

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

  /*!

    @ingroup group-arithmetic
    This function object computes the square root of its parameter.
    For integers it is the truncation of the real square root.

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

    @par Decorators

    - std_ calls std::sqrt

    - raw_ for floating entries can gain some speed with less accuracy
    on some architectures.

    @see rsqrt, sqr_abs, sqr

    @par Example:

      @snippet sqrt.cpp sqrt

    @par Possible output:

      @snippet sqrt.txt sqrt

  **/
  Value sqrt(Value const& x);
} }
#endif

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

#endif


#ifndef STAN_MATH_PRIM_FUN_INC_BETA_DDZ_HPP
#define STAN_MATH_PRIM_FUN_INC_BETA_DDZ_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/fun/exp.hpp>
#include <stan/math/prim/fun/lgamma.hpp>
#include <stan/math/prim/fun/log.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Returns the partial derivative of the regularized
 * incomplete beta function, I_{z}(a, b) with respect to z.
 *
 * @tparam T scalar types of arguments
 * @param a first argument
 * @param b second argument
 * @param z upper bound of the integral
 * @return partial derivative of the incomplete beta with respect to z
 *
 * @pre a > 0
 * @pre b > 0
 * @pre 0 < z <= 1
 */
template <typename T>
T inc_beta_ddz(T a, T b, T z) {
  using std::exp;
  using std::log;
  return exp((b - 1) * log(1 - z) + (a - 1) * log(z) + lgamma(a + b) - lgamma(a)
             - lgamma(b));
}

template <>
inline double inc_beta_ddz(double a, double b, double z) {
  using boost::math::ibeta_derivative;
  return ibeta_derivative(a, b, z);
}

}  // namespace math
}  // namespace stan
#endif


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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 <fstream>
#include <vector>
#include <boost/tuple/tuple.hpp>

#include <rokko/utility/xyz_hamiltonian.hpp>
#include <rokko/localized_matrix.hpp>
#include <rokko/localized_vector.hpp>

int main(int argc, char *argv[]) {
  int L, N, num_bonds;
  L = 4;
  num_bonds = L - 1;
  N = 1 << L;
  std::vector<std::pair<int, int> > lattice;
  std::vector<boost::tuple<double, double, double> > coupling;
  for (int i=0; i<L-1; ++i) {
    lattice.push_back(std::make_pair(i, i+1));
    coupling.push_back(boost::make_tuple(1, 1, 1));
  }

  std::cout << "dim=" << N << std::endl;
  std::cout << "L=" << L << std::endl;
  for (int i=0; i<num_bonds; ++i) {
    std::cout << lattice[i].first << " " << lattice[i].second << " " << coupling[i].get<0>() << " " << coupling[i].get<1>() << " " << coupling[i].get<2>() << std::endl;
  }

  rokko::localized_matrix<double, rokko::matrix_col_major> mat1(N, N);
  std::cout << "multiply:" << std::endl;
  for (int i=0; i<N; ++i) {
    std::vector<double> v, w;
    v.assign(N, 0);
    v[i] = 1;
    w.assign(N, 0);
    rokko::xyz_hamiltonian::multiply(L, lattice, coupling, v, w);
    for (int j=0; j<N; ++j) {
      mat1(j,i) = w[j];
      std::cout << w[j] << " ";
    }
    std::cout << std::endl;
  }

  std::cout << "fill_diagonal:" << std::endl;
  rokko::localized_vector<double> diagonal(N);
  std::vector<double> v(N);
  rokko::xyz_hamiltonian::fill_diagonal(L, lattice, coupling, v);
  for (int j=0; j<N; ++j) {
    diagonal(j) = v[j];
    std::cout << v[j] << " ";
  }
  std::cout << std::endl;

  std::cout << "fill_matrix:" << std::endl;
  rokko::localized_matrix<double, rokko::matrix_col_major> mat2(N, N);
  rokko::xyz_hamiltonian::generate(L, lattice, coupling, mat2);
  for (int i=0; i<N; ++i) {
    for (int j=0; j<N; ++j) {
      std::cout << mat2(i,j) << " ";
    }
    std::cout << std::endl;
  }

  if (mat1 == mat2) {
    std::cout << "OK: matrix by 'multiply' equals to a matrix by 'generate'." << std::endl;
  } else {
    std::cout << "ERROR: matrix by 'multiply' is differnet from a matrix by 'generate'."<< std::endl;
    exit(1);
  }

  if (diagonal == mat2.diagonal()) {
    std::cout << "OK: diagonal by 'fill_diagonal' equals to diagonal elementas of a matrix by 'genertate'."<< std::endl;
  } else {
    std::cout << "ERROR: diagonal by 'fill_diagonal' is differnet from diagonal elementas of a matrix by 'genertate'."<< std::endl;
    exit(1);
  }

}


/* test_chi_squared_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/chi_squared_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::chi_squared_distribution<>
#define BOOST_RANDOM_ARG1 n
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MIN 0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0.0
#define BOOST_RANDOM_TEST1_MAX 100.0

#define BOOST_RANDOM_TEST2_PARAMS (10000.0)
#define BOOST_RANDOM_TEST2_MIN 100.0

#include "test_distribution.ipp"


#include "hoNDFFT.h"
#include "hoNDArray_math.h"
#include "complext.h"
#include <gtest/gtest.h>
#include <boost/random.hpp>
#include <random>

using namespace Gadgetron;
using testing::Types;

template<typename REAL> class hoNDFFT_test : public ::testing::Test {
protected:
	virtual void SetUp(){
		boost::random::mt19937 rng;
		boost::random::uniform_real_distribution<REAL> uni(0,1);
		std::vector<size_t > dimensions(3,128);

		Array = hoNDArray<complext<REAL> >(dimensions);
		complext<REAL>* data = Array.get_data_ptr();

		for (size_t i = 0; i < Array.get_number_of_elements(); i++)
			data[i] = complext<REAL>(uni(rng),uni(rng));

		Array2 = Array;
	}

	hoNDArray<complext<REAL> > Array;

	hoNDArray<complext<REAL> > Array2;

};
typedef Types<float, double> realImplementations;
TYPED_TEST_SUITE(hoNDFFT_test, realImplementations);

TYPED_TEST(hoNDFFT_test,fftNrm2Test){
	hoNDFFT<TypeParam>::instance()->fft(&this->Array);

	EXPECT_NEAR(nrm2(&this->Array2),nrm2(&this->Array),nrm2(&this->Array)*1e-2);

}

TYPED_TEST(hoNDFFT_test,ifftNrm2Test){
	hoNDFFT<TypeParam>::instance()->ifft(&this->Array);

	EXPECT_NEAR(nrm2(&this->Array2),nrm2(&this->Array),nrm2(&this->Array)*1e-2);

}

TYPED_TEST(hoNDFFT_test,fft1Nrm2Test){
    hoNDFFT<TypeParam>::instance()->fft(&this->Array,0);

    EXPECT_NEAR(nrm2(&this->Array2),nrm2(&this->Array),nrm2(&this->Array)*1e-2);

}

TEST(FFTshiftTest,shift1d_inplace){

    auto array = hoNDArray<std::complex<float>>(24);
    array.fill(0.0f);
    array(2) = 1;
    hoNDFFT<float>::instance()->fftshift1D(array);
    EXPECT_EQ(array(14),1.0f);
    EXPECT_EQ(array(2),0.0f);

}

TEST(FFTshiftTest,shift1d){

    auto array = hoNDArray<std::complex<float>>(24);
    array.fill(0.0f);
    array(2) = 1;
    auto output = array;
    output.fill(0.0f);
    hoNDFFT<float>::instance()->fftshift1D(array,output);
    EXPECT_EQ(output(14),1.0f);
    EXPECT_EQ(output(2),0.0f);

}

TEST(FFTshiftTest,shift2d_inplace){

    auto array = hoNDArray<std::complex<float>>(24,8);
    array.fill(0.0f);
    array(2,3) = 1;
    hoNDFFT<float>::instance()->fftshift2D(array);
    EXPECT_EQ(array(14,7),1.0f);
    EXPECT_EQ(array(2,3),0.0f);

}

TEST(FFTshiftTest,shift2d){

    auto array = hoNDArray<std::complex<float>>(24,8);
    array.fill(0.0f);
    array(2,3) = 1;
    auto output = array;
    output.fill(0.0f);
    hoNDFFT<float>::instance()->fftshift2D(array,output);
    EXPECT_EQ(output(14,7),1.0f);
    EXPECT_EQ(output(2,3),0.0f);

}

TEST(FFTshiftTest,shift3d_inplace){

    auto array = hoNDArray<std::complex<float>>(24,8, 26,3);
    array.fill(0.0f);
    array(2,3, 4,0) = 1;
    hoNDFFT<float>::instance()->fftshift3D(array);
    EXPECT_EQ(array(14,7, 17,0),1.0f);
    EXPECT_EQ(array(2,3, 4,0),0.0f);

}

TEST(FFTshiftTest,shift3d){

    auto array = hoNDArray<std::complex<float>>(24,8, 26,3);
    array.fill(0.0f);
    array(2,3, 4,0) = 1;
    auto output = array;
    output.fill(0.0f);
    hoNDFFT<float>::instance()->fftshift3D(array,output);
    EXPECT_EQ(output(14,7,17,0),1.0f);
    EXPECT_EQ(output(2,3, 4,0),0.0f);

}

template<class... INDICES>
static auto make_random_array(INDICES... indices){
    auto array = hoNDArray<std::complex<float>>(indices...);
    std::default_random_engine e1;
    std::uniform_real_distribution<float> dist{};
    for (auto& val : array) val = {dist(e1),dist(e1)};

    return array;
}

TEST(FFTshiftTest, shift3d_random){
    auto array = make_random_array(24,8,26,3);

    const auto array_copy = array;

    hoNDFFT<float>::instance()->fftshift3D(array);
    hoNDFFT<float>::instance()->ifftshift3D(array);

    EXPECT_EQ(array,array_copy);

    auto buffer = array;
    hoNDFFT<float>::instance()->fftshift3D(array,buffer);
    hoNDFFT<float>::instance()->ifftshift3D(buffer,array);

    EXPECT_EQ(array,array_copy);

    hoNDFFT<float>::instance()->ifftshift3D(array, buffer);
    hoNDFFT<float>::instance()->fftshift3D(buffer);

    EXPECT_EQ(buffer,array_copy);

}


// solving A * X = B
// A symmetric
// driver function sysv()

#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/sysv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include "utils2.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cin;
using std::cout;
using std::endl;

typedef double real;
typedef std::complex<real> cmplx_t;

typedef ublas::matrix<real, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;

typedef ublas::symmetric_adaptor<m_t, ublas::lower> symml_t;
typedef ublas::symmetric_adaptor<m_t, ublas::upper> symmu_t;

typedef ublas::symmetric_adaptor<cm_t, ublas::lower> csymml_t;
typedef ublas::symmetric_adaptor<cm_t, ublas::upper> csymmu_t;

template <typename M>
void init_symm2 (M& m) {
  for (int i = 0; i < m.size1(); ++i)
    for (int j = i; j < m.size1(); ++j)
      m (i, j) = m (j, i) = 1 + j - i;
}

int main() {

  cout << endl;

  // symmetric
  cout << "real symmetric\n" << endl;

  size_t n;
  cout << "n -> ";
  cin >> n;
  if (n < 5) n = 5;
  cout << "min n = 5" << endl << endl;
  size_t nrhs = 2;
  m_t al (n, n), au (n, n);  // matrices (storage)
  symml_t sal (al);   // symmetric adaptor
  symmu_t sau (au);   // symmetric adaptor
  m_t x (n, nrhs);
  m_t bl (n, nrhs), bu (n, nrhs);  // RHS matrices

  init_symm2 (al);
  ublas::swap (row (al, 1), row (al, 4));
  ublas::swap (column (al, 1), column (al, 4));

  print_m (al, "al");
  cout << endl;

  init_symm2 (au);
  ublas::swap (row (au, 2), row (au, 3));
  ublas::swap (column (au, 2), column (au, 3));

  print_m (au, "au");
  cout << endl;

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  bl = ublas::prod (sal, x);
  bu = ublas::prod (sau, x);

  print_m (bl, "bl");
  cout << endl;
  print_m (bu, "bu");
  cout << endl;

  m_t al1 (al), au1 (au);  // for part 2
  m_t bl1 (bl), bu1 (bu);

  lapack::sysv (sal, bl);
  print_m (bl, "xl");
  cout << endl;

  lapack::sysv (sau, bu);
  print_m (bu, "xu");
  cout << endl;

  // part 2

  std::vector<int> ipiv (n);
  std::vector<real> work (1);

  int err = lapack::sysv ('L', al1, ipiv, bl1, work);
  print_m (al1, "al1 factored");
  cout << endl;
  print_v (ipiv, "ipiv");
  cout << endl;
  print_m (bl1, "xl1");
  cout << endl;

  err = lapack::sysv ('U', au1, ipiv, bu1, work);
  print_m (au1, "au1 factored");
  cout << endl;
  print_v (ipiv, "ipiv");
  cout << endl;
  print_m (bu1, "xu1");
  cout << endl;
  cout << endl;

  //////////////////////////////////////////////////////////
  cout << "\n==========================================\n" << endl;
  cout << "complex symmetric\n" << endl;

  cm_t cal (n, n), cau (n, n);   // matrices (storage)
  csymml_t scal (cal);   // hermitian adaptor
  csymmu_t scau (cau);   // hermitian adaptor
  cm_t cx (n, 1);
  cm_t cbl (n, 1), cbu (n, 1);  // RHS

  init_symm2 (cal);
  init_symm2 (cau);
  cal *= cmplx_t (1, 1);
  cau *= cmplx_t (1, -0.5);

  print_m (cal, "cal");
  cout << endl;
  print_m (cau, "cau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = ublas::prod (scal, cx);
  cbu = ublas::prod (scau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

  int ierr = lapack::sysv (scal, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  std::vector<cmplx_t> cwork (n);

  ierr = lapack::sysv (scau, ipiv, cbu, cwork);
  if (ierr == 0) {
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu, "cxu");
  }
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

}


#pragma once
#ifndef CELL_HPP
#define CELL_HPP

// c++ libraries
#include <iosfwd>
// eigen libraries
#include <Eigen/Dense>
// ann - serialization
#include "src/mem/serialize.hpp"

//****************************************************************
//Cell class
//****************************************************************

class Cell{
private:
	//==== members ====
	double vol_;//the volume of the simulation cell
	Eigen::Matrix3d R_;//the lattice vector matrix (lattice vectors are columns of the matrix)
	Eigen::Matrix3d RInv_;//the inverse of the lattice vector matrix
	Eigen::Matrix3d K_;//the repiprocal lattice vector matrix (lattice vectors are columns of the matrix
	Eigen::Matrix3d KInv_;//the inverse of the reciprocal lattice vector matrix
public:
	//==== constructors/destructors ====
	Cell(){defaults();}
	Cell(const Eigen::Matrix3d& R){init(R);}
	~Cell(){}

	//==== operators ====
	friend std::ostream& operator<<(std::ostream& out, const Cell& cell);

	//==== access ====
	double& vol(){return vol_;}
	const double& vol()const{return vol_;}
	const Eigen::Matrix3d& R()const{return R_;}
	const Eigen::Matrix3d& RInv()const{return RInv_;}
	const Eigen::Matrix3d& K()const{return K_;}
	const Eigen::Matrix3d& KInv()const{return KInv_;}

	//==== static functions - vector operations ====
	static Eigen::Vector3d& sum(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& sum, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);
	static Eigen::Vector3d& diff(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& diff, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);
	static double dist(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& tmp, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);
	static double dist(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);
	static Eigen::Vector3d& fracToCart(const Eigen::Vector3d& vFrac, Eigen::Vector3d& vCart, const Eigen::Matrix3d& R);
	static Eigen::Vector3d& cartToFrac(const Eigen::Vector3d& vCart, Eigen::Vector3d& vFrac, const Eigen::Matrix3d& RInv);
	static Eigen::Vector3d& returnToCell(const Eigen::Vector3d& v1, Eigen::Vector3d& v2, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);

	//==== vector operations ====
	Eigen::Vector3d& sum(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& sum)const;
	Eigen::Vector3d& diff(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& diff)const;
	double dist(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)const;
	double dist(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& tmp)const;
	double dist2(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& tmp)const;
	Eigen::Vector3d& modv(const Eigen::Vector3d& v1, Eigen::Vector3d& v2);

	//==== static functions - modification ====
	static Cell& make_super(const Eigen::Vector3i& s, const Cell& cell1, Cell& cell2);

	//==== member functions ====
	void defaults();
	void clear(){defaults();}
	void init(const Eigen::Matrix3d& R);
};

bool operator==(const Cell& c1, const Cell& c2);
bool operator!=(const Cell& c1, const Cell& c2);

namespace serialize{

	//**********************************************
	// byte measures
	//**********************************************

	template <> int nbytes(const Cell& obj);

	//**********************************************
	// packing
	//**********************************************

	template <> int pack(const Cell& obj, char* arr);

	//**********************************************
	// unpacking
	//**********************************************

	template <> int unpack(Cell& obj, const char* arr);

}

#endif


/* Copyright 2020 Oinam Romesh Meitei

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cmath>
#include <vector>
#include "pulsec.h"

/* Piecewise square pulse */

std::complex<double> pcoef(double &t, std::vector< double > &amp,
			   std::vector< double > &tseq,
			   double &freq,
			   double &tfinal){

  double  etmp;
  int i, tlen;
  std::complex <double>  coef;

  std::complex<double> etmp1(0.0, -1.0*freq*t);
  etmp1 = std::exp(etmp1);

  tlen = tseq.size();
  for (i=0;i<tlen;i++){
    if (i==0){
      if (0.0 < t && t <= tseq[i]){
	coef = amp[i] * etmp1;
      }
    }
    else {
      if (tseq[i-1] < t && t <= tseq[i]){
	coef = amp[i] * etmp1;
      }
    }
  }
  if (tseq[tlen-1] < t && t <= tfinal){
    coef = amp[tlen] * etmp1;

  }
  return coef;
}

Eigen::SparseMatrix<std::complex<double> >
getham(double &t, pulsec &pobj,
       std::vector< std::vector< Eigen::SparseMatrix<double,0,ptrdiff_t> > > &hdrive,
       std::vector< std::complex<double> > &dsham, int &dsham_len,
       Eigen::SparseMatrix<std::complex<double> > &matexp_){

  // dsham is different from python version, here it's the diagonal of
  // -1j*hobj.dsham in the python version.

  Eigen::SparseMatrix<std::complex<double> > hamdr;
  std::complex<double> hcoef, hcoefc;

  int i;
  for (i=0;i<pobj.nqubit;i++) {
    hcoef = pcoef( t, pobj.amp[i], pobj.tseq[i], pobj.freq[i],
		   pobj.duration);

    hcoefc = std::conj(hcoef);

    if (i==0){
      hamdr = hcoef * hdrive[i][0];
    } else {
      hamdr += hcoef * hdrive[i][0];
    }

    hamdr += hcoefc * hdrive[i][1];
  }

  for (i=0;i<dsham_len; i++){
    matexp_.coeffRef(i,i) = std::exp(dsham[i] * t);
  }

  Eigen::SparseMatrix<std::complex<double> >
  hamr_ = (matexp_.conjugate().transpose() * hamdr);
  hamr_ = (hamr_ * matexp_).pruned();

  return hamr_;
}