//
// Created by Сергей Кривонос on 01.09.17.
//
#include "Exponentiation.h"

#include "Valuable.h"
#include "e.h"
#include "i.h"
#include "Infinity.h"
#include "pi.h"
#include "Integer.h"
#include "Sum.h"
#include "Product.h"

#include <cmath>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

namespace omnn{
namespace math {

    Exponentiation::Exponentiation(const Valuable& b, const Valuable& exponentiation)
        : base(b, exponentiation)
    {
        InitVars();
    }

    max_exp_t Exponentiation::getMaxVaExp(const Valuable& b, const Valuable& e)
    {
        if (e.IsInt()) {
            return b.getMaxVaExp() * e.ca();
        } else if (e.FindVa()) {
            auto i = b.getMaxVaExp();
            if (i) {
                auto _ = e;
                const Variable* v;
                while ((v = _.FindVa())) {
                    _.Eval(*v, 0);
                }
                _.optimize();
                i *= static_cast<a_int>(_);
            }
            return i;
        } else {
            auto maxVaExp = e * b.getMaxVaExp();
            if (maxVaExp.IsInt()) {
                return maxVaExp.ca();
            } else if (maxVaExp.IsSimpleFraction()) {
                auto& f = maxVaExp.as<Fraction>();
                return {f.getNumerator().ca(), f.getDenominator().ca()};
            } else if(!optimizations) {
                optimizations = true;
                maxVaExp.optimize();
                optimizations = {};
                if (maxVaExp.IsInt()) {
                    return maxVaExp.ca();
                }
            }
        }

        IMPLEMENT
    }

    max_exp_t Exponentiation::getMaxVaExp() const
    {
        return getMaxVaExp(getBase(), getExponentiation());
    }

    Valuable Exponentiation::varless() const
    {
        if(getBase().IsVa()) {
            return constants::one;
        } else if (FindVa()) {
            IMPLEMENT;
        }
        else
            return *this;
    }

    void Exponentiation::InitVars() {
        v.clear();
        if (ebase().IsVa())
            v[ebase().as<Variable>()] = eexp();
    }

    void Exponentiation::optimize()
    {
        if (optimized) {
            return;
        }

        if (!optimizations)
        {
            hash = ebase().Hash() ^ eexp().Hash();
            InitVars();
            return;
        }

        ebase().optimize();
        eexp().optimize();

        if (ebase().IsExponentiation() || ebase().IsProduct())
        {
            ebase() ^= eexp();
            Become(std::move(ebase()));
            return;
        }

        if (eexp().IsSum())
        {
            auto& s = eexp().as<Sum>();
            auto sz = s.size();
            auto v = 1_v;
            for(auto it = s.begin(), e = s.end();
                it != e; )
            {
                if (it->IsInt()) {
                    v *= ebase()^*it;
                    s.Delete(it);
                }
                else
                    ++it;
            }
            if (sz != s.size()) {
                Become(*this * v);
                return;
            }
        }

        // todo : check it, comment this and try System test
//        if (ebase().IsSum() && eexp().IsFraction())
//        {
//            auto f = Fraction::cast(eexp());
//            auto& d = f->getDenominator();
//            if (d == ebase().getMaxVaExp()) {
//                auto vars = ebase().Vars();
//                if (vars.size() == 1) {
//                    auto va = *vars.begin();
//                    auto baseToSolve = ebase();
//                    baseToSolve.SetView(View::Solving);
//                    baseToSolve.optimize();
//                    auto eq = va - baseToSolve(va);
//                    auto sq = eq ^ d;
//                    auto check = ebase()/sq;
//                    if (check.IsInt() || check.IsSimpleFraction()) {
//                        ebase() = eq;
//                        eexp() = f->getNumerator();
//                    } else {
//                        // TODO : IMPLEMENT
//                    }
//                }
//            }
//        }
//
//        if (ebase().IsSum() && eexp().IsFraction())
//        {
//            auto f = Fraction::cast(eexp());
//            auto& d = f->getDenominator();
//            auto e = ebase() ^ f->getNumerator();
//            if (d == e.getMaxVaExp()) {
//                auto vars = e.Vars();
//                if (vars.size() == 1) {
//                    auto va = *vars.begin();
//                    auto eq = va - e(va);
//                    auto sq = eq ^ d;
//                    auto check = e / sq;
//                    if (check.IsInt() || check.IsSimpleFraction()) {
//                        Become(std::move(eq));
//                        return;
//                    } else {
//                        // TODO : IMPLEMENT
//                    }
//                }
//            }
//        }

        if (ebase().IsFraction() && eexp().IsMultival()==YesNoMaybe::No) {
            auto& f = ebase().as<Fraction>();
            auto _ = (f.getNumerator() ^ eexp()) / (f.getDenominator() ^ eexp());
            if (_.IsExponentiation()) {
                auto& e = _.as<Exponentiation>();
                if (!(e.ebase()==ebase() && eexp()==e.eexp())) {
                    IMPLEMENT
                }
            } else {
                Become(std::move(_));
                return;
            }
        }

        if (ebase().IsFraction() && eexp().IsInt() && eexp() < 0_v) {
            eexp() = -eexp();
            ebase() = ebase().as<Fraction>().Reciprocal();
        }

		// e^(i*pi) = -1
                // it is a fundamental equation that gives us a hint on cross-dimmensional relations
                // because i and -1 are 1 of different signs/dimmensions
        if (ebase().Is_e()) {
            if (eexp().IsProduct()) {
                auto& p = eexp().as<Product>();
                if (p.Has(constants::pi) &&
                    p.Has(constants::i)) { // TODO : sequence does matter :
																 // e^(i*pi) =?= e^(pi*i)
																 // https://en.wikipedia.org/wiki/Commutative_property#Division,_subtraction,_and_exponentiation
																 // what about Commutativity on irrationals product?
																 // lets assume yes for this particular expression, but there are some doubts, need a prove

						// here is implementation for case of no commutativity:
                                    // auto e = p.end();
                                    // auto it = std::find(p.begin(), e, constant::i);
                                    // auto has_i = it != e;
                                    // if (has_i) {
                                    //     it = std::find(++it, e, constant::pi);
                                    //     auto has_pi_next_to_i = it != e; // maybe sequence does matter
                                    // }

                    p /= constant::i;
                    p /= constant::pi;
                    Become(Exponentiation{-1, p});
                    return;
                }
            }
        }
        // todo : check
        if (ebase().IsSimple()) {
            if (eexp().IsProduct()) {
                auto& p = eexp().as<Product>();
                auto it = p.GetFirstOccurence<Integer>();
                auto in = ebase() ^ *it;  // IsExponentiationSimplifiable
                if (in.IsInt()) {
                    ebase() = in;
                    p.Delete(it);
                    if (p.size() == 1) {
                        eexp().optimize();
                    }
                }
            }
            if (ebase()==1) {
                if (eexp().IsInt()) {
                    Become(std::move(ebase()));
                    return;
                } else if (eexp().IsSimpleFraction() && eexp() > 0_v) {
                    auto& f = eexp().as<Fraction>();
                    auto& n = f.getNumerator();
                    if (n.IsEven() == YesNoMaybe::Yes) {
                        auto&& toOptimize = std::move(eexp());
                        toOptimize.optimize();
                        setExponentiation(std::move(toOptimize));
                        optimize();
                        return;
                    }
                    auto& dn = f.getDenominator();
                    if (dn.bit(0)) {
                        Become(std::move(ebase()));
                        return;
                    } else if (n != 1_v) {
                        hash ^= f.Hash();
                        f.update1(1_v);
                        hash ^= f.Hash();
					}
                } else if (!!eexp().IsMultival()) {
                } else if (!(eexp().IsInfinity() || eexp().IsMInfinity())) {
                    Become(std::move(ebase()));
                    return;
                } else
                    IMPLEMENT;
            } else if (ebase() == -1 && eexp().IsInt() && eexp() > 0 && eexp() != 1) {
                    eexp() = eexp().bit(0);
            } else if (eexp()==-1) {
                Become(Fraction{1,ebase()});
                return;
            } else if (eexp().IsInfinity()) {
                IMPLEMENT
            } else if (eexp().IsFraction()) {
                auto& f = eexp().as<Fraction>();
                auto& n = f.getNumerator();
                if (n != 1) {
                    // TODO: auto is = ebase().IsExponentiationSimplifiable(n);
                    auto newBase = ebase() ^ n;
                    if(!newBase.IsExponentiation()){
                        Become(newBase ^ (1_v / f.getDenominator()));
                        return;
                    }
                }
            }
        }

        bool ebz = ebase() == 0_v;
        bool exz = eexp() == 0_v;
        if(exz)
        {
            if (ebase().IsInfinity() || ebase().IsMInfinity()) {
                IMPLEMENT
            }
            if(ebz)
                throw "NaN";

            Become(1_v);
            return;
        }
        else if(eexp() == 1_v)
        {
            Become(std::move(ebase()));
            return;
        }
        else if (ebz)
        {
            if (exz)
                throw "NaN";
            Become(0_v);
            return;
        }
        else if (ebase().IsInfinity())
        {
            if (eexp() > 0) {
                Become(std::move(ebase()));
            } else
                IMPLEMENT
        }
        else if (ebase().IsMInfinity())
        {
            if (eexp() > 0) {
                if ((eexp() % 2) > 0) // TODO : test with non-ints
                    Become(std::move(ebase()));
                else
                    Become(Infinity());
            } else
                IMPLEMENT
        }
        else if (ebase().IsVa() && eexp().IsSimple())
        {
        }
        else
        {
            switch(view)
            {
                case View::Solving:

                case View::None:
                case View::Calc:
                {
                    if (eexp().IsInt() && eexp()>0) {
                        auto b = 1_v;
                        for (; eexp()--;) {
                            b *= ebase();
                        }
                        Become(std::move(b));
                        return;
                    }
                    if (ebase().IsInt() && eexp().IsInt()) {
                        Become(ebase() ^ eexp());
                        return;
                    }
                    break;
                }
                case View::Flat: {

                    if(eexp().IsInt())
                    {
                        if (ebase().IsVa()) {
                            break;
                        }
                        if (eexp() != 0_v) {
                            if (eexp() > 1) {
                                Valuable x = ebase();
                                Valuable n = eexp();
                                if (n < 0_v)
                                {
                                    x = 1_v / x;
                                    n = -n;
                                }
                                if (n == 0_v)
                                {
                                    Become(1_v);
                                    return;
                                }
                                auto y = 1_v;
                                while(n > 1)
                                {
                                    bool isInt = n.IsInt();
                                    if (!isInt)
                                        IMPLEMENT
                                    if (isInt && n.bit(0) == 0_v)
                                    {
                                        x.sq();
                                        n /= 2;
                                    }
                                    else
                                    {
                                        y *= x;
                                        x.sq();
                                        --n;
                                        n /= 2;
                                    }
                                }
                                x *= y;
                                Become(std::move(x));
                            } else if (eexp()!=-1){
                                // negative
                                Become(1_v/(ebase()^(-eexp())));
                            }
                        }
                        else { // zero
                            if (ebase() == 0_v)
                            {
                                IMPLEMENT
                                throw "NaN"; // feel free to handle this properly
                            }
                            else
                            {
                                Become(1_v);
                            }
                        }
                    }
//                    else
//                    IMPLEMENT
                    break;
                }
                case View::Equation: {
                    if(eexp().IsSimple())
                        Become(std::move(ebase()));
                    break;
                }
                default:
                	LOG_AND_IMPLEMENT(str() << " mode is " << view);
            }
        }

        if(IsExponentiation() && ebase().IsExponentiation())
        {
            auto& e = ebase().as<Exponentiation>();
            auto& eeexp = e.getExponentiation();
            if ((eeexp.FindVa() == nullptr) == (eexp().FindVa() == nullptr)) {
                eexp() *= eeexp;
                // todo : copy if it shared
                ebase() = std::move(const_cast<Valuable&>((e.getBase())));
            }
        }

        if (IsExponentiation()) {
            hash = ebase().Hash() ^ eexp().Hash();
            optimized = true;
            InitVars();
        }
    }

