#pragma once

//! All the number theory goes in here

#include <boost/multiprecision/gmp.hpp>

namespace rubbishrsa {
  // Saves me a lot of typing
  namespace bmp = boost::multiprecision;
  using bigint = bmp::mpz_int;

  // Extended Euclid's algorithm is the name of this algorithm (I think)
  struct egcd_result { bigint gcd; std::pair<bigint, bigint> coefficients; };
  egcd_result egcd(const bigint& a, const bigint& b);

  // This is actually implemented in the numeric library I have used, but that would be cheating
  /// Performs the Miller-Rabin primality check `certainty_log_2` times
  bool is_prime(const bigint& candidate, uint_fast8_t certainty_log_4 = 64);

  /// Generates a prime that is at least 2^(bits - 1) long.
  //
  // Apparently "strong primes" are better, but computing these is much harder, and RSA say they are unnecceary
  //
  // Because RSA (company) can be trusted. Yes.
  bigint generate_prime(uint_fast16_t bits);

  /// Calculate the lowest common multiple of two numbers
  bigint lcm(const bigint& a, const bigint& b);

  // Calculating Carmichael's function for an arbitrary number is complex and pointless
  //
  // Instead, we can just calculate it for our special case
  inline bigint carmichael_semiprime(const bigint& p, const bigint& q) {
    return lcm(p - 1, q - 1);
  }

  // Again, exists in our library, but I don't want to cheat
  /// Computes a^(-1) mod n
  bigint modinv(const bigint& a, const bigint& n);

  /// An implementation of Pollard's rho algorithm
  ///
  /// @param a: the x^0 term of the polynomial
  bigint pollard_rho(const bigint& n);

  /// Selects the fastest implemented factorisation algorithm for the given semiprime, and returns the factors
  std::pair<bigint, bigint> factorise_semiprime(const bigint& semiprime);

  // Some functions that convert between ascii and bigint
  bigint ascii2bigint(std::string_view str);
  bigint ascii2bigint(std::istream& str);
  std::string bigint2ascii(bigint str);
  bigint hex2bigint(std::string_view hex);

  inline size_t floor_log2(bigint i) {
    size_t bits = 0;
    while (i) {
      i >>= 1;
      ++bits;
    }
    return bits;
  }
}


/**
 * @file engquistoshernumericalflux.cc
 * @brief NPDE homework "EngquistOsherNumericalFlux" code
 * @author Oliver Rietmann
 * @date 23.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "engquistoshernumericalflux.h"

#include <Eigen/Core>
#include <algorithm>
#include <cmath>

namespace EngquistOsherNumericalFlux {

/* SAM_LISTING_BEGIN_1 */
double EngquistOsherNumFlux(double v, double w) {
  double result;
  //====================
  // Your code goes here
  if(v>=0 && w>=0){
    result=std::cosh(v);
  } else if (w>0 && v<=0){
    result=std::cosh(v)+std::cosh(0);
  } else if (w<0 && v>=0){
    result=std::cosh(v)+std::cosh(w)-std::cosh(0);
  } else if (w<0 && v<0){
    result=std::cosh(w);
  }
  //====================
  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd solveCP(double a, double b, Eigen::VectorXd u0, double T) {
  // Find the maximal speed of propagation
  double A = u0.minCoeff();
  double B = u0.maxCoeff();
  double K = std::max(std::abs(std::sinh(A)), std::abs(std::sinh(B)));
  // Set uniform timestep according to CFL condition
  int N = u0.size();
  double h = (b - a) / N;
  double tau_max = h / K;
  double timesteps = std::ceil(T / tau_max);
  double tau = T / timesteps;

  // Main timestepping loop
  //====================
  // Your code goes here
  for (int i = 0; i < timesteps; i++)
  {
    u0(0)=u0(0)-tau/h*(EngquistOsherNumericalFlux(u0(0),u0(1))-EngquistOsherNumericalFlux(u0(0),u0(0)));
    for (int j=1; i<N-1; j++){
      u0(j)=u0(j) - tau/h*(EngquistOsherNumFlux(u0(j),u0(j+1))-EngquistOsherNumFlux(u0(j-1),u0(j)));
    }
    u0(N-1)=u0(N-1)-tau/h*(EngquistOsherNumericalFlux(u0(N-1),u0(N-1))-EngquistOsherNumericalFlux(u0(N-2),u0(N-1)));    /* code */
  }

  //===================//
  return u0;
}
/* SAM_LISTING_END_2 */

}  // namespace EngquistOsherNumericalFlux


#include "bayesclassifier.h"

#include <algorithm> // for std::max_element
#include <cassert>

#include <Eigen/LU>

#include "algorithm.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;
using std::vector;

double multivariate_normal_pd(const VectorXd& x,
                              const VectorXd& mu,
                              const MatrixXd& sigma) {
    // XXX would be more efficient to pre-compute sigma.inverse() and
    // sigma.determinant()
    double d = x.size();
    VectorXd v = x - mu;
    double a = pow(2.0 * M_PI, d/2.0);
    double b = sqrt(sigma.determinant());
    double c = v.transpose() * sigma.inverse() * v;
    return (1.0 / (a*b)) * exp( - c / 2.0);
}

BayesClassifier::BayesClassifier(const vector<VectorXd>& x,
                                 const vector<int>& y) :
k(0), p(), mu(), sigma() {
    // n is the number of points
    unsigned n = x.size();
    assert(n > 0);
    assert(y.size() == n);

    // d is the dimensionality
    int d = x[0].size();
    for (const VectorXd& v : x)
        assert(v.size() == d);

    // number of classes
    k = *(std::max_element(y.cbegin(), y.cend())) + 1;

    for (int i = 0; i < k; ++i) {
        // find all points in class i
        vector<VectorXd> xi;
        for (unsigned j = 0; j < n; ++j)
            if (y[j] == i)
                xi.push_back(x[j]);

        // ni is the number of points in class i
        unsigned ni = xi.size();
        assert(ni > 0);

        // prior probability
        p.push_back((double)ni / (double)n);

        // class mean
        VectorXd m = VectorXd::Zero(d);
        for (const VectorXd& v : xi)
            m += v;
        m /= ni;
        mu.push_back(m);

        // centered data matrix
        MatrixXd z(d, ni);
        for (unsigned j = 0; j < ni; ++j)
            z.col(j) = xi[j] - m;

        // covariance matrix
        sigma.push_back((1.0/(double)ni) * z * z.transpose());
    }
}

int BayesClassifier::predict(const VectorXd& x) const {
    return arg_max(0, k, [&x,this](int i) {
        return multivariate_normal_pd(x, mu[i], sigma[i]) * p[i];
    });
}

int BayesClassifier::nClasses() const {
    return k;
}
double BayesClassifier::priorProbability(int i) const {
    return p[i];
}
VectorXd BayesClassifier::mean(int i) const {
    return mu[i];
}
MatrixXd BayesClassifier::covarianceMatrix(int i) const {
    return sigma[i];
}


#include "Gaussian.h"
#include <iostream>
#include <stdexcept>
#include <cmath>
#include "../Utils.h"
//#include <boost/math/special_functions/erf.hpp>

namespace DNest4
{

Gaussian::Gaussian(double center, double width)
:center(center)
,width(width)
{
    if(width <= 0.0)
        throw std::domain_error("Gaussian distribution must have positive width.");
}

double Gaussian::cdf(double x) const
{
    return normal_cdf((x-center)/width);
}

double Gaussian::cdf_inverse(double x) const
{
    if(x < 0.0 || x > 1.0)
        throw std::domain_error("Input to cdf_inverse must be in [0, 1].");
    return center + width*normal_inverse_cdf(x);
    //return center + width * sqrt(2) * boost::math::erf_inv(2*x - 1);
}

double Gaussian::log_pdf(double x) const
{
	double r = (x - center)/width;
    return -0.5*r*r - _norm_pdf_logC;
}

} // namespace DNest4


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

#ifndef CBR_MATH__INTERP__PIECEWISE_LINEAR_HPP_
#define CBR_MATH__INTERP__PIECEWISE_LINEAR_HPP_

#include <Eigen/Dense>

#include <utility>
#include <vector>
#include <array>

#include "piecewise_poly.hpp"

namespace cbr
{

class PiecewiseLinear
{
protected:
  using row_t = Eigen::Matrix<double, 1, Eigen::Dynamic>;
  using matrix_t = Eigen::MatrixXd;

public:
  /**
   * @brief Create a scalar-valued piecewise linear polynomial
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::row_t
   * @param x sorted breakpoints
   * @param y values
   *
   * The resulting function f is s.t.
   *  f(t) = y[i] + alpha * (y[i+1] - y[i]) if x[i] <= t < x[i+1]
   *     where alpha = (t - x[i]) / (x[i+1] - x[i])
   *  f(t) = y[0] if t < x[0]
   *  f(t) = y[x.size() - 1] if t >= x[x.size() - 1]
   */
  template<typename T1, typename T2>
  static PiecewisePoly fit(
    T1 && x,
    const Eigen::DenseBase<T2> & y)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    auto coeffs = generateCoeffs(x, y);
    return PiecewisePoly(std::forward<T1>(x), std::move(coeffs));
  }

  /**
   * @brief Create a vector-valued piecewise linear polynomial
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::matrix_t or container_t<PiecewisePoly::row_t>
   * @param x sorted breakpoints
   * @param y values
   *
   * The resulting function f is s.t.
   *  f(t) = y[i] + alpha * (y[i+1] - y[i]) if x[i] <= t < x[i+1]
   *     where alpha = (t - x[i]) / (x[i+1] - x[i])
   *  f(t) = y[0] if t < x[0]
   *  f(t) = y[x.size() - 1] if t >= x[x.size() - 1]
   */
  template<typename T1, typename T2>
  static PiecewisePolyND fitND(
    T1 && x,
    const T2 & ys)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    if (x.size() < 2) {
      throw std::invalid_argument("x must be of size > 1.");
    }

    std::vector<matrix_t> coefLists;

    if constexpr (is_eigen_dense_v<T2>) {
      if (ys.rows() < 1) {
        throw std::invalid_argument("Dimension of the data must be > 0.");
      }

      if (ys.cols() != x.size()) {
        throw std::invalid_argument("The number of columns of ys must be equal to the size of x.");
      }
      coefLists.reserve(static_cast<std::size_t>(ys.rows()));

      for (Eigen::Index i = 0; i < ys.rows(); i++) {
        coefLists.push_back(generateCoeffs(x, ys.row(i)));
      }
    } else {
      static_assert(
        is_eigen_dense_v<typename T2::value_type>&& T2::value_type::IsVectorAtCompileTime,
        "ys must be a container of Eigen::DenseBase vector objects");

      if (ys.size() < 1) {
        throw std::invalid_argument("Dimension of the data must be > 0.");
      }

      coefLists.reserve(ys.size());

      for (const auto & y : ys) {
        coefLists.push_back(generateCoeffs(x, y));
      }
    }

    return PiecewisePolyND(std::forward<T1>(x), std::move(coefLists));
  }

protected:
  template<typename T1, typename T2>
  static matrix_t generateCoeffs(
    const Eigen::DenseBase<T1> & x,
    const Eigen::DenseBase<T2> & y)
  {
    static_assert(
      T1::IsVectorAtCompileTime && T2::IsVectorAtCompileTime,
      "x and y must be vectors.");

    if (x.size() != y.size()) {
      throw std::invalid_argument("Each element of ys must have the same size as x.");
    }

    Eigen::Index nj = x.size() - 1;
    matrix_t coeffs(2, nj);

    for (Eigen::Index i = 0; i < nj; i++) {
      coeffs(1, i) = y[i];
      coeffs(0, i) = (y[i + 1] - y[i]) / (x[i + 1] - x[i]);
    }

    return coeffs;
  }
};

}  // namespace cbr

#endif  // CBR_MATH__INTERP__PIECEWISE_LINEAR_HPP_


#pragma once
///@file simpleClusterization_common.hpp
///@brief common definitions that users of the library might need independently from the rest of the files

#include <Eigen/Dense>

using namespace Eigen;

///Shorthand type for a RowMajor Matrix of floats
typedef Matrix<float, Dynamic, Dynamic, RowMajor>   MatrixXfR;
///Shorthand type for a ColMajor Matrix of bools
typedef Matrix<bool, Dynamic, Dynamic>              MatrixXb;
///Shorthand type for a RowMajor Matrix of bools
typedef Matrix<bool, Dynamic, Dynamic, RowMajor>    MatrixXbR;

///The type signature for the norm parameters of the functions in this library
typedef float squaredNorm_t(const VectorXf &v1, const VectorXf &v2);

///A basic example of norm that satisfies the type signature
float euclideanNorm(const VectorXf &v1, const VectorXf &v2);


#include <iostream>

#include <stack>

// #include <boost/lexical_cast.hpp>

#include ".\\headers\\min_path.h"

using namespace std;

/**

 * @brief MinPath::initGraph

 * @note the start vertex must start with '0'

 * @todo fix the vertex info(automatically to certain question)

 */

void MinPath::initGraph() {

    M_vertexNum = 6;

    M_edgeNum = 10;

    int i;

    int j;

    for ( i=0; i<M_vertexNum; i++ ) {

        this->M_vertexs[i] = i;

    }

    for ( i=0; i<M_vertexNum; i++ )

        for ( j=0; j<M_vertexNum; j++ )

            this->M_edges[i][j] = MAXWEIGHT;

    M_edges[0][1] = 3;

    M_edges[0][2] = 2;

    M_edges[0][3] = 5;

    M_edges[2][3] = 1;

    M_edges[3][1] = 2;

    M_edges[1][5] = 7;

    M_edges[4][2] = 5;

    M_edges[3][5] = 5;

    M_edges[4][3] = 3;

    M_edges[4][5] = 1;

    return;

}

