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

/**
 * @file Optimal control problem on SE2.
 */

#ifndef OCP_SE2_HPP_
#define OCP_SE2_HPP_

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <smooth/bundle.hpp>
#include <smooth/derivatives.hpp>
#include <smooth/feedback/ocp.hpp>
#include <smooth/feedback/utils/sparse.hpp>
#include <smooth/se2.hpp>

template<typename T>
using X = smooth::Bundle<smooth::SE2<T>, Eigen::Vector2<T>>;

template<typename T>
using U = Eigen::Vector2<T>;

template<typename T, std::size_t N>
using Vec = Eigen::Vector<T, N>;

/// @brief Objective function
struct SE2Theta
{
  template<typename T>
  T operator()(T tf, const X<T> &, const X<T> &, const Vec<T, 1> & q) const
  {
    return tf + q.x();
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(1, 12);
    ret.coeffRef(0, 0)  = 1;
    ret.coeffRef(0, 11) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(12, 12);
    return ret;
  }
};

/// @brief Dynamics
struct SE2Dyn
{
  template<typename T>
  smooth::Tangent<X<T>> operator()(T, const X<T> & x, const U<T> & u) const
  {
    smooth::Tangent<X<T>> ret;
    ret(0) = x.template part<1>().x();
    ret(1) = T(0);
    ret(2) = x.template part<1>().y();
    ret(3) = u.x();
    ret(4) = u.y();
    return ret;
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(5, 8);
    ret.coeffRef(0, 4) = 1;
    ret.coeffRef(2, 5) = 1;
    ret.coeffRef(3, 6) = 1;
    ret.coeffRef(4, 7) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(8, 5 * 8);
    return ret;
  }
};

/// @brief Target trajectory
const auto xdes = []<typename T>(T t) -> X<T> {
  const Eigen::Vector3<T> vel{1., 0., 0.5};

  X<T> ret;
  ret.template part<0>()     = smooth::SE2<T>::exp(t * vel);
  ret.template part<1>().x() = vel.x();
  ret.template part<1>().y() = vel.z();
  return ret;
};

/// @brief Integrals
struct SE2Integral
{
  template<typename T>
  Vec<T, 1> operator()(T t, const X<T> & x, const U<T> & u) const
  {
    return 0.5 * Vec<T, 1>{(x - xdes(t)).squaredNorm() + u.squaredNorm()};
  }

  Eigen::SparseMatrix<double> jacobian(double t, const X<double> & x, const U<double> & u) const
  {
    const auto a = x - xdes(t);

    Eigen::SparseMatrix<double> ret(1, 8);
    ret.coeffRef(0, 0) = -(a.transpose() * smooth::dl_expinv<X<double>>(a))
                            .dot(Eigen::Vector<double, 5>{1., 0., 0.5, 0, 0});
    smooth::feedback::block_add(ret, 0, 1, smooth::dr_rminus_squarednorm<X<double>>(a));
    ret.coeffRef(0, 6) = u.x();
    ret.coeffRef(0, 7) = u.y();
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double t, const X<double> & x, const U<double> &) const
  {
    const auto H = smooth::d2r_rminus_squarednorm<X<double>>(x - xdes(t));

    Eigen::SparseMatrix<double> ret(8, 8);
    /// @todo don't have derivatives w.r.t. t
    smooth::feedback::block_add(ret, 1, 1, H);
    ret.coeffRef(6, 6) = 1;
    ret.coeffRef(7, 7) = 1;
    return ret;
  }
};

/// @brief Running constraints
struct SE2Cr
{
  template<typename T>
  Vec<T, 2> operator()(T, const X<T> &, const U<T> & u) const
  {
    return u;
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(2, 8);
    ret.coeffRef(0, 6) = 1;
    ret.coeffRef(1, 7) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(8, 2 * 8);
    return ret;
  }
};

/// @brief End constraints
struct SE2Ce
{
  template<typename T>
  Vec<T, 6> operator()(T tf, const X<T> & x0, const X<T> &, const Vec<T, 1> &) const
  {
    Vec<T, 6> ret;
    ret << tf, x0.log();
    return ret;
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> & x0, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(6, 12);
    ret.coeffRef(0, 0) = 1;
    smooth::feedback::block_add(ret, 1, 1, smooth::dr_expinv<X<double>>(x0.log()));
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> & x0, const X<double> &, const Vec<double, 1> &) const
  {
    const auto d2_logx0 = smooth::d2r_rminus<X<double>>(x0.log());

    Eigen::SparseMatrix<double> ret(12, 6 * 12);
    for (auto i = 0u; i < 5; ++i) {
      smooth::feedback::block_add(ret, 1, 12 * (1 + i) + 1, d2_logx0.block(0, i * 5, 5, 5));
    }

    return ret;
  }
};

using OcpSE2 =
  smooth::feedback::OCP<X<double>, U<double>, SE2Theta, SE2Dyn, SE2Integral, SE2Cr, SE2Ce>;

inline const OcpSE2 ocp_se2{
  .theta = SE2Theta{},
  .f     = SE2Dyn{},
  .g     = SE2Integral{},
  .cr    = SE2Cr{},
  .crl   = Vec<double, 2>{{-1, -1}},
  .cru   = Vec<double, 2>{{1, 1}},
  .ce    = SE2Ce{},
  .cel   = Vec<double, 6>{{5, 0, 0, 0, 1, 0}},
  .ceu   = Vec<double, 6>{{5, 0, 0, 0, 1, 0}},
};

#endif  // OCP_SE2_HPP_


#include "q_matrix_tools.h"
#include "rhab/basic_iteration.h"
#include <iomanip>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

// Boost Algorithm
#include <boost/algorithm/minmax_element.hpp>

extern "C"
{
#include <cblas.h>
}

#include <gsl/gsl_multifit.h>
#include <gsl/gsl_multimin.h>

typedef boost::tuple<const std::vector<size_t>*,
                     const std::vector<double>*,
                     const std::vector<double>*
                     > params_t;

/**
 * Apply symmetry condition.
 *
 * We can only use pairs of T(i->j) and T(j->i) for the equation system.
 */
void apply_symmetry_conditions(matrix_int_t &imat, std::vector<size_t> &coords) {
  for (size_t i = 0; i < imat.size1(); i++) {
    for (size_t j = 0; j < imat.size2(); ++j) {
      if (i==j) {
        continue;
      }
      if (imat(i,j) > 0 && imat(j,i) > 0) {
        coords.push_back(i);
        coords.push_back(j);
        continue;
      }
      imat(i,j) = imat(j,i) = 0;
    }
  }
}

double variance_function(const gsl_vector * x, void * params) {
  params_t* p = (params_t*)params;
  const std::vector<size_t>* coords  = boost::get<0>(*p);
  const std::vector<double>* sigma   = boost::get<1>(*p);
  const std::vector<double>* precalc = boost::get<2>(*p);

  double err = 0;

  for (size_t k = 0; k < coords->size(); k+=2) {
    const size_t &i = (*coords)[k];
    const size_t &j = (*coords)[k+1];

    double xi(1.0), xj(1.0);
    if (i < x->size) {
      xi = gsl_vector_get(x, i);
    }
    if (j < x->size) {
      xj = gsl_vector_get(x, j);
    }

    double tmp = xi - xj + (*precalc)[k/2];
    err += tmp*tmp / (*sigma)[k/2];
  }

  return err;
}

bool rhab::calculate_dos_minimization(matrix_int_t imat, matrix_double_t &dmat, vector_double_t &dos) {
  // Histogram
  vector_int_t hist(imat.size1());

  // coordinates of entries in the matrix stored one after another i1,j1,i2,j2,...
  std::vector<size_t> coords;
  std::vector<double> sigma;
  std::vector<double> precalc;

  apply_symmetry_conditions(imat, coords);

  // if we have too few entries left in the matrix abort
  if (coords.size()/2 < dos.size()) {
    return false;
  }

  normalize_q(imat, dmat);

  for (size_t i = 0; i < imat.size1(); i++) {
    hist[i] = sum(row(imat,i));
  }

  // we keep the state with the highest energy fixed
  // and thus do minimization only on N-1 states
  size_t num_states = dos.size()-1;

  // Precalculate variables
  sigma.resize(coords.size()/2);
  precalc.resize(coords.size()/2);

  for (size_t k = 0; k < coords.size(); k+=2) {
    const size_t &i = coords[k];
    const size_t &j = coords[k+1];

    precalc[k/2] = log(dmat(i,j) / dmat(j,i));
    sigma[k/2]   = 1./imat(i,j) + 1./hist[i] + 1./imat(j,i) + 1./hist[j];
  }

  // setup the minimization function and fminimizer
  gsl_vector *ss, *x;
  gsl_multimin_function minex_func;

  params_t params(&coords, &sigma, &precalc);

  minex_func.n = num_states;
  minex_func.f = variance_function;
  minex_func.params = &params;

  // Starting point
  x = gsl_vector_alloc(num_states);
  gsl_vector_set_all(x,  1.0);

  // Set initial step sizes to 1
  ss = gsl_vector_alloc(num_states);
  gsl_vector_set_all(ss, 1.0);

  gsl_multimin_fminimizer *s = NULL;
  s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex2, num_states);
  gsl_multimin_fminimizer_set(s, &minex_func, x, ss);
  int status;
  size_t iter = 0;
  double size;

  do {
    iter++;
    status = gsl_multimin_fminimizer_iterate(s);

    if (status) {
      break;
    }

    size = gsl_multimin_fminimizer_size(s);
    status = gsl_multimin_test_size(size, 1e-6);
    //std::cout << iter << " " << size << " " << status << std::endl;
  } while(status == GSL_CONTINUE /* && iter < 1000000*/);

  std::cout << iter << " " << size;

  for (size_t i = 0; i < s->x->size; i++) {
    dos[i] = gsl_vector_get(s->x, i);
  }
  dos[dos.size()-1] = 1;

  for (size_t i = 0; i < dos.size(); i++) {
    std::cout << " " << dos[i];
  }
  std::cout << std::endl;

  gsl_multimin_fminimizer_free(s);
  gsl_vector_free(x);
  gsl_vector_free(ss);

  return true;
}

bool rhab::calculate_dos_leastsquares(matrix_int_t imat, matrix_double_t &dmat, vector_double_t &dos) {
  vector_int_t hist(imat.size1());

  // coordinates of entries in the matrix stored one after another i1,j1,i2,j2,...
  std::vector<size_t> coords;
  std::vector<size_t> pivot(imat.size1());
  size_t cnt_nnz = 0;

  apply_symmetry_conditions(imat, coords);

  if (coords.size()/2 < dos.size()) {
    return false;
  }

  normalize_q(imat, dmat);

  for (size_t i = 0; i < imat.size1(); i++) {
    hist[cnt_nnz] = sum(row(imat,i));
    if (hist[cnt_nnz] > 0) {
      pivot[i] = cnt_nnz;
      cnt_nnz ++;
    }
  }

  int xn = coords.size()/2;

  gsl_matrix *X, *cov;
  gsl_vector *y, *c;
  double chisq;

  X = gsl_matrix_calloc(xn, cnt_nnz);
  y = gsl_vector_calloc(xn);
  c = gsl_vector_calloc(cnt_nnz);
  cov = gsl_matrix_calloc(cnt_nnz, cnt_nnz);

  for (int k = 0; k < 2*xn; k+=2) {
    const size_t &i = coords[k];
    const size_t &j = coords[k+1];
    gsl_matrix_set(X, k/2, pivot[i],  1.0/sqrt(1./hist[pivot[i]] + 1./hist[pivot[j]] + 1./imat(j,i) + 1./imat(i,j)));
    gsl_matrix_set(X, k/2, pivot[j], -1.0/sqrt(1./hist[pivot[i]] + 1./hist[pivot[j]] + 1./imat(j,i) + 1./imat(i,j)));
    gsl_vector_set(y, k/2, -log( dmat(i,j) / dmat(j,i) )/sqrt(1./hist[pivot[i]] + 1./hist[pivot[j]] + 1./imat(j,i) + 1./imat(i,j)));
  }

  {
    gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc(xn, cnt_nnz);
    gsl_multifit_linear(X,y,c,cov,&chisq, work);
    gsl_multifit_linear_free(work);
  }

  for (size_t i = 0; i < dos.size(); i++) {
    dos[i] = gsl_vector_get(c, pivot[i]);
  }

  gsl_matrix_free(X);
  gsl_vector_free(y);
  gsl_vector_free(c);
  gsl_matrix_free(cov);

  return true;
}

