/*
Copyright 2014, 2015 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Define a Cartesian product of magmas.
*/

#ifndef MATH_PRODUCT_HPP_INCLUDED
#define MATH_PRODUCT_HPP_INCLUDED

#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>

#include <boost/utility/enable_if.hpp>
#include <boost/functional/hash_fwd.hpp>

#include "meta/vector.hpp"
#include "meta/all_of_c.hpp"

#include "utility/returns.hpp"
#include "utility/type_sequence_traits.hpp"

#include "rime/core.hpp"

#include "range/core.hpp"
#include "range/tuple.hpp"
#include "range/call_unpack.hpp"
#include "range/equal.hpp"
#include "range/less_lexicographical.hpp"
#include "range/transform.hpp"
#include "range/any_of.hpp"
#include "range/all_of.hpp"
#include "range/hash_range.hpp"

#include "magma.hpp"
#include "detail/tuple_helper.hpp"

namespace math {

template <class Operation = void> struct with_inverse;

/**
Magma that is the Cartesian product of a number of magmas.

If \a Inverse is with_inverse with an operation, then the inverse of this
operation is implemented.
In that case, the inverse cannot be defined on that component, and therefore
the inverse cannot be defined for the whole product.
If any component of this product is an annihilator for that operation, the whole
product must therefore be an annihilator.
Therefore, with <c>with_inverse \<Operation></c>, is_annihilator returns \c true
if any component returns \c true for is_annihilator.
Since any annihilator should be equal, \c equal returns \c true for two products
that have any annihilator for that operation.

To produce (as opposed to detect) an annihilator, each of the components must
have an annihilator for that operation.

\c order is not defined for the product: even if an operation implements a
strict weak ordering on each component, that does not implement any ordering on
the product.

Operations on this product are associative, commutative, idempotent,
distributive, and the product is a semiring, if and only if each of the
components is.

Products supports Boost.Hash, if \c boost/functional/hash.hpp is included.
If the hash values of the components of two products are the same, then the hash
value of the two products will be the same.

\todo This is a heterogeneous tuple. Is that good enough? I think so.
OpenFst has Power<W,n> and SparsePower<W>, which hold n or a variable number
of components.
This may be useful for unknown file formats, but we'd need an any_magma, and
the ability to live with a performance hit.

\tparam Components
    The list of components, given as math::over.
\tparam Inverse
    (optional)
    An inverse operation that is allowed, given as math::with_inverse.
*/
template <class Components, class Inverse = with_inverse<>> class product;

template <class ComponentTags, class Inverse> struct product_tag;

template <class ... Components, class Inverses>
    struct decayed_magma_tag <product <over <Components ...>, Inverses>>
{
    typedef product_tag <
            over <typename decayed_magma_tag <Components>::type ...>, Inverses>
        type;
};

template <class ... Components, class Inverse>
    class product <over <Components ...>, Inverse>
{
public:
    typedef meta::vector <Components ...> component_types;
    typedef range::tuple <Components ...> components_type;
    typedef Inverse inverse_specification;

    static_assert (meta::all_of_c <is_magma <Components>::value ...>::value,
        "Not all components passed to math::product are magmas.");

private:
    components_type components_;

    // Private type so that it cannot be constructed exept inside this class.
    struct dummy_type {};

public:
    // All elements explicitly convertible: implicit constructor.
    template <class ... Arguments, class Enable = typename boost::enable_if <
        utility::are_constructible <
            meta::vector <Components ...>, meta::vector <Arguments ...>>>::type>
    explicit product (Arguments && ... arguments)
    : components_ (std::forward <Arguments> (arguments) ...) {}

    product (product const &) = default;
    product (product &&) = default;

    /**
    Construct from a product with different component types, all of which are
    implicitly convertible to the component types of this.
    This constructor is implicit.
    \param other The product to copy.
    */
    template <class ... OtherComponents, class Enable = typename
        boost::enable_if <utility::are_convertible <
            meta::vector <OtherComponents const & ...>,
            meta::vector <Components ...>>
        >::type>
    product (product <over <OtherComponents ...>, Inverse> const & other)
    : components_ (other.components()) {}

    /**
    Construct from a product with different component types, at least one of
    which is explicitly convertible and not implicitly convertible.
    This constructor is explicit.
    \param other The product to copy.
    \internal \param dummy
        A dummy argument to distinguish this constructor from the implicit one.
    */
    template <class ... OtherComponents>
    explicit product (
        product <over <OtherComponents ...>, Inverse> const & other, typename
        boost::enable_if <
            tuple_helper::components_constructible_only <
                meta::vector <Components ...>,
                meta::vector <OtherComponents const & ...>>,
            dummy_type>::type = dummy_type())
    : components_ (other.components()) {}

    product & operator = (product const &) = default;
    product & operator = (product &&) = default;

    components_type & components() { return components_; }
    components_type const & components() const { return components_; }
};

namespace callable {

    template <class Inverses> struct make_product {
        template <class ... Components>
            product <over <Components ...>, Inverses>
            operator() (Components const & ... components) const
        { return product <over <Components ...>, Inverses> (components ...); }
    };

    template <class Inverses> struct make_product_over {
        template <class Components>
            auto operator() (Components && components) const
        RETURNS (range::call_unpack (
            make_product <Inverses>(), std::forward <Components> (components)));
    };

} // namespace callable

template <class Inverses, class ... Components>
    inline auto make_product (Components const & ... components)
RETURNS (callable::make_product <Inverses>() (components ...));

template <class Inverses, class Components>
    inline auto make_product_over (Components && components)
RETURNS (callable::make_product_over <Inverses>() (
    std::forward <Components> (components)));

namespace product_detail {

    template <class Type> struct is_product_tag : std::false_type {};
    template <class ComponentTags, class Inverses>
        struct is_product_tag <product_tag <ComponentTags, Inverses>>
    : std::true_type {};

} // namespace product_detail

MATH_MAGMA_GENERATE_OPERATORS (product_detail::is_product_tag)

namespace operation {

    /* Queries. */

    template <class Tags, class Inverses>
        struct is_member <product_tag <Tags, Inverses>>
    {
        template <class Product> auto operator() (Product const & product) const
        RETURNS (range::all_of (
            range::transform (product.components(), math::is_member)));
    };

    // is_annihilator.
    /*
    If the operation has an inverse: any component being an annihilator makes
    the whole product an annihilator.
    If not, then the default implementation (compare component-per-component
    with the result of annihilator()) works.
    */
    template <class ComponentTags, class Operation>
        struct is_annihilator <
            product_tag <ComponentTags, with_inverse <Operation>>, Operation>
    {
        template <class Product> auto operator() (Product const & product) const
        RETURNS (range::any_of (range::transform (
            product.components(), callable::is_annihilator <Operation>())));
    };

    // equal.
    // With no inverse: just compare components.
    template <class ComponentTags>
        struct equal <product_tag <ComponentTags, with_inverse<>>>
    : tuple_helper::equal_components <math::callable::equal> {};

    // With inverse: compare annihilators equal, otherwise compare components.
    template <class ComponentTags, class Operation>
        struct equal <product_tag <ComponentTags, with_inverse <Operation>>>
    : tuple_helper::equal_if_annihilator <Operation,
        tuple_helper::equal_components <math::callable::equal>> {};

    // approximately_equal.
    template <class ComponentTags>
        struct approximately_equal <product_tag <ComponentTags, with_inverse<>>>
    : tuple_helper::equal_components <math::callable::approximately_equal> {};

    template <class ComponentTags, class Operation>
        struct approximately_equal <
            product_tag <ComponentTags, with_inverse <Operation>>>
    : tuple_helper::equal_if_annihilator <Operation,
        tuple_helper::equal_components <math::callable::approximately_equal>>
    {};

    // compare.
    template <class ... ComponentTags>
        struct compare <product_tag <over <ComponentTags ...>, with_inverse<>>,
            typename boost::enable_if <meta::all_of_c <
                is_implemented <compare <ComponentTags>>::value ...>>::type>
    : tuple_helper::compare_components <math::callable::compare> {};

    // With inverse: annihilators go at the end.
    template <class ... ComponentTags, class Operation>
        struct compare <product_tag <over <ComponentTags ...>,
            with_inverse <Operation>>,
            typename boost::enable_if <boost::mpl::and_ <
                meta::all_of_c <
                    is_implemented <compare <ComponentTags>>::value ...>,
                // Only instantiate this if the Operation is not void.
                // GCC 4.6 requires this, or it won't realise the the
                // specialisation above is better.
                boost::mpl::not_ <std::is_same <Operation, void>>
            >>::type>
    : tuple_helper::compare_if_annihilator <Operation,
        tuple_helper::compare_components <math::callable::compare>> {};

    namespace tuple_helper {

        template <class ComponentTags, class Inverses>
            struct get_components <product_tag <ComponentTags, Inverses>>
        {
            template <class Product> auto operator() (Product const & p) const
            RETURNS (p.components());
        };

    } // namespace tuple_helper

    /* Produce. */

    // non_member could be implemented if all components implement it.
    // Otherwise, what value to pick for the other components?

    template <class ... Tags, class Inverses, class Operation>
        struct identity <product_tag <over <Tags ...>, Inverses>, Operation>
    : tuple_helper::nullary_operation <callable::make_product <Inverses>,
        meta::vector <identity <Tags, Operation> ...>> {};

    // annihilator: implemented if all components have an annihilator...
    // (otherwise, what value to pick for the other components?)
    template <class ... Tags, class Inverses, class Operation>
        struct annihilator <product_tag <over <Tags ...>, Inverses>, Operation,
        typename boost::enable_if <meta::all_of_c <
            is_implemented <annihilator <Tags, Operation>>::value ...
        >>::type>
    : tuple_helper::nullary_operation <callable::make_product <Inverses>,
        meta::vector <annihilator <Tags, Operation> ...>> {};

    // ... and there is at least one component.
    template <class Inverses, class Operation>
        struct annihilator <product_tag <over<>, Inverses>, Operation>
    : unimplemented {};

    /* Binary operations. */

    template <class ... Tags, class Inverses>
        struct times <product_tag <over <Tags ...>, Inverses>>
    : tuple_helper::binary_operation <callable::make_product <Inverses>,
        meta::vector <times <Tags> ...>> {};

    template <class ... Tags, class Inverses>
        struct plus <product_tag <over <Tags ...>, Inverses>>
    : tuple_helper::binary_operation <callable::make_product <Inverses>,
        meta::vector <plus <Tags> ...>> {};

    // is_semiring iff all components are semirings...
    template <class ... Tags, class Inverses,
            class Direction, class Operation1, class Operation2>
        struct is_semiring <product_tag <over <Tags ...>, Inverses>, Direction,
            Operation1, Operation2>
    : meta::all_of_c <
        is_semiring <Tags, Direction, Operation1, Operation2>::value ...> {};

    // ... except when the product is empty.
    template <class Inverses, class Direction,
        class Operation1, class Operation2>
    struct is_semiring <product_tag <over<>, Inverses>, Direction,
        Operation1, Operation2>
    : rime::false_type {};

    template <class ... Tags, class Inverses,
            class Direction, class Operation1, class Operation2>
        struct is_distributive <product_tag <over <Tags ...>, Inverses>,
            Direction, Operation1, Operation2>
    : meta::all_of_c <
        is_distributive <Tags, Direction, Operation1, Operation2>::value ...>
    {};

    template <class ... Tags,
            class ... Components1, class ... Components2, class Inverses>
        struct unify_type <product_tag <over <Tags ...>, Inverses>,
            product <over <Components1 ...>, Inverses>,
            product <over <Components2 ...>, Inverses>>
    {
        // Unify both underlying types separately.
        typedef product <over <
            typename unify_type <Tags, Components1, Components2>::type ...>,
            Inverses> type;
    };

    // divide: only if the template parameter Inverses is with_inverse <times>.
    template <class ... Tags, class Direction>
        struct divide <product_tag <over <Tags ...>,
            with_inverse <callable::times>>, Direction>
    : tuple_helper::binary_operation <
        callable::make_product <with_inverse <callable::times>>,
        meta::vector <divide <Tags, Direction> ...>> {};

    // minus: only if the template parameter Inverses is with_inverse <plus>.
    template <class ... Tags, class Direction>
        struct minus <product_tag <over <Tags ...>,
            with_inverse <callable::plus>>, Direction>
    : tuple_helper::binary_operation <
        callable::make_product <with_inverse <callable::plus>>,
        meta::vector <minus <Tags, Direction> ...>> {};

