// Copyright 2016-2019 Doug Moen
// Licensed under the Apache License, version 2.0
// See accompanying file LICENSE or https://www.apache.org/licenses/LICENSE-2.0

#include <libcurv/builtin.h>

#include <libcurv/analyser.h>
#include <libcurv/array_op.h>
#include <libcurv/die.h>
#include <libcurv/dir_record.h>
#include <libcurv/exception.h>
#include <libcurv/function.h>
#include <libcurv/sc_compiler.h>
#include <libcurv/sc_context.h>
#include <libcurv/import.h>
#include <libcurv/math.h>
#include <libcurv/pattern.h>
#include <libcurv/picker.h>
#include <libcurv/program.h>
#include <libcurv/source.h>
#include <libcurv/system.h>

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

#include <cassert>
#include <climits>
#include <cmath>
#include <cstdlib>
#include <string>

using namespace std;
using namespace boost::math::double_constants;

namespace curv {

Shared<Meaning>
Builtin_Value::to_meaning(const Identifier& id) const
{
    return make<Constant>(share(id), value_);
}

struct Is_Null_Function : public Legacy_Function
{
    static const char* name() { return "is_null"; }
    Is_Null_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].is_null()};
    }
};
struct Is_Bool_Function : public Legacy_Function
{
    static const char* name() { return "is_bool"; }
    Is_Bool_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {isbool(args[0])};
    }
};
struct Is_Num_Function : public Legacy_Function
{
    static const char* name() { return "is_num"; }
    Is_Num_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {isnum(args[0])};
    }
};
struct Is_String_Function : public Legacy_Function
{
    static const char* name() { return "is_string"; }
    Is_String_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<String>() != nullptr};
    }
};
struct Is_List_Function : public Legacy_Function
{
    static const char* name() { return "is_list"; }
    Is_List_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {islist(args[0])};
    }
};
struct Is_Record_Function : public Legacy_Function
{
    static const char* name() { return "is_record"; }
    Is_Record_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<Record>() != nullptr};
    }
};
struct Is_Fun_Function : public Legacy_Function
{
    static const char* name() { return "is_fun"; }
    Is_Fun_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<Function>() != nullptr};
    }
};

struct Bit_Function : public Legacy_Function
{
    static const char* name() { return "bit"; }
    Bit_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {double(args[0].to_bool(At_Arg(*this, args)))};
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto arg = f[0];
        if (arg.type != SC_Type::Bool())
            throw Exception(At_SC_Arg(0, f),
                stringify(name(),": argument is not a bool"));
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = float("<<arg<<");\n";
        return result;
    }
};