/**

 * @brief MinPath::computeShortestPath

 * @param v0, the start vertex

 * @param pre, array to store the current vertex's pirror vertex

 * @param dist, array to store the current vertex's weight to the start vertex

 */

void MinPath::computeShortestPath(int v0, int *pre, int *dist) {

    bool final[MAXVERTEXNUM];

    int i;

    int w;

    int v;

    int current_tmp_min;

    // 初始化

    for ( v=0; v<=M_vertexNum-1; v++ ) {

        final[v] = false;

        dist[v] = M_edges[v0][v];       // 如果不直接连通，dist[v]就是MAXWEIGHT,否则是相应的权值, 我称此时dist[v] 为 "估计值"

        pre[v] = -1;      // 所有的顶点都无前驱，置pre数组为 -1

        if ( dist[v] < MAXWEIGHT )  // v 到 v0 (直接)连通

            pre[v] = v0;

    }

    // 开始时V0属于Ｓ集合，默认已经找到最短路径

    dist[v0] = 0;

    final[v0] = true;       // (final[i] = true 相当于把i 加入S集合)

    // main loop

    // 寻找其余6个节点

    for ( i=1; i<M_vertexNum+1; i++ ) {

        v = -1;     // ---> if (v==-1)

        current_tmp_min = MAXWEIGHT;    // 当前距V0路径最短的权值 ,初始化为不连通状态

        // 寻找当前离V0最近的顶点 V

        // 第一次main loop 下, current_tmp_min 变化情况: dist[1] -> dist[2]

        for ( w=0; w<M_vertexNum; w++ ) {

            if ( final[w] == false && dist[w] < current_tmp_min ) {  // w还没找到最短路径，并且d[w]比当前min 还要小

                v = w;                          // 更新

                current_tmp_min = dist[v];

            }

        }

        if ( v == -1 )                      // 所有与V0相通的点都找到了最短路径(不满足line 108 的if )，则退出main loop

            break;

        final[v] = true;		// 把V 加入S集合

        // 更新当前最短路径及距离(V 作为中间点)

        for ( w=0; w<M_vertexNum; w++ ) {

            if ( final[w] == false && (current_tmp_min+(M_edges[v][w]) < dist[w]) )  // v0 -> v -> w 比 v0 -> w 短

            {

                // 第一次main loop, dist[1] 更新成 4, 2 作为1 的前驱

                dist[w] = current_tmp_min + (M_edges[v][w]);     // 更新最短路径长度, 此时dist[w]为 "确定值"

                pre[w] = v;     // v作为w的前驱顶点

            }

        }

    }   //end main loop

    return;

}

/**

 * @brief MinPath::showShortestPath

 * @param v0

 * @param pre

 * @param dist

 */

void MinPath::showShortestPath(int v0, int *pre, int *dist) {

    int v;

    int i;

    stack<int> s;

    cout << "从顶点 " << v0 << "　到其他顶点: " << endl;

    for ( v=0; v<M_vertexNum; v++ ) {

        if ( pre[v] == -1 )   //v0 到 v 不通

        {

            cout << "\t" << v << " 没有通路" << endl << endl;

            continue;

        }

        cout <<  "\t顶点" << v << endl;

        cout << "\t最短路径长度: " << dist[v] << endl;

        i = v;

        while ( pre[i] != -1 ) {

            //cout << "前驱: " << pre[i] << endl;

            s.push(pre[i]);             // 入栈，交换顺序

            i = pre[i];

        }

        cout << "\t最短路径: ";

        for ( unsigned long size = s.size(); size > 0; size -- ) {

            cout << s.top() << " -> ";

            s.pop();

        }

        cout << v << endl << endl;

    }

    return;

}

/**

 * @brief main

 * @return

 */

int main() {

    MinPath G;

    int pre[MAXVERTEXNUM];

    int dist[MAXVERTEXNUM];

    G.initGraph();

    G.showOriginalGraph();

    G.computeShortestPath(0, pre, dist);

    G.showShortestPath(0, pre, dist);

    return 0;

}


/**
 * @section DESCRIPTION
 *
 * Perfect Numbers.
 *
 * An integer is said to be a perfect number if the sum of its divisors,
 * including 1 (but not the number itself), is equal to the number. For
 * example, 6 is a perfect number, because 6=1+2+3. Write a function isPerfect
 * that determines whether parameter number is a perfect number. Use this
 * function in a program that determines and prints all the perfect numbers
 * between 1 and 1000. Print the divisors of each perfect number to confirm
 * that the number is indeed perfect. Challenge the power of your computer by
 * testing numbers much larger than 1000.
 */

#include <boost/multiprecision/cpp_int.hpp>
#include <iomanip>
#include <iostream>
#include <set>
#include <memory>

using boost::multiprecision::pow;
using boost::multiprecision::sqrt;
using boost::multiprecision::uint128_t;
using std::cout;
using std::endl;
using std::set;
using std::setw;
using std::unique_ptr;

using int_set_ptr = unique_ptr<set<uint128_t>>;

template <class T>
struct Accumulator final
{
    Accumulator() : sum {0}
    {
    }

    void operator()(const T &input)
    {
        sum += input;
    }
public:
    T sum;
};

static int_set_ptr FindDivisors(const uint128_t &number)
{
    int_set_ptr divisor_set_ptr {new set<uint128_t>};
    const uint128_t kDivisorLimit {sqrt(number)};

    // Base Case:
    // 1 does not have any valid candidate divisor to be considered as a
    // potential perfect number.
    if (number <= 1) {
        return divisor_set_ptr;
    }

    divisor_set_ptr->insert(1);

    for (uint128_t i {2}; i <= kDivisorLimit; ++i) {
        if (number % i == 0) {
            divisor_set_ptr->insert(i);
            divisor_set_ptr->insert(number / i);
        }
    }
    return divisor_set_ptr;
}

static bool IsPerfectNumber(const uint128_t &number,
                            int_set_ptr &divisor_set_ptr)
{
    // Compiler would use copy elision, so a 'move' in unnecessary.
    divisor_set_ptr = FindDivisors(number);

    if (divisor_set_ptr->size() == 0) {
        return false;
    }

    Accumulator<uint128_t> accumulator;

    for (auto &i : *divisor_set_ptr) {
        accumulator(i);
    }

    return accumulator.sum == number;
}

int main(void)
{
    static constexpr int kWidthLeft {15}, kWidthRight {40};
    int_set_ptr divisor_set_ptr;

    cout \
        << setw(kWidthLeft) << "Perfect Number"
        << setw(kWidthRight) << "Divisors" << endl;

    for (uint128_t i = 1; i <= uint128_t(1) << 16; ++i) {
        if (IsPerfectNumber(i, divisor_set_ptr)) {
            cout << setw(kWidthLeft) << i << setw(kWidthRight);
            for (auto divisor_iterator = divisor_set_ptr->cbegin();
                 divisor_iterator != divisor_set_ptr->cend();
                 ++divisor_iterator) {
                cout << *divisor_iterator;
                if (++divisor_iterator == divisor_set_ptr->cend()) {
                    --divisor_iterator;
                    cout << endl;
                } else {
                    --divisor_iterator;
                    cout << ", ";
                }
            }
        }
    }
    return 0;
}


#include <iostream>
#include <tuple>
#include <Eigen/Dense>
#include "linear_algebra_addon.hpp"
using namespace Eigen;
using namespace std;

MatrixXcd bl_bicr_rq(const MatrixXcd& A, const MatrixXcd& B, const double& tol, const int& itermax)
{
    // Tadano et al 2014, Improvement of the accuracy of the approximate solution of the block BiCR method
  double Bnorm= B.norm();
  MatrixXcd X= MatrixXcd::Zero(B.rows(),B.cols()); // Initial guess of X (zeros)
  MatrixXcd R= B-A*X;
  MatrixXcd Q;
  MatrixXcd xi;
  tie(Q,xi)= qr_reduced(R);

  MatrixXcd R_til= R; // or R_til= R.conjugate();
  MatrixXcd Q_til;
  MatrixXcd xi_til;
  tie(Q_til,xi_til)= qr_reduced(R_til);

  MatrixXcd S= Q;
  MatrixXcd S_til= Q_til;
  MatrixXcd U= A*Q;
  MatrixXcd U_til= A.adjoint()*Q_til;
  MatrixXcd V_til= U_til;

  for(int k= 0; k < itermax; ++k){

      MatrixXcd alpha= (U_til.adjoint()*U).fullPivLu().solve(V_til.adjoint()*Q);
      MatrixXcd alpha_til= (U.adjoint()*U_til).fullPivLu().solve(Q.adjoint()*V_til);

      X= X+S*alpha*xi;

      MatrixXcd Qnew;
      MatrixXcd tau;
      tie(Qnew,tau)= qr_reduced(Q-U*alpha);
      MatrixXcd Qnew_til;
      MatrixXcd tau_til;
      tie(Qnew_til,tau_til)= qr_reduced(Q_til-U_til*alpha_til);

      xi= tau*xi;
      MatrixXcd Vnew_til= A.adjoint()*Qnew_til;

      double err= xi.norm()/Bnorm;
      cout << "bl_bicr_rq: " << "iter= " << k << " relative err= " << err << endl;
      if(err < tol) break;

      MatrixXcd beta= (V_til.adjoint()*Q).fullPivLu().solve(tau_til.adjoint()*Vnew_til.adjoint()*Qnew);
      MatrixXcd beta_til= (Q.adjoint()*V_til).fullPivLu().solve(tau.adjoint()*Qnew.adjoint()*Vnew_til);

      Q= Qnew;
      Q_til= Qnew_til;
      V_til= Vnew_til;
      S= Q+S*beta;
      S_til= Q_til+S_til*beta_til;
      U_til= V_til+U_til*beta_til;
      U= A*S;
  }

  if((A*X-B).norm()/Bnorm > 10*tol){
      cerr << "bl_bicr_rq did not converge to solution within error tolerance !" << endl;
     // exit(EXIT_FAILURE);
  }

  return X;
}


#include <Eigen/Dense>
#include "simple_loss.h"

namespace MyDL{

    double cross_entropy_error(MatrixXd& y, MatrixXd& t){
        int batch_size = y.rows();
        double ret = (t.array() * y.array().log()).sum() / batch_size;
        return -ret;
    }

}


#include "../incidencematrices.h"

#include <gtest/gtest.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <memory>

namespace IncidenceMatrices::test {

// Create demo mesh from exercise sheet and test if edge vertex incidence
// matrix is the same as the one calculated by hand
TEST(Homework_2_6, EdgeVertexIncidenceMatrix) {
  std::shared_ptr<lf::mesh::Mesh> demoMesh =
      IncidenceMatrices::createDemoMesh();
  Eigen::SparseMatrix<int> G_sp =
      IncidenceMatrices::computeEdgeVertexIncidenceMatrix(*demoMesh);
  Eigen::MatrixXi G(G_sp);

  Eigen::MatrixXi G_expected(6, 5);
  // clang-format off
  G_expected << 1, -1,  0,  0,  0,
               -1,  0,  0,  1,  0,
                0,  1, -1,  0,  0,
                0, -1,  0,  0,  1,
                0,  0,  1,  0, -1,
                0,  0,  0, -1,  1;
  // clang-format on
  EXPECT_EQ(G.rows(), G_expected.rows());
  EXPECT_EQ(G.cols(), G_expected.cols());
  if (G.rows() == G_expected.rows() && G.cols() == G_expected.cols())
    EXPECT_EQ(G, G_expected);
}

// Create demo mesh from exercise sheet and test if cell edge incidence
// matrix is the same as the one calculated by hand
TEST(Homework_2_6, CellEdgeIncidenceMatrix) {
  std::shared_ptr<lf::mesh::Mesh> demoMesh =
      IncidenceMatrices::createDemoMesh();
  Eigen::SparseMatrix<int> D_sp =
      IncidenceMatrices::computeCellEdgeIncidenceMatrix(*demoMesh);
  Eigen::MatrixXi D(D_sp);

  Eigen::MatrixXi D_expected(2, 6);
  // clang-format off
  D_expected << 0, 0, 1,  1, 1, 0,
                1, 1, 0, -1, 0, 1;
  // clang-format on
  EXPECT_EQ(D.rows(), D_expected.rows());
  EXPECT_EQ(D.cols(), D_expected.cols());
  if (D.rows() == D_expected.rows() && D.cols() == D_expected.cols())
    EXPECT_EQ(D, D_expected);
}

// Test co-chain complex property (D*G = 0) at the example of the mesh
// in the exercise sheet
TEST(Homework_2_6, CoChainComplexProperty) {
  std::shared_ptr<lf::mesh::Mesh> demoMesh =
      IncidenceMatrices::createDemoMesh();
  EXPECT_TRUE(IncidenceMatrices::testZeroIncidenceMatrixProduct(*demoMesh));
}

}  // namespace IncidenceMatrices::test


#include "DenseMatrix.hpp"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <gtest/gtest.h>
#include <math.h>

#define REAL double
#define TOL 1.0E-06

const int SIZE = 100;