void rhab::normalize_q(const matrix_int_t & Q, matrix_double_t & Qd) {
  using namespace boost::numeric::ublas;
  for (size_t i = 0; i < Q.size1(); i++) {
    double s = sum(row(Q,i));
    if (s == 0) {
      row(Qd,i) *= 0;
    } else {
      row(Qd,i) = row(Q,i)/s;
    }
  }
}

void rhab::normalize(vector_double_t &vec) {
  vec /= sum(vec);
}

void rhab::normalize_from_log(vector_double_t &vec) {
  double sub(0), norm(0);
  std::pair<vector_double_t::iterator, vector_double_t::iterator> mm =
    boost::minmax_element(vec.begin(), vec.end());
  sub = (*(mm.second) + *(mm.first))/2;
  for (size_t i = 0; i < vec.size(); i++) {
    norm += exp(vec[i] - sub);
  }
  for (size_t i = 0; i < vec.size(); i++) {
    vec[i] = exp(vec[i] - sub) / norm;
  }
}

bool rhab::calculate_dos_gth(matrix_double_t & inner_mat, vector_double_t &dos) {
  namespace ublas = boost::numeric::ublas;
  std::size_t inner_rows(inner_mat.size1());
  std::size_t inner_cols(inner_mat.size1());
  // we assume small matrix and try GTH method
  // do GTH LU decomposition
  for (std::size_t i = inner_rows-1; i > 0; --i) {
    double s = ublas::norm_1(ublas::subrange(ublas::row(inner_mat,i), 0, i));
    if (s != 0) {
      inner_mat(i,i) = -s;
      for (std::size_t j = 0; j < i; ++j) {
        inner_mat(j,i) /= s;
      }
    }
    for (std::size_t k = 0; k < i; ++k) {
      for (std::size_t j = 0; j < i; ++j) {
        inner_mat(k,j) += inner_mat(k,i)*inner_mat(i,j);
      }
    }
  }
  // now just do modified eqn. 33 of M. Fenwick - J. Chem. Phys. 125, 144905
  std::fill(dos.begin(), dos.end(), 0.0);//dos.clear();
  dos[0] = 1;
  for (std::size_t i = 1; i < inner_rows; ++i) {
    for (std::size_t j = 0; j < i; ++j) {
      if (inner_mat(j,i) > 0) {
        dos[i] += exp(dos[j] + log(inner_mat(j,i)));
      }
    }
    dos[i] = log(dos[i]);
  }
  for (std::size_t ei = 0; ei < inner_cols; ++ei) {
    dos(ei) = exp(dos(ei));
  }

  // eqn. 32 of M. Fenwick - J. Chem. Phys. 125, 144905
  /*
  for (size_t i = 1; i < inner_rows; ++i) {
    for (size_t j = 0; j < i; ++j) {
      dos[i] += dos[j] * inner_mat(j,i);
    }
  }
  */

  normalize(dos);
  return ( std::count_if(dos.begin(), dos.end(), boost::math::isnan<double>) == 0 );
}

bool rhab::calculate_dos_power(const matrix_double_t &imat, vector_double_t &t1) {
  namespace ublas = boost::numeric::ublas;
  matrix_double_t mat(ublas::trans(imat));
  //vector_double_t t1(mat.size1());
  vector_double_t t2(mat.size1());
  vector_double_t t3(mat.size1());
  std::fill(t1.begin(), t1.end(), 1.0/mat.size1());
  size_t max_iter = 10000;
  double lambda, residual, dist;

  rhab::basic_iteration<vector_double_t::iterator> iter(max_iter, 1e-8);
  do {
    ++iter;
    cblas_dgemv(CblasRowMajor, CblasNoTrans, mat.size1(), mat.size2(), 1, &(mat.data()[0]), mat.size1(), &(t1.data()[0]), 1, 0.0, &(t2.data()[0]), 1);
    //ublas::axpy_prod(mat, t1, t2, true);
    t2 /= ublas::norm_1(t2);
    ++iter;
    cblas_dgemv(CblasRowMajor, CblasNoTrans, mat.size1(), mat.size2(), 1, &(mat.data()[0]), mat.size1(), &(t2.data()[0]), 1, 0.0, &(t1.data()[0]), 1);
    //ublas::axpy_prod(mat, t2, t1, true);
    t1 /= ublas::norm_1(t1);
  } while(!iter.converged(t2.begin(), t2.end(), t1.begin(), dist));

  size_t cnt_zero = std::count_if(t2.begin(), t2.end(), std::bind2nd(std::equal_to<double>(), 0.0));
  bool enought_non_zero   = ( cnt_zero < 0.7 * t2.size() );
  bool all_entries_finite = ( std::count_if(t2.begin(), t2.end(),
        boost::math::isfinite<double>) == (long)t2.size() );
  return ( enought_non_zero && all_entries_finite );
}

double rhab::calculate_error(const vector_double_t &exact,
                             const vector_double_t &dos,
                             error_mat_t* error_per_bin,
                             const size_t& index, bool normalize) {
  if (dos.size() == 0 || exact.size() == 0) {
    std::cerr << "exact or calculated density of states vector has zero length!" << std::endl;
    return -1;
  }

  // vector to calculate the error
  vector_double_t err(dos.size());
  std::fill(err.begin(), err.end(), 0.0);

  double sum = 0.0;
  size_t cnt = 0;
  double norm = 0;

  if (normalize) {
    /*
     * Density of states provided in dos is actually \f$\ln(\Omega)\f$.
     * Find the largest value, then subtract it from dos[i]
     * and sum exp(dos[i] - max) to calculate the norm.
     * Then divide the every exp(dos[i] - max) by the norm
     * and subtract the exact value, i.e. exact[i].
     * Calculate the absolute value of it and divide by exact[i].
     *
     * exact[i] is assumed to be positive
     *
     * @todo: calculate both error in density of states and entropy,
     *        i.e. additionally dos[i]-log(exact[i])/log(exact[i])
     */

    // create a copy
    vector_double_t d(dos);
    // and find median
    vector_double_t::iterator middle = d.begin()+(d.end()-d.begin())/2;
    std::nth_element(d.begin(), middle, d.end());
    double sub  = *middle;

    // calculate the norm
    for (size_t i = 0; i < dos.size(); i++) {
      // Be careful here! The least squares and minimization algorithms
      // tend to calculate very large values for states that have never been
      // visited. Having a very large max value results in the exp(foo-max)
      // to become 0
      if ((exact[i]) > 0) {
        norm += exp(dos[i]-sub);
#ifdef DEBUG
        if (!boost::math::isfinite(norm) || !boost::math::isfinite(dos[i])
            || !boost::math::isfinite(exp(dos[i]-sub))) {
          std::cerr << __FILE__ << ":" << __LINE__ << " "
                    << norm << " " << dos[i] << " " << exp(dos[i]-sub) << std::endl;
        }
#endif
      }
    }

    // yeah, I know that one should not compare doubles by equal
    if (norm == 0) {
      norm = 1;
    }

    for (size_t i = 0; i < dos.size(); i++) {
      // Be careful here and do not devide by 0
      if ((exact[i]) > 0) {
        err[i] = fabs( (exp(dos[i]-sub)/norm - exact[i]) / exact[i] );
        sum += err[i];
        cnt ++;
        (*error_per_bin)(index, i)(err[i]);
#ifdef DEBUG
        if (!boost::math::isfinite(sum) || !boost::math::isfinite(exact[i])
            || !boost::math::isfinite(err[i])) {
          std::cerr << __FILE__ << ":" << __LINE__ << " "
                    << sum << " " << norm << " "
                    << dos[i] <<  " " << exact[i] << std::endl;
        }
#endif
      }
    }
  } else {
    for (size_t i = 0; i < dos.size(); i++) {
      if (exact[i] > 0) {
        norm += dos[i];
      }
    }
    for (size_t i = 0; i < dos.size(); i++) {
      // Be careful here and do not divide by 0
      if (exact[i] > 0) {
        err[i] = fabs( (dos[i]/norm - exact[i]) / exact[i] );
        sum += err[i];
        cnt ++;
        (*error_per_bin)(index, i)(err[i]);
      }
#ifdef DEBUG
      if (!boost::math::isfinite(sum) || !boost::math::isfinite(dos[i])
          || !boost::math::isfinite(exact[i])
          || !boost::math::isfinite(err[i])) {
        std::cerr << __FILE__ << ":" << __LINE__ << " "
                  << sum << " " << dos[i] << " " << exact[i] << std::endl;
      }
#endif
    }
  }

  return (sum / cnt);
}

double rhab::calculate_error_q_matrix(const matrix_double_t &Qex, const matrix_double_t &Q) {
  double value(0.0);
  size_t cnt(0);
  for (size_t i = 0; i < Qex.data().size(); i++) {
    if (Qex.data()[i] > 0) {
      double tmp = fabs(Qex.data()[i] - Q.data()[i])/Qex.data()[i];
      value += tmp;
      cnt ++;
    }
  }
  return value / cnt;
}

boost::tuple<double, double, double, bool, bool, bool, double>
rhab::calculate_error_q(const vector_double_t &exact,
                        const matrix_double_t &Qexact,
                        const matrix_int_t &Q, matrix_double_t &Qd,
                        error_mat_tuple_t error_matrices,
                        vector_double_t &dos_lsq,
                        vector_double_t &dos_gth,
                        vector_double_t &dos_pow,
                        const size_t& index) {
  std::fill(dos_lsq.begin(), dos_lsq.end(), 0.0);
  std::fill(dos_gth.begin(), dos_gth.end(), 0.0);
  std::fill(dos_pow.begin(), dos_pow.end(), 0.0);

  // Least Squares
  bool lq = calculate_dos_leastsquares(Q, Qd, dos_lsq);
  //bool lq = calculate_dos_minimization(Q, Qd, dos);
  double error_lsq = calculate_error(exact, dos_lsq, error_matrices.get<0>(), index, true);
  if (!boost::math::isfinite(error_lsq)) {
    lq = false;
  }

  // Least Squares uses Qd as workspace only, so compute Qd
  normalize_q(Q, Qd);

  // calculate error in Q matrix
  double q_error = 0;
  if (Qexact.size1() > 0) {
    q_error = rhab::calculate_error_q_matrix(Qexact, Qd);
  }

  // GTH method
  bool gth = calculate_dos_gth(Qd, dos_gth);
  double error_gth = calculate_error(exact, dos_gth, error_matrices.get<1>(), index, false);

  // GTH Method modifies Qd, so recompute
  normalize_q(Q, Qd);

  // Power method
  bool pow = calculate_dos_power(Qd, dos_pow);
  double error_pow = calculate_error(exact, dos_pow, error_matrices.get<2>(), index, false);

  return boost::make_tuple(error_lsq, error_gth, error_pow,
                           lq,        gth,       pow,
                           q_error);
}

boost::tuple<double, double, double, bool,  bool,   bool>
rhab::calculate_error_q_lj(const vector_double_t &exact,
                           const matrix_int_t &Q, matrix_double_t &Qd,
                           error_mat_tuple_t error_matrices,
                           vector_double_t &dos_lsq,
                           vector_double_t &dos_gth,
                           vector_double_t &dos_pow,
                           const size_t& index) {
  std::fill(dos_lsq.begin(), dos_lsq.end(), 0.0);
  std::fill(dos_gth.begin(), dos_gth.end(), 0.0);
  std::fill(dos_pow.begin(), dos_pow.end(), 0.0);

  // Least Squares
  bool lq = calculate_dos_leastsquares(Q, Qd, dos_lsq);
  //bool lq = calculate_dos_minimization(Q, Qd, dos_lsq);
  double error_lsq = calculate_error(exact, dos_lsq, error_matrices.get<0>(), index, true);
  if (!boost::math::isfinite(error_lsq)) {
    lq = false;
  }

  // Least Squares uses Qd as workspace only, so compute Qd
  normalize_q(Q, Qd);

  // GTH method
  bool gth = calculate_dos_gth(Qd, dos_gth);
  double error_gth = calculate_error(exact, dos_gth, error_matrices.get<1>(), index, false);

  // GTH Method modifies Qd, so recompute
  normalize_q(Q, Qd);

  // Power method
  bool pow = calculate_dos_power(Qd, dos_pow);
  double error_pow = calculate_error(exact, dos_pow, error_matrices.get<2>(), index, false);

  return boost::make_tuple(error_lsq, error_gth, error_pow,
                           lq,        gth,       pow);
}

/* vim: set ts=2 sw=2 sts=2 tw=0 expandtab :*/


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_TRIANGULATION_HPP
#define PIC_COMPUTER_VISION_TRIANGULATION_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../image.hpp"