    template <class ... Tags, class Direction, class Operation>
        struct invert <product_tag <over <Tags ...>, with_inverse <Operation>>,
            Direction, Operation>
    : tuple_helper::unary_operation <
        callable::make_product <with_inverse <Operation>>,
        meta::vector <invert <Tags, Direction, Operation> ...>> {};

    template <class ... Tags, class Inverse, class Operation>
        struct reverse <product_tag <over <Tags ...>, Inverse>, Operation>
    : tuple_helper::unary_operation <
        callable::make_product <Inverse>,
        meta::vector <reverse <Tags, Operation> ...>> {};

    template <class ... Tags, class Inverses>
        struct print <product_tag <over <Tags ...>, Inverses>>
    : tuple_helper::print_components <meta::vector <Tags ...>> {};

} // namespace operation

// Boost.Hash support.

namespace product_detail {

    // Products with inverses need to treat annihilators specially.
    static std::size_t constexpr annihilator_hash =
        std::size_t (0xcba51c150183b7f1 & std::size_t (-1));

} // namespace product_detail

// Without an inverse: just combine the hash values of the components.
template <class Components>
    inline std::size_t hash_value (
        product <Components, with_inverse<>> const & p)
{ return range::hash_range (p.components()); }

// With an inverse: if p is an annihilator, then return a special hash value.
template <class Components, class Operation>
    inline std::size_t hash_value (
        product <Components, with_inverse <Operation>> const & p)
{
    if (is_annihilator <Operation> (p))
        return product_detail::annihilator_hash;
    return range::hash_range (p.components());
}

} // namespace math

#endif // MATH_PRODUCT_HPP_INCLUDED


#include <iostream>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>

using namespace std;
using namespace Eigen;

#ifndef SCALAR
#define SCALAR float
#endif

#ifndef SIZE
#define SIZE 8
#endif

typedef SCALAR Scalar;
typedef NumTraits<Scalar>::Real RealScalar;
typedef Matrix<RealScalar,Dynamic,Dynamic> A;
typedef Matrix</*Real*/Scalar,Dynamic,Dynamic> B;
typedef Matrix<Scalar,Dynamic,Dynamic> C;
typedef Matrix<RealScalar,Dynamic,Dynamic> M;

template<typename Transformation, typename Data>
EIGEN_DONT_INLINE void transform(const Transformation& t, Data& data)
{
  EIGEN_ASM_COMMENT("begin");
  data = t * data;
  EIGEN_ASM_COMMENT("end");
}

template<typename Scalar, typename Data>
EIGEN_DONT_INLINE void transform(const Quaternion<Scalar>& t, Data& data)
{
  EIGEN_ASM_COMMENT("begin quat");
  for(int i=0;i<data.cols();++i)
    data.col(i) = t * data.col(i);
  EIGEN_ASM_COMMENT("end quat");
}

template<typename T> struct ToRotationMatrixWrapper
{
  enum {Dim = T::Dim};
  typedef typename T::Scalar Scalar;
  ToRotationMatrixWrapper(const T& o) : object(o) {}
  T object;
};

template<typename QType, typename Data>
EIGEN_DONT_INLINE void transform(const ToRotationMatrixWrapper<QType>& t, Data& data)
{
  EIGEN_ASM_COMMENT("begin quat via mat");
  data = t.object.toRotationMatrix() * data;
  EIGEN_ASM_COMMENT("end quat via mat");
}

template<typename Scalar, int Dim, typename Data>
EIGEN_DONT_INLINE void transform(const Transform<Scalar,Dim,Projective>& t, Data& data)
{
  data = (t * data.colwise().homogeneous()).template block<Dim,Data::ColsAtCompileTime>(0,0);
}

template<typename T> struct get_dim { enum { Dim = T::Dim }; };
template<typename S, int R, int C, int O, int MR, int MC>
struct get_dim<Matrix<S,R,C,O,MR,MC> > { enum { Dim = R }; };

template<typename Transformation, int N>
struct bench_impl
{
  static EIGEN_DONT_INLINE void run(const Transformation& t)
  {
    Matrix<typename Transformation::Scalar,get_dim<Transformation>::Dim,N> data;
    data.setRandom();
    bench_impl<Transformation,N-1>::run(t);
    BenchTimer timer;
    BENCH(timer,10,100000,transform(t,data));
    cout.width(9);
    cout << timer.best() << " ";
  }
};

template<typename Transformation>
struct bench_impl<Transformation,0>
{
  static EIGEN_DONT_INLINE void run(const Transformation&) {}
};

template<typename Transformation>
EIGEN_DONT_INLINE void bench(const std::string& msg, const Transformation& t)
{
  cout << msg << " ";
  bench_impl<Transformation,SIZE>::run(t);
  std::cout << "\n";
}

int main(int argc, char ** argv)
{
  Matrix<Scalar,3,4> mat34; mat34.setRandom();
  Transform<Scalar,3,Isometry> iso3(mat34);
  Transform<Scalar,3,Affine> aff3(mat34);
  Transform<Scalar,3,AffineCompact> caff3(mat34);
  Transform<Scalar,3,Projective> proj3(mat34);
  Quaternion<Scalar> quat;quat.setIdentity();
  ToRotationMatrixWrapper<Quaternion<Scalar> > quatmat(quat);
  Matrix<Scalar,3,3> mat33; mat33.setRandom();

  cout.precision(4);
  std::cout
     << "N          ";
  for(int i=0;i<SIZE;++i)
  {
    cout.width(9);
    cout << i+1 << " ";
  }
  cout << "\n";

  bench("matrix 3x3", mat33);
  bench("quaternion", quat);
  bench("quat-mat  ", quatmat);
  bench("isometry3 ", iso3);
  bench("affine3   ", aff3);
  bench("c affine3 ", caff3);
  bench("proj3     ", proj3);
}


#include "CurveFitting.hpp"
#include <boost/algorithm/string.hpp>
#include <fstream>
#include <iostream>
#include <string>

int main(int argc, char** argv)
{
    try{
        if(argc!=2){ throw std::string("too few args."); }
        std::string filename(argv[1]);
        std::ifstream ifs(filename);
        if(!ifs.is_open()){ throw std::string("cannot open file")+filename; }

        std::vector<double> x;
        std::vector<double> y;
        std::string line;

        for(int i=0; std::getline(ifs,line); i++){
            std::vector<std::string> elements;
            boost::split(elements, line, boost::is_any_of(","));
            if(elements.size()==1){
                x.push_back(i);
                y.push_back(std::stod(elements[0]));
            }else if(elements.size()==2){
                x.push_back(std::stod(elements[0]));
                y.push_back(std::stod(elements[1]));
            }else{
                throw std::string("bad file format.");
            }
        }

        {
            std::cout << "---Line Fitting---" << std::endl;
            LineFitting line_fit(x,y);

            Eigen::VectorXd x_val(line_fit.num_param());
            x_val.setOnes();
            line_fit.solve(x_val);
            std::cout << x_val << std::endl;

            std::vector<double> vec_x(x_val.data(),x_val.data()+line_fit.num_param());
            std::cout << "residual(vec_x)=" << line_fit.residual(vec_x) << std::endl;
        }

        {
            std::cout << "---Polynomial Fitting---" << std::endl;
            PolynomialFitting poly_fit(x,y,3);

            std::vector<double> x_val(poly_fit.num_param(),1.0);
            poly_fit.solve(x_val);
            std::cout << Eigen::Map<Eigen::VectorXd>(x_val.data(),x_val.size()) << std::endl;

            std::cout << "residual(x_val)=" << poly_fit.residual(x_val) << std::endl;
        }

        {
            std::cout << "---Catenary Fitting---" << std::endl;
            CatenaryFitting catenary_fit(x,y);

            std::vector<double> x_val{3.5, 9.5, -20.0};
            catenary_fit.solve(x_val);
            std::cout << Eigen::Map<Eigen::VectorXd>(x_val.data(),x_val.size()) << std::endl;

            std::cout << "residual(x_val)=" << catenary_fit.residual(x_val) << std::endl;
        }
        {
            std::cout << "---Exponential Fitting---" << std::endl;
            ExponentialFitting exponential_fit(x,y);

            Eigen::VectorXd x_val(exponential_fit.num_param());
            x_val << 10.0, 2.0, 0.0, 0.0;
            exponential_fit.solve(x_val);
            std::cout << x_val << std::endl;

            std::vector<double> vec_x(x_val.data(),x_val.data()+exponential_fit.num_param());
            std::cout << "residual(vec_x)=" << exponential_fit.residual(vec_x) << std::endl;
        }
        {
            std::cout << "---InvCycloid Fitting---" << std::endl;
            InvCycloidFitting invCycloid_fit(y,x); /// Inv -> x y exchanging!

            Eigen::VectorXd x_val(invCycloid_fit.num_param());
            x_val << 3.0, 0.0, 0.0;
            invCycloid_fit.solve(x_val);
            std::cout << x_val << std::endl;

            std::vector<double> vec_x(x_val.data(),x_val.data()+invCycloid_fit.num_param());
            std::cout << "residual(vec_x)=" << invCycloid_fit.residual(vec_x) << std::endl;
        }
    }catch(std::string message){
        std::cout << message << std::endl;
        std::cout << "Usage: CurveFitting data_file.csv" << std::endl;
    }

}


#pragma once
#include "binomials.hpp"
#include <NTL/ZZ.h>
#include <NTL/RR.h>
#include <iomanip>

/***************************Classic ISDs***************************************/

double isd_log_cost_classic_BJMM_approx(const uint32_t n,
                                        const uint32_t k,
                                        const uint32_t t) {
    return ((double)t) * - log((1.0 - (double) k / (double) n)) / log(2);
}

// computes the probability of a random k * k being invertible
const NTL::RR log_probability_k_by_k_is_inv(const NTL::RR &k) {
    NTL::RR log_pinv = NTL::RR(0.5);
    for(long i = 2 ; i <=k ; i++){
        log_pinv = log_pinv * (NTL::RR(1) - NTL::power2_RR(-i));
    }
    return NTL::log(log_pinv);
}

const NTL::RR probability_k_by_k_is_inv(const NTL::RR &k) {
    NTL::RR log_pinv = NTL::RR(0.5);
    for(long i = 2 ; i <=k ; i++){
        log_pinv = log_pinv * (NTL::RR(1) - NTL::power2_RR(-i));
    }
    return log_pinv;
}

const NTL::RR classic_rref_red_cost(const NTL::RR &n, const NTL::RR & r){
    /* simple reduced row echelon form transform, as it is not likely to be the
     * bottleneck */
    NTL::RR k = n-r;
    return r*r*n/NTL::RR(2) +
           (n*r)/NTL::RR(2) -
           r*r*r / NTL::RR(6) +
           r*r +
           r / NTL::RR(6) - NTL::RR(1);
}

const NTL::RR classic_IS_candidate_cost(const NTL::RR &n, const NTL::RR & r){
    return classic_rref_red_cost(n,r)/probability_k_by_k_is_inv(r) + r*r;
}

const NTL::RR Fin_Send_rref_red_cost(const NTL::RR &n,
                                     const NTL::RR &r,
                                     const NTL::RR l){
    /* reduced size reduced row echelon form transformation, only yields an
     * (r-l) sized identity matrix */
    NTL::RR k = n-r;
    return  - l*l*l / NTL::RR(3)
            - l*l*n / NTL::RR(2)
            + l*l*r / NTL::RR(2)
            - 3*l*l / NTL::RR(2)
            - 3*l*n / NTL::RR(2)
            +   l*r / NTL::RR(2)
            -  13*l / NTL::RR(6)
            + n*r*r / NTL::RR(2)
            +   n*r / NTL::RR(2)
            - r*r*r / NTL::RR(6)
            +   r*r
            +     r / NTL::RR(6)
            - NTL::RR(1);
}

const NTL::RR Fin_Send_IS_candidate_cost(const NTL::RR &n,
                                         const NTL::RR &r,
                                         const NTL::RR &l){
    return  Fin_Send_rref_red_cost(n,r,l)/probability_k_by_k_is_inv(r-l) + r*r;
}