TEST(CASE_01, one_dimension_configuration){
  int side = sqrt(SIZE);
  REAL *A = (REAL*)calloc(SIZE, sizeof(REAL));
  REAL *B = (REAL*)calloc(SIZE, sizeof(REAL));
  REAL *C = (REAL*)calloc(SIZE, sizeof(REAL));

  boost::numeric::ublas::matrix<double> A_bst(side, side), B_bst(side, side),
    C_bst(side, side);

  randMatrix(A, side, side); randMatrix(B, side, side);
  for (int i=0; i<SIZE; ++i) C[i] = 0.0;

  for (int i=0; i<side; ++i){
    for (int j=0; j<side; ++j){
      int idx = i*side + j;
      A_bst(i,j) = A[idx];
      B_bst(i,j) = B[idx];
    }
  }
  // Manual Version
  dgemm(A, side, side, B, side, side, C);
  // Boost Version
  boost::numeric::ublas::axpy_prod(A_bst, B_bst, C_bst, true);

  for (int i=0; i<side; ++i){
	  for (int j=0; j<side; ++j){
	    int idx = i*side + j;
	    EXPECT_NEAR(C_bst(i,j), C[idx], TOL);
  	}
  }
  free(A); free(B); free(C);
}

int main(int argc, char *argv[]){
  testing::InitGoogleTest(&argc,argv);
  return RUN_ALL_TESTS();
}


#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <Eigen/Jacobi>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

#include <cmath>

#include <sphericalsfm/plane_estimator.h>
#include <sphericalsfm/so3.h>

namespace sphericalsfm {
    int PlaneEstimator::sampleSize()
    {
        return 3;
    }

    double PlaneEstimator::score( RayPairList::iterator it )
    {
        const Eigen::Vector3d &x = it->first.head(3);
        const double proj = normal.dot( x ) + d;
        return proj*proj;
    }

    bool PlaneEstimator::canRefine()
    {
        return true;
    }

    int PlaneEstimator::compute( RayPairList::iterator begin, RayPairList::iterator end )
    {
        int N = std::distance(begin,end);

        Eigen::MatrixXd A(N,4);

        int i = 0;
        for ( RayPairList::iterator it = begin; it != end; it++,i++ )
        {
            const Eigen::Vector3d x = it->first.head(3);

            A(i,0) = x(0);
            A(i,1) = x(1);
            A(i,2) = x(2);
            A(i,3) = 1;
        }

        Eigen::Matrix4d B = A.jacobiSvd(Eigen::ComputeFullV).matrixV();
        Eigen::Vector4d soln = B.col(3);
        soln = soln/soln.head(3).norm();
        normal = soln.head(3);
        d = soln(3);

        return 1;
    }
}


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

using namespace Eigen;

void My_Polar(const Eigen::Matrix3f& F, Eigen::Matrix3f& R, Eigen::Matrix3f& S) {

	Eigen::JacobiRotation<float> G;
	float tol = 1e-6, max_it = 1e4, S_diff[] = { 100,100,100 }, S_diff_max;
	int it = 0, i_rot = 0, i, j, i_max;

	R = MatrixXf::Identity(3, 3);
	S = F;

	for (i_max = 0; i_max < 3; i_max++) {
		if (i_max == 0)
			S_diff_max = S_diff[0];
		else if (S_diff[i_max] > S_diff[i_max - 1])
			S_diff_max = S_diff[i_max];
	}

	while (it<max_it && S_diff_max > tol) {

		for (i_rot = 0; i_rot < 3; i_rot++) {
			if (i_rot == 0) {
				i = 1;
				j = 2;
			}
			else if (i_rot == 1) {
				i = 0;
				j = 2;
			}
			else if (i_rot == 2) {
				i = 0;
				j = 1;
			}
			G.makeGivens(S(i, i) + S(j, j), S(i, j) - S(j, i));
			R.applyOnTheRight(i, j, G.adjoint());
			S.applyOnTheLeft(i, j, G);

		}

		it++;
		S_diff[0] = std::abs(S(1, 2) - S(2, 1));
		S_diff[1] = std::abs(S(0, 2) - S(2, 0));
		S_diff[2] = std::abs(S(0, 1) - S(1, 0));
		for (i_max = 0; i_max < 3; i_max++) {
			if (i_max == 0)
				S_diff_max = S_diff[0];
			else if (S_diff[i_max] > S_diff[i_max - 1])
				S_diff_max = S_diff[i_max];
		}

	}

}

int main()
{
	Eigen::Matrix3f F, R, S;

	F << 1, 2, 6,
		4, 3, 2,
		8, 4, 6;

	My_Polar(F, R, S);

	std::cout << R << '\n' << '\n';
	std::cout << R*R.transpose() << '\n' << '\n';
	std::cout << S << '\n' << '\n';
	std::cout << R*S << '\n' << '\n';
	std::cout << F << '\n';

	system("pause");

	return 0;

}


#ifndef POLYNOMIAL_HPP
#define POLYNOMIAL_HPP

#include <Eigen/Core>

#include <cmath>

namespace polynomial {

    // Evaluate polynomial using Horner's scheme
    template <typename T> double eval(const Eigen::DenseBase<T>& c, double x) {
        double y = 0;
        for (int i = c.size()-1; i >= 0; i--)
            y = c(i) + y * x;
        return y;
    }

    // Evaluate polynomial derivative using Horner's scheme
    template <typename T> double deriv(const Eigen::DenseBase<T>& c, double x) {
        double yd = 0;
        for (int i = c.size()-1; i > 0; i--)
            yd = (i+1) * c(i) + yd * x;
        return yd;
    }

    // Find root of polynomial using Newton's method
    template <typename T> double solve(const Eigen::DenseBase<T>& c,
            double x, double tol) {
        double dx;
        int it = 0;
        do {
            dx = eval(c, x) / deriv(c, x);
            x -= dx;
            it++;
        } while (fabs(dx) > tol && it < 10);
        return x;
    }

}

#endif


#include <iostream>

#include <Eigen/Core>
#include <catch2/catch.hpp>

#include <finitediff.hpp>

TEST_CASE("Test finite difference jacobian of linear", "[jacobian]")
{
    int n = GENERATE(1, 2, 4, 10, 100);

    // f(x) = Ax
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(n, n);

    const auto f = [&](const Eigen::VectorXd x) -> Eigen::VectorXd {
        return A * x;
    };

    Eigen::VectorXd x = Eigen::VectorXd::Random(n);

    Eigen::MatrixXd jac = A;

    fd::AccuracyOrder accuracy = fd::AccuracyOrder(GENERATE(range(0, 4)));

    Eigen::MatrixXd fjac;
    fd::finite_jacobian(x, f, fjac, accuracy);

    CHECK(fd::compare_jacobian(jac, fjac));
}

TEST_CASE("Test finite difference jacobian of trig", "[jacobian]")
{
    int n = GENERATE(1, 2, 4, 10, 100);

    const auto f = [&](const Eigen::VectorXd x) -> Eigen::VectorXd {
        return x.array().sin();
    };

    Eigen::VectorXd x = Eigen::VectorXd::Random(n);

    Eigen::MatrixXd jac = x.array().cos().matrix().asDiagonal();

    fd::AccuracyOrder accuracy = fd::AccuracyOrder(GENERATE(range(0, 4)));

    Eigen::MatrixXd fjac;
    fd::finite_jacobian(x, f, fjac, accuracy);

    CHECK(fd::compare_jacobian(jac, fjac));
}


/**
 * @file
 * @brief function realizing an embedded Runge-Kutta-Fehlberg explicit
 * single-step method
 * @author Ralf Hiptmair
 * @date   April 2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <iostream>
#include <vector>

namespace EmbeddedRKSSM {

/**
 * @brief Adaptive embedded Runge-Kutta-Fehlberg timestepping
 *
 * @tparam RHSFunction functor type for right-hand-side vector field
 * @param f_rhs functor providing the right-hand-side function of the autonomous
 * ODE through its evaluation operator
 * @param A Butcher matrix of size s x s (only strictly lower triangular part
 * used)
 * @param b weight vector for RKSSM of order p+1
 * @param bh weight vector for RKSSM of order p
 * @param p order of lower-order method
 * @param y0 intial value
 * @param T final time
 * @param h0 initial stepsize
 * @param reltol relative tolerance for timestep control
 * @param abstol absolute tolerance for timestep control
 * @param hmin minimal admissible stepsize
 */
template <typename RHSFunction>
std::vector<std::pair<double, Eigen::VectorXd>> embeddedRKSSM(
    RHSFunction &&f_rhs, const Eigen::MatrixXd &A, const Eigen::VectorXd &b,
    const Eigen::VectorXd &bh, unsigned int p, const Eigen::VectorXd &y0,
    double T, double h0, double reltol, double abstol, double hmin) {
  // Check parameters defining embedded RK-SSM
  unsigned int s = A.cols();  // Number of stages
  assert((s == A.rows()) && "Butcher matrix must be square");
  assert((s == b.size()) && "Length of weight vector b != no of stages");
  assert((s == bh.size()) && "Length of weight vector bh != no of stages");
  unsigned int N = y0.size();  // Dimension of state space
  Eigen::MatrixXd K(N, s);     // Columns hold increment vectors

  double t = 0.0;  // Initial time zero for autonomous initial-value problem
  double h = h0;   // Current timestep size
  std::vector<std::pair<double, Eigen::VectorXd>> states{
      {t, y0}};            // State sequence
  Eigen::VectorXd y = y0;  // Current state
  states.emplace_back(t, y);
  // Main timestepping loop
  while ((states.back().first < T) && (h >= hmin)) {
    // Compute increments
    K.col(0) = f_rhs(y);
    for (int l = 1; l < s; ++l) {
      Eigen::VectorXd v{Eigen::VectorXd::Zero(N)};
      for (int i = 0; i < l; ++i) {
        v += A(l, i) * K.col(i);
      }
      K.col(l) = f_rhs(y + h * v);
    }
    // Compute next two approximate states
    auto yh = y + h * K * b;   // high-order method
    auto yH = y + h * K * bh;  // low-order method
    double est = (yh - yH).norm();
    double tol = std::max(reltol * y.norm(), abstol);
    if (est <= tol) {
      y = yh;  // Advance to next approximate state
      t += h;  // Next time
      states.emplace_back(t, y);
      // std::cout << "t = " << t << ", y = " << y.transpose() << std::endl;
    }
    // else {
    //   std::cout << "tol/est = " << tol / est << ", h = " << h << std::endl;
    // }
    h *= std::max(0.5, std::min(2., 0.9 * std::pow(tol / est, 1. / (p + 1))));
    if (h < hmin) {
      std::cerr
          << "Warning: Failure at t=" << states.back().first
          << ". Unable to meet integration tolerances without reducing the step"
          << " size below the smallest value allowed (" << hmin
          << ") at time t." << std::endl;
    } else {
      h = std::min(T - t + hmin, h);
    }
  }
  return states;
}

// Helper function for testing
void testrun(const Eigen::MatrixXd &A, const Eigen::VectorXd &b,
             const Eigen::VectorXd &bh, unsigned int p) {
  std::cout << "Test run of embedded RK-SSM" << std::endl;
  std::cout << "Butcher matrix = \n " << A << std::endl;
  std::cout << "Weight vector (order p+1) = " << b.transpose() << std::endl;
  std::cout << "Weight vector (order p) = " << bh.transpose() << std::endl;
  // Simple linear test case: rotation ODE
  auto f = [](Eigen::Vector2d y) -> Eigen::Vector2d {
    return Eigen::Vector2d(-y[1], y[0]);
  };
  Eigen::VectorXd y0(2);
  y0 << 1.0, 0.0;
  // Final time
  const double T = 2.0 * 3.14159265358979323846;
  const double hmin = T / 1E6;
  const double h0 = T / 100;
  // Test different tolerances
  const int m = 6;
  std::array<double, m> rtol{0.1, 0.01, 0.001, 0.0001, 0.00001, 0.000001};
  std::array<double, m> atol{0.01, 0.001, 0.0001, 0.00001, 0.000001, 0.0000001};
  for (int j = 0; j < m; ++j) {
    auto res = embeddedRKSSM(f, A, b, bh, p, y0, T, h0, rtol[j], atol[j], hmin);
    double err = 0.0;
    for (const auto &i : res) {
      const double t = i.first;
      const Eigen::Vector2d exact(std::cos(t), std::sin(t));
      const Eigen::Vector2d approx = i.second;
      err = std::max(err, (exact - approx).norm());
    }
    std::cout << "rtol = " << rtol[j] << ", atol = " << atol[j] << " : "
              << (res.size() - 1) << " steps, err = " << err << std::endl;
  }
}

}  // namespace EmbeddedRKSSM

int main(int /*argc*/, char ** /*argv*/) {
  std::cout << "Adaptive embedded Runge-Kutta-Fehlberg single-step method"
            << std::endl;
  {
    // Simplest embedded method: Euler - Heun
    Eigen::MatrixXd A(2, 2);
    A << 0, 0, 1, 0;
    Eigen::VectorXd b(2);
    b << 0.5, 0.5;
    Eigen::VectorXd bh(2);
    bh << 1.0, 0.0;
    EmbeddedRKSSM::testrun(A, b, bh, 1);
  }

  {
    // More complicated: Bogacki-Shampine
    // https://en.wikipedia.org/wiki/List_of_Runge%E2%80%93Kutta_methods#Embedded_methods
    Eigen::MatrixXd A(4, 4);
    A << 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0.75, 0, 0, 2. / 9., 1. / 3., 4. / 9., 0;
    Eigen::VectorXd b(4);
    b << 2.0 / 9., 1. / 3., 4. / 9., 0;
    Eigen::VectorXd bh(4);
    bh << 7. / 24., 1. / 4., 1. / 3., 1. / 8.;
    EmbeddedRKSSM::testrun(A, b, bh, 2);
  }
  return 0;
}