#define UNARY_NUMERIC_FUNCTION(Class_Name,curv_name,c_name,glsl_name) \
struct Class_Name : public Legacy_Function \
{ \
    static const char* name() { return #curv_name; } \
    Class_Name() : Legacy_Function(1,name()) {} \
    struct Scalar_Op { \
        static double call(double x) { return c_name(x); } \
        Shared<Operation> make_expr(Shared<Operation> x) const \
        { \
            return make<Call_Expr>( \
                cx.call_frame_.call_phrase_, \
                make<Constant>( \
                    func_part(cx.call_frame_.call_phrase_), \
                    Value{share(cx.fun_)}), \
                x); \
        } \
        static Shared<const String> callstr(Value x) { \
            return stringify(x); \
        } \
        At_Arg cx; \
        Scalar_Op(Function& fun, Frame& args) : cx(fun,args) {} \
    }; \
    static Unary_Numeric_Array_Op<Scalar_Op> array_op; \
    Value call(Frame& args) override \
    { \
        return array_op.op(Scalar_Op(*this, args), args[0]); \
    } \
    SC_Value sc_call(SC_Frame& f) const override \
    { \
        return sc_call_unary_numeric(f, #glsl_name); \
    } \
}; \

UNARY_NUMERIC_FUNCTION(Sqrt_Function, sqrt, sqrt, sqrt)
UNARY_NUMERIC_FUNCTION(Log_Function, log, log, log)
UNARY_NUMERIC_FUNCTION(Abs_Function, abs, abs, abs)
UNARY_NUMERIC_FUNCTION(Floor_Function, floor, floor, floor)
UNARY_NUMERIC_FUNCTION(Ceil_Function, ceil, ceil, ceil)
UNARY_NUMERIC_FUNCTION(Trunc_Function, trunc, trunc, trunc)
UNARY_NUMERIC_FUNCTION(Round_Function, round, rint, roundEven)

inline double frac(double n) { return n - floor(n); }
UNARY_NUMERIC_FUNCTION(Frac_Function, frac, frac, fract)

UNARY_NUMERIC_FUNCTION(Sin_Function, sin, sin, sin)
UNARY_NUMERIC_FUNCTION(Cos_Function, cos, cos, cos)
UNARY_NUMERIC_FUNCTION(Tan_Function, tan, tan, tan)
UNARY_NUMERIC_FUNCTION(Acos_Function, acos, acos, acos)
UNARY_NUMERIC_FUNCTION(Asin_Function, asin, asin, asin)
UNARY_NUMERIC_FUNCTION(Atan_Function, atan, atan, atan)

UNARY_NUMERIC_FUNCTION(Sinh_Function, sinh, sinh, sinh)
UNARY_NUMERIC_FUNCTION(Cosh_Function, cosh, cosh, cosh)
UNARY_NUMERIC_FUNCTION(Tanh_Function, tanh, tanh, tanh)
UNARY_NUMERIC_FUNCTION(Acosh_Function, acosh, acosh, acosh)
UNARY_NUMERIC_FUNCTION(Asinh_Function, asinh, asinh, asinh)
UNARY_NUMERIC_FUNCTION(Atanh_Function, atanh, atanh, atanh)

double fhash(double f)
{
    union {
        double f;
        uint64_t u;
    } data;
    data.f = f;
    data.u =
        ((data.u & 0x000F'FFFF'FFFF'FFFF) // strip sign bit and exponent
        | 0x3FF0'0000'0000'0000) // set sign to 0 and exponent to 0
        ^ (data.u >> 12) // xor exponent and sign on top of mantissa
        ;
    // If f is normalized, then 1 <= data.f < 2.
    // If f is denormalized, then 0 <= data.f < 1. This includes f==0.
    return data.f - floor(data.f);
}
UNARY_NUMERIC_FUNCTION(Fhash_Function, fhash, fhash, fhash)

struct Atan2_Function : public Legacy_Function
{
    static const char* name() { return "atan2"; }
    Atan2_Function() : Legacy_Function(2,name()) {}

    struct Scalar_Op {
        static double call(double x, double y) { return atan2(x, y); }
        Shared<Operation> make_expr(
            Shared<Operation> x, Shared<Operation> y) const
        {
            throw Exception(cx,
                "Internal error: atan2 applied to a reactive value");
            //return make<Divide_Expr>(share(syntax), std::move(x), std::move(y));
        }
        static const char* name() { return "atan2"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("[",x,",",y,"]");
        }
        At_Arg cx;
        Scalar_Op(Function& fun, Frame& args) : cx(fun, args) {}
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(Scalar_Op(*this, args), args[0], args[1]);
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto x = f[0];
        auto y = f[1];

        SC_Type rtype = SC_Type::Bool();
        if (x.type == y.type)
            rtype = x.type;
        else if (x.type == SC_Type::Num())
            rtype = y.type;
        else if (y.type == SC_Type::Num())
            rtype = x.type;
        if (rtype == SC_Type::Bool())
            throw Exception(At_SC_Phrase(f.call_phrase_, f),
                "domain error");

        SC_Value result = f.sc_.newvalue(rtype);
        f.sc_.out() <<"  "<<rtype<<" "<<result<<" = atan(";
        sc_put_as(f, x, At_SC_Arg(0, f), rtype);
        f.sc_.out() << ",";
        sc_put_as(f, y, At_SC_Arg(1, f), rtype);
        f.sc_.out() << ");\n";
        return result;
    }
};

SC_Value sc_minmax(const char* name, Operation& argx, SC_Frame& f)
{
    auto list = dynamic_cast<List_Expr*>(&argx);
    if (list) {
        std::list<SC_Value> args;
        SC_Type type = SC_Type::Num();
        for (auto op : *list) {
            auto val = sc_eval_op(f, *op);
            args.push_back(val);
            if (val.type == SC_Type::Num())
                ;
            else if (val.type.is_vec()) {
                if (type == SC_Type::Num())
                    type = val.type;
                else if (type != val.type)
                    throw Exception(At_SC_Phrase(op->syntax_, f), stringify(
                        name, ": vector arguments of different lengths"));
            } else {
                throw Exception(At_SC_Phrase(op->syntax_, f), stringify(
                    name,": argument has bad type"));
            }
        }
        auto result = f.sc_.newvalue(type);
        if (args.size() == 0)
            f.sc_.out() << "  " << type << " " << result << " = -0.0/0.0;\n";
        else if (args.size() == 1)
            return args.front();
        else {
            f.sc_.out() << "  " << type << " " << result << " = ";
            int rparens = 0;
            while (args.size() > 2) {
                f.sc_.out() << name << "(" << args.front() << ",";
                args.pop_front();
                ++rparens;
            }
            f.sc_.out() << name << "(" << args.front() << "," << args.back() << ")";
            while (rparens > 0) {
                f.sc_.out() << ")";
                --rparens;
            }
            f.sc_.out() << ";\n";
        }
        return result;
    } else {
        auto arg = sc_eval_op(f, argx);
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = ";
        if (arg.type == SC_Type::Vec(2))
            f.sc_.out() << name <<"("<<arg<<".x,"<<arg<<".y);\n";
        else if (arg.type == SC_Type::Vec(3))
            f.sc_.out() << name<<"("<<name<<"("<<arg<<".x,"<<arg<<".y),"
                <<arg<<".z);\n";
        else if (arg.type == SC_Type::Vec(4))
            f.sc_.out() << name<<"("<<name<<"("<<name<<"("<<arg<<".x,"<<arg<<".y),"
                <<arg<<".z),"<<arg<<".w);\n";
        else
            throw Exception(At_SC_Phrase(argx.syntax_, f), stringify(
                name,": argument is not a vector"));
        return result;
    }
}

struct Max_Function : public Legacy_Function
{
    static const char* name() { return "max"; }
    Max_Function() : Legacy_Function(1,name()) {}

    struct Scalar_Op {
        static double call(double x, double y) {
            // return NaN if either argument is NaN.
            if (x >= y) return x;
            if (x < y) return y;
            return 0.0/0.0;
        }
        Shared<Operation> make_expr(
            Shared<Operation> x, Shared<Operation> y) const
        {
            Shared<List_Expr> args =
                List_Expr::make({x, y}, arg_part(cx.call_frame_.call_phrase_));
            args->init();
            return make<Call_Expr>(
                cx.call_frame_.call_phrase_,
                make<Constant>(
                    func_part(cx.call_frame_.call_phrase_),
                    Value{share(cx.fun_)}),
                args);
        }
        static const char* name() { return "max"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("[",x,",",y,"]");
        }
        At_Arg cx;
        Scalar_Op(Function& fun, Frame& args) : cx(fun,args) {}
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.reduce(Scalar_Op(*this, args), -INFINITY, args[0]);
    }
    SC_Value sc_call_expr(Operation& argx, Shared<const Phrase>, SC_Frame& f)
    const override
    {
        return sc_minmax(name(),argx,f);
    }
};

struct Min_Function : public Legacy_Function
{
    static const char* name() { return "min"; }
    Min_Function() : Legacy_Function(1,name()) {}

    struct Scalar_Op {
        static double call(double x, double y) {
            // return NaN if either argument is NaN
            if (x <= y) return x;
            if (x > y) return y;
            return 0.0/0.0;
        }
        Shared<Operation> make_expr(
            Shared<Operation> x, Shared<Operation> y) const
        {
            Shared<List_Expr> args =
                List_Expr::make({x, y}, arg_part(cx.call_frame_.call_phrase_));
            args->init();
            return make<Call_Expr>(
                cx.call_frame_.call_phrase_,
                make<Constant>(
                    func_part(cx.call_frame_.call_phrase_),
                    Value{share(cx.fun_)}),
                args);
        }
        static const char* name() { return "min"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("[",x,",",y,"]");
        }
        At_Arg cx;
        Scalar_Op(Function& fun, Frame& args) : cx(fun, args) {}
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.reduce(Scalar_Op(*this, args), INFINITY, args[0]);
    }
    SC_Value sc_call_expr(Operation& argx, Shared<const Phrase>, SC_Frame& f)
    const override
    {
        return sc_minmax("min",argx,f);
    }
};

// Generalized dot product that includes vector dot product and matrix product.
// Same as Mathematica Dot[A,B]. Like APL A+.×B, Python numpy.dot(A,B)
struct Dot_Function : public Legacy_Function
{
    static const char* name() { return "dot"; }
    Dot_Function() : Legacy_Function(2,name()) {}
    Value call(Frame& args) override
    {
        return dot(args[0], args[1], At_Arg(*this, args));
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto a = f[0];
        auto b = f[1];
        if (!a.type.is_vec())
            throw Exception(At_SC_Arg(0, f), "dot: argument is not a vector");
        if (a.type != b.type)
            throw Exception(At_SC_Arg(1, f), "dot: arguments have different types");
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = dot("<<a<<","<<b<<");\n";
        return result;
    }
};

struct Mag_Function : public Legacy_Function
{
    static const char* name() { return "mag"; }
    Mag_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        // TODO: use hypot() or BLAS DNRM2 or Eigen stableNorm/blueNorm?
        // Avoids overflow/underflow due to squaring of large/small values.
        // Slower.  https://forum.kde.org/viewtopic.php?f=74&t=62402
        auto list = args[0].to<List>(At_Arg(*this, args));
        // Fast path: assume we have a list of number, compute a result.
        double sum = 0.0;
        for (auto val : *list) {
            double x = val.get_num_or_nan();
            sum += x * x;
        }
        if (sum == sum)
            return {sqrt(sum)};
        // The computation failed. Second fastest path: assume a mix of numbers
        // and reactive numbers, try to return a reactive result.
        Shared<List_Expr> rlist =
            List_Expr::make(list->size(),arg_part(args.call_phrase_));
        for (unsigned i = 0; i < list->size(); ++i) {
            Value val = list->at(i);
            if (val.is_num()) {
                rlist->at(i) = make<Constant>(arg_part(args.call_phrase_), val);
                continue;
            }
            auto r = val.dycast<Reactive_Value>();
            if (r && r->sctype_ == SC_Type::Num()) {
                rlist->at(i) = r->expr(*arg_part(args.call_phrase_));
                continue;
            }
            rlist = nullptr;
            break;
        }
        if (rlist) {
            rlist->init();
            return {make<Reactive_Expression>(
                SC_Type::Num(),
                make<Call_Expr>(
                    args.call_phrase_,
                    make<Constant>(
                        func_part(args.call_phrase_),
                        Value{share(*this)}),
                    rlist),
                At_Arg(*this, args))};
        }
        throw Exception(At_Arg(*this, args),
            stringify(args[0],": domain error"));
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto arg = f[0];
        if (!arg.type.is_vec())
            throw Exception(At_SC_Arg(0, f), "mag: argument is not a vector");
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = length("<<arg<<");\n";
        return result;
    }
};

struct Count_Function : public Legacy_Function
{
    static const char* name() { return "count"; }
    Count_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto list = args[0].dycast<const List>())
            return {double(list->size())};
        if (auto string = args[0].dycast<const String>())
            return {double(string->size())};
        if (auto re = args[0].dycast<const Reactive_Value>()) {
            if (re->sctype_.is_list())
                return {double(re->sctype_.count())};
            //TODO:
            //if (re->sctype_ == SC_Type::Any())
        }
        throw Exception(At_Arg(*this, args), "not a list or string");
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto arg = f[0];
        if (!arg.type.is_list())
            throw Exception(At_SC_Arg(0, f), "count: argument is not a list");
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = "<<arg.type.count()<<";\n";
        return result;
    }
};
struct Fields_Function : public Legacy_Function
{
    static const char* name() { return "fields"; }
    Fields_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto record = args[0].dycast<const Record>())
            return {record->fields()};
        throw Exception(At_Arg(*this, args), "not a record");
    }
};

struct Strcat_Function : public Legacy_Function
{
    static const char* name() { return "strcat"; }
    Strcat_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto list = args[0].dycast<const List>()) {
            String_Builder sb;
            for (auto val : *list) {
                if (auto str = val.dycast<const String>())
                    sb << str;
                else
                    sb << val;
            }
            return {sb.get_string()};
        }
        throw Exception(At_Arg(*this, args), "not a list");
    }
};
struct Repr_Function : public Legacy_Function
{
    static const char* name() { return "repr"; }
    Repr_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        String_Builder sb;
        sb << args[0];
        return {sb.get_string()};
    }
};
struct Decode_Function : public Legacy_Function
{
    static const char* name() { return "decode"; }
    Decode_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        String_Builder sb;
        At_Arg cx(*this, f);
        auto list = f[0].to<List>(cx);
        for (size_t i = 0; i < list->size(); ++i)
            sb << (char)(*list)[i].to_int(1, 127, At_Index(i,cx));
        return {sb.get_string()};
    }
};
struct Encode_Function : public Legacy_Function
{
    static const char* name() { return "encode"; }
    Encode_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        List_Builder lb;
        At_Arg cx(*this, f);
        auto str = f[0].to<String>(cx);
        for (size_t i = 0; i < str->size(); ++i)
            lb.push_back({(double)(int)str->at(i)});
        return {lb.get_list()};
    }
};

