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

#include "Fraction.h"
#include "Modulo.h"
#include "Infinity.h"
#include "Integer.h"
#include "Sum.h"
#include "e.h"
#include "i.h"
#include "pi.h"
#include <type_traits>
#include <boost/multiprecision/cpp_int.hpp>

namespace omnn{
namespace math {

    using namespace std;

    type_index order[] = {
        // for fast optimizing
        typeid(NaN),
        typeid(MInfinity),
        typeid(Infinity),
        typeid(Sum),
        typeid(Product),
        // general order
        typeid(Integer),
        typeid(MinusOneSq),
        typeid(Euler),
        typeid(Pi),
        typeid(Fraction),
        typeid(Exponentiation),
        typeid(Variable),
        typeid(Modulo),
    };

    auto ob = std::begin(order);
    auto oe = std::end(order);

    // inequality should cover all cases
    bool ProductOrderComparator::operator()(const Valuable& x, const Valuable& y) const
    {
        auto it1 = std::find(ob, oe, x.Type());
        if (it1==oe) IMPLEMENT

        auto it2 = std::find(ob, oe, y.Type());
        if (it2==oe) IMPLEMENT

        return it1 == it2 ? x.IsComesBefore(y) : it1 < it2;
    }

    Product::Product() : members{1}
    {
        hash = members.begin()->Hash();
    }

    Product::Product(const std::initializer_list<Valuable>& l)
    {
        for (const auto& arg : l)
        {
            if (arg.IsProduct())
                for (auto& m : arg.as<Product>().members)
                    this->Add(m, end());
            else
                this->Add(arg, end());
        }
    }

    int Product::findMaxVaExp()
    {
        vaExpsSum = 0;
        for (auto& i:vars) {
            if (!i.second.IsInt()) {
                IMPLEMENT;
            }
            vaExpsSum += i.second;
        }
        auto it = std::max_element(vars.begin(), vars.end(), [](auto& x, auto& y){
            return x.second < y.second;
        });
        if (it != vars.end()) {
            return static_cast<int>(it->second.as<Integer>());
        }
        return 0;
    }

    void Product::AddToVars(const Variable & va, const Valuable & exponentiation)
    {
        if (!exponentiation.IsInt()) {
            std::cerr << va.str() << '^' << exponentiation.str() << std::endl;
            IMPLEMENT // estimate in to be greater for those which you want to see first in sum sequence
        }
        if(exponentiation==0)
            return;

        auto& eai = exponentiation.as<Integer>();
        vaExpsSum += eai;

        auto& e = vars[va];
        if (!e.IsInt()) {
            IMPLEMENT
        }
        auto wasMax = maxVaExp == max_exp_t(e.ca(), 1);
        e += exponentiation;

        auto isMax = maxVaExp < e.ca();
        if (isMax) {
            maxVaExp = e.ca();
        }

        if (e == 0) {
            vars.erase(va);
        }

        if (!isMax && wasMax) {
            assert(eai < 0);
            maxVaExp = findMaxVaExp();
        }
    }

    void Product::AddToVarsIfVaOrVaExp(const Valuable &item)
    {
        if(item.IsVa())
        {
            AddToVars(item.as<Variable>(), 1_v);
        }
        else if (item.IsExponentiation())
        {
            auto& e = item.as<Exponentiation>();
            auto& base = e.getBase();
            if (base.IsVa())
            {
                AddToVars(base.as<Variable>(), e.getExponentiation());
            }
            else
            {
                auto itemMaxVaExp = item.getMaxVaExp();
                if (itemMaxVaExp > maxVaExp) {
                    maxVaExp = itemMaxVaExp;
                }
            }
        }
        else if (!item.IsSum() && item.FindVa())
        {
            IMPLEMENT
        }
    }

    Product::iterator Product::Had(iterator it)
    {
        hash ^= it->Hash();
        AddToVarsIfVaOrVaExp(*it);
        base::Update(it, *it ^ 2);
        return it;
    }

    const Product::iterator Product::Add(const Valuable& item, const iterator hint)
    {
        iterator it;

        if (members.size() == 1 && !item.IsSimple()) {
            it = begin();
            if (it->Same(1_v)) {
                Delete(it);
            }
        }

        if (item.IsInt()) {
            if (item == 1_v)
                it = begin();
            else if ((it = GetFirstOccurence<Integer>()) != end()
                || ((it = GetFirstOccurence<Fraction>()) != end() && it->IsSimpleFraction())
            ) {
                Update(it, *it * item);
            } else {
                it = base::Add(item, hint);
            }
        } else if (item.IsSimpleFraction()) {
            if ((it = GetFirstOccurence<Fraction>()) != end()
                || (it = GetFirstOccurence<Integer>()) != end()
            ) {
                Update(it, item * *it);
            } else {
                it = base::Add(item, hint);
            }
        } else if (item.IsProduct()) {
            it = hint;
            for (auto& i : item.as<Product>()) {
                it = Add(i, it);
            }
        } else if (item.IsFraction() && item.FindVa()) {
            auto& f = item.as<Fraction>();
            auto cmp = members.value_comp();
            it = std::min(Add(f.getNumerator()),
                          Add(f.getDenominator() ^ -1),
                          [&](auto& _1, auto& _2){ return cmp(*_1, *_2); });
        }
        else
        {
            it = std::find(members.begin(), members.end(), item);
            if(it==end()) {
                it = base::Add(item, hint);
                AddToVarsIfVaOrVaExp(item);
            } else
                Update(it, item.Sq());
        }
        return it;
    }

    void Product::Delete(typename cont::iterator& it)
    {
        std::function<void()> addToVars;
        if(it->IsVa())
        {
            addToVars = std::bind(&Product::AddToVars, this, it->as<Variable>(), -1_v);
        }
        else if (it->IsExponentiation())
        {
            auto& e = it->as<Exponentiation>();
            auto& ebase = e.getBase();
            if (ebase.IsVa()) {
                addToVars = std::bind(&Product::AddToVars, this, ebase.as<Variable>(), -e.getExponentiation());
            }
        }

        base::Delete(it);

        if(addToVars)
            addToVars();
    }

    void Product::optimize()
    {
        if (!optimizations || optimized)
            return;
        optimized = true;

        // zero
        auto it = GetFirstOccurence<Integer>();
        if (it != end()) {
            if (it->Same(0)) {
                Become(0);
                return;
            }

        // one
            if (it->Same(1) && size() > 1) {
                Delete(it);
            }
        }

        // fractionless
        bool updated;
        do {
            updated = {};
            for (auto it = GetFirstOccurence<Fraction>(); it != end(); ++it) {
                if (it->IsFraction() && it->FindVa()) {
                    auto& f = it->as<Fraction>();
                    auto& dn = f.getDenominator();
                    auto defract = dn.FindVa() ? (dn ^ (-1_v)) : 1_v / dn;
                    auto& n = f.getNumerator();
                    if(n==1_v)
                    {
                        Delete(it);
                    }
                    else
                    {
                        Update(it, n);
                        OptimizeOff o;
                        operator*=(defract);
                    }
                    updated = true;
                }
            }
        } while (updated);

        // optimize members, if found a sum then become the sum multiplied by other members
        for (auto it = members.begin(); it != members.end();)
        {
            if (it->IsSum())
            {
                auto sum = std::move(const_cast<Valuable&&>(*it));
                sum.optimize();
                Delete(it);
//                auto was = optimizations;
//                optimizations = false;
                for (auto& it : members)
                {
                    sum *= it;
                }
//                optimizations = was;
                Become(std::move(sum));
                return;
            }
            auto c = *it;
            c.optimize();
            if (!it->Same(c)) {
                Update(it, c);
                continue;
            }
            else
                ++it;
        }

        // emerge inner products
        for (auto it = members.begin(); it != members.end();)
        {
            if (it->IsProduct()) {
                for (auto& m : it->as<Product>())
                    Add(m);
                Delete(it);
            }
            else
                ++it;
        }

        do
        {
            updated = {};
            for (auto it = members.begin(); it != members.end();)
            {
                if (members.size() == 1)
                {
                    Become(std::move(*const_cast<Valuable*>(&*it)));
                    return;
                }

                if (*it == 1)
                {
                    Delete(it);
                    continue;
                }

                if (*it == 0)
                {
                    Become(0);
                    return;
                }

                auto c = *it;
                auto it2 = it;
                ++it2;
                while (it2 != members.end())
                {
                    if (c.MultiplyIfSimplifiable(*it2)) {
                        if (c.IsProduct()) {
                            auto& cAsP = c.as<Product>();
                            if(cAsP.size() == 2 && cAsP.begin()->IsSimple()) {
                                break;
                            } else {
                                IMPLEMENT
                            }
                        }
                        Delete(it2);
                        continue;
                    }
                    else
                        ++it2;
                }

                if (!it->Same(c)) {
                    Update(it, c);
                    updated = true;
                } else {
                    ++it;
                }
            }
        } while (updated);

        // fraction optimizations
        auto f = GetFirstOccurence<Fraction>();
        if (f != members.end()) {
            Valuable fo = *f;
            auto& dn = fo.as<Fraction>().getDenominator();
            if (dn.IsProduct()) {
                auto& pd = dn.as<Product>();
                for (auto it = members.begin(); it != members.end();)
                {
                    if (it != f && pd.Has(*it)) {
                        fo *= *it;
                        Delete(it);

                        if (!fo.IsFraction() ||
                            !fo.as<Fraction>().getDenominator().IsProduct()
                            ) {
                            break;
                        }
                    }
                    else  ++it;
                }
            }

            fo.optimize();

            if (fo.IsFraction()) {
                auto& dn = fo.as<Fraction>().getDenominator();
                if (!dn.IsProduct()) {
                    for (auto it = members.begin(); it != members.end();)
                    {
                        if (dn == *it) {
                            Delete(it);
                            fo *= dn;
                            break;
                        } else if (it->IsExponentiation() && it->as<Exponentiation>().getBase() == dn) {
                            Update(it, *it/dn);
                            fo *= dn;
                            break;
                        }
                        else  ++it;
                    }
                }
            }

            if(!f->Same(fo))
            {
                Update(f,fo);
            }
        }

        if(members.size()==0)
            Become(1_v);
        else if (members.size()==1)
            Become(std::move(const_cast<Valuable&&>(*members.begin())));
    }

    Valuable Product::Sqrt() const
    {
        return Each([](auto& m){ return m.Sqrt(); });
    }

    Valuable & Product::sq()
    {
        Product p;
        for (auto m : members)
        {
            p.Add(m.Sq());
        }
        return Become(std::move(p));
    }

    const Product::vars_cont_t& Product::getCommonVars() const
    {
        return vars;
    }

    Product::vars_cont_t Product::getCommonVars(const vars_cont_t& with) const
    {
        vars_cont_t common;
        for(auto& kv : vars)
        {
            auto it = with.find(kv.first);
            if (it != with.end()) {
                auto& i1 = kv.second.ca();
                auto& i2 = it->second.ca();
                if (i1 > 0_v && i2 > 0_v) {
                    common[kv.first] = std::min(i1, i2);
                }
                else
                {
                    IMPLEMENT
                }
            }
        }
        return common;
    }

