#include <iostream>

#include <posit/posit>
#include <boost/range/combine.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

cpp_dec_float_100 decimal_accuracy(cpp_dec_float_100 exact, cpp_dec_float_100 computed) {
    if (boost::math::isnan(exact) || boost::math::isnan(computed) ||
        (boost::math::sign(exact) != boost::math::sign(computed))) {
        return std::numeric_limits<cpp_dec_float_100>::quiet_NaN();
    } else if (exact == computed) {
        return std::numeric_limits<cpp_dec_float_100>::infinity();
    } else if ((exact == std::numeric_limits<cpp_dec_float_100>::infinity() &&
                computed != std::numeric_limits<cpp_dec_float_100>::infinity()) ||
               (exact != std::numeric_limits<cpp_dec_float_100>::infinity() &&
                computed == std::numeric_limits<cpp_dec_float_100>::infinity()) || (exact == 0 && computed != 0) ||
               (exact != 0 && computed == 0)) {
        return -std::numeric_limits<cpp_dec_float_100>::infinity();
    } else {
        return -log10(abs(log10(computed / exact)));
    }
}

int main() {

    ofstream outfile("da.txt", ios::out);
    outfile << "Q,da_f,da_p2,da_p3" << endl;

    for(int Q = 1; Q <= 100; Q++) {
        cpp_dec_float_100 dec = pow(10.0, -(cpp_dec_float_100)Q/10);

        float f = powl(10.0, -(long double)Q/10);
        posit<32,2> p2 = powl(10.0, -(long double)Q/10);
        posit<32,3> p3 = powl(10.0, -(long double)Q/10);

        cpp_dec_float_100 da_f, da_p2, da_p3;
        da_f = decimal_accuracy(dec, static_cast<cpp_dec_float_100>(f));
        da_p2 = decimal_accuracy(dec, static_cast<cpp_dec_float_100>(p2));
        da_p3 = decimal_accuracy(dec, static_cast<cpp_dec_float_100>(p3));

        outfile << Q << ",";
        outfile << setprecision(100) << fixed << da_f << "," << da_p2 << "," << da_p3 << endl << flush;
    }
    outfile.close();

}


#ifndef STAN_MATH_PRIM_SCAL_FUN_LOGIT_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOGIT_HPP

#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {

/**
 * Return the log odds of the argument.
 *
 * The logit function is defined as for \f$x \in [0, 1]\f$ by
 * returning the log odds of \f$x\f$ treated as a probability,
 *
 * \f$\mbox{logit}(x) = \log \left( \frac{x}{1 - x} \right)\f$.
 *
 * The inverse to this function is inv_logit.
 *
 *
 \f[
 \mbox{logit}(x) =
 \begin{cases}
 \textrm{NaN}& \mbox{if } x < 0 \textrm{ or } x > 1\\
 \ln\frac{x}{1-x} & \mbox{if } 0\leq x \leq 1 \\[6pt]
 \textrm{NaN} & \mbox{if } x = \textrm{NaN}
 \end{cases}
 \f]

 \f[
 \frac{\partial\, \mbox{logit}(x)}{\partial x} =
 \begin{cases}
 \textrm{NaN}& \mbox{if } x < 0 \textrm{ or } x > 1\\
 \frac{1}{x-x^2}& \mbox{if } 0\leq x\leq 1 \\[6pt]
 \textrm{NaN} & \mbox{if } x = \textrm{NaN}
 \end{cases}
 \f]
 *
 * @param u argument
 * @return log odds of argument
 */
inline double logit(double u) {
  using std::log;
  return log(u / (1 - u));
}

/**
 * Return the log odds of the argument.
 *
 * @param u argument
 * @return log odds of argument
 */
inline double logit(int u) { return logit(static_cast<double>(u)); }

}  // namespace math
}  // namespace stan
#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_STATISTICS_FUNCTIONS_GENERIC_EVSTAT_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_GENERIC_EVSTAT_HPP_INCLUDED
#include <nt2/statistics/functions/evstat.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/include/constants/euler.hpp>
#include <nt2/include/constants/oneo_6.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/functions/sqr.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT   ( evstat_, tag::cpu_
                                    , (A0)(A1)(A2)(A3)
                                    , (scalar_ < floating_<A0> > )
                                      (scalar_ < floating_<A1> > )
                                      (scalar_ < floating_<A2> > )
                                      (scalar_ < floating_<A3> > )
                                    )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()( A0 const& mu, A1 const& sigma
                                            , A2 & m, A3 & v) const
    {
      m = mu-Euler<A0>()*sigma;
      v = sqr(Pi<A0>()*sigma)*Oneo_6<A0>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT   ( evstat_, tag::cpu_
                                    , (A0)(A1)(A2)
                                    , (scalar_ < floating_<A0> > )
                                      (scalar_ < floating_<A1> > )
                                      (scalar_ < floating_<A2> > )
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()( A0 const& mu, A1 const& sigma
                                            , A2 & v) const
    {
      v = sqr(Pi<result_type>()*sigma)*Oneo_6<result_type>();
      return mu-Euler<result_type>()*sigma;
    }
  };

  BOOST_DISPATCH_IMPLEMENT   ( evstat_, tag::cpu_
                                    , (A0)(A1)
                                    , (scalar_ < floating_<A0> > )
                                      (scalar_ < floating_<A1> > )
                                    )
  {
    typedef std::pair<A0,A0> result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& mu, A1 const& sigma) const
    {
      return result_type(mu-Euler<A0>()*sigma
                        , sqr(Pi<A0>()*sigma)*Oneo_6<A0>());
    }
  };

} }

#endif


/*!
 * \file khovanov.cpp
 * \author Jun Yoshida
 * \copyright (c) 2020 Jun Yoshida.
 * The project is released under the 2-clause BSD License.
 * \date August, 2020: created
 */

#include <vector>

#include <Eigen/Dense>

#include "khovanov.hpp"
#include "cubes.hpp"
#include "enhancements.hpp"

/* Debug
#include "debug/debug.hpp"
//*/

using namespace khover;

using matrix_t = ChainIntegral::matrix_t;

/***********************************
 *** Implementation of functions ***
 ***********************************/
// Compute Khovanov complex of a given link diagram.
std::optional<ChainIntegral>
khover::khChain(
    LinkDiagram const& diagram,
    SmoothCube const& cube,
    std::vector<EnhancementProperty> const& enh_prop
    //int qdeg
    ) noexcept
{
    // Compute the matrices representing matrices.
    ChainIntegral result(
        -diagram.npositive()-1,
        matrix_t(
            0, binom(cube.back().ncomp, enh_prop.back().xcnt)));
    for(int i = cube.dim(); i > 0; --i) {
        std::size_t maxst_cod = cube.maxState(i).to_ulong();
        std::size_t maxst_dom = cube.maxState(i-1).to_ulong();

        matrix_t diffmat = matrix_t::Zero(
            enh_prop[maxst_cod].headidx
            + binom(cube[maxst_cod].ncomp, enh_prop[maxst_cod].xcnt),
            enh_prop[maxst_dom].headidx
            + binom(cube[maxst_dom].ncomp, enh_prop[maxst_dom].xcnt));

        // Traverse all the state pairs
        for(std::size_t stidx_cod = 0;
            stidx_cod < binom(cube.dim(), i);
            ++stidx_cod)
        {
            // The state associated with the index.
            auto st_cod = bitsWithPop<max_crosses>(i, stidx_cod).to_ullong();

            for(std::size_t stidx_dom = 0;
                stidx_dom < binom(cube.dim(), i-1);
                ++stidx_dom)
            {
                // The state associated with the index.
                auto st_dom = bitsWithPop<max_crosses>(i-1, stidx_dom).to_ullong();

                // The coefficient between the domain/codomain states.
                auto coeff = diagram.stateCoeff(
                    state_t{st_dom}, state_t{st_cod});

                // Skip the case where the codomain state is not adjacent to the domain state.
                if(coeff == 0)
                    continue;

                // Find the position where saddle operation is applied.
                for(std::size_t arc = 0; arc < diagram.narcs(); ++arc) {
                    // The saddle causes multiplication.
                    if(cube[st_cod].arccomp[arc] < cube[st_dom].arccomp[arc])
                    {
                        for(auto [r,c] : matrix_mult(
                                enh_prop[st_cod].xcnt,
                                cube[st_cod].ncomp,
                                cube[st_cod].arccomp[arc],
                                cube[st_dom].arccomp[arc])) {
                            diffmat.coeffRef(
                                enh_prop[st_cod].headidx+r,
                                enh_prop[st_dom].headidx+c
                                ) += coeff;
                        }
                        break;
                    }
                    // The saddle causes comultiplication
                    else if(cube[st_cod].arccomp[arc]
                            > cube[st_dom].arccomp[arc])
                    {
                        for(auto [r,c] : matrix_comult(
                                enh_prop[st_cod].xcnt,
                                cube[st_cod].ncomp,
                                cube[st_dom].arccomp[arc],
                                cube[st_cod].arccomp[arc]))
                        {
                            diffmat.coeffRef(
                                enh_prop[st_cod].headidx+r,
                                enh_prop[st_dom].headidx+c
                                ) += coeff;
                        }
                        break;
                    }
                }
            }
        }

        // Append the matrix as a differential.
        if (!result.prepend(std::move(diffmat))) {
            std::cerr << "Wrong size matrix..." << std::endl;
            return std::nullopt;
        }
    }

    return std::make_optional(std::move(result));
}

// Compute crux complex of a given link diagram and a given crossing.
std::optional<ChainIntegral>
khover::cruxChain(
    LinkDiagram const& diagram,
    std::size_t dblpt,
    CruxCube const& cube,
    std::vector<EnhancementProperty> const& enh_prop
    //int qdeg
    ) noexcept
{
    // The double point is out-of-range.
    if(dblpt >= diagram.ncrosses()) {
        return std::nullopt;
    }

    // Compute the matrices representing matrices.
    ChainIntegral result(
        -diagram.npositive() - (diagram.getSign(dblpt) > 0 ? 0 : 1),
        matrix_t(0, binom(cube.back().ncomp, enh_prop.back().xcnt))
        );

    for(int i = diagram.ncrosses()-1; i > 0; --i) {
        std::size_t maxst_cod = cube.maxState(i).to_ulong();
        std::size_t maxst_dom = cube.maxState(i-1).to_ulong();
        matrix_t diffmat = matrix_t::Zero(
            enh_prop[maxst_cod].headidx
            + binom(cube[maxst_cod].ncomp, enh_prop[maxst_cod].xcnt),
            enh_prop[maxst_dom].headidx
            + binom(cube[maxst_dom].ncomp, enh_prop[maxst_dom].xcnt)
            );

        // Traverse all the state pairs
        for(std::size_t stidx_cod = 0;
            stidx_cod < binom(diagram.ncrosses()-1, i);
            ++stidx_cod)
        {
            // The state associated with the index.
            auto stbits_cod = bitsWithPop<max_crosses>(i, stidx_cod);
            auto st_cod = stbits_cod.to_ulong();

            // Skip states that has no enhancement in the q-degree.
            if(enh_prop[st_cod].xcnt < 0
               || (enh_prop[st_cod].xcnt
                   > static_cast<int>(cube[st_cod].ncomp)))
            {
                continue;
            }

            for(std::size_t c = 0; c < diagram.ncrosses()-1; ++c) {
                if(!stbits_cod.test(c))
                    continue;

                auto stbits_dom = stbits_cod;
                stbits_dom.set(c,false);
                auto st_dom = stbits_dom.to_ulong();

                // Skip states that has no enhancement in the q-degree.
                if(enh_prop[st_dom].xcnt < 0
                   || (enh_prop[st_dom].xcnt
                       > static_cast<int>(cube[st_cod].ncomp)))
                {
                    continue;
                }

                // The coefficient between the domain/codomain states.
                auto coeff = diagram.stateCoeff(
                    cube[st_dom].state, cube[st_cod].state);

                // Skip the case where the codomain state is not adjacent to the domain state.
                if(coeff == 0)
                    continue;

                // Find the position where saddle operation is applied.
                for(std::size_t arc = 0; arc < diagram.narcs(); ++arc) {
                    // The saddle causes multiplication.
                    if(cube[st_cod].arccomp[arc]
                       < cube[st_dom].arccomp[arc])
                    {
                        // Enabled if the operation is involved with twisted arcs.
                        std::optional<std::size_t> action_arc;
                        if ((cube[st_dom].twist
                             ^ cube[st_cod].twist).any())
                        {
                            // In that case, action_arc is a non-twisted arc that acts on the twisted arc.
                            if(cube[st_dom].twist.test(arc))
                                action_arc = cube[st_cod].arccomp[arc];
                            else
                                action_arc = cube[st_dom].arccomp[arc];
                        }

                        for(auto [r,c] : matrix_mult(
                                enh_prop[st_cod].xcnt,
                                cube[st_cod].ncomp,
                                cube[st_cod].arccomp[arc],
                                cube[st_dom].arccomp[arc]))
                        {
                            // -1 if 'x' on the act circle.
                            diffmat.coeffRef(
                                enh_prop[st_cod].headidx+r,
                                enh_prop[st_dom].headidx+c
                                ) += action_arc && bitsWithPop<max_components>(
                                    enh_prop[st_dom].xcnt, c).test(*action_arc)
                                ? -coeff : coeff;
                        }
                        break;
                    }
                    // The saddle causes comultiplication
                    else if (cube[st_cod].arccomp[arc]
                             > cube[st_dom].arccomp[arc])
                    {
                        // Enabled if the operation is involved with twisted arcs.
                        std::optional<std::size_t> coact_arc;
                        if ((cube[st_dom].twist
                             ^ cube[st_cod].twist).any())
                        {
                            // In that case, coact_arc is a non-twisted arc that coacts on the twisted arc.
                            if(cube[st_cod].twist.test(arc))
                                coact_arc = cube[st_dom].arccomp[arc];
                            else
                                coact_arc = cube[st_cod].arccomp[arc];
                        }

                        for(auto [r,c] : matrix_comult(
                                enh_prop[st_cod].xcnt,
                                cube[st_cod].ncomp,
                                cube[st_dom].arccomp[arc],
                                cube[st_cod].arccomp[arc]))
                        {
                            // -1 if '1' on the coact circle.
                            diffmat.coeffRef(
                                enh_prop[st_cod].headidx+r,
                                enh_prop[st_dom].headidx+c
                                ) += coact_arc && bitsWithPop<max_components>(
                                    enh_prop[st_cod].xcnt, r).test(*coact_arc)
                                ? coeff : -coeff;
                        }
                        break;
                    }
                }
            }
        }

        // Append the matrix as a differential.
        if (!result.prepend(std::move(diffmat))) {
            std::cerr << "Wrong size matrix..." << std::endl;
            return std::nullopt;
        }
    }

    return std::optional<ChainIntegral>(std::move(result));
}