struct Match_Function : public Legacy_Function
{
    static const char* name() { return "match"; }
    Match_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        At_Arg ctx0(*this, f);
        auto list = f[0].to<List>(ctx0);
        std::vector<Shared<Function>> cases;
        for (size_t i = 0; i < list->size(); ++i)
            cases.push_back(list->at(i).to<Function>(At_Index(i,ctx0)));
        auto mf = make<Piecewise_Function>(cases);
        mf->name_ = name_;
        mf->argpos_ = 1;
        return {mf};
    }
};

// The filename argument to "file", if it is a relative filename,
// is interpreted relative to the parent directory of the source file from
// which "file" is called.
//
// Because "file" has this hidden parameter (the name of the source file from
// which it is called), it is not a pure function. For this reason, it isn't
// a function value at all, it's a metafunction.
struct File_Expr : public Just_Expression
{
    Shared<Operation> arg_;
    File_Expr(Shared<const Call_Phrase> src, Shared<Operation> arg)
    :
        Just_Expression(std::move(src)),
        arg_(std::move(arg))
    {}
    virtual Value eval(Frame& f) const override
    {
        // Metafunction calls do not automatically get a new stack frame
        // allocated. But I want the call to `file pathname` to appear
        // in stack traces, so I need a Frame.
        auto& callphrase = dynamic_cast<const Call_Phrase&>(*syntax_);
        std::unique_ptr<Frame> f2 =
            Frame::make(0, f.system_, &f, &callphrase, nullptr);
        At_Metacall_With_Call_Frame cx("file", 0, *f2);

        // construct file pathname from argument
        Value arg = arg_->eval(f);
        auto argstr = arg.to<String>(cx);
        namespace fs = boost::filesystem;
        fs::path filepath;
        auto caller_filename = syntax_->location().source().name_;
        if (caller_filename->empty()) {
            filepath = fs::path(argstr->c_str());
        } else {
            filepath = fs::path(caller_filename->c_str()).parent_path()
                / fs::path(argstr->c_str());
        }

        return import(filepath, cx);
    }
};
struct File_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<File_Expr>(share(ph), analyse_op(*ph.arg_, env));
    }
};

/// The meaning of a call to `print`, such as `print "foo"`.
struct Print_Action : public Operation
{
    Shared<Operation> arg_;
    Print_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f, Executor&) const override
    {
        Value arg = arg_->eval(f);
        auto str = arg.dycast<String>();
        if (str == nullptr)
            str = stringify(arg);
        f.system_.print(str->c_str());
    }
};
/// The meaning of the phrase `print` in isolation.
struct Print_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Print_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

struct Warning_Action : public Operation
{
    Shared<Operation> arg_;
    Warning_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f, Executor&) const override
    {
        Value arg = arg_->eval(f);
        Shared<String> msg;
        if (auto str = arg.dycast<String>())
            msg = str;
        else
            msg = stringify(arg);
        Exception exc{At_Phrase(*syntax_, f), msg};
        f.system_.warning(exc);
    }
};
/// The meaning of the phrase `warning` in isolation.
struct Warning_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Warning_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

/// The meaning of a call to `error`, such as `error("foo")`.
struct Error_Operation : public Operation
{
    Shared<Operation> arg_;
    Error_Operation(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    [[noreturn]] void run(Frame& f) const
    {
        Value val = arg_->eval(f);
        Shared<const String> msg;
        if (auto s = val.dycast<String>())
            msg = s;
        else
            msg = stringify(val);
        throw Exception{At_Phrase(*syntax_, f), msg};
    }
    virtual void exec(Frame& f, Executor&) const override
    {
        run(f);
    }
    virtual Value eval(Frame& f) const override
    {
        run(f);
    }
};
/// The meaning of the phrase `error` in isolation.
struct Error_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Error_Operation>(share(ph), analyse_op(*ph.arg_, env));
    }
};

// exec(expr) -- a debug action that evaluates expr, then discards the result.
// It is used to call functions or source files for their side effects.
struct Exec_Action : public Operation
{
    Shared<Operation> arg_;
    Exec_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f, Executor&) const override
    {
        arg_->eval(f);
    }
};
struct Exec_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Exec_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

struct Assert_Action : public Operation
{
    Shared<Operation> arg_;
    Assert_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f, Executor&) const override
    {
        At_Metacall cx{"assert", 0, *arg_->syntax_, f};
        bool b = arg_->eval(f).to_bool(cx);
        if (!b)
            throw Exception(At_Phrase(*syntax_, f), "assertion failed");
    }
};
struct Assert_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto arg = analyse_op(*ph.arg_, env);
        return make<Assert_Action>(share(ph), arg);
    }
};

struct Assert_Error_Action : public Operation
{
    Shared<Operation> expected_message_;
    Shared<const String> actual_message_;
    Shared<Operation> expr_;

    Assert_Error_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> expected_message,
        Shared<const String> actual_message,
        Shared<Operation> expr)
    :
        Operation(std::move(syntax)),
        expected_message_(std::move(expected_message)),
        actual_message_(std::move(actual_message)),
        expr_(std::move(expr))
    {}

    virtual void exec(Frame& f, Executor&) const override
    {
        Value expected_msg_val = expected_message_->eval(f);
        auto expected_msg_str = expected_msg_val.to<const String>(
            At_Phrase(*expected_message_->syntax_, f));

        if (actual_message_ != nullptr) {
            if (*actual_message_ != *expected_msg_str)
                throw Exception(At_Phrase(*syntax_, f),
                    stringify("assertion failed: expected error \"",
                        expected_msg_str,
                        "\", actual error \"",
                        actual_message_,
                        "\""));
            return;
        }

        Value result;
        try {
            result = expr_->eval(f);
        } catch (Exception& e) {
            if (*e.shared_what() != *expected_msg_str) {
                throw Exception(At_Phrase(*syntax_, f),
                    stringify("assertion failed: expected error \"",
                        expected_msg_str,
                        "\", actual error \"",
                        e.shared_what(),
                        "\""));
            }
            return;
        }
        throw Exception(At_Phrase(*syntax_, f),
            stringify("assertion failed: expected error \"",
                expected_msg_str,
                "\", got value ", result));
    }
};
struct Assert_Error_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto parens = cast<Paren_Phrase>(ph.arg_);
        Shared<Comma_Phrase> commas = nullptr;
        if (parens) commas = cast<Comma_Phrase>(parens->body_);
        if (parens && commas && commas->args_.size() == 2) {
            auto msg = analyse_op(*commas->args_[0].expr_, env);
            Shared<Operation> expr = nullptr;
            Shared<const String> actual_msg = nullptr;
            try {
                expr = analyse_op(*commas->args_[1].expr_, env);
            } catch (Exception& e) {
                actual_msg = e.shared_what();
            }
            return make<Assert_Error_Action>(share(ph), msg, actual_msg, expr);
        } else {
            throw Exception(At_Phrase(ph, env),
                "assert_error: expecting 2 arguments");
        }
    }
};

struct Defined_Expression : public Just_Expression
{
    Shared<const Operation> expr_;
    Symbol_Expr selector_;

    Defined_Expression(
        Shared<const Phrase> syntax,
        Shared<const Operation> expr,
        Symbol_Expr selector)
    :
        Just_Expression(std::move(syntax)),
        expr_(std::move(expr)),
        selector_(std::move(selector))
    {
    }

    virtual Value eval(Frame& f) const override
    {
        auto val = expr_->eval(f);
        auto s = val.dycast<Record>();
        if (s) {
            auto id = selector_.eval(f);
            return {s->hasfield(id)};
        } else {
            return {false};
        }
    }
};
struct Defined_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto arg = analyse_op(*ph.arg_, env);
        auto dot = cast<Dot_Expr>(arg);
        if (dot != nullptr)
            return make<Defined_Expression>(
                share(ph), dot->base_, dot->selector_);
        throw Exception(At_Phrase(*ph.arg_, env),
            "defined: argument must be `expression.identifier`");
    }
};