#include "../util/math.hpp"

#include "../util/eigen_util.hpp"

#include "../computer_vision/nelder_mead_opt_triangulation.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief triangulationLonguetHiggins computes triangulation using Longuet-Higgins equations.
 * @param point_0 is the point from the first view that matches point_1
 * @param point_1 is the point from the second view that matches point_0
 * @param R is the rotation matrix between the two views.
 * @param t is the translation matrix between the two views.
 * @return
 */
PIC_INLINE Eigen::Vector3d triangulationLonguetHiggins(Eigen::Vector3d &point_0, Eigen::Vector3d &point_1, Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
    Eigen::Vector3d ret;

    Eigen::Vector3d r_0 = Eigen::Vector3d(R(0, 0), R(0, 1), R(0, 2));
    Eigen::Vector3d r_2 = Eigen::Vector3d(R(2, 0), R(2, 1), R(2, 2));

    Eigen::Vector3d tmp = r_0 - point_1[0] * r_2;

    ret[2] = tmp.dot(t) / tmp.dot(point_0);

    ret[0] = point_0[0] * ret[2];
    ret[1] = point_0[1] * ret[2];

    return ret;
}

/**
 * @brief triangulationHartl
 * Sturm
 * @param point_0
 * @param point_1
 * @param R
 * @param t
 * @return
 */
PIC_INLINE Eigen::Vector4d triangulationHartleySturm(Eigen::Vector3d &point_0, Eigen::Vector3d &point_1,
                                          Eigen::Matrix34d &M0, Eigen::Matrix34d &M1, int maxIter = 100)
{
    Eigen::Vector4d M0_row[3], M1_row[3];

    for(int i = 0; i < 3; i++) {
        M0_row[i] = Eigen::Vector4d(M0(i, 0), M0(i, 1), M0(i, 2), M0(i, 3));
        M1_row[i] = Eigen::Vector4d(M1(i, 0), M1(i, 1), M1(i, 2), M1(i, 3));
    }

    Eigen::Vector4d x;
    double weight0 = 1.0;
    double weight0_prev = 1.0;

    double weight1 = 1.0;
    double weight1_prev = 1.0;

    int j = 0;
    while(j < maxIter) {
        Eigen::Vector4d A0 = (M0_row[0] - point_0[0] * M0_row[2]) / weight0;
        Eigen::Vector4d A1 = (M0_row[1] - point_0[1] * M0_row[2]) / weight0;

        Eigen::Vector4d A2 = (M1_row[0] - point_1[0] * M1_row[2]) / weight1;
        Eigen::Vector4d A3 = (M1_row[1] - point_1[1] * M1_row[2]) / weight1;

        Eigen::MatrixXd A(4, 4);
        for(int i = 0; i < 4; i++) {
            A(0, i) = A0[i];
            A(1, i) = A1[i];
            A(2, i) = A2[i];
            A(3, i) = A3[i];
        }

        Eigen::JacobiSVD< Eigen::MatrixXd > svdA(A, Eigen::ComputeFullV);
        Eigen::MatrixXd V = svdA.matrixV();
        int n = int(V.cols()) - 1;

        x[0] = V(0, n);
        x[1] = V(1, n);
        x[2] = V(2, n);
        x[3] = V(3, n);
        x /= x[3];

        weight0_prev = weight0;
        weight1_prev = weight1;

        weight0 = x.dot(M0_row[2]);
        weight1 = x.dot(M1_row[2]);

        double d0 = weight0_prev - weight0;
        double d1 = weight1_prev - weight1;
        double err = sqrt(d0 * d0 + d1 * d1);

        if(err < 1e-12){
            break;
        }

        j++;
    }

    #ifdef PIC_DEBUG
        printf("triangulationHartleySturm's Iterations: %d\n",j);
    #endif

    return x;
}

/**
 * @brief triangulationPoints
 * @param M0
 * @param M1
 * @param m0f
 * @param m1f
 * @param points_3d
 * @param colors
 * @param bColor
 */