    Valuable Product::calcFreeMember() const
    {
        Valuable _ = 1_v;
        if (getCommonVars().empty()) {
            for(auto& m : *this) {
                auto c = m.calcFreeMember();
                if(c.IsInt()) {
                    _ *= c;
                } else
                    IMPLEMENT
            }
        } else
            _ = 0_v;
        return _;
    }

    Valuable Product::getCommVal(const Product& with) const
    {
        return VaVal(getCommonVars(with.getCommonVars()));
    }

    Valuable Product::InCommonWith(const Valuable& v) const
    {
        auto _ = 1_v;
        auto check = [&](auto& with){
            if (std::find(members.begin(), members.end(), with) != members.end())
                _ *= with;
            else
                for(auto&m:members){
                    auto c = m.InCommonWith(with);
                    if (c!=1_v)
                        _*=c;
                }
        };

        if (v.IsProduct())
            for(auto& m : v.as<Product>())
                check(m);
        else if (v.IsSum())
            _ = v.InCommonWith(*this);
        else
            check(v);
        return _;
    }

    // NOTE : inequality must cover all cases for bugless Sum::Add
    bool Product::IsComesBefore(const Valuable& v) const
    {
        auto mae = getMaxVaExp();
        auto vme = v.getMaxVaExp();

        if (mae != vme)
            return mae > vme;

        char vp[sizeof(Product)];
        auto isSameType = v.IsProduct();
        if(!isSameType)
        {
            static const ProductOrderComparator oc;
            return oc(*this,v);
        }
        auto p = isSameType ? &v.as<Product>() : new(vp) Product{v};
        auto d = [isSameType](const Product* _){
            if (!isSameType && _){
                _->~Product();
            }
        };
        std::unique_ptr<const Product , decltype(d)> up(p,d);
        if (p)
        {
            if (vme != p->getMaxVaExp()) {
                std::cout << vme << std::endl;
                std::cout << p->getMaxVaExp() << std::endl;
                IMPLEMENT
            }

            if (vaExpsSum.ca() != p->vaExpsSum.ca())
            {
                return vaExpsSum > p->vaExpsSum;
            }

            if (members == p->members)
                return false;

            if (members.size() != p->members.size()) {
                return members.size() > p->members.size();
            }

            auto i1 = members.begin();
            auto i2 = p->members.begin();
            for (; i1 != members.end(); ++i1, ++i2) {
                auto it1 = std::find(ob, oe, i1->Type());
                assert(it1!=oe); // IMPLEMENT, add to order table
                auto it2 = std::find(ob, oe, i2->Type());
                assert(it2!=oe); // IMPLEMENT
                if (it1 != it2) {
                    return it1 < it2;
                }
            }
            // same types set, compare by value
            i1 = members.begin();
            i2 = p->members.begin();
            for (; i1 != members.end(); ++i1, ++i2) {
                if(*i1 != *i2)
                {
                    return i1->IsComesBefore(*i2);
                }
            }

            // everything is equal, should not be so
            IMPLEMENT
        }
        else
            IMPLEMENT
    }

    Valuable& Product::operator +=(const Valuable& v)
    {
        if(v == 0_v)
            return *this;
        if (*this == v)
            return *this *= 2;
        if(*this == -v)
            return Become(0_v);
        if(v.IsProduct()){
            auto& vAsP = v.as<Product>();
            Product thisHasNotCommonWithV, vHasNotInCommonWithThis;
            Product common;
            for(auto&& item : GetCont()){
                if(vAsP.Has(item))
                    common.Add(std::move(item));
                else
                    thisHasNotCommonWithV.Add(std::move(item));
            }
            for(auto& item : vAsP.GetConstCont()){
                if(!common.Has(item))
                    vHasNotInCommonWithThis.Add(item);
            }
            if(!(common.size()==1 && *common.begin()==1)){ // unchanged
                Valuable sum = Sum {thisHasNotCommonWithV, vHasNotInCommonWithThis};
                sum.optimize();
                sum *= common;
                sum.optimize();
                return Become(std::move(sum));
            }
        } else if (v.IsSum()) {
            return Become(v + *this);
        }
        else{
        auto& cv = getCommonVars();
        if (!cv.empty() && cv == v.getCommonVars())
        {
            auto valuable = varless() + v.varless();
            if(!valuable.IsSum())
                valuable *= getVaVal();
            return Become(std::move(valuable));
        }
        }
        return Become(Sum { *this, v });
    }

    std::pair<Valuable, Valuable> Product::SplitSimplePart() const {
        std::pair<Valuable, Valuable> parts;
        auto it = begin();
        IMPLEMENT
    }

    std::pair<Valuable, Valuable> Product::split_simple_part(){
        IMPLEMENT

    }

   std::pair<bool,Valuable> Product::IsSummationSimplifiable(const Valuable& v) const
   {
       std::pair<bool,Valuable> is;
       is.first = v == 0;
       if (is.first)
           is.second = *this;
       else if ((is.first = operator==(v)))
           is.second = *this * 2;
       else if ((is.first = operator==(-v)))
           is.second = 0;
       else if (Has(v)) {
           //OptimizeOn o;
           auto div = *this / v;
           auto divPlusOneIsSimple = div.IsSummationSimplifiable(vo<1>::get());
           is.first = divPlusOneIsSimple.first;
           if(divPlusOneIsSimple.first) {
               is.second = divPlusOneIsSimple.second * v;
           }
       } else if (v.IsExponentiation()) {
           is.first = Has(v) && size() == 2 && begin()->IsSimple();
           if(is.first) {
               is.second++ = *this;
               is.first = !is.second.IsSum();
           }
       } else if (v.IsSimple()) {
       } else if (v.IsProduct()
                  || v.IsVa()
                  || v.IsFraction()
                  ) {
           //OptimizeOn o;
           auto icw = InCommonWith(v);
           if (icw != 1) {
               auto thisNoCommon = *this / icw;
               if (!operator==(thisNoCommon)) { //multivalue cases (example ((-17)/16)*(4^((1/2))) / (1/2)*(4^((1/2))) = ((-17)/16)*(4^((1/2)))
                   auto vNoCommon = v / icw;
                   //std::cout << thisNoCommon << ".IsSummationSimplifiable(" << vNoCommon << ')' << std::endl;
                   is = thisNoCommon.IsSummationSimplifiable(vNoCommon);
                   if(is.first){
                       is.second *= icw;
                   }
               }
           }
//           auto& vp = v.as<Product>();
//           auto sp = SplitSimplePart();
//           auto vsp = vp.SplitSimplePart();
//           if(sp.second == vsp.second){
//
//               IMPLEMENT
//           }
       } else {
//           std::cout << v <<std::endl;
           LOG_AND_IMPLEMENT("Unknown case (need to implement) in Product::IsSummationSimplifiable for " << *this << " and " << v);
       }
       return is;
   }

    Valuable& Product::operator *=(const Valuable& v)
    {
        if ((size() == 1 && *begin() == 1) || size() == 0)
            return Become(Valuable(v));

        if (v.IsInt()){
            if (v==0) {
                return Become(0);
            } else if (v==1) {
                goto yes;
            } else {
                auto b = begin();
                if(b->IsSimple()){
                    auto up = *b * v;
                    if(up == 1)
                        Delete(b);
                    else
                        Update(b, up);
                    goto yes;
                }
            }
        }

        if (v.IsSum())
            return Become(v**this);

        if (v.IsVa())
        {
            auto& va = v.as<Variable>();
            for (auto it = members.begin(); it != members.end(); ++it)
            {
                if (it->Same(v)) {
                    Update(it, Exponentiation(va, 2));
                    goto yes;
                }
                else if (it->IsExponentiation())
                {
                    auto& e = it->as<Exponentiation>();
                    if (e.getBase() == va) {
                        Update(it, va ^ (e.getExponentiation()+1));
                        goto yes;
                    }
                }
            }
        }
        else if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            auto& vExpBase = e.getBase();
            for (auto it = members.begin(); it != members.end();)
            {
                if (it->IsExponentiation() &&
                    it->as<Exponentiation>().getBase() == vExpBase)
                {
                    Update(it, *it*v);
                    goto yes;
                }
                else
                {
                    if (vExpBase == *it) {
                        Update(it, vExpBase ^ (e.getExponentiation()+1));
                        goto yes;
                    }
                    else
                        ++it;
                }
            }
        }
        else if (v.IsProduct())
        {
            if(operator==(v))
                sq();
            else
                for(auto& m : v.as<Product>())
                    base::Add(m);
            goto yes;
        }
        else
        {
            for (auto it = members.begin(); it != members.end();)
            {
                if (it->OfSameType(v)) {
                    auto up = *it * v;
                    if(up == 1){
                        Delete(it);
                    } else {
                        Update(it, up);
                    }
                    goto yes;
                }
                else
                    ++it;
            }
        }

        // add new member
        Add(v);

    yes:
        optimize();
        return *this;
    }

    Valuable& Product::operator /=(const Valuable& v)
    {
        if (v.IsProduct()) {
            auto opts = optimizations;
            optimizations = {};
            for(auto& i : v.as<Product>())
                *this /= i;
            optimizations = opts;
            optimize();
            return *this;
        }
        else if (v.IsSimple()) {
            if (v == 1_v) {
                return *this;
            }
            auto first = begin();
            if (first->IsSimple()) {
                auto _ = (*first) / v;
                if (_.IsInt() && _.ca() == 1)
                    Delete(first);
                else {
                    Update(first, _);
                    optimized = {};
                }
                optimize();
                return *this;
            }
            else {
                return operator *=(Fraction{ 1_v,v });
            }
        }
        else
        {
            auto it = std::find(begin(), end(), v);
            if(it != end()){
                Delete(it); // TODO : Review this branch to make it compatible with multivalue v
                optimize();
                return *this;
            }
            auto vIsExp = v.IsExponentiation();
            auto e = vIsExp ? &v.as<Exponentiation>() : nullptr;
            for (auto it = members.begin(); it != members.end(); ++it)
            {
                if (*it == v)
                {
                    if (v.IsMultival() == YesNoMaybe::No) {
                        Delete(it);
                        optimize();
                        return *this;
//                    } else if (v.IsMultival() == YesNoMaybe::Maybe) {

                    } else {
                        Update(it, *it * (v^-1));
                        optimize();
                        return *this;
                    }
                }
                else if (vIsExp && *it == e->getBase())
                {
                    return *this *= e->getBase() ^ (-e->getExponentiation());
                }
                else if (it->IsExponentiation() && it->as<Exponentiation>().getBase() == v)
                {
                    Update(it, *it / v);
                    optimize();
                    return *this;
                }
            }
        }
//        else if (v.IsVa())
//        {
//            auto it = getCommonVars().find(v);
//            if (it != getCommonVars().end() && *it != 0_v) {
//                if (*it == 1_v) {
//                    auto _ = find(begin(), end(), v);
//                    if (_ == end()) {
//                        IMPLEMENT
//                    }
//                    else
//                    {
//                        Delete(_);
//                    }
//                } else {
//                    for(auto i = begin(); i != end(); ++i)
//                    {
//                        if (i->IsExponentiation()) {
//                            auto e = Exponentiation::cast(*i);
//                            if (e->getBase() == v) {
//                                Update(i, *e / v);
//                                optimize();
//                                break;
//                            }
//                        }
//                    }
//                }
//            }
//        }
        return *this *= v ^ -1;
	}