const Namespace&
builtin_namespace()
{
    #define FUNCTION(f) {f::name(), make<Builtin_Value>(Value{make<f>()})}

    static const Namespace names = {
    {"pi", make<Builtin_Value>(pi)},
    {"tau", make<Builtin_Value>(two_pi)},
    {"inf", make<Builtin_Value>(INFINITY)},
    {"null", make<Builtin_Value>(Value())},
    {"false", make<Builtin_Value>(Value(false))},
    {"true", make<Builtin_Value>(Value(true))},

    FUNCTION(Is_Null_Function),
    FUNCTION(Is_Bool_Function),
    FUNCTION(Is_Num_Function),
    FUNCTION(Is_String_Function),
    FUNCTION(Is_List_Function),
    FUNCTION(Is_Record_Function),
    FUNCTION(Is_Fun_Function),
    FUNCTION(Bit_Function),
    FUNCTION(Sqrt_Function),
    FUNCTION(Log_Function),
    FUNCTION(Abs_Function),
    FUNCTION(Floor_Function),
    FUNCTION(Ceil_Function),
    FUNCTION(Trunc_Function),
    FUNCTION(Round_Function),
    FUNCTION(Frac_Function),
    FUNCTION(Fhash_Function),
    FUNCTION(Sin_Function),
    FUNCTION(Cos_Function),
    FUNCTION(Tan_Function),
    FUNCTION(Asin_Function),
    FUNCTION(Acos_Function),
    FUNCTION(Atan_Function),
    FUNCTION(Atan2_Function),
    FUNCTION(Sinh_Function),
    FUNCTION(Cosh_Function),
    FUNCTION(Tanh_Function),
    FUNCTION(Asinh_Function),
    FUNCTION(Acosh_Function),
    FUNCTION(Atanh_Function),
    FUNCTION(Max_Function),
    FUNCTION(Min_Function),
    FUNCTION(Dot_Function),
    FUNCTION(Mag_Function),
    FUNCTION(Count_Function),
    FUNCTION(Fields_Function),
    FUNCTION(Strcat_Function),
    FUNCTION(Repr_Function),
    FUNCTION(Decode_Function),
    FUNCTION(Encode_Function),
    FUNCTION(Match_Function),

    {"file", make<Builtin_Meaning<File_Metafunction>>()},
    {"print", make<Builtin_Meaning<Print_Metafunction>>()},
    {"warning", make<Builtin_Meaning<Warning_Metafunction>>()},
    {"error", make<Builtin_Meaning<Error_Metafunction>>()},
    {"assert", make<Builtin_Meaning<Assert_Metafunction>>()},
    {"assert_error", make<Builtin_Meaning<Assert_Error_Metafunction>>()},
    {"exec", make<Builtin_Meaning<Exec_Metafunction>>()},
    {"defined", make<Builtin_Meaning<Defined_Metafunction>>()},
    };
    return names;
}

} // namespace curv


/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * 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 project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "LowRankGPModelBuilder.h"
#include "StatisticalModel.h"
#include "vtkPolyDataReader.h"
#include "vtkStandardMeshRepresenter.h"

#include "Kernels.h"
#include "KernelCombinators.h"

#include <iostream>
#include <boost/scoped_ptr.hpp>

using namespace statismo;

/**
 * A scalar valued gaussian kernel.
 */
class GaussianKernel: public ScalarValuedKernel<vtkPoint> {
  public:

    GaussianKernel(double sigma) : m_sigma(sigma), m_sigma2(sigma * sigma) {
    }

    inline double operator()(const vtkPoint& x, const vtkPoint& y) const {
        VectorType r(3);
        r << x[0] - y[0], x[1] - y[1], x[2] - y[2];
        return exp(-r.dot(r) / m_sigma2);
    }

    std::string GetKernelInfo() const {
        std::ostringstream os;
        os << "GaussianKernel(" << m_sigma << ")";
        return os.str();
    }

  private:

    double m_sigma;
    double m_sigma2;
};

vtkPolyData* loadVTKPolyData(const std::string& filename) {
    vtkPolyDataReader* reader = vtkPolyDataReader::New();
    reader->SetFileName(filename.c_str());
    reader->Update();
    vtkPolyData* pd = vtkPolyData::New();
    pd->ShallowCopy(reader->GetOutput());
    return pd;
}

//
// This example illustrates, how the flexibility of a statistical (shape) model can be extended by combining its covariance function
// with a Gaussian Kernel function.
//
int main(int argc, char** argv) {

    if (argc < 5) {
        std::cout << "Usage " << argv[0] << " model gaussianKernelWidth numberOfComponents, outputmodelName" << std::endl;
        exit(-1);
    }
    std::string modelFilename(argv[1]);
    double gaussianKernelSigma = std::atof(argv[2]);
    int numberOfComponents = std::atoi(argv[3]);
    std::string outputModelFilename(argv[4]);

    // All the statismo classes have to be parameterized with the RepresenterType.

    typedef vtkStandardMeshRepresenter RepresenterType;
    typedef LowRankGPModelBuilder<vtkPolyData> ModelBuilderType;
    typedef StatisticalModel<vtkPolyData> StatisticalModelType;
    typedef GaussianKernel GaussianKernelType;
    typedef MatrixValuedKernel<vtkPoint> MatrixValuedKernelType;

    try {

        // we load an existing statistical model and create a StatisticalModelKernel from it. The statisticlModelKernel
        // takes the covariance (matrix) of the model and defines a kernel function from it.
        vtkStandardMeshRepresenter* representer = vtkStandardMeshRepresenter::Create();
        boost::scoped_ptr<StatisticalModelType> model(StatisticalModelType::Load(representer, modelFilename));
        const MatrixValuedKernelType& statModelKernel = StatisticalModelKernel<vtkPolyData>(model.get());

        // Create a (scalar valued) gaussian kernel. This kernel is then made matrix-valued. We use a UncorrelatedMatrixValuedKernel,
        // which assumes that each output component is independent.

        const GaussianKernel gk = GaussianKernel(gaussianKernelSigma);
        const MatrixValuedKernelType& mvGk = UncorrelatedMatrixValuedKernel<vtkPoint>(&gk, model->GetRepresenter()->GetDimensions());

        // We scale the kernel (and hence the resulting deformations) of the Gaussian kernel by  a factor of 100, in order
        // to achieve a visible effect.
        const MatrixValuedKernelType& scaledGk = ScaledKernel<vtkPoint>(&mvGk, 100.0);

        // The model kernel and the Gaussian kernel are combined to a new kernel.
        const MatrixValuedKernelType& combinedModelAndGaussKernel = SumKernel<vtkPoint>(&statModelKernel, &scaledGk);

        // We create a new model using the combined kernel. The new model will be more flexible than the original statistical model.
        boost::scoped_ptr<ModelBuilderType> modelBuilder(ModelBuilderType::Create(model->GetRepresenter()));
        boost::scoped_ptr<StatisticalModelType> combinedModel(modelBuilder->BuildNewModel(model->DrawMean(), combinedModelAndGaussKernel, numberOfComponents));

        // Once we have built the model, we can save it to disk.
        combinedModel->Save(outputModelFilename);
        std::cout << "Successfully saved shape model as " << outputModelFilename << std::endl;

    } catch (StatisticalModelException& e) {
        std::cout << "Exception occured while building the shape model" << std::endl;
        std::cout << e.what() << std::endl;
    }
}


#ifndef HEADER_SHAPEMODEL
#define HEADER_SHAPEMODEL

#include <string>
#include <iostream>
#include <armadillo>
#include <set>
#include <map>
#include <limits>

#include <ShapeUQLib/FrameGraph.hpp>

class Element;

/**
Declaration of the ShapeModel class. Base class for
the implementation of shape model
*/
template <class PointType>
class ShapeModel {

public:

	/**
	Constructor
	*/
	ShapeModel();

	/**
	Constructor
	@param frame_graph Pointer to the graph storing
	reference frame relationships
	@param frame_graph Pointer to the reference frame graph
	*/
	ShapeModel(std::string ref_frame_name,
		FrameGraph * frame_graph);

	/**
	Returns the dimensions of the bounding box
	@param Bounding box dimension to be computed (xmin,ymin,zmin,xmax,ymax,zmax)
	*/
	void get_bounding_box(double * bounding_box,arma::mat M = arma::eye<arma::mat>(3,3)) const;

	/**
	Translates the shape model by x
	@param x translation vector applied to the coordinates of each control point
	*/
	void translate(const arma::vec::fixed<3> &);

	/**
	Rotates the shape model by
	@param M rotation matrix
	*/
	void rotate(const arma::mat::fixed<3,3> & M);