PIC_INLINE void triangulationPoints(Eigen::Matrix34d &M0,
                                    Eigen::Matrix34d &M1,
                                    std::vector< Eigen::Vector2f > &m0f,
                                    std::vector< Eigen::Vector2f > &m1f,
                                    std::vector< Eigen::Vector3d > &points_3d,
                                    std::vector< unsigned char > &colors,
                                    Image *img0 = NULL,
                                    Image *img1 = NULL,
                                    bool bColor = false
                                  )
{
    if(m0f.size() != m1f.size()) {
        return;
    }

    NelderMeadOptTriangulation nmTri(M0, M1);
    for(unsigned int i = 0; i < m0f.size(); i++) {
        //normalized coordinates
        Eigen::Vector3d p0 = Eigen::Vector3d(m0f[i][0], m0f[i][1], 1.0);
        Eigen::Vector3d p1 = Eigen::Vector3d(m1f[i][0], m1f[i][1], 1.0);

        //triangulation
        Eigen::Vector4d point = triangulationHartleySturm(p0, p1, M0, M1);

        //non-linear refinement
        nmTri.update(m0f[i], m1f[i]);
        double tmpp[] = {point[0], point[1], point[2]};
        double out[3];
        nmTri.run(tmpp, 3, 1e-9f, 10000, &out[0]);

        //output
        points_3d.push_back(Eigen::Vector3d(out[0], out[1], out[2]));

        if(bColor) {
            float *color0 = (*img0)(int(m0f[i][0]), int(m0f[i][1]));
            float *color1 = (*img1)(int(m1f[i][0]), int(m1f[i][1]));

            for(int j = 0; j < img0->channels; j++) {
                float c_mean = (color0[j] + color1[j]) * 0.5f;
                c_mean = CLAMPi(c_mean, 0.0f, 1.0f);
                unsigned char c = int(c_mean * 255.0f);
                colors.push_back(c);
            }
        }
    }
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_TRIANGULATION_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Fabien Le Floc'h

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file analytichestonengine.hpp
    \brief analytic Heston expansion engine
*/

#ifndef quantlib_heston_expansion_engine_hpp
#define quantlib_heston_expansion_engine_hpp

#include <ql/pricingengines/genericmodelengine.hpp>
#include <ql/models/equity/hestonmodel.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <boost/function.hpp>

namespace QuantLib {

    //! Heston-model engine for European options based on analytic expansions
    /*! References:

        M Forde, A Jacquier, R Lee, The small-time smile and term
        structure of implied volatility under the Heston model
        SIAM Journal on Financial Mathematics, 2012 - SIAM

        M Lorig, S Pagliarani, A Pascucci, Explicit implied vols for
        multifactor local-stochastic vol models
        arXiv preprint arXiv:1306.5447v3, 2014 - arxiv.org

        \ingroup vanillaengines
    */
    class HestonExpansionEngine
        : public GenericModelEngine<HestonModel,
                                    VanillaOption::arguments,
                                    VanillaOption::results> {
      public:
        enum HestonExpansionFormula { LPP2, LPP3, Forde };

        HestonExpansionEngine(const boost::shared_ptr<HestonModel>& model,
                              HestonExpansionFormula formula);

        void calculate() const;

      private:
        const HestonExpansionFormula formula_;
    };

    /*! Interface to represent some Heston expansion formula.
        During calibration, it would typically be initialized once per
        implied volatility surface slice, then calls for each surface
        strike to impliedVolatility(strike, forward) would be
        performed.
    */
    class HestonExpansion {
      public:
        virtual ~HestonExpansion() {}
        virtual Real impliedVolatility(const Real strike,
                                       const Real forward) const = 0;
    };

    /*! Lorig Pagliarani Pascucci expansion of order-2 for the Heston model.
        During calibration, it can be initialized once per expiry, and
        called many times with different strikes.  The formula is also
        available in the Mathematica notebook from the authors at
        http://explicitsolutions.wordpress.com/
    */
    class LPP2HestonExpansion : public HestonExpansion {
      public:
        LPP2HestonExpansion(const Real kappa, const Real theta,
                            const Real sigma, const Real v0,
                            const Real rho, const Real term);
        virtual Real impliedVolatility(const Real strike,
                                       const Real forward) const;
      private:
        Real coeffs[3];
        Real ekt, e2kt, e3kt, e4kt;
        Real z0(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z1(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z2(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
    };

    /*! Lorig Pagliarani Pascucci expansion of order-3 for the Heston model.
        During calibration, it can be initialized once per expiry, and
        called many times with different strikes.  The formula is also
        available in the Mathematica notebook from the authors at
        http://explicitsolutions.wordpress.com/
    */
    class LPP3HestonExpansion : public HestonExpansion{
      public:
        LPP3HestonExpansion(const Real kappa, const Real theta,
                            const Real sigma, const Real v0,
                            const Real rho, const Real term);
        virtual Real impliedVolatility(const Real strike,
                                       const Real forward) const;
      private:
        Real coeffs[4];
        Real ekt, e2kt, e3kt, e4kt;
        Real z0(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z1(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z2(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z3(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
    };

    /*! Small-time expansion from
        "The small-time smile and term structure of implied volatility
        under the Heston model" M Forde, A Jacquier, R Lee - SIAM
        Journal on Financial Mathematics, 2012 - SIAM
    */
    class FordeHestonExpansion : public HestonExpansion {
      public:
        FordeHestonExpansion(const Real kappa, const Real theta,
                             const Real sigma, const Real v0,
                             const Real rho, const Real term);
        virtual Real impliedVolatility(const Real strike,
                                       const Real forward) const;
      private:
        Real coeffs[5];
    };

}

#endif


#include <algorithm>
#include <iostream>
#include <fstream>
#include <numeric>
#include <vector>
#include <NTL/ZZ.h>

NTL::ZZ mortal_fib(uintmax_t n, uintmax_t m)
{
    std::vector<NTL::ZZ> rabbits(m);
    rabbits.at(0) = 1;
    for (decltype(n) i = 0; i < n - 1; ++i) {
        auto tmp = rabbits.at(0), total = NTL::to_ZZ(0);
        for (decltype(m) j = 1; j < m; ++j) {
            total += rabbits.at(j);
            std::swap(rabbits.at(j), tmp);
        }
        rabbits.at(0) = total;
    }
    return std::accumulate(std::begin(rabbits),
            std::end(rabbits),
            NTL::to_ZZ(0));
}

int main()
{
    std::ifstream f("data/rosalind_fibd.txt");
    uintmax_t n, m;
    f >> n;
    f >> m;
    f.close();
    std::cout << mortal_fib(n, m) << std::endl;
    return EXIT_SUCCESS;
}


#pragma once

#include <boost/math/constants/constants.hpp>
#include <cmath>

#include "GNAObject.hh"

class GaussianPeakWithBackground: public GNAObject,
                                  public TransformationBind<GaussianPeakWithBackground> {
public:
  GaussianPeakWithBackground() {
    variable_(&m_b, "BackgroundRate");
    variable_(&m_mu, "Mu");
    variable_(&m_E0, "E0");
    variable_(&m_w, "Width");

    transformation_("rate")
      .input("E")
      .output("rate")
      .types(Atypes::pass<0,0>)
      .func(&GaussianPeakWithBackground::calcRate)
      ;
  }

  void calcRate(Args args, Rets rets) {
    const double pi = boost::math::constants::pi<double>();
    const auto &E = args[0].arr;
    rets[0].arr = m_b + m_mu*(1./std::sqrt(2*pi*m_w))*(-(E-m_E0).square()/(2*m_w*m_w)).exp();
  }
protected:
  variable<double> m_b, m_mu, m_E0, m_w;
};


#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Eigen_solver_traits.h>
#include <CGAL/Mean_curvature_flow_skeletonization.h>
#include <CGAL/iterator.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/IO/Polyhedron_iostream.h>
#include <CGAL/Bbox_3.h>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <CGAL/boost/iterator/transform_iterator.hpp>

#include <Eigen/SparseLU>
#include <Eigen/Sparse>

#include <fstream>
#include <map>

typedef CGAL::Simple_cartesian<double>                                    Kernel;
typedef Kernel::Point_3                                                    Point;
typedef CGAL::Polyhedron_3<Kernel, CGAL::Polyhedron_items_with_id_3> Polyhedron;

typedef boost::graph_traits<Polyhedron>::vertex_descriptor    vertex_descriptor;

typedef CGAL::Eigen_solver_traits<
        Eigen::SparseLU<
        CGAL::Eigen_sparse_matrix<double>::EigenType,
        Eigen::COLAMDOrdering<int> >  >                         SparseLU_solver;

typedef CGAL::Eigen_solver_traits<
        Eigen::SimplicialLDLT<
        CGAL::Eigen_sparse_matrix<double>::EigenType
         >  >                                             SimplicialLDLT_solver;

typedef CGAL::Default                                                         D;

// The input of the skeletonization algorithm must be a pure triangular closed
// mesh and has only one component.
bool is_mesh_valid(Polyhedron& pMesh)
{
  if (!pMesh.is_closed())
  {
    std::cerr << "The mesh is not closed.";
    return false;
  }
  if (!pMesh.is_pure_triangle())
  {
    std::cerr << "The mesh is not a pure triangle mesh.";
    return false;
  }

  // the algorithm is only applicable on a mesh
  // that has only one connected component
  std::size_t num_component;
  CGAL::Counting_output_iterator output_it(&num_component);
  CGAL::internal::corefinement::extract_connected_components(pMesh, output_it);
  ++output_it;
  if (num_component != 1)
  {
    std::cerr << "The mesh is not a single closed mesh. It has "
              << num_component << " components.";
    return false;
  }
  return true;
}

int main()
{
  Polyhedron mesh;
  std::ifstream input(CGAL::data_file_path("meshes/elephant.off"));

  if ( !input || !(input >> mesh) || mesh.empty() ) {
    std::cerr << "Cannot open data/elephant.off" << std::endl;
    return EXIT_FAILURE;
  }
  if (!is_mesh_valid(mesh)) {
    return EXIT_FAILURE;
  }

  int NTEST = 10;
  double sum = 0;
  for (int i = 0; i < NTEST; i++)
  {
    typedef CGAL::Mean_curvature_flow_skeletonization<Polyhedron, D, D, SparseLU_solver> MCF_skel;
    MCF_skel::Skeleton skeleton;

    CGAL::Timer timer;
    timer.start();
    MCF_skel mcf_skel(mesh);
    mcf_skel(skeleton);
    timer.stop();
    sum += timer.time();
  }
  std::cout << "Time of SparseLU: " << sum / NTEST << "\n";

  sum = 0;
  for (int i = 0; i < NTEST; i++)
  {
    typedef CGAL::Mean_curvature_flow_skeletonization<Polyhedron, D, D, SimplicialLDLT_solver> MCF_skel;
    MCF_skel::Skeleton skeleton;

    CGAL::Timer timer;
    timer.start();
    MCF_skel mcf_skel(mesh);
    mcf_skel(skeleton);
    timer.stop();
    sum += timer.time();
  }
  std::cout << "Time of SimplicialLDLT: " << sum / NTEST << "\n";

  return EXIT_SUCCESS;
}


#include <fstream>
#include <assert.h>
#include <stdlib.h>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/plod_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graph_traits.hpp>

void printUsageAndExit()
{
  printf("%s", "Usage:./plodg x\n");
  printf("%s", "x is the size of the graph\n");
  exit(0);
}

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

  /* " The Power Law Out Degree (PLOD) algorithm generates a scale-free graph from three parameters, n, alpha, and beta.
  [...] The value of beta controls the y-intercept of the curve, so that increasing beta increases the average degree of vertices (credit = beta*x^-alpha).
  [...] The value of alpha controls how steeply the curve drops off, with larger values indicating a steeper curve. */
  // From Boost documentation http://www.boost.org/doc/libs/1_47_0/libs/graph/doc/plod_generator.html

  // we use setS aka std::set for edges storage
  // so we have at most one edges between 2 vertices
  // the extra cost is O(log(E/V)).
  typedef boost::adjacency_list<boost::setS> Graph;
  typedef boost::plod_iterator<boost::minstd_rand, Graph> SFGen;

  if (argc < 2) printUsageAndExit();
  int size = atoi (argv[1]);
  assert (size > 1 && size < INT_MAX);
  double alpha = 2.57; // It is known that web graphs have alpha ~ 2.72.
  double beta = size*512+1024; // This will give an average degree ~ 15

  // generation
  std::cout << "generating ... "<<'\n';
  boost::minstd_rand gen;
  Graph g(SFGen(gen, size, alpha, beta, false), SFGen(), size);
  boost::graph_traits<Graph>::edge_iterator edge, edge_end;

  std::cout << "vertices : "      << num_vertices(g) <<'\n';
  std::cout << "edges : "         << num_edges(g) <<'\n';
  std::cout << "average degree : "<< static_cast<float>(num_edges(g))/num_vertices(g)<< '\n';
  // Print in matrix coordinate real general format
  std::cout << "writing ... "<<'\n';
  std::stringstream tmp;
  tmp <<"local_test_data/plod_graph_" << size << ".mtx";
  const std::string filename = tmp.str();
  std::ofstream fout(tmp.str().c_str()) ;

  if (argv[2]==NULL)
  {
    // Power law out degree with random weights
    fout << "%%MatrixMarket matrix coordinate real general\n";
    fout << num_vertices(g) <<' '<< num_vertices(g)  <<' '<< num_edges(g) << '\n';
    float val;
    for( boost::tie(edge, edge_end) = boost::edges(g); edge != edge_end; ++edge)
    {
      val = (rand()%10)+(rand()%100)*(1e-2f);
      fout << boost::source(*edge, g) << ' ' << boost::target(*edge, g)<< ' ' << val << '\n';
    }
  }
  else if (argv[2][0]=='i')
  {
    // Power law in degree (ie the transpose will have a power law)
    // -- Edges only --
    // * Wraning * edges will be unsorted, use sort_edges.cpp to sort the dataset.
    fout << num_vertices(g) <<' '<< num_edges(g) << '\n';
    for( boost::tie(edge, edge_end) = boost::edges(g); edge != edge_end; ++edge)
      fout <<boost::target(*edge, g)<< ' ' << boost::source(*edge, g) << '\n';
  }
  else if (argv[2][0]=='o')
  {
    // Power law out degree
    // -- Edges only --
    fout << num_vertices(g) <<' '<< num_edges(g) << '\n';
    for( boost::tie(edge, edge_end) = boost::edges(g); edge != edge_end; ++edge)
      fout << boost::source(*edge, g) << ' ' << boost::target(*edge, g)<< '\n';
  }
  else printUsageAndExit();

  fout.close();
  std::cout << "done!"<<'\n';
  return 0;
}


/* Tags: Delaunay Triangulation, Union-Find, Connected Components, Closest Neighbor

  Key idea: 1:1 correspondence between points being connected (connected components in EMST)
                <==>
                we can move between the disks of the points
                (for a radius >= 2 * minimal distance betw. them)
            Can copy code from the EMST template, and adapt:
            * three UF structures, one for the given radius U_p,
              U_a: minimum power needed to execute _all_ missions
              U_b: minimum power needed to execute same set of missions as with inital p
            * First connect components of U_p with edges <= p
            * then see, for each mission, if 4*distance <= p and
              components are connected, mission can be executed
            * If can be executed: enlarge U_b until it is covered
            * In any case: enlarge U_a until it is covered

*/
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <boost/pending/disjoint_sets.hpp>
#include <vector>
#include <tuple>
#include <algorithm>
#include <iostream>

// Epic kernel is enough, no constructions needed, provided the squared distance
// fits into a double (!)
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
// we want to store an index with each vertex
typedef std::size_t                                            Index;
typedef CGAL::Triangulation_vertex_base_with_info_2<Index,K>   Vb;
typedef CGAL::Triangulation_face_base_2<K>                     Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>            Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                  Triangulation;

// As edges are not explicitly represented in the triangulation, we extract them
// from the triangulation to be able to sort and process them. We store the
// indices of the two endpoints, first the smaller, second the larger, and third
// the squared length of the edge. The i-th entry, for i=0,... of a tuple t can
// be accessed using std::get<i>(t).
typedef std::tuple<Index,Index,double> Edge;
typedef std::vector<Edge> EdgeV;

void solve() {
  Index n, m;
  double p;
  std::cin >> n >> m >> p;

  // read points
  typedef std::pair<K::Point_2,Index> IPoint;
  std::vector<IPoint> jammers;
  jammers.reserve(n);
  for (Index i = 0; i < n; ++i) {
    int x, y;
    std::cin >> x >> y;
    jammers.push_back({K::Point_2(x, y), i});
  }
  // construct triangulation
  Triangulation t;
  t.insert(jammers.begin(), jammers.end());

  // extract edges and sort by (squared) length
  // This step takes O(n log n) time (for the sorting).
  EdgeV edges;
  edges.reserve(3*n); // there can be no more in a planar graph
  for (auto e = t.finite_edges_begin(); e != t.finite_edges_end(); ++e) {
    Index i1 = e->first->vertex((e->second+1)%3)->info();
    Index i2 = e->first->vertex((e->second+2)%3)->info();
    // ensure smaller index comes first
    if (i1 > i2) std::swap(i1, i2);
    edges.emplace_back(i1, i2, t.segment(e).squared_length());
  }
  std::sort(edges.begin(), edges.end(),
      [](const Edge& e1, const Edge& e2) -> bool {
        return std::get<2>(e1) < std::get<2>(e2);
            });

  // setup and initialize union-find data structure for initial power
  boost::disjoint_sets_with_storage<> uf_p(n);
  for (EdgeV::const_iterator e = edges.begin(); e != edges.end(); ++e) {
    // determine components of endpoints
    Index c1 = uf_p.find_set(std::get<0>(*e));
    Index c2 = uf_p.find_set(std::get<1>(*e));
    double squared_dist = std::get<2>(*e);
    if (squared_dist <= p){
      if(c1 != c2) {
       // this edge connects two different components => part of the emst
        uf_p.link(c1, c2);
      }
    } else {
      break;
    }
  }

  K::FT a = 0, b = 0;
  // setup and initialize union-find data structure for
  // a: minimum power needed to execute _all_ missions
  // b: minimum power needed to execute same set of missions as with inital p

  boost::disjoint_sets_with_storage<> uf_a(n);
  boost::disjoint_sets_with_storage<> uf_b(n);

  // we continously enlarge the tree components for a and b,
  // so we need to remember where we left off for new missions
  EdgeV::const_iterator e_iter_a = edges.begin();
  EdgeV::const_iterator e_iter_b = edges.begin();
  Index n_components_a = n;
  Index n_components_b = n;
  for (Index j = 0; j < m; j++) {
    int x0, y0, x1, y1; std::cin >> x0 >> y0 >> x1 >> y1;
    auto sj = K::Point_2(x0, y0);
    auto tj = K::Point_2(x1, y1);

    auto vertex_sj = t.nearest_vertex(sj);
    auto vertex_tj = t.nearest_vertex(tj);
    double dist_sj = CGAL::squared_distance(sj, vertex_sj->point());
    double dist_tj = CGAL::squared_distance(tj, vertex_tj->point());
    K::FT max_dist_start = 4 * std::max(dist_sj, dist_tj);

    Index vi0 = vertex_sj->info();
    Index vi1 = vertex_tj->info();

    // mission is possible with intial power
    if(max_dist_start <= p && uf_p.find_set(vi0) == uf_p.find_set(vi1)) {
      std::cout << "y";
      double squared_dist_needed = 0;
      // since covered, increase the tree of b until points are connected
      while(uf_b.find_set(vi0) != uf_b.find_set(vi1) && e_iter_b != edges.end()
            && n_components_b != 0) {
        Index c1 = uf_b.find_set(std::get<0>(*e_iter_b));
        Index c2 = uf_b.find_set(std::get<1>(*e_iter_b));
        squared_dist_needed = std::get<2>(*e_iter_b);
        if(c1 != c2) {
          // this edge connects two different components
          uf_b.link(c1, c2);
          n_components_b--;
        }
        e_iter_b++;
      }
      b = std::max({squared_dist_needed, max_dist_start, b});
    } else {
      std::cout << "n";
    }

    // same as above but for trees a
    double squared_dist_needed = 0;
    // since covered, increase the tree of b until points are connected
    while(uf_a.find_set(vi0) != uf_a.find_set(vi1) && e_iter_a != edges.end()
           && n_components_a != 0) {
      Index c1 = uf_a.find_set(std::get<0>(*e_iter_a));
      Index c2 = uf_a.find_set(std::get<1>(*e_iter_a));
      squared_dist_needed = std::get<2>(*e_iter_a);
      if(c1 != c2) {
        // this edge connects two different components
        uf_a.link(c1, c2);
        n_components_a--;
      }
      e_iter_a++;
    }
    a = std::max({squared_dist_needed, max_dist_start, a});
  }

  std::cout << "\n" << a << "\n" << b << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(0);
  std::size_t t;
  for (std::cin >> t; t > 0; --t) solve();
  return 0;
}


/*! Little utility to generate a camflow grid.
 *  Laurence McGlashan (lrm29@cam.ac.uk)
 */

#include "boost/program_options.hpp"
#include <boost/math/distributions/triangular.hpp>
#include <boost/math/distributions/normal.hpp>
#include <fstream>
#include <iomanip>
#include <numeric>
#include <iostream>

using namespace boost::program_options;
using namespace boost::math;
using namespace std;

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

    cout << argv[0] << " by Laurence McGlashan\n" << endl;

    string outputFile = "grid.inp";
    string distType;
    double stMixFrac = -1;
    int numberOfCells, numberOfCellsLower, numberOfCellsUpper = -1;

    {
        // Parse arguments.
        options_description desc("Allowed options for program");
        desc.add_options()
            ("help", "Show this help message.")
            ("stoich", value<double>(), "Stoichiometric mixture fraction.")
            ("outputFile", value<string>(), "Output file for grid.")
            ("cells", value<int>(), "Number of cells.")
            ("distribution", value<string>(), "Type of distribution to weight points against (triangular|normal).");

        variables_map vm;
        store(parse_command_line(argc, argv, desc), vm);
        notify(vm);

        if (vm.count("help")) {
            cout << desc << "\n";
            return 1;
        }

        if (vm.count("stoich")) {
            stMixFrac = vm["stoich"].as<double>();
            if (stMixFrac <= 0)
                throw std::logic_error("Stochiometric mixture fraction must be greater than 0.\n");
            cout<< "Stochiometric mixture fraction is " << stMixFrac << ".\n";
        } else {
            throw std::logic_error("Stochiometric mixture fraction was not provided.\n");
        }

        if (vm.count("cells")) {
            numberOfCells = vm["cells"].as<int>();
            if (numberOfCells <= 0)
                throw std::logic_error("Number of cells must be greater than 2.\n");
            if (numberOfCells%2 != 0)
                throw std::logic_error("Number of cells must be even.\n");
            cout<< "Number of cells is " << numberOfCells << ".\n";
        } else {
            throw std::logic_error("Number of cells was not provided.\n");
        }

        if (vm.count("distribution")) {
            distType = vm["distribution"].as<string>();
            if (distType != "triangular" && distType != "normal")
                    throw std::logic_error("Distribution " + distType + " not available.\n");
            cout<< "Distribution used will be " << distType << ".\n";
        } else {
            throw std::logic_error("Distribution was not provided.\n");
        }

        if (vm.count("outputFile")) {
            outputFile = vm["outputFile"].as<string>();
            cout<< "Grid will be output to "
                << outputFile << ".\n";
        } else {
            cout<< "outputFile was not provided. Use default of grid.inp.\n";
        }
    }

    // Calculate the grid here. The values are the cell edges.
    vector<double> grid;
    grid.push_back(0.0);
    grid.push_back(stMixFrac);
    grid.push_back(1.0);

    numberOfCellsLower = numberOfCells/2.0;
    numberOfCellsUpper = numberOfCells/2.0;

    //////////// Generate grid here ///////////

    // Construct uniform grid.
    for (size_t i=1; i<numberOfCellsLower; ++i)
    {
            grid.push_back(2.0*i*stMixFrac/numberOfCells);
    }
    for (size_t i=1; i<numberOfCellsUpper; ++i)
    {
            grid.push_back(stMixFrac + 2.0*i*(1.0-stMixFrac)/numberOfCells);
    }

    // Sort the values.
    sort(grid.begin(), grid.end());

    if (distType == "triangular")
    {
        triangular s(0.0,stMixFrac,1.0);
        double max = pdf(s,stMixFrac);
        for (size_t i=1; i<=numberOfCells; ++i)
        {
            grid[i] = stMixFrac + (grid[i]-stMixFrac)*(1.0-pdf(s,grid[i])/max);
        }
    }

    if (distType == "normal")
    {
        normal s(stMixFrac,0.5);
        vector<double> spacingFactor, pdfSaved;

        for (size_t i=0; i<=numberOfCells; ++i)
            pdfSaved.push_back(pdf(s,grid[i]));

        for (size_t i=0; i<numberOfCells; ++i)
            spacingFactor.push_back(abs(pdfSaved[i+1] - pdfSaved[i]));

        double normalise = accumulate(spacingFactor.begin(),spacingFactor.end(),0.0);
        for (size_t i=1; i<numberOfCells; ++i)
            grid[i] = grid[i-1] + abs(pdfSaved[i]-pdfSaved[i-1])/normalise;
    }

    //////////// End Generate grid here ///////

    // Sort the values.
    sort(grid.begin(), grid.end());

    // Output the grid.
    ofstream out;
    out.open(outputFile.c_str(), ios::trunc);
    if (out.good())
    {
        for (size_t i=0; i<grid.size(); ++i)
        {
            out << setprecision(10) << grid[i] << endl;
        }
    }

    cout << "\nProgram End." << endl;

    return 0;

}


#ifndef QUADRATICPROBLEM_H
#define QUADRATICPROBLEM_H

#include <CoMISo/Config/config.hh>
#include <CoMISo/Utils/StopWatch.hh>
#include <vector>
#include <CoMISo/NSolver/NProblemInterface.hh>
#include <Base/Code/Quality.hh>
LOW_CODE_QUALITY_SECTION_BEGIN
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Sparse>
LOW_CODE_QUALITY_SECTION_END

//== NAMESPACES ===============================================================

namespace COMISO {

// this problem optimizes the quadratic functional 0.5*x^T A x -x^t b + c
class QuadraticProblem : public COMISO::NProblemInterface
{
public:

  // Sparse Matrix Type
  //  typedef Eigen::DynamicSparseMatrix<double,Eigen::ColMajor> SMatrixNP;

  QuadraticProblem()
  : A_(0,0), b_(Eigen::VectorXd::Index(0)), c_(0.0)
  {

  }

  QuadraticProblem(SMatrixNP& _A, Eigen::VectorXd& _b, const double _c)
  : A_(_A), c_(_c)
  {
    if(A_.rows() != A_.cols())
      std::cerr << "Warning: matrix not square in QuadraticProblem" << std::endl;
    b_ = _b;
    x_ = Eigen::VectorXd::Zero(A_.cols());
  }

  // number of unknowns
  virtual int n_unknowns()
  {
     return A_.rows();
  }

  // initial value where the optimization should start from
  virtual void initial_x(double* _x)
  {
        for( int i=0; i<this->n_unknowns(); ++i)
            _x[i] = x_[i];
  }

  // function evaluation at location _x
  virtual double eval_f( const double* _x )
  {
    Eigen::Map<const Eigen::VectorXd> x(_x, this->n_unknowns());

    return (double)(x.transpose()*A_*x)*0.5 - (double)(x.transpose()*b_) + c_;
  }

  // gradient evaluation at location _x
  virtual void   eval_gradient( const double* _x, double*    _g)
  {
    Eigen::Map<const Eigen::VectorXd> x(_x, this->n_unknowns());
    Eigen::Map<Eigen::VectorXd> g(_g, this->n_unknowns());

    g = A_*x - b_;
   }

  // hessian matrix evaluation at location _x
  virtual void   eval_hessian ( const double* _x, SMatrixNP& _H)
  {
    _H = A_;
  }

  // print result
  virtual void   store_result ( const double* _x               )
  {
    Eigen::Map<const Eigen::VectorXd> x(_x, this->n_unknowns());
    x_ = x;
  }

  // get current solution
  Eigen::VectorXd& x() { return x_;}

  // advanced properties
  virtual bool   constant_hessian() const { return true; }

  void set_A(const SMatrixNP& _A)
  {
    A_ = _A;
    if(A_.rows() != A_.cols())
        std::cerr << "Warning: matrix not square in QuadraticProblem" << std::endl;
    x_ = Eigen::VectorXd::Zero(A_.cols());
  }

  void set_b(const Eigen::VectorXd& _b)
  {
    b_ = _b;
  }

  void set_c( const double _c)
  {
    c_ = _c;
  }

private:

  // quadratic problem 0.5*x^T A x -x^t b + c
 SMatrixNP       A_;
 Eigen::VectorXd b_;
 double          c_;
 // current solution, which is also used as initial value
 Eigen::VectorXd x_;
};

//=============================================================================
} // namespace COMISO
//=============================================================================

#endif // QUADRATICPROBLEM_H


#include "pch.h"
#include "CalculatorCore.h"
#include <stack>
#include <sstream>
#include <string>
#include <unordered_map>
#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace std;
using namespace boost::multiprecision;

auto op_dict = unordered_map<char, int>{
    {'\n', 0},
    {')',  0},
    {'&',  1},
    {'|',  1},
    {'+',  2},
    {'-',  2},
    {'*',  3},
    {'/',  3},
    {'!',  4},
    {'(',  5}
};

auto __calculator(stringstream& in, bool isInitial) -> cpp_dec_float_100 {
    auto ops = stack<char>();
    auto nums = stack<cpp_dec_float_100>();
    auto calculate = [&] {
        if (nums.empty())
            throw runtime_error("Invalid expression");
        auto op = ops.top();
        ops.pop();
        auto res = (cpp_dec_float_100)0;
        if (op == '!') {
            auto x = nums.top();
            nums.pop();
            res = !x;
            nums.push(res);
            return;
        }
        if (nums.size() < 2)
            throw runtime_error("Invalid expression");
        auto y = nums.top();
        nums.pop();
        auto x = nums.top();
        nums.pop();
        if (op == '+')
            res = x + y;
        else if (op == '-')
            res = x - y;
        else if (op == '*')
            res = x * y;
        else if (op == '&')
            res = x && y;
        else if (op == '|')
            res = x || y;
        else {
            if (y == 0)
                throw runtime_error("Math error");
            res = x / y;
        }
        nums.push(res);
    };
    while (!in.eof()) {
        if (isdigit(in.peek())) {
            auto temp = string();
            while (isdigit(in.peek()))
                temp += (char)in.get();
            nums.push(cpp_dec_float_100(temp));
            continue;
        }
        auto temp = (char)in.get();
        auto priority = op_dict[temp];
        if (priority == 2) {
            while (op_dict[in.peek()] == 2)
                temp = temp == in.get() ? '+' : '-';
            if (nums.empty())
                nums.push(0);
        }
        else if (priority == 3) {
            auto ch = '+';
            while (op_dict[in.peek()] == 2)
                ch = ch == in.get() ? '+' : '-';
            if (!nums.empty() && ch == '-') {
                auto temp_num = nums.top();
                nums.pop();
                nums.push(-temp_num);
            }
        }
        if (temp == '(') {
            nums.push(__calculator(in, false));
            continue;
        }
        while (!ops.empty() && priority <= op_dict[ops.top()])
            calculate();
        if (temp == ')') {
            if (isInitial)
                throw runtime_error("Invalid expression");
            break;
        }
        if (temp == '\n')
            break;
        ops.push(temp);
    }
    if (!ops.empty() || nums.size() != 1)
        throw runtime_error("Invalid expression");
    return nums.top();
}

auto calculator(stringstream& in) -> cpp_dec_float_100 {
    return __calculator(in, true);
}


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

#pragma once

#include <cstdint>
#include <array>
#include <queue>

#include <Eigen/Dense>

#include "hnf.hpp"

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

namespace khover {

//! Representation of abelian groups by direct sums of cyclic groups.
struct AbGroupCyc {
    //! Rank of the free part.
    std::size_t freerank;
    //! A list of torsions.
    std::vector<int> torsions;

    //! Check if the group is torion-free
    inline bool isTorFree() const noexcept {
        return torsions.empty();
    }

    //! Check if the group is finite (<=> trivial after tensored with Q)
    inline bool isFinite() const noexcept {
        return freerank == 0;
    }

    //! Check if the group is trivial
    inline bool isZero() const noexcept {
        return isFinite() && isTorFree();
    }

    //! Pretty printer
    std::string pretty() const noexcept {
        std::string str_free
            = freerank == 0 ? std::string{} : freerank == 1 ? std::string("Z") : "Z^" + std::to_string(freerank);
        std::string str_tor{};

        for(int t : torsions) {
            if (!str_tor.empty())
                str_tor += "+";
            str_tor += "Z/" + std::to_string(t);
        }

        if(str_free.empty() && str_tor.empty())
            return std::string("0");
        else if (str_free.empty())
            return str_tor;
        else if (str_tor.empty())
            return str_free;
        else
            return str_free + "+" + str_tor;
    }
};

//! Representation of abelian groups by representation matrices.
class AbelianGroup {
public:
    using integer_t = int64_t;
    using matrix_t = Eigen::Matrix<integer_t,Eigen::Dynamic,Eigen::Dynamic>;

private:
    //! The presentation matrix, which is kept to be a column HNF.
    matrix_t m_repMat;
    //! The least rank of the free part of the abelian group.
    std::size_t m_freerk;

public:
    //! Default constructor.
    AbelianGroup() noexcept : m_repMat(0,0), m_freerk(0) {}

    //! Constructor from a presentation matrix.
    //! Represent an abelian group by a presentation matrix.
    //! \tparam IsColHNF If IsColHNF::value == true, then repMat must be in the column HNF. Otherwise, its column HNF is computed.
    template <class Derived, class IsColHNF>
    AbelianGroup(Eigen::MatrixBase<Derived> const& repMat, IsColHNF, std::size_t freerk = 0) noexcept
        : m_repMat(repMat), m_freerk(freerk)
    {
        if constexpr(!IsColHNF::value) {
            auto rk = hnf_LLL<khover::colops>(m_repMat, {}, {});
            if(rk) {
                m_repMat = m_repMat.leftCols(*rk).eval();
            }
            else {
                ERR_MSG("Something bad happended.");
            }
        }
    }

    //! Nothing special to do in destructor.
    ~AbelianGroup() = default;

    //! Get the number of generators.
    inline std::size_t ngens() const noexcept {
        return m_repMat.rows() + m_freerk;
    }

    //! Get the number of relations.
    inline std::size_t nrels() const noexcept { return m_repMat.cols(); }

    //! Get the presentation matrix.
    inline auto get_repmatrix() const noexcept {
        return (matrix_t(ngens(),nrels()) << m_repMat, matrix_t::Zero(m_freerk, m_repMat.cols())).finished();
    }

    //! Reduce the number of generators and relations by computing the Hermite normal form of the representation matrix.
    //! This will suffice in order to compute the rank of the free part.
    //! \param post Homomorphisms whose domains are *this*. They will be transformed so that their domains will be the reduced one.
    //! \param pre Homomorphisms whose codomains are *this*. They will be transformed so that their codomains will be the reduced one.
    //! \return If the function fails to compute HNF correctly, or if the homomorphisms are wrong, then it returns false.
    template <class...Posts, class...Pres>
    bool reduce(
        std::tuple<Posts&...> const & posts,
        std::tuple<Pres&...> const & pres
        ) noexcept
    {
        static_assert(
            std::conjunction_v<
            std::bool_constant<Posts::ColsAtCompileTime == Eigen::Dynamic>...
            >,
            "The function may change the number of columns of matrices in the posts parameter, so make sure they have dynamic numbers of columns.");

        static_assert(
            std::conjunction_v<
            std::bool_constant<Pres::RowsAtCompileTime == Eigen::Dynamic>...
            >,
            "The function may change the number of rows of matrices in the posts parameter, so make sure they have dynamic numbers of rows.");

        /*
         * Take columns with pivot = 1 to left
         */
        // The rank of the presentation matrix over Q (the field of rationals).
        std::size_t rk = 0;
        // The number of columns with pivot = 1.
        std::size_t upivs = 0;

        // Traverse pivots
        using Ops = khover::colops;
        for(std::size_t i=0; i < Ops::size(m_repMat) && rk < Ops::dual_t::size(m_repMat); ++i) {
            // A column with pivot 1 is found.
            if (Ops::at(m_repMat, rk).coeff(i) == 1) {
                // If there is a column with non-unit pivot on left, swap with it.
                if (rk > upivs) {
                    Ops::swap(m_repMat, rk, upivs);
                }
                // If the pivot is below diagonal, raise it.
                if (i > upivs) {
                    Ops::dual_t::swap(m_repMat, i, upivs);
                    khover::for_each_tuple(
                        pres,
                        [i,upivs](auto& u) {
                            Ops::dual_t::swap(u, i, upivs);
                        });
                    khover::for_each_tuple(
                        posts,
                        [i,upivs](auto& v) {
                            Ops::swap(v, i, upivs);
                        });
                }
                ++upivs;
                ++rk;
            }
            // A column with pivot != 1 is found.
            else if (Ops::at(m_repMat, rk).coeff(i) != 0) {
                ++rk;
            }
        }

        // Reduce homomorphisms
        matrix_t redPiv_mat
            = matrix_t::Identity(
                m_repMat.rows() + m_freerk,
                m_repMat.rows() + m_freerk
                ).bottomRows(m_repMat.rows() + m_freerk - upivs);
        redPiv_mat.block(0, 0, m_repMat.rows() - upivs, upivs).noalias()
            = - m_repMat.bottomLeftCorner(m_repMat.rows() - upivs, upivs);

        khover::for_each_tuple(
            pres,
            [&](auto& u) {
                u = redPiv_mat.bottomRows(m_repMat.rows() + m_freerk - upivs) * u;
            } );
        khover::for_each_tuple(
            posts,
            [upivs](auto& v) {
                v = v.rightCols(v.cols() - upivs); //v.block(0, upivs, v.rows(), v.cols()-upivs).eval();
            } );

        // Forget thw row/columns with unital pivots.
        m_freerk += m_repMat.rows() - rk;
        m_repMat = m_repMat.block(upivs, upivs, m_repMat.rows() - upivs, rk - upivs).eval();
        rk -= upivs;

        // Compute the row HNF
        if (!hnf_LLL<khover::rowops>(m_repMat,posts, pres))
            return false;

        // Reduce the presentation matrix again.
        m_repMat = m_repMat.topRows(rk).eval();

        // Keep the presentation matrix in column HNF.
        khover::hnf_LLL<khover::colops>(m_repMat, {}, {});

        // Finish successfully.
        return true;
    }

    //! Compute the abelian group in the form of the pair of the free rank and the list of torsions.
    //! \remark The result is not necessarily in the normal form.
    AbGroupCyc
    compute() noexcept {
        do {
            reduce({}, {});
        } while(!m_repMat.isDiagonal());

        auto diag = m_repMat.diagonal();
        std::vector<int> torsions{};
        std::size_t r = 0;

        for(int i = 0; i < diag.size(); ++i) {
            if (diag.coeff(i) == 0)
                ++r;
            else if(std::abs(diag.coeff(i)) != 1)
                torsions.push_back(std::abs(diag.coeff(i)));
        }

        return {r+m_freerk, std::move(torsions)};
    }

    //! Compute the image of a homomorphism whose codomain is this abelian group.
    //! \tparam ReturnMorphism If ReturnMorphism::value == true, then the function also returns the matrix representing the morphism from the image to this group.
    template <class Derived, class DoesReturnMorphism = std::false_type>
    auto
    image(
        Eigen::MatrixBase<Derived> const& morph,
        DoesReturnMorphism = DoesReturnMorphism{}
        ) const noexcept
        -> std::conditional_t<
            DoesReturnMorphism::value,
            std::optional<std::pair<AbelianGroup,matrix_t>>,
            std::optional<AbelianGroup>
            >
    {
        // If the number of rows doesn't agree, return immediately.
        if(morph.rows() != static_cast<int>(m_freerk) + m_repMat.rows()) {
            ERR_MSG("Incompatible rows:"
                    << morph.rows() << " != "
                    << static_cast<int>(m_freerk) + m_repMat.rows());
            return std::nullopt;
        }

        // Sum of the images of the homomorphism and the representation matrix.
        matrix_t sumspace(morph.rows(), morph.cols() + m_repMat.cols());
        sumspace << morph, get_repmatrix();

        // Compute the column HNF of the sumspace.
        matrix_t u = matrix_t::Identity(sumspace.cols(), sumspace.cols());
        auto rk = hnf_LLL<khover::colops>(sumspace, std::tie(u), {});

        // Error occured.
        if(!rk) {
            ERR_MSG("Failed to compute HNF.");
            return std::nullopt;
        }

        // Return the resulting abelian group together with the homomorphism from it.
        if constexpr (DoesReturnMorphism::value) {
            return std::make_pair(
                AbelianGroup(
                    u.topRightCorner(*rk, m_repMat.cols()),
                    std::false_type{}),
                sumspace.leftCols(*rk)
                );
        }
        else {
            return AbelianGroup(
                u.topRightCorner(*rk, m_repMat.cols()),
                std::false_type{});
        }
    }
};

} // end namespace khover


/*
replay
Software Library

Copyright (c) 2010-2019 Marius Elvert

 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.

*/

#ifndef replay_math_hpp
#define replay_math_hpp

#include <boost/math/special_functions/sign.hpp>
#include <replay/interval.hpp>

namespace replay
{

/** generic linear interpolation.
    \ingroup Math
*/
template <class type, class delta_type> inline constexpr type lerp(type a, type b, delta_type x)
{
    return a + x * (b - a);
}

/** Math related functions.
*/
namespace math
{

/** default numerical error tolerance.
    \ingroup Math
*/
float const default_epsilon = 0.000001f;

/** multiply a only by the sign of b.
    \ingroup Math
*/
inline void mult_ref_by_sign(float& a, float b)
{
    a = boost::math::copysign(a, a * b);
}

/** multiply a only by the sign of b.
    \ingroup Math
*/
inline void mult_by_sign(float a, float b, float& result)
{
    result = boost::math::copysign(a, a * b);
}

/** copies the sign.
    \ingroup Math
*/
inline float copy_sign(float value, float sign)
{
    return boost::math::copysign(value, sign);
}

/** return true if the value is within a threshold of zero.
    \ingroup Math
*/
inline bool fuzzy_zero(float value, float epsilon)
{
    return std::abs(value) < epsilon;
}

/** return true if the value is within a threshold of zero.
    \ingroup Math
*/
inline bool fuzzy_zero(float value)
{
    return std::abs(value) < default_epsilon;
}

/** Return true if a is within a treshold of b.
    This is used to compare floating point numbers.
    \ingroup Math
*/
inline bool fuzzy_equals(float a, float b, float epsilon = default_epsilon)
{
    return std::abs(a - b) < epsilon;
}

/** check if the value is in the range. borders count as in.
    \ingroup Math
*/
template <class T> inline bool in_range(T value, interval<T> const& range)
{
    return (range[0] <= value) && (value <= range[1]);
}

/** check if the value is in the range. borders count as in.
    \ingroup Math
*/
template <class T> inline bool in_range(T value, const T left, const T right)
{
    return (left <= value) && (value <= right);
}

/** check whether two intervals intersect.
    \ingroup Math
*/
template <class T> inline bool intervals_intersect(interval<T> const& a, interval<T> const& b)
{
    return a[1] > b[0] && a[0] < b[1];
}

/** clamp a value into the range [-abs,abs]
    \ingroup Math
*/
template <class T> inline constexpr T clamp_absolute(const T value, const T abs)
{
    if (value < -abs)
        return -abs;
    else if (value > abs)
        return abs;
    else
        return value;
}

/** Clamp a value into a range.
    \ingroup Math
*/
template <class T> inline constexpr T clamp(T value, interval<T> const& range)
{
    if (value < range[0])
        return range[0];
    else if (value > range[1])
        return range[1];
    else
        return value;
}

/** Saturate the value, i.e., clamp it into the [0..1] range.
    \param x Value to be saturated.
    \ingroup Math
*/
inline constexpr float saturate(float x)
{
    if (x < 0.f)
        return 0.f;
    else
        return std::min(x, 1.f);
}

/** Perform a smooth hermite blend between two edge values.
    Returns 0 for values smaller than edge0 and 1 for values greater than edge1.
    Values in between are interpolated by the polynomial x*x*(3-2*x).
    \ingroup Math
*/
inline float smoothstep(float edge0, float edge1, float x)
{
    // Early out to avoid divisions by zero if edge0==edge1.
    if (x <= edge0)
        return 0.f;
    else if (x >= edge1)
        return 1.f;

    // Do actual interpolation in-between edges.
    x = (x - edge0) / (edge1 - edge0);
    return x * x * (3.0f - 2.f * x);
}

/** find the sign.
    \ingroup Math
*/
inline unsigned int sign(float value) // returns 1 for - and 0 for +
{
    return value < 0.f ? 1 : 0;
}

/** compare signs.
    \ingroup Math
*/
inline unsigned int same_sign(float a, float b)
{
    return a * b < 0.f ? 0 : 1;
}

/** convert radians to degrees.
    \ingroup Math
*/
inline constexpr float convert_to_degrees(float radians)
{
    constexpr float factor = 180.f / 3.14159265358979323846f;
    return radians * factor;
}

/** convert degrees to radians.
    \ingroup Math
*/
inline constexpr float convert_to_radians(float degrees)
{
    constexpr float factor = 3.14159265358979323846f / 180.f;
    return degrees * factor;
}

/** returns true if the given integer is a power of two.
    \ingroup Math
*/
inline bool is_pow2(unsigned int Number)
{
    return (Number & (Number - 1)) == 0;
}

/** returns true if the given integer is a power of two.
    \ingroup Math
*/
inline bool is_pow2(int Number)
{
    return Number > 0 && is_pow2(static_cast<unsigned int>(Number));
}

/** compute the square.
    \ingroup Math
*/
template <class T> inline constexpr T square(T p)
{
    return p * p;
}

/** Solve a quadratic equation of the form: a*x^2+b*x+c=0
    \ingroup Math
*/
unsigned int solve_quadratic_eq(float a, float b, float c, interval<>& result, float epsilon);

/** interpolation functions.
    \ingroup Math
*/
namespace interpolate
{

/** linear
*/
template <class type, class delta_type> inline type linear(const type a, const type b, const delta_type x)
{
    return a + x * (b - a);
}

/** cubic
*/
template <class type, class delta_type>
inline type cubic(const type& a, const type& b, const type& c, const type& d, const delta_type x)
{
    // 6 mults, 8 adds

    const type P = (d - c) - (a - b);
    const type Q = (a - b) - P;

    return ((P * x + Q) * x + (c - a)) * x + b;
}

/** bicubic
*/
template <class type, class delta_type>
inline type bicubic(const type& v11,
                    const type& v21,
                    const type& v31,
                    const type& v41,
                    const type& v12,
                    const type& v22,
                    const type& v32,
                    const type& v42,
                    const type& v13,
                    const type& v23,
                    const type& v33,
                    const type& v43,
                    const type& v14,
                    const type& v24,
                    const type& v34,
                    const type& v44,
                    const delta_type x,
                    const delta_type y)
{
    return cubic(cubic(v14, v13, v12, v11, y), cubic(v24, v23, v22, v21, y), cubic(v34, v33, v32, v31, y),
                 cubic(v44, v43, v42, v41, y), x);
}
}
}
}

#endif


#include <string>
#include <fstream>
#include <exception>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <ensenso/pathfinder.h>
#include <std_msgs/Float64MultiArray.h>
#include "nn_controller/nn_controller.h"

#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>

using namespace amfc_control;
using namespace boost::numeric::odeint;

//constructor
Controller::Controller(ros::NodeHandle nc, const Eigen::Vector3d& ref, bool print,
						bool useSigma, bool save)
: n_(nc), ref_(ref), updatePoseInfo(false), updateController(false), useSigma(useSigma),
  save(save), resetController(false), updateWeights(false), print(print), counter(0),
  multicast_address("235.255.0.1")
{
	initMatrices();
	sigma_y = 0.01;
	sigma_r = 0.01;
	// BladderTypeEnum bladder_type_;
	ros::param::get("/nn_controller/Control/with_net", with_net_);
	control_pub_ = n_.advertise<ensenso::ValveControl>("/mannequine_head/u_valves", 100);
}

//copy constructor
Controller::Controller()
{

}

// Destructor.
Controller::~Controller()
{
}

void Controller::vectorToHeadPose(Eigen::VectorXd&& pose_info, geometry_msgs::Pose& eig2Pose)
{
    eig2Pose.orientation.x = pose_info(0); // roll
    eig2Pose.position.z = pose_info(1);	// z
    eig2Pose.orientation.y = pose_info(2);	// pitch
}

ros::Time Controller::getTime() {
	return ros::Time::now();
}

/*Subscribers*/
// pose subscriber from ensenso_seg/vicon_sub
void Controller::pose_subscriber(const geometry_msgs::Pose& headPose) {
	getPoseInfo(headPose, pose_info);

	std::lock_guard<std::mutex> pose_locker(pose_mutex);
	this->pose_info = pose_info;
	updatePoseInfo  = true;
}

void Controller::net_control_subscriber(const ensenso::ValveControl& net_control_law){
	Eigen::VectorXd net_control;
	net_control.resize(6);

	net_control << net_control_law.left_bladder_pos, net_control_law.left_bladder_neg,
				   net_control_law.base_bladder_pos, net_control_law.base_bladder_neg,
				   net_control_law.right_bladder_pos, net_control_law.right_bladder_neg;
	update_net_law = true;
	this->net_control.resize(6);
	this->net_control = net_control;
}

void Controller::getPoseInfo(const geometry_msgs::Pose& headPose, Eigen::VectorXd pose_info)
{
	pose_info << headPose.orientation.x, // roll = [left and right]
				 headPose.position.z, 	// base  = [base actuator]
				 headPose.orientation.y; // pitch = [right actuator]

	this->pose_info = pose_info;
	//set ref's non-controlled states to measurement
	ControllerParams(std::move(pose_info));
}

void Controller::ControllerParams(Eigen::VectorXd&& pose_info)
{
	// Am = -0.782405        -0        -0
	//       -0          -0.782405     -0
    //       -0              -0    -0.782405
	// Bm = [1 0 0; 0 1 0; 0 0 1]
	// ref_ = [z, roll, pitch ] given by user
	if(counter == 0){
		ym = pose_info;		// will be 3x1; pose_info is also 3x1
		ym_dot = Am * ym + Bm * ref_;	// will be 3x1
		prev_ym.push_back(ym);

		tracking_error = pose_info - ym; 	// will be 3x1

		ros::param::get("/nn_controller/Utils/filename", filename_);
		pathfinder::getROSPackagePath("nn_controller", nn_controller_path_);
		ss << nn_controller_path_.c_str() << filename_;
		ref_pose_file_ = ss.str();
	}
	else{
		// find ym
		ym_dot = Am * prev_ym.back() + Bm * ref_;  // will be 3x1
		ym = prev_ym.back() + 0.01 * ym_dot;		// will be 3x1
		prev_ym.push_back(ym);    // don't leve the linked list empty

		tracking_error = pose_info - ym;			// will be 3x1

		// // find Ky_hat
		// Ky_hat_dot = -Gamma_y * pose_info * tracking_error.transpose() * P * B  * sgnLambda;
		// Ky_hat = prev_Ky_hat_.back() + 0.01 * Ky_hat_dot;
		// prev_Ky_hat_.push_back(Ky_hat);

		// // find Kr_hat
		// Kr_hat_dot = -Gamma_r * ref_      * tracking_error.transpose() * P * B  * sgnLambda;
		// Kr_hat = prev_Kr_hat_.back() + 0.01 * Kr_hat_dot;
		// prev_Kr_hat_.push_back(Kr_hat);
	}
	// tracking_error = pose_info - ym;

	//use boost ode solver to compute Ky_hat and Kr_hat:: lot more stable than trapezoidal rule
	runge_kutta_dopri5<state,double,state,double,vector_space_algebra> stepper;
	Ky_hat_dot = -Gamma_y * pose_info * tracking_error.transpose() * P * B  * sgnLambda;
	Kr_hat_dot = -Gamma_r * ref_      * tracking_error.transpose() * P * B  * sgnLambda;

	//use reference for the derivative
	stepper.do_step([](const state& x, state & dxdt, const double t)->void{
		dxdt = x;
	}, Ky_hat_dot, counter, Ky_hat, 0.01);
	//integrate Ky_hat_dot
	stepper.do_step([](const state& x, state & dxdt, const double t)->void{
		dxdt = x;
	}, Kr_hat_dot, counter, Kr_hat, 0.01);

	Eigen::VectorXd net_control;
	net_control.resize(m);
	if(update_net_law)	{
		std::lock_guard<std::mutex> net_pred_locker(pred_mutex);
		update_net_law = false;
		net_control = this->net_control;
	}
	/*
	* Calculate Control Law
	*/
	if(with_net_){
		u_control = (Ky_hat.transpose() * pose_info) +
					(Kr_hat.transpose() * ref_) + this->net_control;
	}
	else{
		u_control = (Ky_hat.transpose() * pose_info) +
					(Kr_hat.transpose() * ref_);
	}
	// u_control = this->net_control;
	/*
	Here are the rules that govern the bladders
	l_i --> Roll+	r_i -->Roll-
	l_o --> Roll-   r_o -->Roll+
	b_i --> Pitch+, Z+   b_o -->Pitch-, Z-
	u_{o+} is a suitable controller magnitude; u_+ is a +ve input
	------------------------------------------------------------
	DOF     |  Control Law
	------------------------------------------------------------
	Roll+   |  if f_{li} = u_{+}:
			|	f_{ri} = 0,
			|   f_{lo} = |u_{lo}|
			| 	f_{ro} = |u_{ro}|
	------------------------------------------------------------
	Roll-   | if f_{ri} = u_{+}:
			|  	f_{li} = 0;
			|   f_{ro} = 0  or u_{o+}
	------------------------------------------------------------
	Pitch+  | if f_{bi} = u_{+}
			|    f_{bo} = u_{o+} and f_{li} =f_{ri} = u_{head+}
	------------------------------------------------------------
	Pitch-  | f_{bo} = u_{max-}
			| f_{bi} = 0 or < f_{bo}
	------------------------------------------------------------
	Z+      | f_{li} = f_{ri} = u_{head+}
			| f_{bi} = u_{+} f_{bo} = u_{o+}
	------------------------------------------------------------
	Z-      | f_{bo} = u_{-}
			| f_{bi} = 0 or f_{bo}
	*/
	// // saturate control signals
	// for(auto i = 0; i < 6; ++i){
	// 	if(u_control[i] < 0)
	// 		u_control[i] = 0;
	// 	else if (u_control[i] > 1)
	// 		u_control[i] = 1;
	// 	else
	// 		u_control[i] = u_control[i];
	// }
	u_valves_.left_bladder_pos  = u_control(0);
	u_valves_.left_bladder_neg  = u_control(1);
	u_valves_.base_bladder_pos  = u_control(2);
	u_valves_.base_bladder_neg  = u_control(3);
	u_valves_.right_bladder_pos = u_control(4);
	u_valves_.right_bladder_neg = u_control(5);

	if(save) {
		std::ofstream file_handle;
		file_handle.open(ref_pose_file_, std::ofstream::out | std::ofstream::app);
		file_handle  << ref_(0) <<"\t" <<ref_(1) << "\t" << ref_(2) << "\t" << pose_info(0) <<"\t" <<pose_info(1) << "\t" << pose_info(2) << "\n";
		file_handle.close();
	}
	ros::Rate sleeper(2);
	sleeper.sleep();
	control_pub_.publish(u_valves_);
	vectorToHeadPose(std::move(pose_info), pose_);	// convert from eigen to headpose
	udp::sender s(io_service, boost::asio::ip::address::from_string(multicast_address), u_valves_, ref_, pose_);
	// pose is  [roll, z, pitch]
	// udp::sender s(io_service, boost::asio::ip::address::from_string(multicast_address), pose_); // used for identification

	if(print)	{
		OUT("\nref_: " 			<< ref_.transpose());
		OUT("y  (roll, z,  pitch): " 		 << pose_info.transpose());
		OUT("ym (roll, z,  pitch): " 		 << ym.transpose());
		OUT("e  (y-ym): " << tracking_error.transpose());
		OUT("pred (z, z, pitch, pitch, roll, roll): " << pred.transpose());
		OUT("net_control: " << net_control.transpose());
		OUT("Control Law: " << u_control.transpose());
		OUT("Kr_hat: \n" << Kr_hat);
		OUT("Ky_hat: \n" << Ky_hat);
	}
	++counter;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "controller_node", ros::init_options::AnonymousName);
	ros::NodeHandle n;
	bool print, useSigma, save, useVicon(true);

	Eigen::Vector3d ref;
	ref.resize(3);

	try{
		//supply values from the cmd line or retrieve them
		//from the ros parameter server
		n.getParam("/nn_controller/Reference/z", ref(1));    	//ref z
		n.getParam("/nn_controller/Reference/pitch", ref(2));	//ref pitch
		n.getParam("/nn_controller/Reference/roll", ref(0));	    //ref roll
		n.getParam("/nn_controller/Utils/print", print);
		n.getParam("/nn_controller/Utils/useSigma", useSigma);
		save = n.getParam("/nn_controller/Utils/save", save);
	}
	catch(std::exception& e){
		e.what();
	}

	Controller c(n, ref, print, useSigma, save);

	ros::Subscriber sub_pose = n.subscribe("/mannequine_head/pose", 100, &Controller::pose_subscriber, &c);
	ros::Subscriber sub_pred = n.subscribe("/mannequine_pred/preds", 100, &Controller::net_control_subscriber, &c);
	ros::spin();

	if(!ros::ok())
		ros::shutdown();

	return 0;
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__CARE_HPP_
#define CBR_CONTROL__CARE_HPP_

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <type_traits>
#include <numeric>

namespace cbr
{

/* -------------------------------------------------------------------------- */
/*                               Matrix Balance                               */
/* -------------------------------------------------------------------------- */

// Numerical Recipes p.593 - Balancing Trasformation
// D^-1 * A * D = B

template<std::size_t nx, class T = double>
void matrix_balance(
  const Eigen::Ref<const Eigen::Matrix<T, 2 * nx, 2 * nx>> A,
  Eigen::Ref<Eigen::Matrix<T, 2 * nx, 2 * nx>> D,
  Eigen::Ref<Eigen::Matrix<T, 2 * nx, 2 * nx>> B
)
{
  std::size_t n = 2 * nx;

  // Initialize D and B
  D.setIdentity();
  B = A;

  double RADIX = 2.0;
  double sqrdx = RADIX * RADIX;

  std::size_t done = 0;

  while (done != 1) {
    done = 1;

    for (std::size_t i = 0; i < n; i++) {
      double r = 0.;
      double c = 0.;

      for (std::size_t j = 0; j < n; j++) {
        if (j != i) {
          c = c + std::abs(B(j, i));
          r = r + std::abs(B(i, j));
        }
      }

      if ((c != 0) && (r != 0)) {
        double g = r / RADIX;
        double f = 1.0;
        double s = c + r;

        while (c < g) {
          f = f * RADIX;
          c = c * sqrdx;
        }

        g = r * RADIX;

        while (c > g) {
          f = f / RADIX;
          c = c / sqrdx;
        }

        if (((c + r) / f) < 0.95 * s) {
          done = 0;
          g = 1 / f;
          D(i, i) = D(i, i) * f;

          for (std::size_t j = 0; j < n; j++) {
            B(i, j) = B(i, j) / f;
          }

          for (std::size_t j = 0; j < n; j++) {
            B(j, i) = B(j, i) * f;
          }
        }
      }
    }
  }
}

// Finds the solution to the ARE by finding the Eigen-decomposition of the Hamiltonean.
template<std::size_t nx, std::size_t nu, class T = double>
bool care(
  const Eigen::Ref<const Eigen::Matrix<T, nx, nx>> A,
  const Eigen::Ref<const Eigen::Matrix<T, nx, nu>> B,
  const Eigen::Ref<const Eigen::Matrix<T, nx, nx>> Q,
  const Eigen::Ref<const Eigen::Matrix<T, nu, nu>> R,
  Eigen::Ref<Eigen::Matrix<T, nx, nx>> P,
  Eigen::Ref<Eigen::Matrix<T, nu, nx>> K
)
{
// Ensure R positive definite (R>0)
  const Eigen::LLT<Eigen::Matrix<T, nu, nu>> Rdecomposed(R.transpose());
  if (Rdecomposed.info() == Eigen::NumericalIssue) {
    return false;
  }

  using H_t = Eigen::Matrix<T, 2 * nx, 2 * nx>;
  using Hc_t = Eigen::Matrix<std::complex<T>, 2 * nx, 2 * nx>;
  using Ac_t = Eigen::Matrix<std::complex<T>, nx, nx>;

//  1. Define Hamiltonean:
//
//                      [  A | -(B/R)*B' ]
//                H =   [ ---|-----------]
//                      [ -Q |    -A'    ]

  H_t H;
  H.template topLeftCorner<nx, nx>() = A;
  H.template topRightCorner<nx,
    nx>() = -Rdecomposed.solve(B.transpose()).transpose() * B.transpose();
  H.template bottomLeftCorner<nx, nx>() = -Q;
  H.template bottomRightCorner<nx, nx>() = -A.transpose();

  //  2. Balance the Hamiltonean
  H_t D;
  H_t Hb;
  matrix_balance<nx, T>(H, D, Hb);

  //  3. Solve the ARE through eigen decomposition of Hb
  // Start by obtaining eigenvalues and eigenvectors
  const Eigen::EigenSolver<H_t> es(Hb);
  auto V = es.eigenvectors();
  const auto & l = es.eigenvalues();
  V = D * V;

  // Idendify which eigenvalues are positive and which are negative
  std::array<int, 2 * nx> ord_L;
  std::fill(ord_L.begin(), ord_L.end(), 0);

  for (std::size_t k = 0; k < (2 * nx); k++) {
    if (std::real(l[k]) < 0) {
      ord_L[k] = -1;
    } else if (std::real(l[k]) > 0) {
      ord_L[k] = 1;
    }
  }

  // Sorting indices - place positive on left side, negative on right
  std::array<std::size_t, 2 * nx> ord_index;
  std::iota(ord_index.begin(), ord_index.end(), 0LU);
  std::sort(
    ord_index.begin(), ord_index.end(),
    [&](const std::size_t i1, const std::size_t i2) {
      const auto & l1 = ord_L[i1];
      const auto & l2 = ord_L[i2];
      return l1 > l2;
    });

  // Sort Eigenvector based on ord_index array
  Hc_t V_ord;
  for (std::size_t i = 0; i < 2 * nx; i++) {
    V_ord.col(i) = V.col(ord_index[i]);
  }

  // Define upper and lower right side block matrices
  const Ac_t V12 = V_ord.template topRightCorner<nx, nx>();
  const Ac_t V22 = V_ord.template bottomRightCorner<nx, nx>();

  const Eigen::FullPivLU<Ac_t> V12decomposed(V12.transpose());
  const Ac_t P_complex = V12decomposed.solve(V22.transpose()).transpose();  // P_complex = T22/T12

  // 4. Write Results
  P = P_complex.unaryExpr([](const std::complex<T> & v) {return std::real(v);});

  K = R.inverse() * B.transpose() * P;

  return true;
}

}    // namespace cbr

#endif  // CBR_CONTROL__CARE_HPP_


#ifndef _POSE_ESTIMATION_UKF_HPP
#define _POSE_ESTIMATION_UKF_HPP

#include <iostream>
#include <stdexcept>
#include <ukfom/ukf.hpp>
#include <ukfom/mtkwrap.hpp>
#include <boost/shared_ptr.hpp>
#include <base/Time.hpp>
#include <boost/noncopyable.hpp>

namespace pose_estimation
{

template<typename Manifold>
class UnscentedKalmanFilter : private boost::noncopyable
{
public:
    enum {
        DOF = Manifold::DOF
    };
    typedef Manifold State;
    typedef ukfom::mtkwrap<Manifold> WState;
    typedef ukfom::ukf<WState> MTK_UKF;
    typedef typename MTK_UKF::cov Covariance;

    UnscentedKalmanFilter()
    {
        process_noise_cov = Covariance::Zero();
        last_measurement_time.microseconds = 0;
        min_time_delta = 1.0e-9;
        max_time_delta = std::numeric_limits<double>::max();
    }

    virtual ~UnscentedKalmanFilter() {}

    /**
     * (Re-)initializes the UKF filter from a given state.
     */
    void initializeFilter(const State& initial_state, const Covariance& state_cov)
    {
        ukf.reset(new MTK_UKF(initial_state, state_cov));
        last_measurement_time.microseconds = 0;
    }

    /**
     * Provides the current state and covariance of the filter.
     *
     * @returns false if the filter has not been initialized.
     */
    bool getCurrentState(State& state, Covariance& state_cov) const
    {
        if(ukf.get() != NULL)
        {
            state = ukf->mu();
            state_cov = ukf->sigma();
            return true;
        }
        return false;
    }

    /**
     * Provides the current state of the filter.
     *
     * @returns false if the filter has not been initialized.
     */
    bool getCurrentState(State& state) const
    {
        if(ukf.get() != NULL)
        {
            state = ukf->mu();
            return true;
        }
        return false;
    }

    /**
     * Computes the time delta from a given sample timestamp and
     * calls predictionStep(delta_t)
     *
     * @throws runtime_error if delta_t is negative or greater then the allowed maximum.
     */
    void predictionStepFromSampleTime(const base::Time& sample_time)
    {
        // first call
        if(last_measurement_time.isNull())
        {
            last_measurement_time = sample_time;
            return;
        }

        // compute delta t
        double delta_t = (sample_time - last_measurement_time).toSeconds();

        // set new last measurement time
        if(delta_t > min_time_delta)
            last_measurement_time = sample_time;

        predictionStep(delta_t);
    }

    /**
     * Calls the predictionStepImpl after checking the delta_t value.
     *
     * @throws runtime_error if delta_t is negative or greater then the allowed maximum.
     */
    void predictionStep(double delta_t)
    {
        // check delta time
        if(delta_t < 0.0)
        {
            throw std::runtime_error("Delta time is negative!");
        }
        else if(delta_t <= min_time_delta)
        {
            // delta time is zero or close to zero
            return;
        }
        else if(delta_t > max_time_delta)
        {
            throw std::runtime_error("Delta time is greater then the allowed maximum!");
        }

        predictionStepImpl(delta_t);
    }

    unsigned getStateSize() const {return unsigned(WState::DOF);}
    bool isInitialized() const {return ukf.get() != NULL;}
    const Covariance& getProcessNoiseCovariance() const {return process_noise_cov;}
    void setProcessNoiseCovariance(const Covariance& noise_cov) {process_noise_cov = noise_cov;}
    const base::Time& getLastMeasurementTime() const {return last_measurement_time;}
    void setLastMeasurementTime(const base::Time& last_measurement_time)
                               {this->last_measurement_time = last_measurement_time;}
    double getMaxTimeDelta() const {return max_time_delta;}
    void setMaxTimeDelta(double max_time_delta) {this->max_time_delta = max_time_delta;}
    double getMinTimeDelta() const {return min_time_delta;}
    void setMinTimeDelta(double min_time_delta) {this->min_time_delta = min_time_delta;}

protected:
    virtual void predictionStepImpl(double delta_t) = 0;

    template<int DIM, typename scalar_type>
    void checkMeasurment(const Eigen::Matrix<scalar_type, DIM, 1>& mu, const Eigen::Matrix<scalar_type, DIM, DIM>& cov) const
    {
        if(!mu.allFinite() || !cov.allFinite())
            throw std::runtime_error("Measurement or covariance contains non-finite values!");
    }

protected:
    boost::shared_ptr<MTK_UKF> ukf;
    Covariance process_noise_cov;
    base::Time last_measurement_time;
    double max_time_delta;
    double min_time_delta;
};

}

#endif


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl; using namespace std;
    typedef  mtl::dense2D<double>            Matrix;

    double array[][4]= {{2, 3,   4,   5},
                        {4, 10, 13,  16},
                        {6, 25, 38,  46},
		        {8, 32, 77, 100}};
    Matrix 		A(array), I(4, 4);
    I= 1.0;

    Matrix LU(A);
    dense_vector<std::size_t> v(4);
    lu(LU, v);
    mat::traits::permutation<>::type P(permutation(v));

    cout << "A is:\n" << A << "\nPermuted A is \n" << Matrix(P * A);

    Matrix L(I + strict_lower(LU)), U(upper(LU)), A2(L * U);
    cout << "L [permuted] is:\n" << L << "U [permuted] is:\n" << U
	 << "L * U [permuted] is:\n" << A2
	 << "L * U is:\n" << Matrix(trans(P) * A2);

    Matrix UI(inv_upper(U));
    cout << "inv(U) [permuted] is:\n" << UI << "UI * U is:\n" << UI * U;

    Matrix LI(inv_lower(L));
    cout << "inv(L) [permuted] is:\n" << LI << "LI * L is:\n" << LI * L;

    Matrix AI(UI * LI * P);
    cout << "inv(A) [inv(U) * inv(L) * P] is \n" << AI << "Test: A * AI is\n" << AI * A;

    mat::traits::inv<Matrix>::type A_inv(inv(A));
    cout << "inv(A) is \n" << A_inv << "Test: A * AI is\n" << A_inv * A;

    return 0;
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic cotangent argument \f$\frac12\log\frac{x+1}{x-1}\f$

    @see cosh, sinh, acosh, asinh, atanh, asech, acosh, acsch

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

    @par Example:

      @snippet acoth.cpp acoth

    @par Possible output:

      @snippet acoth.txt acoth

  **/
  IEEEValue acoth(IEEEValue const & x);
} }
#endif

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

#endif