    Valuable& Product::operator ^=(const Valuable& v)
    {
        auto _ = 1_v;
        for(auto& m : members)
        {
            _ *= m ^ v;
        }
        Become(std::move(_));
        return *this;
    }

	Valuable& Product::operator %=(const Valuable& v)
	{
		return base::operator %=(v);
	}

	namespace {
        constexpr std::hash<a_int> Hasher;
        const size_t Hash1 = Hasher(1);
    }
    bool Product::operator ==(const Valuable& v) const
    {
        auto sameHash = (Valuable::hash & ~Hash1) == (v.Hash() & ~Hash1); // ignore multiplication by 1
        auto same = v.Is<Product>() && sameHash;
        auto& c1 = GetConstCont();
        auto sz1 = c1.size();
        if (same) {
            auto& vp = v.as<Product>();
            auto& c2 = vp.GetConstCont();
            auto sz2 = c2.size();
            auto sameSizes = sz1 == sz2;
            if(sameSizes) {
                same = c1 == c2;
            } else if (sz1-sz2==1 || sz2-sz1==1) {
                auto it1 = c1.begin(), it2 = c2.begin();
                auto e1 = c1.end(), e2 = c2.end();
                for(same = true;
                    same && it1 != e1 && it2 != e2;
                    ++it1, ++it2)
                {
                    if(it1->Same(1_v)){
                        ++it1;
                    }
                    if(it2->Same(1_v)){
                        ++it2;
                    }

                    if(it1 == e1 || it2 == e2)
                        continue;

                    same = same && it1->Same(*it2);
                }

                same = same && it1 == e1 && it2 == e2;
            } else {
                same = {};
            }
        }
        else if (sameHash
                 && (sz1 == 1
                     || (sz1 == 2 && c1.begin()->Same(1_v) )))
        {
            same = c1.rbegin()->operator==(v);
        }
        return same;
    }

    Product::operator double() const
    {
        double d=1;
        for(auto& i:members)
        {
            d*=static_cast<double>(i);
        }
        return d;
    }

    Valuable& Product::d(const Variable& x)
    {
        if(vars.find(x) != vars.end())
        {
            *this *= vars[x];
            *this /= x;
            optimize();
        }else
            Become(0_v);
        return *this;
    }

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

    Valuable Product::operator()(const Variable& va, const Valuable& augmentation) const
    {
        Valuable s; s.SetView(Valuable::View::Flat);

        if(augmentation.HasVa(va)) {
            IMPLEMENT;
        } else {
            auto coVa = getCommonVars();
            auto it = coVa.find(va);
            if (it != coVa.end()) {
                if (it->second < 0) {
                    s = ((*this / (it->first ^ it->second)) / augmentation) ^ (1_v / -it->second);
                }
                else
                {
                    s = (augmentation / (*this / (it->first ^ it->second))) ^ (1_v / it->second);
                }
            }
            else
            {
                auto a = 1_v;
                auto aug = augmentation;
                for(auto& m : members)
                    if (m.HasVa(va))
                        a *= m;
                    else
                        aug *= m;
                if (a==1) {
                    IMPLEMENT
                }
                s = a(va,aug);
            }
        }

//        if(augmentation.HasVa(va)) {
//            IMPLEMENT;
//        } else {
//            auto _ = augmentation;
//            auto left = 1_v;
//            for(auto& m : members)
//            {
//                if (m.HasVa(va)) {
//                    left *= m;
//                } else {
//                    _ /= m;
//                }
//            }
//
//            left.optimize();
//            if (left.IsProduct()) {
//                IMPLEMENT
//            }
//
//            return left(va, _);
//        }
//        auto cova = getCommonVars();
//        auto it = cova.find(va);
//        if (it != cova.end()) {
//            auto _ = augmentation / (*this / (va ^ it->second));
//            if(_.HasVa(va))
//                IMPLEMENT;
//            s.insert(_);
//            if (it->second % 2 == 0) {
//                s.insert(-_);
//            }
//        }
//        else
//        {
//            IMPLEMENT
//        }
        return s;
    }

    void Product::solve(const Variable& va, solutions_t& solutions) const
    {
        auto it = std::find(members.begin(), members.end(), va);
        if(it != members.end())
            solutions.insert(0_v);
        else {
            it = begin();
            bool found = {};
            const Exponentiation* e = {};
            while (it != end()
                    && !(it->IsExponentiation()
                     && (found = it->as<Exponentiation>().getBase() == va)
                     ) ) {
                ++it;
            }
            if (found) {
                if (e->getExponentiation() == 0_v) {
                    IMPLEMENT
                }
                solutions.insert(0_v);
            }
            else {
                std::cout << "Solving " << str() << std::endl;
                IMPLEMENT // TODO: find exponentiations of va
            }
        }
    }

    Valuable::solutions_t Product::Distinct() const {
        solutions_t branches = { 1_v };
        for (auto& m : members) {
            solutions_t newBranches;
            for (auto&& branch : m.Distinct()) {
                for (auto& b : branches) {
                    newBranches.emplace(b * branch);
                }
            }
            branches = std::move(newBranches);
        }
        return branches;
    }

	std::ostream& Product::print(std::ostream& out) const
	{
        std::stringstream s;
        constexpr char sep[] = "*";
        for (auto& b : members)
            s << b << sep;
        auto str = s.str();
        auto cstr = const_cast<char*>(str.c_str());
        cstr[str.size() - sizeof(sep) + 1] = 0;
        out << cstr;
        return out;
	}

}}


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/adapter/path.hpp>
#include <svgpp/detail/adapt_context.hpp>
#include <boost/mpl/set.hpp>
#include <boost/optional.hpp>
#include <boost/noncopyable.hpp>

namespace svgpp
{

typedef boost::mpl::set6<
  tag::attribute::x,
  tag::attribute::y,
  tag::attribute::rx,
  tag::attribute::ry,
  tag::attribute::width,
  tag::attribute::height
> rect_shape_attributes;

template<class Length>
class collect_rect_attributes_adapter: boost::noncopyable
{
public:
  template<class Context>
  bool on_exit_attributes(Context & context) const
  {
    typedef detail::unwrap_context<Context, tag::length_policy> length_policy_context;
    typedef typename length_policy_context::policy length_policy_t;
    typedef detail::unwrap_context<Context, tag::error_policy> error_policy;

    typename length_policy_t::length_factory_type & converter
      = length_policy_t::length_factory(length_policy_context::get(context));

    typename length_policy_t::length_factory_type::coordinate_type
      x = 0, y = 0, rx = 0, ry = 0, width, height;
    if (x_)
      x = converter.length_to_user_coordinate(*x_, tag::length_dimension::width());
    if (y_)
      y = converter.length_to_user_coordinate(*y_, tag::length_dimension::height());
    width = converter.length_to_user_coordinate(width_, tag::length_dimension::width());
    height = converter.length_to_user_coordinate(height_, tag::length_dimension::height());
    if (rx_)
    {
      rx = converter.length_to_user_coordinate(*rx_, tag::length_dimension::width());
      if (!ry_)
        ry = rx;
    }
    if (ry_)
    {
      ry = converter.length_to_user_coordinate(*ry_, tag::length_dimension::height());
      if (!rx_)
        rx = ry;
    }
    if (rx < 0)
      return error_policy::policy::negative_value(error_policy::get(context), tag::attribute::rx());
    if (ry < 0)
      return error_policy::policy::negative_value(error_policy::get(context), tag::attribute::ry());
    if (width < 0)
      return error_policy::policy::negative_value(error_policy::get(context), tag::attribute::width());
    if (height < 0)
      return error_policy::policy::negative_value(error_policy::get(context), tag::attribute::height());

    if (width == 0 || height == 0)
      return true;

    typedef detail::unwrap_context<Context, tag::basic_shapes_events_policy> basic_shapes_events;
    basic_shapes_events::policy::set_rect(basic_shapes_events::get(context), x, y, width, height, rx, ry);
    return true;
  }

  void set(tag::attribute::x, Length const & val) { x_ = val; }
  void set(tag::attribute::y, Length const & val) { y_ = val; }
  void set(tag::attribute::rx, Length const & val) { rx_ = val; }
  void set(tag::attribute::ry, Length const & val) { ry_ = val; }
  void set(tag::attribute::width, Length const & val) { width_ = val; }
  void set(tag::attribute::height, Length const & val) { height_ = val; }

private:
  boost::optional<Length> x_, y_, rx_, ry_;
  Length width_, height_; // Required attributes
};

namespace detail
{
  template<class EventsPolicy, class Context, class Coordinate>
  inline void context_set_rounded_rect(Context & context, Coordinate x, Coordinate y, Coordinate width, Coordinate height,
    Coordinate rx, Coordinate ry)
  {
    rx = std::min(rx, width / 2);
    ry = std::min(ry, height / 2);
    EventsPolicy::path_move_to(context, x + rx, y, tag::coordinate::absolute());
    EventsPolicy::path_line_to_ortho(context, x+width-rx, true, tag::coordinate::absolute());
    EventsPolicy::path_elliptical_arc_to(context, rx, ry, Coordinate(0), false, true, x+width, y+ry, tag::coordinate::absolute());
    EventsPolicy::path_line_to_ortho(context, y+height-ry, false, tag::coordinate::absolute());
    EventsPolicy::path_elliptical_arc_to(context, rx, ry, Coordinate(0), false, true, x+width-rx, y+height, tag::coordinate::absolute());
    EventsPolicy::path_line_to_ortho(context, x+rx, true, tag::coordinate::absolute());
    EventsPolicy::path_elliptical_arc_to(context, rx, ry, Coordinate(0), false, true, x, y+height-ry, tag::coordinate::absolute());
    EventsPolicy::path_line_to_ortho(context, y+ry, false, tag::coordinate::absolute());
    EventsPolicy::path_elliptical_arc_to(context, rx, ry, Coordinate(0), false, true, x+rx, y, tag::coordinate::absolute());
    EventsPolicy::path_close_subpath(context);
    EventsPolicy::path_exit(context);
  }
}

struct rect_to_path_adapter
{
  template<class Context, class Coordinate>
  static void set_rect(Context & context, Coordinate x, Coordinate y, Coordinate width, Coordinate height,
    Coordinate rx, Coordinate ry)
  {
    typedef detail::unwrap_context<Context, tag::path_events_policy> path_events;

    typename path_events::type & path_context = path_events::get(context);
    if (rx == 0 || ry == 0)
    {
      path_events::policy::path_move_to(path_context, x, y, tag::coordinate::absolute());
      path_events::policy::path_line_to_ortho(path_context, width,  true,  tag::coordinate::relative());
      path_events::policy::path_line_to_ortho(path_context, height, false, tag::coordinate::relative());
      path_events::policy::path_line_to_ortho(path_context, -width, true,  tag::coordinate::relative());
      path_events::policy::path_close_subpath(path_context);
      path_events::policy::path_exit(path_context);
    }
    else
    {
      detail::context_set_rounded_rect<typename path_events::policy>(path_context, x, y, width, height, rx, ry);
    }
  }
};

struct rounded_rect_to_path_adapter
{
  template<class Context, class Coordinate>
  static void set_rect(Context & context, Coordinate x, Coordinate y, Coordinate width, Coordinate height,
    Coordinate rx, Coordinate ry)
  {
    if (rx == 0 || ry == 0)
    {
      typedef typename detail::unwrap_context<Context, tag::basic_shapes_events_policy> basic_shapes_events;
      basic_shapes_events::policy::set_rect(basic_shapes_events::get(context), x, y, width, height);
    }
    else
    {
      typedef typename detail::unwrap_context<Context, tag::path_events_policy> path_events;
      detail::context_set_rounded_rect<typename path_events::policy>(path_events::get(context), x, y, width, height, rx, ry);
    }
  }
};

}