#ifndef REGISTRATION_COST_HPP
#define REGISTRATION_COST_HPP

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

struct LiTAMIN2CostFunction
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    LiTAMIN2CostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov,
                           Eigen::Matrix3d lambdaI,double sigma_ICP) :
                        p_mean_(p_mean), q_mean_(q_mean), p_cov_(p_cov), q_cov_(q_cov),lambdaI_(lambdaI),sigma_ICP_(sigma_ICP) {}

    template <typename T>
    bool operator()(const T *const q, const T *const t, T *residuals) const
    {
        Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(residuals);
        Eigen::Matrix<T, 3, 1> p_m(p_mean_.cast<T>());
        Eigen::Matrix<T, 3, 1> q_m(q_mean_.cast<T>());
        Eigen::Matrix<T, 3, 3> p_c = p_cov_.cast<T>();
        Eigen::Matrix<T, 3, 3> q_c = q_cov_.cast<T>();
        Eigen::Matrix<T, 3, 3> lambI = lambdaI_.cast<T>();
        Eigen::Quaternion<T> quat(q);
        Eigen::Matrix<T, 3, 1> translation(t);

        Eigen::Matrix<T, 3, 3> mahalanobis = (q_c + quat * p_c * (quat.inverse()) + lambdaI_).inverse();
        mahalanobis.normalize();
        Eigen::Matrix<T,3,3> LT = mahalanobis.llt().matrixL().transpose();
        residuals_map = LT * (q_m - (quat * p_m + translation));
        T EICP = T(residuals_map.squaredNorm());
        T sigma_square = T(sigma_ICP_*sigma_ICP_);
        residuals_map = (T(1.)-(EICP/(EICP+sigma_square)))*residuals_map;

        return true;
    }

    static ceres::CostFunction *Create(Eigen::Vector3d p_mean_, Eigen::Vector3d q_mean_, Eigen::Matrix3d p_cov_,
    Eigen::Matrix3d q_cov_,Eigen::Matrix3d lambdaI_, double sigma_ICP_){
        // 残差是三维的,变量分别是思维和三维
        return (new ceres::AutoDiffCostFunction<LiTAMIN2CostFunction,3,4,3>
        (new LiTAMIN2CostFunction(p_mean_,q_mean_,p_cov_,q_cov_,lambdaI_,sigma_ICP_)));
    }

    Eigen::Vector3d p_mean_, q_mean_;
    Eigen::Matrix3d p_cov_, q_cov_;
    Eigen::Matrix3d lambdaI_;
    double sigma_ICP_;
};
#endif


#include <NTL/ZZ.h>
#include <cstdint>
#include <cmath>

#define NUM_BITS_REAL_MANTISSA  128
#define IGNORE_DECODING_COST      0
#define SKIP_BJMM 0
#define LOG_COST_CRITERION 1

#include "proper_primes.hpp"
#include "binomials.hpp"
#include "bit_error_probabilities.hpp"
#include "partitions_permanents.hpp"
#include "isd_cost_estimate.hpp"
#include <cmath>

uint32_t estimate_t_val(const uint32_t c_sec_level,
                        const uint32_t q_sec_level,
                        const uint32_t n_0,
                        const uint32_t p){
    double achieved_c_sec_level = c_sec_level;
    double achieved_q_sec_level = q_sec_level;
    uint32_t lo = 1, t, t_prec;
    uint32_t hi;
    hi = p < 4*c_sec_level ? p : 4*c_sec_level;
    t = lo;
    t_prec = lo;
    while (hi - lo > 1){
       t_prec = t;
       t = (lo + hi)/2;
       std::cerr << "testing t " <<  t << std::endl;
       achieved_c_sec_level = c_isd_log_cost(n_0*p,((n_0-1)*p),t,p,0);
       achieved_q_sec_level = q_isd_log_cost(n_0*p,((n_0-1)*p),t,p,0);
       if ( (achieved_c_sec_level >= c_sec_level) &&
            (achieved_q_sec_level >= q_sec_level)    ){
         hi = t;
       } else {
         lo = t;
       }
    }
    if( (achieved_c_sec_level >= c_sec_level) &&
        (achieved_q_sec_level >= q_sec_level)    ){
        return t;
    }
    return t_prec;
}

int ComputeDvMPartition(const uint64_t d_v_prime,
                        const uint64_t n_0,
                        uint64_t mpartition[],
                        uint64_t &d_v){
     d_v = floor(sqrt(d_v_prime));
     d_v = (d_v & 0x01) ? d_v : d_v + 1;
     uint64_t m = ceil( (double) d_v_prime / (double) d_v );

     int partition_ok;
     partition_ok = FindmPartition(m,mpartition,n_0);

     while(!partition_ok  && (d_v_prime/d_v) >= n_0){
          d_v += 2;
          m = ceil( (double) d_v_prime / (double) d_v );
          partition_ok = FindmPartition(m,mpartition,n_0);
     }
     return partition_ok;
}

uint64_t estimate_dv (const uint32_t c_sec_level, // expressed as
                      const uint32_t q_sec_level,
                      const uint32_t n_0,
                      const uint32_t p,
                      uint64_t mpartition[]){
    double achieved_c_sec_level = 0.0;
    double achieved_q_sec_level = 0.0;
    double achieved_c_enum_sec_level = 0.0;
    double achieved_q_enum_sec_level = 0.0;

    NTL::ZZ keyspace;

    uint32_t lo = 1, d_v_prime, hi;
    uint64_t d_v,d_v_prec =0;
    int found_dv_mpartition = 0;
    // recalling that the weight of the sought codeword in a KRA is
    // d_c_prime = n_0 * d_v_prime, d_c_prime < p
    // d_v_prime should not be greater than p/n_0
    hi = (p/n_0) < 4*c_sec_level ? (p/n_0) : 4*c_sec_level;
    d_v_prime = lo;
    d_v = (int) sqrt(lo);

    while (hi - lo > 1){
       d_v_prec = d_v;
       d_v_prime = (lo + hi)/2;
       found_dv_mpartition = ComputeDvMPartition(d_v_prime,n_0,mpartition,d_v);
       if(found_dv_mpartition) {
          keyspace = 1;
          for(int i =0; i < (int)n_0; i++){
              keyspace *= binomial_wrapper(p,mpartition[i]);
          }
          keyspace = NTL::power(keyspace,n_0);
          keyspace += NTL::power(binomial_wrapper(p,d_v),n_0);
          achieved_c_enum_sec_level = NTL::conv<double>(log2_RR(NTL::to_RR(keyspace)));
          achieved_q_enum_sec_level = achieved_c_enum_sec_level/2;
          if ((achieved_q_enum_sec_level >= q_sec_level) &&
              (achieved_c_enum_sec_level >= c_sec_level) ){
              /* last parameter indicates a KRA, reduce margin by p due to
              quasi cyclicity */
              achieved_c_sec_level = c_isd_log_cost(n_0*p,p,n_0*d_v_prime,p,1);
              achieved_q_sec_level = q_isd_log_cost(n_0*p,p,n_0*d_v_prime,p,1);
          }
       }

       if ( (found_dv_mpartition) &&
            (achieved_q_enum_sec_level >= q_sec_level) &&
            (achieved_c_enum_sec_level >= c_sec_level) &&
            (achieved_c_sec_level >= c_sec_level) &&
            (achieved_q_sec_level >= q_sec_level)   ){
          hi = d_v_prime;
       } else {
          lo = d_v_prime;
       }
    }

    if ( (found_dv_mpartition) &&
            (achieved_q_enum_sec_level >= q_sec_level) &&
            (achieved_c_enum_sec_level >= c_sec_level) &&
            (achieved_c_sec_level >= c_sec_level) &&
            (achieved_q_sec_level >= q_sec_level)   ){
        return d_v;
    }
    return d_v_prec;
}

int main(int argc, char* argv[]){

  if(argc != 6){
     std::cout << "Code Parameter Computer for LEDA[kem|pkc]" << std::endl << " Usage "
               << argv[0] << " security_level_classic security_level_pq n_0 epsilon starting_prime_lb" << std::endl;
    return -1;
  }
  uint32_t c_sec_level = atoi(argv[1]);
  uint32_t q_sec_level = atoi(argv[2]);
  uint32_t n_0 = atoi(argv[3]);
  float epsilon = atof(argv[4]);
  uint32_t starting_prime_lower_bound = atoi(argv[5]);

  std::cerr << "Computing the parameter set for security level classic:2^" <<
  c_sec_level << " post-q:2^" << q_sec_level << " n_0 " << n_0 << " epsilon "
  << epsilon << std::endl;

  uint64_t p, p_th, t, d_v_prime, d_v;
  uint64_t mpartition[n_0] = {0};

  int current_prime_pos = 0;
  while (proper_primes[current_prime_pos] < starting_prime_lower_bound){
          current_prime_pos++;
  }
  p_th = proper_primes[current_prime_pos];

  InitBinomials();
  NTL::RR::SetPrecision(NUM_BITS_REAL_MANTISSA);
  pi = NTL::ComputePi_RR();

  /* since some values of p may yield no acceptable partitions for m, binary
   * search on p is not feasible. Fall back to fast increase of the value of
   * p exploiting the value of the p expected to be correcting the required t
   * errors */

  std::cout << "finding parameters" << std::endl;
  do {
      /* estimate the current prime as the closest
       * to the previous p_th * (1+epsilon) */
      uint32_t next_prime = ceil(p_th * (1.0+epsilon));
      current_prime_pos = 0;
      while (proper_primes[current_prime_pos] < next_prime){
          current_prime_pos++;
      }
      p = proper_primes[current_prime_pos];
      std::cout << " -- testing p: " << p << std::endl;

      // Estimate number of errors to ward off ISD decoding
      t = estimate_t_val(c_sec_level,q_sec_level,n_0,p);
      std::cout << " -- found t: " << t << std::endl;

      /* Estimate H*Q density to avoid key recovery via ISD and enumeration
       * of H and Q */
      d_v = estimate_dv(c_sec_level,q_sec_level,n_0,p,mpartition);
      std::cout << " -- found d_v: " << d_v << std::endl;

      // Estimate the bit flipping thresholds and correction capability
      d_v_prime=0;
      for(int i=0;i< (int)n_0;i++){
         d_v_prime += mpartition[i];
      }
      d_v_prime = d_v * d_v_prime;
      p_th=Findpth(n_0, d_v_prime, t);
      std::cout << " -- p should be at least " << (1.0+epsilon)* p_th <<
         "to correct the errors" << std::endl;
  }  while ((p <= (1.0+epsilon)* p_th) &&
            (current_prime_pos < PRIMES_NO) );

  std::cout << "refining parameters" << std::endl;

  uint64_t p_ok, t_ok, d_v_ok, mpartition_ok[n_0] = {0};
  /* refinement step taking into account possible invalid m partitions */

  do {
      p = proper_primes[current_prime_pos];
      std::cout << " -- testing p: " << p << std::endl;

      // Estimate number of errors to ward off ISD decoding
      t = estimate_t_val(c_sec_level,q_sec_level,n_0,p);
      std::cout << " -- found t: " << t << std::endl;

      /* Estimate H*Q density to avoid key recovery via ISD and enumeration
       * of H and Q */
      d_v = estimate_dv(c_sec_level,q_sec_level,n_0,p,mpartition);
      std::cout << " -- found d_v: " << d_v << std::endl;

      // Estimate the bit flipping thresholds and correction capability
      d_v_prime=0;
      for(int i=0;i< (int)n_0;i++){
         d_v_prime += mpartition[i];
      }
      d_v_prime = d_v * d_v_prime;
      p_th=Findpth(n_0, d_v_prime, t);
       std::cout << " -- the threshold value for p to be correcting errors is " << p_th << std::endl;

      if(p > (1.0+epsilon)* p_th ) { //store last valid parameter set
       std::cout << " -- p is at least " << (1.0+epsilon)*p_th <<
       "; it corrects the errors" << std::endl;
           p_ok = p; t_ok = t; d_v_ok = d_v;
           for(unsigned i = 0; i < n_0 ; i++){
               mpartition_ok[i] = mpartition[i];
          }
      }
      current_prime_pos--;
  }  while ((p > (1.0+epsilon)* p_th )  && (current_prime_pos > 0));

  std::cout << "parameter set found: p:" << p_ok << " t: " << t_ok;
  std::cout << " d_v : " << d_v_ok << " mpartition: [ ";
  for (unsigned i = 0; i < n_0 ; i++ ){
   std::cout << mpartition_ok[i] << " ";
  }
  std::cout << " ]" << std::endl;
  return 0;
}


