/**
 * @file polynomial_gcd.hh
 * @author 李昊坤 (ker@pm.me)
 * @brief 定义polynomial_gcd
*/
#ifndef CLPOLY_POLYNOMIAL_GCD_HH
#define CLPOLY_POLYNOMIAL_GCD_HH
#include <clpoly/polynomial.hh>
#include <clpoly/upolynomial.hh>
#include <boost/math/special_functions/prime.hpp>
#include <cmath>
#include <cstdint>
#include <vector>
#include <cassert>
#include <random>

namespace clpoly{

    template <class comp>
    basic_monomial<comp> gcd(const basic_monomial<comp> &m1,const basic_monomial<comp> &m2)
    {
        assert(comp_consistent(m1.comp(),m2.comp()));
        basic_monomial<comp> m(m1.comp_ptr());
        if (m1.empty()|| m2.empty())
        {
            return m;
        }
        m.reserve(m1.size());
        auto m1_ptr=m1.begin();
        auto m2_ptr=m2.begin();
        while (m1_ptr!=m1.end() && m2_ptr!=m2.end())
        {
            if (m1.comp(m1_ptr->first,m2_ptr->first))
            {
                ++m1_ptr;
            }
            else
            {
                if (m1_ptr->first==m2_ptr->first)
                {
                    m.push_back({m1_ptr->first,std::min(m1_ptr->second,m2_ptr->second)});
                    ++m1_ptr;
                }
                ++m2_ptr;
            }
        }
        return m;
    }
    template<class var_order>
    polynomial_<ZZ,lex_<var_order>> cont(const polynomial_<ZZ,lex_<var_order>> &F_);
    template<class var_order>
    polynomial_<ZZ,lex_<var_order>> leadcoeff(const polynomial_<ZZ,lex_<var_order>> &F_);
    template<class var_order>
    int64_t  __polynomial_GCD(       polynomial_<Zp,lex_<var_order>> & Pout,
                            const polynomial_<Zp,lex_<var_order>> & F,
                            const polynomial_<Zp,lex_<var_order>> & G,
                            const polynomial_<Zp,lex_<var_order>> & Lc_gcd,
                            int64_t deg);
    template<class var_order>
    polynomial_<ZZ,lex_<var_order>> polynomial_GCD(polynomial_<ZZ,lex_<var_order>>  F, polynomial_<ZZ,lex_<var_order>>  G);
    template <class compare>
    inline polynomial_<ZZ,compare> polynomial_GCD(const polynomial_<ZZ,compare>  &F, const polynomial_<ZZ,compare> & G)
    {
        polynomial_<ZZ,lex> F_,G_;
        poly_convert(F,F_);
        poly_convert(G,G_);
        polynomial_<ZZ,compare>  Pout(F.comp_ptr());
        poly_convert(polynomial_GCD(std::move(F_),std::move(G_)),Pout);
        return Pout;
    }
    template <class comp>
    inline polynomial_<ZZ,comp> gcd(const polynomial_<ZZ,comp> &F,const polynomial_<ZZ,comp> & G)
    {
        return polynomial_GCD(F,G);
    }
    template<class var_order>
    bool is_squarefree (const polynomial_<ZZ,lex_<var_order>> &  F)
    {
        if (F.empty())
            return false;
        if (is_number(F))
            return true;
        auto f_cont=cont(F);
        if (is_squarefree(f_cont))
        {
            auto F_=F/f_cont;
            auto F_1=polynomial_GCD(F_,derivative(F_));
            if (is_number(F_1))
                return true;
        }
        return false;
    }
    template<class comp>
    bool is_squarefree (const polynomial_<ZZ,comp> &  F)
    {
        polynomial_<ZZ,lex> F_;
        poly_convert(F,F_);
        return is_squarefree(F_);
    }
    template<class var_order>
    std::vector<std::pair<polynomial_<ZZ,lex_<var_order>>,uint64_t>> squarefreefactorize (const polynomial_<ZZ,lex_<var_order>> &  F)
    {
        // std::cout<<"F:"<<F<<std::endl;
        if (F.empty())
            return {};
        if (is_number(F))
            return {{F,1}};
        auto f_cont=cont(F);
        auto F_=F/f_cont;
        auto lst=squarefreefactorize(f_cont);
        polynomial_<ZZ,lex_<var_order>> F_1(F.comp_ptr()),F_2(F.comp_ptr()),F_3(F.comp_ptr());
        for(uint64_t i=1;;++i)
        {
            // std::cout<<"F_:"<<F_<<std::endl;
            // std::cout<<"D(F_):"<<derivative(F_)<<std::endl;

            F_1=polynomial_GCD(F_,derivative(F_));
            // std::cout<<"F_1:"<<F_1<<std::endl;
            if (is_number(F_1))
            {
                if (i>1 && F_!=F_3 )
                {
                    lst.push_back({F_3/F_,i-1});
                }
                lst.push_back({F_,i});
                break;
            }
            F_2=F_/F_1;
            // std::cout<<"F_2:"<<F_2<<std::endl;
            if (F_2!=F_3)
            {
                if (i>1)
                {
                    lst.push_back({F_3/F_2,i-1});
                }
                F_3=std::move(F_2);

            }
            F_=std::move(F_1);
        }
        return lst;
    }
    template <class comp>
    std::vector<std::pair<polynomial_<ZZ,comp>,uint64_t>> squarefreefactorize (const polynomial_<ZZ,comp> &  F)
    {
        polynomial_<ZZ,lex> F_;
        poly_convert(F,F_);
        auto lst=squarefreefactorize(F_);
        std::vector<std::pair<polynomial_<ZZ,comp>,uint64_t>> lst_(lst.size(),{polynomial_<ZZ,comp>(F.comp_ptr()),0});
        for (uint64_t i=0;i<lst.size();++i)
        {
            poly_convert(lst[i].first,lst_[i].first);
            lst_[i].second=lst[i].second;
        }
        return lst_;
    }
    template<class var_order>
    std::pair<std::vector<polynomial_<ZZ,lex_<var_order>>>,
              std::vector<std::vector<std::pair<uint64_t,uint64_t>>>>
    squarefreebasis (const std::vector<polynomial_<ZZ,lex_<var_order>>>&  F)
    {
        std::vector<polynomial_<ZZ,lex_<var_order>>> lst,lst_;
        std::vector<std::vector<std::pair<uint64_t,uint64_t>>> I,I_;
        for (size_t i=0;i<F.size();++i)
        {
            lst_.clear();
            I_.clear();
            std::vector<std::pair<polynomial_<ZZ,lex_<var_order>>,uint64_t>>  l_=squarefreefactorize(F[i]);
            if (l_.empty())
                continue;
            auto l_ptr=l_.begin();
            for (++l_ptr;l_ptr!=l_.end();++l_ptr)
            {
                auto tmp=std::move(l_ptr->first);
                for (size_t j=0;j<lst.size();++j)
                {
                    auto F1=polynomial_GCD(lst[j],tmp);
                    if (!is_number(F1))
                    {
                        tmp=tmp/F1;
                        if (F1!=lst[j])
                        {
                            lst[j]=lst[j]/F1;
                            lst_.push_back(std::move(F1));
                            I_.push_back(I[j]);
                            I_.back().push_back({i,l_ptr->second});
                        }
                        else{
                            I[j].push_back({i,l_ptr->second});
                        }
                        if (is_number(tmp))
                            break;
                    }
                }
                if (!is_number(tmp))
                {
                    lst_.push_back(std::move(tmp));
                    I_.push_back({{i,l_ptr->second}});
                }
            }
            // lst.reserve(lst.size()+lst_.size());
            lst.insert(lst.end(),lst_.begin(),lst_.end());
            I.insert(I.end(),I_.begin(),I_.end());
            // for (auto &j:lst_)
            // {
            //      lst.push_back(std::move(j));
            // }
        }
        return {std::move(lst),std::move(I)};
    }

    template <class comp>
    std::pair<std::vector<polynomial_<ZZ,comp>>,
              std::vector<std::vector<std::pair<uint64_t,uint64_t>>>>
    squarefreebasis (const std::vector<polynomial_<ZZ,comp>> &  F)
    {
        std::vector<polynomial_<ZZ,comp>> L;
        if (F.empty())
            return {{},{}};
        std::vector<polynomial_<ZZ,lex>> lst;
        polynomial_<ZZ,lex> p;
        lst.reserve(F.size());
        for (auto &i:F)
        {
            poly_convert(i,p);
            lst.push_back(std::move(p));
        }
        auto l_=squarefreebasis(lst);
        lst=std::move(l_.first);
        L.reserve(lst.size());
        polynomial_<ZZ,comp> p1(F.front().comp_ptr());
        for (auto &i:lst)
        {
            poly_convert(i,p1);
            L.push_back(std::move(p1));
        }
        return {std::move(L),std::move(l_.second)};
    }