    Valuable& Exponentiation::operator +=(const Valuable& v)
    {
        return Become(Sum {*this, v});
    }

    Valuable& Exponentiation::operator *=(const Valuable& v)
    {
        const Exponentiation* e;
        const Fraction* f;
        const Product* fdn = {};
        const Exponentiation* fdne;
        auto isProdHasExpWithSameBase = [this](const Product* p) -> const Exponentiation*
        {
            for(auto& it : *p){
                if (it.IsExponentiation()) {
                    auto& e = it.as<Exponentiation>();
                    if (ebase() == e.getBase()) {
                        return &e;
                    }
                }
            }
            return {};
        };
        auto& b = ebase();
        if (v.IsExponentiation()
            && b == (e = &v.as<Exponentiation>())->getBase()
            && (eexp().IsInt() || eexp().IsSimpleFraction()) && eexp() > 0
            && (e->eexp().IsInt() || e->eexp().IsSimpleFraction()) && e->eexp() > 0
            )
        {
            updateExponentiation(eexp() + e->getExponentiation());
            optimized={};
        }
        else if(v.IsFraction()
                && (f = &v.as<Fraction>())->getDenominator() == ebase())
        {
            --eexp();
            optimized={};
            optimize();
            return *this *= f->getNumerator();
        }
        else if(v.IsFraction()
                && f->getDenominator().IsProduct()
                && (fdn = &f->getDenominator().as<Product>())->Has(ebase()))
        {
            --eexp();
            optimized={};
            optimize();
            return *this *= f->getNumerator() / (*fdn / ebase());
        }
        else if(fdn
                && (fdne = isProdHasExpWithSameBase(fdn)))
        {
            eexp() -= fdne->getExponentiation();
            optimized={};
            optimize();
            return *this *= f->getNumerator() / (*fdn / *fdne);
        }
        else if(b == v && v.FindVa())
        {
            updateExponentiation(eexp()+1);
            optimized={};
        }
        else if(-b == v && v.FindVa())
        {
            updateExponentiation(eexp()+1);
            optimized = {};
            return Become(Product{-1, *this});
        }
        else if(v.IsProduct())
        {
            return Become(v * *this);
        }
        else if(v.IsInt())
        {
            if(v==1)
                return *this;
            else if(eexp()==-1 && ebase().IsInt())
                return Become(v/ebase());
            else
                return Become(Product{v, *this});
        }
        else
            return Become(Product{v, *this});

        optimize();
        return *this;
    }

    bool Exponentiation::MultiplyIfSimplifiable(const Valuable& v)
    {
        auto is = v == getBase();
        if (is) {
            ++eexp();
            optimized = {};
            optimize();
        } else if (v == constants::one) {
            is = true;
        } else if (!FindVa() && IsMultival() == YesNoMaybe::Yes && !v.FindVa()) {
            solutions_t values;
            for (auto& value : Distinct()) {
                auto&& extract = std::move(const_cast<Valuable&>(value));
                if (extract.MultiplyIfSimplifiable(v)) {
                    is = true;
				} else {
                    extract *= v;
				}
                values.emplace(std::move(extract));
            }
            Become(Valuable(std::move(values)));
        } else if (v.IsExponentiation()) {
            auto& vexpo = v.as<Exponentiation>();
            is = vexpo.getBase() == getBase();
            if (is) {
                eexp() += vexpo.eexp();
                optimized = {};
                optimize();
            } // TODO : else if ? (base^2 == v.base)
            else {
                is = vexpo.getExponentiation() == getExponentiation();
                if (is) {
                    auto wasBaseHash = ebase().Hash();
                    is = ebase().MultiplyIfSimplifiable(vexpo.getBase());
                    if (is) {
                        Valuable::hash ^= wasBaseHash ^ ebase().Hash();
                        optimized = {};
                        optimize();
                    }
                }
            }
        } else if (v.IsMultival() == YesNoMaybe::Yes) {
            LOG_AND_IMPLEMENT(str() << " Exponentiation::MultiplyIfSimplifiable " << v);
        } else if (v.IsInt()) {
            IMPLEMENT
        } else {
//            std::cout << str() << " * " << v.str() << std::endl;
        }
        return is;
    }

    std::pair<bool,Valuable> Exponentiation::IsMultiplicationSimplifiable(const Valuable& v) const
    {
        std::pair<bool,Valuable> is, expSumSimplifiable = {};
        is.first = v == getBase()
            && (expSumSimplifiable = eexp().IsSummationSimplifiable(constants::one)).first;
        if (is.first) {
            is.second = getBase() ^ expSumSimplifiable.second;
        } else if (v.IsExponentiation()) {
            auto& vexpo = v.as<Exponentiation>();
            is.first = vexpo.getBase() == getBase();
            if (is.first) {
                is.second = ebase() ^ (eexp() + vexpo.eexp());
            } // TODO : else if ? (base^2 == v.base)
        } else if (v.IsSimple()) {
            auto& ee = getExponentiation();
            //is = v.IsExponentiationSimplifiable(ee);  // TODO: Implement IsExponentiationSimplifiable
            // FIXME: Until IsExponentiationSimplifiable ready:
            if (ee.IsSimpleFraction()) {
                is.second = v ^ ee.as<Fraction>().Reciprocal(); // v.IsExponentiationSimplifiable(ee)
                is.first = is.second.MultiplyIfSimplifiable(getBase());
                if(is.first){
                    auto copy = *this;
                    copy.updateBase(std::move(is.second));
                    is.second = copy;
                }
            }
            else if (ee == constants::minus_1)
            {
                is.second = v ^ constants::minus_1; // v.IsExponentiationSimplifiable(ee)
                is.first = is.second.MultiplyIfSimplifiable(getBase());
                if(is.first){
                    auto copy = *this;
                    copy.updateBase(std::move(is.second));
                    is.second = copy;
                }
            }
            else if (ee.IsInt() && (ee > constants::zero)) // TODO: ee < 0 too
            {
//                is.second = v ^ (ee ^ constants::minus_1); // v.IsExponentiationSimplifiable(ee)
//                is.first = is.second.MultiplyIfSimplifiable(getBase());
//                if(is.first){
//                    auto copy = *this;
//                    copy.updateBase(std::move(is.second));
//                    is.second = copy;
//                }
            }
//            if (getBase().IsVa()) {
//            } else if (getExponentiation().IsSimpleFraction()) {
//                auto
//                is.first = IsMultiplicationSimplifiable()
//            } else if (getExponentiation().IsSimple()) {
//                is = base::IsMultiplicationSimplifiable(v);
//            } else {
//                IMPLEMENT
//            }
        } else if (v.IsVa()) {
            // covered by (v==base()) case
        } else if (v.IsProduct()) {
            is=v.IsMultiplicationSimplifiable(*this);
        } else if (v.IsSum()) {
            auto& sum=v.as<Sum>();
            is.first=sum==getBase()||-sum==getBase();
            if(is.first){
                is.second=*this*sum;
            }else{
                is=sum.IsMultiplicationSimplifiable(*this);
            }
        } else {
#ifndef NDEBUG
            std::cout << "IsMultiplication simplifiable?: " << str() << " * " << v.str() << std::endl;
#endif
        }
        return is;
    }

    bool Exponentiation::SumIfSimplifiable(const Valuable& v)
    {
        auto is = !v.IsSimple() && !v.IsFraction() && !v.IsExponentiation();
        if(is){
            auto sumIfSimplifiable = v.IsSummationSimplifiable(*this);
            is = sumIfSimplifiable.first;
            if (is)
                Become(std::move(sumIfSimplifiable.second));
        }
        return is;
    }

    std::pair<bool,Valuable> Exponentiation::IsSummationSimplifiable(const Valuable& v) const
    {
        std::pair<bool,Valuable> is;
        is.first = operator==(v);
        if (is.first) {
            is.second = *this * 2;
        } else if ((is.first = operator==(-v))) {
                is.second = 0;
        } else if (v.IsSimple()
                || v.IsExponentiation()
                || v.IsVa()
                || v.IsFraction())
        {
        } else {
            is = v.IsSummationSimplifiable(*this);
        }
        return is;
    }

    Valuable& Exponentiation::operator /=(const Valuable& v)
    {
        auto isMultival = IsMultival()==YesNoMaybe::Yes;
        auto vIsMultival = v.IsMultival()==YesNoMaybe::Yes;
        if(isMultival && vIsMultival) {
            solutions_t vals, thisValues;
            Values([&](auto& thisVal){
                thisValues.insert(thisVal);
                return true;
            });

            v.Values([&](auto&vVal){
                for(auto& tv:thisValues)
                    vals.insert(tv/vVal);
                return true;
            });

            return Become(Valuable(std::move(vals)));
        }
        else if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            if(ebase() == e.ebase() && (ebase().IsVa() || !ebase().IsMultival()))
            {
                eexp() -= e.eexp();
            }
            else
            {
                Become(Fraction(*this, v));
                return *this;
            }
        }
        else if(v.IsFraction())
        {
            *this *= v.as<Fraction>().Reciprocal();
            return *this;
        }
        else if(ebase() == v)
        {
            --eexp();
        }
        else
        {
            Become(Fraction(*this, v));
            return *this;
        }

        optimized={};
        optimize();
        return *this;
    }

    Valuable& Exponentiation::operator^=(const Valuable& v)
    {
        eexp() *= v;
        optimized={};
        optimize();
        return *this;
    }

    bool Exponentiation::operator ==(const Valuable& v) const
    {
        auto eq = v.IsExponentiation() && Hash()==v.Hash();
        if(eq){
            auto& e = v.as<Exponentiation>();
            eq = _1.Hash() == e._1.Hash()
                && _2.Hash() == e._2.Hash()
                && _1 == e._1
                && _2 == e._2;
        } else if (v.IsFraction()) {
            eq = eexp().IsInt()
                 && eexp() < 0
                 && ebase() == (v.as<Fraction>().getDenominator() ^ (-eexp()));
        }
        return eq;
    }

    Exponentiation::operator double() const
    {
        return std::pow(static_cast<double>(ebase()), static_cast<double>(eexp()));
    }