#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <rr_common/CameraGeometry.h>
#include <sensor_msgs/Image.h>

#include <boost/algorithm/string.hpp>
#include <cmath>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;
using namespace ros;

double px_per_meter;           // resolution of overhead view
double camera_dist_max;        // distance to look ahead of the car
double camera_dist_min;        // avoid the front bumper
double camera_fov_horizontal;  // radians
double camera_fov_vertical;
double cam_mount_angle;   // angle of camera from horizontal
double cam_mount_height;  // camera height from ground in meters
double cam_mount_x;       // distance from camera to base_footprint

Size mapSize;  // pixels = cm
Size imageSize;
Mat transform_matrix;

map<string, Publisher> transform_pubs;

/*
 * Start with a horizontal line on the groud at dmin meters horizonally in front of the
 * camera. It fills half the camera's FOV, from the center to the right edge. Then back
 * up the car so that the line is dmax meters away horizontally from the camera. The
 * apparent length of this hypothetical line (in pixels) is the output of this function.
 * See https://www.desmos.com/calculator/jsofcq1bi5
 * See https://drive.google.com/file/d/0Bw7-7Y3CUDw1Z0ZqdmdRZ3dUTE0/view?usp=sharing
 */
double pxFromDist_X(double dmin, double dmax) {
    double min_hyp = sqrt(dmin * dmin + cam_mount_height * cam_mount_height);
    double max_hyp = sqrt(dmax * dmax + cam_mount_height * cam_mount_height);
    double theta1 = atan((min_hyp / max_hyp) * tan(camera_fov_horizontal / 2));
    return imageSize.width * (theta1 / camera_fov_horizontal);
}

// calculate the y coord of the input image from the specified distance
// see https://www.desmos.com/calculator/pwjwlnnx77
double pxFromDist_Y(double dist) {
    double tmp = atan(cam_mount_height / dist) - cam_mount_angle + camera_fov_vertical / 2;
    return imageSize.height * tmp / (camera_fov_vertical);
}

void setTransformFromGeometry() {
    // set width and height of the rectangle in front of the robot
    // the actual output image will show more than this rectangle
    float close_corner_dist = sqrt(pow(camera_dist_min, 2) + pow(cam_mount_height, 2));
    float rectangle_w = close_corner_dist * tan(camera_fov_horizontal / 2) * px_per_meter * 2;
    float rectangle_h = (camera_dist_max - camera_dist_min) * px_per_meter;

    // find coordinates for corners above rectangle in input image
    float x_top_spread = pxFromDist_X(camera_dist_min, camera_dist_max);
    float y_bottom = pxFromDist_Y(camera_dist_min);
    float y_top = pxFromDist_Y(camera_dist_max);

    // set the ouput image size to include the whole transformed image,
    // not just the target rectangle
    mapSize.width = static_cast<int>(rectangle_w * (imageSize.width / x_top_spread) / 2.0);
    mapSize.height = static_cast<int>(rectangle_h);

    Point2f src[4] = {
        Point2f(imageSize.width / 2.f - x_top_spread, y_top),  // top left
        Point2f(imageSize.width / 2.f + x_top_spread, y_top),  // top right
        Point2f(0, y_bottom),                                  // bottom left
        Point2f(imageSize.width, y_bottom)                     // bottom right
    };

    Point2f dst[4] = {
        Point2f(mapSize.width / 2.f - rectangle_w / 2.f, 0),               // top left
        Point2f(mapSize.width / 2.f + rectangle_w / 2.f, 0),               // top right
        Point2f(mapSize.width / 2.f - rectangle_w / 2.f, mapSize.height),  // bottom left
        Point2f(mapSize.width / 2.f + rectangle_w / 2.f, mapSize.height)   // bottom right
    };

    transform_matrix = getPerspectiveTransform(src, dst);
}

void TransformImage(const sensor_msgs::ImageConstPtr& msg, string& topic) {
    // if no one is listening or the transform is undefined, give up
    if (transform_pubs[topic].getNumSubscribers() == 0 || transform_matrix.empty()) {
        return;
    }

    cv_bridge::CvImagePtr cv_ptr;
    cv_ptr = cv_bridge::toCvCopy(msg, "mono8");
    const Mat& inimage = cv_ptr->image;

    Mat warp_img;
    warpPerspective(inimage, warp_img, transform_matrix, mapSize);

    double map_length = camera_dist_max + cam_mount_x;
    Mat outimage(static_cast<int>(map_length * px_per_meter), warp_img.cols, CV_8UC1, Scalar(0));
    Rect out_warp_roi(0, 0, warp_img.cols, warp_img.rows);

    warp_img.copyTo(outimage(out_warp_roi));

    sensor_msgs::Image outmsg;
    cv_ptr->image = outimage;
    cv_ptr->toImageMsg(outmsg);
    transform_pubs[topic].publish(outmsg);
}

int main(int argc, char** argv) {
    init(argc, argv, "image_transform");
    NodeHandle nh;
    NodeHandle pnh("~");

    bool all_defined = true;
    all_defined &= pnh.getParam("px_per_meter", px_per_meter);
    all_defined &= pnh.getParam("map_dist_max", camera_dist_max);
    all_defined &= pnh.getParam("map_dist_min", camera_dist_min);

    std::string camera_info_topic;
    all_defined &= pnh.getParam("camera_info_topic", camera_info_topic);
    std::string camera_link_name;
    all_defined &= pnh.getParam("camera_link_name", camera_link_name);

    // the launch file can provide camera information in case camera_info is not published
    all_defined &= pnh.getParam("fallback_fov_horizontal", camera_fov_horizontal);
    all_defined &= pnh.getParam("fallback_fov_vertical", camera_fov_vertical);
    all_defined &= pnh.getParam("fallback_image_width", imageSize.width);
    all_defined &= pnh.getParam("fallback_image_height", imageSize.height);

    if (!all_defined) {
        ROS_WARN("[Image Transform] Not all launch params defined");
    }

    // load camera geometry
    rr::CameraGeometry cam_geom;
    cam_geom.LoadInfo(nh, camera_info_topic, camera_link_name, 60.0);

    // set relevant camera geometry fields for this node
    camera_fov_horizontal = cam_geom.GetFOVHorizontal();
    camera_fov_vertical = cam_geom.GetFOVVertical();
    cam_mount_angle = std::get<1>(cam_geom.GetCameraOrientationRPY());
    cam_mount_height = cam_geom.GetCameraLocation().z;
    cam_mount_x = cam_geom.GetCameraLocation().x;

    setTransformFromGeometry();
    ROS_INFO("Calculated perspective transform. Used height %f and angle %f", cam_mount_height, cam_mount_angle);

    string topicsConcat;
    pnh.getParam("transform_topics", topicsConcat);
    vector<string> topics;
    boost::split(topics, topicsConcat, boost::is_any_of(" ,"));
    vector<Subscriber> transform_subs;
    ROS_INFO_STREAM("Found " << topics.size() << " topics in param.");
    for (const string& topic : topics) {
        if (topic.size() == 0) {
            continue;
        }

        transform_subs.push_back(nh.subscribe<sensor_msgs::Image>(topic, 1, boost::bind(TransformImage, _1, topic)));
        ROS_INFO_STREAM("Image_transform subscribed to " << topic);
        string newTopic(topic + "_transformed");
        ROS_INFO_STREAM("Creating new topic " << newTopic);
        transform_pubs[topic] = nh.advertise<sensor_msgs::Image>(newTopic, 1);
    }

    spin();

    return 0;
}


#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <iomanip>
#include <Eigen/Dense>
#include <cmath>
#include <vector>

using namespace std;

typedef float (* node_function_t)(float a) ;

typedef float (* loss_function_t)(float y, float y_hat);

class Functions
{
private:
    static float loss_function(float y, float y_hat)
    {
        return (y - y_hat)*(y - y_hat);
    }
    static float der_loss_function(float y, float y_hat)
    {
        return 2*(y-y_hat);
    }
    static float node_function(float x)
    {
        return tanh(0.01*x);
    }
    static float der_node_function(float x)
    {
        float der = 1/(cosh(0.01*x)*cosh(0.01*x));

        return der;
    }

public:
    node_function_t f = node_function;
    node_function_t df = der_node_function;
    loss_function_t l = loss_function;
    loss_function_t dl = der_loss_function;
};

class Layer {
public:
    Eigen::VectorXf result;
    Eigen::MatrixXf w;
    Eigen::VectorXf dres;
    Layer (int layer_nodes_count, int prev_nodes_count) {
        w.resize(layer_nodes_count, prev_nodes_count);
        w.setOnes();
        // w = w*0.01;
        // w = w*0.01;

    }

    float operator[](int index){
        return this->result[index];
    }

    Eigen::VectorXf computer_out(const Eigen::VectorXf& x, node_function_t f, node_function_t df) {
        Eigen::VectorXf res = w * x;
        // if(res.size()==784){
        //   std::cout << w*x << '\n';
        // }
        dres.resize(res.size());
        for (int i = 0; i < w.rows(); i++) {
            dres[i] = df(res[i]);
            res[i] = f(res[i]);
        }
        result = res;
        return res;
    }

};

class Network
{
protected:
    Layer input;
    Layer hl ;
    Layer output;
    float learning_rate = 0.3;
    // float learning_rate = 0.002;
    float deviation = 0.02;
    // float deviation = 0.02;
    Functions functions;

public:
    Network(int input_count, int hidden_layer, int output_count, Functions functools)
    : input(input_count, input_count), hl(hidden_layer, input_count),
    output(output_count, hidden_layer){
        functions = functools;
    }

    Eigen::VectorXf learn_sample(const Eigen::VectorXf& x, const Eigen::VectorXf& out)
    {
        gradient_descent(x, out);
        return compute_result(x);
    }

    Eigen::VectorXf compute_input(const Eigen::VectorXf& x)
    {
        return input.computer_out(x, functions.f, functions.df);
    }

    Eigen::VectorXf compute_hidden(const Eigen::VectorXf& x)
    {
        return hl.computer_out(x, functions.f, functions.df);
    }

    Eigen::VectorXf compute_output(const Eigen::VectorXf& x)
    {
        return output.computer_out(x, functions.f, functions.df);
    }

    Eigen::VectorXf compute_result(const Eigen::VectorXf& x)
    {
      // std::cout << x << '\n';
        return compute_output(compute_hidden(compute_input(x)));
    }

    Eigen::VectorXf compute_result(const Eigen::VectorXf& x, const Eigen::VectorXf& out)
    {
      // std::cout << x << '\n';
      learn_sample(x,out);
        return compute_output(compute_hidden(compute_input(x)));
    }

    void set_learning_rate(float rate)
    {
        learning_rate = rate;
    }

    void set_deviation(float max_deviation)
    {
        deviation = max_deviation;
    }

    void gradient_descent(const Eigen::VectorXf& x, const Eigen::VectorXf& out, int max_count = 4)
    {
        int count = 0;
        float D, D1;
        Eigen::VectorXf D_v, dDdz;
        Eigen::MatrixXf dw, du, dv;
        // for (size_t i = 0; i < out.size(); i++) {
        //   if(out[i]!=0)
        //   {
        //     std::cout << " Out: "<< i << '\n';
        //     std::cout << compute_result(x)[i] << '\n';
        //   }
        // }
        do {
            D_v = compute_loss_function(out);
            dDdz = compute_der_d_z(out);
            D = D_v.sum();
            // std::cout << grad_output(dDdz) << '\n';
            descent(grad_output(dDdz), grad_hidden_2(dDdz), grad_input_2(dDdz, x));
            compute_result(x);
            D1 = compute_loss_function(out).sum();
            // std::cout << "D =" << D <<' '<<D1 << "Difference: " <<D-D1<<'\n';
            count++;
        }
        while (
          // D>1
        // );
          D - D1 > deviation &&
           count<max_count);
    }

    void descent(const Eigen::MatrixXf& dw, const Eigen::MatrixXf& du, const Eigen::MatrixXf& dv )
    {
        input.w -= dv;
        hl.w -= du;
        output.w -= dw;
    }

    Eigen::VectorXf compute_loss_function(const Eigen::VectorXf& out)
    {
        Eigen::VectorXf D(output.result.size());
        for (int i = 0; i < output.result.size(); i++)
        {
            D[i] = functions.l(output[i], out[i]);
        }
        return D;
    }

    Eigen::VectorXf compute_der_d_z(const Eigen::VectorXf& out)
    {
        Eigen::VectorXf res(output.result.size());

        for (int i = 0; i < output.result.size(); ++i) {
            res[i] = functions.dl(output.result[i], out[i]);
        }
        return res;
    }

    Eigen::MatrixXf grad_output(const Eigen::VectorXf& dDdZ)
    {
        Eigen::MatrixXf dw(output.w.rows(), output.w.cols());

        for (int i = 0; i < output.w.rows(); i++)
        {
            for(int j = 0; j < output.w.cols(); j++)
            {
                dw(i,j) = dDdZ[i]*hl.result[j]*output.dres[i];
            }
        }

        return dw*learning_rate;
    }

