/** \file   cmr_time_stamp.h
    \brief  Implement functionalities to handle cardiac time stamps
    \author Hui Xue
*/

#include "cmr_time_stamp.h"
#include "hoNDArray_reductions.h"
#include "hoNDArray_elemwise.h"
#include <boost/math/special_functions/sign.hpp>

namespace Gadgetron {

    template <typename T>
    void simple_line_fit(const std::vector<T>& x, const std::vector<T>& y, T& a, T& b)
    {
        try
        {
            size_t num = x.size();

            if(num<2)
            {
                a = 0;
                b = 0;
                return;
            }

            T sx(0), sy(0);

            size_t n;
            for (n=0; n<num; n++)
            {
                sx += x[n];
                sy += y[n];
            }

            T mx = sx / (T)(num);
            T syy = 0;
            b = 0;
            for (n=0; n<num; n++)
            {
                T v = (x[n] - mx);
                syy += v*v;
                b += v*y[n];
            }

            syy = (std::abs(syy) > 0 ? syy : boost::math::sign(syy)*FLT_EPSILON);
            b /= syy;
            a = (sy - sx*b) / (T)(num);
        }
        catch(...)
        {
            GADGET_THROW("Exceptions happened in simple_line_fit ... ");
        }
    }

    void correct_time_stamp_with_fitting(hoNDArray<float>& time_stamp, size_t startE1, size_t endE1)
    {
        try
        {
            size_t E1 = time_stamp.get_size(0);
            size_t N = time_stamp.get_size(1);
            size_t rE1 = endE1 - startE1 + 1;

            size_t e1, n;

            size_t num_acq_read_outs = 0;
            for ( n=0; n<N; n++ )
            {
                for ( e1=0; e1<E1; e1++ )
                {
                    if ( time_stamp(e1, n) > 0 )
                    {
                        num_acq_read_outs++;
                    }
                }
            }

            GDEBUG_STREAM(" Number of acquired lines : " << num_acq_read_outs);

            float a, b; // y = a + b*x
            {
                std::vector<float> x(num_acq_read_outs), y(num_acq_read_outs);

                size_t ind = 0;
                for ( n=0; n<N; n++ )
                {
                    for ( e1=startE1; e1<=endE1; e1++ )
                    {
                        float acq_time = time_stamp(e1, n);
                        if ( acq_time > 0 )
                        {
                            x[ind] = (float)(e1-startE1 + n*rE1);
                            y[ind] = acq_time;
                            ind++;
                        }
                    }
                }

                Gadgetron::simple_line_fit(x, y, a, b);
            }

            for ( n=0; n<N; n++ )
            {
                for ( e1=startE1; e1<=endE1; e1++ )
                {
                    float x_v = (float)(e1-startE1 + n*rE1);
                    time_stamp(e1, n) = a + b*x_v;
                }
            }
        }
        catch(...)
        {
            GADGET_THROW("Exceptions happened in correct_time_stamp_with_fitting(...) ... ");
        }
    }

    void detect_heart_beat_with_time_stamp(hoNDArray<float>& cpt_time_stamp, hoNDArray<int>& ind_hb,
                                        std::vector<size_t>& start_e1_hb, std::vector<size_t>& end_e1_hb,
                                        std::vector<size_t>& start_n_hb, std::vector<size_t>& end_n_hb )
    {
        try
        {
            size_t E1 = cpt_time_stamp.get_size(0);
            size_t N = cpt_time_stamp.get_size(1);

            size_t e1, n, ind, ii;

            size_t num_acq_read_outs = 0;
            for ( n=0; n<N; n++ )
            {
                for ( e1=0; e1<E1; e1++ )
                {
                    if ( cpt_time_stamp(e1, n) >= 0 )
                    {
                        num_acq_read_outs++;
                    }
                }
            }

            ind_hb.create(E1, N);
            Gadgetron::clear(ind_hb);

            // --------------------------------------------------------
            // cpt time stamps
            // --------------------------------------------------------

            std::vector<float> acquired_cpt(num_acq_read_outs);
            std::vector<size_t> ind_acquired_cpt(num_acq_read_outs);

            ind = 0;
            for ( n=0; n<N; n++ )
            {
                for ( e1=0; e1<E1; e1++ )
                {
                    if ( cpt_time_stamp(e1, n) > -1 )
                    {
                        acquired_cpt[ind] = cpt_time_stamp(e1, n);
                        ind_acquired_cpt[ind] = e1 + n*E1;
                        ind++;
                    }
                }
            }

            // --------------------------------------------------------
            // find the number of heart beats
            // --------------------------------------------------------
            size_t numOfHB = 0;

            // store the line indexes for every heart beat
            std::vector<size_t> ind_HB_start, ind_HB_end;
            ind_HB_start.push_back(0);

            for ( ind=1; ind<num_acq_read_outs; ind++ )
            {
                if ( acquired_cpt[ind] < acquired_cpt[ind-1] )
                {
                    // find a new heart beat
                    numOfHB++;

                    size_t end_ind_prev_HB = ind_acquired_cpt[ind-1];
                    size_t start_ind_curr_HB = ind_acquired_cpt[ind];

                    // if there is a gap between end and start ind, fill the gap
                    if ( end_ind_prev_HB+1 != start_ind_curr_HB )
                    {
                        long long gap = start_ind_curr_HB - end_ind_prev_HB - 1;
                        if ( gap % 2 == 0 )
                        {
                            end_ind_prev_HB += gap;
                        }
                        else
                        {
                            end_ind_prev_HB += gap;
                        }

                        if ( end_ind_prev_HB+1 != start_ind_curr_HB )
                        {
                            GWARN_STREAM("end_ind_prev_HB+1 ~= start_ind_curr_HB : " << end_ind_prev_HB << " " << start_ind_curr_HB);
                        }
                    }

                    ind_HB_end.push_back( end_ind_prev_HB );
                    ind_HB_start.push_back( start_ind_curr_HB );
                }
            }

            ind_HB_end.push_back( E1*N-1 );
            numOfHB = ind_HB_end.size();

            // --------------------------------------------------------
            // fill the start and end indexes
            // --------------------------------------------------------
            start_e1_hb.resize(numOfHB, 0);
            end_e1_hb.resize(numOfHB, 0);

            start_n_hb.resize(numOfHB, 0);
            end_n_hb.resize(numOfHB, 0);

            std::vector<size_t> start, end;
            for ( ii=0; ii<numOfHB; ii++ )
            {
                start_n_hb[ii] = ind_HB_start[ii] / E1;
                start_e1_hb[ii] = ind_HB_start[ii] - start_n_hb[ii] * E1;

                end_n_hb[ii] = ind_HB_end[ii] / E1;
                end_e1_hb[ii] = ind_HB_end[ii] - end_n_hb[ii]*E1;

                for (ind=ind_HB_start[ii]; ind<=ind_HB_end[ii]; ind++)
                {
                    ind_hb(ind) = ii;
                }
            }
        }
        catch(...)
        {
            GADGET_THROW("Exceptions happened in detect_heart_beat_with_time_stamp(...) ... ");
        }
    }

    void correct_heart_beat_time_stamp_with_fitting(hoNDArray<float>& cpt_time_stamp, hoNDArray<int>& ind_hb, size_t startE1, size_t endE1,
                                                const std::vector<size_t>& start_e1_hb, const std::vector<size_t>& end_e1_hb,
                                                const std::vector<size_t>& start_n_hb, const std::vector<size_t>& end_n_hb )
    {
        try
        {
            size_t E1 = cpt_time_stamp.get_size(0);
            size_t N = cpt_time_stamp.get_size(1);
            size_t rE1 = endE1-startE1+1;

            size_t e1, n, ind, ii;

            size_t num_acq_read_outs = 0;
            for ( n=0; n<N; n++ )
            {
                for ( e1=0; e1<E1; e1++ )
                {
                    if ( cpt_time_stamp(e1, n) >= 0 )
                    {
                        num_acq_read_outs++;
                    }
                }
            }

            size_t numOfHB = start_e1_hb.size();

            std::vector<size_t> ind_HB_start(numOfHB);
            std::vector<size_t> ind_HB_end(numOfHB);

            for ( ind=0; ind<numOfHB; ind++ )
            {
                ind_HB_start[ind] = start_e1_hb[ind] + start_n_hb[ind] * E1;
                ind_HB_end[ind] = end_e1_hb[ind] + end_n_hb[ind] * E1;
            }

            // --------------------------------------------------------
            // fit a line to every heart beat
            // --------------------------------------------------------
            float a, b;
            std::vector<float> A(numOfHB, 0.0f), B(numOfHB, 0.0f);
            for ( ind=0; ind<numOfHB; ind++ )
            {
                std::vector<float> x, y;

                size_t cpt;
                for ( cpt=ind_HB_start[ind]; cpt<=ind_HB_end[ind]; cpt++ )
                {
                    size_t n = cpt / E1;
                    size_t e1 = cpt - n*E1;

                    if(e1>=startE1 && e1<=endE1)
                    {
                        if ( cpt_time_stamp[cpt] > -1 )
                        {
                            size_t x_ind = (e1-startE1) + n*rE1;
                            x.push_back( (float)x_ind );
                            y.push_back(cpt_time_stamp[cpt]);
                        }
                    }
                }

                if ( !x.empty() )
                {
                    Gadgetron::simple_line_fit(x, y, a, b);
                    A[ind] = a;
                    B[ind] = b;
                }
            }

            // --------------------------------------------------------
            // compute cpt time stamp for every line
            // --------------------------------------------------------
            size_t num = cpt_time_stamp.get_number_of_elements();
            for ( ind=0; ind<num; ind++ )
            {
                n = ind / E1;
                e1 = ind - n*E1;

                if(e1>=startE1 && e1<=endE1)
                {
                    // find to which heart beat this line belongs
                    bool foundHB = false;
                    for ( ii=0; ii<numOfHB; ii++ )
                    {
                        size_t startHB = ind_HB_start[ii];
                        size_t endHB = ind_HB_end[ii];

                        if ( ii==0 && ind<=startHB )
                        {
                            foundHB = true;
                            break;
                        }

                        if ( ii==numOfHB-1 && ind>=endHB )
                        {
                            foundHB = true;
                            break;
                        }

                        if ( ind>=startHB && ind<=endHB )
                        {
                            foundHB = true;
                            break;
                        }
                    }

                    // if cannot find a heart beat, this kspace line will not be used
                    if ( foundHB && (std::abs(B[ii])>0) )
                    {
                        ind_hb(e1, n) = ii;

                        size_t x_ind = (e1-startE1) + n*rE1;
                        cpt_time_stamp(e1, n) = (float)(A[ii] + B[ii]*x_ind);
                    }
                    else
                    {
                        ind_hb(e1, n) = -1;
                    }
                }
                else
                {
                    cpt_time_stamp(e1, n) = -1;
                    ind_hb(e1, n) = -1;
                }
            }
        }
        catch(...)
        {
            GADGET_THROW("Exceptions happened in correct_heart_beat_time_stamp_with_fitting(...) ... ");
        }
    }

    void compute_phase_time_stamp(const hoNDArray<float>& time_stamp, const hoNDArray<float>& cpt_time_stamp, size_t startE1, size_t endE1,
        hoNDArray<float>& phs_time_stamp, hoNDArray<float>& phs_cpt_time_stamp)
    {
        try
        {
            size_t E1 = time_stamp.get_size(0);
            size_t N = time_stamp.get_size(1);
            size_t rE1 = endE1 - startE1 + 1;

            size_t e1, n;

            for ( n=0; n<N; n++ )
            {
                // phase time stamp as the mean of all aquired lines
                size_t num = 0;
                float tt = 0.0f;
                for ( e1=startE1; e1<=endE1; e1++ )
                {
                    if(time_stamp(e1, n)>0)
                    {
                        tt += time_stamp(e1, n);
                        num++;
                    }
                }
                phs_time_stamp(n, 0) = tt/((num>0) ? num : 1);

                //// phase cpt time as the median of all acquired lines
                //std::vector<float> cpt_buf(rE1, 0);
                //for ( e1=startE1; e1<=endE1; e1++ )
                //{
                //    if(cpt_time_stamp(e1, n)>=0)
                //        cpt_buf[e1-startE1] = cpt_time_stamp(e1, n);
                //}

                //std::sort(cpt_buf.begin(), cpt_buf.end());
                //phs_cpt_time_stamp(n, 0) = cpt_buf[E1/2-startE1];

                // phase cpt time as the cpt time of center line
                phs_cpt_time_stamp(n, 0) = cpt_time_stamp(E1/2, n);
            }
        }
        catch(...)
        {
            GADGET_THROW("Exceptions happened in compute_phase_time_stamp(...) ... ");
        }
    }