	/**
	Shifts the coordinates of the shape model
	so as to have (0,0,0) aligned with its barycenter
	The resulting barycenter coordinates are (0,0,0)
	*/
	void shift_to_barycenter();

	/**
	Applies a rotation that aligns the body
	with its principal axes.
	This assumes that the body has been shifted so
	that (0,0,0) lies at its barycenter
	The resulting inertia tensor is diagonal
	Undefined behavior if
	the inertia tensor has not been computed beforehand
	*/
	void align_with_principal_axes();

	/**
	Returns the volume of the provided shape model
	@return volume (U^2 where U is the unit of the shape coordinates)
	*/
	double get_volume() const;

	/**
	Returns the principal axes and principal moments of the shape model
	@param axes M as in X = MX' where X' is a position expressed in the principal frame
	@param moments dimensionless inertia moments in ascending order
	*/
	void get_principal_inertias(arma::mat & axes,arma::vec & moments) const;

	/**
	Defines the reference frame attached to the shape model
	@param ref_frame Pointer to the reference frame attached
	to the shape model
	*/
	void set_ref_frame_name(std::string ref_frame_name);

	/**
	Returns the name of the reference frame attached to this
	ref frame
	@return name of reference frame
	*/
	std::string get_ref_frame_name() const;

	PointType & get_point(unsigned int i) ;
	const arma::vec::fixed<3> & get_point_coordinates(unsigned int i) const;

	virtual arma::vec::fixed<3> get_point_normal_coordinates(unsigned int i) const = 0;

	/**
	Pointer to the shape model's control points
	@return vertices pointer to the control points
	*/
	const std::vector<PointType> & get_points() const;

	unsigned int get_point_index(std::shared_ptr<PointType> point) const;

	/**
	Returns the geometrical center of the shape
	@return geometrical center
	*/
	arma::vec::fixed<3> get_center() const;

	/**
	Augment the internal container storing vertices with a new (and not already inserted)
	one
	@param control_point pointer to the new control point to be inserted
	*/
	void add_control_point(PointType & control_point);

	virtual void clear() = 0;

	/**
	Returns number of elements
	@return number of elements
	*/
	virtual unsigned int get_NElements() const  = 0;

	/**
	Returns number of control points
	@return number of control points
	*/
	unsigned int get_NControlPoints() const ;

	/**
	Computes the surface area of the shape model
	*/
	virtual void compute_surface_area() = 0;
	/**
	Computes the volume of the shape model
	*/
	virtual void compute_volume() = 0;

	/**
	Computes the center of mass of the shape model
	*/
	virtual void compute_center_of_mass() = 0;
	/**
	Computes the inertia tensor of the shape model
	*/
	virtual void compute_inertia() = 0;

	/**
	Constructs a connectivity table associated a control point pointer to its index
	in this shape model control points vector
	*/
	void initialize_index_table();

	/**
	Returns the non-dimensional inertia tensor of the body in the body-fixed
	principal axes. (rho == 1, l = (volume)^(1/3))
	@return principal inertia tensor
	*/
	const arma::mat::fixed<3,3> & get_inertia() const;

	/**
	Updates shape geometric and mass properties
	*/
	virtual void update_mass_properties() = 0;

	/**
	Returns the surface area of the shape model
	@return surface area (U^2 where U is the unit of the shape coordinates)
	*/
	double get_surface_area() const;

	/**
	Returns the location of the center of mass
	@return pointer to center of mass
	*/
	const arma::vec::fixed<3> & get_center_of_mass() const;

	/**
	Builds the covariance of the provided control points
	@param P covariance to set
	@param Ci pointer to first point
	@param Cj pointer to second point
	@param Ck pointer to third point
	@param Cl pointer to fourth point
	@param Cm pointer to fifth point
	@param Cp pointer to sixth point
	*/
	static void assemble_covariance(arma::mat & P,
		const PointType & Ci,
		const PointType & Cj,
		const PointType & Ck,
		const PointType & Cl,
		const PointType & Cm,
		const PointType & Cp);

	/**
	Builds the covariance of the provided control points
	@param P covariance to set
	@param Ci pointer to first point
	@param Cj pointer to second point
	@param Ck pointer to third point
	@param Cl pointer to fourth point
	@param Cm pointer to fifth point
	@param Cp pointer to sixth point
	@param Cq pointer to seventh point
	*/
	static void assemble_covariance(arma::mat & P,
		const PointType & Ci,
		const PointType & Cj,
		const PointType & Ck,
		const PointType & Cl,
		const PointType & Cm,
		const PointType & Cp,
		const PointType & Cq);

	/**
	Builds the covariance of the provided control points
	@param P covariance to set
	@param Ci pointer to first point
	@param Cj pointer to second point
	@param Ck pointer to third point
	@param Cl pointer to fourth point
	@param Cm pointer to fifth point
	@param Cp pointer to sixth point
	@param Cq pointer to seventh point
	@param Cq pointer to eigth point
	*/
	static void assemble_covariance(arma::mat & P,
		const PointType & Ci,
		const PointType & Cj,
		const PointType & Ck,
		const PointType & Cl,
		const PointType & Cm,
		const PointType & Cp,
		const PointType & Cq,
		const PointType & Cr);

	/**
	Returns radius of circumscribing sphere, measured from the shape's center of mass
	@return radius of circumscribing sphere
	*/
	double get_circumscribing_radius() const;

	arma::vec get_inertia_param() const;

	virtual const std::vector<int> & get_element_control_points(int e) const = 0;

protected:

	std::vector<std::set<int> > edges;
	std::vector<PointType> control_points;

	std::map<std::shared_ptr<PointType> ,unsigned int> pointer_to_global_index;

	FrameGraph * frame_graph;
	std::string ref_frame_name;

	arma::mat::fixed<3,3> inertia;
	arma::vec::fixed<3> cm;
	double volume;
	double surface_area;

	/**
	Radius of sphere of equivalent volume
	*/
	double r_avg;

};

#endif


/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  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.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#ifndef GRAPHLAB_RANDOM_HPP
#define GRAPHLAB_RANDOM_HPP

#include <cstdlib>
#include <stdint.h>

#include <vector>
#include <limits>
#include <algorithm>

#include <boost/random.hpp>
#include <graphlab/util/timer.hpp>
#include <graphlab/parallel/pthread_tools.hpp>

namespace graphlab {

  /**
   * \ingroup random
   * A collection of thread safe random number routines.  Each thread
   * is assigned its own generator however assigning a seed affects
   * all current and future generators.
   */
  namespace random {

    ///////////////////////////////////////////////////////////////////////
    //// Underlying generator definition

    namespace distributions {
      /**
       * The uniform distribution struct is used for partial function
       * specialization. Generating uniform random real numbers is
       * accomplished slightly differently than for integers.
       * Therefore the base case is for integers and we then
       * specialize the two real number types (floats and doubles).
       */
      template<typename IntType>
      struct uniform {
        typedef boost::uniform_int<IntType> distribution_type;
        template<typename RealRNG, typename DiscreteRNG>
        static inline IntType sample(RealRNG& real_rng,
                                     DiscreteRNG& discrete_rng,
                                     const IntType& min, const IntType& max) {
          return distribution_type(min, max)(discrete_rng);
        }
      };
      template<>
      struct uniform<double> {
        typedef boost::uniform_real<double> distribution_type;
        template<typename RealRNG, typename DiscreteRNG>
        static inline double sample(RealRNG& real_rng,
                                    DiscreteRNG& discrete_rng,
                                    const double& min, const double& max) {
          return distribution_type(min, max)(real_rng);
        }
      };
      template<>
      struct uniform<float> {
        typedef boost::uniform_real<float> distribution_type;
        template<typename RealRNG, typename DiscreteRNG>
        static inline float sample(RealRNG& real_rng,
                                  DiscreteRNG& discrete_rng,
                                  const float& min, const float& max) {
          return distribution_type(min, max)(real_rng);
        }
      };
    }; // end of namespace distributions

    /**
     * The generator class is the base underlying type used to
     * generate random numbers.  User threads should use the functions
     * provided in the random namespace.
     */
    class generator {
    public:
      // base Generator types
      typedef boost::lagged_fibonacci607 real_rng_type;
      typedef boost::mt11213b            discrete_rng_type;
      typedef boost::rand48              fast_discrete_rng_type;

      generator() {
        time_seed();
      }

      //! Seed the generator using the default seed
      inline void seed() {
        mut.lock();
        real_rng.seed();
        discrete_rng.seed();
        fast_discrete_rng.seed();
        mut.unlock();
      }

      //! Seed the generator nondeterministically
      void nondet_seed();

      //! Seed the generator using the current time in microseconds
      inline void time_seed() {
        seed( graphlab::timer::usec_of_day() );
      }