#include <boost/test/unit_test.hpp>

#ifdef BUILD_WITH_BOOST_THREAD

#include <boost/thread.hpp>

#include <k52/common/constants.h>
#include <k52/dsp/transform/i_fourier_transform.h>
#include <k52/dsp/transform/fast_fourier_transform.h>
#include <k52/parallel/i_worker_pool.h>
#include <k52/parallel/worker_pool_factory.h>

using std::vector;
using std::complex;
using k52::common::Constants;
using k52::parallel::ITask;
using k52::parallel::ITaskResult;
using k52::parallel::IWorkerPool;
using k52::parallel::WorkerPoolFactory;
using k52::dsp::IFourierTransform;
using k52::dsp::FastFourierTransform;

void complex_harmonic_transform(const IFourierTransform* ft)
{
    //Prepare
    size_t N = 128;
    size_t k0 = 5;

    vector< complex <double> > samples(N);

    for (size_t n = 0; n < N; ++n)
    {
        samples[n] = exp( 2 * Constants::Pi *
                          Constants::ImaginaryUnit * (double)k0 * (double)n / (double)N);
    }

    //Test
    vector< complex <double> > result = ft->Transform(samples);
}

BOOST_AUTO_TEST_SUITE(fourier_transform_functional_tests);

void test_function ()
{
    //Prepare
    size_t N = 1000;
    FastFourierTransform fft(N);
    vector< complex <double> > samples(N);

    //Test
    vector< complex <double> > result = fft.Transform(samples);
}

BOOST_AUTO_TEST_CASE(multithread_test)
{
    boost::thread_group threads;

    for (int i = 0; i < 10; ++i)
    {
        threads.create_thread(&test_function);
    }

    threads.join_all();
}

class TestTaskResult : public ITaskResult
{
};

class TestTask : public ITask
{
public:
    ITaskResult* Perform() const
    {
        FastFourierTransform fft(128);
        complex_harmonic_transform(&fft);

        return new TestTaskResult();
    }
};

BOOST_AUTO_TEST_CASE(thread_worker_pool_test)
{
    //Prepare
    IWorkerPool::shared_ptr pool =
            WorkerPoolFactory::CreateWorkerPool(WorkerPoolFactory::kThreadWorkerPool);

    vector<ITask::shared_ptr> tasks;
    vector<const ITask*> tasks_pointers;
    for (int i = 0; i < 10; ++i)
    {
        tasks.push_back(ITask::shared_ptr(new TestTask()));
        tasks_pointers.push_back(tasks[i].get());
    }

    //Test
    pool->DoTasks(tasks_pointers);
}

BOOST_AUTO_TEST_SUITE_END();

#endif


// Copyright 2014 The CRAVE developers. All rights reserved.

#pragma once

#include <boost/mpl/times.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/type_traits/is_signed.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/static_assert.hpp>

namespace crave {

template <typename T, typename Enable = void>
struct to_uint64 {
  uint64_t operator()(T const& value) { return value; }
};

template <typename T>
class randv;
template <typename T>
struct Variable;
template <typename T>
struct WriteReference;
template <typename T>
struct var_tag;
template <typename T>
struct write_ref_tag;
template <typename T>
struct read_ref_tag;
template <typename T>
struct vector_tag;
struct placeholder_tag;

template <typename T, typename Enable = void>
struct bitsize_traits : boost::mpl::int_<0> {
  BOOST_STATIC_ASSERT(sizeof(T) == 0);
};

template <typename Integer>
struct bitsize_traits<
    Integer,
    typename boost::enable_if<boost::is_integral<Integer> >::type> : boost::mpl::times<boost::mpl::sizeof_<Integer>,
                                                                                boost::mpl::int_<8> >::type {};

template <>
struct bitsize_traits<bool> : boost::mpl::int_<1> {};

template <>
struct bitsize_traits<placeholder_tag> : public bitsize_traits<unsigned> {};

template <typename T>
struct bitsize_traits<randv<T> > : public bitsize_traits<T> {};

template <typename T>
struct bitsize_traits<Variable<T> > : public bitsize_traits<T> {};

template <typename T>
struct bitsize_traits<WriteReference<T> > : public bitsize_traits<T> {};

template <typename T>
struct bitsize_traits<var_tag<T> > : public bitsize_traits<T> {};

template <typename T>
struct bitsize_traits<write_ref_tag<T> > : public bitsize_traits<T> {};

template <typename T>
struct bitsize_traits<read_ref_tag<T> > : public bitsize_traits<T> {};

template <typename T>
struct bitsize_traits<vector_tag<T> > : public bitsize_traits<T> {};

template <typename Integral, Integral I>
struct bitsize_traits<boost::mpl::integral_c<Integral, I> > : public bitsize_traits<Integral> {};

template <typename Enum>
struct bitsize_traits<Enum, typename boost::enable_if<boost::is_enum<Enum> >::type> : bitsize_traits<int>::type {};

template <typename T>
struct is_crave_variable : boost::mpl::false_ {};

template <typename T>
struct is_crave_variable<randv<T> > : boost::mpl::true_ {};

template <typename T>
struct is_crave_variable<write_ref_tag<T> > : boost::mpl::true_ {};

template <typename T>
struct is_signed : public boost::is_signed<T> {};

template <>
struct is_signed<bool> : public boost::is_signed<int> {};

template <typename T>
struct is_signed<crave::randv<T> > : public crave::is_signed<T> {};

template <typename T>
struct is_signed<crave::Variable<T> > : public crave::is_signed<T> {};

template <typename T>
struct is_signed<crave::var_tag<T> > : public crave::is_signed<T> {};

template <typename T>
struct is_signed<crave::write_ref_tag<T> > : public crave::is_signed<T> {};

template <typename T>
struct is_signed<crave::read_ref_tag<T> > : public crave::is_signed<T> {};

template <typename T>
struct is_signed<crave::vector_tag<T> > : public crave::is_signed<T> {};

}  // namespace crave

//  vim: ft=cpp:ts=2:sw=2:expandtab


/**
 * @file t_NumericPropertyScorer.cpp
 * @brief test NumericPropertyScorer to calculate score for numeric value.
 * @author Jun Jiang
 * @date Created 2012-11-26
 */

#include "NumericPropertyScorerTestFixture.h"
#include <boost/test/unit_test.hpp>

using namespace sf1r;

BOOST_FIXTURE_TEST_SUITE(NumericPropertyScorerTest,
                         NumericPropertyScorerTestFixture)

BOOST_AUTO_TEST_CASE(emptyNumericValue)
{
    BOOST_TEST_MESSAGE("check empty numeric value");

    setNumericValue("");
    createScorer();

    score_t gold = 0;
    docid_t maxDocId = 3;
    for (docid_t docId = 1; docId <= maxDocId; ++docId)
    {
        checkScore(docId, gold);
    }
}

BOOST_AUTO_TEST_CASE(oneNumericValue)
{
    BOOST_TEST_MESSAGE("check one numeric value");

    setNumericValue("5");
    createScorer();

    score_t gold = 0;
    docid_t maxDocId = 3;
    for (docid_t docId = 1; docId <= maxDocId; ++docId)
    {
        checkScore(docId, gold);
    }
}

BOOST_AUTO_TEST_CASE(twoEqualNumericValues)
{
    BOOST_TEST_MESSAGE("check two equal numeric values");

    setNumericValue("3 3");
    createScorer();

    score_t gold = 0;
    docid_t maxDocId = 3;
    for (docid_t docId = 1; docId <= maxDocId; ++docId)
    {
        checkScore(docId, gold);
    }
}

BOOST_AUTO_TEST_CASE(twoDiffNumericValues)
{
    BOOST_TEST_MESSAGE("check two different numeric values");

    setNumericValue("1 3"); // max - min = 2
    createScorer();

    checkScore(1, (1.0-1) / 2);
    checkScore(2, (3.0-1) / 2);
    checkScore(3, 0);   // no numeric value
}

BOOST_AUTO_TEST_CASE(threeNumericValues)
{
    BOOST_TEST_MESSAGE("check three numeric values");

    setNumericValue("3 1 2"); // max - min = 2
    createScorer();

    checkScore(1, (3.0-1) / 2);
    checkScore(2, (1.0-1) / 2);
    checkScore(3, (2.0-1) / 2);
    checkScore(4, 0);   // no numeric value

    // max & min numeric values have been fixed in createScorer()
    setNumericValue("0 4 2 3");

    checkScore(1, 0);   // (0-1) / 2 -> min score 0
    checkScore(2, 1);   // (4-1) / 2 -> max score 1
    checkScore(3, (2.0-1) / 2);
    checkScore(4, (3.0-1) / 2);
    checkScore(5, 0);   // no numeric value
}