    Eigen::MatrixXf grad_hidden(const Eigen::VectorXf& dDdZ)
    {
        Eigen::MatrixXf du(hl.w.rows(), hl.w.cols());
        du.setZero();
        for (int i = 0; i < hl.w.rows(); i++)
        {
            for(int j = 0; j < hl.w.cols(); j++)
            {
                for (int k = 0; k < output.w.rows(); k++)
                {
                  // std::cout << hl.w.rows()<<' '<<j<<' '<<k << '\n';
                    du(i,j) += dDdZ[k]*output.w(k,i)*output.dres[k];
                }
                du(i,j) *= input.result[j]*hl.dres[i];
            }
        }

        return du*learning_rate;
    }

    Eigen::MatrixXf grad_hidden_2(const Eigen::VectorXf& dDdZ)
    {
        Eigen::MatrixXf du(hl.w.rows(), hl.w.cols());
        du.setZero();
        Eigen::MatrixXf temp = output.dres.asDiagonal()*output.w;
        temp = dDdZ.asDiagonal()*temp;
        float temp_sum;
        for (int i = 0; i < hl.w.rows(); i++)
        {
            temp_sum = temp.col(i).sum();
            for(int j = 0; j < hl.w.cols(); j++)
            {
                du(i,j) =temp_sum* input.result[j]*hl.dres[i];
            }
        }

        return du*learning_rate;
    }

    Eigen::MatrixXf grad_input(const Eigen::VectorXf& dDdZ, const Eigen::VectorXf& in)
    {
        int iwc = input.w.cols();
        Eigen::MatrixXf dv(input.w.rows(), iwc);
        dv.setZero();
        int ors = output.result.size();
        for(int i = 0; i < input.w.rows(); i++)
        {
            for(int j = 0; j < iwc; j++)
            {
                for(int k= 0; k < ors; k++)
                {
                    for(int t = 0; t < hl.w.rows(); t++)
                    {
                        dv(i,j) +=hl.w(t,i)*output.w(k,t)*hl.dres[t];
                    }
                    dv(i,j)*=dDdZ[k]*output.dres[k];
                     // std::cout << dv(i,j) << '\n';

                }
                dv(i,j)*=input.dres[i]*in[j];
            }

        }

        return dv*learning_rate;
    }

        Eigen::MatrixXf grad_input_2(const Eigen::VectorXf& dDdZ, const Eigen::VectorXf& in)
        {
            int iwc = input.w.cols();
            Eigen::MatrixXf dv(input.w.rows(), iwc);
            Eigen::MatrixXf temp = hl.dres.asDiagonal()*hl.w ;
            temp =output.w*temp;
            //std::cout << temp << '\n';
            dv.setZero();
            float temp_sum;
            int ors = output.result.size();
            for(int i = 0; i < input.w.rows(); i++)
            {
              temp_sum = temp.col(i).sum();
                for(int j = 0; j < iwc; j++)
                {
                    for(int k= 0; k < ors; k++)
                    {
                        dv(i,j) =dDdZ[k]*output.dres[k]*temp_sum ;
                    }
                    dv(i,j)*=input.dres[i]*in[j];
                }
                //std::cout << i << dv(i,0)<< '\n';
            }

            return dv*learning_rate;
        }

    void debug_output()
    {
      //std::cout << input.w << '\n';
    //  std::cout << hl.w << '\n';
      // std::cout << output.w << '\n';
    }

    void write_in_file()
    {
      ofstream off("weights.in");
      for (size_t i = 0; i < output.w.rows(); i++) {
        for (size_t j = 0; j < output.w.cols(); j++) {
          off<< output.w(i,j)<<endl;
        }
      }
      for (size_t i = 0; i < hl.w.rows(); i++) {
        for (size_t j = 0; j < hl.w.cols(); j++) {
          off<< hl.w(i,j)<<endl;
        }
      }
      for (size_t i = 0; i < input.w.rows(); i++) {
        for (size_t j = 0; j < input.w.cols(); j++) {
          off<< input.w(i,j)<<endl;
        }
      }
      off.close();
    }

    void read_from_file() {
      ifstream off("weights.in");
      if (off.is_open())
      {
      for (size_t i = 0; i < output.w.rows(); i++) {
        for (size_t j = 0; j < output.w.cols(); j++) {
          off>> output.w(i,j);
        }
      }
      for (size_t i = 0; i < hl.w.rows(); i++) {
        for (size_t j = 0; j < hl.w.cols(); j++) {
          off>> hl.w(i,j);
        }
      }
      for (size_t i = 0; i < input.w.rows(); i++) {
        for (size_t j = 0; j < input.w.cols(); j++) {
          off>> input.w(i,j);
        }
      }
    }
      off.close();
    }

};

  Eigen::MatrixXf read_mnist(int samples)
  {
    Eigen::MatrixXf x(28*28, samples);

      ifstream file ("t10k-images-idx3-ubyte");
      int magic_number;
      file.read((char*)&magic_number,sizeof(int));
      file.read((char*)&magic_number,sizeof(int));
      file.read((char*)&magic_number,sizeof(int));
      file.read((char*)&magic_number,sizeof(int));
      if (file.is_open())
      {
          for(int i=0;i<samples;i++)
          {
              for(int r=0;r<784;r++)
              {
                unsigned char temp=0;
                file.read((char*)&temp,sizeof(temp));
                x(r, i)=(float)temp/255;
                 // std::cout <<r<<" "<< (float)temp/255 << ' '<<x(r, i)<< '\n';
              }
          }
      }
      return x;
  }

  Eigen::VectorXf read_mnist_labels(int samples)
  {
      Eigen::VectorXf out(samples);
      ifstream file ("t10k-labels-idx1-ubyte");
      int magic_number;
      file.read((char*)&magic_number,sizeof(int));
      file.read((char*)&magic_number,sizeof(int));
      if (file.is_open())
      {
          for(int i=0;i<samples;i++)
          {
            unsigned char temp=0;
            file.read((char*)&temp,sizeof(temp));
            out[i] = (int)temp;

          }
      }
      return out;
  }

  struct Result
  {
    int predict;
    float pred_pers;
    float error;
    std::vector<int> image;
  };

struct Result test(int sample)
{
  Eigen::MatrixXf temp = read_mnist(sample+1);
  Eigen::VectorXf labels = read_mnist_labels(sample+1);
  Network nt(784, 256, 10, Functions());
  nt.read_from_file();
  Result result;
  Eigen::VectorXf res;
  float error = 0;
    Eigen::VectorXf out_v(10);
    out_v.setZero();
    out_v[labels[sample]] = 1;

    Eigen::VectorXf in = temp.col(sample);

    for (size_t i = 0; i < 784; i++) {
      result.image.push_back(in[i]);
    }
    res = nt.compute_result(in, out_v);

    for (size_t j = 0; j < res.size(); j++) {
      if(res[j]>0.9)
      {
        result.predict = j;
        result.pred_pers = res[j];
      }
      else
      {
        error +=res[j]
      }
    }
    result.error = error;

  return result;
}

void train(int sample)
{
  Eigen::MatrixXf temp = read_mnist(sample);
  Eigen::VectorXf labels = read_mnist_labels(sample);
  Network nt(784, 256, 10, Functions());

   for (size_t i = 0; i < sample; i++)
   {
    Eigen::VectorXf out_v(10);
    out_v.setZero();
    out_v[labels[i]] = 1;
    Eigen::VectorXf in = temp.col(i);
    nt.learn_sample(in, out_v);
    // std::cout << i+1 << '\n';
   }
   nt.write_in_file();
}


#include "triangle.hpp"

#include "geometry/projection.hpp"

#include <Eigen/Geometry>

namespace neon
{
triangle3::triangle3(triangle_quadrature::point const p)
    : surface_interpolation(std::make_unique<triangle_quadrature>(p), 3)
{
    this->precompute_shape_functions();
}

void triangle3::precompute_shape_functions()
{
    // Initialize nodal coordinates array as r and s
    m_quadrature->evaluate([&](auto const& coordinates) {
        auto const& [l, r, s] = coordinates;

        vector N(3);
        matrix rhea(3, 2);

        N(0) = r;
        N(1) = s;
        N(2) = 1.0 - r - s;

        rhea(0, 0) = 1.0;
        rhea(1, 0) = 0.0;
        rhea(2, 0) = -1.0;

        rhea(0, 1) = 0.0;
        rhea(1, 1) = 1.0;
        rhea(2, 1) = -1.0;

        return std::make_tuple(N, rhea);
    });

    extrapolation = matrix::Ones(number_of_nodes(), 1);
}

double triangle3::compute_measure(matrix const& nodal_coordinates) const
{
    // Use the cross product identity 2A = | a x b | to compute face area
    vector3 const direction0 = nodal_coordinates.col(0) - nodal_coordinates.col(2);
    vector3 const direction1 = nodal_coordinates.col(1) - nodal_coordinates.col(2);

    vector3 const normal = direction0.cross(direction1);

    return normal.norm() / 2.0;
}

triangle6::triangle6(triangle_quadrature::point const p)
    : surface_interpolation(std::make_unique<surface_quadrature>(triangle_quadrature(p)), 6)
{
    this->precompute_shape_functions();
}

void triangle6::precompute_shape_functions()
{
    using NodalCoordinate = std::tuple<int, double, double>;

    // Initialize nodal coordinates array as Xi, Eta, Zeta
    std::array<NodalCoordinate, 6> constexpr local_coordinates{{
        {0, 1.0, 0.0},
        {1, 0.0, 1.0},
        {2, 0.0, 0.0},
        {3, 0.5, 0.5},
        {4, 0.0, 0.5},
        {5, 0.5, 0.0},
    }};

    matrix N_matrix(m_quadrature->points(), number_of_nodes());
    matrix local_quadrature_coordinates = matrix::Ones(m_quadrature->points(), 3);

    m_quadrature->evaluate([&](auto const& coordinate) {
        auto const& [l, r, s] = coordinate;

        auto const t = 1.0 - r - s;

        vector N(6);
        matrix rhea(6, 2);

        N(0) = r * (2.0 * r - 1.0);
        N(1) = s * (2.0 * s - 1.0);
        N(2) = t * (2.0 * t - 1.0);
        N(3) = 4.0 * r * s;
        N(4) = 4.0 * s * t;
        N(5) = 4.0 * r * t;

        // r coordinates
        rhea(0, 0) = 4.0 * r - 1.0;
        rhea(1, 0) = 0.0;
        rhea(2, 0) = -4.0 * t + 1.0;
        rhea(3, 0) = 4.0 * s;
        rhea(4, 0) = -4.0 * s;
        rhea(5, 0) = 4.0 * t - 4.0 * r;

        // s coordinates
        rhea(0, 1) = 0.0;
        rhea(1, 1) = 4.0 * s - 1.0;
        rhea(2, 1) = -4.0 * t + 1.0;
        rhea(3, 1) = 4.0 * r;
        rhea(4, 1) = 4.0 * t - 4.0 * s;
        rhea(5, 1) = -4.0 * r;

        local_quadrature_coordinates(l, 0) = r;
        local_quadrature_coordinates(l, 1) = s;

        N_matrix.row(l) = N;

        return std::make_tuple(N, rhea);
    });

    // Compute extrapolation algorithm matkrices
    matrix local_nodal_coordinates = matrix::Ones(number_of_nodes(), 3);

    for (auto const& [a, r, s] : local_coordinates)
    {
        local_nodal_coordinates(a, 0) = r;
        local_nodal_coordinates(a, 1) = s;
    }
    compute_extrapolation_matrix(N_matrix, local_nodal_coordinates, local_quadrature_coordinates);
}

double triangle6::compute_measure(matrix const& nodal_coordinates)
{
    return m_quadrature->integrate(0.0, [&](auto const& femval, auto) {
        auto const& [N, dN] = femval;

        matrix2 const Jacobian = geometry::project_to_plane(nodal_coordinates) * dN;

        return Jacobian.determinant();
    });
}
}


#include <vector>

#include <glm/glm.hpp>

#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "Types.h"
#include "Dynamic.h"

namespace{
    using namespace BalloonFEM;
    const Vec3 v[4] = {Vec3(0), Vec3(1, 0, 0), Vec3(0, 1, 0), Vec3(0, 0, 1)};
    const Mat3 m[4][3] = {
        { Mat3(v[1], v[0], v[0]), Mat3(v[2], v[0], v[0]), Mat3(v[3], v[0], v[0])},
        { Mat3(v[0], v[1], v[0]), Mat3(v[0], v[2], v[0]), Mat3(v[0], v[3], v[0])},
        { Mat3(v[0], v[0], v[1]), Mat3(v[0], v[0], v[2]), Mat3(v[0], v[0], v[3])},
        { -Mat3(v[1], v[1], v[1]), -Mat3(v[2], v[2], v[2]), -Mat3(v[3], v[3], v[3])}
    };

}

namespace BalloonFEM
{
    void Engine::computeElasticForces(ObjState &state, Vvec3 &f_sum)
    {
        Vvec3 &pos = state.world_space_pos;

        for(TIter t = m_tetra->tetrahedrons.begin();
                t != m_tetra->tetrahedrons.end(); t++)
        {
            iVec4 &id = t->v_id;
            Vec3 &v0 = pos[id[0]];
            Vec3 &v1 = pos[id[1]];
            Vec3 &v2 = pos[id[2]];
            Vec3 &v3 = pos[id[3]];

            /* calculate deformation in world space */
            Mat3 Ds = Mat3(v0 - v3, v1 - v3, v2 - v3);

            /* calculate deformation gradient */
            Mat3 F = Ds * t->Bm;

            /* calculate Piola for this tetra */
            Mat3 P = m_volume_model->Piola(F);

            /* calculate forces contributed from this tetra */
            Mat3 H = - t->W * P * transpose(t->Bm);

            f_sum[id[0]] += H[0];
            f_sum[id[1]] += H[1];
            f_sum[id[2]] += H[2];
            f_sum[id[3]] -= H[0] + H[1] + H[2];
        }
    }