      //! Seed the random number generator based on a number
      void seed(size_t number) {
        mut.lock();
        fast_discrete_rng.seed(number);
        real_rng.seed(fast_discrete_rng);
        discrete_rng.seed(fast_discrete_rng);
        mut.unlock();
      }

      //! Seed the generator using another generator
      void seed(generator& other){
        mut.lock();
        real_rng.seed(other.real_rng);
        discrete_rng.seed(other.discrete_rng);
        fast_discrete_rng.seed(other.fast_discrete_rng());
        mut.unlock();
      }

      /**
       * Generate a random number in the uniform real with range [min,
       * max) or [min, max] if the number type is discrete.
       */
      template<typename NumType>
      inline NumType uniform(const NumType min, const NumType max) {
        mut.lock();
        const NumType result = distributions::uniform<NumType>::
          sample(real_rng, discrete_rng, min, max);
        mut.unlock();
        return result;
      } // end of uniform

      /**
       * Generate a random number in the uniform real with range [min,
       * max) or [min, max] if the number type is discrete.
       */
      template<typename NumType>
      inline NumType fast_uniform(const NumType min, const NumType max) {
        mut.lock();
        const NumType result = distributions::uniform<NumType>::
          sample(real_rng, fast_discrete_rng, min, max);
        mut.unlock();
        return result;
      } // end of fast_uniform

      /**
       * Generate a random number in the uniform real with range [min,
       * max);
       */
      inline double gamma(const double alpha = double(1)) {
        boost::gamma_distribution<double> gamma_dist(alpha);
        mut.lock();
        const double result = gamma_dist(real_rng);
        mut.unlock();
        return result;
      } // end of gamma

      /**
       * Generate a gaussian random variable with zero mean and unit
       * variance.
       */
      inline double gaussian(const double mean = double(0),
                             const double stdev = double(1)) {
        boost::normal_distribution<double> normal_dist(mean,stdev);
        mut.lock();
        const double result = normal_dist(real_rng);
        mut.unlock();
        return result;
      } // end of gaussian

      /**
       * Generate a gaussian random variable with zero mean and unit
       * variance.
       */
      inline double normal(const double mean = double(0),
                           const double stdev = double(1)) {
        return gaussian(mean, stdev);
      } // end of normal

      inline bool bernoulli(const double p = double(0.5)) {
        boost::bernoulli_distribution<double> dist(p);
        mut.lock();
        const double result(dist(discrete_rng));
        mut.unlock();
        return result;
      } // end of bernoulli

      inline bool fast_bernoulli(const double p = double(0.5)) {
        boost::bernoulli_distribution<double> dist(p);
        mut.lock();
        const double result(dist(fast_discrete_rng));
        mut.unlock();
        return result;
      } // end of bernoulli

      /**
       * Draw a random number from a multinomial
       */
      template<typename Double>
      size_t multinomial(const std::vector<Double>& prb) {
        ASSERT_GT(prb.size(),0);
        if (prb.size() == 1) { return 0; }
        Double sum(0);
        for(size_t i = 0; i < prb.size(); ++i) {
          ASSERT_GE(prb[i], 0); // Each entry must be P[i] >= 0
          sum += prb[i];
        }
        ASSERT_GT(sum, 0); // Normalizer must be positive
        // actually draw the random number
        const Double rnd(uniform<Double>(0,1));
        size_t ind = 0;
        for(Double cumsum(prb[ind]/sum);
            rnd > cumsum && (ind+1) < prb.size();
            cumsum += (prb[++ind]/sum));
        return ind;
      } // end of multinomial

      /**
       * Generate a draw from a multinomial using a CDF.  This is
       * slightly more efficient since normalization is not required
       * and a binary search can be used.
       */
      template<typename Double>
      inline size_t multinomial_cdf(const std::vector<Double>& cdf) {
        return std::upper_bound(cdf.begin(), cdf.end(),
                                uniform<Double>(0,1)) - cdf.begin();

      } // end of multinomial_cdf

      /**
       * Construct a random permutation
       */
      template<typename T>
      inline std::vector<T> permutation(const size_t nelems) {
        std::vector<T> perm(nelems);
        for(T i = 0; i < nelems; ++i) perm[i] = i;
        shuffle(perm);
        return perm;
      } // end of construct a permutation

      /**
       * Shuffle a standard vector
       */
      template<typename T>
      void shuffle(std::vector<T>& vec) { shuffle(vec.begin(), vec.end()); }

      /**
       * Shuffle a range using the begin and end iterators
       */
      template<typename Iterator>
      void shuffle(Iterator begin, Iterator end) {
        mut.lock();
        shuffle_functor functor(*this);
        std::random_shuffle(begin, end, functor);
        mut.unlock();
      } // end of shuffle

    private:
      //////////////////////////////////////////////////////
      /// Data members
      struct shuffle_functor {
        generator& gen;
        inline shuffle_functor(generator& gen) : gen(gen) { }
        inline std::ptrdiff_t operator()(std::ptrdiff_t end) {
          return distributions::uniform<ptrdiff_t>::
            sample(gen.real_rng, gen.fast_discrete_rng, 0, end-1);
        }
      };

      //! The real random number generator
      real_rng_type real_rng;
      //! The discrete random number generator
      discrete_rng_type discrete_rng;
      //! The fast discrete random number generator
      fast_discrete_rng_type fast_discrete_rng;
      //! lock used to access local members
      mutex mut;
    }; // end of class generator

    /**
     * \ingroup random
     * Seed all generators using the default seed
     */
    void seed();

    /**
     * \ingroup random
     * Seed all generators using an integer
     */
    void seed(size_t seed_value);

    /**
     * \ingroup random
     * Seed all generators using a nondeterministic source
     */
    void nondet_seed();

    /**
     * \ingroup random
     * Seed all generators using the current time in microseconds
     */
    void time_seed();

    /**
     * \ingroup random
     * Get the local generator
     */
    generator& get_source();

    /**
     * \ingroup random
     * Generate a random number in the uniform real with range [min,
     * max) or [min, max] if the number type is discrete.
     */
    template<typename NumType>
    inline NumType uniform(const NumType min, const NumType max) {
      if (min == max) return min;
      return get_source().uniform<NumType>(min, max);
    } // end of uniform

    /**
     * \ingroup random
     * Generate a random number in the uniform real with range [min,
     * max) or [min, max] if the number type is discrete.
     */
    template<typename NumType>
    inline NumType fast_uniform(const NumType min, const NumType max) {
      if (min == max) return min;
      return get_source().fast_uniform<NumType>(min, max);
    } // end of fast_uniform

    /**
     * \ingroup random
     * Generate a random number between 0 and 1
     */
    inline double rand01() { return uniform<double>(0, 1); }

    /**
     * \ingroup random
     * Simulates the standard rand function as defined in cstdlib
     */
    inline int rand() { return fast_uniform(0, RAND_MAX); }

    /**
     * \ingroup random
     * Generate a random number from a gamma distribution.
     */
    inline double gamma(const double alpha = double(1)) {
      return get_source().gamma(alpha);
    }

    /**
     * \ingroup random
     * Generate a gaussian random variable with zero mean and unit
     * standard deviation.
     */
    inline double gaussian(const double mean = double(0),
                           const double stdev = double(1)) {
      return get_source().gaussian(mean, stdev);
    }

    /**
     * \ingroup random
     * Generate a gaussian random variable with zero mean and unit
     * standard deviation.
     */
    inline double normal(const double mean = double(0),
                         const double stdev = double(1)) {
      return get_source().normal(mean, stdev);
    }

    /**
     * \ingroup random
     * Draw a sample from a bernoulli distribution
     */
    inline bool bernoulli(const double p = double(0.5)) {
      return get_source().bernoulli(p);
    }

    /**
     * \ingroup random
     * Draw a sample form a bernoulli distribution using the faster generator
     */
    inline bool fast_bernoulli(const double p = double(0.5)) {
      return get_source().fast_bernoulli(p);
    }

    /**
     * \ingroup random
     * Generate a draw from a multinomial.  This function
     * automatically normalizes as well.
     */
    template<typename Double>
    inline size_t multinomial(const std::vector<Double>& prb) {
      return get_source().multinomial(prb);
    }

    /**
     * \ingroup random
     * Generate a draw from a cdf;
     */
    template<typename Double>
    inline size_t multinomial_cdf(const std::vector<Double>& cdf) {
      return get_source().multinomial_cdf(cdf);
    }

    /**
     * \ingroup random
     * Construct a random permutation
     */
    template<typename T>
    inline std::vector<T> permutation(const size_t nelems) {
      return get_source().permutation<T>(nelems);
    }