    Valuable& Exponentiation::d(const Variable& x)
    {
        optimized={};
        bool bhx = ebase().HasVa(x);
        bool ehx = eexp().HasVa(x);
        if(ehx) {
            IMPLEMENT
            if(bhx){

            }else{

            }
        } else if (bhx) {
            if(ebase() == x)
                Become(eexp() * (ebase() ^ (eexp()-1)));
            else
                IMPLEMENT
        } else
            Become(0_v);
        optimize();
        return *this;
    }

    void Exponentiation::integral(const Variable& x, const Variable& C)
    {
        if ((eexp().IsInt() || eexp().IsSimpleFraction()) && ebase()==x) {
            ++eexp();
            operator/=(eexp());
            operator+=(C);
        } else {
            IMPLEMENT
        }

        optimize();
    }

    bool Exponentiation::operator <(const Valuable& v) const
    {
        if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            if (e.getBase() == getBase())
                return getExponentiation() < e.getExponentiation();
            if (e.getExponentiation() == getExponentiation())
                return getBase() < e.getBase();
        }

        return base::operator <(v);
    }

    std::ostream& Exponentiation::print_sign(std::ostream& out) const
    {
        return out << "^";
    }

    Valuable::YesNoMaybe Exponentiation::IsMultival() const
    {
        auto is = _1.IsMultival() || _2.IsMultival();
        if (is != YesNoMaybe::Yes && _2.IsFraction())
            is = _2.as<Fraction>().getDenominator().IsEven() || is;
        return is;
    }

    void Exponentiation::Values(const std::function<bool(const Valuable&)>& fun) const
    {
        if (fun) {
            auto cache = optimized; // TODO: multival caching (inspect all optimized and optimization transisions) auto isCached =

            std::set<Valuable> vals;
            {
            std::deque<Valuable> d1;
            _1.Values([&](auto& v){
                d1.push_back(v);
                return true;
            });

            _2.Values([&](auto& v){
                auto vIsFrac = v.IsFraction();
                const Fraction* f;
                if(vIsFrac)
                    f = &v.template as<Fraction>();
                auto vMakesMultival = vIsFrac && f->getDenominator().IsEven()==YesNoMaybe::Yes;

                for(auto& item1:d1){
                    if(vMakesMultival){
                        static const Variable x;
                        auto& dn = f->getDenominator();
                        auto solutions = (x ^ dn).Equals(*this ^ dn).Solutions(x);
                        for(auto&& s:solutions)
                            vals.insert(s);
                    } else {
                        auto value=item1^v;
                        if(value.IsMultival()==YesNoMaybe::No)
                            vals.insert(value);
                        else {
                            IMPLEMENT
                        }
                    }
                }
                return true;
            });
            }

            for(auto& v:vals)
                fun(v);
        }
    }

    std::ostream& Exponentiation::code(std::ostream& out) const
    {
        if(!getExponentiation().IsInt())
            IMPLEMENT;

        out << "(1";
        for (auto i=getExponentiation(); i-->0;) {
            out << '*' << getBase();
        }
        out << ')';

        return out;
    }

    bool Exponentiation::IsComesBefore(const Valuable& v) const
    {
        auto mve = getMaxVaExp();
        auto vmve = v.getMaxVaExp();
        auto is = mve > vmve;
        if (mve != vmve)
        {}
        else if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            bool baseIsVa = getBase().IsVa();
            bool vbaseIsVa = e.getBase().IsVa();
            if (baseIsVa && vbaseIsVa)
                is = getExponentiation() == e.getExponentiation() ? getBase().IsComesBefore(e.getBase()) : getExponentiation() > e.getExponentiation();
            else if(baseIsVa)
                is = false;
            else if(vbaseIsVa)
                is = true;
            else if(getBase() == e.ebase())
                is = getExponentiation().IsComesBefore(e.getExponentiation());
            else if(getExponentiation() == e.getExponentiation())
                is = getBase().IsComesBefore(e.getBase());
            else
            {
                auto c = Complexity();
                auto ec = e.Complexity();
                if (c != ec)
                    is = c > ec;
                else {
                    is = getBase().IsComesBefore(e.getBase()) ||
                        (!e.ebase().IsComesBefore(ebase()) && getExponentiation().IsComesBefore(e.getExponentiation())); //  || str().length() > e->str().length();
    //                auto expComesBefore = eexp().IsComesBefore(e->eexp());
    //                auto ebase()ComesBefore = ebase().IsComesBefore(e->ebase());
    //                is = expComesBefore==ebase()ComesBefore || str().length() > e->str().length();
                }
            }
        }
        else if(v.IsProduct())
        {
            is = !(v.IsComesBefore(*this) || operator==(v));
        }
        else if(v.IsInt())
            is = true;
//        else if(v.IsFraction())
//        {is=}
        else if(v.IsVa())
            is = !!FindVa();
        else if(v.IsSum())
            is = IsComesBefore(*v.as<Sum>().begin());
        else
            IMPLEMENT

        return is;
    }

    Valuable Exponentiation::calcFreeMember() const
    {
        Valuable c;
        if(getBase().IsSum() && getExponentiation().IsInt()){
            c = getBase().calcFreeMember() ^ getExponentiation();
        } else if(getBase().IsVa()) {
            c = 0_v;
        } else
            IMPLEMENT;
        return c;
    }

    Valuable & Exponentiation::sq()
    {
        eexp() *= 2;
        optimized = {};
        optimize();
        return *this;
    }

    const Valuable::vars_cont_t& Exponentiation::getCommonVars() const
    {
#ifndef NDEBUG
        auto& b = ebase();
        if (b.IsVa()) { // TODO: FindVa too
            auto va = b.as<Variable>();
            auto it = v.find(va);
            if (it != v.end() && !it->second.Same(eexp()))
                LOG_AND_IMPLEMENT(*this << " Exponentiation::getCommonVars not ready");
        }
#endif
        return v;
    }

    Valuable Exponentiation::InCommonWith(const Valuable& v) const
    {
        auto c = 1_v;
        if (v.IsProduct()) {
            for(auto& m: v.as<Product>()){
                c = InCommonWith(m);
                if (c != 1_v) {
                    break;
                }
            }
        } else if (v.IsExponentiation()) {
            auto& e = v.as<Exponentiation>();
            if (e.getBase() == getBase()) {
                if (e.getExponentiation() == getExponentiation()) {
                    c = e;
                } else if (getExponentiation().IsSimple() && e.getExponentiation().IsSimple()) {
                    if (getExponentiation() > 0 || e.getExponentiation() > 0) {
                        if (e.getExponentiation() >= getExponentiation()) {
                            c = *this;
                        } else
                            c = e;
                    } else if (getExponentiation() < 0 || e.getExponentiation() < 0) {
                        if (e.getExponentiation() >= getExponentiation()) {
                            c = e;
                        } else
                            c = *this;
                    } else {
                        IMPLEMENT
                    }
                } else if (getExponentiation().IsSimpleFraction() && e.getExponentiation().IsSimpleFraction()) {
                    if (getExponentiation()<0 == e.getExponentiation()<0) {
                        c = getBase() ^ getExponentiation().InCommonWith(e.getExponentiation());
                    }
                } else if (getExponentiation().IsSum()) {
                    auto sz = getExponentiation().as<Sum>().size();
                    auto diff = getExponentiation() - e.getExponentiation();
                    if (!diff.IsSum() || diff.as<Sum>().size() < sz)
                        c = v;
                } else if (e.getExponentiation().IsSum()) {
                    c = e.InCommonWith(*this);
                } else if (e.getExponentiation().IsProduct()) {
                    c = ebase() ^ e.eexp().InCommonWith(eexp());
                } else {
                    IMPLEMENT
                }
            }
        } else if (getExponentiation().IsInt()) {
            if(getExponentiation() > 0)
                c = getBase().InCommonWith(v);
        } else if (getExponentiation().IsFraction()) {
        } else if (v.IsVa()) {
            c = v.InCommonWith(*this);
        } else if (v.IsInt() || v.IsSimpleFraction()) {
        } else if (getExponentiation().IsVa()) {
        } else {
            IMPLEMENT
        }
        return c;
    }

    Valuable Exponentiation::operator()(const Variable& va) const
    {
        return operator()(va, 0_v);
    }

    Valuable Exponentiation::operator()(const Variable& v, const Valuable& augmentation) const
    {
        if (!getExponentiation().FindVa() && getExponentiation()!=0 && augmentation==0) {
            return getBase()(v,augmentation);
        } else if (getExponentiation().IsSimpleFraction()) {
            auto& f = getExponentiation().as<Fraction>();
            return (getBase()^f.getNumerator())(v,augmentation^f.getDenominator());
        } else {
            IMPLEMENT
        }
    }

    Valuable::solutions_t Exponentiation::Distinct() const
    {
        solutions_t branches;
        if (eexp().IsSimpleFraction()){
            auto& f = eexp().as<Fraction>();
            auto& denom = f.denominator();
            if (denom.IsEven() == YesNoMaybe::Yes) {
                // TODO : de-recoursefy:
//                auto branchesSz = boost::multiprecision::msb(denom); // the largest bit
//                branches.reserve(branchesSz);
//                ...
                auto& exponentiationBase = ebase();
                if(!exponentiationBase.IsSimple()){
                    if (!exponentiationBase.FindVa() && exponentiationBase.IsMultival() == YesNoMaybe::Yes) {
                        for (auto&& branch : ebase().Distinct()) {
                            auto branchDistinct = (branch ^ eexp()).Distinct();
                            branches.insert(branchDistinct.begin(), branchDistinct.end());
                        }
                    } else {
                        LOG_AND_IMPLEMENT("Distinct for " << *this);
                    }
                } else {
                    auto b = ebase() ^ f.numerator();
                    auto d = denom;
                    do {
                        b.sqrt();
                        d.shr();
                    } while(d.IsEven() == Valuable::YesNoMaybe::Yes);
                    auto _ = b ^ (1_v / d);
                    auto distinct = _.Distinct();
                    if(denom==2)
                        for (auto& branch : distinct)
                        {
                            branches.emplace(-branch);
                            branches.emplace(std::move(const_cast<decltype(distinct)::reference>(branch)));
                        }
                    else if(denom==4)
                        for (auto& branch : distinct)
                        {
                            branches.emplace(-branch * constants::i);
                            branches.emplace(branch * constants::i);
                            branches.emplace(-branch);
                            branches.emplace(std::move(const_cast<decltype(distinct)::reference>(branch)));
                        }
                    else
                        LOG_AND_IMPLEMENT("Implement support for " << denom << " dimmensions");
                }
            }
        } else {
            branches.emplace(*this);
        }
        return branches;
    }
}}


#include <fstream>
#include <string>
#include <math.h>
#include <time.h>
#include "model.h"
#include <stdlib.h>
#include <armadillo>
#include "model_fancy.h"

using namespace arma;

int initStateFile(char *file)
{
        std::ofstream out(file,std::ios_base::trunc);

        if (!out)
        {
            perror("output file error\n");
                return -1;
        }

        out.close();

        return 0;
}

void addState(char *file, fmat u)
{
        std::ofstream out(file,std::ios_base::app);
        for(unsigned int i=0; i < u.n_rows; ++i){
                out << u[i] << " ";
        }
        out << std::endl;
        out.close();
}