BOOST_AUTO_TEST_CASE(skipInvalidValues)
{
    BOOST_TEST_MESSAGE("check skipping invalid values");

    setNumericValue("2 3");
    setNumericValue(5, 8);
    setNumericValue(7, 4); // max - min = 6
    createScorer();

    checkScore(1, (2.0-2) / 6);
    checkScore(2, (3.0-2) / 6);
    checkScore(3, 0);   // no numeric value
    checkScore(4, 0);   // no numeric value
    checkScore(5, (8.0-2) / 6);
    checkScore(6, 0);   // no numeric value
    checkScore(7, (4.0-2) / 6);
    checkScore(8, 0);   // no numeric value
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2018-2019 Christian Mazakas (christian dot mazakas at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/LeonineKing1199/furi
//

#define CATCH_CONFIG_MAIN

#include <furi/pct_encode.hpp>
#include <boost/spirit/home/x3.hpp>

#include <string>
#include <cstdint>
#include <iterator>
#include <vector>
#include <bitset>
#include <array>

#include <catch2/catch.hpp>

namespace x3 = boost::spirit::x3;

TEST_CASE("pct_encode_test")
{
  SECTION(
    "... should be able to convert any Unicode code point to its underlying UTF-8 binary "
    "representation")
  {
    // [U+0000,  U+007F]
    //
    {
      auto const bytes_per_code_point = 1;
      auto const code_point_begin     = char32_t{0x0000};
      auto const code_point_end       = char32_t{0x0080};

      auto code_points = std::vector<char32_t>();
      code_points.reserve(code_point_end - code_point_begin);
      for (char32_t code_point = code_point_begin; code_point < code_point_end; ++code_point) {
        code_points.push_back(code_point);
      }

      auto utf8_bytes = std::vector<char>();
      utf8_bytes.resize(bytes_per_code_point * code_points.size());

      furi::utf8_encode(code_points.begin(), code_points.end(), utf8_bytes.begin());

      auto is_valid_encoding = true;

      for (std::size_t idx = 0; idx < code_points.size(); ++idx) {
        auto const code_point = std::bitset<8>(code_points[idx]);

        auto const first_byte = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 0]);

        is_valid_encoding = is_valid_encoding && !first_byte[7];
        is_valid_encoding = is_valid_encoding && (first_byte[6] == code_point[6]);
        is_valid_encoding = is_valid_encoding && (first_byte[5] == code_point[5]);
        is_valid_encoding = is_valid_encoding && (first_byte[4] == code_point[4]);
        is_valid_encoding = is_valid_encoding && (first_byte[3] == code_point[3]);
        is_valid_encoding = is_valid_encoding && (first_byte[2] == code_point[2]);
        is_valid_encoding = is_valid_encoding && (first_byte[1] == code_point[1]);
        is_valid_encoding = is_valid_encoding && (first_byte[0] == code_point[0]);
      }

      CHECK(is_valid_encoding);
    }

    // [U+0080, U+0800]
    //
    {
      auto const bytes_per_code_point = 2;
      auto const code_point_begin     = char32_t{0x0080};
      auto const code_point_end       = char32_t{0x0800};

      auto code_points = std::vector<char32_t>();
      code_points.reserve(code_point_end - code_point_begin);
      for (char32_t code_point = code_point_begin; code_point < code_point_end; ++code_point) {
        code_points.push_back(code_point);
      }

      auto utf8_bytes = std::vector<char>();
      utf8_bytes.resize(bytes_per_code_point * code_points.size());

      furi::utf8_encode(code_points.begin(), code_points.end(), utf8_bytes.begin());

      auto is_valid_encoding = true;

      for (std::size_t idx = 0; idx < code_points.size(); ++idx) {
        auto const code_point = std::bitset<32>(code_points[idx]);

        auto const first_byte  = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 0]);
        auto const second_byte = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 1]);

        is_valid_encoding = is_valid_encoding && first_byte[7];
        is_valid_encoding = is_valid_encoding && first_byte[6];
        is_valid_encoding = is_valid_encoding && !first_byte[5];
        is_valid_encoding = is_valid_encoding && (first_byte[4] == code_point[10]);
        is_valid_encoding = is_valid_encoding && (first_byte[3] == code_point[9]);
        is_valid_encoding = is_valid_encoding && (first_byte[2] == code_point[8]);
        is_valid_encoding = is_valid_encoding && (first_byte[1] == code_point[7]);
        is_valid_encoding = is_valid_encoding && (first_byte[0] == code_point[6]);

        is_valid_encoding = is_valid_encoding && second_byte[7];
        is_valid_encoding = is_valid_encoding && !second_byte[6];
        is_valid_encoding = is_valid_encoding && (second_byte[5] == code_point[5]);
        is_valid_encoding = is_valid_encoding && (second_byte[4] == code_point[4]);
        is_valid_encoding = is_valid_encoding && (second_byte[3] == code_point[3]);
        is_valid_encoding = is_valid_encoding && (second_byte[2] == code_point[2]);
        is_valid_encoding = is_valid_encoding && (second_byte[1] == code_point[1]);
        is_valid_encoding = is_valid_encoding && (second_byte[0] == code_point[0]);
      }

      CHECK(is_valid_encoding);
    }

    // [U+0800, U+10000]
    //
    {
      auto const bytes_per_code_point = 3;
      auto const code_point_begin     = char32_t{0x0800};
      auto const code_point_end       = char32_t{0x10000};

      auto code_points = std::vector<char32_t>();
      code_points.reserve(code_point_end - code_point_begin);
      for (char32_t code_point = code_point_begin; code_point < code_point_end; ++code_point) {
        code_points.push_back(code_point);
      }

      auto utf8_bytes = std::vector<char>();
      utf8_bytes.resize(bytes_per_code_point * code_points.size());

      furi::utf8_encode(code_points.begin(), code_points.end(), utf8_bytes.begin());

      auto is_valid_encoding = true;

      for (std::size_t idx = 0; idx < code_points.size(); ++idx) {
        auto const code_point = std::bitset<16>(code_points[idx]);

        auto const first_byte  = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 0]);
        auto const second_byte = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 1]);
        auto const third_byte  = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 2]);

        is_valid_encoding = is_valid_encoding && first_byte[7];
        is_valid_encoding = is_valid_encoding && first_byte[6];
        is_valid_encoding = is_valid_encoding && first_byte[5];
        is_valid_encoding = is_valid_encoding && !first_byte[4];
        is_valid_encoding = is_valid_encoding && (first_byte[3] == code_point[15]);
        is_valid_encoding = is_valid_encoding && (first_byte[2] == code_point[14]);
        is_valid_encoding = is_valid_encoding && (first_byte[1] == code_point[13]);
        is_valid_encoding = is_valid_encoding && (first_byte[0] == code_point[12]);

        is_valid_encoding = is_valid_encoding && second_byte[7];
        is_valid_encoding = is_valid_encoding && !second_byte[6];
        is_valid_encoding = is_valid_encoding && (second_byte[5] == code_point[11]);
        is_valid_encoding = is_valid_encoding && (second_byte[4] == code_point[10]);
        is_valid_encoding = is_valid_encoding && (second_byte[3] == code_point[9]);
        is_valid_encoding = is_valid_encoding && (second_byte[2] == code_point[8]);
        is_valid_encoding = is_valid_encoding && (second_byte[1] == code_point[7]);
        is_valid_encoding = is_valid_encoding && (second_byte[0] == code_point[6]);

        is_valid_encoding = is_valid_encoding && third_byte[7];
        is_valid_encoding = is_valid_encoding && !third_byte[6];
        is_valid_encoding = is_valid_encoding && (third_byte[5] == code_point[5]);
        is_valid_encoding = is_valid_encoding && (third_byte[4] == code_point[4]);
        is_valid_encoding = is_valid_encoding && (third_byte[3] == code_point[3]);
        is_valid_encoding = is_valid_encoding && (third_byte[2] == code_point[2]);
        is_valid_encoding = is_valid_encoding && (third_byte[1] == code_point[1]);
        is_valid_encoding = is_valid_encoding && (third_byte[0] == code_point[0]);
      }

      CHECK(is_valid_encoding);
    }

    // [U+10000, U+110000]
    //
    {
      auto const bytes_per_code_point = 4;
      auto const code_point_begin     = char32_t{0x10000};
      auto const code_point_end       = char32_t{0x110000};

      auto code_points = std::vector<char32_t>();
      code_points.reserve(code_point_end - code_point_begin);
      for (char32_t code_point = code_point_begin; code_point < code_point_end; ++code_point) {
        code_points.push_back(code_point);
      }

      auto utf8_bytes = std::vector<char>();
      utf8_bytes.resize(bytes_per_code_point * code_points.size());

      furi::utf8_encode(code_points.begin(), code_points.end(), utf8_bytes.begin());

      auto is_valid_encoding = true;

      for (std::size_t idx = 0; idx < code_points.size(); ++idx) {
        auto const code_point = std::bitset<21>(code_points[idx]);

        auto const first_byte  = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 0]);
        auto const second_byte = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 1]);
        auto const third_byte  = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 2]);
        auto const fourth_byte = std::bitset<8>(utf8_bytes[(bytes_per_code_point * idx) + 3]);

        is_valid_encoding = is_valid_encoding && first_byte[7];
        is_valid_encoding = is_valid_encoding && first_byte[6];
        is_valid_encoding = is_valid_encoding && first_byte[5];
        is_valid_encoding = is_valid_encoding && first_byte[4];
        is_valid_encoding = is_valid_encoding && !first_byte[3];
        is_valid_encoding = is_valid_encoding && (first_byte[2] == code_point[20]);
        is_valid_encoding = is_valid_encoding && (first_byte[1] == code_point[19]);
        is_valid_encoding = is_valid_encoding && (first_byte[0] == code_point[18]);

        is_valid_encoding = is_valid_encoding && second_byte[7];
        is_valid_encoding = is_valid_encoding && !second_byte[6];
        is_valid_encoding = is_valid_encoding && (second_byte[5] == code_point[17]);
        is_valid_encoding = is_valid_encoding && (second_byte[4] == code_point[16]);
        is_valid_encoding = is_valid_encoding && (second_byte[3] == code_point[15]);
        is_valid_encoding = is_valid_encoding && (second_byte[2] == code_point[14]);
        is_valid_encoding = is_valid_encoding && (second_byte[1] == code_point[13]);
        is_valid_encoding = is_valid_encoding && (second_byte[0] == code_point[12]);

        is_valid_encoding = is_valid_encoding && third_byte[7];
        is_valid_encoding = is_valid_encoding && !third_byte[6];
        is_valid_encoding = is_valid_encoding && (third_byte[5] == code_point[11]);
        is_valid_encoding = is_valid_encoding && (third_byte[4] == code_point[10]);
        is_valid_encoding = is_valid_encoding && (third_byte[3] == code_point[9]);
        is_valid_encoding = is_valid_encoding && (third_byte[2] == code_point[8]);
        is_valid_encoding = is_valid_encoding && (third_byte[1] == code_point[7]);
        is_valid_encoding = is_valid_encoding && (third_byte[0] == code_point[6]);

        is_valid_encoding = is_valid_encoding && fourth_byte[7];
        is_valid_encoding = is_valid_encoding && !fourth_byte[6];
        is_valid_encoding = is_valid_encoding && (fourth_byte[5] == code_point[5]);
        is_valid_encoding = is_valid_encoding && (fourth_byte[4] == code_point[4]);
        is_valid_encoding = is_valid_encoding && (fourth_byte[3] == code_point[3]);
        is_valid_encoding = is_valid_encoding && (fourth_byte[2] == code_point[2]);
        is_valid_encoding = is_valid_encoding && (fourth_byte[1] == code_point[1]);
        is_valid_encoding = is_valid_encoding && (fourth_byte[0] == code_point[0]);
      }

      CHECK(is_valid_encoding);
    }
  }

  SECTION("[host] should not encode pct-encoded and the sub-delims")
  {
    auto const host = std::u32string_view(U"%20%13%24!$'()*+,;=");

    auto out = std::string(256, '\0');

    auto const end          = furi::uri::encode_host(host, out.begin());
    auto const encoded_host = std::string_view(out.data(), end - out.begin());

    CHECK(encoded_host == "%20%13%24!$'()*+,;=");
  }

  SECTION("[host] should pct-encode misc. whitespace chars")
  {
    auto const host = std::u32string_view(U"hello world!\n");
    auto       out  = std::string(256, '\0');

    auto const encoded_host =
      std::string_view(out.data(), furi::uri::encode_host(host, out.begin()) - out.begin());

    CHECK(encoded_host == "hello%20world!%0a");
  }

  SECTION("[host] should pct-encode a Polish hostname")
  {
    auto const host = std::u32string_view(U"www.\u017C\u00F3\u0142\u0107.pl");
    auto       out  = std::string(256, '\0');

    auto const encoded_host =
      std::string_view(out.data(), furi::uri::encode_host(host, out.begin()) - out.begin());

    CHECK(encoded_host == "www.%c5%bc%c3%b3%c5%82%c4%87.pl");
  }

  SECTION("[host] should not pct-encode a valid IPv4 address")
  {
    auto const host = std::u32string_view(U"127.0.0.1");
    auto       out  = std::string(256, '\0');

    auto const encoded_host =
      std::string_view(out.data(), furi::uri::encode_host(host, out.begin()) - out.begin());

    CHECK(encoded_host == "127.0.0.1");
  }

  SECTION("[host] should not pct-encode a valid IP-literal [IPv6 edition]")
  {
    auto const host = std::u32string_view(U"[::]");
    auto       out  = std::string(256, '\0');

    auto const encoded_host =
      std::string_view(out.data(), furi::uri::encode_host(host, out.begin()) - out.begin());

    CHECK(encoded_host == "[::]");
  }

  SECTION("[host] should pct-encode seemingly random entries into the ASCII set")
  {
    auto const host = std::u32string_view(U"\"#/<>?@[\\]^`{|}");
    auto       out  = std::string(256, '\0');

    auto const encoded_host =
      std::string_view(out.data(), furi::uri::encode_host(host, out.begin()) - out.begin());

    CHECK(encoded_host == "%22%23%2f%3c%3e%3f%40%5b%5c%5d%5e%60%7b%7c%7d");
  }

  SECTION("[host] should not pct-encode the ALPHA | DIGIT | -._~ | sub-delims")
  {
    auto const host = std::u32string_view(
      U"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-._~!$&'()*+,;=");

    auto out = std::string(256, '\0');

    auto const encoded_host =
      std::string_view(out.data(), furi::uri::encode_host(host, out.begin()) - out.begin());

    CHECK(encoded_host ==
          "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-._~!$&'()*+,;=");
  }

  SECTION("[path] should not pct-encode normal paths")
  {
    auto const path = std::u32string_view(
      U"/ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-._~!$&'()*+,;=");

    auto out = std::string(256, '\0');

    auto const encoded_path =
      std::string_view(out.data(), furi::uri::encode_path(path, out.begin()) - out.begin());

    CHECK(encoded_path ==
          "/ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-._~!$&'()*+,;=");
  }

  SECTION("[path] should not make decisions about pct-encoding the / separator")
  {
    auto const path = std::u32string_view(U"////1.234/432/@@");

    auto out = std::string(256, '\0');

    auto const encoded_path =
      std::string_view(out.data(), furi::uri::encode_path(path, out.begin()) - out.begin());

    CHECK(encoded_path == "////1.234/432/@@");
  }

  SECTION("[path] should pct-encode odd characters in the path")
  {
    auto const path = std::u32string_view(U"/\"#<>?@[\\]^`{|}:::");
    auto       out  = std::string(256, '\0');

    auto const encoded_path =
      std::string_view(out.data(), furi::uri::encode_path(path, out.begin()) - out.begin());

    CHECK(encoded_path == "/%22%23%3c%3e%3f@%5b%5c%5d%5e%60%7b%7c%7d:::");
  }

  SECTION("[path] shouldn't double-encode pct-encoded params in the path")
  {
    auto const path = std::u32string_view(U"/%20%21%22%23");
    auto       out  = std::string(256, '\0');

    auto const encoded_path =
      std::string_view(out.data(), furi::uri::encode_path(path, out.begin()) - out.begin());

    CHECK(encoded_path == "/%20%21%22%23");
  }

  SECTION("[query] shouldn't double-encode pct-encoded params in the query")
  {
    auto const query = std::u32string_view(U"?%20%21%22%23");
    auto       out   = std::string(256, '\0');

    auto const encoded_query =
      std::string_view(out.data(), furi::uri::encode_query(query, out.begin()) - out.begin());

    CHECK(encoded_query == "?%20%21%22%23");
  }

  SECTION("[query] shouldn't pct-encode the unreserved + sub-delims + /?")
  {
    auto const query = std::u32string_view(
      U"?ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-._~!$&'()*+,;=/?");
    auto out = std::string(256, '\0');

    auto const encoded_query =
      std::string_view(out.data(), furi::uri::encode_query(query, out.begin()) - out.begin());

    CHECK(encoded_query ==
          "?ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-._~!$&'()*+,;=/?");
  }

  SECTION("[query] should pct-encode seemingly random entries into the ASCII set")
  {
    auto const query = std::u32string_view(U"?//\":#/<>?@[\\]^`{|}");
    auto       out   = std::string(256, '\0');

    auto const encoded_query =
      std::string_view(out.data(), furi::uri::encode_query(query, out.begin()) - out.begin());

    CHECK(encoded_query == "?//%22%3a%23/%3c%3e?%40%5b%5c%5d%5e%60%7b%7c%7d");
  }

  SECTION("[query] should pct-encode Russian chars")
  {
    auto const query =
      std::u32string_view(U"?q=\u0412\u0441\u0435\u043c \u043f\u0440\u0438\u0432\u0435\u0442");
    auto out = std::string(256, '\0');

    auto const encoded_query =
      std::string_view(out.data(), furi::uri::encode_query(query, out.begin()) - out.begin());

    CHECK(encoded_query == "?q=%d0%92%d1%81%d0%b5%d0%bc%20%d0%bf%d1%80%d0%b8%d0%b2%d0%b5%d1%82");
  }
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/genmaskc.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/function/plus.hpp>