/***************************************************************************************
    File: data_generator.cpp

    Description:
      produces a linearly separable dataset from a weight vector with randomly
      generated weights. All features will be uniformally drawn from [-1, 1]
      as bounded rationals.
      I.e if the bound for the dataset is 100 the denominator is set to 100 and
      the numerator is randomly chosen from [-100, 100] (integral values).

    Usage:
    data_generator
      -o <OUTPUT_FILE>
      -t <TRAINING_COUNT>
      -f <FEATURE_COUNT>
      -b <BOUND>
      -w (STDOUT | FEATURE | BOTH) // prints weights to stdout or at end of features_file
 ***************************************************************************************/
#include <iostream>
#include <fstream>
#include <vector>
#include <random>
#include <algorithm>
#include <string>
#include <sstream>

#include <boost/rational.hpp>
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/random.hpp>

typedef boost::multiprecision::mpz_int Z;
typedef boost::rational<Z> Q;
typedef std::vector<Q> Qvec;

std::ostream& operator << (std::ostream& outs, const Q& q){
  return outs << q.numerator() << " % " << q.denominator();
}

class Random{
  public:
    Random (Z b) : bound(b), engine(std::random_device{}()) {}

    Q operator()() {
      return Q(randZ(), bound);
    }

  private:
    Z bound;
    std::mt19937 engine;

    Z randZ(){
      return boost::random::uniform_int_distribution<Z>(-bound, bound)(engine);
    }
};

int main(int argc, char ** argv){
  if (!(argc % 2) || argc > 11){
    std::cerr << "\033[1;31mError: Invalid number of Arguments.\n\033[0m";
    return -1;
  }

  std::string file_string = "test_features.dat";
  size_t training_count = 100;
  size_t feature_count = 10;
  char show_weight = 0; // 0 = don't print, 1 = std_out, 2 = features_file
  Z bound = 100;

  for (size_t i = 1; i < argc; i += 2){
    std::stringstream argss(std::string(argv[i]) + " " + std::string(argv[i+1]));
    std::string option;
    argss >> option;

    if (option == "-o"){
      argss >> file_string;

    } else if (option == "-t"){
      if (!(argss >> training_count) || training_count < 2) {
        std::cerr << "\033[1;31mError: Invalid training count.\n\033[0m";
        return -1;
      }

    } else if (option == "-f"){
      if (!(argss >> feature_count) || feature_count < 1) {
        std::cerr << "\033[1;31mError: Invalid feature count.\n\033[0m";
        return -1;
      }

    } else if (option == "-b"){
      if (!(argss >> bound) || bound < 1) {
        std::cerr << "\033[1;31mError: Invalid feature count.\n\033[0m";
        return -1;
      }

    } else if (option == "-w"){
      std::string w;
      argss >> w;
      if (w == "STDOUT"){
        show_weight = 1;
      } else if (w == "BOTH"){
        show_weight = 2;
      } else if (w == "FEATURE"){
        show_weight = 3;
      } else {
        std::cerr << "\033[1;31mError: Invalid selection: " << w << ".\n\033[0m";
        return -1;
      }

    } else {
      std::cerr << "\033[1;31mError: Invalid option: " << option << ".\n\033[0m";
      return -1;
    }
  }

  std::ofstream features_ofs(file_string);
  if (!features_ofs.is_open()) {
    std::cerr << "\033[1;31mError: Invalid Output file: " << file_string << ".\n\033[0m";
    return -1;
  }

  Qvec weights(feature_count + 1); // Adds bias term
  Qvec features(feature_count);

  std::generate(weights.begin(), weights.end(), Random(bound));

  features_ofs << training_count << std::endl;
  features_ofs << feature_count << std::endl;

  while (training_count--){
    Q dot;
    do {
      dot = weights[0];
      std::generate(features.begin(), features.end(), Random(bound));
      for (size_t i = 0; i < features.size(); ++i){
        dot += weights[i+1] * features[i];
      }
    } while (!dot); // make sure point does not lie on boundary

    features_ofs << (dot > 0) << std::endl;
    for (size_t i = 0; i < features.size(); ++i){
      features_ofs << features[i] << std::endl;
    }
  }

  if (show_weight == 1 || show_weight == 2){
    for (size_t i = 0; i < weights.size(); ++i){
      std::cout << weights[i] << std::endl;
    }
  } else if (show_weight == 2 || show_weight == 3){
    for (size_t i = 0; i < weights.size(); ++i){
      features_ofs << weights[i] << std::endl;
    }
  }

  features_ofs.close();
  return 0;
}


/* adcpp_eigen_backward.test.cpp
 *
 *  Created on: 21 Aug 2019
 *      Author: Fabian Meyer
 */

#include <catch2/catch.hpp>
#include <adcpp/adcpp_eigen.hpp>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include "assert/eigen_require.hpp"

using namespace adcpp;

TEST_CASE("Eigen backward algorithmic differentiation")
{
    double eps = 1e-6;

    SECTION("exp")
    {
        bwd::Vector2d x;
        x << 3, 2;

        Eigen::Vector2d valExp;
        valExp <<
            std::exp(x(0).value()),std::exp(x(1).value());
        Eigen::Matrix2d jacExp;
        jacExp << std::exp(x(0).value()), 0,
            0, std::exp(x(1).value());

        bwd::Vector2d f = x.array().exp();

        Eigen::Matrix2d jacAct;
        bwd::jacobian(x, f, jacAct);

        REQUIRE_MATRIX_APPROX(valExp, f.template cast<double>(), eps);
        REQUIRE_MATRIX_APPROX(jacExp, jacAct, eps);
    }

    // SECTION("singular value decomposition")
    // {
    //     bwd::Matrix4d A;
    //     A << 2, 3, 11, 5,
    //         1, 1, 5, 2,
    //         2, 1, -3, 2,
    //         1, 1, -3, 4;
    //     bwd::Vector4d b;
    //     b << 2, 1, -3, -3;
    //
    //     Eigen::Vector4d valExp;
    //     Eigen::Vector4d gradExp;
    //     valExp << -0.5, -0.1875, 0.4375, -0.25;
    //     gradExp << -0.205283, 0.687338, -0.0722404, -0.117474;
    //
    //     Eigen::JacobiSVD<bwd::Matrix4d, Eigen::FullPivHouseholderQRPreconditioner>
    //         solver(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    //     bwd::Vector4d f = solver.solve(b);
    //
    //     Eigen::MatrixXd jacAct(4, 4);
    //     std::cout << "jacobian" << std::endl;
    //     jacobian(b, f, jacAct);
    //
    //     std::cout << jacAct << std::endl;
    //     REQUIRE_MATRIX_APPROX(valExp, f.template cast<double>(), eps);
    // }

    // SECTION("eigen value decomposition")
    // {
    //     bwd::Matrix4d A;
    //     A << 2, 3, 11, 5,
    //         1, 1, 5, 2,
    //         2, 1, -3, 2,
    //         1, 1, -3, 4;
    //     bwd::Vector4d b;
    //     b << 2, 1, -3, -3;
    //
    //     Eigen::Vector4d eigvalsExp;
    //     eigvalsExp << 7.27048, -5.64984, -0.291657, 2.67103;
    //     Eigen::Vector4d eiggradExp;
    //     eiggradExp <<  0.536189,  0.463811, 0, 0;
    //
    //     Eigen::EigenSolver<bwd::Matrix4d> solver(A);
    //     bwd::Vector4d eigvals = solver.eigenvalues().real();
    //     bwd::Matrix4d eigvecs = solver.eigenvectors().real();
    //
    //     Eigen::MatrixXd jacAct(4, 4);
    //     std::cout << "jacobian" << std::endl;
    //     jacobian(b, eigvals, jacAct);
    //
    //     REQUIRE_MATRIX_APPROX(eigvalsExp, eigvals.template cast<double>(), eps);
    // }

    SECTION("multiple outputs")
    {
        bwd::Vector2d x;
        x << bwd::Double(3), bwd::Double(2);

        bwd::Matrix2d c;
        c << bwd::Double(2.1), bwd::Double(3.4),
            bwd::Double(1.6), bwd::Double(2.3);

        Eigen::Vector2d valExp;
        valExp <<
            x(0).value() * c(0, 0).value() +  x(1).value() * c(0, 1).value(),
            x(0).value() * c(1, 0).value() +  x(1).value() * c(1, 1).value();
        Eigen::Matrix2d jacExp;
        jacExp << c(0, 0).value(), c(0, 1).value(),
            c(1, 0).value(), c(1, 1).value();

        bwd::Vector2d f = c * x;
        Eigen::Matrix2d jacAct;
        jacobian(x, f, jacAct);

        REQUIRE_MATRIX_APPROX(valExp, f.template cast<double>(), eps);
        REQUIRE_MATRIX_APPROX(jacExp, jacAct, eps);
    }
}


/**
 * @date 2021.3.4
 * @author hqy - sentinel
 * @note 数字图像处理第一次作业
 */

#include <utility>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <Eigen/Core>
#include "include/utils.hpp"

/// 求得图像均值以及方差
std::pair<double, double> getMeanVar(const cv::Mat& src, bool use_buildin = false) {
    if (use_buildin == false){      // 手写实现
        int img_sz = src.cols * src.rows;
        uchar* data = src.data;
        double mean = 0.0, var = 0.0;
        uint64_t start_t = getCurrentTime();
        for (size_t i = 0; i < img_sz; i++){
            mean += double(data[i]);
        }
        mean /= double(img_sz);
        for (size_t i = 0; i < img_sz; i++){
            var += std::pow(double(data[i]) - mean, 2);
        }
        var /= double(img_sz);
        return std::make_pair(mean, var);
    }
    else{           // opencv 调库两行
        cv::Scalar mean, var2;
        cv::meanStdDev(src, mean, var2);
        return std::make_pair(mean[0], std::pow(var2[0], 2));
    }
}

void imgCrop(const uchar* const data, int px, int py, int step, uchar* buf) {
    int cnt = 0;
    for (int i = 0; i < 4; i++){
        int offset = (py + i) * step + px;
        for (int j = 0; j < 4; j++, cnt++){
            buf[cnt] = data[offset + j];
        }
    }
}

void linearInterpZoom(const cv::Mat& src, cv::Mat& dst, int k = 4) {
    cv::Mat padding;
    int nrows = src.rows * k, ncols = src.cols * k;
    dst = cv::Mat::zeros(cv::Size(ncols, nrows), CV_8UC1);
    cv::copyMakeBorder(src, padding, 1, 1, 1, 1, CV_HAL_BORDER_REPLICATE);      // opencv padding操作
    uint64_t start_t = getCurrentTime();
    #pragma omp parallel for num_threads(8)
    for (size_t i = 0; i < nrows; i++) {
        int base = i * ncols;
        for (size_t j = 0; j < ncols; j++) {
            ;
        }
    }
}

template<typename T>
void getWeightVector(T* res, double z, double a = -0.5){
    for (int i = -1, cnt = 0; cnt < 4; i++, cnt++){
        double x = std::abs(i - z);
        if (x <= 1)
            res[cnt] = (a + 2) * std::pow(x, 3) - (a + 3) * std::pow(x, 2) + 1;
        else if (x < 2)
            res[cnt] = a * std::pow(x, 3) - 5 * a * std::pow(x, 2) + 8 * a * x - 4 * a;
    }
}

template<typename T = double>
uchar calcWeightSum(const T* const wx, const T* const wy, const uchar* const buf) {
    int cnt = 0;
    T res = 0.0;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++, cnt++) {
            res += wx[j] * wy[i] * T(buf[cnt]);
        }
    }
    return uchar(res);
}

void biCubicInterpZoom(const cv::Mat& src, cv::Mat& dst, int k = 4) {
    cv::Mat padding;
    int nrows = src.rows * k, ncols = src.cols * k;
    dst = cv::Mat::zeros(cv::Size(ncols, nrows), CV_8UC1);
    cv::copyMakeBorder(src, padding, 1, 2, 1, 2, CV_HAL_BORDER_REPLICATE);      // opencv padding操作
    uint64_t start_t = getCurrentTime();
    #pragma omp parallel for num_threads(8)
    for (size_t i = 0; i < nrows; i++) {
        int base = i * ncols;
        for (size_t j = 0; j < ncols; j++) {
            int px = j / k, py = i / k;
            double u = double(j) / double(k) - double(px);
            double v = double(i) / double(k) - double(py);
            double wx[4] = {0, 0, 0, 0};
            double wy[4] = {0, 0, 0, 0};
            getWeightVector<double>(wx, u);
            getWeightVector<double>(wy, v);
            uchar crop[16];
            imgCrop(padding.data, px, py, padding.cols, crop);
            dst.data[base + j] = calcWeightSum<double>(wx, wy, crop);
        }
    }
    uint64_t end_t = getCurrentTime();
    printf("Time elapse: %lf ms\n", double(end_t - start_t) / 1e6);
}

void rotate(const cv::Mat& src, cv::Mat& dst, double angle) {
    dst.create(src.rows, src.cols, CV_8UC1);
}

void shear(const cv::Mat& src, cv::Mat& dst, double ratio) {
    dst.create(src.rows, src.cols, CV_8UC1);
}

int main() {
    cv::Mat img = cv::imread("../data/lena.bmp", 0);
    cv::Mat dst;
    biCubicInterpZoom(img, dst);
    cv::imshow("disp", dst);
    cv::waitKey(0);
    cv::imwrite("../data/czoomed.bmp", dst);
    std::pair<double, double> res = getMeanVar(img, false);
    printf("Mean: %lf, Var: %lf\n", res.first, res.second);
    res = getMeanVar(img, true);
    printf("Mean: %lf, Var: %lf\n", res.first, res.second);

    return 0;
}