int main (int argc, char* argv[]){
        int num = 0;
        if (argc<3){
                perror("Error");
                printf("Please call program like:\n");
                printf("./datagen filename_original filename_measurement numberOfStates\n");
                return -1;
        }
        char *file_original = argv[1];
        char *file_out = argv[2];
        num = atoi(argv[3]);

        Model *process = new ModelFancy();
        process->initialize();

        printf("Dimension of state: %d\n",process->getProcessDimension());

        fmat state(process->getProcessDimension(),1);

        fmat measurement(process->getMeasurementDimension(),1);

        state.zeros();

        initStateFile(file_out);	// initialize output file
        initStateFile(file_original);

        addState(file_out, state);

        for (int k=0; k<num;k++)
        {
            state = process->ffun(&state);
            printf("x: %e  y: %e\n",state(0),state(1));
            addState(file_original,state);
            measurement = process->hfun(&state);
            printf("x: %e  y: %e\n\n",measurement(0),measurement(1));
            addState(file_out, measurement);
        }

        printf("Done!");

        return 0;
}


#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <algorithm>
#include <unordered_map>
#include <functional>
#include <Eigen/Dense>
#include "../include/model.h"
#include "../include/numerical_gradient.h"

namespace MyDL
{

    using namespace Eigen;
    using std::cout;
    using std::endl;
    using std::make_shared;
    using std::shared_ptr;
    using std::string;
    using std::unordered_map;
    using std::vector;

    TwoLayerMLP::TwoLayerMLP(int input_size, int hidden_size, int output_size, double weight_init_std)
    {
        // 内部保持パラメータ
        _input_size = input_size;
        _hidden_size = hidden_size;
        _output_size = output_size;
        _weight_init_std = weight_init_std;

        // Affine Layer用のパラメータ -> スマートポインタで保持し、それをパラメータのリストに格納
        auto W1 = make_shared<MatrixXd>(input_size, hidden_size);
        auto W2 = make_shared<MatrixXd>(hidden_size, output_size);
        auto b1 = make_shared<MatrixXd>(1, hidden_size);
        auto b2 = make_shared<MatrixXd>(1, output_size);

        *W1 = weight_init_std * MatrixXd::Random(input_size, hidden_size);
        *W2 = weight_init_std * MatrixXd::Random(hidden_size, output_size);
        *b1 = MatrixXd::Zero(1, hidden_size);
        *b2 = MatrixXd::Zero(1, output_size);

        // Layer作成 -> スマートポインタで実装(コンストラクタを抜けた時に、実体が消されないようにするため)
        // 左辺の型はautoにしてはいけない(BaseLayerで統一し、コンテナに格納する ※ポリモーフィズムの実現)
        shared_ptr<BaseLayer> affine1 = make_shared<MyDL::Affine>(W1, b1);
        shared_ptr<BaseLayer> affine2 = make_shared<MyDL::Affine>(W2, b2);
        shared_ptr<BaseLayer> relu1 = make_shared<ReLU>();
        shared_ptr<BaseLayer> last_layer = make_shared<SoftmaxWithLoss>(); // predictではaffine2の出力、lossではlastlayerの出力を使うので分ける

        // 生のポインタを格納すると実体がスコープ外となり解放されてしまうので、shared_ptrで対処
        _layers["Affine1"] = affine1;
        _layers["ReLU1"] = relu1;
        _layers["Affine2"] = affine2;
        _last_layer = last_layer;

        // 各パラメータへのポインタを格納
        if (auto cast_affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            params["W1"] = cast_affine1->pW;
            params["b1"] = cast_affine1->pb;
        }
        if (auto cast_affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            params["W2"] = cast_affine2->pW;
            params["b2"] = cast_affine2->pb;
        }

        // unordered_mapでは追加順が保存されないので、別途順番通り名称を格納したコンテナを用意
        _layer_list.push_back("Affine1");
        // _layer_list.push_back("BatchNorm"); // for batchnorm debug 21/03/21追加
        _layer_list.push_back("ReLU1");
        _layer_list.push_back("Affine2");

        // ------------------------------------------------
        // for batchnorm debug 21/03/21追加
        // ------------------------------------------------
        // auto gamma = make_shared<MatrixXd>(1, hidden_size);
        // auto beta = make_shared<MatrixXd>(1, hidden_size);
        // *gamma = weight_init_std * MatrixXd::Random(1, hidden_size);
        // *beta = MatrixXd::Zero(1, hidden_size);
        // shared_ptr<BaseLayer> batch_norm = make_shared<BatchNorm>(gamma, beta);
        // _layers["BatchNorm"] = batch_norm;
        // if (auto cast_batchnorm = std::dynamic_pointer_cast<BatchNorm>(_layers["BatchNorm"]))
        // {
        //     params["gamma"] = cast_batchnorm->pgamma;
        //     params["beta"] = cast_batchnorm->pbeta;
        // }
    }

    vector<MatrixXd> TwoLayerMLP::predict(vector<MatrixXd> inputs)
    {
        // inputのバリデーションをしておくか？
        vector<MatrixXd> X = inputs; // 入力もvectorなので、そのまま受ければOK
        vector<MatrixXd> tmp_X;

        // mapのrange-forは内部的にstd::pairが返される
        for (auto layer : _layer_list)
        {
            // cout << layer << endl;
            tmp_X = _layers[layer]->forward(X);
            X.swap(tmp_X); // 中身入れ替え
        }
        return X;
    }

    vector<MatrixXd> TwoLayerMLP::loss(vector<MatrixXd> inputs, MatrixXd& t)
    {
        vector<MatrixXd> pred_input, pred_out, loss_inputs, loss_output;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        loss_inputs.push_back(pred_out[0]);
        loss_inputs.push_back(t);

        loss_output = _last_layer->forward(loss_inputs);
        return loss_output;
    }

    double TwoLayerMLP::accuracy(vector<MatrixXd> inputs, MatrixXd& t)
    {
        vector<MatrixXd> pred_out;
        pred_out = predict(inputs);

        MatrixXd y;
        y = pred_out[0];
        double batch_size = t.rows();
        double accuracy = 0;

        MatrixXd::Index y_row, y_col, t_row, t_col;
        for (int i = 0; i < batch_size; i++)
        {
            y.row(i).maxCoeff(&y_row, &y_col);
            t.row(i).maxCoeff(&t_row, &t_col);

            accuracy += (double)(y_col == t_col);
        }

        return accuracy / batch_size;
    }

    unordered_map<string, MatrixXd> TwoLayerMLP::gradient(vector<MatrixXd> inputs, MatrixXd& t)
    {
        // Forward
        vector<MatrixXd> output;
        output = loss(inputs, t); // forward -> 逆伝播計算に必要な情報を各レイヤにキャッシュ

        // Backward
        vector<MatrixXd> dout, tmp_dout;
        dout.push_back(MatrixXd::Ones(1, 1));

        dout = _last_layer->backward(dout);

        // 逆順ループ → Boostライブラリのboost::adaptors::reverse()を使う方がEasyではある
        for (auto it = _layer_list.rbegin(); it != _layer_list.rend(); it++)
        {
            string layer = *it;
            tmp_dout = _layers[layer]->backward(dout);
            dout.swap(tmp_dout);
        }

        unordered_map<string, MatrixXd> grads;
        // _layersに格納している変数はBaseLayerにアップキャストしているので、ダウンキャストが必要 -> nullptrのときは実行しないようにする
        if (shared_ptr<MyDL::Affine> affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            grads["W1"] = affine1->dW;
            grads["b1"] = affine1->db;
        }
        if (shared_ptr<MyDL::Affine> affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            grads["W2"] = affine2->dW;
            grads["b2"] = affine2->db;
        }

        // for batchnorm debug 21/03/21追加
        if (auto batchnorm = std::dynamic_pointer_cast<BatchNorm>(_layers["BatchNorm"]))
        {
            grads["gamma"] = batchnorm->dgamma;
            grads["beta"] = batchnorm->dbeta;
        }

        return grads;
    }

    // unordered_map<string, MatrixXd> TwoLayerMLP::numerical_gradient(vector<MatrixXd> inputs)
    // {
    //     // [&]は、スコープ外の変数を参照するというキャプチャー(ここではthisポインタを使うために指定)
    //     std::function<vector<MatrixXd>(MatrixXd)> loss_W = [this, &inputs](MatrixXd W) -> vector<MatrixXd> { return this->loss(inputs); };
    //     std::function<vector<MatrixXd>(VectorXd)> loss_W2 = [this, &inputs](VectorXd W) -> vector<MatrixXd> { return this->loss(inputs); };
    //     unordered_map<string, MatrixXd> grads;

    //     MatrixXd dW1, dW2, db1, db2;

    //     // 直接内部のレイヤのパラメータにアクセスするので、ダウンキャストが必要
    //     if (shared_ptr<MyDL::Affine> affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
    //     {
    //         dW1 = MyDL::numerical_gradient(loss_W, affine1->_W);
    //         db1 = MyDL::numerical_gradient(loss_W2, affine1->_b);
    //     }
    //     if (shared_ptr<MyDL::Affine> affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
    //     {
    //         dW2 = MyDL::numerical_gradient(loss_W, affine2->_W);
    //         db2 = MyDL::numerical_gradient(loss_W2, affine2->_b);
    //     }

    //     grads["dW1"] = dW1;
    //     grads["dW2"] = dW2;
    //     grads["db1"] = db1;
    //     grads["db2"] = db2;

    //     return grads;
    // }

    // -----------------------------------------------------------
    // MultiLayerModel: Weight Decay の検証用
    // 【21/04/06】
    // とりあえず動作することを目指すので、最初はreluで構成
    // 後ほどsigmoid含めて動くように変更。初期値も XavierとHeの両方を選択できるようにする。
    // -----------------------------------------------------------