    template<class var_order>
    polynomial_<ZZ,lex_<var_order>> polynomial_GCD(polynomial_<ZZ,lex_<var_order>>  F, polynomial_<ZZ,lex_<var_order>>  G)
    {
        assert(comp_consistent(F.comp(),G.comp()));
        if (F.empty())
            return G;
        if (G.empty())
            return F;
        if (F.size()==1 || G.size()==1)
        {
            auto ptr=F.begin();
            auto m=ptr->first;
            auto c=ptr->second;
            for(++ptr;ptr!=F.end();++ptr)
            {
                m=gcd(m,ptr->first);
                c=gcd(c,ptr->second);
            }
            for(ptr=G.begin();ptr!=G.end();++ptr)
            {
                m=gcd(m,ptr->first);
                c=gcd(c,ptr->second);
            }
            polynomial_<ZZ,lex_<var_order>> Pout(F.comp_ptr());
            Pout={{m,c}};
            return Pout;

        }

        variable ffv,gfv;
        while ((ffv=get_first_var(F))!=(gfv=get_first_var(G)) || !ffv.serial())
        {
            if (!ffv.serial() || !gfv.serial())
            {
                ZZ cont=0;
                for(auto &i:F)
                    cont=gcd(cont,i.second);
                for(auto &i:G)
                    cont=gcd(cont,i.second);
                polynomial_<ZZ,lex_<var_order>> Pout(F.comp_ptr());
                Pout={{basic_monomial<lex_<var_order>>(F.comp_ptr()),cont}};
                return Pout;
            }
            if (F.comp(ffv,gfv))
            {
                F=cont(F);
            }
            else
            {
                G=cont(G);
            }
        }
        polynomial_<ZZ,lex_<var_order>> F_cont=cont(F);
        polynomial_<ZZ,lex_<var_order>> G_cont=cont(G);
        polynomial_<ZZ,lex_<var_order>> cont_gcd=polynomial_GCD(cont(F),cont(G));

        F=F/F_cont;G=G/G_cont;
        polynomial_<ZZ,lex_<var_order>> lc_gcd=polynomial_GCD(leadcoeff(F),leadcoeff(G));

        // std::cout<<"F:"<<F<<std::endl;
        // std::cout<<"G:"<<G<<std::endl;
        // std::cout<<"cont_gcd:"<<cont_gcd<<std::endl;
        // std::cout<<"lc_gcd:"<<lc_gcd<<std::endl;

        std::uint32_t tmp_x=std::max(degree(F),degree(G));
        if (tmp_x<2) tmp_x=2;
        std::uint32_t p_index=tmp_x/std::log(tmp_x);
        std::uint32_t prime=boost::math::prime(p_index);
        while (prime <tmp_x)
        {
            prime=boost::math::prime(++p_index);
        }

        polynomial_<ZZ,lex_<var_order>> Pout_(F.comp_ptr()),tmp_Pout_(F.comp_ptr()),R(F.comp_ptr());
        polynomial_<Zp,lex_<var_order>> Pout_mod(F.comp_ptr()),f_p(F.comp_ptr()),g_p(F.comp_ptr()),lc_gcd_p(F.comp_ptr());
        polynomial_<ZZ,lex_<var_order>> tmp(F.comp_ptr());
        ZZ Pout_prime;
        std::int64_t Pout_d=INT64_MAX;
        std::int64_t tmp_Pout_d=INT64_MAX;

        while (1)
        {

            while (F.begin()->second % prime ==0 || G.begin()->second % prime ==0)
            {
                prime=boost::math::prime(++p_index);
            }
            f_p=polynomial_mod(F,prime);
            g_p=polynomial_mod(G,prime);
            lc_gcd_p=polynomial_mod(lc_gcd,prime);

            // std::cout<<"p:"<<prime<<std::endl;
            // std::cout<<"f_p:"<<f_p<<std::endl;
            // std::cout<<"g_p:"<<g_p<<std::endl;
            // std::cout<<"lc_gcd_p:"<<lc_gcd_p<<std::endl;

            tmp_Pout_d=__polynomial_GCD(Pout_mod,f_p,g_p,lc_gcd_p,Pout_d);
            if (tmp_Pout_d==-1)
            {
                prime=boost::math::prime(++p_index);
                continue;
            }

            // std::cout<<"poly_mod:"<<Pout_mod<<std::endl;

            if (tmp_Pout_d < Pout_d)
            {
                Pout_d=tmp_Pout_d;
                Pout_prime=prime;
                poly_convert(Pout_mod,Pout_);
                for (auto &i:Pout_)
                {
                    i.second%=Pout_prime;
                    if (i.second>Pout_prime/2)
                    {
                        i.second-=Pout_prime;
                    }
                }

                // std::cout<<"Pout_:"<<Pout_<<std::endl;

            }
            else
            {
                Zp tmp_inv(Pout_prime,prime);
                tmp_inv=tmp_inv.inv();
                tmp_Pout_.clear();
                tmp_Pout_.reserve(Pout_.size());
                auto Pout_ptr=Pout_.begin();
                auto Pout_end=Pout_.end();
                auto Pm_ptr=Pout_mod.begin();
                auto Pm_end=Pout_mod.end();
                while (Pout_ptr!=Pout_end && Pm_ptr!=Pm_end)
                {
                    if (F.comp(Pout_ptr->first,Pm_ptr->first))
                    {
                        tmp_Pout_.push_back({Pout_ptr->first,Pout_ptr->second-Pout_ptr->second*tmp_inv.number()*Pout_prime});
                        ++Pout_ptr;
                    }
                    else
                    {
                        if (Pout_ptr->first==Pm_ptr->first)
                        {
                            tmp_Pout_.push_back({std::move(Pm_ptr->first),Pout_ptr->second+
                            (Pm_ptr->second.number()-Pout_ptr->second)*tmp_inv.number()*Pout_prime});
                            ++Pout_ptr;
                        }
                        else
                        {
                            tmp_Pout_.push_back({std::move(Pm_ptr->first),Pm_ptr->second.number()*tmp_inv.number()*Pout_prime});

                        }
                        ++Pm_ptr;
                    }

                }
                while (Pout_ptr!=Pout_end)
                {
                    tmp_Pout_.push_back({std::move(Pout_ptr->first),Pout_ptr->second-Pout_ptr->second*tmp_inv.number()*Pout_prime});
                    ++Pout_ptr;
                }
                while (Pm_ptr!=Pm_end)
                {
                    tmp_Pout_.push_back({std::move(Pm_ptr->first),Pm_ptr->second.number()*tmp_inv.number()*Pout_prime});
                    ++Pm_ptr;
                }
                Pout_prime*=prime;
                for (auto &i:tmp_Pout_)
                {
                    i.second%=Pout_prime;
                    if (i.second>Pout_prime/2)
                    {
                        i.second-=Pout_prime;
                    }
                    else if (i.second<-Pout_prime/2)
                    {
                        i.second+=Pout_prime;
                    }
                }

                // std::cout<<"Pout_:"<<tmp_Pout_<<std::endl;

                if (tmp_Pout_==Pout_)
                {
                    if (get_first_var(Pout_)==get_first_var(F))
                    {
                        auto cont_=cont(Pout_);
                        // std::cout<<"cont_:"<<cont_<<std::endl;
                        pair_vec_div(tmp.data(),tmp_Pout_.data(),cont_.data(),F.comp());
                        std::swap(tmp,tmp_Pout_);

                        // std::cout<<tmp_Pout_<<std::endl;

                        pair_vec_div(tmp.data(),R.data(),F.data(),tmp_Pout_.data(),F.comp());
                        if (R.empty())
                        {
                            pair_vec_div(tmp.data(),R.data(),G.data(),tmp_Pout_.data(),F.comp());
                            if (R.empty())
                            {
                                pair_vec_multiplies(tmp.data(),tmp_Pout_.data(),cont_gcd.data(),F.comp());
                                return tmp;
                            }
                        }
                    }
                    else
                    {
                        return cont_gcd;
                    }
                }
                swap(tmp_Pout_.data(),Pout_.data());

            }
            prime=boost::math::prime(++p_index);
        }

    }

    template<class var_order>
    polynomial_<ZZ,lex_<var_order>> cont(const polynomial_<ZZ,lex_<var_order>> &F_)
    {
        // std::cout<<F_<<std::endl;
        polynomial_<ZZ,lex_<var_order>>  cont(F_.comp_ptr()), tmp(F_.comp_ptr());
        auto v=get_first_var(F_);
        int64_t deg=get_first_deg(F_);
        int64_t tmp_deg=deg;
        basic_monomial<lex_<var_order>> m(F_.comp_ptr());
        for (auto &i:F_)
        {
            if ((!i.first.empty() &&  i.first.front().first==v && i.first.front().second == tmp_deg) ||
                ((i.first.empty() || i.first.front().first!=v)  && tmp_deg==0))
            {
                m.clear();
                m.reserve(i.first.size());
                auto ptr=i.first.begin();
                if (tmp_deg)
                    ++ptr;
                for (;ptr!=i.first.end();++ptr)
                {
                    m.push_back(*ptr);
                }
                tmp.push_back({std::move(m),i.second});
            }
            else{
                if (tmp_deg==deg)
                {
                    cont=std::move(tmp);
                    // std::cout<<"c:"<<cont<<std::endl;
                }
                else
                {
                    cont=polynomial_GCD(cont,tmp);
                    // std::cout<<"c:"<<tmp<<std::endl;
                    // std::cout<<"c:"<<cont<<std::endl;
                }
                tmp_deg=(!i.first.empty() && i.first.front().first==v )?i.first.front().second : 0;
                tmp.clear();
                m.clear();
                m.reserve(i.first.size());
                auto ptr=i.first.begin();
                if (tmp_deg)
                    ++ptr;
                for (;ptr!=i.first.end();++ptr)
                {
                    m.push_back(*ptr);
                }
                tmp.push_back({std::move(m),i.second});
            }
        }
        if (tmp_deg==deg)
        {
            cont=std::move(tmp);
            // std::cout<<"c:"<<cont<<std::endl;
        }
        else
        {
            cont=polynomial_GCD(cont,tmp);
            // std::cout<<"c:"<<tmp<<std::endl;
            // std::cout<<"c:"<<cont<<std::endl;
        }
        return cont;
    }
    // polynomial_<ZZ,univariate_priority_order> cont(const polynomial_<ZZ,univariate_priority_order> &F_)
    // {
    //     auto & v_order=F_.comp();
    //     polynomial_<ZZ,univariate_priority_order>  cont(&v_order),
    //                                                cont_(&v_order),
    //                                                tmp(&v_order);
    //     int64_t deg=get_up_deg(F_);
    //     int64_t tmp_deg=deg;
    //     for (auto &i:F_)
    //     {
    //         if (get_up_deg(i.first)==tmp_deg)
    //         {
    //             tmp.push_back(i);
    //             if (tmp_deg)
    //                 tmp.back().first.pop_back();
    //         }
    //         else{
    //             if (tmp_deg==deg)
    //             {
    //                 cont=std::move(tmp);

    //             }
    //             else
    //             {
    //                 cont=polynomial_GCD(cont,tmp);

    //             }
    //             tmp_deg=get_up_deg(i.first);
    //             tmp.clear();
    //             tmp.push_back(i);
    //             if (tmp_deg)
    //                 tmp.back().first.pop_back();
    //         }
    //     }
    //     if (tmp_deg==deg)
    //     {
    //         cont=std::move(tmp);
    //     }
    //     else
    //     {
    //         cont=polynomial_GCD(cont,tmp);
    //     }
    //     return cont;
    // }