double isd_log_cost_classic_Prange(const uint32_t n,
                                   const uint32_t k,
                                   const uint32_t t) {
   NTL::RR n_real = NTL::RR(n);
   NTL::RR k_real = NTL::RR(k);
   NTL::RR t_real = NTL::RR(t);

   NTL::RR cost_iter = classic_IS_candidate_cost(n_real,n_real-k_real);
   NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                       NTL::to_RR(binomial_wrapper(n-k,t));

   NTL::RR log_cost = log2_RR(num_iter)+ log2_RR(cost_iter);
   return NTL::conv<double>( log_cost );
}

#define P_MAX_LB 20
double isd_log_cost_classic_LB(const uint32_t n,
                               const uint32_t k,
                               const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_p = 1;
    uint32_t constrained_max_p = P_MAX_LB > t ? t : P_MAX_LB;
    NTL::RR IS_candidate_cost;
    IS_candidate_cost = classic_IS_candidate_cost(n_real,n_real-k_real);
    for(uint32_t p = 1 ;p < constrained_max_p; p++ ){
       NTL::RR p_real = NTL::RR(p);
       NTL::RR cost_iter = IS_candidate_cost +
                           NTL::to_RR(binomial_wrapper(k,p)*p*(n-k));
       NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                           NTL::to_RR( binomial_wrapper(k,p) *
                                       binomial_wrapper(n-k,t-p) );
       log_cost = (NTL::log(num_iter)+NTL::log(cost_iter)) / NTL::log(NTL::RR(2));
       if(min_log_cost > log_cost){
           min_log_cost = log_cost;
           best_p=p;
       }
    }
    std::cerr << std::endl << "Lee-Brickell best p: " << best_p << std::endl;
    return NTL::conv<double>( min_log_cost );
}

#define P_MAX_Leon P_MAX_LB
#define L_MAX_Leon 200
double isd_log_cost_classic_Leon(const uint32_t n,
                                 const uint32_t k,
                                 const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_l=0,best_p=1, constrained_max_l, constrained_max_p;

    NTL::RR IS_candidate_cost;
    IS_candidate_cost = classic_IS_candidate_cost(n_real,n_real-k_real);
    constrained_max_p = P_MAX_Leon > t ? t : P_MAX_Leon;
    for(uint32_t p = 1; p < constrained_max_p; p++ ){
      constrained_max_l = ( L_MAX_Leon > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_Leon);
      NTL::RR p_real = NTL::RR(p);
      for(uint32_t l = 0; l < constrained_max_l; l++){
          NTL::RR KChooseP = NTL::to_RR( binomial_wrapper(k,p) );
          NTL::RR cost_iter = IS_candidate_cost +
                   KChooseP * p_real * NTL::to_RR(l) +
                   ( KChooseP / NTL::power2_RR(l))* NTL::RR(p * (n-k - l));
          NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                              NTL::to_RR( binomial_wrapper(k,p) *
                                          binomial_wrapper(n-k-l,t-p) );
          log_cost = ( NTL::log(num_iter) + NTL::log(cost_iter) ) / NTL::log(NTL::RR(2));
          if(min_log_cost > log_cost){
              min_log_cost = log_cost;
              best_l = l;
              best_p = p;
          }
       }
    }
    std::cerr << std::endl << "Leon Best l: " << best_l << " best p: " << best_p << std::endl;
    return NTL::conv<double>( min_log_cost );
}

#define P_MAX_Stern P_MAX_Leon
#define L_MAX_Stern L_MAX_Leon
double isd_log_cost_classic_Stern(const uint32_t n,
                                 const uint32_t k,
                                 const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_l = 0,best_p = 2, constrained_max_l, constrained_max_p;

    NTL::RR IS_candidate_cost;
    IS_candidate_cost = classic_IS_candidate_cost(n_real,n_real-k_real);

    constrained_max_p = P_MAX_Stern > t ? t : P_MAX_Stern;
    for(uint32_t p = 2; p < constrained_max_p; p = p+2 ){
      constrained_max_l = ( L_MAX_Stern > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_Stern);
      NTL::ZZ kHalfChoosePHalf;
      for(uint32_t  l = 0; l < constrained_max_l; l++){
          NTL::RR p_real = NTL::RR(p);
          kHalfChoosePHalf = binomial_wrapper(k/2,p/2);
          NTL::RR kHalfChoosePHalf_real = NTL::to_RR(kHalfChoosePHalf);

          NTL::RR cost_iter = IS_candidate_cost +
                  kHalfChoosePHalf_real *
                              ( NTL::to_RR(l)*p_real +
                                (kHalfChoosePHalf_real / NTL::power2_RR(l))  * NTL::RR(p * (n-k - l))
                              );
// #if LOG_COST_CRITERION == 1
          NTL::RR log_stern_list_size = kHalfChoosePHalf_real *
                                   ( p_real/NTL::RR(2) * NTL::log( k_real/NTL::RR(2))/NTL::log(NTL::RR(2) ) +NTL::to_RR(l));
                  log_stern_list_size = NTL::log(log_stern_list_size) / NTL::log(NTL::RR(2));
                  cost_iter = cost_iter*log_stern_list_size;
// #endif
          NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                              NTL::to_RR( kHalfChoosePHalf*kHalfChoosePHalf *
                                          binomial_wrapper(n-k-l,t-p) );
          log_cost = log2_RR(num_iter) + log2_RR(cost_iter);
          if(min_log_cost > log_cost){
              min_log_cost = log_cost;
              best_l = l;
              best_p = p;
          }
       }
    }

    std::cerr << std::endl << "Stern Best l: " << best_l << " best p: " << best_p << std::endl;
    return NTL::conv<double>( min_log_cost );
}

#define P_MAX_FS P_MAX_Stern
#define L_MAX_FS L_MAX_Stern
double isd_log_cost_classic_FS(const uint32_t n,
                                 const uint32_t k,
                                 const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_l = 0, best_p = 2,constrained_max_l, constrained_max_p;

    NTL::RR IS_candidate_cost;
    constrained_max_p = P_MAX_Stern > t ? t : P_MAX_Stern;
    for(uint32_t p = 2; p < constrained_max_p; p = p+2 ){
      constrained_max_l = ( L_MAX_Stern > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_Stern);
      NTL::RR p_real = NTL::RR(p);
      NTL::ZZ kPlusLHalfChoosePHalf;
      for(uint32_t  l = 0; l < constrained_max_l; l++){
       IS_candidate_cost = Fin_Send_IS_candidate_cost(n_real,n_real-k_real,NTL::RR(l));
          kPlusLHalfChoosePHalf = binomial_wrapper((k+l)/2,p/2);
          NTL::RR kPlusLHalfChoosePHalf_real = NTL::to_RR(kPlusLHalfChoosePHalf);
          NTL::RR cost_iter = IS_candidate_cost +
                  kPlusLHalfChoosePHalf_real *
                              ( NTL::to_RR(l)*p_real +
                                ( kPlusLHalfChoosePHalf_real / NTL::power2_RR(l)) *
                                  NTL::RR(p * (n-k - l))
                              );
// #if LOG_COST_CRITERION == 1
          NTL::RR l_real = NTL::to_RR(l);
          NTL::RR log_FS_list_size = kPlusLHalfChoosePHalf_real *
                                   ( p_real/NTL::RR(2) * NTL::log( (k_real+l_real)/NTL::RR(2))/NTL::log(NTL::RR(2) ) +l_real);
                  log_FS_list_size = log2_RR(log_FS_list_size);
                  cost_iter = cost_iter*log_FS_list_size;
// #endif
          NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                              NTL::to_RR( kPlusLHalfChoosePHalf * kPlusLHalfChoosePHalf *
                                          binomial_wrapper(n-k-l,t-p) );

          log_cost = log2_RR(num_iter) + log2_RR(cost_iter);
          if(min_log_cost > log_cost){
              min_log_cost = log_cost;
              best_l = l;
              best_p = p;
          }
       }
    }
    std::cerr << std::endl << "FS Best l: " << best_l << " best p: " << best_p << std::endl;
    return NTL::conv<double>( min_log_cost );
}

#define P_MAX_MMT (P_MAX_FS+25) // P_MAX_MMT
#define L_MAX_MMT 350 //L_MAX_MMT
#define L_MIN_MMT 2
double isd_log_cost_classic_MMT(const uint32_t  n,
                                 const uint32_t k,
                                 const uint32_t t) {
    uint32_t r = n-k;
    NTL::RR n_real = NTL::RR(n);
    NTL::RR r_real = NTL::RR(r);
    NTL::RR k_real = n_real-r_real;

    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost, log_mem_cost;
    uint32_t best_l= L_MIN_MMT, best_l1, best_p = 4,
             constrained_max_l = 0, constrained_max_p;

    NTL::RR FS_IS_candidate_cost;
    constrained_max_p = P_MAX_MMT > t ? t : P_MAX_MMT;
    /* p should be divisible by 4 in MMT */
    for(uint32_t p = 4; p <= constrained_max_p; p = p+4 ){
      constrained_max_l = ( L_MAX_MMT > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_MMT );
             for(uint32_t l = L_MIN_MMT; l <= constrained_max_l; l++){
                NTL::RR l_real = NTL::to_RR(l);
                NTL::ZZ kPlusLHalfChoosePHalf = binomial_wrapper((k+l)/2,p/2);
                NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                              NTL::to_RR( kPlusLHalfChoosePHalf * kPlusLHalfChoosePHalf *
                                          binomial_wrapper(n-k-l,t-p) );
                FS_IS_candidate_cost = Fin_Send_IS_candidate_cost(n_real,r_real,l_real);
                NTL::ZZ  kPlusLHalfChoosePFourths = binomial_wrapper((k+l)/2,p/4);
                NTL::RR  kPlusLHalfChoosePFourths_real = NTL::to_RR(kPlusLHalfChoosePFourths);
                NTL::RR  minOperandRight, min;
                NTL::RR  PChoosePHalf = NTL::to_RR(binomial_wrapper(p,p/2));
                NTL::RR  kPlusLChoosePHalf = NTL::to_RR(binomial_wrapper((k+l),p/2));
                minOperandRight = NTL::to_RR(binomial_wrapper((k+l)/2,p/2)) / PChoosePHalf;
                min =  kPlusLHalfChoosePFourths_real > minOperandRight ? minOperandRight : kPlusLHalfChoosePFourths_real;

               /* hoist out anything not depending on l_1/l_2 split*/
#if defined(EXPLORE_REPRS)
               for(uint32_t l_1 = 1 ; l_1 <= l ; l_1++){
                  uint32_t l_2= l-l_1;
#else
                  uint32_t l_2 = NTL::conv<unsigned int>(log2_RR(kPlusLHalfChoosePFourths_real / NTL::to_RR(binomial_wrapper(p,p/2))));
                  /*clamp l_2 to a safe value , 0 < l_2 < l*/
                  l_2 = l_2 <= 0 ? 1 : l_2;
                  l_2 = l_2 >= l ? l-1 : l_2;

                  uint32_t l_1= l - l_2;
#endif
                  NTL::RR interm = kPlusLHalfChoosePFourths_real / NTL::power2_RR(l_2) *
                                         NTL::to_RR(p/2*l_1);

                  NTL::RR otherFactor = ( NTL::to_RR(p/4*l_2) + interm );
                  NTL::RR cost_iter = FS_IS_candidate_cost +
                                      min*otherFactor +
                                      kPlusLHalfChoosePFourths_real * NTL::to_RR(p/2*l_2);

                  NTL::RR lastAddend = otherFactor +
                                        kPlusLHalfChoosePFourths_real *
                                         kPlusLChoosePHalf * PChoosePHalf /
                                         NTL::power2_RR(l)   *
                                       NTL::to_RR( p*(r-l) );
                  lastAddend = lastAddend * kPlusLHalfChoosePFourths_real;
                  cost_iter += lastAddend;
// #if 0

          NTL::RR log_MMT_space = r_real*n_real +
                                  kPlusLHalfChoosePFourths_real *
                                       (NTL::to_RR(p/4)* log2_RR(NTL::to_RR(k+l/2))+ NTL::to_RR(l_2) )+
                                  NTL::to_RR(min) * (NTL::to_RR(p/2)* log2_RR(NTL::to_RR(k+l))+ NTL::to_RR(l) );
                  log_MMT_space = log2_RR(log_MMT_space);
                  cost_iter = cost_iter*log_MMT_space;
// #endif
                  log_cost = log2_RR(num_iter) + log2_RR(cost_iter);
                  if(min_log_cost > log_cost){
                      min_log_cost = log_cost;
                      best_l = l;
                      best_l1 = l_1;
                      best_p = p;
                      log_mem_cost = log_MMT_space;
                  }
#if defined(EXPLORE_REPRS)
               }
#endif
            }
    }
    std::cerr << std::endl << "MMT Best l: " << best_l
                           << " best p: "   << best_p
                           << " best l1: "  << best_l1
                           << std::endl;
   if(best_p == constrained_max_p){
          std::cerr << std::endl << "Warning: p on exploration edge! " << std::endl;
   }
   if(best_l == constrained_max_l){
          std::cerr << std::endl << "Warning: l on exploration edge! " << std::endl;
   }
   //std::cerr << log_mem_cost << " ";
   return NTL::conv<double>( min_log_cost );
}