    SpMat Engine::computeElasticDiffMat(ObjState &state)
    {
        printf("building elastic differential matrix \n");
        /* project from constrained freedom state to world space */
		Vvec3 &pos = state.world_space_pos;

        /* compute elastic force Differentials */
		std::vector<T> coefficients;
		coefficients.clear();
		coefficients.reserve( 12 * 12 * m_tetra->num_vertex);

		for(TIter t = m_tetra->tetrahedrons.begin();
				t != m_tetra->tetrahedrons.end(); t++)
		{
			/* assgin world space position */
			iVec4 &id = t->v_id;
			Vec3 &v0 = pos[id[0]];
			Vec3 &v1 = pos[id[1]];
			Vec3 &v2 = pos[id[2]];
			Vec3 &v3 = pos[id[3]];

			/* calculate deformation in world space */
			Mat3 Ds = Mat3(v0 - v3, v1 - v3, v2 - v3);

			/* calculate deformation gradient */
			Mat3 F = Ds * t->Bm;

			/* i is index of vertex, j is index of dimention */
			for (size_t i = 0; i < 4; i++)
				for(size_t j = 0; j < 3; j++)
			{
				/* calculate delta deformation in world space */
				Mat3 dDs = m[i][j];

				/* calculate delta deformation gradient */
				Mat3 dF = dDs * t->Bm;

				/* calculate delta Piola */
				Mat3 dP = m_volume_model->StressDiff(F, dF);

				/* calculate forces contributed from this tetra */
				Mat3 dH = - t->W * dP * transpose(t->Bm);

				for(size_t w = 0; w < 3; w++)
					for(size_t l = 0; l < 3; l++)
						coefficients.push_back( T(3*id[w] + l, 3*id[i] + j,  dH[w][l]));

				Vec3 df_4 = - dH[0] - dH[1] - dH[2];
				for(size_t l = 0; l < 3; l++)
					coefficients.push_back( T(3*id[3] + l, 3*id[i] + j,  df_4[l]));
			}
		}
		SpMat E( 3 * pos.size(), 3 * pos.size());
		E.setFromTriplets(coefficients.begin(), coefficients.end());

        return E;
    }
}


/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2014 Basil Fierz
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

// VCL configuration
#include <vcl/config/global.h>

// C++ standard library
#include <iostream>
#include <random>

// Eigen library
#include <Eigen/Dense>

// Google benchmark
#include "benchmark/benchmark.h"

// VCL
#include <vcl/core/simd/vectorscalar.h>
#include <vcl/core/interleavedarray.h>
#include <vcl/math/jacobieigen33_selfadjoint.h>
#include <vcl/math/jacobieigen33_selfadjoint_quat.h>
#include <vcl/util/precisetimer.h>

#include "problems.h"

// Global data store for one time problem setup
const size_t nr_problems = 1024 * 1024;

// Problem set
Vcl::Core::InterleavedArray<float, 3, 3, -1> F(nr_problems);

void perfEigenIterative(benchmark::State& state)
{
	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(state.range(0));
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(state.range(0));

	for (auto _ : state)
	{
		for (int i = 0; i < state.range(0); ++i)
		{
			Vcl::Matrix3f A = F.at<float>(i);

			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
			solver.compute(A, Eigen::ComputeEigenvectors);

			resU.at<float>(i) = solver.eigenvectors();
			resS.at<float>(i) = solver.eigenvalues();
		}
	}

	state.counters["Iterations"] = 0;
	benchmark::DoNotOptimize(resU);
	benchmark::DoNotOptimize(resS);

	state.SetItemsProcessed(state.iterations() * state.range(0));
}

void perfEigenDirect(benchmark::State& state)
{
	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(state.range(0));
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(state.range(0));

	for (auto _ : state)
	{
		for (int i = 0; i < state.range(0); ++i)
		{
			Vcl::Matrix3f A = F.at<float>(i);

			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
			solver.computeDirect(A, Eigen::ComputeEigenvectors);

			resU.at<float>(i) = solver.eigenvectors();
			resS.at<float>(i) = solver.eigenvalues();
		}
	}

	state.counters["Iterations"] = 0;
	benchmark::DoNotOptimize(resU);
	benchmark::DoNotOptimize(resS);

	state.SetItemsProcessed(state.iterations() * state.range(0));
}

template<typename WideScalar>
void perfJacobi(benchmark::State& state)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(state.range(0));
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(state.range(0));

	int avg_nr_iter = 0;
	for (auto _ : state)
	{
		avg_nr_iter = 0;
		for (int i = 0; i < state.range(0) / width; ++i)
		{
			matrix3_t A = F.at<real_t>(i);
			matrix3_t U = matrix3_t::Identity();

			avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigen(A, U);

			resU.at<real_t>(i) = U;
			resS.at<real_t>(i) = A.diagonal();
		}
	}

	state.counters["Iterations"] = (double)(avg_nr_iter * width) / (double)state.range(0);
	benchmark::DoNotOptimize(resU);
	benchmark::DoNotOptimize(resS);

	state.SetItemsProcessed(state.iterations() * state.range(0));
}

template<typename WideScalar>
void perfJacobiQuat(benchmark::State& state)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(state.range(0));
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(state.range(0));

	int avg_nr_iter = 0;
	for (auto _ : state)
	{
		avg_nr_iter = 0;
		for (int i = 0; i < state.range(0) / width; ++i)
		{
			matrix3_t A = F.at<real_t>(i);
			matrix3_t U = matrix3_t::Identity();

			avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigenQuat(A, U);

			resU.at<real_t>(i) = U;
			resS.at<real_t>(i) = A.diagonal();
		}
	}

	state.counters["Iterations"] = (double)(avg_nr_iter * width) / (double)state.range(0);
	benchmark::DoNotOptimize(resU);
	benchmark::DoNotOptimize(resS);

	state.SetItemsProcessed(state.iterations() * state.range(0));
}

using Vcl::float16;
using Vcl::float4;
using Vcl::float8;

BENCHMARK(perfEigenIterative)->Arg(128); // ->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK(perfEigenDirect)->Arg(128);    // ->Arg(512)->Arg(8192)->ThreadRange(1, 16);

BENCHMARK_TEMPLATE(perfJacobi, float)->Arg(128);   //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobi, float4)->Arg(128);  //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobi, float8)->Arg(128);  //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobi, float16)->Arg(128); //->Arg(512)->Arg(8192)->ThreadRange(1, 16);

BENCHMARK_TEMPLATE(perfJacobiQuat, float)->Arg(128);   //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobiQuat, float4)->Arg(128);  //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobiQuat, float8)->Arg(128);  //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobiQuat, float16)->Arg(128); //->Arg(512)->Arg(8192)->ThreadRange(1, 16);

int main(int argc, char** argv)
{
	// Initialize data
	createSymmetricProblems(nr_problems, F);

	::benchmark::Initialize(&argc, argv);
	::benchmark::RunSpecifiedBenchmarks();
}


//  Copyright (c) 2006 Xiaogang Zhang
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_BESSEL_YN_HPP
#define BOOST_MATH_BESSEL_YN_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/detail/bessel_y0.hpp>
#include <boost/math/special_functions/detail/bessel_y1.hpp>
#include <boost/math/special_functions/detail/bessel_jy_series.hpp>
#include <boost/math/policies/error_handling.hpp>

// Bessel function of the second kind of integer order
// Y_n(z) is the dominant solution, forward recurrence always OK (though unstable)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T bessel_yn(int n, T x, const Policy& pol)
{
    BOOST_MATH_STD_USING
    T value, factor, current, prev;

    using namespace boost::math::tools;

    static const char* function = "boost::math::bessel_yn<%1%>(%1%,%1%)";

    if ((x == 0) && (n == 0))
    {
       return -policies::raise_overflow_error<T>(function, 0, pol);
    }
    if (x <= 0)
    {
       return policies::raise_domain_error<T>(function,
            "Got x = %1%, but x must be > 0, complex result not supported.", x, pol);
    }

    //
    // Reflection comes first:
    //
    if (n < 0)
    {
        factor = (n & 0x1) ? -1 : 1;  // Y_{-n}(z) = (-1)^n Y_n(z)
        n = -n;
    }
    else
    {
        factor = 1;
    }

    if(x < policies::get_epsilon<T, Policy>())
    {
       T scale = 1;
       value = bessel_yn_small_z(n, x, &scale, pol);
       if(tools::max_value<T>() * fabs(scale) < fabs(value))
          return boost::math::sign(scale) * boost::math::sign(value) * policies::raise_overflow_error<T>(function, 0, pol);
       value /= scale;
    }
    else if (n == 0)
    {
        value = bessel_y0(x, pol);
    }
    else if (n == 1)
    {
        value = factor * bessel_y1(x, pol);
    }
    else
    {
       prev = bessel_y0(x, pol);
       current = bessel_y1(x, pol);
       int k = 1;
       BOOST_ASSERT(k < n);
       do
       {
           T fact = 2 * k / x;
           if((tools::max_value<T>() - fabs(prev)) / fact < fabs(current))
           {
              prev /= current;
              factor /= current;
              current = 1;
           }
           value = fact * current - prev;
           prev = current;
           current = value;
           ++k;
       }
       while(k < n);
       if(fabs(tools::max_value<T>() * factor) < fabs(value))
          return sign(value) * sign(value) * policies::raise_overflow_error<T>(function, 0, pol);
       value /= factor;
    }
    return value;
}

}}} // namespaces

#endif // BOOST_MATH_BESSEL_YN_HPP


#include <iostream>
#include <vector>
#include <map>
#include <string>
#include <sstream>

#pragma warning( push )
# pragma warning (disable:4800)
#include <Eigen/Dense>
#include <Eigen/StdVector> //NodeArray, EdgeArray
#pragma warning( pop )

namespace cmg {
	typedef double Precision;
	typedef Eigen::Matrix<Precision, Eigen::Dynamic, Eigen::Dynamic> MatT;
	typedef Eigen::Matrix<Precision, Eigen::Dynamic, 1> VecT;
	typedef Eigen::Matrix<Precision, 2, 2> Mat2T;
	typedef Eigen::Matrix<Precision, 2, 1> Vec2T;
	typedef Eigen::Matrix<Precision, 3, 3> Mat3T;
	typedef Eigen::Matrix<Precision, 3, 1> Vec3T;
	typedef Eigen::Matrix<Precision, 4, 4> Mat4T;
	typedef Eigen::Matrix<Precision, 4, 1> Vec4T;
	typedef Eigen::Matrix<Precision, 6, 6> Mat6T;
	typedef Eigen::Matrix<Precision, 6, 1> Vec6T;
	typedef Eigen::Matrix<Precision, 2, 4> Mat2x4T;
	typedef Eigen::Matrix<Precision, 4, 2> Mat4x2T;
	typedef Eigen::Matrix<Precision, 3, 4> Mat3x4T;
	typedef Eigen::Matrix<Precision, 4, 3> Mat4x3T;

	inline static Precision eps() {
		return 10*std::numeric_limits<Precision>::epsilon();
	}

	typedef std::vector<int> Veci;
	inline static void range(const int start, const int end, const int step, Veci& ret)
	{
		ret.clear();
		ret.reserve((end-start+1)/step);
		for(int v=start; v<=end; v+=step) ret.push_back(v);
		ret.resize(ret.size());
	}

	inline static Precision perimeter(const Mat2x4T& u)
	{
		Precision ret=0;
		for(int i=0; i<4; ++i) {
			ret += (u.col(i) - u.col((i+1)%4)).norm();
		}
		return ret;
	}

	struct Calibration {
		Vec4T k;		//[fx, fy, cx, cy]
		Vec2T d;		//[k1, k2]
		Mat4T Ck;		//Cov[k]
		Mat2T Cd;		//Cov[d]
	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		inline Mat3T K() const
		{
			Mat3T ret;
			ret.setIdentity();
			ret(0,0)=k(0);
			ret(1,1)=k(1);
			ret(0,2)=k(2);
			ret(1,2)=k(3);
			return ret;
		}

		inline void print() const
		{
			std::cout<<"k="<<k.transpose()<<std::endl;
			std::cout<<"d="<<d.transpose()<<std::endl;
			//std::cout<<"Ck=\n"<<Ck<<std::endl; //TODO: Ck, Cd not used yet
			//std::cout<<"Cd=\n"<<Cd<<std::endl;
		}

		template<typename T>
		void project(const T X[3], T U[2]) const
		{
			T fx(k(0)), fy(k(1)), cx(k(2)), cy(k(3));
			T k1(d(0)), k2(d(1));

			T xn( X[0]/X[2] ), yn( X[1]/X[2] );
			T r2 = xn*xn + yn*yn;
			T factor=T(1)+(k2*r2+k1)*r2;
			T xnp=xn*factor;
			T ynp=yn*factor;
			U[0] = fx*xnp+cx;
			U[1] = fy*ynp+cy;
		}

		Vec2T project(const Vec3T& X) const
		{
			Vec2T ret;
			project(X.data(), ret.data());
			return ret;
		}
	};

	//6D pose
	struct Pose {
		Vec6T p;		//[ra*rx,ra*ry,ra*rz,tx,ty,tz]
		Mat6T Cp;		//Cov[p]
	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		static int nParams() { return 6; }