STF_CASE_TPL (" genmaskc real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::genmaskc;
  using r_t = decltype(genmaskc(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(genmaskc(bs::Inf<T>()), bs::Zero<r_t>());
  STF_EQUAL(genmaskc(bs::Minf<T>()), bs::Zero<r_t>());
  STF_EQUAL(genmaskc(bs::Nan<T>()), bs::Zero<r_t>());
#endif
  STF_IEEE_EQUAL(genmaskc(bs::Zero<T>()), bs::Allbits<r_t>());
} // end of test for floating_

STF_CASE_TPL (" genmaskc integer",  (int16_t)(int32_t)(int64_t)(uint32_t)(uint64_t))//STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::genmaskc;

  using r_t = decltype(genmaskc(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(genmaskc(bs::One<T>()), bs::Zero<r_t>());
  STF_EQUAL(genmaskc(bs::Valmax<T>()), bs::Zero<r_t>());
  STF_EQUAL(genmaskc(bs::plus(bs::Valmin<T>(), bs::One<T>())), bs::Zero<r_t>());
  STF_IEEE_EQUAL(genmaskc(bs::Zero<T>()), bs::Allbits<r_t>());
} // end of test for integer_


/*
 [auto_generated]
 libs/numeric/odeint/test/generation.cpp

 [begin_description]
 This file tests the generation functions.
 [end_description]

 Copyright 2011-2012 Karsten Ahnert
 Copyright 2011-2012 Mario Mulansky

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

#define BOOST_TEST_MODULE odeint_generation

#include <vector>

#include <boost/test/unit_test.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/numeric/odeint/config.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp>

using namespace boost::numeric::odeint;

template< class Stepper1 , class Stepper2 >
void check_stepper_type( const Stepper1 &s1 , const Stepper2 &s2 )
{
    BOOST_STATIC_ASSERT(( boost::is_same< Stepper1 , Stepper2 >::value ));
}

BOOST_AUTO_TEST_SUITE( generation_test )

typedef std::vector< double > state_type;

typedef runge_kutta_cash_karp54< state_type > rk54_type;
typedef runge_kutta_cash_karp54_classic< state_type > rk54_classic_type;
typedef runge_kutta_dopri5< state_type > dopri5_type;
typedef runge_kutta_fehlberg78< state_type > fehlberg78_type;
typedef rosenbrock4< double > rosenbrock4_type;

BOOST_AUTO_TEST_CASE( test_generation_dopri5 )
{
    check_stepper_type( make_controlled( 1.0e-6 , 1.0e-10 , dopri5_type() ) , controlled_runge_kutta< dopri5_type >() );
    check_stepper_type( make_controlled< dopri5_type >( 1.0e-6 , 1.0e-10 ) , controlled_runge_kutta< dopri5_type >() );

    check_stepper_type( make_dense_output( 1.0e-6 , 1.0e-10 , dopri5_type() ) , dense_output_runge_kutta< controlled_runge_kutta< dopri5_type > >() );
    check_stepper_type( make_dense_output< dopri5_type >( 1.0e-6 , 1.0e-10 ) , dense_output_runge_kutta< controlled_runge_kutta< dopri5_type > >() );
}

BOOST_AUTO_TEST_CASE( test_generation_cash_karp54 )
{
    check_stepper_type( make_controlled( 1.0e-6 , 1.0e-10 , rk54_type() ) , controlled_runge_kutta< rk54_type >() );
    check_stepper_type( make_controlled< rk54_type >( 1.0e-6 , 1.0e-10 ) , controlled_runge_kutta< rk54_type >() );
}

BOOST_AUTO_TEST_CASE( test_generation_cash_karp54_classic )
{
    check_stepper_type( make_controlled( 1.0e-6 , 1.0e-10 , rk54_classic_type() ) , controlled_runge_kutta< rk54_classic_type >() );
    check_stepper_type( make_controlled< rk54_classic_type >( 1.0e-6 , 1.0e-10 ) , controlled_runge_kutta< rk54_classic_type >() );
}

BOOST_AUTO_TEST_CASE( test_generation_fehlberg78 )
{
    check_stepper_type( make_controlled( 1.0e-6 , 1.0e-10 , fehlberg78_type() ) , controlled_runge_kutta< fehlberg78_type >() );
    check_stepper_type( make_controlled< fehlberg78_type >( 1.0e-6 , 1.0e-10 ) , controlled_runge_kutta< fehlberg78_type >() );
}

BOOST_AUTO_TEST_CASE( test_generation_rosenbrock4 )
{
    check_stepper_type( make_controlled( 1.0e-6 , 1.0e-10 , rosenbrock4_type() ) , rosenbrock4_controller< rosenbrock4_type >() );
    check_stepper_type( make_controlled< rosenbrock4_type >( 1.0e-6 , 1.0e-10 ) , rosenbrock4_controller< rosenbrock4_type >() );

    check_stepper_type( make_dense_output( 1.0e-6 , 1.0e-10 , rosenbrock4_type() ) , rosenbrock4_dense_output< rosenbrock4_controller< rosenbrock4_type > >() );
    check_stepper_type( make_dense_output< rosenbrock4_type >( 1.0e-6 , 1.0e-10 ) , rosenbrock4_dense_output< rosenbrock4_controller< rosenbrock4_type > >() );
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, Eigen::Dynamic, 4> test_float_D_4(const Eigen::Matrix<float, Eigen::Dynamic, 4> & M)
{
	return M;
}
void export_float_D_4()
{
	boost::python::def("test_float_D_4",test_float_D_4);
}


#include <boost/fusion/adapted/std_tuple.hpp>
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/home/x3.hpp>

#include "../include/parser_message_prefix.hpp"

namespace open_protocol::api
{

std::tuple<uint32_t, uint32_t, uint32_t> parse_message_prefix_revision(const std::string &s)
{

  namespace x3 = boost::spirit::x3;

  auto const uint_3_digits = x3::uint_parser<std::uint32_t, 10, 3, 3>{};
  auto const uint_4_digits = x3::uint_parser<std::uint32_t, 10, 4, 4>{};

  auto iter = s.cbegin();
  auto end_iter = s.cend();

  std::tuple<uint32_t, uint32_t, uint32_t> len_id_rev;

  x3::parse(iter, end_iter,
            uint_4_digits >> uint_4_digits >> uint_3_digits,
            len_id_rev);

  return len_id_rev;
}

} // namespace open_protocol::api


/*!
 * @file
 * Contains unit tests for the @ref Iterable typeclass.
 */

#include <boost/mpl11/iterable.hpp>

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/core.hpp>
#include <boost/mpl11/detail/test/iterable.hpp>

struct Minimal;
template <typename ...xs>
struct minimal_ { using mpl_datatype = Minimal; };

template <typename ...xs>
struct minimal { using type = minimal_<xs...>; };

namespace boost { namespace mpl11 {
    template <>
    struct Iterable<Minimal> : instantiate<Iterable>::with<Minimal> {
        template <typename>
        struct head_impl;

        template <typename x, typename ...xs>
        struct head_impl<minimal_<x, xs...>> : x { };

        template <typename>
        struct tail_impl;

        template <typename x, typename ...xs>
        struct tail_impl<minimal_<x, xs...>>
            : minimal<xs...>
        { };

        template <typename>
        struct is_empty_impl;

        template <typename ...xs>
        struct is_empty_impl<minimal_<xs...>>
            : bool_<sizeof...(xs) == 0>
        { };
    };
}} // end namespace boost::mpl11

struct tests
    : boost::mpl11::detail::test_Iterable<minimal>
{ };

int main() { }


//--------------------------------------------------------------------
//
// CovarianceMatrix.cpp
//
// Created on: Feb 1, 2018
// Author: Max
//
//--------------------------------------------------------------------

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <fstream>

#include <Launcher.h>
#include <CovarianceMatrix.h>

static const char COMMENT_CHAR = '#';
static const char NUMBER_SEPARATOR_CHAR = ',';
static const char ATTRIBUTE_NAME_CHAR = ':';

using namespace std;

using namespace multifaq::params;
using namespace multifaq::dir;
using namespace multifaq::config;

namespace phoenix = boost::phoenix;
using namespace boost::spirit;

CovarianceMatrix::CovarianceMatrix(shared_ptr<Launcher> launcher)
{
    _compiler = launcher->getCompiler();
    _td = launcher->getTreeDecomposition();
}

CovarianceMatrix::~CovarianceMatrix()
{
    delete[] _queryRootIndex;
}

void CovarianceMatrix::run()
{
    loadFeatures();
    modelToQueries();
    _compiler->compile();
}

void CovarianceMatrix::modelToQueries()
{
    size_t numberOfFunctions = 0;
    size_t featureToFunction[NUM_OF_VARIABLES],
        quadFeatureToFunction[NUM_OF_VARIABLES];

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var) && !_categoricalFeatures.test(var))
        {
            _compiler->addFunction(
                new Function({var}, Operation::sum));
            featureToFunction[var] = numberOfFunctions;
            numberOfFunctions++;
        }
    }

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var) && !_categoricalFeatures.test(var))
        {
            _compiler->addFunction(
                new Function({var}, Operation::quadratic_sum));
            quadFeatureToFunction[var] = numberOfFunctions;
            numberOfFunctions++;
        }
    }

    Aggregate* agg_count = new Aggregate();
    agg_count->_agg.push_back(prod_bitset());

    Query* count = new Query();
    count->_aggregates.push_back(agg_count);
    count->_rootID = _td->_root->_id;
    _compiler->addQuery(count);
    listOfQueries.push_back(count);

    size_t numberOfQueries = 0;
    size_t cont_var_root = _td->_root->_id;

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features[var])
            continue;

        prod_bitset prod_linear_v1;
        prod_bitset prod_quad_v1;

        if (!_categoricalFeatures[var])
        {
            // Linear function per Feature
            prod_linear_v1.set(featureToFunction[var]);

            Aggregate* agg_linear = new Aggregate();
            agg_linear->_agg.push_back(prod_linear_v1);

            Query* linear = new Query();
            linear->_aggregates.push_back(agg_linear);
            linear->_rootID = cont_var_root;

            _compiler->addQuery(linear);

            listOfQueries.push_back(linear);
            ++numberOfQueries;

            // Quadratic function for each feature
            prod_quad_v1.set(quadFeatureToFunction[var]);

            Aggregate* agg_quad = new Aggregate();
            agg_quad->_agg.push_back(prod_quad_v1);

            Query* quad = new Query();
            quad->_aggregates.push_back(agg_quad);
            quad->_rootID = _td->_root->_id; // cont_var_root;

            _compiler->addQuery(quad);

            listOfQueries.push_back(quad);
            ++numberOfQueries;
        }
        else
        {
            Aggregate* agg_linear = new Aggregate();
            agg_linear->_agg.push_back(prod_linear_v1);

            Query* linear = new Query();
            linear->_aggregates.push_back(agg_linear);
            linear->_fVars.set(var);
            linear->_rootID = _queryRootIndex[var];
            _compiler->addQuery(linear);

            listOfQueries.push_back(linear);
            ++numberOfQueries;
        }

        for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
        {
            if (_features[var2])
            {
                //*****************************************//
                prod_bitset prod_quad_v2 = prod_linear_v1;

                Aggregate* agg_quad_v2 = new Aggregate();

                Query* quad_v2 = new Query();
                quad_v2->_rootID = _td->_root->_id;

                // if (_categoricalFeatures[var])
                // {
                //     quad_v2->_rootID = _queryRootIndex[var];
                //     quad_v2->_fVars.set(var);
                //     if (_categoricalFeatures[var2])
                //     {
                //         quad_v2->_fVars.set(var2);
                //         // If both varaibles are categoricalVars - we choose the
                //         // var2 as the root
                //         // TODO: We should use the root that is highest to the
                //         // original root
                //         if (_queryRootIndex[var2] <= _queryRootIndex[var])
                //             quad_v2->_rootID = _queryRootIndex[var2];
                //     }
                //     else
                //     {
                //         prod_quad_v2.set(featureToFunction[var2]);
                //     }
                // }
                // else if (_categoricalFeatures[var2])
                // {
                //     quad_v2->_fVars.set(var2);
                //     quad_v2->_rootID = _queryRootIndex[var2];
                // }
                // else
                // {
                //     // create prod_quad_v2 of both var and var2
                //     prod_quad_v2.set(featureToFunction[var2]);
                // }
                if (_categoricalFeatures[var])
                {
                    quad_v2->_rootID = _queryRootIndex[var];
                    quad_v2->_fVars.set(var);
                }

                if (_categoricalFeatures[var2])
                {
                    quad_v2->_fVars.set(var2);
                    // If both varaibles are categoricalVars - we choose the
                    // var2 as the root
                    quad_v2->_rootID = _queryRootIndex[var2];
                }
                else
                {
                    // create prod_quad_v2 of both var and var2
                    prod_quad_v2.set(featureToFunction[var2]);
                }

                agg_quad_v2->_agg.push_back(prod_quad_v2);

                quad_v2->_aggregates.push_back(agg_quad_v2);
                _compiler->addQuery(quad_v2);

                // TODO: Here we add both sides! multiply var times param of
                // var2 -- add that to gradient of var. And then multiply var2
                // times param of var and add to gradient of var2! -- this can
                // be done togther in one pass over the vector!

                listOfQueries.push_back(quad_v2);
                ++numberOfQueries;
                // TODO: once we have identified the queries and how they are
                // multiplied, we then need to identify the actual views that
                // are computed over and use the information group computation
                // over this view!
            }
        }
    }
}