    template <typename T>
    void resample_cardiac_phase_cmr_array(const hoNDArray<T>& data, size_t output_N, hoNDArray<T>& res, size_t spline_degree)
    {
        try
        {
            std::vector<size_t> dims;
            data.get_dimensions(dims);

            size_t num_of_dims = dims.size();

            GADGET_CHECK_THROW(num_of_dims >=2);

            size_t N = dims[num_of_dims -1];

            GADGET_CHECK_THROW(N>2);

            std::vector<size_t> dims_res(dims);
            dims_res[num_of_dims - 1] = output_N;

            res.create(dims_res);
            Gadgetron::clear(res);

            long long num = data.get_number_of_elements()/N;

            long long n;

#pragma omp parallel default(none) shared(N, output_N, num, data, res, spline_degree, num_of_dims) private (n)
            {
                hoNDArray<T> data_in(N);
                hoNDArray<T> data_res(output_N);
                hoNDArray<T> coeff(N);
                hoNDBSpline< T, 1 > interp;

                float dx = (float(N)-1)/(float(output_N)-1);

                std::vector<size_t> ind;

                #pragma omp for
                for (n=0; n<num; n++)
                {
                    ind = data.calculate_index(n);

                    size_t ii;
                    for(ii=0; ii<N; ii++)
                    {
                        ind[num_of_dims - 1] = ii;
                        data_in(ii) = data(ind);
                    }

                    interp.computeBSplineCoefficients(data_in, spline_degree, coeff);

                    for (ii = 0; ii<output_N; ii++)
                    {
                        ind[num_of_dims - 1] = ii;
                        data_res(ii) = interp.evaluateBSpline(coeff.begin(), N, spline_degree, 0, ii*dx);
                        res(ind) = data_res(ii);
                    }
                }
            }
        }
        catch(...)
        {
            GADGET_THROW("Exceptions happened in resample_cardiac_phase_cmr_array(...) ... ");
        }
    }

    template EXPORTCMR void resample_cardiac_phase_cmr_array(const hoNDArray<float>& data, size_t output_N, hoNDArray<float>& res, size_t spline_degree);
    template EXPORTCMR void resample_cardiac_phase_cmr_array(const hoNDArray<double>& data, size_t output_N, hoNDArray<double>& res, size_t spline_degree);
    template EXPORTCMR void resample_cardiac_phase_cmr_array(const hoNDArray< std::complex<float> >& data, size_t output_N, hoNDArray< std::complex<float> >& res, size_t spline_degree);
    template EXPORTCMR void resample_cardiac_phase_cmr_array(const hoNDArray< std::complex<double> >& data, size_t output_N, hoNDArray< std::complex<double> >& res, size_t spline_degree);
}


#ifndef VERTEX_COVERING
#define VERTEX_COVERING 1

#include <LEDA/core/list.h>
#include <LEDA/graph/graph.h>
#include <cassert>
#include <queue>

/**
 * Primal-dual algorithm for minimum vertex cover problem
 */
template <typename T>
T min_vc_pd(const leda::graph& G, const leda::node_array<T>& weight,
    leda::list<leda::node>& S)
{
    leda::edge_array<bool> is_covered(G, false);
    leda::node_array<bool> is_vc(G, false);
    leda::node_array<T> gap(weight);

    T total_primal_cost = 0;
    T total_dual_cost = 0;

    leda::edge e;
    forall_edges(e, G)
    {
        if (is_covered[e])
            continue;

        leda::node s = G.source(e);
        leda::node t = G.target(e);
        if (gap[t] < gap[s])
            std::swap(s, t);
        is_vc[s] = true;
        S.append(s);
        gap[t] -= gap[s];
        assert(gap[t] >= T(0));
        // gap[s] = T(0);
        leda::edge e2;
        forall_adj_edges(e2, s)
        {
            is_covered[e2] = true;
        }
        total_primal_cost += weight[s];
        total_dual_cost += gap[s];
    }

    leda::list_item it;
    forall_items(it, S)
    {
        leda::node v = S.inf(it);
        leda::node w;
        bool found = false;
        forall_adj_nodes(w, v)
        {
            if (!is_vc[w])
            {
                found = true;
                break;
            }
        }
        if (!found)
        {
            S.del_item(it);
            total_primal_cost -= weight[v];
            is_vc[v] = false;
        }
    }

    assert(total_primal_cost >= total_dual_cost);
    return total_primal_cost;
}

/**
 * Primal-dual algorithm for graph bipartization problem
 */
template <typename T>
T min_bipartization_pd(const leda::graph& G, const leda::node_array<T>& weight,
    leda::list<leda::node>& S)
{
    leda::face_map<bool> is_covered(G);
    leda::node_array<bool> is_vc(G, false);
    leda::node_array<T> gap(weight);

    std::queue<leda::face> Q;

    T total_primal_cost = 0;
    T total_dual_cost = 0;

    leda::face f;
    forall_faces(f, G)
    {
        size_t deg = 0;
        leda::edge e;
        forall_face_edges(e, f)++ deg;
        if ((deg & 1) == 1)
        { // odd
            Q.push(f);
            is_covered[f] = false;
        }
        else
        { // even
            is_covered[f] = true;
        }
    }

    while (!Q.empty())
    {
        leda::face f = Q.top();
        Q.pop();
        if (is_covered[f])
            continue;

        leda::edge e = G.first_face_edge(f);
        leda::node s = G.source(e);
        T min_gap = gap[s];
        forall_face_edges(e, f)
        {
            leda::node v = G.source(e);
            if (min_gap > gap[v])
            {
                s = v;
                min_gap = gap[v];
            }
        }

        is_vc[s] = true;
        S.append(s);
        forall_face_edges(e, f)
        {
            leda::node v = G.source(e);
            gap[v] -= min_gap;
        }
        assert(gap[s] == 0);

        leda::face f2;
        forall_adj_faces(f2, s)
        { // ???
            is_covered[f2] = true;
        }

        // add new odd-cycle face ???

        total_primal_cost += weight[s];
        total_dual_cost += min_gap;
    }

    leda::list_item it;
    forall_items(it, S)
    {
        leda::node v = S.inf(it);
        leda::node w;
        bool found = false;
        forall_adj_nodes(w, v)
        {
            if (!is_vc[w])
            {
                found = true;
                break;
            }
        }
        if (!found)
        {
            S.del_item(it);
            total_primal_cost -= weight[v];
            is_vc[v] = false;
        }
    }

    assert(total_primal_cost >= total_dual_cost);
    return total_primal_cost;
}

#include <boost/heap/binomial_heap.hpp>

template <typename T>
class vc_order // for boost::heap
{
  public:
    vc_order(const leda::node_array<T>& W, const leda::node_array<size_t>& D)
        : _W(&W)
        , _D(&D)
    {
    }
    ~vc_order() { }

    bool operator()(leda::node v, leda::node w) const
    {
        return (*_W)[v] * (*_D)[w] > (*_W)[w] * (*_D)[v];
        // return (*_W)[v] > (*_W)[w];
        // return (*_D)[w] > (*_D)[v];
    }

  private:
    const leda::node_array<T>* _W;
    const leda::node_array<size_t>* _D;
};

/**
 * Greedy algorithm for minimun vertex cover problem
 */
template <typename T>
T min_vc_greedy(const leda::graph& G, const leda::node_array<T>& weight,
    leda::list<leda::node>& S)
{
    typedef typename boost::heap::binomial_heap<leda::node,
        boost::heap::compare<vc_order<T>>>
        bpq_t;
    typedef typename bpq_t::handle_type bh_t;

    leda::node_array<size_t> D(G); // number of uncovered edges

    leda::node v;
    forall_nodes(v, G) D[v] = G.degree(v);

    bpq_t Bpq(vc_order<T>(weight, D));
    leda::node_array<bh_t> bh_handlers(G);
    forall_nodes(v, G)
    {
        bh_t bh = Bpq.push(v);
        bh_handlers[v] = bh;
    }

    leda::edge_array<bool> is_covered(G, false);
    leda::node_array<bool> is_vc(G, false);

    T total_cost = 0;
    size_t num_edges = G.number_of_edges();

    while (num_edges > 0 && !Bpq.empty())
    {
        leda::node s = Bpq.top();
        Bpq.pop();
        is_vc[s] = true;
        S.append(s);
        total_cost += weight[s];

        leda::edge e2;
        forall_adj_edges(e2, s)
        {
            if (is_covered[e2])
                continue;
            is_covered[e2] = true;
            leda::node t = G.opposite(e2, s);
            D[t] -= 1;
            Bpq.update(bh_handlers[t]);
            --num_edges;
        }
    }

    leda::list_item it;
    forall_items(it, S)
    {
        leda::node v = S.inf(it);
        leda::node w;
        bool found = false;
        forall_adj_nodes(w, v)
        {
            if (!is_vc[w])
            {
                found = true;
                break;
            }
        }
        if (!found)
        {
            S.del_item(it);
            total_cost -= weight[v];
            is_vc[v] = false;
        }
    }

    return total_cost;
}

#endif


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FMA_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FMA_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-arithmetic
    This function object computes the (fused) multiply add of these three parameters.

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

    @par Notes
    The call `fma(x, y, z)` is similar to `x*y+z`

    But really conformant fused multiply/add also implies

    - only one rounding

    - no "intermediate" overflow

    fma provides this for all integral types and also each time it is reasonable
    in terms of performance for floating ones (i.e. if the system has the hard
    wired capability).

    If you need pedantic fma capabilities in all circumstances in your own
    code you can use the pedantic_ or std_ decorator
    (although both can be very expensive).

    @par Decorators

    - std_ for floating entries to call directly std::fma. This generally implies pedantic
      fma behaviour, but in no way improved performances.
    - pedantic_ ensures the fma properties and allows SIMD acceleration if available.

    @see fms, fnma, fnms

    @par Example:

      @snippet fma.cpp fma

    @par Possible output:

      @snippet fma.txt fma
  **/
  Value fma(Value const& x, Value const& y, Value const& z);
} }
#endif

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

#endif


/*
 * COPYRIGHT AND PERMISSION NOTICE
 * Penn Software MSCKF_VIO
 * Copyright (C) 2017 The Trustees of the University of Pennsylvania
 * All rights reserved.
 */

// The original file belongs to MSCKF_VIO (https://github.com/KumarRobotics/msckf_vio/)
// Several changes have been made to use it in orcvio

#ifndef FEATURE_HPP
#define FEATURE_HPP

#include <iostream>
#include <map>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include <orcvio/utils/math_utils.hpp>
#include <orcvio/imu_state.h>

namespace orcvio
{

  /*
 * @brief Feature Salient part of an image. Please refer
 *    to the Appendix of "A Multi-State Constraint Kalman
 *    Filter for Vision-aided Inertial Navigation" for how
 *    the 3d position of a feature is initialized.
 */
  struct Feature
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /*
   * @brief OptimizationConfig Configuration parameters
   *    for 3d feature position optimization.
   */
    struct OptimizationConfig
    {
      double translation_threshold;
      double huber_epsilon;
      double estimation_precision;
      double initial_damping;
      int outer_loop_max_iteration;
      int inner_loop_max_iteration;
      double cost_threshold;
      double init_final_dist_threshold;

      OptimizationConfig() : translation_threshold(0.2),
                             huber_epsilon(0.01),
                             estimation_precision(5e-7),
                             initial_damping(1e-3),
                             outer_loop_max_iteration(10),
                             inner_loop_max_iteration(10),
                             cost_threshold(4.7673e-04),
                             init_final_dist_threshold(5)
      {
        return;
      }
    };

    // Constructors for the struct.
    Feature() : id(0), position(Eigen::Vector3d::Zero()),
                is_initialized(false), best_cost(99999.9),
                id_anchor(-1), invParam(Eigen::Vector3d::Zero()),
                in_state(false), totalObsNum(0), ekf_feature(false) {}

    Feature(const FeatureIDType &new_id) : id(new_id),
                                           position(Eigen::Vector3d::Zero()),
                                           is_initialized(false), best_cost(99999.9),
                                           id_anchor(-1), invParam(Eigen::Vector3d::Zero()),
                                           in_state(false), totalObsNum(0), ekf_feature(false) {}