#define P_MAX_BJMM 20 // P_MAX_MMT
#define L_MAX_BJMM 90 //L_MAX_MMT
#define Eps1_MAX_BJMM 4
#define Eps2_MAX_BJMM 4
double isd_log_cost_classic_BJMM(const uint32_t n,
                                 const uint32_t k,
                                 const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    uint32_t r = n-k;
    NTL::RR r_real = NTL::RR(r);

    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_l, best_p,
             best_eps_1, best_eps_2,
             constrained_max_l, constrained_max_p;

    NTL::RR FS_IS_candidate_cost;
    constrained_max_p = P_MAX_BJMM > t ? t : P_MAX_BJMM;
    /*p should be divisible by 2 in BJMM */
    for(uint32_t p = 2; p < constrained_max_p; p = p+2 ){
        /* sweep over all the valid eps1 knowing that p/2 + eps1 should be a
         * multiple of 4*/
        constrained_max_l = ( L_MAX_BJMM > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_BJMM );
        for(uint32_t  l = 0; l < constrained_max_l; l++){
            for(uint32_t eps1 = 2+(p%2) ; eps1 < Eps1_MAX_BJMM; eps1 = eps1 + 2) {
                uint32_t p_1 = p/2 + eps1;
            /* sweep over all the valid eps2 knowing that p_1/2 + eps2 should
             * be even */
                for(uint32_t eps2 = (p_1%2) ; eps2 < Eps2_MAX_BJMM; eps2 = eps2 + 2){
                    uint32_t p_2 = p_1/2 + eps2;

                    /* Available parameters p, p_1,p_2,p_3, l */
                    NTL::RR l_real = NTL::RR(l);
                    FS_IS_candidate_cost = Fin_Send_IS_candidate_cost(n_real,n_real-k_real,l_real);
                    uint32_t p_3 = p_2/2;

                    NTL::ZZ L3_list_len = binomial_wrapper((k+l)/2,p_3);
                    NTL::RR L3_list_len_real = NTL::to_RR(L3_list_len);
                    /* the BJMM number of iterations depends only on L3 parameters
                    * precompute it */
                    NTL::RR num_iter  = NTL::to_RR( binomial_wrapper(n,t) ) /
                                        NTL::to_RR( binomial_wrapper((k+l),p) *
                                                    binomial_wrapper(r-l,t-p)
                                                );
                    NTL::RR P_invalid_splits = NTL::power(L3_list_len_real,2) /
                                            NTL::to_RR( binomial_wrapper(k+l,p_2));
                    num_iter = num_iter / NTL::power(P_invalid_splits,4);

                    /* lengths of lists 2 to 0 have to be divided by the number of repr.s*/
                    NTL::RR L2_list_len = NTL::to_RR(binomial_wrapper(k+l,p_2)) *
                                        NTL::power(P_invalid_splits,1);
                    NTL::RR L1_list_len = NTL::to_RR(binomial_wrapper(k+l,p_1)) *
                                        NTL::power(P_invalid_splits,2);
                    /* estimating the range for r_1 and r_2 requires to compute the
                    * number of representations rho_1 and rho_2 */

                    NTL::ZZ rho_2 = binomial_wrapper(p_1,p_1/2) *
                                    binomial_wrapper(k+l-p_1,eps2);
                    NTL::ZZ rho_1 = binomial_wrapper(p,p/2) *
                                    binomial_wrapper(k+l-p,eps1);
                    int min_r2 = NTL::conv<int>(NTL::log(NTL::to_RR(rho_2)) /
                                NTL::log(NTL::RR(2)));
                    int max_r1 = NTL::conv<int>(NTL::log(NTL::to_RR(rho_1)) /
                                NTL::log(NTL::RR(2)));

                    /*enumerate r_1 and r_2 over the suggested range
                    * log(rho_2) < r2 < r_1 < log(rho_1)*/
                    /* clamp to safe values */
                    min_r2 = min_r2 > 0 ? min_r2 : 1;
                    max_r1 = max_r1 < (int)l ? max_r1 : l-1;

                    NTL::RR p_real = NTL::RR(p);
                    for(int r_2 = min_r2 ; r_2 < max_r1 - 1; r_2++){
                        for(int r_1 = r_2+1; r_1 < max_r1 ; r_1++){

                            /*add the cost of building Layer 3 to cost_iter */
                            NTL::RR cost_iter = NTL::to_RR(4) *
                                                (k + l + 2*L3_list_len_real +
                                                r_2 +
                                                NTL::power(L3_list_len_real,2)*
                                                NTL::to_RR(2*p_3*r_2));

                             /* add the cost of building Layer 2 */
                            cost_iter +=    2 * (NTL::power((NTL::to_RR(rho_2) /
                                            (NTL::power2_RR(r_2)))*
                                            NTL::power(L3_list_len_real,2),2)
                                            * 2 * p_2 * (r_1-r_2));

                            /* add the cost of building Layer 1 */
                            cost_iter +=    NTL::power((NTL::to_RR(rho_1) /
                                            NTL::power2_RR(r_1)) *
                                            (NTL::to_RR(rho_2) /
                                            NTL::power2_RR(r_2))*
                                            NTL::power(L3_list_len_real,2),4) * 2 * p_1 * l;

                             /* add the cost of building L0 */
                            cost_iter +=    p * (r - l) *
                                            NTL::power((NTL::to_RR(rho_1) / NTL::power2_RR(r_1)) *
                                            (NTL::to_RR(rho_2) /
                                            NTL::power2_RR(r_2))*
                                            NTL::power(L3_list_len_real,2),4)
                                            / NTL::to_RR(l);

                            log_cost = log2_RR(num_iter) + log2_RR(cost_iter);

                            if(min_log_cost >  log_cost){
                                min_log_cost = log_cost;
                                best_l = l;
                                best_p = p;
                                best_eps_1 = eps1;
                                best_eps_2 = eps2;
                            }
                        }
                    }

                } /*end of iteration over l */
            /* to review up to to here */
            } /* end for over eps2 */
         } /* end for over eps1 */
    } /* end for over p*/
    std::cerr << std::endl << "BJMM Best l: " << best_l
                            << " best p: "   << best_p
                            << " best eps1: "  << best_eps_1
                            << " best eps2: "  << best_eps_2
                            << std::endl;
   return NTL::conv<double>( min_log_cost );
}

/***************************Quantum ISDs***************************************/

const NTL::RR quantum_gauss_red_cost(const NTL::RR &n,
                                     const NTL::RR & k) {
    return 0.5* NTL::power(n-k,3) + k*NTL::power((n-k),2);
}

double isd_log_cost_quantum_LB(const uint32_t n,
                               const uint32_t k,
                               const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR log_pi_fourths = NTL::log(pi*0.25);
    NTL::RR log_pinv = log_probability_k_by_k_is_inv(k_real);
    NTL::RR iteration_cost = quantum_gauss_red_cost(n_real,k_real) +
                             NTL::to_RR(binomial_wrapper(k,2) * 2 * (n-k));
    NTL::RR log_cost = (lnBinom(n_real,t_real) -
                        (lnBinom(k_real,NTL::RR(2)) +
                         lnBinom(n_real-k_real,t-NTL::RR(2)))
                       )*0.5 +
                       log_pi_fourths;
    log_cost += NTL::log(iteration_cost);
    log_cost = log_cost / NTL::log(NTL::RR(2));
    return NTL::conv<double>( log_cost );
}

#define MAX_M (t/2)

double isd_log_cost_quantum_stern(const uint32_t n,
                                  const uint32_t k,
                                  const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR current_complexity, log_p_success, c_it, c_dec;

    // Start computing Stern's parameter invariant portions of complexity
    NTL::RR log_pi_fourths = NTL::log(pi*0.25);
    // compute the probability of a random k * k being invertible
    NTL::RR log_pinv = log_probability_k_by_k_is_inv(k_real);
    // compute the cost of inverting the matrix, in a quantum execution env.
    NTL::RR c_inv = quantum_gauss_red_cost(n_real,k_real);

    // optimize Stern's parameters :
    // m : the # of errors in half of the chosen dimensions
    // l : the length of the run of zeroes in the not chosen dimensions
    // done via exhaustive parameter space search, minimizing the total
    // complexity.
    // Initial value set to codeword bruteforce to ensure the minimum is found.
    NTL::RR min_stern_complexity = NTL::RR(n)*NTL::log(NTL::RR(2));

    for(long m = 1; m <= MAX_M; m++){
        NTL::RR m_real = NTL::RR(m);
        /* previous best complexity as a function of l alone.
         * initialize to bruteforce-equivalent,  break optimization loop as soon
         * as a minimum is found */
        NTL::RR prev_best_complexity = NTL::RR(t);
        for(long l = 0; l < (n-k-(t-2*m)); l++ ){

          NTL::RR  l_real = NTL::RR(l);
          log_p_success = lnBinom(t_real, 2*m_real) +
                   lnBinom(n_real-t_real, k_real-2*m_real) +
                   lnBinom(2*m_real,m_real) +
                   lnBinom(n_real-k_real-t_real+2*m_real,l_real);
          log_p_success = log_p_success - ( m_real*NTL::log(NTL::RR(4)) +
                              lnBinom(n_real,k_real) +
                              lnBinom(n_real -k_real, l_real));
         current_complexity = -(log_p_success+log_pinv)*0.5 + log_pi_fourths;
         /* to match specifications , the term should be
          * (n_real-k_real), as per in deVries, although
          * David Hobach thesis mentions it to be
          * (n_real-k_real-l_real), and it seems to match.
          * amend specs for the typo. */
         c_it = l_real +
            (n_real-k_real-l_real)* NTL::to_RR(binomial_wrapper(k/2,m)) /
            NTL::power2_RR(-l);

         c_it = c_it * 2*m_real * NTL::to_RR(binomial_wrapper(k/2,m));
#if IGNORE_DECODING_COST == 1
        c_dec = 0.0;
#elif IGNORE_DECODING_COST == 0
        /*cost of decoding estimated as per Golomb CWDEC
         * decoding an n-bit vector with weight k is
         * CWDEC_cost(k,n)=O(n^2 log_2(n))  and following deVries, where
         * c_dec = CWDEC_cost(n-k, n) + k + CWDEC_cost(l,n-k)*/
        c_dec = n_real*n_real*NTL::log(n_real) + k_real +
                (n_real-k_real)*(n_real-k_real)*NTL::log((n_real-k_real));
#endif
         current_complexity = current_complexity + NTL::log(c_it+c_inv+c_dec);
          if(current_complexity < prev_best_complexity){
             prev_best_complexity = current_complexity;
          } else{
             break;
          }
        }
       if(current_complexity < min_stern_complexity){
           min_stern_complexity = current_complexity;
       }
    }
    return NTL::conv<double>( min_stern_complexity / NTL::log(NTL::RR(2.0)) );
}

/***************************Aggregation ***************************************/

double c_isd_log_cost(const uint32_t n,
                      const uint32_t k,
                      const uint32_t t,
                      const uint32_t qc_order,
                      const uint32_t is_kra) {
    double min_cost = n, current_cost;
    /* for key recovery attacks the advantage from quasi-cyclicity is p,
     * for an ISD, the DOOM advantage is just sqrt(p) */
    double qc_red_factor= is_kra ? logl(qc_order) : logl(qc_order)/2.0;
    qc_red_factor = qc_red_factor/logl(2);

    std::cout << "Classic ";
    current_cost = isd_log_cost_classic_Prange(n,k,t) - qc_red_factor;
    std::cerr << "Classic Prange: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = current_cost;

    current_cost = isd_log_cost_classic_LB(n,k,t)- qc_red_factor;
    std::cerr << "Classic Lee-Brickell ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

    current_cost = isd_log_cost_classic_Leon(n,k,t)- qc_red_factor;
     std::cerr << "Classic Leon ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

    current_cost = isd_log_cost_classic_Stern(n,k,t)- qc_red_factor;
    std::cerr << "Classic Stern ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

    current_cost = isd_log_cost_classic_FS(n,k,t)- qc_red_factor;
    std::cerr << "Classic Fin-Send ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

    current_cost = isd_log_cost_classic_MMT(n,k,t)- qc_red_factor;
    std::cerr << "Classic MMT ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

#if SKIP_BJMM == 0
    current_cost = isd_log_cost_classic_BJMM(n,k,t)- qc_red_factor;
    std::cerr << "Classic BJMM ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;
#endif
    std::cout << std::endl;

    return min_cost;
}