#include <iostream>
#include <string>
#include <Eigen/Dense>
using Eigen::MatrixXd;
using std::string;
extern "C" {
	MatrixXd newMatrixXd(int aCols,int aRows,double* aRaw)
	{
		 MatrixXd m(aCols,aRows);
		 for(int i=0;i<aCols;i++)
		 {
			 for(int j=0;j<aRows;j++)
			 {
				 m(i,j) =aRaw[i*aRows+j];
			 }
		}
		return m;
	}
	const char* printMatrixXd(int aCols,int aRows,double* aRaw)
	{
		 std::cout <<"aCols="<< aCols <<",aRows="<< aRows<< std::endl;
		 MatrixXd m(aCols,aRows);
		 for(int i=0;i<aCols;i++)
		 {
			 for(int j=0;j<aRows;j++)
			 {
				 int index=i*aRows+j;
				 m(i,j) =aRaw[index];
				std::cout <<index<<","<<"m("<<i<<","<<j<<")="<<m(i,j)<< std::endl;
			 }
		 }
		std::stringstream str;
		str << m;
		// std::cout << m << std::endl;
		return str.str().c_str() ;
	}

	const char* addMatrixXd(int aCols,int aRows,double* aRaw,int bCols,int bRows,double* bRaw)
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		MatrixXd  B=newMatrixXd(bCols,bRows,bRaw);
		MatrixXd C=A+B;
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"+"<< std::endl<<B<< std::endl<<"="<< std::endl<<C << std::endl;
		return str.str().c_str() ;
	}
	const char* subMatrixXd(int aCols,int aRows,double* aRaw,int bCols,int bRows,double* bRaw)
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		MatrixXd  B=newMatrixXd(bCols,bRows,bRaw);
		MatrixXd C=A-B;
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"-"<< std::endl<<B<< std::endl<<"="<< std::endl<<C<< std::endl;
		return str.str().c_str() ;
	}
	const char* trMatrixXd(int aCols,int aRows,double* aRaw)
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		MatrixXd C=A.transpose();
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"transpose"<< std::endl<<C<< std::endl;
		return str.str().c_str() ;
	}
	const char* muiltMatrixXd(int aCols,int aRows,double* aRaw,int bCols,int bRows,double* bRaw)
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		MatrixXd  B=newMatrixXd(bCols,bRows,bRaw);
		MatrixXd C=A*B;
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"*"<< std::endl<<B<< std::endl<<"="<< std::endl<<C<< std::endl;
		return str.str().c_str() ;
	}
	const char* inverseMatrixXd(int aCols,int aRows,double* aRaw)
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		MatrixXd C=A.inverse();
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"inverse"<< std::endl<<C<< std::endl;
		return str.str().c_str() ;
	}
	const char* adjointMatrixXd(int aCols,int aRows,double* aRaw)
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		MatrixXd C=A.adjoint();
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"adjoint"<< std::endl<<C<< std::endl;
		return str.str().c_str() ;
	}
	const char* traceMatrixXd(int aCols,int aRows,double* aRaw )
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		double C=A.trace();
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"trace"<< std::endl<<C<< std::endl;
		return str.str().c_str() ;
	}
	const char* detMatrixXd(int aCols,int aRows,double* aRaw)
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		double C=A.determinant();
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"det"<< std::endl<<C<< std::endl;
		return str.str().c_str() ;
	}
	const char* rankMatrixXd(int aCols,int aRows,double* aRaw)
	{
		MatrixXd  A=newMatrixXd(aCols,aRows,aRaw);
		Eigen::FullPivLU<MatrixXd> lu_decomp(A);
		double C=lu_decomp.rank();
		std::stringstream str;
		str << C;
		std::cout << A<< std::endl<<"LU rank"<< std::endl<<C<< std::endl;
		return str.str().c_str() ;
	}
}


#include "../../Headers/Edmonton.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using namespace std;
using boost::multiprecision::cpp_int;

/*
Really straightforward problem to brute-force.
We don't really need to worry about the actual decimals, the numerator and denominator follow an obvious pattern where:
1. numerator += 2 * denominator
2. denominator = numerator - denominator
for each of the expansion/iteration we are testing.
*/

int main(int argc, char *argv[]) {
	int num_count = 0;
	cpp_int numerator = 3, denominator = 2;
	for(int i = 1; i < 1'000; i++) {
		numerator += 2 * denominator;
		denominator = numerator - denominator;
		if(Edmonton::digitCount<int, cpp_int>(numerator) > Edmonton::digitCount<int, cpp_int>(denominator)) {
			num_count++;
		}
	}
	cout << num_count << endl;
	return 0;
}


// -*- coding: utf-8 -*-

#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <chrono>

int main()
{
  std::cout << "" << std::endl;
  std::cout << " # C++ Eigen benchmark" << std::endl;

  int n;
  std::ifstream fr("input");
  fr >> n;
  std::cout << "n " << n << std::endl;

  std::srand((unsigned int)time(NULL));

  Eigen::MatrixXd A = Eigen::MatrixXd::Random(n, n);
  Eigen::MatrixXd B = Eigen::MatrixXd::Random(n, n);
  std::chrono::system_clock::time_point start, end;
  double time;

  // multiplication
  start = std::chrono::system_clock::now();
  Eigen::MatrixXd C = A*B;
  end = std::chrono::system_clock::now();
  time = static_cast<double>(std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count()/1000.0);
  std::cout << "multiplication time " << time << std::endl;

  // inverse
  start = std::chrono::system_clock::now();
  Eigen::MatrixXd D = A.inverse();
  end = std::chrono::system_clock::now();
  time = static_cast<double>(std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count()/1000.0);
  std::cout << "inverse time " << time << std::endl;

  // LU inverse
  start = std::chrono::system_clock::now();
  Eigen::MatrixXd E = A.partialPivLu().inverse();
  end = std::chrono::system_clock::now();
  time = static_cast<double>(std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count()/1000.0);
  std::cout << "LU inverse time " << time << std::endl;

  // LU decomposition
  start = std::chrono::system_clock::now();
  Eigen::MatrixXd F = A.partialPivLu().matrixLU();
  end = std::chrono::system_clock::now();
  time = static_cast<double>(std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count()/1000.0);
  std::cout << "LU decomposition time " << time << std::endl;

  // rank-revealing QR
  start = std::chrono::system_clock::now();
  Eigen::MatrixXd G = A.colPivHouseholderQr().matrixQR();
  end = std::chrono::system_clock::now();
  time = static_cast<double>(std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count()/1000.0);
  std::cout << "rank-revealing QR time " << time << std::endl;
}


/**
 * @file expfittedupwind.cc
 * @brief NPDE homework ExpFittedUpwind
 * @author Amélie Loher, Philippe Peter
 * @date 07.01.2021
 * @copyright Developed at ETH Zurich
 */

#include "expfittedupwind.h"

#include <lf/base/base.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>

#include <Eigen/Core>
#include <cmath>
#include <memory>
#include <vector>

namespace ExpFittedUpwind {

/**
 * @brief Computes the Bernoulli function B(tau)
 **/
double Bernoulli(double tau) {
  if (std::abs(tau) < 1e-10) {
    return 1.0;
  } else if (std::abs(tau) < 1e-3) {
    return 1.0 / (1.0 + (0.5 + 1.0 / 6.0 * tau) * tau);
  } else {
    return tau / (std::exp(tau) - 1.0);
  }
}

/**
 * @brief computes the quantities \beta(e) for all the edges e of a mesh
 * @param mesh_p underlying mesh
 * @param mu vector of nodal values of a potential Psi
 * @return  Mesh Data set containing the quantities \beta(e)
 */
std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<double>> CompBeta(
    std::shared_ptr<const lf::mesh::Mesh> mesh_p, const Eigen::VectorXd& mu) {
  // data set over all edges of the mesh.
  auto beta_p = lf::mesh::utils::make_CodimMeshDataSet(mesh_p, 1, 1.0);

  // compute beta(e) for all edges of the mesh
  for (const lf::mesh::Entity* edge : mesh_p->Entities(1)) {
    // compute the indices of the endpoints of the edge
    // These are needed to  access the correct nodal values of mu
    auto endpoints = edge->SubEntities(1);
    unsigned int i = mesh_p->Index(*(endpoints[0]));
    unsigned int j = mesh_p->Index(*(endpoints[1]));

    (*beta_p)(*edge) = std::exp(mu(j)) * Bernoulli(mu(j) - mu(i));
  }

  return beta_p;
}

/**
 * @brief actual computation of the element matrix
 * @param cell reference to the triangle for which the matrix is evaluated
 * @return 3x3 dense matrix containg the element matrix
 */
Eigen::Matrix3d ExpFittedEMP::Eval(const lf::mesh::Entity& cell) {
  LF_VERIFY_MSG(cell.RefEl() == lf::base::RefEl::kTria(),
                "Only 2D triangles are supported.");

  // Evaluate the element matrix A_K
  Eigen::Matrix3d AK = laplace_provider_.Eval(cell).block<3, 3>(0, 0);

  Eigen::Matrix3d result;

  // get the values of beta on the edges of the triangle.
  // by the Lehrfem++ numbering convention
  // b = [beta(e_0), beta(e_1), beta(e_2)]' = [\beta_{1,2}, \beta_{2,3},
  // \beta_{1,3}]'
  Eigen::Vector3d b = beta_loc(cell);

  // evaluate the element matrix using the formula in subproblem h)
  result << AK(0, 1) * b(0) + AK(0, 2) * b(2), -AK(0, 1) * b(0),
      -AK(0, 2) * b(2), -AK(0, 1) * b(0), AK(0, 1) * b(0) + AK(1, 2) * b(1),
      -AK(1, 2) * b(1), -AK(0, 2) * b(2), -AK(1, 2) * b(1),
      AK(0, 2) * b(2) + AK(1, 2) * b(1);

  Eigen::Vector3d mu_exp = (-mu_loc(cell)).array().exp();
  result *= mu_exp.asDiagonal();

  return std::move(result);
}

/**
 * @brief returns the quanties beta(e) for  the
 * three edges e_0, e_1 and e_2 of a triangle.
 * @param cell reference to the triangle for which the quantities are needed
 * @return vector  [beta(e_0),beta(e_1),beta(e_2)]'
 **/
Eigen::Vector3d ExpFittedEMP::beta_loc(const lf::mesh::Entity& cell) {
  Eigen::Vector3d b;
  auto edges = cell.SubEntities(1);
  for (int i = 0; i < 3; ++i) {
    b(i) = (*beta_)(*(edges[i]));
  }
  return b;
}

/** @brief returns the nodal values of the potential Psi for the
 * three vertices a_1, a_2 and a_3 of a triangle
 * @param cell reference to the triangle for which the quantities are needed
 * @return vector [Psi(a_1), Psi(a_2), Psi(a_3)]'
 **/
Eigen::Vector3d ExpFittedEMP::mu_loc(const lf::mesh::Entity& cell) {
  Eigen::Vector3d m;
  auto mesh_p = fe_space_->Mesh();
  auto vertices = cell.SubEntities(2);
  for (int i = 0; i < 3; ++i) {
    int index = mesh_p->Index(*(vertices[i]));
    m(i) = mu_(index);
  }
  return m;
}

} /* namespace ExpFittedUpwind */


#include "math/rotation.h"

#include <ceres/rotation.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <limits>

namespace GraphSfM {
// Eigen::Vector3d MultiplyRotations(const Eigen::Vector3d& rotation1,
//                                   const Eigen::Vector3d& rotation2) {
//   const double theta1_sq = rotation1.squaredNorm();
//   const double theta2_sq = rotation2.squaredNorm();

//   // Compute the sin and cosine terms below. Take care to ensure that there will
//   // not be any divide-by-zeros when the rotations are small.
//   double cos_a;
//   double cos_b;
//   Eigen::Vector3d sin_a_times_v1;
//   Eigen::Vector3d sin_b_times_v2;

//   // We need to explicity handle the cases when the rotations are near zero.
//   // Near zero, the first order Taylor approximation of the rotation matrix R
//   // corresponding to a vector w and angle w is
//   //
//   //   R = I + hat(w) * sin(theta)
//   //
//   // But sintheta ~ theta and theta * w = angle_axis, which gives us
//   //
//   //  R = I + hat(angle_axis)
//   //
//   // We will use this in the special cases below to ensure stable computation
//   // when composing the rotations and avoid dividing by zero when theta is
//   // small.
//   if (theta1_sq < std::numeric_limits<double>::epsilon()) {
//     // Use the fact that theta ~ sin(theta) when theta is small. Since
//     // a = 0.5 * theta1, we end up with:
//     //   sin(a) * v1 = sin(theta / 2) * v1 = theta / 2 * v1 = rotation1 / 2.
//     sin_a_times_v1 = 0.5 * rotation1;
//     // When a is small, cos(a) ~ 1.0 by the first order taylor approximation.
//     cos_a = 1.0;
//   } else {
//     const double theta1 = std::sqrt(theta1_sq);
//     const double sin_a = std::sin(0.5 * theta1);
//     cos_a = std::cos(0.5 * theta1);
//     sin_a_times_v1 = sin_a * rotation1 / theta1;
//   }

//   // Same as above, but for theta2.
//   if (theta2_sq < std::numeric_limits<double>::epsilon()) {
//     sin_b_times_v2 = 0.5 * rotation2;
//     cos_b = 1.0;
//   } else {
//     const double theta2 = std::sqrt(theta2_sq);
//     const double sin_b = std::sin(0.5 * theta2);
//     cos_b = std::cos(0.5 * theta2);
//     sin_b_times_v2 = sin_b * rotation2 / theta2;
//   }

//   // Compute sin(c) * v3 using the formula above.
//   const Eigen::Vector3d sin_c_times_v3 = cos_b * sin_a_times_v1 +
//                                          cos_a * sin_b_times_v2 +
//                                          sin_a_times_v1.cross(sin_b_times_v2);

//   // If sin(c) is near zero then we again need to take care to avoid dividing by
//   // zero. We can use the first order Taylor approximation again, noting that
//   // sin(c) ~ c, which gives us:
//   //   rotation3 = theta * v3 = 2 * c * v3 ~ 2 * sin(c) * v3
//   const double sin_c_sq = sin_c_times_v3.squaredNorm();
//   if (sinc_c < std::numeric_limits<double>::epsilon()) {
//     const double diff = (2.0 * sin_c_times_v3 - rotation_aa).norm();
//     return 2.0 * sin_c_times_v3;
//   } else {
//     // Otherwise, we use the formula above. The angle axis rotation is the axis
//     // (v3) times the angle theta3.
//     const double sin_c = std::sqrt(sin_c_sq);
//     const double theta3 = 2.0 * std::asin(sin_c);
//     const Eigen::Vector3d v3 = sin_c_times_v3 / sin_c;

//     return theta3 * v3;
//   }
// }

// Use Ceres to perform a stable composition of rotations. This is not as
// efficient as directly composing angle axis vectors (see the old
// implementation commented above) but is more stable.
Eigen::Vector3d MultiplyRotations(const Eigen::Vector3d& rotation1,
                                  const Eigen::Vector3d& rotation2) {
  Eigen::Matrix3d rotation1_mat, rotation2_mat;
  ceres::AngleAxisToRotationMatrix(rotation1.data(), rotation1_mat.data());
  ceres::AngleAxisToRotationMatrix(rotation2.data(), rotation2_mat.data());

  const Eigen::Matrix3d rotation = rotation1_mat * rotation2_mat;
  Eigen::Vector3d rotation_aa;
  ceres::RotationMatrixToAngleAxis(rotation.data(), rotation_aa.data());
  return rotation_aa;
}

}  // namespace GraphSfM