    /*
   * @brief triangulate feature position based on initial guess and LM
   * @param cam_poses: all the camera poses
   * @param measurements: measurements
   * @param final_position: to hold the initial position
   * @param final_position: to hold the position after LM
   * @param T_c_w_last: to hold the pose of last camera
   * @return a flag indicating whether this feature is valid
   */
    inline bool triangulate_position(std::vector<Eigen::Isometry3d,
                                                 Eigen::aligned_allocator<Eigen::Isometry3d>> &cam_poses,
                                     const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> &measurements,
                                     Eigen::Vector3d &solution, Eigen::Vector3d &final_position, Eigen::Isometry3d &T_c_w_last);

    /*
   * @brief cost Compute the cost of the camera observations
   * @param T_c0_c1 A rigid body transformation takes
   *    a vector in c0 frame to ci frame.
   * @param x The current estimation.
   * @param z The ith measurement of the feature j in ci frame.
   * @return e The cost of this observation.
   */
    inline void cost(const Eigen::Isometry3d &T_c0_ci,
                     const Eigen::Vector3d &x, const Eigen::Vector2d &z,
                     double &e) const;

    /*
   * @brief jacobian Compute the Jacobian of the camera observation
   * @param T_c0_c1 A rigid body transformation takes
   *    a vector in c0 frame to ci frame.
   * @param x The current estimation.
   * @param z The actual measurement of the feature in ci frame.
   * @return J The computed Jacobian.
   * @return r The computed residual.
   * @return w Weight induced by huber kernel.
   */
    inline void jacobian(const Eigen::Isometry3d &T_c0_ci,
                         const Eigen::Vector3d &x, const Eigen::Vector2d &z,
                         Eigen::Matrix<double, 2, 3> &J, Eigen::Vector2d &r,
                         double &w) const;

    /*
   * @brief generateInitialGuess Compute the initial guess of
   *    the feature's 3d position using only two views.
   * @param T_c1_c2: A rigid body transformation taking
   *    a vector from c2 frame to c1 frame.
   * @param z1: feature observation in c1 frame.
   * @param z2: feature observation in c2 frame.
   * @return p: Computed feature position in c1 frame.
   */
    inline void generateInitialGuess(
        const Eigen::Isometry3d &T_c1_c2, const Eigen::Vector2d &z1,
        const Eigen::Vector2d &z2, Eigen::Vector3d &p) const;

    /*
   * @brief checkMotion Check the input camera poses to ensure
   *    there is enough translation to triangulate the feature
   *    positon.
   * @param imu_states : input to aquire camera poses.
   × @param if_tracked : if feature be tracked now.
   * @return True if the translation between the input camera
   *    poses is sufficient.
   */
    inline bool checkMotion(
        const IMUStateServer &imu_states, bool if_tracked) const;

    /*
   * @brief InitializePosition Intialize the feature position
   *    based on all current available measurements.
   * @param imu_states: A map containing the camera poses with its
   *    ID as the associated key value.
   * @param curr_id: current camera id.
   * @return The computed 3d position is used to set the position
   *    member variable. Note the resulted position is in world
   *    frame.
   * @return True if the estimated 3d position of the feature
   *    is valid.
   */
    inline bool initializePosition(
        const IMUStateServer &imu_states, const StateIDType &curr_id);

    /*
   * @brief initializePosition_AssignAnchor Intialize the feature position
   *    based on all current available measurements. Current camera would
   *    be the anchor
   * @param imu_states: A map containing the camera poses with its
   *    ID as the associated key value.
   * @param anchor_id: Assigned anchor camera id.
   * @return The computed 3d position is used to set the position
   *    member variable. Note the resulted position is in world
   *    frame.
   * @return True if the estimated 3d position of the feature
   *    is valid.
   */
    inline bool initializePosition_AssignAnchor(
        const IMUStateServer &imu_states);

    /*
   * @brief initializeInvParamPosition Intialize the feature position
   *    based on all current available measurements. Result in
   *    inverse depth parameterization in anchor frame.
   * @param imu_states: A map containing the camera poses with its
   *    ID as the associated key value.
   * @param curr_id: current camera id.
   * @return The computed 3d position is used to set the position
   *    member variable. Note the resulted position is in world
   *    frame.
   * @return The computed inverse depth parameterizations is used to
   *    set the invParam variable. Note the resulted inverse depth
   *    parameterization is in the anchor frame, which is the newest
   *    frame observing this feature except for current frame.
   * @return True if the estimated 3d position of the feature
   *    is valid.
   */
    inline bool initializeInvParamPosition(
        const IMUStateServer &imu_states, const StateIDType &curr_id);

    // An unique identifier for the feature.
    // In case of long time running, the variable
    // type of id is set to FeatureIDType in order
    // to avoid duplication.
    FeatureIDType id;

    // id for next feature
    static FeatureIDType next_id;

    // Store the observations of the features in the
    // state_id(key)-image_coordinates(value) manner.
    std::map<StateIDType, Eigen::Vector2d, std::less<StateIDType>,
             Eigen::aligned_allocator<
                 std::pair<const StateIDType, Eigen::Vector2d>>>
        observations;

    // Store the observations of the features velocity in the
    // state_id(key)-features_velocity(value) manner.
    std::map<StateIDType, Eigen::Vector2d, std::less<StateIDType>,
             Eigen::aligned_allocator<
                 std::pair<const StateIDType, Eigen::Vector2d>>>
        observations_vel;

    // 3d postion of the feature in the world frame.
    Eigen::Vector3d position;

    // First estimate 3d position in world frame
    Eigen::Vector3d position_FEJ;

    // Best normalized cost, added by QXC
    double best_cost;

    // First observed camera pose, added by QXC
    Eigen::Isometry3d firstCamPose;

    // If failed because of nagtive depth of big reprojection error
    bool failed_by_neg_dpth;
    bool failed_by_big_proj;

    // [x/z, y/z] under first observed camera coordinate, added by QXC
    Eigen::Vector2d solutionInFirstCam;

    // A indicator to show if the 3d postion of the feature
    // has been initialized or not.
    bool is_initialized;

    // Optimization configuration for solving the 3d position.
    static OptimizationConfig optimization_config;

    // QXC debug log
    inline double getMaxObsDiff();
    inline double getTotalObsChange();

    // Anchor camera id
    StateIDType id_anchor;

    // 3d inverse depth parameter position in anchor camera frame.
    Eigen::Vector3d invParam;

    // 1d inverse depth parameter position in anchor camera frame.
    double invDepth;
    // Corrected observation in anchor camera frame.
    Eigen::Vector3d obs_anchor;

    // If this feature is in filter state
    bool in_state;

    // Record total observation number
    int totalObsNum;