double q_isd_log_cost(const uint32_t n,
                      const uint32_t k,
                      const uint32_t t,
                      const uint32_t qc_order,
                      const uint32_t is_kra) {
    double min_cost = n, current_cost;
    /* for key recovery attacks the advantage from quasi-cyclicity is p,
     * for an ISD, the DOOM advantage is just sqrt(p) */
    double qc_red_factor= is_kra ? logl(qc_order) : logl(qc_order)/2.0;
    qc_red_factor = qc_red_factor/logl(2);
    std::cout << "Quantum ";

    current_cost = isd_log_cost_quantum_LB(n,k,t)- qc_red_factor;
    std::cout << current_cost << " ";
//     std::cout << " Q-Lee-Brickell ISD: " << /**/current_cost << std::endl;
    min_cost = current_cost;

    current_cost = isd_log_cost_quantum_stern(n, k, t)- qc_red_factor;
    std::cout << current_cost << " ";
//     std::cout << ", Q-Stern ISD: " << current_cost << std::endl;
    min_cost = min_cost > current_cost ? current_cost : min_cost;
    std::cout << std::endl;

    return min_cost;
}


#include "eigen-runtime.h"
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/Sparse>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/GenEigsRealShiftSolver.h>
#include <Spectra/SymEigsSolver.h>
#include <Spectra/SymEigsShiftSolver.h>
#include <Spectra/SymGEigsSolver.h>
#include <Spectra/MatOp/DenseSymMatProd.h>
#include <Spectra/MatOp/SparseGenRealShiftSolve.h>
#include <Spectra/MatOp/SparseSymShiftSolve.h>
#include <Spectra/MatOp/SparseCholesky.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/Util/GEigsMode.h>

using namespace Spectra;
using namespace Eigen;

extern "C" RET eigen_eig(
    void* d, void* v,
    const void* p, int n)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    typedef Map< Matrix<T3,Dynamic,Dynamic> > MapComplexMatrix;
    MapMatrix M((T1*)p, n, n);
    MapComplexMatrix D((T3*)d, n, 1);
    MapComplexMatrix V((T3*)v, n, n);
    EigenSolver<MatrixXd> es(M);
    D = es.eigenvalues();
    V = es.eigenvectors();
}

extern "C" const int spectral_eigs(
    const int k,   // number of eigenvectors to return
    void* d, void* v,  // pointer to the results
    const void* p, const int n,  // pointer to the input and dimensionality
    const int ncv, int maxit, double tol,
    const int mode, const double sigma,
    const int select)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    typedef Map< Matrix<T3,Dynamic,Dynamic> > MapComplexMatrix;
    MapMatrix M((T1*)p, n, n);
    MapComplexMatrix D((T3*)d, k, 1);
    MapComplexMatrix V((T3*)v, n, k);

    if(mode == 0) {
        DenseGenMatProd<double> op(M);
        GenEigsSolver<DenseGenMatProd<double>> eigs(op, k, ncv);
        eigs.init();
        int nconv = eigs.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigs.info() == CompInfo::Successful) {
            D = eigs.eigenvalues();
            V = eigs.eigenvectors();
        }
        return static_cast<int>(eigs.info());
    } else {
        DenseGenRealShiftSolve<double> op(M);
        GenEigsRealShiftSolver<DenseGenRealShiftSolve<double>> eigs(op, k, ncv, sigma);
        eigs.init();
        int nconv = eigs.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigs.info() == CompInfo::Successful) {
            D = eigs.eigenvalues();
            V = eigs.eigenvectors();
        }
        return static_cast<int>(eigs.info());
    }
}

extern "C" const int spectral_eigsh(
    int k,
    void* d, void* v,
    const void* p, int n,
    const int ncv, int maxit, double tol,
    const int mode, const double sigma,
    const int select)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    MapMatrix M((T1*)p, n, n);
    MapMatrix D((T1*)d, k, 1);
    MapMatrix V((T1*)v, n, k);

    if(mode == 0) {
        DenseGenMatProd<double> op(M);
        SymEigsSolver<DenseGenMatProd<double>> eigsh(op, k, ncv);
        eigsh.init();
        int nconv = eigsh.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigsh.info() == CompInfo::Successful) {
            D = eigsh.eigenvalues();
            V = eigsh.eigenvectors();
        }
        return static_cast<int>(eigsh.info());
    } else {
        DenseSymShiftSolve<double> op(M);
        SymEigsShiftSolver<DenseSymShiftSolve<double>> eigsh(op, k, ncv, sigma);
        eigsh.init();
        int nconv = eigsh.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigsh.info() == CompInfo::Successful) {
            D = eigsh.eigenvalues();
            V = eigsh.eigenvectors();
        }
        return static_cast<int>(eigsh.info());
    }
}

extern "C" const int spectral_seigs(
    int k,
    void* d, void* v,
    const void* values, const void* outerIndexPtr, const void* innerIndices, int n, int s,
    const int ncv, int maxit, double tol,
    const int mode, const double sigma,
    const int select)
{
    typedef Map< Matrix<T3,Dynamic,Dynamic> > MapComplexMatrix;
    typedef Map<const SparseMatrix<T1> > MapSparseMatrix;
    MapSparseMatrix M(n, n, s, (int*)outerIndexPtr, (int*)innerIndices, (T1*)values);
    MapComplexMatrix D((T3*)d, k, 1);
    MapComplexMatrix V((T3*)v, n, k);

    if(mode == 0) {
        SparseGenMatProd<double> op(M);
        GenEigsSolver<SparseGenMatProd<double>> eigs(op, k, ncv);
        eigs.init();
        int nconv = eigs.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigs.info() == CompInfo::Successful) {
            D = eigs.eigenvalues();
            V = eigs.eigenvectors();
        }
        return static_cast<int>(eigs.info());
    } else {
        SparseGenRealShiftSolve<double> op(M);
        GenEigsRealShiftSolver<SparseGenRealShiftSolve<double>> eigs(op, k, ncv, sigma);
        eigs.init();
        int nconv = eigs.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigs.info() == CompInfo::Successful) {
            D = eigs.eigenvalues();
            V = eigs.eigenvectors();
        }
        return static_cast<int>(eigs.info());
    }
}

extern "C" const int spectral_seigsh(
    int k,
    void* d, void* v,
    const void* values, const void* outerIndexPtr, const void* innerIndices, int n, int s,
    const int ncv, int maxit, double tol,
    const int mode, const double sigma,
    const int select)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    typedef Map<const SparseMatrix<T1> > MapSparseMatrix;
    MapSparseMatrix M(n, n, s, (int*)outerIndexPtr, (int*)innerIndices, (T1*)values);
    MapMatrix D((T1*)d, k, 1);
    MapMatrix V((T1*)v, n, k);

    if(mode == 0) {
        SparseGenMatProd<double> op(M);
        SymEigsSolver<SparseGenMatProd<double>> eigsh(op, k, ncv);
        eigsh.init();
        int nconv = eigsh.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigsh.info() == CompInfo::Successful) {
            D = eigsh.eigenvalues();
            V = eigsh.eigenvectors();
        }
        return static_cast<int>(eigsh.info());
    } else {
        SparseSymShiftSolve<double> op(M);
        SymEigsShiftSolver<SparseSymShiftSolve<double>> eigsh(op, k, ncv, 0);
        eigsh.init();
        int nconv = eigsh.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigsh.info() == CompInfo::Successful) {
            D = eigsh.eigenvalues();
            V = eigsh.eigenvectors();
        }
        return static_cast<int>(eigsh.info());
    }
}

// Generalized eigen solver for real symmetric matrices
extern "C" const int spectral_geigsh(
    int k,
    void* d, void* v,
    const void* a, int n,
    const void* values, const void* outerIndexPtr, const void* innerIndices, int s,
    const int ncv, int maxit, double tol,
    const int select)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    typedef Map<const SparseMatrix<T1> > MapSparseMatrix;
    MapMatrix A((T1*)a, n, n);
    MapSparseMatrix B(n, n, s, (int*)outerIndexPtr, (int*)innerIndices, (T1*)values);
    MapMatrix D((T1*)d, k, 1);
    MapMatrix V((T1*)v, n, k);

    DenseSymMatProd<double> op(A);
    SparseCholesky<double> Bop(B);

    SymGEigsSolver<DenseSymMatProd<double>, SparseCholesky<double>, GEigsMode::Cholesky> geigs(op, Bop, k, ncv);

    geigs.init();
    int nconv = geigs.compute(static_cast<SortRule>(select), maxit, tol);
    if(geigs.info() == CompInfo::Successful) {
        D = geigs.eigenvalues();
        V = geigs.eigenvectors();
    }
    return static_cast<int>(geigs.info());
}

template <class T>
RET cholesky(void* px, const void* pa, int n)
{
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    MapMatrix x((T*)px, n, n);
    MapMatrix A((T*)pa, n, n);
    x = A.llt().matrixL();
    return 0;
}
API(cholesky, (int code,
    void* px, const void* pa, int n), (px,pa,n));

template <class T, class TT>
RET bdcsvd(
    void* pu, void* ps, void* pv,
    const void* px, int r, int c)
{
    int m = r < c ? r : c;
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    typedef Map< Matrix<TT,Dynamic,Dynamic> > MapMatrix2;
    MapMatrix A((T*)px, r, c);
    MapMatrix U((T*)pu, r, m);
    MapMatrix2 s((TT*)ps, m, 1);
    MapMatrix V((T*)pv, c, m);
    BDCSVD< Matrix<T,Dynamic,Dynamic>> svd(A, ComputeThinU|ComputeThinV);
    U = svd.matrixU();
    V = svd.matrixV();
    s = svd.singularValues();
    return 0;
}
API2(bdcsvd, (int code,
    void* pu, void* ps, void* pv, const void* px, int r, int c), (pu,ps,pv,px,r,c));


/**
 * @file    gaussNewton.hpp
 * @brief   This provides the Gauss-Newton algorithm for Non-Linear Least Squares
 * @author  Shubham Shrivastava
 */

#ifndef GAUSS_NEWTON_H_
#define GAUSS_NEWTON_H_

#include <Eigen/Dense>
#include "tapl/common/common.hpp"

namespace tapl {
    namespace optim {

        /**< Gauss-Newton Optimizer */
        class GaussNewtonOptimizer {
        public:
            /**
             * @brief Compute reprojection error of an estimated 3d point in multiple cameras
             *
             * @param[in] point3d estimated 3d point
             * @param[in] points2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             * @param[out] errors reprojection error in 'n' cameras (n x 2)
             */
            Eigen::MatrixXd reprojectionError(
                                    const Eigen::MatrixXd &point3d,
                                    const std::vector<tapl::Point2d> &points2d,
                                    const std::vector<Eigen::MatrixXd> &projectionMatrices) {

                // build homogeneous point
                Eigen::MatrixXd point3d_homogeneous(4,1);
                point3d_homogeneous.block<3,1>(0,0) << point3d;
                point3d_homogeneous.block<1,1>(3,0) << 1.0;

                // reprojection errors
                Eigen::MatrixXd errors(projectionMatrices.size()*2,1);
                // go through each camera
                for (auto it=projectionMatrices.begin();
                          it!=projectionMatrices.end();
                          ++it) {
                    // project to camera plane
                    Eigen::MatrixXd point2d_homogeneous(1,3);
                    point2d_homogeneous = (*it) * point3d_homogeneous;
                    Eigen::MatrixXd point2d_euclidean(2,1);
                    point2d_euclidean << (point2d_homogeneous(0,0) / point2d_homogeneous(2,0)),
                                         (point2d_homogeneous(1,0) / point2d_homogeneous(2,0));
                    // compute reprojection error
                    auto cam_idx = std::distance(projectionMatrices.begin(), it);
                    Eigen::MatrixXd reprojection_err(2,1);
                    Eigen::MatrixXd points2d_eigen(2,1);
                    points2d_eigen << points2d[cam_idx].x, points2d[cam_idx].y;
                    reprojection_err << (point2d_euclidean - points2d_eigen);
                    errors.block<2,1>(2*cam_idx,0) = reprojection_err;
                }
                // return reprojection errors
                return errors;
            }