    MultiLayerModel::MultiLayerModel(const int input_size,
                                     const vector<int> hidden_size,
                                     const int output_size,
                                     const double weight_decay_lambda,
                                     string activation,
                                     const string weight_initializer,
                                     const bool use_dropout,
                                     const double dropout_ratio,
                                     const bool use_batchnorm)
    {
        _input_size = input_size;
        _hidden_size_list = hidden_size;
        _output_size = output_size;
        _weight_decay_lambda = weight_decay_lambda;

        _all_size_list.insert(_all_size_list.end(), {_input_size});
        _all_size_list.insert(_all_size_list.end(), _hidden_size_list.begin(), _hidden_size_list.end());
        _all_size_list.insert(_all_size_list.end(), {_output_size});

        std::transform(activation.begin(), activation.end(), activation.begin(), ::tolower);

        // Create Layers
        for (int i = 1; i < _hidden_size_list.size()+1; i++)
        {
            string tmp_num_str = std::to_string(i);
            _layers["Affine" + tmp_num_str] = make_shared<MyDL::Affine>(_all_size_list[i-1], _all_size_list[i]);
            _layer_list.push_back("Affine" + tmp_num_str);

            // BatchNormalization
            if (use_batchnorm)
            {
                _layers["BatchNorm" + tmp_num_str] = make_shared<BatchNorm>(_all_size_list[i], 0.9); // パラメータも指定できるようにする？
                _layer_list.push_back("BatchNorm" + tmp_num_str);
            }

            // Activation
            if (activation == "relu")
            {
                _layers["ReLU" + tmp_num_str] = make_shared<ReLU>();
                _layer_list.push_back("ReLU" + tmp_num_str);
            }
            else if(activation == "sigmoid")
            {
                _layers["Sigmoid" + tmp_num_str] = make_shared<Sigmoid>();
                _layer_list.push_back("Sigmoid" + tmp_num_str);
            }

            // Dropout
            if (use_dropout)
            {
                _layers["Dropout" + tmp_num_str] = make_shared<Dropout>(dropout_ratio);
                _layer_list.push_back("Dropout" + tmp_num_str);
            }

        }
        int last_num = _hidden_size_list.size()+1;
        string last_num_str = std::to_string(last_num);
        _layers["Affine" + last_num_str] = make_shared<MyDL::Affine>(_all_size_list[last_num-1], _all_size_list[last_num]);
        _layer_list.push_back("Affine" + last_num_str);

        _last_layer = make_shared<SoftmaxWithLoss>(); // Loss Layer

        // Get pointer to Layer Parameters
        for (int layer_num = 1; layer_num < _all_size_list.size(); layer_num++)
        {
            if (auto cast_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine" + std::to_string(layer_num)]))
            {
                params["W" + std::to_string(layer_num)] = cast_affine->pW;
                params["b" + std::to_string(layer_num)] = cast_affine->pb;
            }
        }

        // Affine Layerの変数初期化
        this->_init_weight(weight_initializer);

    }

    void MultiLayerModel::_init_weight(string weight_initializer)
    {
        // weight_initializerの文字列を小文字に変換
        std::transform(weight_initializer.begin(),
                       weight_initializer.end(),
                       weight_initializer.begin(),
                       ::tolower);

        double scale = 1;

        if (weight_initializer == "relu" or weight_initializer == "he")
        {
            for (int layer_num = 1; layer_num <= _hidden_size_list.size()+1; layer_num++)
            {
                scale = sqrt(2.0 / _all_size_list[layer_num-1]);
                *(params["W" + std::to_string(layer_num)]) = scale * MatrixXd::Random(_all_size_list[layer_num-1], _all_size_list[layer_num]);
            }
        }
        else if (weight_initializer == "sigmoid" or weight_initializer == "xavier")
        {
            for (int layer_num = 1; layer_num <= _hidden_size_list.size() + 1; layer_num++)
            {
                scale = sqrt(1.0 / _all_size_list[layer_num - 1]);
                *(params["W" + std::to_string(layer_num)]) = scale * MatrixXd::Random(_all_size_list[layer_num - 1], _all_size_list[layer_num]);
            }
        }
    }

    vector<MatrixXd> MultiLayerModel::predict(vector<MatrixXd> inputs)
    {
        vector<MatrixXd> X = inputs;
        vector<MatrixXd> tmp_X;

        for (auto layer : _layer_list)
        {
            tmp_X = _layers[layer]->forward(X);
            X.swap(tmp_X);
        }
        return X;
    }

    vector<MatrixXd> MultiLayerModel::loss(vector<MatrixXd> inputs, MatrixXd &t)
    {
        vector<MatrixXd> pred_input, pred_out, loss_inputs, loss_output;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        loss_inputs.push_back(pred_out[0]);
        loss_inputs.push_back(t);

        loss_output = _last_layer->forward(loss_inputs);

        // あとは Weight Decay の項も計算してLossに加える
        double weight_decay = 0;
        for (auto param : params)
        {
            weight_decay += 0.5 * _weight_decay_lambda * (*(param.second)).sum();
        }

        loss_output[0](0) = loss_output[0](0) + weight_decay;

        return loss_output;
    }

    double MultiLayerModel::accuracy(vector<MatrixXd> inputs, MatrixXd &t)
    {
        vector<MatrixXd> pred_out;
        pred_out = predict(inputs);

        MatrixXd y;
        y = pred_out[0];
        double batch_size = t.rows();
        double accuracy = 0;

        MatrixXd::Index y_row, y_col, t_row, t_col;
        for (int i = 0; i < batch_size; i++)
        {
            y.row(i).maxCoeff(&y_row, &y_col);
            t.row(i).maxCoeff(&t_row, &t_col);

            accuracy += (double)(y_col == t_col);
        }

        return accuracy / batch_size;
    }

    unordered_map<string, MatrixXd> MultiLayerModel::gradient(vector<MatrixXd> inputs, MatrixXd &t)
    {
        // Forward
        vector<MatrixXd> output;
        output = loss(inputs, t);

        // Backward
        vector<MatrixXd> dout, tmp_dout;
        dout.push_back(MatrixXd::Ones(1, 1));

        dout = _last_layer->backward(dout);

        for (auto it = _layer_list.rbegin(); it != _layer_list.rend(); it++)
        {
            string layer = *it;
            tmp_dout = _layers[layer]->backward(dout);
            dout.swap(tmp_dout);
        }

        unordered_map<string, MatrixXd> grads;

        for (int i = 1; i <= _hidden_size_list.size() + 1; i++)
        {
            if (auto tmp_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine" + std::to_string(i)]))
            {
                grads["W" + std::to_string(i)] = tmp_affine->dW + _weight_decay_lambda * (*(tmp_affine->pW));
                grads["b" + std::to_string(i)] = tmp_affine->db;
            }
        }

        return grads;
    }

    // -----------------------------------------------------------
    // SimpleConvModel: Convolutionの検証用
    // -----------------------------------------------------------

    SimpleConvModel::SimpleConvModel(const int input_channels,
                                     const int input_height,
                                     const int input_width,
                                     const int filter_num,
                                     const int filter_size,
                                     const int pad,
                                     const int stride,
                                     const int hidden_size,
                                     const int output_size,
                                     const double weight_init_std)
    : _C(input_channels), _H(input_height), _W(input_width), _filter_num(filter_num), _filter_size(filter_size), _pad(pad), _stride(stride), _hidden_size(hidden_size), _output_size(output_size)
    {
        int Oh = (_pad*2 + _H - _filter_size) / _stride + 1;
        int Ow = (_pad*2 + _W - _filter_size) / _stride + 1;
        int Ph = 2;
        int Pw = 2; // Poolingのサイズは固定
        int p_stride = 2;
        int p_pad = 0;

        int pool_output_size = _filter_num * (Oh / 2) * (Ow / 2);

        _layers["Conv1"] = make_shared<Conv2D>(_C, _H, _W, _filter_size, _filter_size, _filter_num, _stride, _pad, weight_init_std);
        _layers["Relu1"] = make_shared<ReLU>();
        _layers["Pool1"] = make_shared<Pooling>(_filter_num, Oh, Ow, Ph, Pw, p_stride, p_pad);
        _layers["Affine1"] = make_shared<MyDL::Affine>(pool_output_size, _hidden_size, weight_init_std);
        _layers["Affine2"] = make_shared<MyDL::Affine>(hidden_size, output_size, weight_init_std);

        _last_layer = make_shared<SoftmaxWithLoss>();

        _layer_list.push_back("Conv1");
        _layer_list.push_back("Relu1");
        _layer_list.push_back("Pool1");
        _layer_list.push_back("Affine1");
        _layer_list.push_back("Affine2");

        if (auto cast_conv = std::dynamic_pointer_cast<Conv2D>(_layers["Conv1"]))
        {
            params["W1"] = cast_conv->pW;
            params["b1"] = cast_conv->pb;
        }

        if (auto cast_affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            params["W2"] = cast_affine1->pW;
            params["b2"] = cast_affine1->pb;
        }

        if (auto cast_affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            params["W3"] = cast_affine2->pW;
            params["b3"] = cast_affine2->pb;
        }

    }

    vector<MatrixXd> SimpleConvModel::predict(vector<MatrixXd> inputs)
    {
        vector<MatrixXd> X = inputs;
        vector<MatrixXd> tmp_X;

        for (auto layer : _layer_list)
        {
            tmp_X = _layers[layer]->forward(X);
            X.swap(tmp_X);
        }
        return X;
    }

    vector<MatrixXd> SimpleConvModel::loss(vector<MatrixXd> inputs, MatrixXd &t)
    {
        vector<MatrixXd> pred_input, pred_out, loss_inputs, loss_output;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        loss_inputs.push_back(pred_out[0]);
        loss_inputs.push_back(t);

        loss_output = _last_layer->forward(loss_inputs);

        return loss_output;
    }

    double SimpleConvModel::accuracy(vector<MatrixXd> inputs, MatrixXd &t)
    {
        vector<MatrixXd> pred_out;
        pred_out = predict(inputs);

        MatrixXd y;
        y = pred_out[0];
        double batch_size = t.rows();
        double accuracy = 0;

        MatrixXd::Index y_row, y_col, t_row, t_col;
        for (int i = 0; i < batch_size; i++)
        {
            y.row(i).maxCoeff(&y_row, &y_col);
            t.row(i).maxCoeff(&t_row, &t_col);

            accuracy += (double)(y_col == t_col);
        }

        return accuracy / batch_size;
    }

    unordered_map<string, MatrixXd> SimpleConvModel::gradient(vector<MatrixXd> inputs, MatrixXd &t)
    {
        // Forward
        vector<MatrixXd> output;
        output = loss(inputs, t);

        // Backward
        vector<MatrixXd> dout, tmp_dout;
        dout.push_back(MatrixXd::Ones(1, 1));

        dout = _last_layer->backward(dout);

        for (auto it = _layer_list.rbegin(); it != _layer_list.rend(); it++)
        {
            string layer = *it;
            tmp_dout = _layers[layer]->backward(dout);
            dout.swap(tmp_dout);
        }

        unordered_map<string, MatrixXd> grads;

        if (auto cast_conv = std::dynamic_pointer_cast<Conv2D>(_layers["Conv1"]))
        {
            grads["W1"] = cast_conv->dW;
            grads["b1"] = cast_conv->db;
        }

        if (auto cast_affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            grads["W2"] = cast_affine1->dW;
            grads["b2"] = cast_affine1->db;
        }

        if (auto cast_affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            grads["W3"] = cast_affine2->dW;
            grads["b3"] = cast_affine2->db;
        }

        return grads;
    }
}


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [direct_trigonometric]
#include <boost/simd/trigonometric.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <boost/simd/constant/pi.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 8>;

int main()
{
  pack_ft p_pi = bs::enumerate<pack_ft>(-1.0f, 0.25f);
  pack_ft p_ra = p_pi*bs::Pi<pack_ft>();
  pack_ft p_dg = bs::indeg(p_ra);
  std::cout << " p_ra =  " << p_ra << std::endl
            << " p_pi =  " << p_pi << std::endl
            << " p_dg =  " << p_dg << std::endl
            << " -> bs::cos(p_ra) =   " << bs::cos(p_ra)   << std::endl
            << " -> bs::cospi(p_pi) = " << bs::cospi(p_pi) << std::endl
            << " -> bs::cosd(p_dg) =  " << bs::cosd(p_dg)  << std::endl;
  return 0;
}
//! [direct_trigonometric]


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

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

 * See LICENSE for the license information

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

/**
 * @file    Rot3.cpp
 * @brief   Rotation, common code between Rotation matrix and Quaternion
 * @author  Alireza Fathi
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @author  Richard Roberts
 */

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>

#include <cmath>
#include <random>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
  gtsam::print((Matrix)matrix(), s);
}

/* ************************************************************************* */
Rot3 Rot3::Random(std::mt19937& rng) {
  // TODO allow any engine without including all of boost :-(
  Unit3 axis = Unit3::Random(rng);
  uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
  double angle = randomAngle(rng);
  return AxisAngle(axis, angle);
}

/* ************************************************************************* */
Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) {
  // if a_p is already aligned with b_p, return the identity rotation
  if (std::abs(a_p.dot(b_p)) > 0.999999999) {
    return Rot3();
  }

  // Check axis was not degenerate cross product
  const Vector3 z = axis.unitVector();
  if (z.hasNaN())
    throw std::runtime_error("AlignSinglePair: axis has Nans");

  // Now, calculate rotation that takes b_p to a_p
  const Matrix3 P = I_3x3 - z * z.transpose();  // orthogonal projector
  const Vector3 a_po = P * a_p.unitVector();    // point in a orthogonal to axis
  const Vector3 b_po = P * b_p.unitVector();    // point in b orthogonal to axis
  const Vector3 x = a_po.normalized();          // x-axis in axis-orthogonal plane, along a_p vector
  const Vector3 y = z.cross(x);                 // y-axis in axis-orthogonal plane
  const double u = x.dot(b_po);                 // x-coordinate for b_po
  const double v = y.dot(b_po);                 // y-coordinate for b_po
  double angle = std::atan2(v, u);
  return Rot3::AxisAngle(z, -angle);
}