		inline Vec3T transform(const Vec3T& X) const
		{
			return (toT() * X.homogeneous()).hnormalized();
		}

		//return rotation matrix R
		inline Mat3T R() const
		{
			Precision ang = p.head<3>().norm();
			Vec3T axis(1,0,0);
			if(ang>eps())
				axis = p.head<3>().normalized();
			else
				ang = 0;
			Eigen::AngleAxis<Precision> aa(ang, axis);
			return aa.toRotationMatrix();
		}

		//return translation vector t
		inline Vec3T t() const
		{
			return p.tail<3>();
		}

		//return T=[R,t;0,1]
		inline Mat4T toT() const
		{
			Mat4T ret;
			ret.setIdentity();
			ret.topLeftCorner<3,3>() = R();
			ret.topRightCorner<3,1>() = t();
			return ret;
		}

		inline Mat4T invT() const
		{
			Mat3T rot = R().transpose();
			Mat4T T;
			T.setIdentity();
			T.topLeftCorner<3,3>() = rot;
			T.topRightCorner<3,1>() = -rot * t();
			return T;
		}

		inline Vec6T invp() const
		{
			return T2p(invT());
		}

		inline void fromR(const Mat3T& R)
		{
			Eigen::AngleAxis<Precision> aa;
			aa.fromRotationMatrix(R);
			p.head<3>() = aa.axis() * aa.angle();
		}

		inline void fromt(const Vec3T& t)
		{
			p.tail<3>() = t;
		}

		inline void fromT(const Mat4T& T)
		{
			fromR(T.topLeftCorner<3,3>());
			fromt(T.topRightCorner<3,1>());
		}

		inline static Vec6T T2p(const Mat4T& T)
		{
			return Pose(T).p;
		}

		inline static Mat4T p2T(const Vec6T& p)
		{
			return Pose(p).toT();
		}

		Pose() { p.setZero(); Cp.setZero(); }
		Pose(const Vec6T& p_) : p(p_) {
			Cp.setZero();
		}
		Pose(const Mat4T& T) {
			fromT(T);
			Cp.setZero();
		}
		Pose(const Mat4T& T, const Mat6T& Covp) : Cp(Covp)
		{
			fromT(T);
		}
	};
	typedef std::vector< Vec6T, Eigen::aligned_allocator<Vec6T> > Vec6TArray;

	struct Node : public Pose {
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		std::string name;	//name of the marker
		Veci vmeids;		//vmEdges' ids linked to this marker (not used now)
	};
	typedef std::vector< Node, Eigen::aligned_allocator<Node> > NodeArray;
	typedef int NID;
	const NID INVALID_NID = -1;

	struct Edge {
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		int vid;	//id of view
		int mid;	//id of marker
		Mat2x4T u;	//observations, 2x4, TODO: allow different observations
	};
	typedef std::vector< Edge, Eigen::aligned_allocator<Edge> > EdgeArray;
	typedef int EID;
	const EID INVALID_EID = -1;

	struct Observation {
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		std::string name;		//marker's name
		Pose init_marker_pose;	//marker pose in view, i.e., Tmc
		Mat2x4T u;				//observed marker corners in view

		bool operator<(const Observation& other) const {
			return perimeter(u) > perimeter(other.u);
		}
	};
	typedef std::vector< Observation,  Eigen::aligned_allocator<Observation> > ObsArray;
	typedef std::vector<ObsArray> VecObsArray;

	typedef std::map<std::string, int> Str2Int;

	class CMGraph {
	public: //member variables
		NodeArray markers;
		NodeArray views;
		EdgeArray edges;

		Pose fixed_marker_pose;
		NID fixed_marker_id;

		Calibration calib;
		Str2Int name2mid; //name -> makrer id

		Precision marker_half_size;
		bool verbose;

		struct Callback {
			virtual void operator()(const CMGraph& G) = 0;
		};
		typedef Callback* CallbackPtr;
		CallbackPtr cb_addObsFromNewView;
		CallbackPtr cb_optimizeNewViewPose;
		CallbackPtr cb_optimizeOneViewPose;
		CallbackPtr cb_optimizePose;

	public: //member functions
		CMGraph() : fixed_marker_id(INVALID_NID), marker_half_size(1), verbose(true),
			cb_addObsFromNewView(0), cb_optimizeNewViewPose(0), cb_optimizeOneViewPose(0), cb_optimizePose(0)
		{}

		//print input information
		inline void print() const
		{
			std::cout<<"-------------------"<<std::endl;
			std::cout<<"calib:"<<std::endl;
			calib.print();
			std::cout<<std::endl;

			std::cout<<"marker_half_size="<<marker_half_size<<std::endl;
			std::cout<<"verbose="<<verbose<<std::endl;
			std::cout<<"-------------------"<<std::endl;
		}

		//print CMGraph state
		void report(std::ofstream& out) const;

		inline int nParams() const
		{
			return Pose::nParams()*static_cast<int>(markers.size()+views.size());
		}

		inline int nResiduals() const
		{
			return nObsResiduals() + nCstResiduals();
		}

		inline int nObsResiduals() const
		{
			return 8*static_cast<int>(edges.size()); //TODO: allow each marker to have more than 4 point observations
		}

		inline int nCstResiduals() const
		{
			return Pose::nParams();
		}

		inline bool empty() const
		{
			return nParams()<=0;
		}

		NID setFixedMarker(const std::string &fixed_marker_name,
			const Precision p_ang=1e-4,
			const Precision p_pos=1e-2);

		//add all observed markers in this view to the graph, and also add view and edges
		//oa will be sorted by descending order of perimeters of the observed markers in the image
		NID addObsFromNewView(
			ObsArray& oa,
			const std::string &view_name,
			const bool addNewMarker=true);

		bool optimizeNewViewPose(
			ObsArray& oa,
			Node& newView,
			const Precision sigma_u,
			const int max_iter,
			const Precision huber_loss_bandwidth=10, // +/- 10 pixels
			const Precision error_rel_tol=1e-2,
			const bool computeCovariance=false);

		bool optimizeOneViewPose(
			const NID vid,
			const Precision sigma_u,
			const int max_iter,
			const Precision huber_loss_bandwidth=10, // +/- 10 pixels
			const Precision error_rel_tol=1e-2,
			const bool computeCovariance=false);

		//bundle adjustment to optimize all markers' and views' poses
		bool optimizePose(
			const Precision sigma_u,
			const int max_iter,
			const Precision huber_loss_bandwidth=10, // +/- 10 pixels
			const Precision error_rel_tol=1e-2,
			const bool computeCovariance=false);

	public: //static functions
		//return 2D coordinates of a marker's 4 corners
		inline static Mat2x4T marker_x(const Precision half_size=1)
		{
			Precision ret_[]={
				-1,-1,
				 1,-1,
				 1, 1,
				-1, 1
			};

			Mat2x4T ret(ret_);
			return ret * half_size;
		}

		//return 3D coordinates of a marker's 4 corners
		inline static Mat3x4T marker_X(const Precision half_size=1)
		{
			Precision ret_[]={
				-1,-1, 0,
				 1,-1, 0,
				 1, 1, 0,
				-1, 1, 0
			};

			Mat3x4T ret(ret_);
			return ret * half_size;
		}

		static void BatchProcess(
			const VecObsArray &frames, //each frame's Observations' order would be sorted
			const Calibration &calib,
			CMGraph& G,
			const std::string &fixed_marker_name="",
			const Precision p_ang=1e-4,
			const Precision p_pos=1e-2,
			const Precision sigma_u=0.2,
			const int max_iter_per_opt=20,
			const bool do_covariance_estimation=true);

	protected:
		NID newMarker(const Vec6T& p, const Mat6T& Cp, const std::string& name);

		NID newView(const Vec6T& p, const Mat6T& Cp, const std::string& name);

		EID newEdge(const NID vid, const NID mid, const MatT& u);
	};
}


#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

// this file implements the class defined in:
#include "calibration/Calibration.h"

using namespace Eigen;
using namespace calibration;
// constructor
Calibration::Calibration()
{
	RzOK_ = Matrix3f::Identity();
	RxOK_ = Matrix3f::Identity();
	TzOK_ = Matrix4f::Identity();
	TxOK_ = Matrix4f::Identity();
}

// destructor
Calibration::~Calibration()
{
	// empty destructor
}

// set functions
void Calibration::setInput(const std::vector<Matrix4f> Ta, const std::vector<Matrix4f> Tb)
{

	// set poses number from transformations size
	// check the transformations have the same poses number

	int na = Ta.size();
	int nb = Tb.size();

	if ( (na != nb) )
	{
		std::cerr << "rotation matrix dimension not compatible!! " << std::endl;
		return;
	}

	// n_poses_ is the actual number of poses recorded by camera
	n_poses_ = (na%2 == 0)? na : na - 1;

	// std::cout << "Calibrating for " << n_poses_ << " poses..." << std::endl;

	//resizeAllParametres();
	//std::cout << "all matrices resized " << std::endl;

	// push rotations and translations in respective vectors

	Ra_.clear();
	Rb_.clear();
	ta_.clear();
	tb_.clear();

	for (int i = 0 ; i < n_poses_  ;i++)
	{
		Ra_.push_back( Ta[i].block<3,3>(0,0) );

		// std::cout << "Ra_ pose  " << i << std::endl << Ra_[i] << std::endl;

		Rb_.push_back( Tb[i].block<3,3>(0,0) );

		// std::cout << "Rb_ pose  " << i << std::endl << Rb_[i] << std::endl;

		ta_.push_back( Ta[i].block<3,1>(0,3) );

		// std::cout << "ta_ pose  " << i << std::endl << ta_[i] << std::endl;

		tb_.push_back( Tb[i].block<3,1>(0,3) );

		// std::cout << "tb_ pose  " << i << std::endl << tb_[i] << std::endl;

	}

	//std::cout << "rotation part extracted " << std::endl;

}

// resize all parametres

int Calibration::resizeAllParametres()
{
	QqA_.resize(n_poses_);
	WqB_.resize(n_poses_);
	Ci_.resize(n_poses_);
	Ra_.resize(n_poses_);
	Rb_.resize(n_poses_);
	Rx_.resize(n_poses_);
	Rz_.resize(n_poses_);
	Sq_a.resize(n_poses_);
	Sq_b.resize(n_poses_);
	qa_.resize(n_poses_);
	qb_.resize(n_poses_);
	//qx_.resize(n_elem_);
	//qz_.resize(n_elem_);
	ta_.resize(n_poses_);
	tb_.resize(n_poses_);
	tx_.resize(n_poses_/2);
	tz_.resize(n_poses_/2);
	return 0;
}

// dual quaternions evaluation
/*
	by the skewsymmetrics we can obtain the dual quaternions Q and W as
	combination of the skewsymmetric and the relative quaternion so to have
	qAi * qX = Q(qAi) qX, where "*" is vect prod, and also
	qBi * qZ = W(qBi) qZ, that can substitute qA * qX = qZ *qB as
	Q(qAi) qX - W(qBi) qZ = 0

	Q(qAi) = [  qA0         -qA'
				qA      qA0*I + S(qA)]

	W(qBi) = [  qB0         -qB'
				qB      qB0*I - S(qB)]

*/
int Calibration::computeDualQuaternion()
{
	// compute skewsymmetric

	Matrix3f Sqa, Sqb;
	Matrix4f QqA, WqB;
	Quaternionf use;

	for (int i = 0 ; i < n_poses_  ;i++)
	{
		Sqa << 0, -qa_[i].z(), qa_[i].y(),
			   qa_[i].z(), 0, -qa_[i].x(),
			   -qa_[i].y(), qa_[i].x(), 0;

		Sq_a.push_back(Sqa);

		//std::cout << "from quaternion qa_" << std::endl << Quaternionf(qa_[i]) << std::endl << "the skewsymmetric Sq_a is " << std::endl << Matrix4f(Sq_a[i]) << std::endl;

		Sqb <<  0, -qb_[i].z(), qb_[i].y(),
				qb_[i].z(), 0, -qb_[i].x(),
			  	-qb_[i].y(), qb_[i].x(), 0;

		//std::cout << "from quaternion qb_" << std::endl << Quaternionf(qb_[i]) << std::endl << "the skewsymmetric Sq_b is " << std::endl << Matrix4f(Sq_b[i]) << std::endl;

		QqA.block<3,3>(1,1) = Sqa + (qa_[i].w() * Matrix3f::Identity ());
		use = qa_[i].conjugate();
		QqA.block<1,4>(0,0) = Vector4f( use.w(), use.x(), use.y(), use.z() ).transpose();
		use = qa_[i];
		QqA.block<4,1>(0,0) = Vector4f( use.w(), use.x(), use.y(), use.z() );

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

		QqA_.push_back(QqA);

		WqB.block<3,3>(1,1) = -Sqb + (qb_[i].w() * Matrix3f::Identity ());
		use = qb_[i].conjugate();
		WqB.block<1,4>(0,0) = Vector4f( use.w(), use.x(), use.y(), use.z() ).transpose();
		use = qb_[i];
		WqB.block<4,1>(0,0) = Vector4f( use.w(), use.x(), use.y(), use.z() );

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

		WqB_.push_back( WqB );
	}

	return 0;
}

int Calibration::computeClosedForm()
{
	transformRotations2Quaternions();
	computeDualQuaternion();
	computeC();
	computeFinalQuaternions();
	transformQuaterions2Rotations();
	computeTranslations();
	setOutput();
	computeError();
	std::cout << "Compute closed form done!" << std::endl;
	return 0;
}