    // If this is a potential ekf feature
    bool ekf_feature;
  };

  typedef std::map<FeatureIDType, Feature, std::less<int>,
                   Eigen::aligned_allocator<
                       std::pair<const FeatureIDType, Feature>>>
      MapServer;

  void Feature::cost(const Eigen::Isometry3d &T_c0_ci,
                     const Eigen::Vector3d &x, const Eigen::Vector2d &z,
                     double &e) const
  {
    // Compute hi1, hi2, and hi3 as Equation (37).
    const double &alpha = x(0);
    const double &beta = x(1);
    const double &rho = x(2);

    Eigen::Vector3d h = T_c0_ci.linear() *
                            Eigen::Vector3d(alpha, beta, 1.0) +
                        rho * T_c0_ci.translation();
    double &h1 = h(0);
    double &h2 = h(1);
    double &h3 = h(2);

    // Predict the feature observation in ci frame.
    Eigen::Vector2d z_hat(h1 / h3, h2 / h3);

    // Compute the residual.
    e = (z_hat - z).squaredNorm();
    return;
  }

  void Feature::jacobian(const Eigen::Isometry3d &T_c0_ci,
                         const Eigen::Vector3d &x, const Eigen::Vector2d &z,
                         Eigen::Matrix<double, 2, 3> &J, Eigen::Vector2d &r,
                         double &w) const
  {

    // Compute hi1, hi2, and hi3 as Equation (37).
    const double &alpha = x(0);
    const double &beta = x(1);
    const double &rho = x(2);

    Eigen::Vector3d h = T_c0_ci.linear() *
                            Eigen::Vector3d(alpha, beta, 1.0) +
                        rho * T_c0_ci.translation();
    double &h1 = h(0);
    double &h2 = h(1);
    double &h3 = h(2);

    // Compute the Jacobian.
    Eigen::Matrix3d W;
    W.leftCols<2>() = T_c0_ci.linear().leftCols<2>();
    W.rightCols<1>() = T_c0_ci.translation();

    J.row(0) = 1 / h3 * W.row(0) - h1 / (h3 * h3) * W.row(2);
    J.row(1) = 1 / h3 * W.row(1) - h2 / (h3 * h3) * W.row(2);

    // Compute the residual.
    Eigen::Vector2d z_hat(h1 / h3, h2 / h3);
    r = z_hat - z;

    // Compute the weight based on the residual.
    double e = r.norm();
    if (e <= optimization_config.huber_epsilon)
      w = 1.0;
    else
      w = std::sqrt(2.0 * optimization_config.huber_epsilon / e);

    return;
  }

  void Feature::generateInitialGuess(
      const Eigen::Isometry3d &T_c1_c2, const Eigen::Vector2d &z1,
      const Eigen::Vector2d &z2, Eigen::Vector3d &p) const
  {
    // Construct a least square problem to solve the depth.
    Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);

    Eigen::Vector2d A(0.0, 0.0);
    A(0) = m(0) - z2(0) * m(2);
    A(1) = m(1) - z2(1) * m(2);

    Eigen::Vector2d b(0.0, 0.0);
    b(0) = z2(0) * T_c1_c2.translation()(2) - T_c1_c2.translation()(0);
    b(1) = z2(1) * T_c1_c2.translation()(2) - T_c1_c2.translation()(1);

    // Solve for the depth.
    double depth = (A.transpose() * A).inverse() * A.transpose() * b;
    p(0) = z1(0) * depth;
    p(1) = z1(1) * depth;
    p(2) = depth;
    return;
  }

  bool Feature::checkMotion(
      const IMUStateServer &imu_states, bool if_tracked) const
  {

    StateIDType first_cam_id = observations.begin()->first;
    StateIDType last_cam_id;
    if (if_tracked)
      last_cam_id = (--(--observations.end()))->first;
    else
      last_cam_id = (--observations.end())->first;

    Eigen::Isometry3d first_cam_pose;
    first_cam_pose.linear() = imu_states.find(first_cam_id)->second.orientation_cam;
    first_cam_pose.translation() =
        imu_states.find(first_cam_id)->second.position_cam;

    Eigen::Isometry3d last_cam_pose;
    last_cam_pose.linear() = imu_states.find(last_cam_id)->second.orientation_cam;
    last_cam_pose.translation() =
        imu_states.find(last_cam_id)->second.position_cam;

    // Get the direction of the feature when it is first observed.
    // This direction is represented in the world frame.
    Eigen::Vector3d feature_direction(
        observations.begin()->second(0),
        observations.begin()->second(1), 1.0);
    feature_direction = feature_direction / feature_direction.norm();
    feature_direction = first_cam_pose.linear() * feature_direction;

    // Compute the translation between the first frame
    // and the last frame. We assume the first frame and
    // the last frame will provide the largest motion to
    // speed up the checking process.
    Eigen::Vector3d translation = last_cam_pose.translation() -
                                  first_cam_pose.translation();
    double parallel_translation =
        translation.transpose() * feature_direction;
    Eigen::Vector3d orthogonal_translation = translation -
                                             parallel_translation * feature_direction;

    if (orthogonal_translation.norm() >
        optimization_config.translation_threshold)
      return true;
    else
      return false;
  }

  bool Feature::initializePosition(
      const IMUStateServer &imu_states, const StateIDType &curr_id)
  {
    // Organize camera poses and feature observations properly.
    std::vector<Eigen::Isometry3d,
                Eigen::aligned_allocator<Eigen::Isometry3d>>
        cam_poses(0);
    std::vector<Eigen::Vector2d,
                Eigen::aligned_allocator<Eigen::Vector2d>>
        measurements(0);

    std::vector<StateIDType> cam_ids(0);
    for (auto &m : observations)
    {
      // TODO: This should be handled properly. Normally, the
      //    required camera states should all be available in
      //    the input imu_states buffer.
      auto state_iter = imu_states.find(m.first);
      if (state_iter == imu_states.end())
        continue;

      if (curr_id == state_iter->first)
        continue;

      // Add the measurement.
      measurements.push_back(m.second.head<2>());

      // This camera pose will take a vector from this camera frame
      // to the world frame.
      Eigen::Isometry3d cam_pose;
      cam_pose.linear() = state_iter->second.orientation_cam;
      cam_pose.translation() = state_iter->second.position_cam;

      cam_poses.push_back(cam_pose);

      cam_ids.push_back(state_iter->first);
    }

    Eigen::Vector3d solution, final_position;
    Eigen::Isometry3d T_c_w_last;
    bool is_valid_solution = triangulate_position(cam_poses, measurements, solution, final_position, T_c_w_last);

    if (is_valid_solution)
    {
      if (!is_initialized)
        position_FEJ = position;
      is_initialized = true;
      position = T_c_w_last.linear() * final_position + T_c_w_last.translation();
      invParam = solution;
      id_anchor = cam_ids[cam_ids.size() - 1];
      invDepth = 1 / final_position(2);
      obs_anchor = Eigen::Vector3d(final_position(0) * invDepth, // correct observation
                                   final_position(1) * invDepth, 1);
      // obs_anchor = Eigen::Vector3d(measurements[cam_ids.size()-1](0),     // do not correct observation
      //     measurements[cam_ids.size()-1](1), 1);
    }

    return is_valid_solution;
  }

  bool Feature::initializePosition_AssignAnchor(
      const IMUStateServer &imu_states)
  {
    // Organize camera poses and feature observations properly.
    std::vector<Eigen::Isometry3d,
                Eigen::aligned_allocator<Eigen::Isometry3d>>
        cam_poses(0);
    std::vector<Eigen::Vector2d,
                Eigen::aligned_allocator<Eigen::Vector2d>>
        measurements(0);

    std::vector<StateIDType> cam_ids(0);
    for (auto &m : observations)
    {
      // TODO: This should be handled properly. Normally, the
      //    required camera states should all be available in
      //    the input imu_states buffer.
      auto state_iter = imu_states.find(m.first);
      if (state_iter == imu_states.end())
        continue;

      // Add the measurement.
      measurements.push_back(m.second.head<2>());

      // This camera pose will take a vector from this camera frame
      // to the world frame.
      Eigen::Isometry3d cam_pose;
      cam_pose.linear() = state_iter->second.orientation_cam;
      cam_pose.translation() = state_iter->second.position_cam;

      cam_poses.push_back(cam_pose);

      cam_ids.push_back(state_iter->first);
    }

    Eigen::Vector3d solution, final_position;
    Eigen::Isometry3d T_c_w_last;
    bool is_valid_solution = triangulate_position(cam_poses, measurements, solution, final_position, T_c_w_last);

    if (is_valid_solution)
    {
      if (!is_initialized)
        position_FEJ = position;
      is_initialized = true;
      position = T_c_w_last.linear() * final_position + T_c_w_last.translation();
      invParam = solution;
      id_anchor = cam_ids[cam_ids.size() - 1];
      invDepth = 1 / final_position(2);
      obs_anchor = Eigen::Vector3d(final_position(0) * invDepth, // correct observation
                                   final_position(1) * invDepth, 1);
      // obs_anchor = Eigen::Vector3d(measurements[cam_ids.size()-1](0),     // do not correct observation
      //     measurements[cam_ids.size()-1](1), 1);
    }

    return is_valid_solution;
  }

  bool Feature::initializeInvParamPosition(
      const IMUStateServer &imu_states, const StateIDType &curr_id)
  {
    // Organize camera poses and feature observations properly.
    std::vector<Eigen::Isometry3d,
                Eigen::aligned_allocator<Eigen::Isometry3d>>
        cam_poses(0);
    std::vector<Eigen::Vector2d,
                Eigen::aligned_allocator<Eigen::Vector2d>>
        measurements(0);

    std::vector<StateIDType> cam_ids(0);
    for (auto &m : observations)
    {
      // TODO: This should be handled properly. Normally, the
      //    required camera states should all be available in
      //    the input imu_states buffer.
      auto state_iter = imu_states.find(m.first);
      if (state_iter == imu_states.end())
        continue;

      if (curr_id == state_iter->first)
        continue;

      // Add the measurement.
      measurements.push_back(m.second.head<2>());

      // This camera pose will take a vector from this camera frame
      // to the world frame.
      Eigen::Isometry3d cam_pose;
      cam_pose.linear() = state_iter->second.orientation_cam;
      cam_pose.translation() = state_iter->second.position_cam;

      cam_poses.push_back(cam_pose);

      cam_ids.push_back(state_iter->first);
    }

    Eigen::Vector3d solution, final_position;
    Eigen::Isometry3d T_c_w_last;
    bool is_valid_solution = triangulate_position(cam_poses, measurements, solution, final_position, T_c_w_last);

    if (is_valid_solution)
    {
      if (!is_initialized)
        position_FEJ = position;
      ekf_feature = true;
      is_initialized = true;
      position = T_c_w_last.linear() * final_position + T_c_w_last.translation();
      invParam = solution;
      id_anchor = cam_ids[cam_ids.size() - 1];
      invDepth = 1 / final_position(2);
      obs_anchor = Eigen::Vector3d(final_position(0) * invDepth, // correct observation
                                   final_position(1) * invDepth, 1);
      // obs_anchor = Eigen::Vector3d(measurements[cam_ids.size()-1](0),     // do not correct observation
      //     measurements[cam_ids.size()-1](1), 1);
    }

    return is_valid_solution;
  }

  double Feature::getMaxObsDiff()
  {
    Eigen::Vector2d p0(observations.begin()->second(0),
                       observations.begin()->second(1));
    Eigen::Vector2d p1((--observations.end())->second(0),
                       (--observations.end())->second(1));

    return (p0 - p1).norm();
  }

  double Feature::getTotalObsChange()
  {

    std::vector<Eigen::Vector2d,
                Eigen::aligned_allocator<Eigen::Vector2d>>
        p(0);
    for (const auto &obs : observations)
      p.push_back(Eigen::Vector2d(obs.second(0), obs.second(1)));

    double diff = 0.0;
    for (int i = 1; i < p.size(); i++)
    {
      diff += (p[i] - p[i - 1]).norm();
    }
    return diff;
  }

  bool Feature::triangulate_position(std::vector<Eigen::Isometry3d,
                                                 Eigen::aligned_allocator<Eigen::Isometry3d>> &cam_poses,
                                     const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> &measurements,
                                     Eigen::Vector3d &solution, Eigen::Vector3d &final_position, Eigen::Isometry3d &T_c_w_last)
  {

    // All camera poses should be modified such that it takes a
    // vector from the last camera frame in the buffer to this
    // camera frame.
    T_c_w_last = cam_poses[cam_poses.size() - 1];
    for (auto &pose : cam_poses)
      pose = pose.inverse() * T_c_w_last;

    // Generate initial guess
    Eigen::Vector3d initial_position(0.0, 0.0, 0.0);
    if (!is_initialized)
      generateInitialGuess(cam_poses[0], measurements[cam_poses.size() - 1],
                           measurements[0], initial_position);
    else
      initial_position = T_c_w_last.inverse() * position;

    solution << initial_position(0) / initial_position(2),
        initial_position(1) / initial_position(2),
        1.0 / initial_position(2);

    // Apply Levenberg-Marquart method to solve for the 3d position.
    double lambda = optimization_config.initial_damping;
    int inner_loop_cntr = 0;
    int outer_loop_cntr = 0;
    bool is_cost_reduced = false;
    double delta_norm = 0;

    // Compute the initial cost.
    double total_cost = 0.0;
    for (int i = 0; i < cam_poses.size(); ++i)
    {
      double this_cost = 0.0;
      cost(cam_poses[i], solution, measurements[i], this_cost);
      total_cost += this_cost;
    }

    // Outer loop.
    do
    {
      Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
      Eigen::Vector3d b = Eigen::Vector3d::Zero();

      for (int i = 0; i < cam_poses.size(); ++i)
      {
        Eigen::Matrix<double, 2, 3> J;
        Eigen::Vector2d r;
        double w;

        jacobian(cam_poses[i], solution, measurements[i], J, r, w);

        if (w == 1)
        {
          A += J.transpose() * J;
          b += J.transpose() * r;
        }
        else
        {
          double w_square = w * w;
          A += w_square * J.transpose() * J;
          b += w_square * J.transpose() * r;
        }
      }

      // Inner loop.
      // Solve for the delta that can reduce the total cost.
      do
      {
        Eigen::Matrix3d damper = lambda * Eigen::Matrix3d::Identity();
        Eigen::Vector3d delta = (A + damper).ldlt().solve(b);
        Eigen::Vector3d new_solution = solution - delta;
        delta_norm = delta.norm();

        double new_cost = 0.0;
        for (int i = 0; i < cam_poses.size(); ++i)
        {
          double this_cost = 0.0;
          cost(cam_poses[i], new_solution, measurements[i], this_cost);
          new_cost += this_cost;
        }

        if (new_cost < total_cost)
        {
          is_cost_reduced = true;
          solution = new_solution;
          total_cost = new_cost;
          lambda = lambda / 10 > 1e-10 ? lambda / 10 : 1e-10;
        }
        else
        {
          is_cost_reduced = false;
          lambda = lambda * 10 < 1e12 ? lambda * 10 : 1e12;
        }

      } while (inner_loop_cntr++ <
                   optimization_config.inner_loop_max_iteration &&
               !is_cost_reduced);

      inner_loop_cntr = 0;

    } while (outer_loop_cntr++ <
                 optimization_config.outer_loop_max_iteration &&
             delta_norm > optimization_config.estimation_precision);

    // Covert the feature position from inverse depth
    // representation to its 3d coordinate.
    // Eigen::Vector3d final_position(solution(0)/solution(2),
    //     solution(1)/solution(2), 1.0/solution(2));
    final_position << solution(0) / solution(2),
        solution(1) / solution(2), 1.0 / solution(2);

    // Check if the solution is valid. Make sure the feature
    // is in front of every camera frame observing it.
    bool is_valid_solution = true;
    failed_by_neg_dpth = false;
    for (const auto &pose : cam_poses)
    {
      Eigen::Vector3d pos =
          pose.linear() * final_position + pose.translation();
      if (pos(2) <= 0)
      {
        is_valid_solution = false;
        failed_by_neg_dpth = true;
        break;
      }
    }

    //  check total_cost
    double normalized_cost =
        total_cost / (2 * cam_poses.size() * cam_poses.size());
    double uv_cost =
        sqrt(total_cost / cam_poses.size());
    failed_by_big_proj = false;

    if ((final_position - initial_position).norm() > optimization_config.init_final_dist_threshold)
    {
      is_valid_solution = false;
      failed_by_big_proj = true;
    }

    if (normalized_cost > optimization_config.cost_threshold)
    {
      is_valid_solution = false;
      failed_by_big_proj = true;
    }

    return is_valid_solution;
  }

} // namespace orcvio

#endif // FEATURE_HPP


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 5/30/2021, 4:50:13 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <boost/integer/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
#include <atcoder/all>
using namespace std;
// using namespace atcoder;
using boost::rational;
using boost::integer::gcd; // for C++14 or for cpp_int
using boost::integer::lcm; // for C++14 or for cpp_int
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
/***************************************************
    Fundermental Tools
 ***************************************************/
// ----- max_heap and min_heap -----
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- mint -----
using mint = atcoder::modint1000000007;
// using mint = atcoder::modint998244353;
// using mint = atcoder::modint; // atcoder::modint::set_mod(xxx);
// using mint = atcoder::static_modint<1000000009>;
// using mint0 = dynamic_modint<xxx>;
// using mint1 = dynamic_modint<yyy>;
istream &operator>>(istream &is, mint &x)
{
  ll t;
  is >> t;
  x = t;
  return is;
}
ostream &operator<<(ostream &os, mint const &x)
{
  return os << x.val();
}
// ----- Combination -----
template <typename Mint = mint>
class Combination
{
public:
  constexpr static ll MAX_SIZE{4'000'010LL};
  // constexpr static ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed

  vector<mint> inv, fact, factinv;

  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[Mint::mod() % i]) * (Mint::mod() / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }

  Mint operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }

  Mint catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using combination = Combination<mint>;
// ----- for C++17 -----
template <typename T, typename enable_if<is_integral<T>::value>::type * = nullptr>
size_t popcount(T x) { return bitset<64>(x).count(); }
size_t popcount(string const &S) { return bitset<200010>{S}.count(); }
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
/***************************************************
    AtCoder Library / Data Structures
 ***************************************************/