/* ************************************************************************* */
Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p,  //
                         const Unit3& a_q, const Unit3& b_q) {
  // there are three frames in play:
  // a: the first frame in which p and q are measured
  // b: the second frame in which p and q are measured
  // i: intermediate, after aligning first pair

  // First, find rotation around that aligns a_p and b_p
  Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p);

  // Rotate points in frame b to the intermediate frame,
  // in which we expect the point p to be aligned now
  Unit3 i_q = i_R_b * b_q;
  assert(assert_equal(a_p, i_R_b * b_p, 1e-6));

  // Now align second pair: we need to align i_q to a_q
  Rot3 a_R_i = AlignPair(a_p, a_q, i_q);
  assert(assert_equal(a_p, a_R_i * a_p, 1e-6));
  assert(assert_equal(a_q, a_R_i * i_q, 1e-6));

  // The desired rotation is the product of both
  Rot3 a_R_b = a_R_i * i_R_b;
  return a_R_b;
}

/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
  return equal_with_abs_tol(matrix(), R.matrix(), tol);
}

/* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
  Matrix32 Dp;
  Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
  if (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
  if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
  Matrix32 Dp;
  Unit3 q = Unit3(unrotate(p.point3(Dp)));
  if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
  if (HR) *HR = q.basis().transpose() * q.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::operator*(const Unit3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
    OptionalJacobian<3,3> H2) const {
  const Matrix3& Rt = transpose();
  Point3 q(Rt * p); // q = Rt*p
  const double wx = q.x(), wy = q.y(), wz = q.z();
  if (H1)
    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
  if (H2)
    *H2 = Rt;
  return q;
}

/* ************************************************************************* */
Point3 Rot3::column(int index) const{
  if(index == 3)
    return r3();
  else if(index == 2)
    return r2();
  else if(index == 1)
    return r1(); // default returns r1
  else
    throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}

/* ************************************************************************* */
Vector3 Rot3::xyz() const {
  Matrix3 I;Vector3 q;
  boost::tie(I,q)=RQ(matrix());
  return q;
}

/* ************************************************************************* */
Vector3 Rot3::ypr() const {
  Vector3 q = xyz();
  return Vector3(q(2),q(1),q(0));
}

/* ************************************************************************* */
Vector3 Rot3::rpy() const {
  return xyz();
}

/* ************************************************************************* */
Vector Rot3::quaternion() const {
  gtsam::Quaternion q = toQuaternion();
  Vector v(4);
  v(0) = q.w();
  v(1) = q.x();
  v(2) = q.y();
  v(3) = q.z();
  return v;
}

/* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
  return SO3::ExpmapDerivative(x);
}

/* ************************************************************************* */
Matrix3 Rot3::LogmapDerivative(const Vector3& x)    {
  return SO3::LogmapDerivative(x);
}

/* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) {

  double x = -atan2(-A(2, 1), A(2, 2));
  Rot3 Qx = Rot3::Rx(-x);
  Matrix3 B = A * Qx.matrix();

  double y = -atan2(B(2, 0), B(2, 2));
  Rot3 Qy = Rot3::Ry(-y);
  Matrix3 C = B * Qy.matrix();

  double z = -atan2(-C(1, 0), C(1, 1));
  Rot3 Qz = Rot3::Rz(-z);
  Matrix3 R = C * Qz.matrix();

  Vector xyz = Vector3(x, y, z);
  return make_pair(R, xyz);
}

/* ************************************************************************* */
ostream &operator<<(ostream &os, const Rot3& R) {
  os << "\n";
  os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
  os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
  os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
  return os;
}

/* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const {
  return interpolate(*this, other, t);
}

/* ************************************************************************* */

} // namespace gtsam


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>
#include <iostream>

#include <tdp/eigen/std_vector.h>

namespace tdp {
/// This is a simple version of the DPvMFmeansSimple algorithm in dpmeans.hpp
/// without inheritance or the use of CLData structures which can make
/// it a bit hard to read the other algorithm.
///
/// This implementation is ment as a lightweight alternative for small
/// number of datapoints or if you just want to have a look at how the
/// algorithm works.
template<class T, int D, int Options>
class DPvMFmeansSimple
{
public:
  /// Constructor
  ///
  /// lambda = cos(lambda_in_degree * M_PI/180.) - 1.
  DPvMFmeansSimple(T lambda);
  virtual ~DPvMFmeansSimple();

  /// Adds an observation (adds obs, computes label, and potentially
  /// adds new cluster depending on label assignment).
  virtual void addObservation(Eigen::Matrix<T,D,1,Options>* x, uint16_t* z);
  /// Updates all labels of all data currently stored with the object.
  virtual void updateLabels();
  /// Updates all centers based on the current data and label
  /// assignments.
  virtual void updateCenters();

  /// Iterate updates for centers and labels until cost function
  /// convergence.
  virtual bool iterateToConvergence(uint32_t maxIter, T eps);
  /// Compuyte the current cost function value.
  virtual T cost();

  uint32_t GetK() const {return K_;};
  const std::vector<int32_t>& GetNs() const {return Ns_;};
  bool GetCenter(uint32_t k, Eigen::Matrix<T,D,1,Options>& mu) const {
    if (k<K_) {mu = mus_[k]; return true; } else { return false; } };
  const Eigen::Matrix<T,D,1,Options>& GetCenter(uint32_t k) const {
    if (k<K_) {return mus_[k]; } else { return Eigen::Matrix<T,D,1,Options>::Zero(); } };
  const std::vector<uint16_t*>& GetZs() const { return zs_;};
  bool GetX(uint32_t i, Eigen::Matrix<T,D,1,Options>& x) const {
    if (i<xs_.size()) {x=*xs_[i]; return true; } else { return false; } };

protected:
  T lambda_;
  uint32_t K_;
  std::vector<Eigen::Matrix<T,D,1,Options>*> xs_;
  std::vector<uint16_t*> zs_;
  eigen_vector<Eigen::Matrix<T,D,1,Options>> mus_;
  eigen_vector<Eigen::Matrix<T,D,1,Options>> xSums_;
  std::vector<int32_t> Ns_;