            /**
             * @brief Given a 3D point and its corresponding points in the image
             *         planes, compute the associated Jacobian
             *
             * @param[in] point3d estimated 3d point
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             *
             * @return J Jacobian Matrix
             */
            const Eigen::MatrixXd jacobian(
                            const Eigen::MatrixXd &point3d,
                            const std::vector<Eigen::MatrixXd> &projectionMatrices) {

                // build homogeneous point
                Eigen::MatrixXd point3d_homogeneous(4,1);
                point3d_homogeneous.block<3,1>(0,0) << point3d;
                point3d_homogeneous.block<1,1>(3,0) << 1.0;

                // jacobian matrix
                Eigen::MatrixXd J(projectionMatrices.size()*2,3);
                // go through each camera
                for (auto it=projectionMatrices.begin();
                          it!=projectionMatrices.end();
                          ++it) {
                    double m1P_hat = ((*it).block<1,4>(0,0) * point3d_homogeneous)(0,0);
                    double m2P_hat = ((*it).block<1,4>(1,0) * point3d_homogeneous)(0,0);
                    double m3P_hat = ((*it).block<1,4>(2,0) * point3d_homogeneous)(0,0);

                    // build Jacobian
                    auto cam_idx = std::distance(projectionMatrices.begin(), it);
                    J.block<1,3>(2*cam_idx,0) << (((*it)(0,0)*m3P_hat - (*it)(2,0)*m1P_hat) / (m3P_hat*m3P_hat)),
                                                 (((*it)(0,1)*m3P_hat - (*it)(2,1)*m1P_hat) / (m3P_hat*m3P_hat)),
                                                 (((*it)(0,2)*m3P_hat - (*it)(2,2)*m1P_hat) / (m3P_hat*m3P_hat));
                    J.block<1,3>((2*cam_idx)+1,0) << (((*it)(1,0)*m3P_hat - (*it)(2,0)*m2P_hat) / (m3P_hat*m3P_hat)),
                                                     (((*it)(1,1)*m3P_hat - (*it)(2,1)*m2P_hat) / (m3P_hat*m3P_hat)),
                                                     (((*it)(1,2)*m3P_hat - (*it)(2,2)*m2P_hat) / (m3P_hat*m3P_hat));
                }

                // return the Jacobian Matrix
                return J;
            }

            /**
             * @brief Compute L2 reprojection error of an estimated 3d point in multiple cameras
             *
             * @param[in] point3d estimated 3d point
             * @param[in] points2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             * @param[out] errors reprojection error in 'n' cameras (n x 2)
             */
            Eigen::MatrixXd reprojectionErrorL2(
                                    const Eigen::MatrixXd &point3d,
                                    const std::vector<tapl::Point2d> &points2d,
                                    const std::vector<Eigen::MatrixXd> &projectionMatrices) {

                // build homogeneous point
                Eigen::MatrixXd point3d_homogeneous(4,1);
                point3d_homogeneous.block<3,1>(0,0) << point3d;
                point3d_homogeneous.block<1,1>(3,0) << 1.0;

                // reprojection errors
                Eigen::MatrixXd errors(projectionMatrices.size()*2,1);
                // go through each camera
                for (auto it=projectionMatrices.begin();
                          it!=projectionMatrices.end();
                          ++it) {
                    // project to camera plane
                    Eigen::MatrixXd point2d_homogeneous(1,3);
                    point2d_homogeneous = (*it) * point3d_homogeneous;
                    Eigen::MatrixXd point2d_euclidean(2,1);
                    point2d_euclidean << (point2d_homogeneous(0,0) / point2d_homogeneous(2,0)),
                                         (point2d_homogeneous(1,0) / point2d_homogeneous(2,0));
                    // compute reprojection error
                    auto cam_idx = std::distance(projectionMatrices.begin(), it);
                    Eigen::MatrixXd reprojection_err(2,1);
                    Eigen::MatrixXd points2d_eigen(2,1);
                    points2d_eigen << points2d[cam_idx].x, points2d[cam_idx].y;
                    reprojection_err << (point2d_euclidean - points2d_eigen)*(point2d_euclidean - points2d_eigen);
                    errors.block<2,1>(2*cam_idx,0) = reprojection_err;
                }
                // return reprojection errors
                return errors;
            }

            /**
             * @brief Given a 3D point and its corresponding points in the image
             *         planes, compute the associated Jacobian for L2 reprojection errors
             *
             * @param[in] point3d estimated 3d point
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             *
             * @return J Jacobian Matrix
             */
            const Eigen::MatrixXd jacobianL2(
                            const Eigen::MatrixXd &point3d,
                            const std::vector<tapl::Point2d> &points2d,
                            const std::vector<Eigen::MatrixXd> &projectionMatrices) {

                // build homogeneous point
                Eigen::MatrixXd point3d_homogeneous(4,1);
                point3d_homogeneous.block<3,1>(0,0) << point3d;
                point3d_homogeneous.block<1,1>(3,0) << 1.0;

                // jacobian matrix
                Eigen::MatrixXd J(projectionMatrices.size()*2,3);
                // compute L1 reprojection error
                Eigen::MatrixXd reprErr = reprojectionError(point3d, points2d, projectionMatrices);
                // go through each camera
                for (auto it=projectionMatrices.begin();
                          it!=projectionMatrices.end();
                          ++it) {
                    double m1P_hat = ((*it).block<1,4>(0,0) * point3d_homogeneous)(0,0);
                    double m2P_hat = ((*it).block<1,4>(1,0) * point3d_homogeneous)(0,0);
                    double m3P_hat = ((*it).block<1,4>(2,0) * point3d_homogeneous)(0,0);

                    // build Jacobian
                    auto cam_idx = std::distance(projectionMatrices.begin(), it);
                    J.block<1,3>(2*cam_idx,0) << 2*reprErr(2*cam_idx,0)*(((*it)(0,0)*m3P_hat - (*it)(2,0)*m1P_hat) / (m3P_hat*m3P_hat)),
                                                 2*reprErr(2*cam_idx,0)*(((*it)(0,1)*m3P_hat - (*it)(2,1)*m1P_hat) / (m3P_hat*m3P_hat)),
                                                 2*reprErr(2*cam_idx,0)*(((*it)(0,2)*m3P_hat - (*it)(2,2)*m1P_hat) / (m3P_hat*m3P_hat));
                    J.block<1,3>((2*cam_idx)+1,0) << 2*reprErr(2*cam_idx+1,0)*(((*it)(1,0)*m3P_hat - (*it)(2,0)*m2P_hat) / (m3P_hat*m3P_hat)),
                                                     2*reprErr(2*cam_idx+1,0)*(((*it)(1,1)*m3P_hat - (*it)(2,1)*m2P_hat) / (m3P_hat*m3P_hat)),
                                                     2*reprErr(2*cam_idx+1,0)*(((*it)(1,2)*m3P_hat - (*it)(2,2)*m2P_hat) / (m3P_hat*m3P_hat));
                }

                // return the Jacobian Matrix
                return J;
            }

            /**
             * @brief Given a 3D point and its corresponding points in the image
             *         planes, compute the associated Jacobian
             *
             * @param[in] point3d initial estimate of the 3d point
             * @param[in] points2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             * @param[in] nIterations projection matrices of 'n' cameras (n x 3 x 4)
             * @param[out] optimPoint3d optimized point 3d
             *
             * @return pair of pre-optimization and post-optimization reprojection error norms
             */
             std::pair<std::vector<float>,std::vector<float>> optimize(
                        const tapl::Point3d &point3d,
                        const std::vector<tapl::Point2d> &points2d,
                        const std::vector<Eigen::MatrixXd> &projectionMatrices,
                        const uint16_t nIterations,
                        const float reprErrorThresh,
                        tapl::Point3d &optimPoint3d) {

                Eigen::MatrixXd point3d_eigen(3,1);
                point3d_eigen << point3d.x, point3d.y, point3d.z;
                std::vector<float> preOptimReprErrNorm(points2d.size());
                std::vector<float> postOptimReprErrNorm(points2d.size());
                // log reprojection errors
                Eigen::MatrixXd errPreOptim = reprojectionErrorL2(point3d_eigen, points2d, projectionMatrices);
                for (auto i=0; i<points2d.size(); ++i) {
                    preOptimReprErrNorm.at(i) = sqrt( errPreOptim(2*i,0)*errPreOptim(2*i,0) +
                                                      errPreOptim(2*i+1,0)*errPreOptim(2*i+1,0));
                }
                // start optimization
                for (auto n=0; n<nIterations; ++n) {
                    errPreOptim =
                        reprojectionErrorL2(point3d_eigen, points2d, projectionMatrices);
                    auto errPreOptimNorm = sqrt( errPreOptim(0,0)*errPreOptim(0,0) +
                                                 errPreOptim(1,0)*errPreOptim(1,0));
                    if (errPreOptimNorm < reprErrorThresh) {break;}
                    auto J = jacobianL2(point3d_eigen, points2d, projectionMatrices);
                    point3d_eigen = point3d_eigen - (((J.transpose() * J).inverse()) * J.transpose() * errPreOptim);
                }
                Eigen::MatrixXd errPostOptim = reprojectionErrorL2(point3d_eigen, points2d, projectionMatrices);
                for (auto i=0; i<points2d.size(); ++i) {
                    postOptimReprErrNorm.at(i) = sqrt( errPostOptim(2*i,0)*errPostOptim(2*i,0) +
                                                       errPostOptim(2*i+1,0)*errPostOptim(2*i+1,0));
                }

                // optimized point
                optimPoint3d = *(new tapl::Point3d(point3d_eigen(0), point3d_eigen(1), point3d_eigen(2)));

                // return optimization errors
                return std::make_pair(preOptimReprErrNorm, postOptimReprErrNorm);
            }
        };
    }
}

#endif /* GAUSS_NEWTON_H_ */


// simple feed-forward neural network
#pragma once
#include <Eigen/Core>

using Eigen::Matrix;

// MatrixXd Sigmoid(MatrixXd const& x) {
//   return 1.0 / (1.0 + (-x).array().exp());
// }

inline float relu(float x) {
  return (x > 0) ? x : 0;
}

// measured 2x overall slowdown when using MatrixXf instead of template
template <int n_inputs, int n_hidden, int n_outputs>
class SmallNN {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Matrix<float, n_hidden, n_inputs> w0;
  Matrix<float, n_hidden, 1> b0;
  Matrix<float, n_outputs, n_hidden> w1;
  Matrix<float, n_outputs, 1> b1;

  Matrix<float, n_outputs, 1> predict(Matrix<float, n_inputs, 1> inputs) {
    Matrix<float, n_hidden, 1> a1 = w0 * inputs + b0;
    for (int i=0; i<n_hidden; i++) a1(i) = relu(a1(i));  // faster than unaryExpr
    Matrix<float, n_outputs, 1> a2 = w1 * a1 + b1;

    // somewhat hacky residual connection, but it measurably helps
    a2 *= 0.1;
    static_assert(n_outputs <= n_hidden, "n_hidden must be >= n_outputs");
    for (int i=0; i<n_outputs; i++) {
      a2(i) += a1(i);
    }
    a2 *= 1.0 / (1.0 + 0.1);

    return a2;
  }
};


#ifndef CANNON_ML_WOLFE_CONDITIONS_H
#define CANNON_ML_WOLFE_CONDITIONS_H

/*!
 * \file cannon/ml/wolfe_conditions.hpp
 * \brief File containing utility functions relating to line search and
 * sufficient decrease conditions.
 */

#include <functional>

#include <Eigen/Dense>

using namespace Eigen;

using RealFunc = std::function<double(const VectorXd&)>;
using MultiFunc = std::function<VectorXd(const VectorXd&)>;

namespace cannon {
  namespace ml {