int Calibration::computeNonLinOpt()
{
	computeClosedForm();
	computeOneShot();
	setOutput();
	computeError();
	std::cout << "Compute non linear optimization done!" << std::endl;
	return 0;
}

// C evaluation

/*

C matrix is the compute by the sum of the several Ci (orthogonal matrix of rank equal to 4)
obtained as Ci = -Q (qAi)'W(qBi)

C importance is due to its bond with qz and qx, because of qz is an eigenvector of the symmetric
semipositive definite matrix C'C and by that we can obtain qx = C qz/(lambda-n_poses)

*/

int Calibration::computeC()
{
	C_ = Matrix4f::Zero();
	for ( int i=0; i < n_poses_; i++ )
		C_ += -QqA_[i].transpose()* WqB_[i];
	return 0;
}

// final quaternions qxOK and qzOK evaluation
// alpha and lambda evaluation by eigenvalues

/*

alpha is a 4 float vector made of the C'C eigenvalues
lambda is n_poses,alpha combination (lambda = n_poses +/- sqrt (alpha))
so to have the minimum positive of lambda choosing the best alpha of the list

k_ is the best alpha index so to remember it also when compute the relative eigenvector

*/

int Calibration::computeFinalQuaternions()
{
	EigenSolver<Matrix4f> solver (C_.transpose()*C_ );
	alpha_ = solver.eigenvalues().real();
	k_= 0;
	float Lt;

	lambda_ = ((n_poses_ - sqrt(alpha_(k_)))>0) ? (n_poses_ - sqrt(alpha_(k_))) : (n_poses_ + sqrt(alpha_(k_)));

	for (int i=1; i < 4;i++)
	{
		Lt = n_poses_ - sqrt(alpha_[i]);

		if (Lt < lambda_ && Lt >0)
		{
			lambda_ = Lt;
			k_ = i;
		}
		else
		{
			Lt = n_poses_ + sqrt(alpha_[i]);
			if (Lt < lambda_)
			{
				lambda_ = Lt;
				k_ = i;
			}
		}
	}

	Vector4f qzOK, qxOK;

	qzOK = solver.eigenvectors().col(k_).real();
	qxOK = C_*qzOK/(lambda_ - n_poses_);

	qxOK.normalize();

	qzOK_ = Quaternionf( qzOK[0], qzOK[1], qzOK[2], qzOK[3] );
	qxOK_ = Quaternionf( qxOK[0], qxOK[1], qxOK[2], qxOK[3] );

	return 0;
}

// transform all rotations needed to quaternions

int Calibration::transformRotations2Quaternions()
{
	for (int i = 0; i < n_poses_; i++)
	{
		qa_.push_back( Quaternionf (Ra_[i]) );
		qb_.push_back( Quaternionf (Rb_[i]) );
	}
	return 0;
}

// transform all quaternions needed to rotations (qxOK and qzOK)

int Calibration::transformQuaterions2Rotations()
{
	RxOK_ = qxOK_.toRotationMatrix();
	RzOK_ = qzOK_.toRotationMatrix();
}

int Calibration::computeTranslations()
{
	if (n_poses_ == 1)
	{
		std::cerr << "translations not computable by only one rotation!! " << std::endl;
		return 0;
	}

	txOK_ = Vector3f::Zero ();
	tzOK_ = Vector3f::Zero ();

	Eigen::MatrixXf A;
	A.resize(3*n_poses_,6);
	Eigen::VectorXf c;
	c.resize(3*n_poses_);
	Eigen::VectorXf x;
	x.resize(6);

	for (int i = 0; i < n_poses_ ; i++ )
	{
		A.block<3,3>(3*i,0) = Ra_[i];
		A.block<3,3>(3*i,3) = -1*Eigen::Matrix3f::Identity();

		c.block<3,1>(3*i,0) = RzOK_*tb_[i] - ta_[i];
	}

	MatrixXf Ainv;
	Ainv.resize(6,6);
	Ainv = A.transpose()*A;

	x = Ainv.inverse()*A.transpose()*c;

	txOK_ = x.block<3,1>(0,0);
	tzOK_ = x.block<3,1>(3,0);

	/*for (int i = 0; i < n_poses_; i += 2)
	{
		txOK_ += (Matrix3f ( Ra_[i] - Ra_[i+1]).inverse() )*( RzOK_*(tb_[i] - tb_[i+1]) + ta_[i+1] - ta_[i] );
	}

	txOK_ *= 2/n_poses_;

	for (int i = 0; i < n_poses_; i += 2)
	{
		tzOK_ += Ra_[i]*txOK_ + ta_[i] - RzOK_*tb_[i];
	}

	tzOK_ *= 2/n_poses_;*/

	return 0;
}

int Calibration::computeError()
{
	Er_ = 0;

	float EtNum = 0, EtDen = 1;

	for (int i = 0; i < n_poses_; i++)
	{
		Er_ += Matrix3f( Ra_[i]*RxOK_ - RzOK_*Rb_[i] ).squaredNorm();

		EtDen += Vector3f( Ra_[i]*txOK_ - ta_[i] ).squaredNorm();

		EtNum += Vector3f( Ra_[i]*txOK_ + ta_[i] - RzOK_*tb_[i] - tzOK_ ).squaredNorm();

	}

	//Et_ = sqrt(EtNum/EtDen);
	Et_ = EtNum/n_poses_;
	Er_ = Er_/n_poses_;

	std::cout << "Rotation error is: " << Er_ << std::endl;
	std::cout << "Translation error is: " << Et_ << std::endl;

	return 0;
}

int Calibration::convertFromParamVect2ValuesOk(const VectorXf &in, Matrix3f &RzOK, Matrix3f &RxOK, Vector3f &tzOK, Vector3f &txOK)
{
	for (int i = 0; i < 3; i++)
	{
		RxOK.col(i) = in.block<3,1>(3*i,0);
		RzOK.col(i) = in.block<3,1>((3*i)+9,0);
	}

	txOK = in.block<3,1>(18,0);
	tzOK = in.block<3,1>(21,0);

return 0;

}

int Calibration::convertFromValuesOk2ParamVect(const Matrix3f &RzOK, const Matrix3f &RxOK, const Vector3f &tzOK, const Vector3f &txOK, VectorXf &out)

{
	for (int i = 0; i < 3; i++)
	{
		out.block<3,1>(3*i,0) = RxOK.col(i);
		out.block<3,1>((3*i)+9,0) = RzOK.col(i);
	}

	out.block<3,1>(18,0) = txOK;
	out.block<3,1>(21,0) = tzOK;

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

return 0;

}

void Calibration::setOutput()
{
	TzOK_.block<3,3>(0,0) = RzOK_;
	TzOK_.block<3,1>(0,3) = tzOK_;

	TxOK_.block<3,3>(0,0) = RxOK_;
	TxOK_.block<3,1>(0,3) = txOK_;

	return;
}

void Calibration::getOutput(Matrix4f &TzOK, Matrix4f &TxOK)
{
	TzOK = TzOK_;
	TxOK = TxOK_;
	return;
}

void Calibration::getFullOutput(Matrix4f &TzOK, Matrix4f &TxOK, float &Er, float &Et)
{
	getOutput(TzOK, TxOK);
	Er = Er_;
	Et = Et_;
	return;
}

int Calibration::computeOneShot()
{
	FF_.n_poses__ = n_poses_;
	FF_.Ra__ = Ra_;
	FF_.Rb__ = Rb_;
	FF_.ta__ = ta_;
	FF_.tb__ = tb_;

	Eigen::NumericalDiff<my_functor> numDiff(FF_);
	Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,float> lm_solver(numDiff);

	Eigen::VectorXf x(24);
	Eigen::VectorXf x_ini(24);

	// from initial guess to x
	convertFromValuesOk2ParamVect(RzOK_, RxOK_, tzOK_, txOK_, x);

	//x.setRandom();
	x_ini = x;
	// std::cout << "initial guess x : " << std::endl << x << std::endl;

	// init solver parameters
	lm_solver.parameters.maxfev = 2000;
	//lm_solver.parameters.xtol = 1.0e-10;
	//lm_solver.parameters.ftol = 1.0e-10;

	Eigen::LevenbergMarquardtSpace::Status status = lm_solver.minimize(x);

	std::cout << "Iterations in optimization before ending: " << lm_solver.iter << std::endl;
	std::cout << "lm_solver.nfev " << lm_solver.nfev << std::endl;
	std::cout << "lm_solver.njev " << lm_solver.njev << std::endl;
	std::cout << "lm_solver.fnorm " << lm_solver.fnorm << std::endl;
	std::cout << "status code: " << status << std::endl;
	// std::cout << "x that minimizes the function: " << std::endl << x << std::endl;

	std::cout << "|x_ini - x_opt|: " << std::endl << x_ini-x << std::endl;

	// from x to solution
	convertFromParamVect2ValuesOk(x, RzOK_, RxOK_, tzOK_, txOK_);

	std::cout << "RxOK_.determinant() : " << RxOK_.determinant() << std::endl;
	std::cout << "RzOK_.determinant() : " << RzOK_.determinant() << std::endl;

	return 0;
}


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

using namespace Eigen;
using namespace std;

int main() {
  MatrixXf m(2, 2);
  MatrixXf n(2, 2);
  MatrixXf result(2, 2);

  m << 1, 2,
      3, 4;
  n << 5, 6,
      7, 8;

  result = (m.array() + 4).matrix() * m;
  cout << "-- Combination 1: --" << endl << result << endl << endl;
  result = (m.array() * n.array()).matrix() * m;
  cout << "-- Combination 2: --" << endl << result << endl << endl;
}


/**
 * ECI to ECEF conversion matrices
 *
 * Copyright 2013 Bruce Ide
 *
 *  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.
 */

#include <Eigen/Core>
#include "gmst.hpp"

#ifndef _HPP_CONVERSION_MATRICES
#define _HPP_CONVERSION_MATRICES

namespace fr {

  namespace coordinates {

    // 6x6 population is the same for both classes, so may as well
    // only write it once.
    class ec_conversion_matrix_interface {
    public:
      ec_conversion_matrix_interface()
      {
      }

      virtual Eigen::Matrix3d get() = 0;
      virtual Eigen::Matrix3d get_dot() = 0;

      virtual Eigen::Matrix<double,6,6> get_xyz_vel()
      {
	Eigen::Matrix3d mat = get();
	Eigen::Matrix3d mat_dot = get_dot();
	Eigen::Matrix<double,6,6> retval;

	for (register int i = 0; i < 3; ++i) {
	  for (register int j = 0; j < 3; ++j) {
	    retval(i,j) = mat(i,j);
	    retval(i,j+3) = 0;
	    retval(i+3,j) = mat_dot(i,j);
	    retval(i+3,j+3) = mat(i,j);
	  }
	}
	return retval;
      }
    };

    /**
     * This class requires a time at which the coordinate was observed.
     */

    class eci_to_ecef : public ec_conversion_matrix_interface {
      double at_time;
      double gha_rad;
      double st,ct;
      double we;

    public:
      eci_to_ecef(const double &at_time) : at_time(at_time)
      {
	fr::time::gmst time_gmst(at_time);
	gha_rad = time_gmst.get_gmst() * 2.0 * fr::constants::pi / fr::constants::secs_per_ut1_day;
	st = sin(gha_rad);
	ct = cos(gha_rad);
	we = fr::constants::ut1_sideral_day_ratio * 2.0 * fr::constants::pi / fr::constants::secs_per_ut1_day;
      }

      ~eci_to_ecef()
      {
      }

      Eigen::Matrix3d get()
      {
	Eigen::Matrix3d retval;
	retval << ct,st,0.0,
	  -1.0 * st,ct,0.0,
	  0.0,0.0,1.0;
	return retval;
      }

      Eigen::Matrix3d get_dot()
      {
	Eigen::Matrix3d retval;
	retval << (-1.0 * we) * st, we * ct, 0.0,
	  (-1.0 * we) * ct, -we * st, 0.0,
	  0.0,0.0,0.0;
	return retval;
      }

    };

    // We can just transpose the eci to ecef matrix to get the ecef to
    // eci matrix

    class ecef_to_eci : public ec_conversion_matrix_interface {
      eci_to_ecef worker;
    public:
      ecef_to_eci(const double &at_time) : worker(at_time)
      {
      }

      ~ecef_to_eci()
      {
      }

      Eigen::Matrix3d get()
      {
	Eigen::Matrix3d interim = worker.get();
	Eigen::Matrix3d retval = interim.transpose();
	return retval;
      }

      Eigen::Matrix3d get_dot()
      {
	Eigen::Matrix3d retval = worker.get_dot().transpose();
	return retval;
      }

    };

  }

}

#endif


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Dense>
#include <math.h>

#include <unsupported/Eigen/MatrixFunctions>
#include <Eigen/Core>

using namespace std;
using namespace Eigen;
//using Eigen::MatrixXd;

MatrixXd readMatrix(const char *filename)
{
    std::ifstream indata;

    indata.open(filename);

    std::string line;
    getline(indata, line);
    std::stringstream lineStream(line);
    std::string cell;
    int count = 0, raw, coll;
    while (std::getline(lineStream, cell, ','))
    {
        if(count == 0) raw = stoi(cell);
        else coll = stoi( cell );
        count++;
    }
    MatrixXd res(raw,coll);
    cout<<"Matrix has been created"<<endl;
    raw = 0; coll = 0;
    while (getline(indata, line))
    {

        std::stringstream lineStream(line);
        std::string cell;

        while (std::getline(lineStream, cell, ','))
        {

            //cout<<"Raw: "<<raw<<" Coll: "<<coll<<endl;
            res(raw,coll) = stof(cell);
            coll++;
        }
        coll = 0;
        raw++;
    }
    indata.close();
    return res;
};