  /// resets all clusters (mus_ and Ks_) and resizes them to K_
  void resetClusters();
  /// Removes all empty clusters.
  void removeEmptyClusters();
  /// Computes the index of the closest cluster (may be K_ in which
  /// case a new cluster has to be added).
  uint16_t indOfClosestCluster(const Eigen::Matrix<T,D,1,Options>& x, T& sim_closest,
      uint16_t* zExclude=nullptr);
};

typedef DPvMFmeansSimple<float,3,Eigen::DontAlign> DPvMFmeansSimple3fda;
typedef DPvMFmeansSimple<float,4,Eigen::DontAlign> DPvMFmeansSimple4fda;

// -------------------------------- impl ----------------------------------
template<class T, int D, int Options>
DPvMFmeansSimple<T,D,Options>::DPvMFmeansSimple(T lambda)
  : lambda_(lambda), K_(0)
{}
template<class T, int D, int Options>
DPvMFmeansSimple<T,D,Options>::~DPvMFmeansSimple()
{}

template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::addObservation(Eigen::Matrix<T,D,1,Options>* x, uint16_t* z) {
  xs_.push_back(x);
  T sim_closest = 0;
  zs_.push_back(z);
//  if (*z == 0xFFFF) return;
  *z = indOfClosestCluster(*x, sim_closest);
  if (*z == K_) {
    mus_.push_back(*x);
    xSums_.push_back(*x);
    Ns_.push_back(0);
    ++K_;
//    std::cout << "adding cluster " << mus_.size() << " "
//      << xSums_.size() << " " << K_ << " "
//      << x.transpose() << std::endl;
  }
  Ns_[*z] ++;
};

template<class T, int D, int Options>
uint16_t DPvMFmeansSimple<T,D,Options>::indOfClosestCluster(const
    Eigen::Matrix<T,D,1,Options>& x, T& sim_closest, uint16_t* zExclude)
{
  uint16_t z_i = K_;
  sim_closest = lambda_;
  for (uint32_t k=0; k<K_; ++k) {
    if (zExclude && k == *zExclude)
      continue;
    T sim_k = mus_[k].dot(x);
    if(sim_k > sim_closest) {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  return z_i;
};

template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::updateLabels()
{
  if (xs_.size() == 0) return;
  for(uint32_t i=0; i<xs_.size(); ++i) {
    T sim_closest = 0;
    uint16_t zPrev = *zs_[i];
//    if (zPrev == 0xFFFF) continue;
    uint16_t z = indOfClosestCluster(*xs_[i], sim_closest);
    if (z==zPrev && Ns_[z] == 1) {
      z = indOfClosestCluster(*xs_[i], sim_closest, &z);
//      std::cout << "single cluster " << z << " " << zPrev << std::endl;
    }
    if (z == K_) {
//      std::cout << "adding cluster " << mus_.size() << " "
//        << xSums_.size() << " " << K_ << " "
//        << xs_[i].transpose() << std::endl;
      mus_.push_back(*xs_[i]);
      xSums_.push_back(Eigen::Matrix<T,D,1,Options>::Zero());
      Ns_.push_back(0);
      ++K_;
    }
    if (z != zPrev) {
      Ns_[zPrev] --;
      xSums_[zPrev] -= *xs_[i];
      Ns_[z] ++;
      xSums_[z] += *xs_[i];
    }
    *zs_[i] = z;
  }
};

// General update centers assumes Euclidean
template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::updateCenters()
{
  if (xs_.size() == 0) return;
//  resetClusters();
//  for(uint32_t i=0; i<xs_.size(); ++i) {
//    ++Ns_[zs_[i]];
//    mus_[zs_[i]] += xs_[i];
//  }
  // Spherical mean computation
  for(uint32_t k=0; k<K_; ++k) {
//    mus_[k] /= mus_[k].norm();
    mus_[k] = xSums_[k].normalized();
  }
  removeEmptyClusters();
};

template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::resetClusters() {
  Ns_.resize(K_, 0);
  for(uint32_t k=0; k<K_; ++k) {
    mus_[k].fill(0);
    Ns_[k] = 0;
  }
};

template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::removeEmptyClusters() {
  if (K_ < 1) return;
  uint32_t kNew = K_;
  std::vector<bool> toDelete(K_,false);
  for(int32_t k=K_-1; k>-1; --k)
    if(Ns_[k] == 0) {
      toDelete[k] = true;
//      std::cout<<"cluster k "<<k<<" empty"<<std::endl;
//#pragma omp parallel for
      for(uint32_t i=0; i<xs_.size(); ++i)
//        if(static_cast<int32_t>(*zs_[i]) >= k && *zs_[i] != 0xFFFF) *zs_[i] -= 1;
        if(static_cast<int32_t>(*zs_[i]) >= k) *zs_[i] -= 1;
      kNew --;
    }
  uint32_t j=0;
  for(uint32_t k=0; k<K_; ++k) {
    mus_[j] = mus_[k];
    xSums_[j] = xSums_[k];
    Ns_[j] = Ns_[k];
    if(!toDelete[k]) {
      ++j;
    }
  }
//  std::cout << "K " << K_ << " -> " << kNew << std::endl;
  K_ = kNew;
  Ns_.resize(K_);
  mus_.resize(K_);
  xSums_.resize(K_);
//  for(uint32_t k=0; k<K_; ++k)
//    std::cout << mus_[k].transpose() << std::endl;
};

template<class T, int D, int Options>
T DPvMFmeansSimple<T,D,Options>::cost() {
  T f = lambda_*K_;
//  std::cout << "f="<<f<< std::endl;
  for(uint32_t i=0; i<xs_.size(); ++i)  {
//    if (*zs_[i] == 0xFFFF) continue;
    f += mus_[*zs_[i]].dot(*xs_[i]);
//    std::cout << zs_[i] << ", " << xs_[i].transpose() << ", "
//      << mus_[zs_[i]].transpose();
//    std::cout << " f="<<f<< std::endl;
  }
  return f;
}

template<class T, int D, int Options>
bool DPvMFmeansSimple<T,D,Options>::iterateToConvergence(uint32_t maxIter, T eps) {
  uint32_t iter = 0;
  T fPrev = 1e9;
  T f = cost();
  updateCenters();
//  std::cout << "f=" << f << " fPrev=" << fPrev << std::endl;
  while (iter < maxIter && fabs(fPrev - f)/f > eps) {
    updateLabels();
    updateCenters();
    fPrev = f;
    f = cost();
    ++iter;
//    std::cout << iter << ": f=" << f << " fPrev=" << fPrev << ": ";
//    int32_t Nall = 0;
//    for (const auto& N : Ns_) {
//      std::cout << N << " ";
//      Nall += N;
//    } std::cout << " sum= " << Nall << std::endl;
  }
//  if (f != f || fPrev != fPrev || f > 1e9 || iter == maxIter-1) {
  std::cout << iter << ": f=" << f << " fPrev=" << fPrev << ": ";
  int32_t Nall = 0;
  for (const auto& N : Ns_) {
    std::cout << N << " ";
    Nall += N;
  } std::cout << " sum= " << Nall << std::endl;
//  }
  return iter < maxIter;
}

}


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

#include "utils/base_classes.h"
#include "utils/math.h"
#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>

using namespace cv;

boost::minstd_rand gIntGenStd;
boost::mt19937 gIntGen19937;
boost::variate_generator<boost::mt19937, boost::normal_distribution<> > gNormal(gIntGen19937, boost::normal_distribution<>(0,1));
boost::uniform_01<boost::minstd_rand> gUniform(gIntGenStd);

double  Distributions::normalDist() {
    return gNormal();
}
double  Distributions::normalDist(double sigma) {
    return sigma * gNormal();
}
double  Distributions::normalDist(double mean, double sigma) {
    return mean + sigma * gNormal();
}
void Distributions::normalDist(cv::Vec<double,3> &mean, cv::Vec<double,3> sigma) {
    mean[0] = normalDist(mean[0], sigma[0]);
    mean[1] = normalDist(mean[1], sigma[1]);
    mean[2] = normalDist(mean[2], sigma[2]);
}
double Distributions::uniformDist(double min, double max){
    double s = max - min;
    return min + s * gUniform();
}
void Distributions::uniformDist(double min, double max, cv::Vec<double,3> &des){
    des[0] = uniformDist(min, max);
    des[1] = uniformDist(min, max);
    des[2] = uniformDist(min, max);
}

void Gauss::add_w_sample(double val, double w){
    entry_no++;
    double temp  = w + sumweight;
    double delta = val - mean;
    double r = delta * w /temp;
    mean += r;
    m2 += sumweight * delta * r;
    sumweight = temp;
}

double Gauss::getMean(){
    return mean;
}
double Gauss::getVariance(){
    if(entry_no<=1){
        //std::cout<<"WARNING: requesting variance of less than 2 entries"<<std::endl;
        return 0;
    }
    return (m2/sumweight)* (double)entry_no/((double)entry_no - 1.0);
}
double Gauss::getSampleVariance(){
    return m2/sumweight;
}

void NormalFunc::set(double _mean, double _deviation, double _no_sigma_dev){
    mean = _mean;
    sigma = _deviation / _no_sigma_dev;
}

double NormalFunc::f(double x){
    return std::exp( - sqr( x - mean ) / ( 2.0 * sqr( sigma ) ) );
}

///------------------------------------------------------------------------------------------------------------------------------------------------///

Line get_line_param(xy p1,xy p2){
    double a=p2.y-p1.y;
    double b=p1.x-p2.x;
    double c=-(a*p2.x+b*p2.y);

    return Line(a,b,c);
}
xy get_line_inters(Line l,xy p){
    return xy( (l.b*(l.b*p.x-l.a*p.y)-l.a*l.c)/(sqr(l.a)+sqr(l.b)), (l.a*(-l.b*p.x+l.a*p.y)-l.b*l.c)/(sqr(l.a)+sqr(l.b)));
}

double get_dist_p(Line l,xy p0, xy* p_d){
    *p_d = xy((l.b * (   l.b * p0.x - l.a * p0.y) - l.a * l.c) / (sqr(l.a) + sqr(l.b)),
              (l.a * ( - l.b * p0.x + l.a * p0.y) - l.b * l.c) / (sqr(l.a) + sqr(l.b)));
    return fabs(l.a * p0.x + l.b * p0.y + l.c) / sqrt(sqr(l.a) + sqr(l.b));
}

xy get_line_inters(Line l1,Line l2){
    double x=10000,y=10000;
    if(l1.a*l2.b!=l1.b*l2.a){
        x=(l2.b*l1.c-l2.c*l1.b)/(l2.a*l1.b-l1.a*l2.b);
        y=(l2.c*l1.a-l1.c*l2.a)/(l2.a*l1.b-l1.a*l2.b);
        return xy(x,y);
    }
    else if((l1.a==l2.a)&&(l1.b==l2.b)&&(l1.c==l2.c)){
        std::cout<<"paralel overlapping lines when computing get_line_intersection"<<std::endl;
    }
    else{
        std::cout<<"paralel NONoverlapping lines when computing get_line_intersection"<<std::endl;
    }
    return xy(0,0);
}

///------------------------------------------------------------------------------------------------------------------------------------------------///

polar::polar() {
    r=0;angle=0;
}
polar::polar(double _r, double _angle) {
    r=_r;angle=_angle;
}
polar::polar(const polar &_p){
    r=_p.r;angle=_p.angle;
}
polar::polar(PointData &_p){
    r=_p.r;angle=_p.angle;
}

polar polar_diff(polar p1,polar p2){
    return polar(sqrt(sqr(p2.r*cos(p2.angle)-p1.r*cos(p1.angle))+sqr(p2.r*sin(p2.angle)-p1.r*sin(p1.angle))),
                 atan2(p2.r*sin(p2.angle)-p1.r*sin(p1.angle),p2.r*cos(p2.angle)-p1.r*cos(p1.angle)));
}

polar polar_diff(polar p1,PointData &p2){
    return polar_diff(p1,polar(p2.r,p2.angle));
}
polar polar_diff(PointData &p1,polar p2){
    return polar_diff(polar(p1.r,p1.angle),p2);
}

polar polar_diff(PointData p1,PointData &p2){
    return polar_diff(polar(p1.r,p1.angle),polar(p2.r,p2.angle));
}

polar radial_diff(polar p1,polar p2){
    double d_angle=fabs(p1.angle-p2.angle);
    normalizeAngle(d_angle);
    return polar(d_angle*p1.r,d_angle);
}
polar radial_diff(polar p1,PointData &p2){
    return radial_diff(p1,polar(p2.r,p2.angle));
}

xy to_xy(polar p){
    return xy(p.r*cos(p.angle),p.r*sin(p.angle));
}
xy to_xy(PointData &p){
    return to_xy(polar(p.r,p.angle));
}

polar to_polar(xy c){
    return polar(sqrt(c.x*c.x+c.y*c.y),atan2(c.y,c.x));
}

///------------------------------------------------------------------------------------------------------------------------------------------------///

double normalizeAngle(double &angle) {
    while(angle > M_PI){
      angle = angle - (2*M_PI);
    }
    while(angle <= -M_PI){
      angle = angle + (2*M_PI);
    }
    return angle;
}

double deg_to_rad(double angle_deg){
    return (angle_deg*M_PI)/180.0;
}

double rad_to_deg(double angle_rad){
    return (angle_rad*180.0)/(double)M_PI;
}

double sqr(double x){
    return x*x;
}

double sgn(double x){
    if(x>=0){
        return 1;
    }
    else{
        return -1;
    }
}

void angular_bounds(polar p, double circle_rad, double* search_angle){
    search_angle[0] = p.angle;
    search_angle[1] = search_angle[0];
    if( fabs( circle_rad / p.r ) > 1.0  ){
        search_angle[0] -= 2 * M_PI;
        search_angle[1] += 2 * M_PI;
    }
    else{
        search_angle[0] -= asin( circle_rad / p.r );
        search_angle[1] += asin( circle_rad / p.r );
    }
}
void angular_bounds(PointData &p,double circle_rad, double* search_angle){
    return angular_bounds(polar(p.r,p.angle),circle_rad,search_angle);
}

void angular_bounds(PointDataCpy &p,double circle_rad, double* search_angle){
    return angular_bounds(polar(p.r,p.angle),circle_rad,search_angle);
}

///------------------------------------------------------------------------------------------------------------------------------------------------///

void set_tf_mat(cv::Matx<double,3,3>& tf, xy trans, double rot){
    cv::Matx<double,3,3> R ( cos(rot), -sin(rot),   0,
                             sin(rot),  cos(rot),   0,
                                      0,           0,   1);    // Rotate
    cv::Matx<double,3,3> T (          1,           0, trans.x,
                                      0,           1, trans.y,
                                      0,           0,   1);    // Translate
    // Calculate final transformation matrix
    tf = T * R;
    //Mptf = Mptf.inv();
    //std::cout <<  "Ms2r = " << std::endl << Ms2r << std::endl;
    //std::cout <<  "Mr2s = " << std::endl << Mr2s << std::endl;
}

xy   mat_mult(cv::Matx<double,3,3>& tf,xy p){
    cv::Matx<double,3,1> pw (p.x, p.y, 1.0);
    cv::Matx<double,3,1> pi = tf * pw;
    return  xy (pi(0,0), pi(1,0));
}

cv::RotatedRect cov2rect(cv::Matx<double, 2, 2> _C,xy _center){
    cv::RotatedRect ellipse;
    cv::Mat_<double> eigval, eigvec;
    cv::eigen(_C, eigval, eigvec);

    /// Exercise4
    bool index_x;
    ellipse.center = _center;
    if(_C(0,0)>_C(1,1)){
        index_x=0;
    }
    else{
        index_x=1;
    }
    ellipse.size.height=sqrt(fabs(eigval(0,!index_x)))*2.4477;//y
    ellipse.size.width=sqrt(fabs(eigval(0,index_x)))*2.4477;//x
    if((eigval(0,index_x)!=0)&&(eigval(0,!index_x)!=0))
        ellipse.angle=atan2(eigvec(index_x,1),eigvec(index_x,0))*(180/M_PI);

    return ellipse;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <array>
#include <unordered_set>
#include <unordered_map>
#include <boost/format.hpp>
#include "decision_tree.h"
#include "utils.h"

using namespace std;
using namespace Eigen;

DecisionTree::DecisionTree(int min_samples_split, double min_impurity, int max_depth) : min_samples_split(min_samples_split), min_impurity(min_impurity), max_depth(max_depth) {}

DecisionTree::~DecisionTree() {}

void DecisionTree::fit(const MatrixXd& X, const VectorXd& y){
	root = build_tree(X, y, 0);
}

DecisionNode* DecisionTree::build_tree(const MatrixXd& X, const VectorXd& y, int current_depth){
	double largest_impurity = 0.0;
	//cout << "start..." << endl;
	MatrixXd Xy = MatrixXd(X.rows(), X.cols()+1);
	Xy << X, y;
	//cout << "its ok?"<<endl;
	//cout << "..............." <<endl;
	//cout << "X.rows: " << X.rows() << "  X.cols:" << X.cols() << " y.size:" << y.size() << "  ..Xy.rows:" << Xy.rows() << "..Xy.cols:" << Xy.cols() << endl;
	int n_samples = X.rows();
	int n_features = X.cols();

	array<double, 2> best_criteria;
	array<MatrixXd, 4> best_sets;

	if(n_samples >= min_samples_split && current_depth < max_depth){
		// calculate the impurity for each feature
		for(int i = 0; i < n_features; i++){
			// all values of feature index i
			VectorXd feature_values = X.col(i);
			unordered_set<double> unique_values;
			for(int j = 0; j < feature_values.size(); j++){
				unique_values.insert(feature_values[j]);
			}

			// iterate through all unique values of feature column i and
			// calculate the impurity
			for(auto p = unique_values.begin(); p != unique_values.end(); p++){
				//cout << "start div..." << endl;
				array<MatrixXd,2> X_div = Utils::divide_on_feature(Xy, i, *p);
				MatrixXd Xy1 = X_div[0];
				MatrixXd Xy2 = X_div[1];
				//cout << "Xy1: " << Xy1.rows() << ".." << Xy1.cols() << endl;
				//cout << "Xy2: " << Xy2.rows() << ".." << Xy2.cols() << endl;

				if(Xy1.rows() > 0 && Xy2.rows() > 0){
					// select the y_values of the two sets
					VectorXd y1 = Xy1.col(Xy1.cols()-1);
					VectorXd y2 = Xy2.col(Xy2.cols()-1);
					//cout << "y1:" << y1.size() << "y2: " << y2.size() << endl;
					// calculate impurity
					double impurity = impurity_calculation(y, y1, y2);
					//cout << "impurity done " << impurity << endl;
					// if this threshold resulted in a higher information gain than previously
					// recorded save the threshod value and the feature
					if(impurity > largest_impurity){
						largest_impurity = impurity;
						best_criteria[0] = i;
						best_criteria[1] = *p;
						//cout << "best_criteria " << best_criteria[0] << " " << best_criteria[1] << endl;

						//MatrixXd Xy1_feature = Xy1.leftCols(Xy1.cols()-1);
						//MatrixXd Xy2_feature = Xy2.leftCols(Xy2.cols()-1);

						MatrixXd Xy1_feature(Xy1.rows(), Xy1.cols()-1), Xy2_feature(Xy2.rows(), Xy2.cols()-1);
						for(int k = 0; k < Xy1.cols()-1; k++){
							Xy1_feature.col(k) = Xy1.col(k);
						}
						for(int k = 0; k < Xy2.cols()-1; k++){
							Xy2_feature.col(k) = Xy2.col(k);
						}
						best_sets[0] = Xy1_feature;
						best_sets[1] = y1;
						best_sets[2] = Xy2_feature;
						best_sets[3] = y2;
					}
				}
			}
		}
	}
	//cout << "bulid tree next" << endl;
	if(largest_impurity > min_impurity){
		DecisionNode* true_branch = build_tree(best_sets[0], best_sets[1], current_depth+1);
		DecisionNode* false_branch = build_tree(best_sets[2], best_sets[3], current_depth+1);
		return new DecisionNode(static_cast<int>(best_criteria[0]), best_criteria[1], -1, true_branch, false_branch);
	}
	// we're at leaf ==> determind value
	double leaf_value = leaf_value_calculation(y);
	cout << "leaf value: " << leaf_value << endl;
	return new DecisionNode(leaf_value);
}

int DecisionTree::predict(const VectorXd& X, DecisionNode* r){
	if(r == nullptr){
		r = root;
	}
	if(r->value != -1){
		return r->value;
	}
	//cout << "r->feature:" << r->feature << endl;
	double feature_value = X(r->feature);
	//cout << "nani ..." << endl;
	DecisionNode* branch = r->false_branch;
	if(feature_value >= r->threshold){
		branch = r->true_branch;
	}
	return predict(X, branch);
}

VectorXi DecisionTree::predict(const MatrixXd& X){
	VectorXi ret(X.rows());
	for(int i = 0; i < X.rows(); i++){
		ret(i) = predict(X.row(i), root);
	}
	return ret;
}

double DecisionTree::impurity_calculation(const VectorXd& y, const VectorXd& y1, const VectorXd& y2){
	// calculate information gain
	double prob = y1.size()/double(y2.size());
	double entropy = Utils::calculate_entropy(y);
	double info_gain = entropy - prob*Utils::calculate_entropy(y1) - (1-prob)*Utils::calculate_entropy(y2);
	return info_gain;
}

int DecisionTree::leaf_value_calculation(const VectorXd& y){
	int most_common;
	int max_count = 0;
	unordered_map<int,int> m;
	for(int i = 0; i < y.size(); i++){
		m[y(i)]++;
	}
	for(auto p = m.begin(); p != m.end(); p++){
		if(p->second > max_count){
			most_common = p->first;
			max_count = p->second;
		}
	}
	return most_common;
}


#include <cmath>
#include <cstring>
#include <cassert>
#include <complex>
#include <filesystem>

#include <signal.h>
#include <boost/fiber/buffered_channel.hpp>

#include <scluk/language_extension.hpp>
#include <scluk/math.hpp>
#include <scluk/array.hpp>
#include <scluk/functional.hpp>
#include <scluk/sliding_queue.hpp>

#include "dft/sliding_dft.hpp"
#include "sdl_gui_thread.hpp"
#include "audio_params.hpp"

int main() {
    using namespace scluk::language_extension;

    std::filesystem::current_path(EXECUTABLE_DIR);

    sdl_gui_thread gui_thread;

	//interrupt signal handling
    signal(SIGINT, scluk::lambda_to_fnptr<void(int)>([&gui_thread](int) {
        out("caught sigint!");
        gui_thread.data.do_exit = true;
    }));

    audio::sliding_dft dft;
    audio::duplex_chan cb_chan;
    portaudio::async_stream stream({ .frames_per_buffer=audio::ft_dist, .rate=audio::rate, .log=EXECUTABLE_DIR"/log.txt" }, audio::cb, cb_chan);

    scluk::sliding_queue<audio::ift_chunk, audio::ift_overlap> ift_queue(audio::ift_chunk(0.f));
    //first iteration, just to populate the arrays
    dft.push_frames(cb_chan.cb_to_main.value_pop());
    audio::dft_array phase_adjusted_dft(dft), old_dft(dft);

    //main loop
    while(!gui_thread.data.do_exit) {
        //push the frames received from portaudio
        dft.push_frames_fft(cb_chan.cb_to_main.value_pop());

        const f32 pitch_mul = gui_thread.data.do_apply_effect ? std::pow(2.f, f32(gui_thread.data.pitch)/12.f) : 1.f;
        //phase adjustment to avoid artifacts (this is what makes this a phase vocoder)
        for(u64 i : index(dft)) {
            using std::abs, std::arg;
            using scluk::math::pi;

            //p means phase, A means amplitude
            const f32 p_new = arg(dft[i]), p_old = arg(old_dft[i]),
                      p_old_adj = arg(phase_adjusted_dft[i]), A_new = abs(dft[i]);

            //integer division allows me to automatically floor without additional cost
            const f32 unwrap_addend = 2.f*pi * f32(i / audio::ift_overlap);

            const f32 raw_p_delta = p_new - p_old;
            const f32 mod_p_delta = raw_p_delta + std::signbit(raw_p_delta) * 2.f*pi;

            const f32 adj_p_delta = (unwrap_addend + mod_p_delta) * pitch_mul;
            const f32 p_new_adj = p_old_adj + adj_p_delta;

            phase_adjusted_dft[i] = std::polar(A_new, p_new_adj);
        }

        //calculate the latest ift, apply the hann window and enqueue it
        ift_queue << scluk::math::hann_window(phase_adjusted_dft.ifft<audio::ft_win>(pitch_mul));

        audio::frame_chunk frames(0.f);

        for(u64 n : index(ift_queue)) {
            u64 starting_index = (audio::ift_overlap-1 - n) * audio::ft_dist;

            for(u64 i : range(audio::ft_dist))
                frames[i] += ift_queue[n][starting_index + i];
        }

        //send the frames to the callback
        if(gui_thread.data.do_output_audio)
            cb_chan.main_to_cb.push(std::move(frames));
        else cb_chan.main_to_cb.push(audio::frame_chunk(0.f));

        //send the old dft to the gui and repopulate it
        gui_thread.channel.try_push(std::move(old_dft));
        old_dft = dft;
    }
}