void CovarianceMatrix::loadFeatures()
{
    _queryRootIndex = new size_t[NUM_OF_VARIABLES]();

    /* Load the two-pass variables config file into an input stream. */
    ifstream input(FEATURE_CONF);

    if (!input)
    {
        ERROR(FEATURE_CONF +" does not exist. \n");
        exit(1);
    }

    /* String and associated stream to receive lines from the file. */
    string line;
    stringstream ssLine;

    int numOfFeatures = 0;
    int degreeOfInteractions = 0;

    /* Ignore comment and empty lines at the top */
    while (getline(input, line))
    {
        if (line[0] == COMMENT_CHAR || line == "")
            continue;

        break;
    }

    /*
     * Extract number of labels, features and interactions from the config.
     * Parse the line with the three numbers; ignore spaces.
     */
    bool parsingSuccess =
        qi::phrase_parse(line.begin(), line.end(),

                         /* Begin Boost Spirit grammar. */
                         ((qi::int_[phoenix::ref(numOfFeatures) = qi::_1])
                          >> NUMBER_SEPARATOR_CHAR
                          >> (qi::int_[phoenix::ref(degreeOfInteractions) =
                                       qi::_1])),
                         /*  End grammar. */
                         ascii::space);

    assert(parsingSuccess && "The parsing of the features file has failed.");

    /* Read in the features. */
    for (int featureNo = 0; featureNo < numOfFeatures; ++featureNo)
    {
        getline(input, line);
        if (line[0] == COMMENT_CHAR || line == "")
        {
            --featureNo;
            continue;
        }

        ssLine << line;

        string attrName;
        /* Extract the name of the attribute in the current line. */
        getline(ssLine, attrName, ATTRIBUTE_NAME_CHAR);

        string typeOfFeature;
        /* Extract the dimension of the current attribute. */
        getline(ssLine, typeOfFeature, ATTRIBUTE_NAME_CHAR);

        string rootName;
        /* Extract the dimension of the current attribute. */
        getline(ssLine, rootName, ATTRIBUTE_NAME_CHAR);

        int attributeID = _td->getAttributeIndex(attrName);
        int categorical = stoi(typeOfFeature);
        int rootID = _td->getRelationIndex(rootName);

        if (attributeID == -1)
        {
            ERROR("Attribute |"+attrName+"| does not exist.");
            exit(1);
        }

        if (rootID == -1)
        {
            ERROR("Relation |"+rootName+"| for "+attrName+" does not exist.");
            exit(1);
        }

        if (featureNo == 0 && categorical == 1)
        {
            ERROR("The label needs to be continuous! ");
            exit(1);
        }

        _isFeature.set(attributeID);
        _features.set(attributeID);

        Feature f;

        if (categorical)
        {
            _categoricalFeatures.set(attributeID);
            _queryRootIndex[attributeID] = rootID;

            f.head.set(attributeID);
            _isCategoricalFeature.set(attributeID);
        }
        else
        {
            _queryRootIndex[attributeID] = _td->_root->_id;
            ++f.body[attributeID];
        }

        _listOfFeatures.push_back(f);

        if (featureNo == 0)
            labelID = attributeID;

        /* Clear string stream. */
        ssLine.clear();
    }
}