    template<class var_order>
    int64_t  __polynomial_GCD(       polynomial_<Zp,lex_<var_order>> & Pout,
                            const polynomial_<Zp,lex_<var_order>> & F,
                            const polynomial_<Zp,lex_<var_order>> & G,
                            const polynomial_<Zp,lex_<var_order>> & Lc_gcd,
                            int64_t deg)
    {
        int64_t deg_;
        variable fvf,gvf;
        assert(!F.empty() && !G.empty() && !Lc_gcd.empty());
        fvf=get_first_var(F);
        gvf=get_first_var(G);
        assert(fvf==gvf);

        if (!fvf.serial())
        {
            assert(Lc_gcd.size()==1 && Lc_gcd.begin()->first.empty());
            Pout=Lc_gcd;
            return 0;
        }
        auto flvd=get_last_var_deg(F);
        auto glvd=get_last_var_deg(G);
        polynomial_<Zp,lex_<var_order>> Pout_;
        polynomial_<Zp,lex_<var_order>> Pout_1;
        polynomial_<Zp,lex_<var_order>> Pout_2;
        if (fvf==flvd.first && gvf==glvd.first)
        {
            Pout=G;
            pair_vec_div(Pout_2.data(),Pout_1.data(),F.data(),Pout.data(),Pout.comp());
            //std::cout<<Pout_1<<std::endl;
            while(!Pout_1.empty())
            {
                swap(Pout.data(),Pout_.data());
                swap(Pout.data(),Pout_1.data());
                pair_vec_div(Pout_2.data(),Pout_1.data(),Pout_.data(),Pout.data(),Pout.comp());
                //std::cout<<Pout_1<<std::endl;
            }
            assert(Lc_gcd.size()==1 && Lc_gcd.begin()->first.empty());
            Zp lc_inv=Pout.front().second.inv()*Lc_gcd.begin()->second;
            for (auto &i:Pout)
                i.second*=lc_inv;
            deg_=get_first_deg(Pout);
            if (deg_<=deg)
                return deg_;
            else
                return -1;
        }
        else
        {
            int64_t f_d=get_first_deg(F);
            int64_t g_d=get_first_deg(G);
            uint32_t prime=F.begin()->second.prime();
            Zp p_(prime);
            auto & comp=F.comp();
            polynomial_<Zp,lex_<var_order>> F_v(F.comp_ptr());
            polynomial_<Zp,lex_<var_order>> G_v(F.comp_ptr());
            polynomial_<Zp,lex_<var_order>> lc_v(F.comp_ptr());
            int64_t num_s=0;
            int64_t v_d;
            variable v;
            if (flvd.first==glvd.first)
            {
                v=flvd.first;
                v_d=std::max(flvd.second,glvd.second)+1;
            }
            else
            {
                v=comp(flvd.first,glvd.first)?glvd.first:flvd.first;
                v_d=1;
            }
            std::vector<std::pair<basic_monomial<lex_<var_order>>,std::vector<Zp>>> _Pout,tmp_Pout;
            std::vector<Zp> points;
            std::vector<Zp> tmp_p;
            points.reserve(v_d);
            std::random_device rd;
            std::mt19937 gen(rd());
            std::vector<int> v_bool(prime,1);
            uint32_t p_tmp;
            for (int32_t i=0;i<prime;++i)
            {
                // p_.number()=i;
                std::uniform_int_distribution<uint64_t> dis(1, prime-i);
                p_tmp=dis(gen);
                uint64_t j_tmp=0;
                for (;p_tmp>0;p_tmp-=v_bool[j_tmp],++j_tmp);
                v_bool[j_tmp-1]=0;p_.number()=j_tmp-1;

                F_v=assign(F,v,p_);
                G_v=assign(G,v,p_);
                lc_v=assign(Lc_gcd,v,p_);
                //std::cout<<v<<"->"<<p_<<std::endl;
                //std::cout<<"F_v:"<<F_v<<std::endl;
                //std::cout<<"G_v:"<<G_v<<std::endl;
                //std::cout<<"lc_v:"<<Lc_gcd<<std::endl;

                if (get_first_deg(F_v)==f_d && get_first_var(F_v)==fvf && get_first_deg(G_v)==g_d && get_first_var(G_v)==gvf )
                {
                    deg_=__polynomial_GCD(Pout_,F_v,G_v,lc_v,deg);
                    //std::cout<<"v:"<<v<<" deg:"<<deg_<<" GDD:"<<Pout_<<std::endl;
                    if (deg_!=-1 && deg_<=deg)
                    {
                        if (deg_<deg || !num_s)
                        {
                            deg=deg_;
                            num_s=1;
                            _Pout.clear();
                            for (auto &i:Pout_)
                            {
                                _Pout.push_back({std::move(i.first),{i.second}});
                            }
                            points={p_};
                        }
                        else
                        {
                            ++num_s;
                            tmp_Pout.clear();
                            tmp_Pout.reserve(Pout_.size());
                            points.push_back(p_);
                            auto _P_ptr=_Pout.begin();
                            auto _P_end=_Pout.end();
                            auto P_ptr=Pout_.begin();
                            auto P_end=Pout_.end();

                            while (_P_ptr!=_P_end && P_ptr!=P_end)
                            {
                                if (comp(P_ptr->first,_P_ptr->first))
                                {
                                    tmp_p.clear();
                                    tmp_p.resize(num_s,Zp(0,prime));
                                    tmp_p.back()=P_ptr->second;
                                    tmp_Pout.push_back({std::move(P_ptr->first),std::move(tmp_p)});
                                    ++P_ptr;
                                }
                                else{
                                    if (P_ptr->first==_P_ptr->first)
                                    {
                                        tmp_Pout.push_back(std::move(*_P_ptr));
                                        tmp_Pout.back().second.push_back(P_ptr->second);
                                        ++P_ptr;
                                    }else
                                    {
                                        tmp_Pout.push_back(std::move(*_P_ptr));
                                        tmp_Pout.back().second.push_back(Zp(0,prime));
                                    }
                                    ++_P_ptr;
                                }
                            }
                            while (P_ptr!=P_end)
                            {
                                tmp_p.clear();
                                tmp_p.resize(num_s,Zp(0,prime));
                                tmp_p.back()=P_ptr->second;
                                tmp_Pout.push_back({std::move(P_ptr->first),std::move(tmp_p)});
                                ++P_ptr;
                            }
                            while (_P_ptr!=_P_end)
                            {
                                tmp_Pout.push_back(std::move(*_P_ptr));
                                tmp_Pout.back().second.push_back(Zp(0,prime));
                                ++_P_ptr;
                            }
                            swap(tmp_Pout,_Pout);

                        }
                        if (num_s==v_d)
                        {
                            //std::cout<<std::endl;
                            std::vector<Zp> lag;
                            lag.resize(v_d*v_d);
                            Zp tmp_inv;
                            for (int64_t i=0;i<v_d;++i)
                            {
                                lag[i*v_d]=Zp(1,prime);
                                for (int64_t j=1;j<v_d;++j)
                                    lag[i*v_d+j]=Zp(0,prime);
                                for (int64_t j=0;j<v_d;++j)
                                    if (i!=j)
                                    {
                                        tmp_inv=(points[i]-points[j]).inv();
                                        for (int64_t k=v_d-1;k>0;--k)
                                        {
                                            lag[i*v_d+k]=(lag[i*v_d+k-1]-lag[i*v_d+k]*points[j])*tmp_inv;
                                        }
                                        lag[i*v_d]=(-lag[i*v_d]*points[j])*tmp_inv;

                                    }

                            }
                            Pout.clear();
                            Pout.reserve(_Pout.size());
                            basic_monomial<lex_<var_order>> m(F.comp_ptr());
                            for (auto &i:_Pout)
                            {
                                m=i.first;
                                m.push_back({v,0});

                                for (int64_t j=v_d-1;j>=0;--j)
                                {
                                    p_.number()=0;

                                    for (int64_t k=0;k<v_d;++k)
                                    {
                                        p_+=i.second[k]*lag[k*v_d+j];
                                    }
                                    if (p_)
                                    {
                                        if (j)
                                        {
                                            m.back().second=j;
                                            m.deg()+=j;
                                            Pout.push_back({m,p_});
                                            m.deg()-=j;
                                        }
                                        else
                                        {
                                            Pout.push_back({std::move(i.first),p_});
                                        }

                                    }

                                }

                            }
                            return deg;
                        }

                    }

                }
                if (prime<=i+v_d-num_s)
                {
                    return -1;
                }
            }

            return -1;

        }

    }
    inline ZZ cont(const upolynomial_<ZZ> &G)
    {
        if (G.empty())
            return 1;
        auto ptr=G.begin();
        ZZ c=(ptr++)->second;
        for (;ptr!=G.end();++ptr)
        {
            c=gcd(c,ptr->second);
        }
        return c;

    }

    inline umonomial gcd(const umonomial & G,const umonomial& F)
    {
        return umonomial(std::min(G.deg(),F.deg()));
    }
    inline int64_t  __polynomial_GCD(upolynomial_<Zp> &Pout,
                            const upolynomial_<Zp> &G,
                            const upolynomial_<Zp> &F,
                            const Zp & Lc_gcd,
                            int64_t deg)
    {
        int64_t deg_;
        assert(!F.empty() && !G.empty() );
        upolynomial_<Zp> Pout_;
        upolynomial_<Zp> Pout_1;
        upolynomial_<Zp> Pout_2;
        Pout=G;
        pair_vec_div(Pout_2.data(),Pout_1.data(),F.data(),Pout.data(),Pout.comp());
        //std::cout<<Pout_1<<std::endl;
        while(!Pout_1.empty())
        {
            std::swap(Pout.data(),Pout_.data());
            std::swap(Pout.data(),Pout_1.data());
            pair_vec_div(Pout_2.data(),Pout_1.data(),Pout_.data(),Pout.data(),Pout.comp());
            //std::cout<<Pout_1<<std::endl;
        }
        Zp lc_inv=Pout.front().second.inv()*Lc_gcd;
        for (auto &i:Pout)
            i.second*=lc_inv;
        deg_=Pout.front().first.deg();
        if (deg_<=deg)
            return deg_;
        else
            return -1;
    }
    upolynomial_<ZZ>  polynomial_GCD(upolynomial_<ZZ> G,upolynomial_<ZZ> F);

}
#endif


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//          Doug Gregor, D. Kevin McGrath
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/config.hpp>
#include <vector>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/bandwidth.hpp>

/*
  Sample Output
  original bandwidth: 8
  Reverse Cuthill-McKee ordering starting at: 6
    8 3 0 9 2 5 1 4 7 6
    bandwidth: 4
  Reverse Cuthill-McKee ordering starting at: 0
    9 1 4 6 7 2 8 5 3 0
    bandwidth: 4
  Reverse Cuthill-McKee ordering:
    0 8 5 7 3 6 4 2 1 9
    bandwidth: 4
 */