//TODO:
//Saving type of matrix
void writeMatrix(const char *filename, MatrixXd mat)
{
    ofstream outdata(filename,ios_base::out);
    int raws = mat.rows();
    int cols = mat.cols();
    outdata<<raws<<","<<cols<<endl;
    for(int i = 0; i< raws; i++)
    {
        for(int j = 0; j < cols-1; j++)
        {
            outdata<<mat(i,j)<<",";
        }
        outdata<<mat(i,cols-1)<<endl;
    }
    outdata.close();

}

/* void test_read_write_matrix_into_csv()
{
  MatrixXd m = MatrixXd::Random(3,3);
  writeMatrix("test_w.csv", m);
  MatrixXd n = readMatrix("test_w.csv");
  if(m == n) cout<<"True"<<endl;
  cout<<m<<endl;
  cout<<"===="<<endl;
  cout<<n<<endl;
} */

double Exp(double x) // the functor we want to apply
{
    return std::exp(x);
}

int main()
{
    MatrixXd X = readMatrix("X.csv");
    MatrixXd y = readMatrix("y.csv");
    MatrixXd z, z1, p, p1, u, w ;
    float b0 = log(y.mean() / (1 - y.mean()) ) ;
    cout<< b0<<endl;
    VectorXd b = ArrayXd::Zero(20);
    VectorXd b_old = ArrayXd::Zero(20);
    b(0) = b0;
    cout<<"Transposing"<<endl;
    b = b.transpose();
    for(int i = 0; i < 20; i++)
    {
        z = X * b;
        z = -z;
        z = z.unaryExpr([](double d) {return std::exp(d);});
        z1 = z.unaryExpr([](double d) {return d + 1.0;});
        p = z1.unaryExpr([](double d) {return 1.0 / d;});
        p1 = p.unaryExpr([](double d) {return 1 - d;});
        w = p * p1;
        w = w.unaryExpr([](double d) {return 1.0 / d;});
        u = z + (y -  p) * w;
        b_old = b;

    }
    // cout<<m;
    return 0;

}


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <manifold/SO3.h>
#include <manifold/gradientDescentSE3.h>
#include <manifold/newtonSE3.h>
#include <random>

class Gmm2pc : public GDSE3<double> {
 public:
  Gmm2pc(const Eigen::Vector3d& muA, const
      Eigen::Matrix3d& covA, const Eigen::MatrixXd& xB)
    : piA_(1.), muA_(muA), covA_(covA), xB_(xB)
  {
    std::cout << "-A-"
      << muA.transpose() << std::endl
      << covA << std::endl;
    std::cout << "-B-"
      << xB_.cols() << std::endl;
  };

  virtual void ComputeJacobian(const SE3d& theta, Eigen::Matrix<double,6,1>* J, double* f) {
    SE3d T = theta;
    Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3);
    Eigen::Vector3d t = T.matrix().topRightCorner(3,1);
    uint32_t N = xB_.cols();
    if (J) J->fill(0.);
    if (f) *f = 0.;
    for (uint32_t i=0; i<N; ++i) {
      double logCA = -0.5*log(2.*M_PI)*3-0.5*log(covA_.determinant());
      double logD = log(piA_) + logCA;
      Eigen::Vector3d a = R*xB_.col(i)+t-muA_;
      double z = -0.5*a.dot(covA_.ldlt().solve(a));
      if (J) {
        J->topRows(3) -= covA_.ldlt().solve(a);
        for (uint32_t j=0; j<3; ++j) {
          (*J)(3+j) -= a.dot(covA_.ldlt().solve(SO3d::G(j)*R*xB_.col(i)));
        }
      }
      if (f)
        *f += logD + z;
    }
    if (J) *J *= -1./N;
    if (f) *f *= -1./N;
  };
 protected:
  double piA_;
  Eigen::Vector3d muA_;
  Eigen::Matrix3d covA_;
  Eigen::MatrixXd xB_;
};

class Gmm2pcNewton : public NewtonSE3<double> {
 public:
  Gmm2pcNewton(const Eigen::Vector3d& muA, const
      Eigen::Matrix3d& covA, const Eigen::MatrixXd& xB)
    : piA_(1.), muA_(muA), covA_(covA), xB_(xB)
  {
    std::cout << "-A-"
      << muA.transpose() << std::endl
      << covA << std::endl;
    std::cout << "-B-"
      << xB_.cols() << std::endl;
  };

  virtual void ComputeJacobianAndHessian(const SE3d& theta,
      Eigen::Matrix<double,6,6>*H, Eigen::Matrix<double,6,1>* J,
      double* f) {
    SE3d T = theta;
    Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3);
    Eigen::Vector3d t = T.matrix().topRightCorner(3,1);
    uint32_t N = xB_.cols();
    if (H) H->fill(0.);
    if (J) J->fill(0.);
    if (f) *f = 0.;
    for (uint32_t i=0; i<N; ++i) {
      Eigen::Vector3d Rx = R*xB_.col(i);
      double logCA = -0.5*log(2.*M_PI)*3-0.5*log(covA_.determinant());
      double logD = log(piA_) + logCA;
      Eigen::Vector3d a = Rx+t-muA_;
      double z = -0.5*a.dot(covA_.ldlt().solve(a));
      if (H) {
        H->topLeftCorner(3,3) -= covA_.inverse();
        for (uint32_t j=0; j<3; ++j) {
          Eigen::Vector3d Htw_j = covA_.ldlt().solve(SO3d::G(j)*Rx);
          H->block<3,1>(0,j+3) -= Htw_j;
          H->block<1,3>(j+3,0) -= Htw_j.transpose();
        }
        for (uint32_t k=0; k<3; ++k) {
          for (uint32_t j=0; j<3; ++j) {
            (*H)(3+j,3+k) -= 0.5*(a.dot(covA_.ldlt().solve((
                    SO3d::G(k)*SO3d::G(j)+SO3d::G(j)*SO3d::G(k))*Rx)))
              - Rx.dot(SO3d::G(j)*covA_.ldlt().solve(SO3d::G(k)*Rx));
          }
        }
//        std::cout << *H << std::endl << std::endl;
      }
      if (J) {
        J->topRows(3) -= covA_.ldlt().solve(a);
        for (uint32_t j=0; j<3; ++j) {
          (*J)(3+j) -= a.dot(covA_.ldlt().solve(SO3d::G(j)*R*xB_.col(i)));
        }
      }
      if (f)
        *f += logD + z;
    }
    if (H) *H *= -1./N;
    if (J) *J *= -1./N;
    if (f) *f *= -1./N;
  };
 protected:
  double piA_;
  Eigen::Vector3d muA_;
  Eigen::Matrix3d covA_;
  Eigen::MatrixXd xB_;
};

int main (int argc, char** argv) {

  double theta = 15.*M_PI/180.;
  Eigen::Matrix3d R;
  R << 1, 0, 0,
         0, cos(theta), sin(theta),
         0, -sin(theta), cos(theta);
  Eigen::Vector3d t = Eigen::Vector3d::Ones();

  Eigen::Matrix3d covA =   Eigen::Vector3d(1.,.1,3.).asDiagonal();
  Eigen::Vector3d muA = Eigen::Vector3d::Zero();

  std::random_device rd;
  std::mt19937 gen(rd());
  std::normal_distribution<> d1(muA(0),sqrt(covA(0,0)));
  std::normal_distribution<> d2(muA(1),sqrt(covA(1,1)));
  std::normal_distribution<> d3(muA(2),sqrt(covA(2,2)));

  Eigen::Matrix<double,3,Eigen::Dynamic> xB(3,1000);
  for (uint32_t i=0; i<xB.cols(); ++i) {
    xB(0,i) = d1(gen);
    xB(1,i) = d2(gen);
    xB(2,i) = d3(gen);
    xB.col(i) = R*xB.col(i) + t;
  }
  SE3d T;

  Gmm2pc gd(muA, covA, xB);
  gd.Compute(T, 1e-6, 200);
//  gd.Compute(T, 0, 200);
  T = gd.GetMinimum();
  Eigen::Vector3d tEst = T.matrix().topRightCorner(3,1);
  Eigen::Matrix3d REst = T.matrix().topLeftCorner(3,3);
  std::cout << " - T -" << std::endl;
  std::cout << T << std::endl;
  std::cout << " - R -" << std::endl;
  std::cout << R << std::endl;
  std::cout << " - Rest -" << std::endl;
  std::cout << REst.transpose() << std::endl;
  std::cout << " - t -" << std::endl;
  std::cout << t.transpose() << std::endl;
  std::cout << " - tEst -" << std::endl;
  std::cout << (-REst.transpose()*tEst).transpose() << std::endl;
  std::cout << " - dR = "
    << SO3d::Log_(R.transpose()*REst.transpose()).norm()*180./M_PI << std::endl;
  std::cout << " - dt = "
    << (t-(-REst.transpose()*tEst)).norm() << std::endl;

  T = SE3d();
  Gmm2pcNewton newton(muA, covA, xB);
  newton.Compute(T, 1e-6, 300);
//  gd.Compute(T, 0, 200);
  T = newton.GetMinimum();
  tEst = T.matrix().topRightCorner(3,1);
  REst = T.matrix().topLeftCorner(3,3);
  std::cout << " - T -" << std::endl;
  std::cout << T << std::endl;
  std::cout << " - R -" << std::endl;
  std::cout << R << std::endl;
  std::cout << " - Rest -" << std::endl;
  std::cout << REst.transpose() << std::endl;
  std::cout << " - t -" << std::endl;
  std::cout << t.transpose() << std::endl;
  std::cout << " - tEst -" << std::endl;
  std::cout << (-REst.transpose()*tEst).transpose() << std::endl;
  std::cout << " - dR = "
    << SO3d::Log_(R.transpose()*REst.transpose()).norm()*180./M_PI << std::endl;
  std::cout << " - dt = "
    << (t-(-REst.transpose()*tEst)).norm() << std::endl;
}


#ifndef HYPSYS1D_RECONSTRUCTION_HPP
#define HYPSYS1D_RECONSTRUCTION_HPP

#include <Eigen/Dense>
#include <cmath>
#include <memory>

#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/limiters.hpp>
#include <ancse/rate_of_change.hpp>
#include <ancse/simulation_time.hpp>

// Reconstructions:
class PWConstantReconstruction
{
    public:
        void set(const Eigen::MatrixXd &u) const
        {
            up.resize(u.rows(),u.cols());
            up = u;
        }

        /// Compute the left and right trace at the interface i + 1/2.
        /** Note: This API is agnostic to the number of cell-averages required
         *        by the method. Therefore, reconstructions with different stencil
         *        sizes can implement this API; and this call can be used in parts
         *        of the code that do not need to know about the details of the
         *        reconstruction.
         */
        std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()(int i) const
        {
            return (*this)(up.col(i), up.col(i+1));
        }

        /// Compute the left and right trace at the interface.
        /** Piecewise constant reconstruction of the left and right trace only
         *  requires the cell-average to the left and right of the interface.
         *
         *  Note: Compared to the other overload this reduces the assumption on
         *        how the cell-averages are stored. This is useful when testing and
         *        generally makes the function useful in more situations.
         */
        inline std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()
            (Eigen::VectorXd ua, Eigen::VectorXd ub) const
        {
            return {std::move(ua), std::move(ub)};
        }

        // To have method in common with PWLinearReconstruction:
        std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()
            (const Eigen::MatrixXd& u, const int i) const
        {
            return (*this)(u.col(i), u.col(i + 1));
        }

    private:
        mutable Eigen::MatrixXd up;
};

// Wrapping the available scalar slope limiters for use with quantity vectors u
template <class SlopeLimiter>
class VectorSlopeLimiter
{
    public:
        explicit VectorSlopeLimiter(const SlopeLimiter& sigma) : sigma(sigma) {}

        Eigen::VectorXd operator() (const Eigen::VectorXd& sL, const Eigen::VectorXd& sR) const
        {
            assert ((sL.size() == sR.size()));

            const int size= sL.size();
            Eigen::VectorXd limited_s(size);

            for (int i= 0; i < size; i++)
                limited_s(i)= sigma(sL(i), sR(i));

            return limited_s;
        }

    private:
        SlopeLimiter sigma;
};

template <class SlopeLimiter>
class PWLinearReconstruction
{
    public:
        explicit PWLinearReconstruction(const SlopeLimiter& slope_limiter)
            : slope_limiter(slope_limiter) {}

        std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()
            (const Eigen::MatrixXd& u, const int i) const
        {
            return (*this)(u.col(i - 1), u.col(i), u.col(i + 1), u.col(i + 2));
        }

        std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()
            (Eigen::VectorXd ua, Eigen::VectorXd ub, Eigen::VectorXd uc, Eigen::VectorXd ud) const
        {
            Eigen::VectorXd sL= ub - ua;
            Eigen::VectorXd sM= uc - ub;
            Eigen::VectorXd sR= ud - uc;

            Eigen::VectorXd uL= ub + 0.5 * slope_limiter(sL, sM);
            Eigen::VectorXd uR= uc - 0.5 * slope_limiter(sM, sR);

            return {uL, uR};
        }

    private:
        VectorSlopeLimiter<SlopeLimiter> slope_limiter;
};

#endif // HYPSYS1D_RATE_OF_CHANGE_HPP