    /*!
     * \brief Compute whether the input step along the input direction
     * satisfies the Armijo sufficient decrease condition with respect to the
     * function under consideration.
     *
     * \param f The function being optimized
     * \param f_grad Gradient of the function being optimized
     * \param x Current state from which to compute sufficient decrease
     * \param step Step along direction from x
     * \param direction Optimization direction
     * \param c_1 Sufficient decrease parameter
     *
     * \returns Whether the sufficient decrease condition is satisfied.
     */
    bool sufficient_decrease_condition(RealFunc f, MultiFunc f_grad, const
        VectorXd& x, double step, const VectorXd& direction, double c_1);

    /*!
     * \brief Compute whether the input step along the input direction
     * satisfies the curvature condition.
     *
     * \param f The function being optimized
     * \param f_grad Gradient of the function being optimized
     * \param x Current state from which to compute sufficient decrease
     * \param step Step along direction from x
     * \param direction Optimization direction
     * \param c_2 Curvature parameter
     */
    bool curvature_condition(RealFunc f, MultiFunc f_grad, const
        VectorXd& x, double step, const VectorXd& direction, double c_2);

    /*!
     * \brief Compute whether both Wolfe conditions are satisfied by the input
     * step along the input direction.
     *
     * \param f The function being optimized
     * \param f_grad Gradient of the function being optimized
     * \param x Current state from which to compute sufficient decrease
     * \param step Step along direction from x
     * \param direction Optimization direction
     * \param c_1 Sufficient decrease parameter
     * \param c_2 Curvature parameter
     */
    bool wolfe_conditions(RealFunc f, MultiFunc f_grad, const
        VectorXd& x, double step, const VectorXd& direction, double c_1,
        double c_2);

    /*!
     * \brief Compute the optimal line search step between the input minimum
     * and maximum along the input direction.  Note that this is the minimizing
     * step.
     *
     * \param f The function to be minimized.
     * \param f_grad Gradient function for the function to be minimized.
     * \param x State to conduct line search from
     * \param direction Direction for line search
     * \param start_low Initial minimal step
     * \param start_high Initial maximal step
     * \param c_1 Sufficient decrease parameter
     * \param c_2 Curvature parameter
     * \param iterations Maximum number of iterations.
     */
    double line_search_zoom(RealFunc f, MultiFunc f_grad, const VectorXd& x,
        const VectorXd& direction, double start_low, double start_high,
        double c_1, double c_2, unsigned int iterations=100);

    /*!
     * \brief Compute the optimal line search step along the input direction,
     * moving as far as possible while satisfying the Wolfe conditions.  Note
     * that this is the minimizing step.
     *
     * \param f The function to be minimized.
     * \param f_grad Gradient function for the function to be minimized.
     * \param x State to conduct line search from
     * \param direction Direction for line search
     * \param step_1 Initial step to consider
     * \param c_1 Sufficient decrease parameter
     * \param c_2 Curvature parameter
     * \param iterations Maximum number of iterations.
     */
    // Returns optimal step. Note that this is the minimizing step, not the maximizing.
    double wolfe_condition_line_search(RealFunc f, MultiFunc f_grad, const
        VectorXd& x, const VectorXd& direction, double step_1 = 1.0, double c_1 = 1e-4,
        double c_2 = 0.5, unsigned int iterations=100);

  } // namespace ml
} // namespace cannon

#endif /* ifndef CANNON_ML_WOLFE_CONDITIONS_H */


#include "RBGL.hpp"
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

extern "C"
{

    SEXP BGL_KMST_D( SEXP num_verts_in, SEXP num_edges_in,
                     SEXP R_edges_in, SEXP R_weights_in)
    {
        using namespace boost;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);
        property_map < Graph_dd, edge_weight_t >::type weight = get(edge_weight, g);

        std::vector < Edge > spanning_tree;

        kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

        SEXP ansList, ans, answt;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(ans = allocMatrix(INTSXP,2,spanning_tree.size()));
        PROTECT(answt = allocMatrix(REALSXP,1,spanning_tree.size()));
        int k = 0, j = 0;

        for (std::vector < Edge >::iterator ei = spanning_tree.begin();
                ei != spanning_tree.end(); ++ei)
        {
            INTEGER(ans)[k++] = source(*ei,g);
            INTEGER(ans)[k++] = target(*ei,g);
            REAL(answt)[j++] = weight[*ei];
        }

        SET_VECTOR_ELT(ansList,0,ans);
        SET_VECTOR_ELT(ansList,1,answt);
        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_KMST_U( SEXP num_verts_in, SEXP num_edges_in,
                     SEXP R_edges_in, SEXP R_weights_in)
    {
        using namespace boost;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);
        property_map < Graph_ud, edge_weight_t >::type weight = get(edge_weight, g);

        std::vector < Edge > spanning_tree;

        kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

        SEXP ansList, ans, answt;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(ans = allocMatrix(INTSXP,2,spanning_tree.size()));
        PROTECT(answt = allocMatrix(REALSXP,1,spanning_tree.size()));

        int k = 0, j = 0;
        for (std::vector < Edge >::iterator ei = spanning_tree.begin();
                ei != spanning_tree.end(); ++ei)
        {
            INTEGER(ans)[k++] = source(*ei,g);
            INTEGER(ans)[k++] = target(*ei,g);
            REAL(answt)[j++] = weight[*ei];
        }

        SET_VECTOR_ELT(ansList,0,ans);
        SET_VECTOR_ELT(ansList,1,answt);
        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_PRIM_U( SEXP num_verts_in, SEXP num_edges_in,
                     SEXP R_edges_in, SEXP R_weights_in)
    {
        using namespace boost;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

	int NV = asInteger(num_verts_in);
        std::vector <Vertex> parent(NV);

        prim_minimum_spanning_tree(g, &parent[0]);

        property_map<Graph_ud, edge_weight_t>::type weight = get(edge_weight, g);

        SEXP ansList, ans, answt;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(ans = allocMatrix(INTSXP,2,NV));
        PROTECT(answt = allocMatrix(REALSXP,1,NV));

        int k = 0, j = 0;
        for (unsigned int v = 0; v < num_vertices(g); ++v)
	{
            INTEGER(ans)[k++] = parent[v];
            INTEGER(ans)[k++] = v;
            REAL(answt)[j++] = ( parent[v] == v) ?
			 0 : get(weight, edge(parent[v], v, g).first);
	}

        SET_VECTOR_ELT(ansList,0,ans);
        SET_VECTOR_ELT(ansList,1,answt);
        UNPROTECT(3);
        return(ansList);
    }

}


//==================================================
// newton
//==================================================
#include <iostream>
#include <valarray>
#include <boost/function.hpp>
/*
 ◆開発環境
 [OS]: macOS 10.14~
 [c++] gcc version 9.2.0
 [boost] 1.71.0

 */
// テスト用パラメータークラス
class Param
{
    public:
        double t=0;
        double x=0;
        double y=0;
};

class Newton
{
    private:
        double f( double t, double ox, boost::function<double(double, Param)> method, Param _param )
        {
            double x;
            double _dif;
            x = method(t, _param);
            _dif = ox - x;
            return _dif;
        }

        double df( double t, double ox, boost::function<double(double, Param)> method, Param _param )
        {
            double x;
            double h;
            double dx;
            h = 0.00001;
            x = this->f(t, ox, method, _param);
            dx = this->f(t+h, ox, method, _param);
            return (dx-x)/h;
        }

    public:
        double newton( double initval, double ox, boost::function<double(double, Param)> method, Param _param , int count=1000000000, double ep=0.0001)
        {
            /*
             oxに最も近づくt値を近似する。epは収束時の許容誤差
             */
            double t2;
            double rf;
            double rdf;
            double t=initval;
            for (int i=0; i<count; i++)
                {
                    rf = this->f(t, ox, method, _param);
                    rdf = this->df(t, ox, method, _param);
                    std::cout<<"t:  " << t <<",   ox: "<< ox << ",  rf: " << rf << ",    rdf: " << rdf << std::endl;
                    if (rf ==0.0 || rdf==0.0)
                        {
                            return 0;
                        }
                    t2 = t - rf / rdf;
                    if (std::abs(t2-t) < ep)
                        {
                            t=t2;
                            break;
                        }else{
                            t=t2;
                            continue;
                        }
                        return t2;
                }
         return t2;
        }
};

// 動作テスト用
double testMethod(double t, Param _param)
{
    double result;
    result = (_param.x*_param.x*t / (_param.y/2)) *(t*t);
    return result;
};

int main()
{
    double t;
    Param test_param;
    test_param.t = 0.0;
    test_param.x = 3.0;
    test_param.y = 4.0;
    Newton nt;
    t = nt.newton(0.001, 200.34, testMethod, test_param, 1000000000, 0.0001);
    std::cout<<"result:  " <<testMethod(t, test_param)<< std::endl;
    std::cout<<"t:  " <<t<< std::endl;
    return 0;
}


#include <iostream>
#include <numeric>
#include <type.hpp>

#include <Eigen/Eigen>

#include "FEM/FEM.hpp"
#include "imgui/implot.h"
#include "Visualization/Visualizer.h"

static Float LinearInnerProduct(Float xmin, Float x_mid, Float xmax)
{
	return ((-2 + x_mid * x_mid + xmax - x_mid * (1 + xmax)) * cos(x_mid) + 2 * cos(xmax) + (1 - 2 * x_mid + xmax) * sin(x_mid) + (-1 + xmax) * sin(xmax))
		/ (x_mid - xmax) + 1 / (x_mid - xmin) * ((2 + x_mid - x_mid * x_mid + (-1 + x_mid) * xmin) * cos(x_mid) - 2 * cos(xmin) - (1 - 2 * x_mid + xmin) * sin(x_mid) + sin(xmin) - xmin * sin(xmin));
}

static Float LinearFunc(Float x, Float x0, Float x_left, Float x_right)
{
	if (x<x_left || x>x_right)
	{
		return 0;
	}
	if (x < x0)
	{
		return (x - x_left) / (x0 - x_left);
	}
	else return (x - x_right) / (x0 - x_right);
}

class LinearBaseFEM1D :public StaticFEM1D
{
public:
	LinearBaseFEM1D(size_t size) : StaticFEM1D(), size(size)
	{
		mesh.resize(size);
		//A default devision is provided
		Float h = 1.0 / (size + 1);
		for (int i = 0; i < size; ++i)
		{
			mesh[i] = (i + 1) * h;
		}
	}

	std::vector<Float> mesh;

	Float Value(Float x) override
	{
		Float ret = 0;
		for (int i = 0; i < mat_size; ++i)
		{
			ret += LinearFunc(x, MeshValue(i), MeshValue(i - 1), MeshValue(i + 1)) * coeff_()(i);
		}
		return ret;
	}

private:

	//return the indices of the functions related to the current function
	std::vector<int> RelatedFuncIdx(int idx) override
	{
		std::vector<int> ret;
		ret.push_back(idx);
		int left = idx - 1, right = idx + 1;
		if (0 <= left && left < mat_size)
		{
			ret.push_back(left);
		}
		if (0 <= right && right < mat_size)
		{
			ret.push_back(right);
		}
		return ret;
	}

	Float MeshValue(int index)
	{
		if (index < 0)
		{
			return 0;
		}
		if (index >= mat_size)
		{
			return  1.0;
		}
		return mesh[index];
	}

	Float GradientInnerProduct(int i, int j) override
	{
		if (i == j)
		{
			return 1.0 / (MeshValue(i) - MeshValue(i - 1)) + 1.0 / (MeshValue(i + 1) - MeshValue(i));
		}
		else if (abs(i - j) == 1)
		{
			return -1.0 / abs(MeshValue(i) - MeshValue(j));
		}
		else return 0;//For robustness
	}

	Float RHSInnerProduct(int i) override
	{
		return LinearInnerProduct(MeshValue(i - 1), MeshValue(i), MeshValue(i + 1));
	}

protected:

	int size;

	Float SelfInnerProduct(int i, int j) override
	{
		return 0;
	}

	Float GradientSelfInnerProduct(int i, int j) override
	{
		return 0;
	}

	void SetMatSize() override
	{
		mat_size = size;
	}
};

static Float Quadratic(Float x, Float x_left, Float x_middle, Float x_right)
{
	if (x <= x_left || x >= x_right)
	{
		return 0.0;
	}

	return   (x - x_left) * (x - x_right) / (x_middle - x_left) / (x_middle - x_right);
}