// ----- FenwickTree -----
template <typename T = ll>
class FenwickTree : public atcoder::fenwick_tree<T>
{
public:
  using atcoder::fenwick_tree<T>::fenwick_tree;

  FenwickTree(vector<T> const &v) : FenwickTree(static_cast<int>(v.size()))
  {
    for (auto i{size_t{0}}; i < v.size(); ++i)
    {
      atcoder::fenwick_tree<T>::add(i, v[i]);
    }
  }

  T operator[](int i)
  {
    return atcoder::fenwick_tree<T>::sum(i, i + 1);
  }
};
/***************************************************
    AtCoder Library / Math
 ***************************************************/
// ----- Math -----
using atcoder::convolution;
using atcoder::convolution_ll;
using atcoder::floor_sum;
/***************************************************
    AtCoder Library / Graphs
 ***************************************************/
// ----- UnionFind -----
using UnionFind = atcoder::dsu;
// ----- MaxFlow -----
template <typename Cap = ll>
class MaxFlow : public atcoder::mf_graph<Cap>
{
public:
  using atcoder::mf_graph<Cap>::mf_graph;

  int add_edge(int from, int to)
  {
    return atcoder::mf_graph<Cap>::add_edge(from, to, Cap{1});
  }
};
ostream &operator<<(ostream &os, typename atcoder::mf_graph<ll>::edge const &edge)
{
  return os << "from: " << edge.from << ", to: " << edge.to << ", cap: " << edge.cap << ", flow: " << edge.flow;
}
// ----- MinCostFlow -----
template <typename Cap = ll, typename Cost = ll>
class MinCostFlow : public atcoder::mcf_graph<Cap, Cost>
{
private:
  Cost infty;

public:
  using atcoder::mcf_graph<Cap, Cost>::mcf_graph;

  MinCostFlow(int n, Cost infty = Cost{0}) : atcoder::mcf_graph<Cap, Cost>::mcf_graph(n), infty{infty} {}

  int add_edge(int from, int to, Cap cap)
  {
    return atcoder::mcf_graph<Cap, Cost>::add_edge(from, to, cap, Cost{0});
  }

  int add_edge(int from, int to, Cap cap, Cost cost)
  {
    return atcoder::mcf_graph<Cap, Cost>::add_edge(from, to, cap, cost + infty);
  }

  pair<Cap, Cost> flow(int s, int t)
  {
    return flow(s, t, std::numeric_limits<Cap>::max());
  }

  pair<Cap, Cost> flow(int s, int t, Cap flow_limit)
  {
    return slope(s, t, flow_limit).back();
  }

  vector<pair<Cap, Cost>> slope(int s, int t)
  {
    return slope(s, t, numeric_limits<Cap>::max());
  }

  vector<pair<Cap, Cost>> slope(int s, int t, Cap flow_limit)
  {
    auto res{atcoder::mcf_graph<Cap, Cost>::slope(s, t, flow_limit)};
    for (auto &[cap, cost] : res)
    {
      cost -= cap * infty;
    }
    return res;
  }

  typename atcoder::mcf_graph<Cap, Cost>::edge get_edge(int i)
  {
    auto res{atcoder::mcf_graph<Cap, Cost>::get_edge(i)};
    res.cost -= infty;
    return res;
  }

  vector<typename atcoder::mcf_graph<Cap, Cost>::edge> edges()
  {
    auto res{atcoder::mcf_graph<Cap, Cost>::edges()};
    for (auto &e : res)
    {
      e.cost -= infty;
    }
    return res;
  }
};
ostream &operator<<(ostream &os, typename atcoder::mcf_graph<ll, ll>::edge const &edge)
{
  return os << "from: " << edge.from << ", to: " << edge.to << ", cap: " << edge.cap << ", flow: " << edge.flow << ", cost: " << edge.cost;
}
// ----- StronglyConnectedComponents -----
using StronglyConnectedComponents = atcoder::scc_graph;
// ----- TwoSat -----
using TwoSat = atcoder::two_sat;
/***************************************************
    My Library / DP-Typical
 ***************************************************/
// ----- LongestIncreasingSubsequence -----
template <typename T>
class LongestIncreasingSubsequence
{
  struct Cache_LIS
  {
    typename vector<T>::iterator it;
    T value;
  };

  int n; // fixed
  T infty;
  vector<T> dp;
  stack<Cache_LIS> st;

public:
  LongestIncreasingSubsequence() {}
  LongestIncreasingSubsequence(int n, T infty = numeric_limits<T>::max()) : n{n}, infty{infty}, dp(n, infty) {}

  int query(T a)
  {
    auto it{lower_bound(dp.begin(), dp.end(), a)};
    auto value{*it};
    st.push(Cache_LIS{it, value});
    *it = a;
    return lower_bound(dp.begin(), dp.end(), infty) - dp.begin();
  }

  bool rollback()
  {
    if (st.empty())
    {
      return false;
    }
    auto const &c{st.top()};
    *c.it = c.value;
    st.pop();
    return true;
  }

private:
};
/***************************************************
    My Library / Graphs
 ***************************************************/
// ----- LowestCommonAncestor -----
class LowestCommonAncestor
{
  // helper classes
  struct Edge
  {
    // Initialized by initializer list. Take care for the order of the field.
    int src, dst;
    ll cost;
  };

  struct Vertex
  {
    int depth;
    ll length;
  };

  // fields
  int N, root, L;
  vector<vector<Edge>> E;
  vector<Vertex> V;
  vector<vector<int>> ancestors;

  // methods
public:
  LowestCommonAncestor(int N, int root = 0);
  void add_edge(int a, int b, ll c = 0);
  void init();
  void init(int root); // after adding all edges.

  // LowestCommonAncestor
  int operator()(int a, int b);

  int depth(int a, int b) { return V[a].depth + V[b].depth - 2 * V[(*this)(a, b)].depth; }
  int depth(int v) { return depth(v, root); }
  ll length(int a, int b) { return V[a].length + V[b].length - 2 * V[(*this)(a, b)].length; }
  ll length(int v) { return length(v, root); }
  int parent(int v) { return ancestors[v][0]; }

private:
  void dfs(int v, int d = 0, ll l = 0, int p = -1);
};
// LowestCommonAncestor: implement
LowestCommonAncestor::LowestCommonAncestor(int N, int root) : N{N}, root{root}, L{0}, E(N), V(N)
{
  while ((1 << L) < N)
  {
    ++L;
  }
  ancestors = vector<vector<int>>(N + 1, vector<int>(L, N));
}
void LowestCommonAncestor::add_edge(int a, int b, ll c)
{
  E[a].push_back(LowestCommonAncestor::Edge{a, b, c});
  E[b].push_back(LowestCommonAncestor::Edge{b, a, c});
}
void LowestCommonAncestor::init(int root)
{
  LowestCommonAncestor::root = root;
  init();
}
void LowestCommonAncestor::init()
{
  dfs(root);
  for (auto i = 0; i < L - 1; i++)
  {
    for (auto v = 0; v < N; v++)
    {
      if (ancestors[v][i] != -1)
      {
        ancestors[v][i + 1] = ancestors[ancestors[v][i]][i];
      }
    }
  }
}
int LowestCommonAncestor::operator()(int a, int b)
{
  if (V[a].depth > V[b].depth)
  {
    swap(a, b);
  }
  int gap = V[b].depth - V[a].depth;
  for (auto i = L - 1; i >= 0; i--)
  {
    int len{1 << i};
    if (gap >= len)
    {
      gap -= len;
      b = ancestors[b][i];
    }
  }
  if (a == b)
  {
    return a;
  }
  for (auto i = L - 1; i >= 0; i--)
  {
    int na{ancestors[a][i]};
    int nb{ancestors[b][i]};
    if (na != nb)
    {
      a = na;
      b = nb;
    }
  }
  return ancestors[a][0];
}
void LowestCommonAncestor::dfs(int v, int d, ll l, int p)
{
  if (p != -1)
  {
    ancestors[v][0] = p;
  }
  V[v].depth = d;
  V[v].length = l;
  for (auto const &e : E[v])
  {
    int u{e.dst};
    if (u == p)
    {
      continue;
    }
    dfs(u, d + 1, l + e.cost, v);
  }
}
// ----- WarshallFloyd -----
template <typename T>
void WarshallFloyd(vector<vector<T>> &V, T infinity = numeric_limits<T>::max())
{
  // It is valid to apply this method for
  //  - a directed/undirected graph,
  //  - a graph whose edge may be negative.
  //    - Negative cycle can be detected by V[i][i] < 0 if we initialize V[i][i] = 0.
  auto N{static_cast<int>(V.size())};
  for (auto k{0}; k < N; ++k)
  {
    for (auto i{0}; i < N; ++i)
    {
      for (auto j{0}; j < N; ++j)
      {
        if (V[i][k] == infinity || V[k][j] == infinity)
        {
          continue;
        }
        ch_min(V[i][j], V[i][k] + V[k][j]);
      }
    }
  }
}
/***************************************************
    My Library / Math
 ***************************************************/
// ----- Permutation -----
struct Permutation
{
  vector<int> V;

  Permutation() {}
  Permutation(vector<int> const &V) : V{V} {}

  static Permutation unit(size_t N)
  {
    vector<int> X(N);
    for (auto i = 0; i < static_cast<int>(N); ++i)
    {
      X[i] = i;
    }
    return Permutation{X};
  }

  size_t size() const { return V.size(); }
  int operator[](size_t i) const { return V[i]; }

  Permutation inverse() const
  {
    vector<int> Q(size());
    for (auto i = 0; i < static_cast<int>(size()); ++i)
    {
      Q[(*this)[i]] = i;
    }
    return Permutation{Q};
  }

  Permutation operator*(Permutation const &Q) const
  {
    assert(size() == Q.size());
    vector<int> R(size());
    for (auto i = 0; i < static_cast<int>(size()); ++i)
    {
      R[i] = (*this)[Q[i]];
    }
    return Permutation{R};
  }

  Permutation &operator*=(Permutation const &Q)
  {
    return *this = *this * Q;
  }