    /**
     * \ingroup random
     * Shuffle a standard vector
     */
    template<typename T>
    inline void shuffle(std::vector<T>& vec) {
      get_source().shuffle(vec);
    }

    /**
     * \ingroup random
     * Shuffle a range using the begin and end iterators
     */
    template<typename Iterator>
    inline void shuffle(Iterator begin, Iterator end) {
      get_source().shuffle(begin, end);
    }

    /**
     * Converts a discrete PDF into a CDF
     */
    void pdf2cdf(std::vector<double>& pdf);

  }; // end of random
}; // end of graphlab

#endif


/*

createDiffusionTensorImage.cxx
Bishesh Khanal
Asclepios, INRIA Sophia Antipolis

A test program to create diffusion tensor images of desired size.

*/

#include <iostream>
#include <string>
#include <sstream>
#include <boost/program_options.hpp>

#include <itkImage.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkDiffusionTensor3D.h>

#undef __FUNCT__
#define __FUNCT__ "main"
int main(int argc, char **argv)
{
    std::string inRefFileName, outFileName;         //input reference file for origin and spacings; name of the output file to be created.
    std::string elements;       //elements of the tensor separted by comma, without spaces in lower triangular order.
    double D[6];            //six diffusion tensor elements, in LT order.

    //-------------- Set up the command line options-------------------------
    boost::program_options::options_description optionsDescription("Possible options");
    optionsDescription.add_options()
            ("help,h", "displays help message")
            ("reference,r", boost::program_options::value< std::string >(&inRefFileName), "input reference file")
            ("output,o", boost::program_options::value< std::string >(&outFileName), "output filename")
            ("elements,e",boost::program_options::value< std::string >(&elements), "elements separted by comma without any spaces in lower traingular order")
            ;

    boost::program_options::variables_map options;
    boost::program_options::store(boost::program_options::parse_command_line(argc,argv,optionsDescription),options);
    boost::program_options::notify(options);

    //If help is asked!
    if(options.count("help")) {
        std::cout<<optionsDescription<<std::endl;
        return EXIT_SUCCESS;
    }

    //Confirm all the options required are given.
    if(!options.count("reference") || !options.count("output") || !options.count("elements")) {
        std::cerr<<"invalid options! run with --help or -h to see the proper options."<<std::endl;
        return EXIT_FAILURE;
    }

    //Parse the elements into number from the string:
    {
        std::stringstream ss(elements);
        std::cout<<"what is in elements: "<<ss.str()<<std::endl;
        unsigned int i = 0;
        while(ss>>D[i++]) { //get the string until comma to first element of D and then increase i.
            if(ss.peek() == ',') //Still don't know why ss>> gives output upto comma, did not see
                ss.ignore();        //it in the documentation!
        }
    }

    //Check if the elements are properly parsed into D array.
    //for (int i=0;i<6;++i)
    //    std::cout<<std::endl<<D[i];

    //---------------------  Read the reference image type ----------------------//
    typedef itk::Image<double, 3>                       ScalarImageType;
    typedef itk::ImageFileReader<ScalarImageType>       ScalarImageReaderType;

    ScalarImageReaderType::Pointer reader = ScalarImageReaderType::New();
    reader->SetFileName(inRefFileName);
    reader->Update();
    ScalarImageType::Pointer refImg = reader->GetOutput();

    //-------------------- Tensor Image -------------------------------------//
    typedef itk::Image<itk::DiffusionTensor3D<double>, 3>       TensorImageType;
    TensorImageType::Pointer tensorImage = TensorImageType::New();

    //------------ Get important details from the reference image ------------//
    tensorImage->SetRegions(refImg->GetLargestPossibleRegion());
    tensorImage->SetOrigin(refImg->GetOrigin());
    tensorImage->SetSpacing(refImg->GetSpacing());
    tensorImage->SetDirection(refImg->GetDirection());

    // ----------- Allocate memory ---------------------------//
    tensorImage->Allocate();

    // Fill the voxels with the input tensor value
    tensorImage->FillBuffer(D);

    // Test how the values are stored:
    std::cout<<"origin: "<<tensorImage->GetOrigin()<<std::endl;
    std::cout<<"direction: "<<tensorImage->GetDirection()<<std::endl;
    TensorImageType::IndexType posTensor;
    for (unsigned int i = 0; i<3; ++i) posTensor.SetElement(i,10);
    std::cout<<"Tensor value: "<<tensorImage->GetPixel(posTensor)<<std::endl;
    std::cout<<"Tensor values again:\n"<<tensorImage->GetPixel(posTensor)(0,0)<<"\t"<<tensorImage->GetPixel(posTensor)(0,1)<<"\t"<<tensorImage->GetPixel(posTensor)(0,2)<<std::endl;
    std::cout<<tensorImage->GetPixel(posTensor)(1,0)<<"\t"<<tensorImage->GetPixel(posTensor)(1,1)<<"\t"<<tensorImage->GetPixel(posTensor)(1,2)<<std::endl;
    std::cout<<tensorImage->GetPixel(posTensor)(2,0)<<"\t"<<tensorImage->GetPixel(posTensor)(2,1)<<"\t"<<tensorImage->GetPixel(posTensor)(2,2)<<std::endl;

    //------------ Write output image -------------------------//
    typedef itk::ImageFileWriter<TensorImageType>       TensorImageWriterType;
    TensorImageWriterType::Pointer writer = TensorImageWriterType::New();
    writer->SetFileName(outFileName);
    writer->SetInput(tensorImage);
    writer->Update();

    return EXIT_SUCCESS;
}


////////////////////////////////////////////////////////////////////////////////////
//  Geometric utilities for multicamera calibration
//
//  Copyright (C) 2016  David Alejo, Fernando Caballero, Luis Merino
//  Universidad Pablo de Olavide
//  Seville, Spain.
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
////////////////////////////////////////////////////////////////////////////////////

#ifndef GEOMETRIC_UTILS__
#define GEOMETRIC_UTILS__

#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/concept_check.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#ifndef  CALI_VEC_SIZE
#define CALI_VEC_SIZE 7
#endif

typedef boost::variate_generator<boost::mt19937, boost::normal_distribution<> > NormalDist;
static NormalDist norm_dist(boost::mt19937(time(0)), boost::normal_distribution<>(0, 1.0));

// --------------------------- General utilities ----------------------------------
double f_rand(double fMin = 0.0, double fMax = 1.0)
{
  static bool init = false;
  if (!init) {
    init = true;
    srand(time(NULL));
  }
  double f = (double)rand() / RAND_MAX;
  return fMin + f * (fMax - fMin);
}

std::vector<double> eigen2stdvec(const Eigen::Vector3d &v) {
  std::vector<double> ret;
  ret.resize(3);
  for (int i = 0; i < 3; i++) {
    ret[i] = v[i];
  }
  return ret;
}

// ----------------- Geometric utilities ------------------------------

//! Gets a transformation from a parameter
Eigen::Affine3d getTransform(const double *p)
{
  // Get the transformation matrix T from p vector
  Eigen::Affine3d T = Eigen::Affine3d::Identity();
  Eigen::Quaterniond R(p[0], p[1], p[2], p[3]);
  T.rotate(R);
  T.pretranslate(Eigen::Vector3d(p[4], p[5], p[6]));

  return T;
}

void decomposeTransform(const Eigen::Affine3d &T, Eigen::Quaterniond &q, Eigen::Vector3d &v) {
  Eigen::Quaterniond d(T.rotation( ));
  q = d;
  v[0] = T(0,3);
  v[1] = T(1,3);
  v[2] = T(2,3);
}

bool quat2rotvec(const Eigen::Quaterniond &q, Eigen::Vector3d &v) {
  double norm = sqrt(1 - q.w());
  if (norm < 1e-4) {
    // Too small rotation
    v[0] = v[1] = v[2] = 0.0;
    return false;
  }
  norm = 1 / norm;
  v[0] = q.x() * norm;
  v[1] = q.y() * norm;
  v[2] = q.z() * norm;

  return true;
}

Eigen::Quaterniond euler2quat(double roll, double pitch, double yaw) {
  Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d(0.0, 0.0, 1.0));
  Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d(1.0, 0.0, 0.0));
  Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d(0.0, 1.0, 0.0));
  Eigen::Quaterniond q =  yawAngle * pitchAngle * rollAngle; // 3-2-1

  return q;
}

// Returns the 3-2-1 body angles in [roll, pitch, yaw]
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &q) {
  Eigen::Vector3d ret;
  double w = q.w(), x = q.x(), y = q.y(), z = q.z();

  ret[0] = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y));
  ret[1] = asin(0.9999999* (2*(w*y - z*x)));