int main(int , char* [])
{
  using namespace boost;
  using namespace std;
  typedef adjacency_list<vecS, vecS, undirectedS,
     property<vertex_color_t, default_color_type,
       property<vertex_degree_t,int> > > Graph;
  typedef graph_traits<Graph>::vertex_descriptor Vertex;
  typedef graph_traits<Graph>::vertices_size_type size_type;

  typedef std::pair<std::size_t, std::size_t> Pair;
  Pair edges[14] = { Pair(0,3), //a-d
                     Pair(0,5),  //a-f
                     Pair(1,2),  //b-c
                     Pair(1,4),  //b-e
                     Pair(1,6),  //b-g
                     Pair(1,9),  //b-j
                     Pair(2,3),  //c-d
                     Pair(2,4),  //c-e
                     Pair(3,5),  //d-f
                     Pair(3,8),  //d-i
                     Pair(4,6),  //e-g
                     Pair(5,6),  //f-g
                     Pair(5,7),  //f-h
                     Pair(6,7) }; //g-h

  Graph G(10);
  for (int i = 0; i < 14; ++i)
    add_edge(edges[i].first, edges[i].second, G);

  graph_traits<Graph>::vertex_iterator ui, ui_end;

  property_map<Graph,vertex_degree_t>::type deg = get(vertex_degree, G);
  for (boost::tie(ui, ui_end) = vertices(G); ui != ui_end; ++ui)
    deg[*ui] = degree(*ui, G);

  property_map<Graph, vertex_index_t>::type
    index_map = get(vertex_index, G);

  std::cout << "original bandwidth: " << bandwidth(G) << std::endl;

  std::vector<Vertex> inv_perm(num_vertices(G));
  std::vector<size_type> perm(num_vertices(G));
  {
    Vertex s = vertex(6, G);
    //reverse cuthill_mckee_ordering
    cuthill_mckee_ordering(G, s, inv_perm.rbegin(), get(vertex_color, G),
                           get(vertex_degree, G));
    cout << "Reverse Cuthill-McKee ordering starting at: " << s << endl;
    cout << "  ";
    for (std::vector<Vertex>::const_iterator i = inv_perm.begin();
         i != inv_perm.end(); ++i)
      cout << index_map[*i] << " ";
    cout << endl;

    for (size_type c = 0; c != inv_perm.size(); ++c)
      perm[index_map[inv_perm[c]]] = c;
    std::cout << "  bandwidth: "
              << bandwidth(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
  }
  {
    Vertex s = vertex(0, G);
    //reverse cuthill_mckee_ordering
    cuthill_mckee_ordering(G, s, inv_perm.rbegin(), get(vertex_color, G),
                           get(vertex_degree, G));
    cout << "Reverse Cuthill-McKee ordering starting at: " << s << endl;
    cout << "  ";
    for (std::vector<Vertex>::const_iterator i=inv_perm.begin();
       i != inv_perm.end(); ++i)
      cout << index_map[*i] << " ";
    cout << endl;

    for (size_type c = 0; c != inv_perm.size(); ++c)
      perm[index_map[inv_perm[c]]] = c;
    std::cout << "  bandwidth: "
              << bandwidth(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
  }

  {
    //reverse cuthill_mckee_ordering
    cuthill_mckee_ordering(G, inv_perm.rbegin());

    cout << "Reverse Cuthill-McKee ordering:" << endl;
    cout << "  ";
    for (std::vector<Vertex>::const_iterator i=inv_perm.begin();
       i != inv_perm.end(); ++i)
      cout << index_map[*i] << " ";
    cout << endl;

    for (size_type c = 0; c != inv_perm.size(); ++c)
      perm[index_map[inv_perm[c]]] = c;
    std::cout << "  bandwidth: "
              << bandwidth(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
  }
  return 0;
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/numeric/odeint.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/asif.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

using namespace std::chrono_literals;
using namespace boost::numeric::odeint;

using Time = std::chrono::duration<double>;

template<typename T>
using G = Eigen::Matrix<T, 2, 1>;
template<typename T>
using U = Eigen::Matrix<T, 1, 1>;

using Gd = G<double>;
using Ud = U<double>;

int main()
{
  // dynamics
  auto f = []<typename T>(Time, const G<T> & x, const U<T> & u) -> smooth::Tangent<G<T>> {
    return {x(1), u(0)};
  };

  // safety set
  auto h = []<typename T>(T, const G<T> & g) -> Eigen::Vector2<T> {
    return {T(3) - g(0), T(1.5) - g(1)};
  };

  // backup controller
  auto bu = []<typename T>(T, const G<T> &) -> U<T> { return U<T>(-0.6); };

  // parameters
  smooth::feedback::ASIFilterParams<Ud> prm{
    .T  = 0.5,
    .nh = 2,
    .ulim =
      {
        .A = U<double>{{1.}},
        .l = U<double>{{-1.}},
        .u = U<double>{{1.}},
      },
    .asif =
      {
        .K          = 50,
        .alpha      = 1,
        .dt         = 0.01,
        .relax_cost = 100,
      },
    .qp =
      {
        .polish = false,
      },
  };

  // create filter
  smooth::feedback::ASIFilter<Time, Gd, Ud, decltype(f)> asif(f, prm);

  // system variables
  Gd g(-5, 1);
  Ud udes = Ud(1), u;

  // prepare for integrating the closed-loop system
  runge_kutta4<Gd, double, smooth::Tangent<Gd>, double, vector_space_algebra> stepper{};
  const auto ode = [&f, &u](const Gd & x, smooth::Tangent<Gd> & d, double t) {
    d = f(Time(t), x, u);
  };
  std::vector<double> tvec, xvec, vvec, uvec;

  // integrate closed-loop system
  for (std::chrono::milliseconds t = 0s; t < 10s; t += 50ms) {
    auto [u_asif, code] = asif(t, g, udes, h, bu);

    u = u_asif;

    if (code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "Solver failed with code " << static_cast<int>(code) << std::endl;
    }

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(g.x());
    vvec.push_back(g.y());
    uvec.push_back(u.x());

    // step dynamics
    stepper.do_step(ode, g, 0, 0.05);
  }

#ifdef ENABLE_PLOTTING
  using namespace matplot;

  figure();
  hold(on);

  plot(tvec, xvec)->line_width(2);
  plot(tvec, vvec)->line_width(2);
  plot(tvec, transform(tvec, [&](auto) { return 3; }), "--")->line_width(2);
  plot(tvec, transform(tvec, [&](auto) { return 1.5; }), "--")->line_width(2);
  legend({"x", "v", "x_{max}", "v_{max}"});

  figure();
  hold(on);
  title("Input");
  plot(tvec, uvec)->line_width(2);
  plot(tvec, transform(tvec, [](auto) { return 1; }), "--")->line_width(2);

  show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", v=" << vvec[i] << std::endl;
  }
#endif
}


#include <iostream>

#include <Eigen/Dense>
#include <fstream>
#include <vector>
#include <iomanip>      // std::setprecision

using Eigen::MatrixXd;
int main() {

    Eigen::Matrix<double, 2, 1> xi;
    Eigen::Matrix<double, 2, 1> particle_size;

    //! Dimension
    const unsigned Dim = 2;
    //! Nodes in GIMP function
    // 16 for 2D 4 for 1D
    const unsigned Nfunctions = 4;

    //! To store shape functions
    Eigen::Matrix<double, 16, 1> shapefn;

    //! To store grad shape functions
    Eigen::Matrix<double, 16, 2> grad_shapefn;

    //! length of element in local coordinate
    // Keep this value constant
    const double element_length = 2.;

    // Local coordinates of 2D GIMP cells
    /*const Eigen::Matrix<double, Nfunctions, Dim> local_nodes =
            (Eigen::Matrix<double, Nfunctions, Dim>() << -1., -1.,

                    1., -1.,
                    1.,  1.,
                    -1.,  1.,
                    -3., -3.,
                    -1., -3.,
                    1., -3.,
                    3., -3.,
                    3., -1.,
                    3.,  1.,
                    3.,  3.,
                    1.,  3.,
                    -1.,  3.,
                    -3.,  3.,
                    -3.,  1.,
                    -3., -1.).finished();*/

    // Local coordinates of 1D GIMP cells
    const Eigen::Matrix<double, Nfunctions, Dim> local_nodes =
            (Eigen::Matrix<double, Nfunctions, Dim>() <<

		-3.,0.,
		-1.,0.,
		 1.,0.,
		 3.,0.
		).finished();

    // particle location
    xi << -4.0, 0.0;
    // particle size
    particle_size << 1.1, 1.1;

    double interval = 0.05;

    std::vector<double> function_store;
    std::vector<double> gfunction_store;

    for (unsigned k = 0; k < 120; ++k) {

	xi(0) += interval;

	//Function loop
        for (unsigned n = 0; n < Nfunctions; ++n) {
            Eigen::Matrix<double, 2, 1> sni;
            Eigen::Matrix<double, 2, 1> dni;

		// GIMP conditional statement loop
            for (unsigned i = 0; i < Dim; ++i) {
		//length of particle
                double lp = particle_size(i) * 0.5;
		// active node
                double ni = local_nodes(n, i);
		// local particle  - local node
                double npni = xi(i) - ni;
                //! Conditional shape function statement
                // see: Pruijn, N.S., 2016. Eq(4.30)
                if (npni <= (-element_length - lp)) {
                    sni(i) = 0.;
                    dni(i) = 0.;
                } else if ((-element_length - lp) < npni &&
                           npni <= (-element_length + lp)) {

                    sni(i) = std::pow(element_length + lp + npni, 2.) /
                             (4. * (element_length * lp));
                    dni(i) = (element_length + lp + npni) / (2. * element_length * lp);
                } else if ((-element_length + lp) < npni && npni <= -lp) {
                    sni(i) = 1. + (npni / element_length);
                    dni(i) = 1. / element_length;
                } else if (-lp < npni && npni <= lp) {
                    sni(i) =
                            1. - (((npni * npni) + (lp * lp)) / (2. * element_length * lp));
                    dni(i) = -(npni / (element_length * lp));
                } else if (lp < npni && npni <= (element_length - lp)) {
                    sni(i) = 1. - (npni / element_length);
                    dni(i) = -(1. / element_length);
                } else if ((element_length - lp) < npni &&
                           npni <= (element_length + lp)) {
                    sni(i) = std::pow(element_length + lp - npni, 2.) /
                             (4. * element_length * lp);
                    dni(i) = -((element_length + lp - npni) / (2. * element_length * lp));
                } else if ((element_length + lp) < npni) {
                    sni(i) = 0.;
                    dni(i) = 0.;
                } else {
                    throw std::runtime_error(
                            "GIMP grad shapefn: Point location outside area of influence");
                }
            }
            // 2D Shape
            //shapefn(n) = sni(0) * sni(1);

            // 1D Shape value @ node n
	    shapefn(n) = sni(0);

	    // store 1D shape function value at specific node (n)
	    if (n == 1 )
	    function_store.push_back(shapefn(n));

            // 2D Grad
            //grad_shapefn(n, 0) = dni(0) * sni(1);
            //grad_shapefn(n, 1) = dni(1) * sni(0);

            // 1D Grad
            grad_shapefn(n, 0) = dni(0);

	    // store 1D grad function value at specific node (n)
	    if (n == 1 )
	    gfunction_store.push_back(grad_shapefn(n,0));
        }
    }
    std::cout << "function store size: " << function_store.size() << '\n';
    for (unsigned n = 0; n < Nfunctions; ++n) {
        std::cout << n << " "
                  << " s(" << local_nodes(n, 0) << " , " << local_nodes(n, 1)
                  << ") : " << shapefn(n) << '\n';
        //std::cout << n << " " << std::setprecision(9) << grad_shapefn(n,1) << '\n';
    }

    //! Output file
    std::string xvalfilename = "snvals.txt";
    std::fstream xvalfile;
    xvalfile.open(xvalfilename, std::ios::out);

    if (xvalfile.is_open()) {
        //! Write
	//function_store for shape function, gfunction_store for gradient
        for (auto const& xvals : gfunction_store) {
            xvalfile << xvals << '\n';
        }
        xvalfile.close();
    }
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)

#ifndef BOOST_MATH_FFT_ALGORITHMS_HPP
  #define BOOST_MATH_FFT_ALGORITHMS_HPP

  #include <algorithm>
  #include <numeric>
  #include <cmath>
  #include <vector>
  #include <boost/math/constants/constants.hpp>
  #include <boost/math/fft/discrete_maths.hpp>
  #include <boost/container/static_vector.hpp>

  namespace boost { namespace math {  namespace fft {

  namespace detail {

  template<typename InputIterator1,
           typename InputIterator2,
           typename OutputIterator,
           typename allocator_t >
  void raw_convolution(InputIterator1 input1_begin,
                   InputIterator1 input1_end,
                   InputIterator2 input2_begin,
                   OutputIterator output,
                   const allocator_t& alloc);

  template<class ComplexType>
  ComplexType complex_root_of_unity(long n,long p=1)
  /*
    Computes exp(-i 2 pi p/n)
  */
  {
    using real_value_type = typename ComplexType::value_type;
    p = modulo(p,n);

    if(p==0)
      return ComplexType(1,0);

    long g = gcd(p,n);
    n/=g;
    p/=g;
    switch(n)
    {
      case 1:
        return ComplexType(1,0);
      case 2:
        return p==0 ? ComplexType(1,0) : ComplexType(-1,0);
      case 4:
        return p==0 ? ComplexType(1,0) :
               p==1 ? ComplexType(0,-1) :
               p==2 ? ComplexType(-1,0) :
                      ComplexType(0,1) ;
    }
    using std::sin;
    using std::cos;
    real_value_type phase = -2*p*boost::math::constants::pi<real_value_type>()/n;
    return ComplexType(cos(phase),sin(phase));
  }
  template<class ComplexType>
  ComplexType complex_inverse_root_of_unity(long n,long p=1)
  /*
    Computes exp(i 2 pi p/n)
  */
  {
    return complex_root_of_unity<ComplexType>(n,-p);
  }

  template<class complex_value_type>
  inline void complex_dft_2(
    const complex_value_type* in,
    complex_value_type* out, int)
  {
    complex_value_type
        o1 = in[0]+in[1], o2 = in[0]-in[1] ;
    out[0] = o1;
    out[1] = o2;
  }

  template<class T>
  void dft_prime_bruteForce_outofplace(const T* in_first, const T* in_last, T* out, const T w)
  /*
    assumptions:
    - allocated memory in out is enough to hold distance(in_first,in_last) element,
    - out!=in
  */
  {
    const long N = static_cast<long>(std::distance(in_first,in_last));
    if(N<=0)
      return;

    out[0] = std::accumulate(in_first+1,in_last,in_first[0]);

    T wi=w;
    for(long i=1;i<N;++i, wi*=w)
    {
      T wij{wi};
      T sum{in_first[0]};
      for(long j=1;j<N; ++j, wij*=wi)
      {
        sum += in_first[j]*wij;
      }
      out[i] = sum;
    }
  }
  template<class T,class allocator_t>
  void dft_prime_bruteForce_inplace(
    T* in_first,
    T* in_last,
    const T w,
    const allocator_t& alloc)
  {
    std::vector<T,allocator_t> work_space(in_first,in_last,alloc);
    dft_prime_bruteForce_outofplace(in_first,in_last,work_space.data(),w);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class allocator_t>
  void dft_prime_bruteForce(
    const T* in_first,
    const T* in_last,
    T* out,
    const T w,
    const allocator_t& alloc)
  {
    if(in_first==out)
      dft_prime_bruteForce_inplace(out,out+std::distance(in_first,in_last),w,alloc);
    else
      dft_prime_bruteForce_outofplace(in_first,in_last,out,w);
  }

  template<class complex_value_type>
  void complex_dft_prime_bruteForce_outofplace(
    const complex_value_type* in_first,
    const complex_value_type* in_last,
    complex_value_type* out, int sign)
  /*
    assumptions:
    - allocated memory in out is enough to hold distance(in_first,in_last) element,
    - out!=in
  */
  {
    const long N = static_cast<long>(std::distance(in_first,in_last));
    if(N<=0)
      return;

    out[0] = std::accumulate(in_first+1,in_last,in_first[0]);

    for(long i=1;i<N;++i)
    {
      complex_value_type sum{in_first[0]};
      for(long j=1;j<N; ++j)
      {
        sum += in_first[j] * complex_root_of_unity<complex_value_type>(N,i*j*sign);
      }
      out[i] = sum;
    }
  }

  template<class complex_value_type,class Allocator_t>
  void complex_dft_prime_bruteForce_inplace(
    complex_value_type* in_first,
    complex_value_type* in_last,
    int sign,
    const Allocator_t& alloc)
  {
    std::vector<complex_value_type,Allocator_t> work_space(in_first,in_last,alloc);
    complex_dft_prime_bruteForce_outofplace(in_first,in_last,work_space.data(),sign);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class complex_value_type, class Allocator_t>
  void complex_dft_prime_bruteForce(
    const complex_value_type* in_first,
    const complex_value_type* in_last,
    complex_value_type* out,
    int sign,
    const Allocator_t& alloc)
  {
    if(in_first==out)
      complex_dft_prime_bruteForce_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      complex_dft_prime_bruteForce_outofplace(in_first,in_last,out,sign);
  }

  /*
    Rader's FFT on prime sizes
  */
  template<class complex_value_type, class allocator_t>
  void complex_dft_prime_rader(
    const complex_value_type *in_first,
    const complex_value_type *in_last,
    complex_value_type* out,
    int sign,
    const allocator_t& alloc = allocator_t{})
  // precondition: distance(in_first,in_last) is prime > 2
  {
    using allocator_type = allocator_t;
    const long my_n = static_cast<long>(std::distance(in_first,in_last));

    std::vector<complex_value_type,allocator_type> A(my_n-1,complex_value_type(),alloc);
    std::vector<complex_value_type,allocator_type> W(my_n-1,complex_value_type(),alloc);
    std::vector<complex_value_type,allocator_type> B(my_n-1,complex_value_type(),alloc);

    const long g = primitive_root(my_n);
    const long g_inv = power_mod(g,my_n-2,my_n);

    for(long i=0;i<my_n-1;++i)
    {
      W[i] = complex_root_of_unity<complex_value_type>(my_n,sign*power_mod(g_inv,i,my_n));
      A[i] = in_first[ power_mod(g,i+1,my_n) ];
    }

    raw_convolution(A.begin(),A.end(),W.begin(),B.begin(),alloc);

    complex_value_type a0 = in_first[0];
    complex_value_type sum_a {a0};
    for(long i=1;i<my_n;++i)
        sum_a += in_first[i];

    out[0] = sum_a;
    for(long i=1;i<my_n;++i)
    {
      out[i]=a0;
    }
    for(long i=1;i<my_n;++i)
    {
      out[ power_mod(g_inv,i,my_n) ] += B[i-1];
    }
  }

  template <class T, class allocator_t>
  void dft_composite_outofplace(const T *in_first,
                     const T *in_last,
                     T* out,
                     const T e,
                     const allocator_t& alloc)
  {
    /*
      Cooley-Tukey mapping, intrinsically out-of-place, Decimation in Time
      composite sizes.
    */
    using allocator_type = allocator_t;

    const long n = static_cast<unsigned int>(std::distance(in_first,in_last));
    if(n <=0 )
      return;

    if (n == 1)
    {
        out[0]=in_first[0];
        return;
    }
    std::array<int,32> prime_factors;
    const int nfactors = prime_factorization(n,prime_factors.begin());

    // reorder input
    for (long i = 0; i < n; ++i)
    {
        long j = 0, k = i;
        for (int ip=0;ip<nfactors;++ip)
        {
            int p = prime_factors[ip];
            j = j * p + k % p;
            k /= p;
        }
        out[j] = in_first[i];
    }

    std::reverse(prime_factors.begin(), prime_factors.begin()+nfactors);

    // butterfly pattern
    long len = 1;
    for (int ip=0;ip<nfactors;++ip)
    {
      int p = prime_factors[ip];
      long len_old = len;
      len *= p;
      T w_len = power(e, n / len);
      T w_p = power(e,n/p);

      std::vector<T,allocator_type> tmp(p,T(),alloc);
      for (long i = 0; i < n; i += len)
      {
        for(long k=0;k<len_old;++k)
        {
          for(long j=0;j<p;++j)
            if(j==0 || k==0)
              tmp[j] = out[i + j*len_old +k ];
            else
              tmp[j] = out[i + j*len_old +k ] * power(w_len,k*j);

          dft_prime_bruteForce_inplace(tmp.data(),tmp.data()+p,w_p,alloc);

          for(long j=0;j<p;++j)
            out[i+ j*len_old + k] = tmp[j];
        }
      }
    }
  }

  template<class T,class Allocator_t>
  void dft_composite_inplace(
    T* in_first,
    T* in_last,
    const T e,
    const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    dft_composite_outofplace(in_first,in_last,work_space.data(),e,alloc);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void dft_composite(
    const T* in_first,
    const T* in_last,
    T* out,
    const T e,
    const Allocator_t& alloc)
  {
    if(in_first==out)
      dft_composite_inplace(out,out+std::distance(in_first,in_last),e,alloc);
    else
      dft_composite_outofplace(in_first,in_last,out,e,alloc);
  }

  template <class ComplexType, class allocator_t>
  void complex_dft_composite_outofplace(const ComplexType *in_first,
                             const ComplexType *in_last,
                             ComplexType* out,
                             int sign,
                             const allocator_t& alloc)
  {
    /*
      Cooley-Tukey mapping, intrinsically out-of-place, Decimation in Time
      composite sizes.
    */
    using allocator_type = allocator_t;
    const long n = static_cast<long>(std::distance(in_first,in_last));
    if(n <=0 )
      return;

    if (n == 1)
    {
        out[0]=in_first[0];
        return;
    }
    std::array<int,32> prime_factors;
    const int nfactors = prime_factorization(n,prime_factors.begin());

    // reorder input
    for (long i = 0; i < n; ++i)
    {
        long j = 0, k = i;
        for (int ip=0;ip<nfactors;++ip)
        {
            int p = prime_factors[ip];
            j = j * p + k % p;
            k /= p;
        }
        out[j] = in_first[i];
    }

    std::reverse(prime_factors.begin(), prime_factors.begin()+nfactors);

    //auto show = [&] (const ComplexType* beg, const ComplexType* end)
    //{
    //  for(;beg!=end;++beg)
    //  {
    //    std::cout << *beg << ", ";
    //  }
    //  std::cout << "\n";
    //};

    // butterfly pattern
    long len = 1;
    for (int ip=0;ip<nfactors;++ip)
    {
      //std::cout << "pass " << ip << "\n";
      int p = prime_factors[ip];
      long len_old = len;
      len *= p;

      std::vector<ComplexType,allocator_type> tmp(p,ComplexType(),alloc);

      for (long i = 0; i < n; i += len)
      {
        //std::cout << "    i = " << i << "\n";
        for(long k=0;k<len_old;++k)
        {
          for(long j=0;j<p;++j)
            if(j==0 || k==0)
              tmp[j] = out[i + j*len_old +k ];
            else
              tmp[j] = out[i + j*len_old +k ]
                * complex_root_of_unity<ComplexType>(len,k*j*sign);

          if(p==2)
            complex_dft_2(tmp.data(),tmp.data(),sign);
          else
          {
          //  complex_dft_prime_bruteForce(tmp.data(),tmp.data()+p,tmp.data(),sign);
            complex_dft_prime_rader(tmp.data(),tmp.data()+p,tmp.data(),sign,alloc);
          }
          for(long j=0;j<p;++j)
            out[i+ j*len_old + k] = tmp[j];
        }
        //show(out+i,out+i+len);
      }
    }
  }

  template<class complex_value_type,class Allocator_t>
  void complex_dft_composite_inplace(
    complex_value_type* in_first,
    complex_value_type* in_last,
    int sign,
    const Allocator_t& alloc)
  {
    std::vector<complex_value_type,Allocator_t> work_space(in_first,in_last,alloc);
    complex_dft_composite_outofplace(in_first,in_last,work_space.data(),sign,alloc);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class complex_value_type, class Allocator_t>
  void complex_dft_composite(
    const complex_value_type* in_first,
    const complex_value_type* in_last,
    complex_value_type* out,
    int sign,
    const Allocator_t& alloc)
  {
    if(in_first==out)
      complex_dft_composite_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      complex_dft_composite_outofplace(in_first,in_last,out,sign,alloc);
  }

  template <class T>
  void dft_power2(const T *in_first, const T *in_last, T* out, const T e)
  {
    /*
      Cooley-Tukey mapping, in-place Decimation in Time
    */
    const long ptrdiff = static_cast<long>(std::distance(in_first,in_last));
    if(ptrdiff <=0 )
      return;
    const long n = lower_bound_power2(ptrdiff);

    if(in_first!=out)
      std::copy(in_first,in_last,out);

    if (n == 1)
        return;

    // auto _1 = T{1};

    int nbits = 0;
    ::boost::container::static_vector<T,32> e2{e};
    for (int m = n / 2; m > 0; m >>= 1, ++nbits)
      e2.push_back(e2.back() * e2.back());

    std::reverse(e2.begin(), e2.end());

    // Gold-Rader bit-reversal algorithm.
    for(int i=0,j=0;i<n-1;++i)
    {
      if(i<j)
        std::swap(out[i],out[j]);
      for(int k=n>>1;!( (j^=k)&k );k>>=1);
    }

    for (int len = 2, k = 1; len <= n; len <<= 1, ++k)
    {
      for (int i = 0; i < n; i += len)
      {
        {
          int j=0;
          T* u = out + i + j, *v = out + i + j + len / 2;
          T Bu = *u, Bv = *v;
          *u = Bu + Bv;
          *v = Bu - Bv;
        }

        T ej = e2[k];
        for (int j = 1; j < len / 2; ++j)
        {
          T* u = out + i + j, *v = out + i + j + len / 2;
          T Bu = *u, Bv = *v * ej;
          *u = Bu + Bv;
          *v = Bu - Bv;
          ej *= e2[k];
        }
      }
    }
  }

  template <class T>
  void complex_dft_power2(const T *in_first, const T *in_last, T* out, int sign)
  {
    /*
      Cooley-Tukey mapping, in-place Decimation in Time
    */
    const long ptrdiff = static_cast<long>(std::distance(in_first,in_last));
    if(ptrdiff <=0 )
      return;
    const long n = lower_bound_power2(ptrdiff);

    if(in_first!=out)
      std::copy(in_first,in_last,out);

    if (n == 1)
        return;

    // Gold-Rader bit-reversal algorithm.
    for(int i=0,j=0;i<n-1;++i)
    {
      if(i<j)
        std::swap(out[i],out[j]);
      for(int k=n>>1;!( (j^=k)&k );k>>=1);
    }

    for (int len = 2, k = 1; len <= n; len <<= 1, ++k)
    {
      for (int i = 0; i < n; i += len)
      {
        {
          int j=0;
          T* u = out + i + j, *v = out + i + j + len / 2;
          T Bu = *u, Bv = *v;
          *u = Bu + Bv;
          *v = Bu - Bv;
        }
        for (int j = 1; j < len / 2; ++j)
        {
          T cs{complex_root_of_unity<T>(1<<k,j*sign)};

          T* u = out + i + j, *v = out + i + j + len / 2;
          T Bu = *u, Bv = *v * cs;
          *u = Bu + Bv;
          *v = Bu - Bv;
        }
      }
    }
  }

  template<class complex_value_type>
  void complex_dft_power2_dif(
    const complex_value_type *in_first,
    const complex_value_type *in_last,
    complex_value_type* out,
    int sign)
  {
    // Naive in-place complex DFT.
    const long ptrdiff = static_cast<long>(std::distance(in_first,in_last));
    if(ptrdiff <=0 )
      return;
    const long my_n = lower_bound_power2(ptrdiff);

    if(in_first!=out)
      std::copy(in_first,in_last,out);

    if (my_n == 1)
        return;

    if(in_first!=out)
      std::copy(in_first, in_last, out);

    // Recursive decimation in frequency.
    for(long m = my_n; m > 1; m /= 2)
    {
      long mh = m / 2;

      for(long j = 0; j < mh; ++j)
      {
        complex_value_type cs{complex_root_of_unity<complex_value_type>(m,j*sign)};

        for (long t1=j; t1 < j + my_n; t1 += m)
        {
          complex_value_type u = out[t1];
          complex_value_type v = out[t1 + mh];

          out[t1]      =  u + v;
          out[t1 + mh] = (u - v) * cs;
        }
      }
    }

    // data reordering:
    for(long m = 1, j = 0; m < my_n - 1; ++m)
    {
      for(long k = my_n >> 1; (!((j^=k)&k)); k>>=1)
      {
        ;
      }

      if(j > m)
      {
        std::swap(out[m], out[j]);
      }
    }

    // Normalize for backwards transform (done externally).
  }

  template<typename InputIterator1,
           typename InputIterator2,
           typename OutputIterator,
           typename allocator_t  >
  void raw_convolution(InputIterator1 input1_begin,
                   InputIterator1 input1_end,
                   InputIterator2 input2_begin,
                   OutputIterator output,
                   const allocator_t& alloc)
  {
    using input_value_type = typename std::iterator_traits<InputIterator1>::value_type;
    using real_value_type  = typename input_value_type::value_type;
    using allocator_type   = allocator_t;

    const long N = std::distance(input1_begin,input1_end);
    const long N_extended = detail::is_power2(N) ? N : detail::upper_bound_power2(2*N-1);

    std::vector<input_value_type, allocator_type> In1(N_extended,input_value_type(),alloc);
    std::vector<input_value_type, allocator_type> In2(N_extended,input_value_type(),alloc);
    std::vector<input_value_type, allocator_type> Out(N_extended,input_value_type(),alloc);

    std::copy(input1_begin,input1_end,In1.begin());

    InputIterator2 input2_end{input2_begin};
    std::advance(input2_end,N);
    std::copy(input2_begin,input2_end,In2.begin());

    // padding
    for(long i=N;i<N_extended;++i)
      In1[i]=In2[i]=input_value_type{0};

    // fake N-periodicity
    if(N!=N_extended)
    for(long i=1;i<N;++i)
      In2[N_extended-N+i] = In2[i];

    complex_dft_power2(In1.data(),In1.data()+In1.size(),In1.data(),1);
    complex_dft_power2(In2.data(),In2.data()+In1.size(),In2.data(),1);

    // direct convolution
    std::transform(In1.begin(),In1.end(),In2.begin(),Out.begin(),std::multiplies<input_value_type>());

    complex_dft_power2(Out.data(),Out.data()+Out.size(),Out.data(),-1);

    const real_value_type inv_N = real_value_type{1}/N_extended;
    for(auto & x : Out)
        x *= inv_N;

    std::copy(Out.begin(),Out.begin() + N,output);
  }

  } // namespace detail

  } } } // namespace boost::math::fft

#endif // BOOST_MATH_FFT_ALGORITHMS_HPP


/// @file  dist.hpp
/// @brief Declarations for embedding methods using pairwise distances

#pragma once
#ifndef OGT_EMBED_DIST_HPP
#define OGT_EMBED_DIST_HPP

#include <ogt/config.hpp>
#include <Eigen/Dense>

namespace OGT_NAMESPACE {
namespace embed {

/// Produce an embedding of the specified dimensionality from the given
/// Euclidean distance matrix.
///
/// Uses Classical MDS, as described in:
/// [1] I. Borg & P. Groenen (1997): Modern multidimensional scaling: theory
///     and applications. Springer.
Eigen::MatrixXd embedDistWithDims(const Eigen::MatrixXd& dist, size_t nDim);

/// Produce an embedding from the given Euclidean distance matrix.
/// The natural dimensionality is estimated, using all eigenvalues greater than
/// or equal to the (non-negative) threshold lambda.
///
/// Uses Classical MDS, as described in:
/// [1] I. Borg & P. Groenen (1997): Modern multidimensional scaling: theory
///     and applications. Springer.
Eigen::MatrixXd embedDist(const Eigen::MatrixXd& dist,
	double lambda = 1e-12);

/// Produce an embedding using Euclidean distances to a subset of the set.
/// The matrix dist must be a n x (d+1) Euclidean distance matrix, where each
/// column represents the distance to some reference vertex.
/// The reference vertices must be the first (d+1) rows.
/// The produced embedding will be in R^maxDim, or in fewer dimensions if the
/// distances between reference vertices are not large enough to justify the
/// full dimensionality.
///
/// Implements:
/// [1]	M. J. Sippl and H. A. Scheraga, "Solution of the embedding problem and
///     decomposition of symmetric matrices.," Proceedings of the National
///     Academy of Sciences, Apr. 1985.
Eigen::MatrixXd embedDistWithCMReference(const Eigen::MatrixXd& dist,
	size_t maxDim);

/// Produce an embedding into the target dimensionality, attempting to preserve
/// the total ordering of pairwise distances.
/// An ordinal matrix is produced from the given distance matrix, and an
/// eigendecomposition is used to embed the points.
///
/// Implements method from:
/// [1] Dattorro, Jon, "Convex Optimization and Euclidean Distance Geometry,"
///     Meboo Publishing, 2016.
Eigen::MatrixXd embedDistWithOrdEig(const Eigen::MatrixXd& dist, size_t nDim);

} // end namespace embed
} // end namespace OGT_NAMESPACE
#endif /* OGT_EMBED_DIST_HPP */


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2015-2016, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_UTIL_NORMALIZE_SPHEROIDAL_COORDINATES_HPP
#define BOOST_GEOMETRY_UTIL_NORMALIZE_SPHEROIDAL_COORDINATES_HPP

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace math
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename CoordinateType, typename Units>
struct constants_on_spheroid
{
    static inline CoordinateType period()
    {
        return math::two_pi<CoordinateType>();
    }

    static inline CoordinateType half_period()
    {
        return math::pi<CoordinateType>();
    }

    static inline CoordinateType min_longitude()
    {
        static CoordinateType const minus_pi = -math::pi<CoordinateType>();
        return minus_pi;
    }

    static inline CoordinateType max_longitude()
    {
        return math::pi<CoordinateType>();
    }

    static inline CoordinateType min_latitude()
    {
        static CoordinateType const minus_half_pi
            = -math::half_pi<CoordinateType>();
        return minus_half_pi;
    }

    static inline CoordinateType max_latitude()
    {
        return math::half_pi<CoordinateType>();
    }
};

template <typename CoordinateType>
struct constants_on_spheroid<CoordinateType, degree>
{
    static inline CoordinateType period()
    {
        return CoordinateType(360.0);
    }

    static inline CoordinateType half_period()
    {
        return CoordinateType(180.0);
    }

    static inline CoordinateType min_longitude()
    {
        return CoordinateType(-180.0);
    }

    static inline CoordinateType max_longitude()
    {
        return CoordinateType(180.0);
    }

    static inline CoordinateType min_latitude()
    {
        return CoordinateType(-90.0);
    }

    static inline CoordinateType max_latitude()
    {
        return CoordinateType(90.0);
    }
};

template <typename Units, typename CoordinateType>
class normalize_spheroidal_coordinates
{
    typedef constants_on_spheroid<CoordinateType, Units> constants;

protected:
    static inline CoordinateType normalize_up(CoordinateType const& value)
    {
        return
            math::mod(value + constants::half_period(), constants::period())
            - constants::half_period();
    }

    static inline CoordinateType normalize_down(CoordinateType const& value)
    {
        return
            math::mod(value - constants::half_period(), constants::period())
            + constants::half_period();
    }

public:
    static inline void apply(CoordinateType& longitude)
    {
        // normalize longitude
        if (math::equals(math::abs(longitude), constants::half_period()))
        {
            longitude = constants::half_period();
        }
        else if (longitude > constants::half_period())
        {
            longitude = normalize_up(longitude);
            if (math::equals(longitude, -constants::half_period()))
            {
                longitude = constants::half_period();
            }
        }
        else if (longitude < -constants::half_period())
        {
            longitude = normalize_down(longitude);
        }
    }

    static inline void apply(CoordinateType& longitude,
                             CoordinateType& latitude,
                             bool normalize_poles = true)
    {
#ifdef BOOST_GEOMETRY_NORMALIZE_LATITUDE
        // normalize latitude
        if (math::larger(latitude, constants::half_period()))
        {
            latitude = normalize_up(latitude);
        }
        else if (math::smaller(latitude, -constants::half_period()))
        {
            latitude = normalize_down(latitude);
        }

        // fix latitude range
        if (latitude < constants::min_latitude())
        {
            latitude = -constants::half_period() - latitude;
            longitude -= constants::half_period();
        }
        else if (latitude > constants::max_latitude())
        {
            latitude = constants::half_period() - latitude;
            longitude -= constants::half_period();
        }
#endif // BOOST_GEOMETRY_NORMALIZE_LATITUDE

        // normalize longitude
        apply(longitude);

        // finally normalize poles
        if (normalize_poles)
        {
            if (math::equals(math::abs(latitude), constants::max_latitude()))
            {
                // for the north and south pole we set the longitude to 0
                // (works for both radians and degrees)
                longitude = CoordinateType(0);
            }
        }

#ifdef BOOST_GEOMETRY_NORMALIZE_LATITUDE
        BOOST_GEOMETRY_ASSERT(! math::larger(constants::min_latitude(), latitude));
        BOOST_GEOMETRY_ASSERT(! math::larger(latitude, constants::max_latitude()));
#endif // BOOST_GEOMETRY_NORMALIZE_LATITUDE

        BOOST_GEOMETRY_ASSERT(math::smaller(constants::min_longitude(), longitude));
        BOOST_GEOMETRY_ASSERT(! math::larger(longitude, constants::max_longitude()));
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Short utility to normalize the coordinates on a spheroid
\tparam Units The units of the coordindate system in the spheroid
\tparam CoordinateType The type of the coordinates
\param longitude Longitude
\param latitude Latitude
\ingroup utility
*/
template <typename Units, typename CoordinateType>
inline void normalize_spheroidal_coordinates(CoordinateType& longitude,
                                             CoordinateType& latitude)
{
    detail::normalize_spheroidal_coordinates
        <
            Units, CoordinateType
        >::apply(longitude, latitude);
}

/*!
\brief Short utility to normalize the longitude on a spheroid.
       Note that in general both coordinates should be normalized at once.
       This utility is suitable e.g. for normalization of the difference of longitudes.
\tparam Units The units of the coordindate system in the spheroid
\tparam CoordinateType The type of the coordinates
\param longitude Longitude
\ingroup utility
*/
template <typename Units, typename CoordinateType>
inline void normalize_longitude(CoordinateType& longitude)
{
    detail::normalize_spheroidal_coordinates
        <
            Units, CoordinateType
        >::apply(longitude);
}

/*!
\brief Short utility to calculate difference between two longitudes
       normalized in range (-180, 180].
\tparam Units The units of the coordindate system in the spheroid
\tparam CoordinateType The type of the coordinates
\param longitude1 Longitude 1
\param longitude2 Longitude 2
\ingroup utility
*/
template <typename Units, typename CoordinateType>
inline CoordinateType longitude_distance_signed(CoordinateType const& longitude1,
                                                CoordinateType const& longitude2)
{
    CoordinateType diff = longitude2 - longitude1;
    math::normalize_longitude<Units, CoordinateType>(diff);
    return diff;
}

/*!
\brief Short utility to calculate difference between two longitudes
       normalized in range [0, 360).
\tparam Units The units of the coordindate system in the spheroid
\tparam CoordinateType The type of the coordinates
\param longitude1 Longitude 1
\param longitude2 Longitude 2
\ingroup utility
*/
template <typename Units, typename CoordinateType>
inline CoordinateType longitude_distance_unsigned(CoordinateType const& longitude1,
                                                  CoordinateType const& longitude2)
{
    typedef math::detail::constants_on_spheroid
        <
            CoordinateType, Units
        > constants;

    CoordinateType const c0 = 0;
    CoordinateType diff = longitude_distance_signed<Units>(longitude1, longitude2);
    if (diff < c0) // (-180, 180] -> [0, 360)
    {
        diff += constants::period();
    }
    return diff;
}

/*!
\brief The abs difference between longitudes in range [0, 180].
\tparam Units The units of the coordindate system in the spheroid
\tparam CoordinateType The type of the coordinates
\param longitude1 Longitude 1
\param longitude2 Longitude 2
\ingroup utility
*/
template <typename Units, typename CoordinateType>
inline CoordinateType longitude_difference(CoordinateType const& longitude1,
                                           CoordinateType const& longitude2)
{
    return math::abs(math::longitude_distance_signed<Units>(longitude1, longitude2));
}

template <typename Units, typename CoordinateType>
inline CoordinateType longitude_interval_distance_signed(CoordinateType const& longitude_a1,
                                                         CoordinateType const& longitude_a2,
                                                         CoordinateType const& longitude_b)
{
    CoordinateType const c0 = 0;
    CoordinateType dist_a12 = longitude_distance_signed<Units>(longitude_a1, longitude_a2);
    CoordinateType dist_a1b = longitude_distance_signed<Units>(longitude_a1, longitude_b);
    if (dist_a12 < c0)
    {
        dist_a12 = -dist_a12;
        dist_a1b = -dist_a1b;
    }

    return dist_a1b < c0 ? dist_a1b
         : dist_a1b > dist_a12 ? dist_a1b - dist_a12
         : c0;
}

} // namespace math

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_NORMALIZE_SPHEROIDAL_COORDINATES_HPP


// solving A * X = B
// A symmetric
// sytrf() & sytrs()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/sysv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

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

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

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

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

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

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

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

int main (int argc, char **argv) {
  size_t n = 0;
  if (argc > 1) {
    n = atoi(argv [1]);
  }

  cout << endl;

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

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

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

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

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

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

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

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

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

  std::vector<int> ipiv (n);

  int err = lapack::sytrf (sal, ipiv);
  if (err == 0) {
    symml_t isal (sal);
    lapack::sytrs (sal, ipiv, bl);
    print_m (bl, "xl");
    lapack::sytri (isal, ipiv);
    print_m (isal, "isal");
  }
  cout << endl;

  err = lapack::sytrf (sau, ipiv);
  if (err == 0) {
    symmu_t isau (sau);
    lapack::sytrs (sau, ipiv, bu);
    print_m (bu, "xu");
    lapack::sytri (isau, ipiv);
    print_m (isau, "isau");
  }
  else
    cout << "?" << endl;
  cout << endl;

  // part 2

  cout << endl << "part 2" << endl << endl;

  int lw = lapack::sytrf_block ('O', 'L', al1);
  cout << "nb = " << lw << endl;
  lw *= n;
  cout << "lw = " << lw << " == "
       << lapack::sytrf_work ('O', 'L', al1) << endl;
  cout << "mw = " << lapack::sytrf_work ('M', 'L', al1) << endl;
  std::vector<real_t> work (lw);
  int mb = lapack::sytrf_block ('M', 'L', al1);
  cout << "mb = " << mb << endl << endl;

  err = lapack::sytrf ('L', al1, ipiv, work);
  if (err == 0) {
    lapack::sytrs ('L', al1, ipiv, bl1);
    print_m (al1, "al1 factored");
    cout << endl;
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (bl1, "xl1");
  }
  else
    cout << "?" << endl;
  cout << endl;

  lw = lapack::sytrf_block ('O', 'U', au1);
  cout << "nb = " << lw << endl;
  lw *= n;
  cout << "lw = " << lw << " == "
       << lapack::sytrf_work ('O', 'U', au1) << endl;
  cout << "mw = " << lapack::sytrf_work ('M', 'U', au1) << endl;
  if (lw != work.size())
    work.resize (lw);
  mb = lapack::sytrf_block ('M', 'U', au1);
  cout << "mb = " << mb << endl << endl;

  err = lapack::sytrf ('U', au1, ipiv, work);
  if (err == 0) {
    lapack::sytrs ('U', au1, ipiv, bu1);
    print_m (au1, "au1 factored");
    cout << endl;
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (bu1, "xu1");
  }
  else
    cout << "?" << endl;
  cout << endl;
  cout << endl;

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

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

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

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

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

  int ierr = lapack::sytrf (scal, ipiv);
  if (ierr == 0) {
    csymml_t iscal (scal);
    lapack::sytrs (scal, ipiv, cbl);
    print_m (cbl, "cxl");
    lapack::sytri (iscal, ipiv);
    print_m (iscal, "iscal");
  }
  else
    cout << "?" << endl;
  cout << endl;

  lw = lapack::sytrf_block ('O', scau);
  cout << "nb = " << lw << endl;
  lw *= n;
  cout << "lw = " << lw << " == "
       << lapack::sytrf_work ('O', scau) << endl;
  cout << "mw = " << lapack::sytrf_work ('M', scau) << endl;
  std::vector<cmplx_t> cwork (lw);
  mb = lapack::sytrf_block ('M', scau);
  cout << "mb = " << mb << endl << endl;

  ierr = lapack::sytrf (scau, ipiv, cwork);
  if (ierr == 0) {
    csymmu_t iscau (scau);
    lapack::sytrs (scau, ipiv, cbu);
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu, "cxu");
    lapack::sytri (iscau, ipiv);
    print_m (iscau, "iscau");
  }
  else
    cout << "?" << endl;
  cout << endl;

}


#include <stan/math.hpp>
#include <ostream>
#include <boost/math/tools/promotion.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

namespace cambridgeModel_model_namespace {

template <typename T0__>
stan::promote_args_t<stan::value_type_t<T0__>>
dominant_eigenvalue_external(const T0__& A_arg__, std::ostream* pstream__){
  //Return type
  typedef typename stan::promote_args_t<stan::value_type_t<T0__>> scalar_t;
  //inputtype
  typedef T0__ matrix_t;

  //Eigensolver
  Eigen::EigenSolver<matrix_t> es(A_arg__);

  //Eigenvalues
  Eigen::Matrix<std::complex<scalar_t>, Eigen::Dynamic, 1> lambdas = es.eigenvalues();

  //Just take the real parts
  std::vector<scalar_t> lambdas_real(lambdas.rows());
  for (int i = 0; i < lambdas.rows(); i++){
    lambdas_real.at(i) = lambdas(i,0).real();
  }

  scalar_t returnValue;
  //Maximum eigenvalue (basic reproduction number is positive so
  //so we don't have to worry about sign)
  returnValue = stan::math::max(lambdas_real);

  return returnValue;

}

} //cambridgemodel_model_namespace


#include "DenoiseSystem.h"

#include "../Components/DenoiseData.h"

#include <_deps/imgui/imgui.h>

#include <spdlog/spdlog.h>
#include <Eigen/Core>
#include <Eigen/Sparse>

using namespace Ubpa;

void DenoiseSystem::OnUpdate(Ubpa::UECS::Schedule& schedule) {
	schedule.RegisterCommand([](Ubpa::UECS::World* w) {
		auto data = w->entityMngr.GetSingleton<DenoiseData>();
		if (!data)
			return;

		if (ImGui::Begin("Denoise")) {
			if (ImGui::Button("Mesh to HEMesh")) {
				data->heMesh->Clear();
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}

					if (data->mesh->GetSubMeshes().size() != 1) {
						spdlog::warn("number of submeshes isn't 1");
						return;
					}

					data->copy = *data->mesh;

					std::vector<size_t> indices(data->mesh->GetIndices().begin(), data->mesh->GetIndices().end());

					data->heMesh->Init(indices, 3);
					if (!data->heMesh->IsTriMesh())
						spdlog::warn("HEMesh init fail");

					for (size_t i = 0; i < data->mesh->GetPositions().size(); i++) {
						data->heMesh->Vertices().at(i)->position = data->mesh->GetPositions().at(i);
						data->heMesh->Vertices().at(i)->idx = -1;
						data->heMesh->Vertices().at(i)->bidx = -1;
					}

					spdlog::info("Mesh to HEMesh success");
				}();
			}

			if (ImGui::Button("Solve Minimal surface")) {
				[&]() {
					if (!data->heMesh->IsTriMesh()) {
						spdlog::warn("HEMesh isn't triangle mesh");
						return;
					}

					const auto vertices = data->heMesh->Vertices();
					int totalPoints = static_cast<int>(vertices.size()), boundaryPoints = { 0 }, innerPoints = { 0 };
					for (int i = 0; i < totalPoints; ++i) {
						vertices[i]->idx = i;
						if (vertices[i]->IsOnBoundary()) ++boundaryPoints;
					}
					innerPoints = totalPoints - boundaryPoints;

					auto Idx = [&](int idx, int k) {
						return k * totalPoints + idx;
					};
					auto ConsIdx = [&](int idx, int k) {
						return 3 * totalPoints + idx * 3 + k;
					};

					Eigen::SparseMatrix<float> A(3 * (totalPoints + boundaryPoints), 3 * totalPoints);
					Eigen::SparseVector<float> b(3 * (totalPoints + boundaryPoints));
					spdlog::info(totalPoints);

					int b_idx = 0;
					for (auto* v : data->heMesh->Vertices()) {
						const auto P = v->position;

						// A_ij & x_i
						if (v->IsOnBoundary()) {
							for (auto k : { 0, 1, 2 }) {
								b.coeffRef(ConsIdx(b_idx, k)) = P[k];
								A.coeffRef(ConsIdx(b_idx, k), Idx(v->idx, k)) = 1;
							}
							++b_idx;
						}
						const auto& adj_v = v->AdjVertices();
						for (auto k : { 0, 1, 2 })
							A.coeffRef(Idx(v->idx, k), Idx(v->idx, k)) = v->AdjVertices().size();

						for (auto* adj : adj_v) {
							for (auto k : { 0, 1, 2 }) A.coeffRef(Idx(v->idx, k), Idx(adj->idx, k)) = -1;
						}
					}
					spdlog::info("Build Matrix successful");

					Eigen::ConjugateGradient<Eigen::SparseMatrix<float> > solver;
					solver.setTolerance(1e-4);
					solver.compute(A.transpose() * A);
					Eigen::SparseVector<float> x(3 * totalPoints);
					x = solver.solve(A.transpose() * b);

					spdlog::info("Solve Matrix successful");

					for (auto* v : data->heMesh->Vertices()) {
						for (auto k : { 0, 1, 2 }) {
							v->position[k] = x.coeffRef(Idx(v->idx, k));
						}
					}
					spdlog::info("Data transform successful");
				}();
			}

			if (ImGui::Button("Parameterization")) {
				[&]() {
					if (!data->heMesh->IsTriMesh()) {
						spdlog::warn("HEMesh isn't triangle mesh");
						return;
					}

					const auto vertices = data->heMesh->Vertices();
					int totalPoints = static_cast<int>(vertices.size()), boundaryPoints = { 0 }, innerPoints = { 0 };
					for (int i = 0; i < totalPoints; ++i) vertices[i]->idx = i;
					int bpt;
					for (int i = 0; i < totalPoints; ++i)
						if (vertices[i]->IsOnBoundary()) bpt = vertices[i]->idx; // start point
					do {
						vertices[bpt]->bidx = boundaryPoints++;
						const auto& adj_v = vertices[bpt]->AdjVertices();
						for (auto* v : data->heMesh->Vertices()) {
							if (v->IsOnBoundary() && v->bidx < 0) {
								bpt = v->idx;
								break;
							}
						}
					} while (vertices[bpt]->bidx < 0);

					innerPoints = totalPoints - boundaryPoints;

					auto Idx = [&](int idx, int k) {
						return k * totalPoints + idx;
					};
					auto ConsIdx = [&](int idx, int k) {
						return 2 * totalPoints + idx * 2 + k;
					};

					Eigen::SparseMatrix<float> A(2 * (totalPoints + boundaryPoints), 2 * totalPoints);
					Eigen::SparseVector<float> b(2 * (totalPoints + boundaryPoints));
					spdlog::info(totalPoints);

					for (auto* v : data->heMesh->Vertices()) {
						const auto P = v->position;

						// A_ij & x_i
						if (v->IsOnBoundary()) {
							{ // project to UV surface
								float t = 4 * float(v->bidx) / float(boundaryPoints);
								float U = { 0.0f }, V = { 0.0f };
								if (t <= 1.0f) V = t;
								else if (t <= 2.0f) U = t - 1, V = 1.0f;
								else if (t <= 3.0f) U = 1.0f, V = 1.0f - (t - 2.0f);
								else U = 1.0f - (t - 3.0f);

								spdlog::info(std::to_string(U) + " " + std::to_string(V));

								b.coeffRef(ConsIdx(v->bidx, 0)) = U;
								b.coeffRef(ConsIdx(v->bidx, 1)) = V;
							}
							for (auto k : { 0, 1 }) A.coeffRef(ConsIdx(v->bidx, k), Idx(v->idx, k)) = 1;
						}
						const auto& adj_v = v->AdjVertices();
						for (auto k : { 0, 1 })
							A.coeffRef(Idx(v->idx, k), Idx(v->idx, k)) = v->AdjVertices().size();

						for (auto* adj : adj_v) {
							for (auto k : { 0, 1 }) A.coeffRef(Idx(v->idx, k), Idx(adj->idx, k)) = -1;
						}
					}
					spdlog::info("Build Matrix successful");

					Eigen::ConjugateGradient<Eigen::SparseMatrix<float> > solver;
					solver.setTolerance(1e-2);
					solver.compute(A.transpose() * A);
					Eigen::SparseVector<float> x(2 * totalPoints);
					x = solver.solve(A.transpose() * b);

					spdlog::info("Solve Matrix successful");

					for (auto* v : data->heMesh->Vertices()) {
						v->uv = { x.coeffRef(Idx(v->idx, 0)), x.coeffRef(Idx(v->idx, 1)) };
						spdlog::info(std::to_string(v->uv[0]) + " " + std::to_string(v->uv[1]));
					}

					spdlog::info("Data transform successful");
				}();
			}

			if (ImGui::Button("Set Normal to Color")) {
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}

					data->mesh->SetToEditable();
					const auto& normals = data->mesh->GetNormals();
					std::vector<rgbf> colors;
					for (const auto& n : normals)
						colors.push_back((n.as<valf3>() + valf3{ 1.f }) / 2.f);
					data->mesh->SetColors(std::move(colors));

					spdlog::info("Set Normal to Color Success");
				}();
			}

			if (ImGui::Button("HEMesh to Mesh")) {
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}

					if (!data->heMesh->IsTriMesh() || data->heMesh->IsEmpty()) {
						spdlog::warn("HEMesh isn't triangle mesh or is empty");
						return;
					}

					data->mesh->SetToEditable();

					const size_t N = data->heMesh->Vertices().size();
					const size_t M = data->heMesh->Polygons().size();
					std::vector<Ubpa::pointf3> positions(N);
					std::vector<Ubpa::pointf2> uvs(N);
					std::vector<uint32_t> indices(M * 3);
					for (size_t i = 0; i < N; i++) {
						positions[i] = data->heMesh->Vertices().at(i)->position;
						uvs[i] = data->heMesh->Vertices().at(i)->uv;
					}
					for (size_t i = 0; i < M; i++) {
						auto tri = data->heMesh->Indices(data->heMesh->Polygons().at(i));
						indices[3 * i + 0] = static_cast<uint32_t>(tri[0]);
						indices[3 * i + 1] = static_cast<uint32_t>(tri[1]);
						indices[3 * i + 2] = static_cast<uint32_t>(tri[2]);
					}
					data->mesh->SetPositions(std::move(positions));
					data->mesh->SetUV(std::move(uvs));
					data->mesh->SetIndices(std::move(indices));
					data->mesh->SetSubMeshCount(1);
					data->mesh->SetSubMesh(0, { 0, M * 3 });
					data->mesh->GenNormals();
					data->mesh->GenTangents();

					spdlog::info("HEMesh to Mesh success");
				}();
			}

			if (ImGui::Button("Recover Mesh")) {
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}
					if (data->copy.GetPositions().empty()) {
						spdlog::warn("copied mesh is empty");
						return;
					}

					*data->mesh = data->copy;

					spdlog::info("recover success");
				}();
			}
		}
		ImGui::End();
	});
}


#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "Random.h"
#include "params.h"

using namespace std;
using namespace NTL;

vec_ZZ RandomVector()
{
    vec_ZZ w;
    unsigned int i;
    w.SetLength(N0);
    for(i=0; i<N0; i++)
    {
        w[i] = conv<ZZ>(rand())%q1;
    }
    return w;
}

//==============================================================================
//Generates a random polynomial of fixed degree
//==============================================================================
ZZX RandomPoly(const unsigned int degree)
{
    unsigned int i;
    ZZX f;
    f.SetLength(degree+1);
    for(i=0; i<=degree; i++)
    {
        f[i] = rand();
    }
    return f;
}

//==============================================================================
//Generates a random polynomial of fixed degree and "approximately" fixed squared norm
//==============================================================================
ZZX RandomPolyFixedSqNorm(const ZZ& SqNorm, const unsigned int degree)
{
    unsigned int i;
    ZZ SqNorm0, Ratio;
    ZZX f;
    f.SetLength(degree+1);

    RR_t sigma = sqrt( ( (double) conv<double>(SqNorm)/(degree+1) ) );

    for(i=0; i<=degree; i++)
    {
        f[i] = conv<ZZ>(Sample3(sigma));
    }
    f[degree] |= 1;
    return f;
}