  Permutation pow(int n)
  {
    if (n == 0)
    {
      return unit(size());
    }
    if (n & 1)
    {
      auto W{pow(n / 2)};
      return W * W;
    }
    return *this * pow(n - 1);
  }
};
// ----- Mat (matrix) and Vec -----
template <typename T = ll>
using Vec = vector<T>;
template <typename T = ll>
using Mat = Vec<Vec<T>>;
// operators of Vec / Mat
template <typename T = ll>
Vec<T> operator+(Vec<T> const &A)
{
  return A;
}
template <typename T = ll>
Vec<T> &operator+=(Vec<T> &A, Vec<T> const &B)
{
  assert(A.size() == B.size());
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    A[i] += B[i];
  }
  return A;
}
template <typename T = ll>
Vec<T> &operator+=(Vec<T> &A, T K)
{
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    A[i] += K;
  }
  return A;
}
template <typename T = ll>
Vec<T> operator+(Vec<T> const &A, Vec<T> const &B)
{
  auto C{A};
  C += B;
  return C;
}
template <typename T = ll>
Vec<T> operator+(Vec<T> const &A, T K)
{
  auto C{A};
  C += K;
  return C;
}
template <typename T = ll>
Vec<T> operator+(T K, Vec<T> const &A)
{
  return A + K;
}
template <typename T = ll>
Vec<T> operator-(Vec<T> const &A)
{
  Vec<T> C(A.size());
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    C[i] = -A[i];
  }
  return C;
}
template <typename T = ll>
Vec<T> &operator-=(Vec<T> &A, Vec<T> const &B)
{
  return A += (-B);
}
template <typename T = ll>
Vec<T> &operator-=(Vec<T> &A, T K)
{
  return A += (-K);
}
template <typename T = ll>
Vec<T> operator-(Vec<T> const &A, Vec<T> const &B)
{
  return A + (-B);
}
template <typename T = ll>
Vec<T> operator-(Vec<T> const &A, T K)
{
  return A + (-K);
}
template <typename T = ll>
Vec<T> operator-(T K, Vec<T> const &A)
{
  return K + (-A);
}
template <typename T = ll>
Vec<T> &operator*=(Vec<T> &A, T K)
{
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    A[i] *= K;
  }
  return A;
}
template <typename T = ll>
Vec<T> operator*(T K, Vec<T> const &A)
{
  auto C{A};
  C *= K;
  return C;
}
template <typename T = ll>
Vec<T> operator*(Vec<T> const &A, T K)
{
  return K * A;
}
template <typename T = ll>
Vec<T> &operator/=(Vec<T> &A, T K)
{
  return A *= (1 / K);
}
template <typename T = ll>
Vec<T> operator/(Vec<T> const &A, T K)
{
  return A * (1 / K);
}
template <typename T = ll>
Vec<T> &operator++(Vec<T> &A)
{
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    ++A[i];
  }
  return A;
}
template <typename T = ll>
Vec<T> operator++(Vec<T> &A, int)
{
  auto C{A};
  ++A;
  return C;
}
template <typename T = ll>
Vec<T> &operator--(Vec<T> &A)
{
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    --A[i];
  }
  return A;
}
template <typename T = ll>
Vec<T> operator--(Vec<T> &A, int)
{
  auto C{A};
  --A;
  return C;
}
// generator of unit matrix
// call example: Mat<T> A{unit_matrix<T>(s)};
template <typename T = ll>
Mat<T> UnitMatrix(size_t s)
{
  Mat<T> res(s, Vec<T>(s, T{0}));
  for (auto i = size_t{0}; i < s; ++i)
  {
    res[i][i] = T{1};
  }
  return res;
}
// multiply operators
template <typename T = ll>
Mat<T> operator*(Mat<T> const &A, Mat<T> const &B)
{
  assert(A.front().size() == B.size());
  Mat<T> C(A.size(), Vec<T>(B.front().size()));
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    for (auto j = size_t{0}; j < B.front().size(); j++)
    {
      for (auto k = size_t{0}; k < A.front().size(); k++)
      {
        C[i][j] += A[i][k] * B[k][j];
      }
    }
  }
  return C;
}
template <typename T = ll>
Mat<T> &operator*=(Mat<T> &A, Mat<T> const &B)
{
  auto C{A * B};
  return A = C;
}
template <typename T = ll>
Vec<T> operator*(Mat<T> const &A, Vec<T> const &V)
{
  assert(A.front().size() == V.size());
  Vec<T> W(A.size());
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    for (auto j = size_t{0}; j < A.front().size(); j++)
    {
      W[i] += A[i][j] * V[j];
    }
  }
  return W;
}
template <typename T = ll>
Mat<T> Power(Mat<T> const &A, ll n)
{
  assert(A.size() == A.front().size());
  if (n == 0)
  {
    return UnitMatrix<T>(A.size());
  }
  if (n & 1)
  {
    return A * pow(A, n - 1);
  }
  auto B{pow(A, n / 2)};
  return B * B;
}
// Transpose
template <typename T = ll>
Mat<T> Transpose(Mat<T> const &A)
{
  Mat<T> C(A.front().size(), Vec<T>(A.size()));
  for (auto i = size_t{0}; i < A.size(); i++)
  {
    for (auto j = size_t{0}; j < A.front().size(); j++)
    {
      C[j][i] = A[i][j];
    }
  }
  return C;
}
/***************************************************
    My Library / Numbers
 ***************************************************/
// ----- Sieve -----
class Sieve
{
  static constexpr ll MAX_SIZE{1000010LL};
  ll N;
  vector<ll> f;
  vector<ll> prime_nums;

public:
  Sieve(ll N = MAX_SIZE) : N{N}, f(N, 0), prime_nums{}
  {
    f[0] = f[1] = -1;
    for (auto i = 2; i < N; i++)
    {
      if (f[i])
      {
        continue;
      }
      prime_nums.push_back(i);
      f[i] = i;
      for (auto j = 2 * i; j < N; j += i)
      {
        if (!f[j])
        {
          f[j] = i;
        }
      }
    }
  }

  bool is_prime(ll x) const
  { // 2 \leq x \leq MAX_SIZE^2
    if (x < N)
    {
      return f[x] == x;
    }
    for (auto e : prime_nums)
    {
      if (x % e == 0)
      {
        return false;
      }
    }
    return true;
  }

  vector<ll> const &primes() const
  {
    return prime_nums;
  }

  vector<ll> factor_list(ll x) const
  {
    if (x < 2)
    {
      return {};
    }
    vector<ll> res;
    auto it{prime_nums.begin()};
    if (x < N)
    {
      while (x != 1)
      {
        res.push_back(f[x]);
        x /= f[x];
      }
    }
    else
    {
      while (x != 1 && it != prime_nums.end())
      {
        if (x % *it == 0)
        {
          res.push_back(*it);
          x /= *it;
        }
        else
        {
          ++it;
        }
      }
      if (x != 1)
      {
        res.push_back(x);
      }
    }
    return res;
  }

  vector<tuple<ll, ll>> factor(ll x) const
  {
    if (x < 2)
    {
      return {};
    }
    auto factors{factor_list(x)};
    vector<tuple<ll, ll>> res{make_tuple(factors[0], 0)};
    for (auto x : factors)
    {
      if (x == get<0>(res.back()))
      {
        get<1>(res.back())++;
      }
      else
      {
        res.emplace_back(x, 1);
      }
    }
    return res;
  }
};
// ----- EulerNums -----
class EulerNums
{
  static constexpr ll MAX_SIZE{1000010LL};
  ll N;
  vector<ll> table;

public:
  EulerNums(ll N = MAX_SIZE) : N{N}
  {
    table.resize(N);
    for (auto i = 0; i < N; i++)
    {
      table[i] = i;
    }
    for (auto i = 2; i < N; i++)
    {
      if (table[i] == i)
      {
        for (auto j = i; j < N; j += i)
        {
          table[j] = table[j] / i * (i - 1);
        }
      }
    }
  }

  ll euler(ll n)
  {
    if (n < N)
    {
      return table[N];
    }
    else
    {
      ll res{n};
      for (auto i = 2LL; i * i <= n; i++)
      {
        if (n % i == 0)
        {
          res = res / i * (i - 1);
        }
        for (; n % i == 0; n /= i)
        {
        }
      }
      if (n != 1)
      {
        res = res / n * (n - 1);
      }
      return res;
    }
  }
};
class LinearMod
{
  vector<ll> A, B, M;

public:
  LinearMod() {}

  void add(ll a, ll b, ll m)
  {
    A.push_back(a);
    B.push_back(b);
    M.push_back(m);
  }

  tuple<ll, ll> get()
  {
    assert(A.size() == B.size() && B.size() == M.size());
    ll x{0}, m{1};
    for (auto i = size_t{0}; i < A.size(); i++)
    {
      ll a{A[i] * m};
      ll b{B[i] - A[i] * x};
      ll d{gcd(M[i], a)};
      if (b % d != 0)
      {
        return make_tuple(0, -1);
      }
      ll t{b / d * mod_inverse(a / d, M[i] / d) % (M[i] / d)};
      x = x + m * t;
      m *= M[i] / d;
    }
    return make_tuple(x % m, m);
  }

private:
  static ll extra_gcd(ll a, ll b, ll &x, ll &y)
  {
    ll d{a};
    if (b != 0)
    {
      d = extra_gcd(b, a % b, y, x);
      y -= (a / b) * x;
    }
    else
    {
      x = 1;
      y = 0;
    }
    return d;
  }

  static ll mod_inverse(ll a, ll m)
  {
    ll x, y;
    extra_gcd(a, m, x, y);
    return (m + x % m) % m;
  }
};
/***************************************************
    My Library / Tools
 ***************************************************/
// ----- Compressor -----
template <typename T = ll>
class Compressor
{
  vector<T> raw;
  map<T, int> index;

public:
  Compressor() {}

  template <typename Container>
  Compressor(Container const &V) { append(V); }

  template <typename Iter>
  Compressor(Iter first, Iter last) { append(first, last); }

  template <typename Container>
  void append(Container const &V) { append(V.begin(), V.end()); }

  template <typename Iter>
  void append(Iter first, Iter last)
  {
    set<T> S(first, last);
    raw = vector<T>(S.begin(), S.end());
    sort(raw.begin(), raw.end());
    for (auto i = size_t{0}; i < raw.size(); ++i)
    {
      index[raw[i]] = i;
    }
  }

  T to_raw(int i) { return raw[i]; }
  int to_index(T t) { return index[t]; }
};
// ----- RunLengthCompress -----
template <typename T>
auto RunLengthCompress(T const &S) -> vector<tuple<remove_const_t<remove_reference_t<decltype(S[0])>>, int>>
{
  using U = remove_const_t<remove_reference_t<decltype(S[0])>>;
  vector<tuple<U, int>> res;
  U c{S[0]};
  int x{0};
  for (auto e : S)
  {
    if (c == e)
    {
      ++x;
    }
    else
    {
      res.emplace_back(c, x);
      c = e;
      x = 1;
    }
  }
  res.emplace_back(c, x);
  return res;
}
// ----- Make 2D, 3D, 4D Vectors -----
template <typename T>
vector<vector<T>> Make2DVector(size_t d0, size_t d1, T v = T{})
{
  return vector<vector<T>>(d0, vector<T>(d1, v));
}
template <typename T>
vector<vector<vector<T>>> Make3DVector(size_t d0, size_t d1, size_t d2, T v = T{})
{
  return vector<vector<vector<T>>>(d0, Make2DVector(d1, d2, v));
}
template <typename T>
vector<vector<vector<vector<T>>>> Make4DVector(size_t d0, size_t d1, size_t d2, size_t d3, T v = T{})
{
  return vector<vector<vector<vector<T>>>>(d0, Make3DVector(d1, d2, d3, v));
}
// ----- vector istream and ostream -----
template <typename T>
istream &operator>>(istream &is, vector<T> &v)
{
  for (auto it{v.begin()}; it != v.end(); ++it)
  {
    is >> *it;
  }
  return is;
}
template <typename T>
istream &operator>>(istream &is, vector<vector<T> *> &&v)
{
  for (auto it{v.begin()}; it + 1 != v.end(); ++it)
  {
    assert((*it)->size() == (*(it + 1))->size());
  }
  auto s{(*v.begin())->size()};
  for (auto i{size_t{0}}; i < s; ++i)
  {
    for (auto it{v.begin()}; it != v.end(); ++it)
    {
      cin >> (**it)[i];
    }
  }
  return is;
}
template <typename T, typename U>
ostream &FlushVector(vector<T> const &v, U const &s = "\n", ostream &os = cout)
{
  for (auto it{v.begin()}; it != v.end(); ++it)
  {
    os << *it;
    if (it + 1 != v.end())
    {
      os << s;
    }
  }
  return os;
}
template <typename T>
ostream &operator<<(ostream &os, vector<T> const &v)
{
  return FlushVector(v, "\n", os);
}
/***************************************************
    Frequently Used Structures
 ***************************************************/
// ----- Constants -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
/***************************************************
    Solutions
 ***************************************************/

// ----- Solve -----

class Solve
{
  ll N, K, M;
  Combination<mint> C;

public:
  Solve()
  {
    cin >> N >> K;
    M = 2 * N + 1;
  }

  void flush()
  {
    auto p{prob()};
    cout << 1 - p / 2 << endl;
  }

private:
  mint prob()
  {
    mint ans{0};
    for (auto a{0}; M - 2 * a - 1 > 0; ++a)
    {
      for (auto b{0}; b <= M; ++b)
      {
        if (b % 2 != 0)
        {
          continue;
        }
        ll one{(M - 2 * a - 1 - b) / 2};
        if (one <= 0)
        {
          break;
        }
        mint m_one{one};
        auto p{m_one / (m_one + b) * (2 * a + 1) / (2 * a + 1 + b + 1)};
        auto q{C(one + b, K)};
        auto c{C(M, one) - C(M, one - 1)};
#if DEBUG == 1
        if (N == 1)
        {
          cerr << "a = " << a << ", b = " << b << endl;
          cerr << "p = " << p << ", q = " << q << ", c = " << c << endl;
        }
#endif
        ans += p * q * c;
      }
    }
    for (auto a{0}; M - 2 * a > 0; ++a)
    {
      for (auto b{0}; b <= M; ++b)
      {
        if (b % 2 != 1)
        {
          continue;
        }
        ll one{(M - 2 * a - b) / 2};
        if (one <= 0)
        {
          break;
        }
        mint m_one{one};
        auto p{m_one / (m_one + b) * (2 * a + 1) / (2 * a + b + 1)};
        auto q{C(one + b, K)};
        auto c{C(M, one) - C(M, one - 1)};
#if DEBUG == 1
        if (N == 1)
        {
          cerr << "a = " << a << ", b = " << b << endl;
          cerr << "p = " << p << ", q = " << q << ", c = " << c << endl;
        }
#endif
        ans += p * q * c;
      }
    }
    return ans / C(M, K) / mint{2}.pow(M - K);
  }
};