//   cout << "x = " << asin(1.0) << endl;
  ret[2] = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z));

  return ret;
}

#endif


#pragma once

// system includes ------------------------------------------------------------
#include <Eigen/Sparse>
#include <boost/iterator/filter_iterator.hpp>
#include <boost/mpl/identity.hpp>
#include <functional>
#include <type_traits>
#include <iterator>
#include <tuple>

// own includes ------------------------------------------------------------
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"

namespace boltzmann {

namespace spectral {

// ----------------------------------------------------------------------
template <typename BASIS>
unsigned int
get_max_l(const BASIS& basis)
{
  typedef typename std::tuple_element<0, typename BASIS::elem_t::container_t>::type elem_t;
  // radial basis
  typename BASIS::elem_t::Acc::template get<elem_t> get_xir;
  unsigned int maxL = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int l = get_xir(*it).get_id().l;
    if (l > maxL) maxL = l;
  }

  return maxL;
}

// ----------------------------------------------------------------------
/**
 * @Brief Return the max polynomial degree for the Polar-Laguerre basis
 *
 * @param basis
 *
 * @return
 */
template <typename BASIS>
typename std::enable_if<std::is_same<typename BASIS::elem_t::container_t,
                                     typename ::boltzmann::SpectralBasisFactoryKS::elem_t::container_t
                                     >::value,
                        unsigned int>::type
get_max_k(const BASIS& basis)
{
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type elem_t;
  typename BASIS::elem_t::Acc::template get<elem_t> get_phi;
  unsigned int maxK = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_phi(*it).get_id().k;
    if (k > maxK) maxK = k;
  }
  return maxK;
}

// ----------------------------------------------------------------------
/**
 * @Brief Return the max polynomial degree for the Polar-Laguerre basis
 *
 * @param basis
 *
 * @return
 */
template <typename BASIS>
typename std::enable_if<std::is_same<typename BASIS::elem_t::container_t,
                                     typename ::boltzmann::SpectralBasisFactoryKS::elem_t::container_t
                                     >::value,
                        unsigned int>::type
get_K(const BASIS& basis)
{
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type elem_t;
  typename BASIS::elem_t::Acc::template get<elem_t> get_phi;
  unsigned int maxK = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_phi(*it).get_id().k;
    if (k > maxK) maxK = k;
  }
  return maxK+1;
}

// ----------------------------------------------------------------------
/**
 * @Brief Return the max polynomial degree for the Hermite basis
 *
 * @param basis
 *
 * @return
 */
template <typename BASIS>
typename std::enable_if<std::is_same<typename BASIS::elem_t::container_t,
                                     typename ::boltzmann::SpectralBasisFactoryHN::elem_t::container_t
                                     >::value,
                        unsigned int>::type
get_K(const BASIS& basis)
{
  typedef typename std::tuple_element<0, typename BASIS::elem_t::container_t>::type hx_t;
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type hy_t;
  typename BASIS::elem_t::Acc::template get<hx_t> get_hx;
  unsigned int maxKx = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_hx(*it).get_id().k;
    if (k > maxKx) maxKx = k;
  }

  typename BASIS::elem_t::Acc::template get<hy_t> get_hy;
  unsigned int maxKy = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_hx(*it).get_id().k;
    if (k > maxKy) maxKy = k;
  }

  return std::max(maxKx, maxKy)+1;
}

#ifdef USE_CXX14
/**
 * @brief return boost::filter_iterator with basis functions that have angular frequency \f$ l \f$.
 *
 * @param begin
 * @param end
 * @param l    angular frequency
 *
 * @return
 */
template <typename ITERATOR>
auto
filter_freq(const ITERATOR& begin,
            const typename boost::mpl::identity<ITERATOR>::type& end,
            unsigned int l)
{
  typedef typename std::iterator_traits<ITERATOR>::value_type elem_t;

  typedef typename std::tuple_element<0, typename elem_t::container_t>::type aelem_t;

  auto f = [l](const elem_t& elem) {
    typename elem_t::Acc::template get<aelem_t> get_xir;
    return get_xir(elem).get_id().l == l;
  };

  return std::make_tuple(boost::make_filter_iterator(f, begin, end),
                         boost::make_filter_iterator(f, end, end));
}

/**
 * @brief return boost::filter_iterator with basis functions that total polynomial degree \f$ k \f$.
 *
 * @param begin
 * @param end
 * @param k    polynomial degree
 *
 * @return
 */
template <typename ITERATOR>
auto
filter_deg(const ITERATOR& begin,
           const typename boost::mpl::identity<ITERATOR>::type& end,
           unsigned int k)
{
  typedef typename std::iterator_traits<ITERATOR>::value_type elem_t;

  typedef typename std::tuple_element<1, typename elem_t::container_t>::type aelem_t;

  auto f = [k](const elem_t& elem) {
    typename elem_t::Acc::template get<aelem_t> get_lag;
    return get_lag(elem).get_id().k == k;
  };

  return std::make_tuple(boost::make_filter_iterator(f, begin, end),
                         boost::make_filter_iterator(f, end, end));
}
#endif  // USE_CXX14

}  // end namespace spectral_basis
}  // end namespace boltzmann


#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <memory>

#include <boost/lexical_cast.hpp>

struct Token
{
    enum Type { integer, plus, minus, lparen, rparen } type;

    std::string text;

    friend std::ostream& operator<<(std::ostream& oss, const Token& token)
    {
        return oss << "'" << token.text << "'";
    }
};

std::vector<Token> lex(const std::string& input)
{
    std::vector<Token> result;

    for(size_t i = 0; i < input.size(); i++)
    {
        switch(input[i])
        {
            case '+':
                result.push_back( { Token::plus, "+" } );
                break;
            case '-':
                result.push_back( { Token::minus, "-" } );
                break;
            case '(':
                result.push_back( { Token::lparen, "(" } );
                break;
            case ')':
                result.push_back( { Token::rparen, ")" } );
                break;
            default:
                std::ostringstream buffer;
                buffer << input[i];
                for(size_t j = i + 1; j < input.size(); j++)
                {
                    if(std::isdigit(input[j]))
                    {
                        buffer << input[j];
                        ++i;
                    }
                    else
                    {
                        break;
                    }
                }

                result.push_back( { Token::integer, buffer.str() } );
        }
    }

    return result;
}

struct Element
{
    using Ptr = std::shared_ptr<Element>;

    virtual int eval() const = 0;
};

struct Integer : Element
{
    int value;

    Integer(int value) : value (value) {};

    int eval() const override
    {
        return value;
    }
};

struct BinaryOp : Element
{
    enum Type { addition, substraction } type;

    Element::Ptr lhs;
    Element::Ptr rhs;

    int eval() const override
    {
        if(type == addition)
        {
            return lhs->eval() + rhs->eval();
        }
        else
        {
            return lhs->eval() - rhs->eval();
        }
    }
};

Element::Ptr parse(const std::vector<Token>& tokens)
{
    auto result = std::make_shared<BinaryOp>();

    bool have_lhs { false };

    for(size_t i = 0; i < tokens.size(); i++)
    {
        auto token = tokens[i];

        switch(token.type)
        {
            case Token::integer:
            {
                int value = boost::lexical_cast<int>(token.text);
                if(!have_lhs)
                {
                    result->lhs = std::make_shared<Integer>(value);
                    have_lhs = true;
                }
                else
                {
                    result->rhs = std::make_shared<Integer>(value);
                }

            }
                break;
            case Token::plus:
                result->type = BinaryOp::addition;
                break;
            case Token::minus:
                result->type = BinaryOp::substraction;
                break;
            case Token::lparen:
            {
                size_t j = i;
                for(; j < tokens.size(); ++j)
                {
                    if(tokens[j].type == Token::rparen)
                    {
                        break;
                    }
                }

                std::vector<Token> subexpression(&tokens[i + 1], &tokens[j]);
                auto element = parse(subexpression);

                if(!have_lhs)
                {
                    result->lhs = element;
                    have_lhs = true;
                }
                else
                {
                    result->rhs = element;
                }

                i = j;
            }
                break;
            case Token::rparen:
                break;
        }
    }

    return result;
}

int main()
{
    std::string input { "(13-4)-(12+1)" };

    auto tokens = lex(input);

    for(auto& token : tokens)
    {
        std::cout << token << " ";
    }
    std::cout << "\n";

    try
    {
        auto parsed = parse(tokens);
        std::cout << input << " = " << parsed->eval() << std::endl;
    }
    catch(const std::exception& ex)
    {
        std::cout << ex.what() << std::endl;
    }

    return 0;
}