static Float QuadraticInnerProduct(Float xmin, Float xmid, Float xmax)
{
	//return (2. * (1. - 2. * xmax + xmin) * cos(xmax) - 2. * (1. + xmax - 2. * xmin) * cos(xmin) + 6. * (sin(xmax) - sin(xmin)) - (xmax - xmin) * (xmax*sin(xmax) - sin(xmax) + xmin*sin(xmin) - sin(xmin))) / (xmax - xmid) / (xmid - xmin);
	return  (2 * cos(xmax) - 4 * xmax * cos(xmax) + 2 * xmin * cos(xmax) - 2 * cos(xmin) - 2 * xmax * cos(xmin) +
		4 * xmin * cos(xmin) + 6 * sin(xmax) + xmax * sin(xmax) - (xmax * xmax) * sin(xmax) -
		xmin * sin(xmax) + xmax * xmin * sin(xmax) - 6 * sin(xmin) + xmax * sin(xmin) -
		xmin * sin(xmin) - xmax * xmin * sin(xmin) + (xmin * xmin) * sin(xmin)) /
		((xmax - xmid) * (xmid - xmin));
}

static Float QuadraticGradientInnerProduct(Float xmin, Float xmid, Float xmax, Float xmin2, Float xmid2, Float xmax2)
{
	auto left = std::max(xmin, xmin2);
	auto right = std::min(xmax, xmax2);

	if (left > right)
	{
		return  0;
	}
	Float ret = -((left - right) * (4 * (left * left) + 4 * left * right + 4 * (right * right) - 3 * left * xmax -
		3 * right * xmax - 3 * left * xmax2 - 3 * right * xmax2 + 3 * xmax * xmax2 - 3 * left * xmin -
		3 * right * xmin + 3 * xmax2 * xmin - 3 * left * xmin2 - 3 * right * xmin2 + 3 * xmax * xmin2 +
		3 * xmin * xmin2)) / 3. / (xmax - xmid) / (xmax2 - xmid2) / (xmid - xmin) / (xmid2 - xmin2);

	return ret;
}

class QuadraticBaseFEM1D :public StaticFEM1D
{
public:
	QuadraticBaseFEM1D(size_t size) : StaticFEM1D(), mesh_size(size), mat_size_(2 * size + 1)
	{
		mesh.resize(size);
		//A default division is provided
		Float h = 1.0 / (size + 1);
		for (int i = 0; i < size; ++i)
		{
			mesh[i] = (i + 1) * h;
		}
	}

	std::vector<Float> mesh;
	size_t mesh_size;
	size_t mat_size_;

	Float Value(Float x) override
	{
		Float ret = 0;
		for (int i = 0; i < mat_size; ++i)
		{
			Float xmin, xmid, xmax;
			idx_to_mesh(i, xmin, xmid, xmax);

			ret += Quadratic(x, xmin, xmid, xmax) * coeff_()(i);
		}
		return ret;
	}

private:

	//return the indices of the functions related to the current function
	std::vector<int> RelatedFuncIdx(int idx) override
	{
		std::vector<int> ret;
		if (idx < mesh_size)
		{
			ret.push_back(idx);
			int left = idx - 1, right = idx + 1;
			if (0 <= left && left < mesh_size)
			{
				ret.push_back(left);
			}
			if (0 <= right && right < mesh_size)
			{
				ret.push_back(right);
			}
			ret.push_back(idx + mesh_size);

			ret.push_back(idx + mesh_size + 1);
		}
		else
		{
			ret.push_back(idx);
			int left = idx - mesh_size - 1, right = idx - mesh_size;

			if (0 <= left && left < mesh_size)
			{
				ret.push_back(left);
			}
			if (0 <= right && right < mesh_size)
			{
				ret.push_back(right);
			}
		}
		return ret;
	}

	Float MeshValue(int index)
	{
		if (index < 0)
		{
			return 0;
		}
		if (index >= mesh_size)
		{
			return  1.0;
		}
		return mesh[index];
	}

	void idx_to_mesh(int idx, Float& xmin, Float& xmid, Float& xmax)
	{
		if (idx < mesh_size)
		{
			xmin = MeshValue(idx - 1);	xmid = MeshValue(idx);	xmax = MeshValue(idx + 1);
		}
		else
		{
			xmin = MeshValue(idx - 1 - mesh_size);
			xmid = (MeshValue(idx - mesh_size) + MeshValue(idx - 1 - mesh_size)) / 2.0;
			xmax = MeshValue(idx - mesh_size);
		}
	}

	Float GradientInnerProduct(int i, int j) override
	{
		Float xmin, xmid, xmax;
		Float xmin2, xmid2, xmax2;

		idx_to_mesh(i, xmin, xmid, xmax);
		idx_to_mesh(j, xmin2, xmid2, xmax2);

		return QuadraticGradientInnerProduct(xmin, xmid, xmax, xmin2, xmid2, xmax2);
	}

	Float RHSInnerProduct(int i) override
	{
		Float xmin, xmid, xmax;

		idx_to_mesh(i, xmin, xmid, xmax);
		return QuadraticInnerProduct(xmin, xmid, xmax);
	}

protected:
	Float SelfInnerProduct(int i, int j) override
	{
		return 0;
	}

	Float GradientSelfInnerProduct(int i, int j) override
	{
		return 0;
	}

	void SetMatSize() override
	{
		mat_size = mat_size_;
	}
};

class FEM1DVisualizer :public Visualizer
{
protected:

	void evaluate()
	{
		int segement_ = segemnt;
		LinearBaseFEM1D linear(segement_);
		QuadraticBaseFEM1D quadratic(segement_ / 2);
		linear.evaluate();
		quadratic.evaluate();
		for (int i = 0; i < Length; ++i)
		{
			quadratic_val[i] = quadratic.Value(1.0 / (Length - 1) * i);
			linear_val[i] = linear.Value(1.0 / (Length - 1) * i);

			quadratic_diff[i] = quadratic_val[i] - precise_val[i];
			linear_diff[i] = linear_val[i] - precise_val[i];
		}
	}

	void AddPoint();
	void DragPoint();
	void draw(bool* p_open) override;

	std::vector<Point2d> points;
	bool updated = true;
	int segemnt = 1;

	void error(std::vector<Float>& ref, std::vector<Float>& eval, Float& L_1, Float& L_2, Float& L_inf)
	{
		assert(ref.size() == eval.size());

		std::vector<Float> minus(ref.size());

		for (int i = 0; i < ref.size(); ++i)
		{
			minus[i] = abs(ref[i] - eval[i]);
		}

		L_inf = *std::max_element(minus.begin(), minus.end(), [](Float a, Float b) {return a < b; });
		L_2 = sqrt(std::accumulate(minus.begin(), minus.end(), static_cast<Float>(0), [](Float r, Float a) {return r + a * a; }) / Float(ref.size()));
		L_1 = std::accumulate(minus.begin(), minus.end(), static_cast<Float>(0), [](Float r, Float a) {return r + a; }) / Float(ref.size());
	}

public:
	FEM1DVisualizer() {
		Float h = 1.0 / (Length - 1);
		for (int i = 0; i < Length; ++i)
		{
			xs[i] = i * h;
			rhs_f[i] = (xs[i] - 1) * sin(xs[i]);
			precise_val[i] = -(2 - 2 * xs[i] + 2 * xs[i] * cos(1) - 2 * cos(xs[i]) + sin(xs[i]) - xs[i] * sin(xs[i]));
		}
		evaluate();

		Float L1, L2, L_inf;

		segemnt = 1;
		do
		{
			evaluate();

			error(precise_val, linear_val, L1, L2, L_inf);

			pointcount.push_back(segemnt);
			linear_vec_L_1.push_back(L1);
			linear_vec_L_2.push_back(L2);
			linear_vec_L_inf.push_back(L_inf);
			error(precise_val, quadratic_val, L1, L2, L_inf);

			quadratic_vec_L_1.push_back(L1);
			quadratic_vec_L_2.push_back(L2);
			quadratic_vec_L_inf.push_back(L_inf);

			segemnt *= 2;
		} while (segemnt != 1024);
		segemnt = 1;
	}
	const size_t Length = 2001;

	std::vector<Float> xs = std::vector<Float>(Length);
	std::vector<Float> rhs_f = std::vector<Float>(Length);
	std::vector<Float> precise_val = std::vector<Float>(Length);
	std::vector<Float> quadratic_val = std::vector<Float>(Length);
	std::vector<Float> linear_val = std::vector<Float>(Length);
	std::vector<Float> quadratic_diff = std::vector<Float>(Length);
	std::vector<Float> linear_diff = std::vector<Float>(Length);

	std::vector<Float> pointcount;
	std::vector<Float> linear_vec_L_1;
	std::vector<Float> linear_vec_L_2;
	std::vector<Float> linear_vec_L_inf;

	std::vector<Float> quadratic_vec_L_1;
	std::vector<Float> quadratic_vec_L_2;
	std::vector<Float> quadratic_vec_L_inf;
};

void FEM1DVisualizer::AddPoint()
{
	if (ImGui::IsMouseClicked(ImGuiMouseButton_Right))
	{
		auto pos = ImPlot::GetPlotMousePos();
		points.emplace_back(pos.x, pos.y);
		updated = true;
	}
}

void FEM1DVisualizer::DragPoint()
{
	for (int i = 0; i < points.size(); ++i)
	{
		auto& point = points[i];
		updated |= ImPlot::DragPoint(("Point " + std::to_string(i)).c_str(), &point.x(), &point.y());
	}
}
static inline ImVec2 operator-(const ImVec2& lhs, const ImVec2& rhs) { return ImVec2(lhs.x - rhs.x, lhs.y - rhs.y); }
void FEM1DVisualizer::draw(bool* p_open)
{
	if (ImGui::BeginTabBar("Homework 1")) {
		if (ImGui::BeginTabItem("FEM1D"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				//ImPlot::PlotLine("u", &xs[0], &ys1[0], Length);
				ImPlot::PlotLine("Precise solution", &xs[0], &precise_val[0], Length);
				//ImPlot::PlotLine("FEM Result", &xs[0], &ys3[0], Length);
				ImPlot::PlotLine("FEM Quadratic", &xs[0], &quadratic_val[0], Length);
				ImPlot::PlotLine("FEM Linear", &xs[0], &linear_val[0], Length);

				ImPlot::EndPlot();
			}
			if (ImGui::SliderInt("Number of segments", &segemnt, 1, 512))
			{
				evaluate();
			}
			ImGui::EndTabItem();
		}
		if (ImGui::BeginTabItem("FEM1D difference"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				//ImPlot::PlotLine("u", &xs[0], &ys1[0], Length);
				//ImPlot::PlotLine("FEM Result", &xs[0], &ys3[0], Length);
				ImPlot::PlotLine("FEM diff Linear", &xs[0], &linear_diff[0], Length);
				ImPlot::PlotLine("FEM diff Quadratic", &xs[0], &quadratic_diff[0], Length);

				ImPlot::EndPlot();
			}
			if (ImGui::SliderInt("Number of segments", &segemnt, 1, 512))
			{
				segemnt = segemnt < 1 ? 1 : segemnt;
				evaluate();
			}
			ImGui::EndTabItem();
		}

		if (ImGui::BeginTabItem("Error Plot"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus, ImPlotAxisFlags_LogScale, ImPlotAxisFlags_LogScale)) {
				//ImPlot::PlotLine("u", &xs[0], &ys1[0], Length);
				//ImPlot::PlotLine("FEM Result", &xs[0], &ys3[0], Length);
				ImPlot::PlotLine("Linear L1    Error", &pointcount[0], &linear_vec_L_1[0], pointcount.size());
				ImPlot::PlotLine("Linear L2    Error", &pointcount[0], &linear_vec_L_2[0], pointcount.size());
				ImPlot::PlotLine("Linear L_inf Error", &pointcount[0], &linear_vec_L_inf[0], pointcount.size());

				ImPlot::PlotLine("Quadratic L1    Error", &pointcount[0], &quadratic_vec_L_1[0], pointcount.size());
				ImPlot::PlotLine("Quadratic L2    Error", &pointcount[0], &quadratic_vec_L_2[0], pointcount.size());
				ImPlot::PlotLine("Quadratic L_inf Error", &pointcount[0], &quadratic_vec_L_inf[0], pointcount.size());

				ImPlot::EndPlot();
			}
			if (ImGui::SliderInt("Number of segments", &segemnt, 1, 100))
			{
				evaluate();
			}
			ImGui::EndTabItem();
		}
		ImGui::EndTabBar();
	}

	ImGui::End();
}

int main()
{
	FEM1DVisualizer visualizer;
	visualizer.RenderLoop();
}