// ----- main() -----

int main()
{
  Solve solve;
  solve.flush();
}


// Copyright (c) 2020 Chris Richardson & Matthew Scroggs
// FEniCS Project
// SPDX-License-Identifier:    MIT

#include "nedelec.h"
#include "dof-permutations.h"
#include "lagrange.h"
#include "moments.h"
#include "polyset.h"
#include "quadrature.h"
#include "raviart-thomas.h"
#include <Eigen/Dense>
#include <numeric>
#include <vector>

using namespace libtab;

namespace
{
//-----------------------------------------------------------------------------
Eigen::MatrixXd create_nedelec_2d_space(int degree)
{
  // Number of order (degree) vector polynomials
  const int nv = degree * (degree + 1) / 2;

  // Number of order (degree-1) vector polynomials
  const int ns0 = (degree - 1) * degree / 2;

  // Number of additional polynomials in Nedelec set
  const int ns = degree;

  // Tabulate polynomial set at quadrature points
  auto [Qpts, Qwts]
      = quadrature::make_quadrature(cell::type::triangle, 2 * degree);
  Eigen::ArrayXXd Pkp1_at_Qpts
      = polyset::tabulate(cell::type::triangle, degree, 0, Qpts)[0];

  const int psize = Pkp1_at_Qpts.cols();

  // Create coefficients for order (degree-1) vector polynomials
  Eigen::MatrixXd wcoeffs = Eigen::MatrixXd::Zero(nv * 2 + ns, psize * 2);
  wcoeffs.block(0, 0, nv, nv) = Eigen::MatrixXd::Identity(nv, nv);
  wcoeffs.block(nv, psize, nv, nv) = Eigen::MatrixXd::Identity(nv, nv);

  // Create coefficients for the additional Nedelec polynomials
  for (int i = 0; i < ns; ++i)
  {
    for (int k = 0; k < psize; ++k)
    {
      wcoeffs(2 * nv + i, k) = (Qwts * Pkp1_at_Qpts.col(ns0 + i) * Qpts.col(1)
                                * Pkp1_at_Qpts.col(k))
                                   .sum();
      wcoeffs(2 * nv + i, k + psize) = (-Qwts * Pkp1_at_Qpts.col(ns0 + i)
                                        * Qpts.col(0) * Pkp1_at_Qpts.col(k))
                                           .sum();
    }
  }

  return wcoeffs;
}
//-----------------------------------------------------------------------------
Eigen::MatrixXd create_nedelec_2d_dual(int degree)
{
  // Number of dofs and size of polynomial set P(k+1)
  const int ndofs = 3 * degree + degree * (degree - 1);
  const int psize = (degree + 1) * (degree + 2) / 2;

  // Dual space
  Eigen::MatrixXd dual = Eigen::MatrixXd::Zero(ndofs, psize * 2);

  // dof counter
  const int quad_deg = 5 * degree;

  // Integral representation for the boundary (edge) dofs
  dual.block(0, 0, 3 * degree, psize * 2)
      = moments::make_tangent_integral_moments(
          create_dlagrange(cell::type::interval, degree - 1),
          cell::type::triangle, 2, degree, quad_deg);

  if (degree > 1)
  {
    // Interior integral moment
    dual.block(3 * degree, 0, degree * (degree - 1), psize * 2)
        = moments::make_integral_moments(
            create_dlagrange(cell::type::triangle, degree - 2),
            cell::type::triangle, 2, degree, quad_deg);
  }

  return dual;
}
//-----------------------------------------------------------------------------
std::vector<Eigen::MatrixXd> create_nedelec_2d_base_perms(int degree)
{
  const int ndofs = degree * (degree + 2);
  std::vector<Eigen::MatrixXd> base_permutations(
      3, Eigen::MatrixXd::Identity(ndofs, ndofs));

  Eigen::ArrayXi edge_ref = dofperms::interval_reflection(degree);
  for (int edge = 0; edge < 3; ++edge)
  {
    const int start = edge_ref.size() * edge;
    for (int i = 0; i < edge_ref.size(); ++i)
    {
      base_permutations[edge](start + i, start + i) = 0;
      base_permutations[edge](start + i, start + edge_ref[i]) = 1;
    }
  }

  Eigen::ArrayXXd edge_dir
      = dofperms::interval_reflection_tangent_directions(degree);
  for (int edge = 0; edge < 3; ++edge)
  {
    Eigen::MatrixXd directions = Eigen::MatrixXd::Identity(ndofs, ndofs);
    directions.block(edge_dir.rows() * edge, edge_dir.cols() * edge,
                     edge_dir.rows(), edge_dir.cols())
        = edge_dir;
    base_permutations[edge] *= directions;
  }

  return base_permutations;
}
//-----------------------------------------------------------------------------
Eigen::MatrixXd create_nedelec_3d_space(int degree)
{
  // Reference tetrahedron
  const int tdim = 3;

  // Number of order (degree) vector polynomials
  const int nv = degree * (degree + 1) * (degree + 2) / 6;

  // Number of order (degree-1) vector polynomials
  const int ns0 = (degree - 1) * degree * (degree + 1) / 6;
  // Number of additional Nedelec polynomials that could be added
  const int ns = degree * (degree + 1) / 2;
  // Number of polynomials that would be included that are not independent so
  // are removed
  const int ns_remove = degree * (degree - 1) / 2;

  // Number of dofs in the space, ie size of polynomial set
  const int ndofs = 6 * degree + 4 * degree * (degree - 1)
                    + (degree - 2) * (degree - 1) * degree / 2;

  // Tabulate polynomial basis at quadrature points
  auto [Qpts, Qwts]
      = quadrature::make_quadrature(cell::type::tetrahedron, 2 * degree);
  Eigen::ArrayXXd Pkp1_at_Qpts
      = polyset::tabulate(cell::type::tetrahedron, degree, 0, Qpts)[0];
  const int psize = Pkp1_at_Qpts.cols();

  // Create coefficients for order (degree-1) polynomials
  Eigen::MatrixXd wcoeffs = Eigen::MatrixXd::Zero(ndofs, psize * tdim);
  for (int i = 0; i < tdim; ++i)
  {
    wcoeffs.block(nv * i, psize * i, nv, nv)
        = Eigen::MatrixXd::Identity(nv, nv);
  }

  // Create coefficients for additional Nedelec polynomials
  for (int i = 0; i < ns; ++i)
  {
    for (int k = 0; k < psize; ++k)
    {
      const double w = (Qwts * Pkp1_at_Qpts.col(ns0 + i) * Qpts.col(2)
                        * Pkp1_at_Qpts.col(k))
                           .sum();
      // Don't include polynomials (*, *, 0) that are dependant
      if (i >= ns_remove)
        wcoeffs(tdim * nv + i - ns_remove, psize + k) = -w;
      wcoeffs(tdim * nv + i + ns - ns_remove, k) = w;
    }
  }

  for (int i = 0; i < ns; ++i)
  {
    for (int k = 0; k < psize; ++k)
    {
      const double w = (Qwts * Pkp1_at_Qpts.col(ns0 + i) * Qpts.col(1)
                        * Pkp1_at_Qpts.col(k))
                           .sum();
      wcoeffs(tdim * nv + i + ns * 2 - ns_remove, k) = -w;
      // Don't include polynomials (*, *, 0) that are dependant
      if (i >= ns_remove)
        wcoeffs(tdim * nv + i - ns_remove, psize * 2 + k) = w;
    }
  }

  for (int i = 0; i < ns; ++i)
  {
    for (int k = 0; k < psize; ++k)
    {
      const double w = (Qwts * Pkp1_at_Qpts.col(ns0 + i) * Qpts.col(0)
                        * Pkp1_at_Qpts.col(k))
                           .sum();
      wcoeffs(tdim * nv + i + ns - ns_remove, psize * 2 + k) = -w;
      wcoeffs(tdim * nv + i + ns * 2 - ns_remove, psize + k) = w;
    }
  }

  return wcoeffs;
}
//-----------------------------------------------------------------------------
Eigen::MatrixXd create_nedelec_3d_dual(int degree)
{
  const int tdim = 3;

  // Size of polynomial set P(k+1)
  const int psize = (degree + 1) * (degree + 2) * (degree + 3) / 6;

  // Work out number of dofs
  const int ndofs = 6 * degree + 4 * degree * (degree - 1)
                    + (degree - 2) * (degree - 1) * degree / 2;
  Eigen::MatrixXd dual = Eigen::MatrixXd::Zero(ndofs, psize * tdim);

  // Create quadrature scheme on the edge
  const int quad_deg = 5 * degree;

  // Integral representation for the boundary (edge) dofs
  dual.block(0, 0, 6 * degree, psize * 3)
      = moments::make_tangent_integral_moments(
          create_dlagrange(cell::type::interval, degree - 1),
          cell::type::tetrahedron, 3, degree, quad_deg);

  if (degree > 1)
  {
    // Integral moments on faces
    dual.block(6 * degree, 0, 4 * (degree - 1) * degree, psize * 3)
        = moments::make_integral_moments(
            create_dlagrange(cell::type::triangle, degree - 2),
            cell::type::tetrahedron, 3, degree, quad_deg);
  }

  if (degree > 2)
  {
    // Interior integral moment
    dual.block(6 * degree + 4 * degree * (degree - 1), 0,
               (degree - 2) * (degree - 1) * degree / 2, psize * 3)
        = moments::make_integral_moments(
            create_dlagrange(cell::type::tetrahedron, degree - 3),
            cell::type::tetrahedron, 3, degree, quad_deg);
  }

  return dual;
}
//-----------------------------------------------------------------------------
std::vector<Eigen::MatrixXd> create_nedelec_3d_base_perms(int degree)
{
  const int ndofs = 6 * degree + 4 * degree * (degree - 1)
                    + (degree - 2) * (degree - 1) * degree / 2;
  std::vector<Eigen::MatrixXd> base_permutations(
      14, Eigen::MatrixXd::Identity(ndofs, ndofs));

  Eigen::ArrayXi edge_ref = dofperms::interval_reflection(degree);
  for (int edge = 0; edge < 6; ++edge)
  {
    const int start = edge_ref.size() * edge;
    for (int i = 0; i < edge_ref.size(); ++i)
    {
      base_permutations[edge](start + i, start + i) = 0;
      base_permutations[edge](start + i, start + edge_ref[i]) = 1;
    }
  }
  Eigen::ArrayXi face_rot = dofperms::triangle_rotation(degree - 1);
  Eigen::ArrayXi face_ref = dofperms::triangle_reflection(degree - 1);
  for (int face = 0; face < 4; ++face)
  {
    const int start = edge_ref.size() * 6 + face_ref.size() * 2 * face;
    for (int i = 0; i < face_rot.size(); ++i)
    {
      for (int b = 0; b < 2; ++b)
      {
        const int p = 6 + 2 * face;
        const int p1 = start + 2 * i + b;
        base_permutations[p](p1, start + i * 2 + b) = 0;
        base_permutations[p](p1, start + face_rot[i] * 2 + b) = 1;
        base_permutations[p + 1](p1, start + i * 2 + b) = 0;
        base_permutations[p + 1](p1, start + face_ref[i] * 2 + b) = 1;
      }
    }
  }

  Eigen::ArrayXXd edge_dir
      = dofperms::interval_reflection_tangent_directions(degree);
  for (int edge = 0; edge < 6; ++edge)
  {
    Eigen::MatrixXd directions = Eigen::MatrixXd::Identity(ndofs, ndofs);
    directions.block(edge_dir.rows() * edge, edge_dir.cols() * edge,
                     edge_dir.rows(), edge_dir.cols())
        = edge_dir;
    base_permutations[edge] *= directions;
  }

  // Faces
  Eigen::ArrayXXd face_dir_ref
      = dofperms::triangle_reflection_tangent_directions(degree - 1);
  Eigen::ArrayXXd face_dir_rot
      = dofperms::triangle_rotation_tangent_directions(degree - 1);
  for (int face = 0; face < 4; ++face)
  {
    // Rotate face
    Eigen::MatrixXd rotation = Eigen::MatrixXd::Identity(ndofs, ndofs);
    rotation.block(edge_dir.rows() * 6 + face_dir_rot.rows() * face,
                   edge_dir.cols() * 6 + face_dir_rot.rows() * face,
                   face_dir_rot.rows(), face_dir_rot.cols())
        = face_dir_rot;
    base_permutations[6 + 2 * face] *= rotation;

    // Reflect face
    Eigen::MatrixXd reflection = Eigen::MatrixXd::Identity(ndofs, ndofs);
    reflection.block(edge_dir.rows() * 6 + face_dir_ref.rows() * face,
                     edge_dir.cols() * 6 + face_dir_ref.rows() * face,
                     face_dir_ref.rows(), face_dir_ref.cols())
        = face_dir_ref;
    base_permutations[6 + 2 * face + 1] *= reflection;
  }

  return base_permutations;
}

//-----------------------------------------------------------------------------
Eigen::MatrixXd create_nedelec2_2d_dual(int degree)
{
  // Number of dofs and size of polynomial set P(k+1)
  const int ndofs = (degree + 1) * (degree + 2);
  const int psize = (degree + 1) * (degree + 2) / 2;

  // Dual space
  Eigen::MatrixXd dual = Eigen::MatrixXd::Zero(ndofs, psize * 2);

  // dof counter
  int quad_deg = 5 * degree;

  // Integral representation for the boundary (edge) dofs
  dual.block(0, 0, 3 * (degree + 1), psize * 2)
      = moments::make_tangent_integral_moments(
          create_dlagrange(cell::type::interval, degree), cell::type::triangle,
          2, degree, quad_deg);

  if (degree > 1)
  {
    // Interior integral moment
    dual.block(3 * (degree + 1), 0, (degree - 1) * (degree + 1), psize * 2)
        = moments::make_dot_integral_moments(
            create_rt(cell::type::triangle, degree - 1), cell::type::triangle,
            2, degree, quad_deg);
  }

  return dual;
}
//-----------------------------------------------------------------------------
Eigen::MatrixXd create_nedelec2_3d_dual(int degree)
{
  const int tdim = 3;

  // Size of polynomial set P(k+1)
  const int psize = (degree + 1) * (degree + 2) * (degree + 3) / 6;

  // Work out number of dofs
  const int ndofs = (degree + 1) * (degree + 2) * (degree + 3) / 2;

  Eigen::MatrixXd dual = Eigen::MatrixXd::Zero(ndofs, psize * tdim);

  // Create quadrature scheme on the edge
  int quad_deg = 5 * degree;

  // Integral representation for the boundary (edge) dofs
  dual.block(0, 0, 6 * (degree + 1), psize * 3)
      = moments::make_tangent_integral_moments(
          create_dlagrange(cell::type::interval, degree),
          cell::type::tetrahedron, 3, degree, quad_deg);

  if (degree > 1)
  {
    // Integral moments on faces
    dual.block(6 * (degree + 1), 0, 4 * (degree - 1) * (degree + 1), psize * 3)
        = moments::make_dot_integral_moments(
            create_rt(cell::type::triangle, degree - 1),
            cell::type::tetrahedron, 3, degree, quad_deg);
  }

  if (degree > 2)
  {
    // Interior integral moment
    dual.block((6 + 4 * (degree - 1)) * (degree + 1), 0,
               (degree - 1) * (degree - 2) * (degree + 1) / 2, psize * 3)
        = moments::make_integral_moments(
            create_dlagrange(cell::type::tetrahedron, degree - 2),
            cell::type::tetrahedron, 3, degree, quad_deg);
  }

  return dual;
}

} // namespace