#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "vio/eigen_utils.h"

namespace vio{

using namespace Eigen;
using namespace std;
Eigen::Vector3d rotro2eu(Eigen::Matrix3d R)
{
    Eigen::Vector3d euler;
    euler[0] = atan2(R(2,1), R(2,2));
    euler[1] = -(atan2(R(2,0),  sqrt(1 - R(2,0) * R(2,0))));
    euler[2] = atan2(R(1,0), R(0,0));
    return euler;
}

Eigen::Matrix3d roteu2ro(Eigen::Vector3d eul)
{
    double cr = cos(eul[0]); double sr = sin(eul[0]);	//roll
    double cp = cos(eul[1]); double sp = sin(eul[1]);	//pitch
    double ch = cos(eul[2]); double sh = sin(eul[2]);	//heading
    Eigen::Matrix3d dcm;
    dcm(0,0) = cp * ch;
    dcm(0,1) = (sp * sr * ch) - (cr * sh);
    dcm(0,2) = (cr * sp * ch) + (sh * sr);

    dcm(1,0) = cp * sh;
    dcm(1,1) = (sr * sp * sh) + (cr * ch);
    dcm(1,2) = (cr * sp * sh) - (sr * ch);

    dcm(2,0) = -sp;
    dcm(2,1) = sr * cp;
    dcm(2,2) = cr * cp;
    return dcm;
}
//input: lat, long in radians, height is immaterial
//output: Ce2n
Eigen::Matrix3d llh2dcm(const Eigen::Vector3d llh)
{
    double sL = sin(llh[0]);
    double cL = cos(llh[0]);
    double sl = sin(llh[1]);
    double cl = cos(llh[1]);

    Eigen::Matrix3d Ce2n;
    Ce2n<< -sL * cl, -sL * sl, cL ,  -sl, cl, 0 , -cL * cl, -cL * sl, -sL;
    return Ce2n;
}

Eigen::MatrixXd nullspace(const Eigen::MatrixXd& A)
{
    Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
   //ColPivHouseholderQR<MatrixXd> qr(A); //don't use column pivoting because in that case Q*R-A!=0
    Eigen::MatrixXd nullQ = qr.householderQ();

    int rows= A.rows(), cols= A.cols();
    assert( rows> cols); // "Rows should be greater than columns in computing nullspace"
    nullQ= nullQ.block(0,cols,rows,rows-cols).eval();
    return nullQ;
}

void leftNullspaceAndColumnSpace(const Eigen::MatrixXd &A,
                                        Eigen::MatrixXd *Q2,
                                        Eigen::MatrixXd *Q1)
{
    int rows= A.rows(), cols= A.cols();
    assert( rows> cols); // "Rows should be greater than columns in computing left nullspace"
    Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
    //don't use column pivoting because in that case Q*R-A!=0
    Eigen::MatrixXd Q = qr.householderQ();

    Q2->resize(rows, rows-cols);
    *Q2 = Q.block(0,cols,rows,rows-cols);

    Q1->resize(rows, cols);
    *Q1 = Q.block(0,0,rows,cols);
}

Eigen::Matrix<double, Eigen::Dynamic, 1> superdiagonal(
        const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & M)
{
    const int numElements = std::min(M.rows(), M.cols()) - 1;
    Eigen::Matrix<double, Eigen::Dynamic, 1> r(numElements, 1);
    for(int jack = 0; jack< numElements; ++jack)
        r[jack] = M(jack, jack+1);
    return r;
}

Eigen::Matrix<double, Eigen::Dynamic, 1> subdiagonal(
        const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & M)
{
    const int numElements = std::min(M.rows(), M.cols()) - 1;
    Eigen::Matrix<double, Eigen::Dynamic, 1> r(numElements, 1);
    for(int jack = 0; jack< numElements; ++jack)
        r[jack] = M(jack+1, jack);
    return r;
}

void reparameterize_AIDP(const Eigen::Matrix3d &Ri, const Eigen::Matrix3d &Rj, const Eigen::Vector3d &abrhoi,
                         const Eigen::Vector3d &pi, const Eigen::Vector3d &pj, Eigen::Vector3d& abrhoj,
                         Eigen::Matrix<double, 3, 9>* jacobian)
{
   Eigen::Matrix<double, 4,4> Tci2cj =Eigen::Matrix<double, 4,4>::Identity();
   Tci2cj.topLeftCorner<3,3>()= Rj.transpose()*Ri;
   Tci2cj.block<3,1>(0,3).noalias() = Rj.transpose()*(pi-pj);
   Eigen::Matrix<double, 4, 1> homogi;
   homogi<< abrhoi.head<2>(), 1, abrhoi[2];
   double rhoj_drhoi = 1/ (Tci2cj.row(2)* homogi); //\rho_j divided by \rho_i
   abrhoj.head<2>() = rhoj_drhoi* Tci2cj.topLeftCorner<2,4>()*homogi;
   abrhoj[2] = abrhoi[2]*rhoj_drhoi;
   if(jacobian)
   {
       Eigen::Matrix3d lhs;
       lhs.setIdentity();
       lhs.col(2)= - abrhoj;
       //{\alpha, \beta, \rho}_i
       Eigen::Matrix<double, 3, 3> subrhs;
       subrhs<< Tci2cj.topLeftCorner<3,2>(), Tci2cj.block<3,1>(0,3);
       jacobian->topLeftCorner<3,3>() = rhoj_drhoi*lhs*subrhs;
       (*jacobian)(2,2) = rhoj_drhoi*rhoj_drhoi*Tci2cj.block<1,3>(2,0)*homogi.head<3>();
       //{pi, pj}
       Eigen::Matrix<double, 3, 6> rhs;
       rhs.topLeftCorner<3,3>()= abrhoi[2]*Rj.transpose(),
       rhs.block<3,3>(0,3)= - rhs.topLeftCorner<3,3>();
       jacobian->block<3,6>(0,3) = rhoj_drhoi*lhs*rhs;

   }
}

void reparameterizeNumericalJacobian(const Eigen::Matrix3d &Ri, const Eigen::Matrix3d &Rj, const Eigen::Vector3d &abrhoi,
                                     const Eigen::Vector3d &pi, const Eigen::Vector3d &pj, Eigen::Vector3d& abrhoj,
                                     Eigen::Matrix<double, 3, 9>& jacobian){
   // numerical differentation
   Eigen::Vector3d abrhojp;
   reparameterize_AIDP(Ri, Rj, abrhoi, pi, pj, abrhoj);
   double h= 1e-8;
   for(int jack =0; jack<3; ++jack){
       Eigen::Vector3d abrhoip= abrhoi;
       abrhoip[jack]= abrhoi[jack] + h;
       reparameterize_AIDP(Ri, Rj, abrhoip, pi, pj, abrhojp);
       Eigen::Vector3d subJacobian = (abrhojp - abrhoj)/h;
       jacobian.col(jack) = subJacobian;
   }

   for(int jack =0; jack<3; ++jack){
       Eigen::Vector3d pip= pi; pip[jack]= pi[jack] + h;
       reparameterize_AIDP(Ri, Rj, abrhoi, pip, pj, abrhojp);
       Eigen::Vector3d subJacobian = (abrhojp - abrhoj)/h;
       jacobian.col(jack+3) = subJacobian;
   }

   for(int jack =0; jack<3; ++jack){
       Eigen::Vector3d pjp =pj; pjp[jack]= pj[jack] + h;
       reparameterize_AIDP(Ri, Rj, abrhoi, pi, pjp, abrhojp);
       Eigen::Vector3d subJacobian = (abrhojp - abrhoj)/h;
       jacobian.col(jack+6) = subJacobian;
   }
}

void testReparameterize()
{
    double distances[] ={3, 3e2, 3e4, 3e8}; //close to inifity
    for(size_t jack = 0; jack< sizeof(distances)/sizeof(distances[0]); ++jack){
        double dist = distances[jack];

        Eigen::Matrix3d Ri = Eigen::Matrix3d::Identity();
        Eigen::Vector3d ptini;
        ptini<< dist*cos(15*M_PI/180)*cos(45*M_PI/180), -dist*sin(15*M_PI/180),  dist*cos(15*M_PI/180)*sin(45*M_PI/180);
        Eigen::Matrix3d Rj = Eigen::AngleAxisd(30*M_PI/180, Eigen::Vector3d::UnitY()).toRotationMatrix();

        Eigen::Vector3d pi =Eigen::Vector3d::Zero();
        Eigen::Vector3d pj =Eigen::Vector3d::Random();

        Eigen::Vector3d ptinj = Rj.transpose()*(ptini - pj);

        Eigen::Vector3d abrhoi =Eigen::Vector3d(ptini[0],ptini[1],1)/ptini[2];
        Eigen::Vector3d abrhoj;
        Eigen::Matrix<double, 3, 9> jacobian;

        reparameterize_AIDP(Ri, Rj, abrhoi, pi, pj, abrhoj, &jacobian);
        Eigen::Matrix<double, 3, 9> jacobian3;
        reparameterizeNumericalJacobian(Ri, Rj, abrhoi, pi, pj, abrhoj, jacobian3);
        std::cout<<"analytic jacobian " << std::endl<< jacobian<<std::endl;
        std::cout <<"analytic jacobian - numerical jacobian "<< std::endl << (jacobian - jacobian3) <<std::endl;
        std::cout <<"abrhoi and j"<< abrhoi.transpose()<< " "<< abrhoj.transpose()<< std::endl;
    }
    //infinity
    Eigen::Matrix3d Ri = Eigen::Matrix3d::Identity();
    Eigen::Vector3d ptiniRay;
    ptiniRay<< cos(15*M_PI/180)*cos(45*M_PI/180), -sin(15*M_PI/180),  cos(15*M_PI/180)*sin(45*M_PI/180);
    Eigen::Matrix3d Rj = Eigen::AngleAxisd(30*M_PI/180, Eigen::Vector3d::UnitY()).toRotationMatrix();

    Eigen::Vector3d pi =Eigen::Vector3d::Zero();
    Eigen::Vector3d pj =Eigen::Vector3d::Random();

    Eigen::Vector3d ptinjRay = Rj.transpose()*ptiniRay;
    ptinjRay/=ptinjRay[2];
    ptinjRay[2] =0;

    Eigen::Vector3d abrhoi =Eigen::Vector3d(1, -tan(15*M_PI/180)/sin(45*M_PI/180),0);
    Eigen::Vector3d abrhoj;
    Eigen::Matrix<double, 3, 9> jacobian;

    reparameterize_AIDP(Ri, Rj, abrhoi, pi, pj, abrhoj, &jacobian);
    Eigen::Matrix<double, 3, 9> jacobian3;
    reparameterizeNumericalJacobian(Ri, Rj, abrhoi, pi, pj, abrhoj, jacobian3);
    std::cout <<"infinity point case "<< std::endl;
    std::cout<<"analytic jacobian " << std::endl<< jacobian<<std::endl;
    std::cout <<"analytic jacobian - numerical jacobian "<< std::endl << (jacobian - jacobian3) <<std::endl;
    std::cout <<"abrhoi and j"<< abrhoi.transpose()<< " "<< abrhoj.transpose()<< std::endl;
    std::cout <<"expected abrhoj "<< ptinjRay.transpose()<<std::endl;
}

void testExtractBlocks()
{
    Eigen::MatrixXd m(5,5);
    m<< 1,2,3,4,5,
        6,7,8,9,10,
        11,12,13,14,15,
        16,17,18,19,20,
        21,22,23,24,25;

    std::vector<std::pair<size_t, size_t> > vRowStartInterval;
    for(size_t jack=0; jack<5; ++jack)
        vRowStartInterval.push_back(std::make_pair(jack, 1));
    // test deleting none entry
    Eigen::MatrixXd res = extractBlocks(m, vRowStartInterval, vRowStartInterval);
    assert((res- m).lpNorm<Eigen::Infinity>()<1e-8);

    // test deleting odd indexed rows/cols
    vRowStartInterval.clear();
    for(size_t jack=0; jack<5; jack+=2)
        vRowStartInterval.push_back(std::make_pair(jack, 1));
    res = extractBlocks(m, vRowStartInterval, vRowStartInterval);
    Eigen::MatrixXd expected(3,3);
    expected<< 1,3,5,
            11,13,15,
            21,23,25;
    assert((res- expected).lpNorm<Eigen::Infinity>()<1e-8);

// test deleting even indexed rows/cols
    vRowStartInterval.clear();
    for(size_t jack=1; jack<5; jack+=2)
        vRowStartInterval.push_back(std::make_pair(jack, 1));
    res = extractBlocks(m, vRowStartInterval, vRowStartInterval);
    Eigen::MatrixXd expected2(2,2);
    expected2<< 7,9,17,19;
    assert((res- expected2).lpNorm<Eigen::Infinity>()<1e-8);

// test with keeping more than 1 rows/cols each time
    vRowStartInterval.clear();
    vRowStartInterval.push_back(std::make_pair(0, 2));
    vRowStartInterval.push_back(std::make_pair(3, 2));
    res = extractBlocks(m, vRowStartInterval, vRowStartInterval);
    Eigen::MatrixXd expected3(4,4);
    expected3<<1,2,4,5,
            6,7,9,10,
            16,17,19,20,
            21,22,24,25;;
    assert((res- expected3).lpNorm<Eigen::Infinity>()<1e-8);

// test with different rows and cols to keep
    vRowStartInterval.clear();
    vRowStartInterval.push_back(std::make_pair(0, 2));
    vRowStartInterval.push_back(std::make_pair(3, 2));
    std::vector<std::pair<size_t, size_t> > vColStartInterval;
    vColStartInterval.push_back(std::make_pair(0,2));
    vColStartInterval.push_back(std::make_pair(3,1));
    res = extractBlocks(m, vRowStartInterval, vColStartInterval);
    Eigen::MatrixXd expected4(4,3);
    expected4<< 1,2,4,
            6,7,9,
            16,17,19,
            21,22,24;
    assert((res- expected4).lpNorm<Eigen::Infinity>()<1e-8);
}

Eigen::Vector3d unskew3d(const Eigen::Matrix3d & Omega) {
   return 0.5 * Eigen::Vector3d(Omega(2,1) - Omega(1,2), Omega(0,2) - Omega(2,0), Omega(1,0) - Omega(0,1));
}

}