void CovarianceMatrix::generateCode()
{
    std::string dumpListOfQueries = "";

    for (Query* query : listOfQueries)
    {
        std::pair<size_t,size_t>& viewAggPair = query->_aggregates[0]->_incoming[0];

        dumpListOfQueries += std::to_string(viewAggPair.first)+","+
            std::to_string(viewAggPair.second)+"\\n";
    }

    std::string runFunction = offset(1)+"void runApplication()\n"+offset(1)+"{\n"+
        offset(2)+"std::ofstream ofs(\"times.txt\",std::ofstream::out | std::ofstream::app);\n"+
        offset(2)+"ofs << \"\\n\";\n"+
        offset(2)+"ofs.close();\n\n"+
        "#ifdef DUMP_OUTPUT\n"+
        offset(2)+"ofs.open(\"output/covarianceMatrix.out\");\n"+
        offset(2)+"ofs << \""+dumpListOfQueries+"\";\n"+
        offset(2)+"ofs.close();\n"+
        "#endif /* DUMP_OUTPUT */ \n"+
        offset(1)+"}\n";

    std::ofstream ofs(multifaq::dir::OUTPUT_DIRECTORY+"ApplicationHandler.h", std::ofstream::out);
    ofs << "#ifndef INCLUDE_APPLICATIONHANDLER_HPP_\n"<<
        "#define INCLUDE_APPLICATIONHANDLER_HPP_\n\n"<<
        "#include \"DataHandler.h\"\n\n"<<
        "namespace lmfao\n{\n"<<
        offset(1)+"void runApplication();\n"<<
        "}\n\n#endif /* INCLUDE_APPLICATIONHANDLER_HPP_*/\n";
    ofs.close();

    ofs.open(multifaq::dir::OUTPUT_DIRECTORY+"ApplicationHandler.cpp", std::ofstream::out);
    ofs << "#include \"ApplicationHandler.h\"\n"
        << "namespace lmfao\n{\n";
    ofs << runFunction << std::endl;
    ofs << "}\n";
    ofs.close();
}


//
// Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH.
// Copyright (c) 2015-2017, University of Bremen
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// 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.
//
#pragma once

#include "SurfacePatches.hpp"

#include <boost/serialization/access.hpp>
#include <boost/serialization/nvp.hpp>

namespace maps { namespace grid
{

struct OccupancyConfiguration
{
    OccupancyConfiguration(double hit_probability = 0.7, double miss_probability = 0.4,
                         double occupied_probability = 0.8, double free_space_probability = 0.3,
                         double max_probability = 0.971, double min_probability = 0.1192,
                         float uncertainty_threshold = 0.25) :
                        hit_logodds(OccupancyPatch::logodds(hit_probability)),
                        miss_logodds(OccupancyPatch::logodds(miss_probability)),
                        occupied_logodds(OccupancyPatch::logodds(occupied_probability)),
                        free_space_logodds(OccupancyPatch::logodds(free_space_probability)),
                        max_logodds(OccupancyPatch::logodds(max_probability)) ,
                        min_logodds(OccupancyPatch::logodds(min_probability)),
                        uncertainty_threshold(uncertainty_threshold) {}

    float hit_logodds;
    float miss_logodds;
    float occupied_logodds;
    float free_space_logodds;
    float max_logodds;
    float min_logodds;
    float uncertainty_threshold;

protected:
    /** Grants access to boost serialization */
    friend class boost::serialization::access;

    /** Serializes the members of this class*/
    template <typename Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
        ar & BOOST_SERIALIZATION_NVP(hit_logodds);
        ar & BOOST_SERIALIZATION_NVP(miss_logodds);
        ar & BOOST_SERIALIZATION_NVP(occupied_logodds);
        ar & BOOST_SERIALIZATION_NVP(free_space_logodds);
        ar & BOOST_SERIALIZATION_NVP(max_logodds);
        ar & BOOST_SERIALIZATION_NVP(min_logodds);
        ar & BOOST_SERIALIZATION_NVP(uncertainty_threshold);
    }
};

}}


/*
    BSD 3-Clause License

    Copyright (c) 2018, Roboy
            All rights reserved.

    Redistribution and use in source and binary forms, with or without
            modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.

    * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.

    * Neither the name of the copyright holder nor the names of its
    contributors may be used to endorse or promote products derived from
    this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
            IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
            FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
            DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
            SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
            CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
    OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

    author: Simon Trendel ( simon.trendel@tum.de ), 2018
    description: triangulation, pose estimation, lighthouse pose correction, factory calibration
*/

#include "darkroom/Transform.hpp"
#include "darkroom/Triangulation.hpp"
#include "darkroom/PoseEstimatorSensorCloud.hpp"
#include "darkroom/PoseEstimatorMultiLighthouse.hpp"
#include "darkroom/Sensor.hpp"
#include <common_utilities/rviz_visualization.hpp>
#include <common_utilities/CommonDefinitions.h>
#include <roboy_communication_middleware/LighthousePoseCorrection.h>
#include <roboy_communication_middleware/DarkRoomSensor.h>
#include <roboy_communication_middleware/DarkRoomOOTX.h>
#include <roboy_communication_middleware/ArucoPose.h>
#include <common_utilities/CommonDefinitions.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <boost/filesystem.hpp>
#include <atomic>
#include <mutex>
#include "darkroom/InYourGibbousPhase.hpp"
#include "darkroom/InYourGibbousPhase2.hpp"
#include "darkroom/InYourGibbousPhase3.hpp"
#include <ros/package.h>
#include "darkroom/Utilities.hpp"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <optimization.h>
#include <epnp/epnp.h>

using namespace alglib;

namespace fs = boost::filesystem;

#define MAX_ITERATIONS 100
#define ERROR_THRESHOLD 0.0000001

#define NUMBER_OF_SAMPLES 100

#define NUMBER_OF_PARTICLES 1000

static vector<int> DEFAULT_VECTOR;

void function1_fvec(const real_1d_array &x, real_1d_array &fi, void *ptr);

class LighthouseEstimator
        : public DarkRoom::Transform, public rviz_visualization, public Triangulation, public Utilities {
public:
    LighthouseEstimator();

    /**
     * This returns the sensors that are calibrated and visible by both lighthouses
     * @param visible_sensors will be filled with sensor ids
     */
    void getVisibleCalibratedSensors(vector<int> &visible_sensors);

    /**
     * This returns the sensors that are calibrated and visible to the given lighthouse
     * @param visible_sensors will be filled with sensor ids
     */
    void getVisibleCalibratedSensors(bool lighthouse, vector<int> &visible_sensors);

    /**
     * Estimates the pose correction between ligthhouse 1 and 2, such that the squared distances between sensor positions
     * estimated for both lighthouses is minimized.
     * @return success
     */
    bool lighthousePoseEstimationLeastSquares();

    /**
     * Estimates the pose of an object using relative sensor position information and least square matching
     */
    void objectPoseEstimationLeastSquares();

    /**
    * Estimates the sensor distances of all active sensors (or a vector of specified sensor ids)
    * using the known (ie. calibrated) relative distance between the sensors and the lighthouse angles
    * @param lighthouse for which lighthouse
    * @param specificIds if defined, waits until the specified sensors become active
    * @return
    */
    bool estimateSensorPositionsUsingRelativeDistances(bool lighthouse, vector<int> &specificIds = DEFAULT_VECTOR);

    /**
    * Estimates the sensor distances of all active sensors
    * using the known (ie. calibrated) relative distance between the sensors and the lighthouse angles, then estimates
    * the object pose relative to each ligthhouse
    * @return success
    */
    bool estimateObjectPoseUsingRelativeDistances();

    /**
     * Estimates object pose using multi lighthouse approach
     */
    void estimateObjectPoseMultiLighthouse();
    /**
     * Triangulates the sensor positions (the transform between lighthouse 1 and 2 needs to be known, otherwise the
     * triangulated position is not correct)
     */
    void triangulateSensors();

    /**
     * Estimates relative object pose using epnp
     */
    void estimateObjectPoseEPNP();
    /**
     * Publishes the lighthouse rays
     */
    void publishRays();

    /**
     * Measures triangulated sensor locations for 30 seconds. Calculates mean sensor locations and generates
     * relative sensor positions which are saved to a yaml file
     */
    void calibrateRelativeSensorDistances();

    /**
     * Estimates calibration values based on known sensor angles
     * @param lighthouse for this lighthouse
     * @return success
     */
    bool estimateFactoryCalibration(int lighthouse);

    /**
     * Estimates calibration values using epnp
     * @param lighthouse for this lighthouse
     * @return success
     */
    bool estimateFactoryCalibrationEPNP(int lighthouse);

    /**
     * Estimates calibration values using multi lighthouse pose estimator
     * @param lighthouse for this lighthouse
     * @return success
     */
    bool estimateFactoryCalibrationMultiLighthouse(int lighthouse);

    /**
     * Estimates calibration values based on known sensor angles
     * @param lighthouse for this lighthouse
     * @return success
     */
    bool estimateFactoryCalibration2(int lighthouse);

    /**
     * Returns a unique id for #MESSAGE_ID sensor and lighthouse
     * @param type the message type #MESSAGE_ID
     * @param sensor the sensor id
     * @param lighthouse the lighthouse
     * @return a unique id
     */
    int getMessageID(int type, int sensor, bool lighthouse = false);

    enum MESSAGE_ID {
        TRIANGULATED = 0,      // for each sensor
        DISTANCE = 1,           // for each sensor and lighthouse
        RAY = 2,   // for each sensor and lighthouse
        SENSOR_NAME = 3,   // for each sensor
        DISTANCES = 4
    };

    enum POSE_CORRECTION_TYPE {
        RELATIV = 0,
        ABSOLUT = 1,
        OBJECT = 2
    };

    map<int, Sensor> sensors;
    vector<int> calibrated_sensors;
    map<int, vector<double>> calibration_angles;
    int active_sensors = 0;
    atomic<bool> tracking, calibrating, poseestimating, poseestimating_epnp, poseestimating_multiLighthouse, objectposeestimating,
            distances, rays, particle_filtering, use_lighthouse_calibration_data_phase[2],
            use_lighthouse_calibration_data_tilt[2], use_lighthouse_calibration_data_gibphase[2],
            use_lighthouse_calibration_data_gibmag[2];
    mutex mux;
    fs::path mesh;
    bool has_mesh = false;
    string name = "bastiisdoff";
    string imu_topic_name, pose_topic_name;
    ros::Publisher pose_pub;
    tf::Transform pose;
    static int trackedObjectInstance; //! a unique object instance (helps with unique rviz marker ids)
private:
    void receiveOOTXData(const roboy_communication_middleware::DarkRoomOOTX::ConstPtr &msg);

    void applyCalibrationData(Vector2d &lighthouse0_angles, Vector2d &lighthouse1_angles);

    void applyCalibrationData(bool lighthouse, Vector2d &lighthouse_angles);

    void applyCalibrationData(bool lighthouse, double &elevation, double &azimuth);

    MatrixXd Pinv(MatrixXd A);

private:
    ros::NodeHandlePtr nh;
    boost::shared_ptr<ros::AsyncSpinner> spinner;
    ros::Publisher sensor_location_pub, lighthouse_pose_correction;
    ros::Subscriber ootx_sub;
    VectorXd object_pose;
    OOTXframe ootx[2];
};