//-----------------------------------------------------------------------------
FiniteElement libtab::create_nedelec(cell::type celltype, int degree,
                                     const std::string& name)
{
  Eigen::MatrixXd wcoeffs;
  Eigen::MatrixXd dual;
  std::vector<Eigen::MatrixXd> perms;
  std::vector<Eigen::MatrixXd> directions;
  if (celltype == cell::type::triangle)
  {
    wcoeffs = create_nedelec_2d_space(degree);
    dual = create_nedelec_2d_dual(degree);
    perms = create_nedelec_2d_base_perms(degree);
  }
  else if (celltype == cell::type::tetrahedron)
  {
    wcoeffs = create_nedelec_3d_space(degree);
    dual = create_nedelec_3d_dual(degree);
    perms = create_nedelec_3d_base_perms(degree);
  }
  else
    throw std::runtime_error("Invalid celltype in Nedelec");

  // Nedelec has d dofs on each edge, d(d-1) on each face
  // and d(d-1)(d-2)/2 on the interior in 3D
  const std::vector<std::vector<std::vector<int>>> topology
      = cell::topology(celltype);
  std::vector<std::vector<int>> entity_dofs(topology.size());
  entity_dofs[0].resize(topology[0].size(), 0);
  entity_dofs[1].resize(topology[1].size(), degree);
  entity_dofs[2].resize(topology[2].size(), degree * (degree - 1));
  const int tdim = cell::topological_dimension(celltype);
  if (tdim > 2)
    entity_dofs[3] = {degree * (degree - 1) * (degree - 2) / 2};

  const Eigen::MatrixXd coeffs = compute_expansion_coefficients(wcoeffs, dual);
  return FiniteElement(name, celltype, degree, {tdim}, coeffs, entity_dofs,
                       perms);
}
//-----------------------------------------------------------------------------
FiniteElement libtab::create_nedelec2(cell::type celltype, int degree,
                                      const std::string& name)
{
  const int tdim = cell::topological_dimension(celltype);
  const int psize = polyset::dim(celltype, degree);
  Eigen::MatrixXd wcoeffs
      = Eigen::MatrixXd::Identity(tdim * psize, tdim * psize);

  Eigen::MatrixXd dual;
  if (celltype == cell::type::triangle)
    dual = create_nedelec2_2d_dual(degree);
  else if (celltype == cell::type::tetrahedron)
    dual = create_nedelec2_3d_dual(degree);
  else
    throw std::runtime_error("Invalid celltype in Nedelec");

  // TODO: Implement base permutations
  const std::vector<std::vector<std::vector<int>>> topology
      = cell::topology(celltype);
  const int ndofs = dual.rows();
  int perm_count = 0;
  for (int i = 1; i < tdim; ++i)
    perm_count += topology[i].size() * i;
  std::vector<Eigen::MatrixXd> base_permutations(
      perm_count, Eigen::MatrixXd::Identity(ndofs, ndofs));

  const Eigen::MatrixXd coeffs = compute_expansion_coefficients(wcoeffs, dual);

  // Nedelec(2nd kind) has (d+1) dofs on each edge, (d+1)(d-1) on each face
  // and (d-2)(d-1)(d+1)/2 on the interior in 3D
  std::vector<std::vector<int>> entity_dofs(topology.size());
  entity_dofs[0].resize(topology[0].size(), 0);
  entity_dofs[1].resize(topology[1].size(), degree + 1);
  entity_dofs[2].resize(topology[2].size(), (degree + 1) * (degree - 1));
  if (tdim > 2)
    entity_dofs[3] = {(degree - 2) * (degree - 1) * (degree + 1) / 2};

  return FiniteElement(name, celltype, degree, {tdim}, coeffs, entity_dofs,
                       base_permutations);
}
//-----------------------------------------------------------------------------


#pragma once

#include <boost/multiprecision/gmp.hpp>
#include <math.h>
#include <time.h>

#include "FiniteFields.hpp"
#include "FastFourierTransform.hpp"

namespace ligero {
template <typename FieldT>
class 		SecretSharing {
	public:
		/* Constructors allowing some flexibility in the choice of domains */
		SecretSharing(size_t lSecretLength, size_t kDegree, size_t nShares) : lSecretLength_(lSecretLength), kDegree_(kDegree), nNumberShares_(nShares) {
			// In this implementation, we use an evaluation domain that is twice the size of the number of shares, and we discard
			this->dCompositeDomainSize_ = nShares;
		};

		int share(FieldT *secret);
		int reconstruct(FieldT *eval, bool expanded = false);

		int padMany(FieldT *secrets, size_t lrows);
		int padIntrablocRandomnessMany(FieldT *secret, size_t lrows);
		int shareMany(FieldT *secrets, size_t lrows);
		int reconstructMany(FieldT *secrets, size_t lrows);

		bool degreeTest(FieldT *eval);
		bool zeroTest(FieldT *eval, bool expanded = false);
		bool zeroSumTest(FieldT *eval);

	protected:
		int padSecret(FieldT *secret);
		int padPolynominal(FieldT *secret);
		int padShares(FieldT *secret);
		int padIntrablocRandomness(FieldT *secret);

		size_t	lSecretLength_;
		size_t	kDegree_;
		size_t	nNumberShares_;
		size_t	dCompositeDomainSize_;
		std::vector<FieldT> rootsOfUnity_;
};

template <typename FieldT>
int SecretSharing<FieldT>::padSecret(FieldT *secret) {
	FieldT::randomVector(secret + this->lSecretLength_, this->kDegree_ - this->lSecretLength_, true);
	return 0;
}

template <typename FieldT>
int SecretSharing<FieldT>::padIntrablocRandomness(FieldT *secret) {
	for (size_t i = this->lSecretLength_; i < kDegree_ ;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
int SecretSharing<FieldT>::padPolynominal(FieldT *secret) {
	for (size_t i = this->kDegree_; i < nNumberShares_;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
int SecretSharing<FieldT>::padShares(FieldT *secret) {
	for (size_t i = this->nNumberShares_; i < dCompositeDomainSize_ ;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
int SecretSharing<FieldT>::padMany(FieldT *secret, size_t lrows) {

	// Add randomness for padding
	for (size_t i = 0; i < lrows; i++) {
		this->padSecret(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
int SecretSharing<FieldT>::padIntrablocRandomnessMany(FieldT *secret, size_t lrows) {

	// Add randomness for padding
	for (size_t i = 0; i < lrows; i++) {
		this->padIntrablocRandomness(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
int SecretSharing<FieldT>::share(FieldT *secret) {

	// Interpolate a polynomial of degree k for the secret
    iFFT<FieldT>(secret, kDegree_, rootsOfUnity_);

	// Evaluate the polynomial on a coset within the composite domain (size numberShares)
	this->padPolynominal(secret);
	FFT<FieldT>(secret, nNumberShares_, FieldT(FieldT::getOmega(nNumberShares_)), rootsOfUnity_);

	return 0;
}

template <typename FieldT>
inline int SecretSharing<FieldT>::reconstruct(FieldT *eval, bool expandedDegree) {
	size_t degree = kDegree_;
	iFFT<FieldT>(eval, nNumberShares_, FieldT(FieldT::getOmega(nNumberShares_)), rootsOfUnity_);

	if (expandedDegree) {
		for (size_t i = 0; i < kDegree_; i++) {eval[i] = eval[i] + eval[i + kDegree_];}
	}

    FFT<FieldT>(eval, degree, rootsOfUnity_);

	return 0;
}

template <typename FieldT>
bool SecretSharing<FieldT>::degreeTest(FieldT *eval) {

	std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
	iFFT<FieldT>(&localCopy[0], nNumberShares_, FieldT::getOmega(nNumberShares_), rootsOfUnity_);

	for (size_t i = kDegree_; i < nNumberShares_; i++) {
		if (!(localCopy[i] == FieldT(0))) {
			DBG("val:"<< localCopy[i].getValue() << ",degree>" << i);
			return false;
		}
	}

	return true;
}

template <typename FieldT>
bool SecretSharing<FieldT>::zeroTest(FieldT *eval, bool largerDegree) {

	std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
	reconstruct(&localCopy[0], largerDegree);

	for (size_t i = 0; i < this->lSecretLength_; i++) {
		if (!(localCopy[i] == FieldT(0))) return false;
	}

	return true;
}

template <typename FieldT>
bool SecretSharing<FieldT>::zeroSumTest(FieldT *eval) {

	std::vector<FieldT> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	FieldT sum = FieldT(0);
	for (size_t i = 0; i < this->lSecretLength_; i++) {
		sum += localCopy[i];
	}

	if (sum == FieldT(0)) return true;
	else return false;
}

template <typename FieldT>
int SecretSharing<FieldT>::shareMany(FieldT *secret, size_t lrows) {

	// std::cout << "Performing FFTs: " << lrows << std::endl;
	for (size_t i = 0; i < lrows; i++) {
		share(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
int SecretSharing<FieldT>::reconstructMany(FieldT *secret, size_t lrows) {

	for (size_t i = 0; i < lrows; i++) {
		reconstruct(secret + i * this->nNumberShares_);
	}

	return 0;
}

}