/**
 * @file dynamical_systems.hpp
 * @author Manuel Wuthrich
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-08-05
 */

#pragma once

#include <cmath>
#include <math.h>
#include <Eigen/Eigen>
#include <iostream>

#include "mpi_cpp_tools/basic_tools.hpp"
#include "mpi_cpp_tools/math.hpp"

namespace mct
{

class LinearDynamics
{
public:
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 10> Vector;

    LinearDynamics(Eigen::Vector4d parameters): LinearDynamics(parameters[0],
                                                parameters[1],
                                                parameters[2],
                                                parameters[3]) { }

    LinearDynamics(double jerk,
                   double initial_acceleration,
                   double initial_velocity,
                   double initial_position)
    {
        jerk_ = jerk;
        initial_acceleration_ = initial_acceleration;
        initial_velocity_ = initial_velocity;
        initial_position_= initial_position;
    }
    double get_acceleration(mct::NonnegDouble t) const
    {
        return  jerk_ * t +
                initial_acceleration_;
    }
    double get_velocity(mct::NonnegDouble t) const
    {
        return jerk_ * 0.5 * t * t +
                initial_acceleration_ * t +
                initial_velocity_;
    }
    double get_position(mct::NonnegDouble t) const
    {
        return jerk_ * 0.5 * 1./3. * t * t * t +
                initial_acceleration_ * 0.5 * t * t +
                initial_velocity_ * t +
                initial_position_;
    }

    Vector find_t_given_velocity(double velocity) const
    {
        double a = jerk_ * 0.5;
        double b = initial_acceleration_;
        double c = initial_velocity_ - velocity;

        double determinant = b * b - 4 * a * c;

        Vector solutions(Vector::Index(0));
        if(a == 0)
        {
            if(b != 0)
            {
                solutions.resize(1);
                solutions[0] = - c / b;
            }

        }
        else if(determinant == 0)
        {
            solutions.resize(1);
            solutions[0] = -b / 2 / a;
        }
        else if(determinant > 0)
        {
            double determinant_sqrt = std::sqrt(determinant);
            solutions.resize(2);
            solutions[0] = (-b + determinant_sqrt) / 2 / a;
            solutions[1] = (-b - determinant_sqrt) / 2 / a;
        }

        Vector positive_solutions(Vector::Index(0));
        for(int i = 0; i < solutions.size(); i++)
        {
            if(solutions[i] >= 0)
            {
                mct::append_to_vector(positive_solutions, solutions[i]);
            }
        }

        return positive_solutions;
    }

protected:
    double jerk_;
    double initial_acceleration_;
    double initial_velocity_;
    double initial_position_;
};

class LinearDynamicsWithAccelerationConstraint: public LinearDynamics
{
public:

    void print_parameters() const
    {
        std::cout << "-------------------------------------------" << std::endl;
        std::cout << "jerk: " << jerk_ << std::endl
                  << "initial_acceleration: " << initial_acceleration_ << std::endl
                  << "initial_velocity: " << initial_velocity_ << std::endl
                  << "initial_position: " << initial_position_ << std::endl
                  << "acceleration_limit: " << acceleration_limit_ << std::endl
                  << "jerk_duration: " << jerk_duration_ << std::endl;
        std::cout << "-------------------------------------------" << std::endl;
    }

    typedef LinearDynamics::Vector Vector;

    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, 0, 10, 10>
    Matrix;

    LinearDynamicsWithAccelerationConstraint(Eigen::Matrix<double, 5, 1> parameters):
        LinearDynamicsWithAccelerationConstraint(parameters[0],
        parameters[1],
        parameters[2],
        parameters[3],
        parameters[4]) { }

    LinearDynamicsWithAccelerationConstraint(double jerk,
                                             double initial_acceleration,
                                             double initial_velocity,
                                             double initial_position,
                                             mct::NonnegDouble abs_acceleration_limit):
        LinearDynamics(jerk,
                       initial_acceleration,
                       initial_velocity,
                       initial_position)
    {
        if(jerk_ > 0)
            acceleration_limit_ = abs_acceleration_limit;
        else
            acceleration_limit_ = -abs_acceleration_limit;

        set_initial_acceleration(initial_acceleration);
    }

    void set_initial_acceleration(double initial_acceleration)
    {
        if(std::fabs(initial_acceleration) > std::fabs(acceleration_limit_))
            throw std::invalid_argument("expected "
                                        "std::fabs(initial_acceleration) > "
                                        "abs_acceleration_limit");
        initial_acceleration_ = initial_acceleration;
        jerk_duration_ =
                (acceleration_limit_ - initial_acceleration_) / jerk_;
    }

    double get_acceleration(mct::NonnegDouble t) const
    {
        if(t < jerk_duration_)
        {
            return  LinearDynamics::get_acceleration(t);
        }
        else
        {
            return acceleration_limit_;
        }
    }
    double get_velocity(mct::NonnegDouble t) const
    {
        if(t < jerk_duration_)
        {
            return LinearDynamics::get_velocity(t);
        }
        else
        {
            return LinearDynamics::get_velocity(jerk_duration_) +
                    acceleration_limit_ * (t - jerk_duration_);
        }
    }
    double get_position(mct::NonnegDouble t) const
    {
        if(t < jerk_duration_)
        {
            return LinearDynamics::get_position(t);
        }
        else
        {
            return LinearDynamics::get_position(jerk_duration_) +
                    LinearDynamics::get_velocity(jerk_duration_) * (t - jerk_duration_) +
                    acceleration_limit_ * 0.5 * (t - jerk_duration_) * (t - jerk_duration_);
        }
    }

    template<typename Array>
    Array get_positions(const Array& times) const
    {
        Array positions(times.size());
        for(size_t i = 0; i < size_t(times.size()); i++)
        {
            positions[i] = get_position(times[i]);
        }
        return positions;
    }

    Vector find_t_given_velocity(double velocity) const
    {
        Vector potential_solutions =
                LinearDynamics::find_t_given_velocity(velocity);

        Vector solutions(Vector::Index(0));
        for(int i = 0; i < potential_solutions.size(); i++)
        {
            if(potential_solutions[i] <= jerk_duration_)
            {
                mct::append_to_vector(solutions, potential_solutions[i]);
            }
        }

        double potential_solution = jerk_duration_
                + (velocity - LinearDynamics::get_velocity(jerk_duration_))
                / acceleration_limit_;
        if(potential_solution > jerk_duration_ &&
                !(mct::contains(solutions, jerk_duration_) &&
                  mct::approx_equal(potential_solution, jerk_duration_)))
        {
            mct::append_to_vector(solutions, potential_solution);
        }

        if(solutions.size() > 2)
        {
            std::cout << "too many solutions, something went wrong!!!"
                      << std::endl;
            print_parameters();

            std::cout << "potential_solutions[0]: " << potential_solutions[0] << std::endl;

            std::cout << "potential_solutions size: " << potential_solutions.size() << " content: " << potential_solutions.transpose() << std::endl;

            std::cout << "solutions size: " << solutions.size() << " content: " << solutions.transpose() << std::endl;
            exit(-1);
        }
        return solutions;
    }

    bool will_exceed_jointly(const double& max_velocity,
                             const double& max_position) const
    {
        double certificate_time;
        return will_exceed_jointly(max_velocity, max_position, certificate_time);
    }

    bool will_exceed_jointly(const double& max_velocity,
                             const double& max_position,
                             double& certificate_time) const
    {
        if(max_velocity == std::numeric_limits<double>::infinity() ||
                max_position == std::numeric_limits<double>::infinity())
        {
            return false;
        }
        if(jerk_ > 0)
        {
            certificate_time = std::numeric_limits<double>::infinity();
            return true;
        }
        if(jerk_ == 0)
        {
            throw std::domain_error("not implemented for jerk == 0");
        }

        // find maximum achieved position --------------------------------------
        ///\todo we could do this in a cleaner way with candidate points
        //        Matrix candidate_points(0, 0);
        //        Vector candidate_times(0);

        //        mct::append_rows_to_matrix(candidate_points,
        //                              Eigen::Vector2d(initial_velocity_,
        //                                              initial_position_).transpose());
        //        mct::append_to_vector(candidate_times, 0);

        if(initial_velocity_ > max_velocity &&
                initial_position_ > max_position)
        {
            certificate_time = 0;
            return true;
        }

        Vector t_given_zero_velocity = find_t_given_velocity(0);
        if(t_given_zero_velocity.size() > 0)
        {
            Vector position_given_zero_velocity =
                    get_positions(t_given_zero_velocity);

            Vector::Index max_index;
            double max_achieved_position =
                    position_given_zero_velocity.maxCoeff(&max_index);
            if(max_achieved_position < max_position)
            {
                return false;
            }
            if(max_velocity < 0)
            {
                certificate_time = t_given_zero_velocity[max_index];
                return true;
            }
        }

        Vector t_given_max_velocity =
                find_t_given_velocity(max_velocity);
        Vector position_given_max_velocity =
                get_positions(t_given_max_velocity);

        for(int i = 0; i < position_given_max_velocity.size(); i++)
        {
            if(position_given_max_velocity[i] > max_position)
            {
                certificate_time = t_given_max_velocity[i];
                return true;
            }
        }

        return false;
    }

    bool will_deceed_jointly(const double& min_velocity,
                             const double& min_position) const
    {
        double certificate_time;
        return will_deceed_jointly(min_velocity, min_position, certificate_time);
    }

    bool will_deceed_jointly(const double& min_velocity,
                             const double& min_position,
                             double& certificate_time) const
    {
        LinearDynamicsWithAccelerationConstraint
                flipped_dynamics(-jerk_,
                                 -initial_acceleration_,
                                 -initial_velocity_,
                                 -initial_position_,
                                 std::fabs(acceleration_limit_));

        return flipped_dynamics.will_exceed_jointly(-min_velocity,
                                                    -min_position,
                                                    certificate_time);
    }

private:
    double acceleration_limit_;
    mct::NonnegDouble jerk_duration_;
};

double find_max_admissible_acceleration(
        const double& initial_velocity,
        const double& initial_position,
        const double& max_velocity,
        const double& max_position,
        const mct::NonnegDouble& abs_jerk_limit,
        const mct::NonnegDouble& abs_acceleration_limit)
{
    double lower = -abs_acceleration_limit;
    double upper = abs_acceleration_limit;

    LinearDynamicsWithAccelerationConstraint dynamics(-abs_jerk_limit,
                                                      lower,
                                                      initial_velocity,
                                                      initial_position,
                                                      abs_acceleration_limit);

    if(dynamics.will_exceed_jointly(max_velocity, max_position))
    {
        /// \todo: not quite sure what is the right thing to do here
        return lower;
    }

    dynamics.set_initial_acceleration(upper);
    if(!dynamics.will_exceed_jointly(max_velocity, max_position))
    {
        return upper;
    }

    for(size_t i = 0; i < 20; i++)
    {
        double middle = (lower + upper) / 2.0;

        dynamics.set_initial_acceleration(middle);
        if(dynamics.will_exceed_jointly(max_velocity, max_position))
        {
            upper = middle;
        }
        else
        {
            lower = middle;
        }
    }
    return lower;
}

double find_min_admissible_acceleration(
        const double& initial_velocity,
        const double& initial_position,
        const double& min_velocity,
        const double& min_position,
        const mct::NonnegDouble& abs_jerk_limit,
        const mct::NonnegDouble& abs_acceleration_limit)
{
    return -find_max_admissible_acceleration(-initial_velocity,
                                             -initial_position,
                                             -min_velocity,
                                             -min_position,
                                             abs_jerk_limit,
                                             abs_acceleration_limit);
}

class SafetyConstraint
{
public:
    SafetyConstraint()
    {
        min_velocity_ = -std::numeric_limits<double>::infinity();
        min_position_ = -std::numeric_limits<double>::infinity();
        max_velocity_ = std::numeric_limits<double>::infinity();
        max_position_ = std::numeric_limits<double>::infinity();
        max_torque_ = 1.0;
        max_jerk_ = 1.0;
        inertia_ = 1.0;
    }

    SafetyConstraint(double min_velocity,
                     double min_position,
                     double max_velocity,
                     double max_position,
                     mct::NonnegDouble max_torque,
                     mct::NonnegDouble max_jerk,
                     mct::NonnegDouble inertia)
    {
        min_velocity_ = min_velocity;
        min_position_ = min_position;
        max_velocity_ = max_velocity;
        max_position_ = max_position;
        max_torque_ = max_torque;
        max_jerk_ = max_jerk;
        inertia_ = inertia;
    }

    double get_safe_torque(const double& torque,
                           const double& velocity,
                           const double& position)
    {
        double safe_torque = mct::clamp(torque, -max_torque_, max_torque_);

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

        mct::NonnegDouble max_achievable_acc = max_torque_ / inertia_;

        double max_admissible_acc =
                find_max_admissible_acceleration(velocity,
                                                 position,
                                                 max_velocity_,
                                                 max_position_,
                                                 max_jerk_,
                                                 max_achievable_acc);
        double max_admissible_torque = max_admissible_acc * inertia_;

//        LinearDynamicsWithAccelerationConstraint
//                test_dynamics(- max_jerk_,
//                              max_achievable_acc,
//                              velocity,
//                              position,
//                              max_achievable_acc);

//        test_dynamics.print_parameters();
//        std::cout << "will exceed: " << test_dynamics.will_exceed_jointly(max_velocity_, max_position_) << std::endl;
//        std::cout << "max_admissible_acc: " << max_admissible_acc << std::endl;

//        std::cout << "max_achievable_acc: " << max_achievable_acc << std::endl;
//        std::cout << "max_admissible_acc: " << max_admissible_acc << std::endl;
//        std::cout << "max_admissible_torque: " << max_admissible_torque << std::endl;

        double min_admissible_acc =
                find_min_admissible_acceleration(velocity,
                                                 position,
                                                 min_velocity_,
                                                 min_position_,
                                                 max_jerk_,
                                                 max_achievable_acc);
        double min_admissible_torque = min_admissible_acc * inertia_;

//        std::cout << "min_admissible_acc: " << min_admissible_acc << std::endl;
//        std::cout << "min_admissible_torque: " << min_admissible_torque << std::endl;

        if(min_admissible_torque > max_admissible_torque)
        {
            std::cout << "min_admissible_torque > max_admissible_torque!!!!"
                      << std::endl;
            return 0;
        }

        safe_torque = mct::clamp(safe_torque,
                            min_admissible_torque, max_admissible_torque);

        if(safe_torque > max_torque_ || safe_torque < -max_torque_)
        {
            std::cout << "something went horribly horribly wrong " << std::endl;
            return 0;
        }

        return safe_torque;
    }

    double min_velocity_;
    double min_position_;
    double max_velocity_;
    double max_position_;
    mct::NonnegDouble max_torque_;
    mct::NonnegDouble max_jerk_;
    mct::NonnegDouble inertia_;
};

}


/**
 * @copyright
 * Copyright (c) 2012-2016, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>

#include "MathLib/LinAlg/MatrixTools.h"

#include "Tests/TestTools.h"

TEST(MathLib, LocalMatrixDeterminantInverse_Eigen)
{
    Eigen::Matrix3d fMat, fInv;
    fMat << 1, 2, 3,
            0, 1, 4,
            5, 6, 0;
    double fMat_det = MathLib::determinant(fMat);
    MathLib::inverse(fMat, fMat_det, fInv);

    Eigen::MatrixXd dMat(3,3), dInv(3,3);
    dMat = fMat;
    double dMat_det = MathLib::determinant(dMat);
    MathLib::inverse(dMat, dMat_det, dInv);

    ASSERT_NEAR(fMat_det, dMat_det, std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(fInv.data(), dInv.data(), fInv.size(), std::numeric_limits<double>::epsilon());
}


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

// To disable assert*() calls, uncomment this line:
// #define NDEBUG

// This represents a triplet of indices as a column vector of nonnegative
// integers:
// [ i ]
// [ j ]
// [ k ]
typedef Eigen::Matrix<std::size_t, 3, 1> GridIndices;

// Returns the indices of the grid cell containing the point |p| for a grid with
// lower corner |lc| and grid cell width (spacing) |dx|.
//
// If |dx| <= 0 or |p|'s location relative to |lc| would result in negative
// indices being returned and assertions are on, then assertion failures will
// crash this program.
inline GridIndices floor(const Eigen::Vector3d& p, const Eigen::Vector3d& lc,
                         double dx) {
  // Ensure grid spacings are positive.
  assert(dx > 0.0);

  // Compute |p|'s location relative to |lc|.
  // Dividing by |dx| yields a 3D vector indicating the number of grid
  // cells (including fractions of grid cells, as the vector elements are
  // floating-point values) away from |lc| that |p| is located.
  Eigen::Vector3d p_lc_over_dx = (p - lc) / dx;

  // Ensure we won't end up with negative indices.
  assert(p_lc_over_dx[0] >= 0.0);
  assert(p_lc_over_dx[1] >= 0.0);
  assert(p_lc_over_dx[2] >= 0.0);

  // Indices are valid. Construct and return them.
  // This casts the elements of the vector above as nonnegative integers.
  return p_lc_over_dx.cast<std::size_t>();
}

// Prints the provided |indices|.
void Print(const GridIndices& indices) {
  std::cout << "Indices: " << std::endl;
  std::cout << indices << std::endl;
}

int main(int argc, char** argv) {
  // Make a grid with spacing of 2 with lower corner (1, 2, 3).
  double dx = 2.0;
  Eigen::Vector3d lc(1, 2, 3);

  // (i, j, k) should be (floor((6-1)/2), floor((4-2)/2), floor((3-3)/2)),
  // which is (floor(2.5), floor(1), floor(0)) = (2, 1, 0).
  Eigen::Vector3d p1(6, 4, 3);
  GridIndices p1_indices = floor(p1, lc, dx);
  Print(p1_indices);

  // (i, j, k) should be (3, 8, 9).
  Eigen::Vector3d p2(7, 18, 22);
  GridIndices p2_indices = floor(p2, lc, dx);
  Print(p2_indices);

  // Should lead to an assertion failure for index 0 (x-coordinate) being
  // less than the x-coordinate of |lc|. The program should crash before
  // it even gets to the Print command below.
  Eigen::Vector3d p3(-1, 2, 3);
  GridIndices p3_indices = floor(p3, lc, dx);
  Print(p3_indices);

  return 0;
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Example of using Bayesian Filter Class to solve a simple problem.
 *  The example implements a Position and Velocity Filter with a Position observation.
 *  The motion model is the so called IOU Integrated Ornstein-Uhlenbeck Process Ref[1]
 *    Velocity is Brownian with a trend towards zero proportional to the velocity
 *    Position is just Velocity integrated.
 *  This model has a well defined velocity and the mean squared speed is parameterised. Also
 *  the velocity correlation is parameterised.
 *
 * Two implementations are demonstrated
 *  1) A direct filter
 *  2) An indirect filter where the filter is performed on error and state is estimated indirectly
 * Reference
 * [1] "Bayesian Multiple Target Tracking" Lawrence D Stone, Carl A Barlow, Thomas L Corwin
 */

#include "BayesFilter/UDFlt.hpp"
#include "BayesFilter/filters/indirect.hpp"
#include "Test/random.hpp"
#include <cmath>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

namespace
{
	using namespace Bayesian_filter;
	using namespace Bayesian_filter_matrix;

	// Choose Filtering Scheme to use
	typedef UD_scheme FilterScheme;

	// Square
	template <class scalar>
	inline scalar sqr(scalar x)
	{
		return x*x;
	}

	// Random numbers from Boost
	Bayesian_filter_test::Boost_random localRng;

	// Constant Dimensions
	const unsigned NX = 2;			// Filter State dimension 	(Position, Velocity)

	// Filter Parameters
	// Prediction parameters for Integrated Ornstein-Uhlembeck Process
	const Float dt = 0.01;
	const Float V_NOISE = 0.1;	// Velocity noise, giving mean squared error bound
	const Float V_GAMMA = 1.;	// Velocity correlation, giving velocity change time constant
	// Filter's Initial state uncertainty: System state is unknown
	const Float i_P_NOISE = 1000.;
	const Float i_V_NOISE = 10.;
	// Noise on observing system state
	const Float OBS_INTERVAL = 0.10;
	const Float OBS_NOISE = 0.001;

}//namespace

/*
 * Prediction model
 * Linear state predict model
 */
class PVpredict : public Linear_predict_model
{
public:
	PVpredict();
};

PVpredict::PVpredict() : Linear_predict_model(NX, 1)
{
	// Position Velocity dependence
	const Float Fvv = exp(-dt*V_GAMMA);
	Fx(0,0) = 1.;
	Fx(0,1) = dt;
	Fx(1,0) = 0.;
	Fx(1,1) = Fvv;
	// Setup constant noise model: G is identity
	q[0] = dt*sqr((1-Fvv)*V_NOISE);
	G(0,0) = 0.;
	G(1,0) = 1.;
}

/*
 * Position Observation model
 * Linear observation is additive uncorrelated model
 */
class PVobserve : public Linrz_uncorrelated_observe_model
{
	mutable Vec z_pred;
public:
	PVobserve ();
	const Vec& h(const Vec& x) const
	{
		z_pred[0] = x[0];
		return z_pred;
	};
};

PVobserve::PVobserve () :
	Linrz_uncorrelated_observe_model(NX,1), z_pred(1)
{
	// Linear model
	Hx(0,0) = 1;
	Hx(0,1) = 0.;
	// Observation Noise variance
	Zv[0] = sqr(OBS_NOISE);
}

void initialise (Kalman_state_filter& kf, const Vec& initState)
/*
 * Initialise Kalman filter with an initial guess for the system state and fixed covariance
 */
{
	// Initialise state guess and covarince
	kf.X.clear();
	kf.X(0,0) = sqr(i_P_NOISE);
	kf.X(1,1) = sqr(i_V_NOISE);

	kf.init_kalman (initState, kf.X);
}

int main()
{
	// global setup
	std::cout.flags(std::ios::scientific); std::cout.precision(6);

	// Setup the test filters
	Vec x_true (NX);

	// True State to be observed
	x_true[0] = 1000.;	// Position
	x_true[1] = 1.0;	// Velocity

	std::cout << "Position Velocity" << std::endl;
	std::cout << "True Initial  " << x_true << std::endl;

	// Construct Prediction and Observation model and filter
	// Give the filter an initial guess of the system state
	PVpredict linearPredict;
	PVobserve linearObserve;
	Vec x_guess(NX);
	x_guess[0] = 900.;
	x_guess[1] = 1.5;
	std::cout << "Guess Initial " << x_guess << std::endl;

	// f1 Direct filter construct and initialize with initial state guess
	FilterScheme f1(NX,NX);
	initialise (f1, x_guess);

	// f2 Indirect filter construct and Initialize with initial state guess
	FilterScheme error_filter(NX,NX);
	Indirect_kalman_filter<FilterScheme> f2(error_filter);
	initialise (f2, x_guess);

	// Iterate the filter with test observations
	Vec u(1), z_true(1), z(1);
	Float time = 0.; Float obs_time = 0.;
	for (unsigned i = 0; i < 100; ++i)
	{
		// Predict true state using Normally distributed acceleration
		// This is a Guassian
		x_true = linearPredict.f(x_true);
		localRng.normal (u);		// normally distributed mean 0., stdDev for stationary IOU
		x_true[1] += u[0]* sqr(V_NOISE) / (2*V_GAMMA);

		// Predict filter with known perturbation
		f1.predict (linearPredict);
		f2.predict (linearPredict);
		time += dt;

		// Observation time
		if (obs_time <= time)
		{
			// True Observation
			z_true[0] = x_true[0];

			// Observation with additive noise
			localRng.normal (z, z_true[0], OBS_NOISE);	// normally distributed mean z_true[0], stdDev OBS_NOISE.

			// Filter observation
			f1.observe (linearObserve, z);
			f2.observe (linearObserve, z);

			obs_time += OBS_INTERVAL;
		}
	}

	// Update the filter to state and covariance are available
	f1.update ();
	f2.update ();

	// Print everything: filter state and covariance
	std::cout <<"True     " << x_true << std::endl;
	std::cout <<"Direct   " << f1.x << ',' << f1.X <<std::endl;
	std::cout <<"Indirect " << f2.x << ',' << f2.X << std::endl;;
	return 0;
}