#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 <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


/**
 * @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;
}


/**
 * @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() ;
	}
}


// -*- 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_;
};

}


#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;
}


#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
   Matrix3f A;
   A << 1, 2, 5,
        2, 1, 4,
        3, 0, 3;
   cout << "Here is the matrix A:\n" << A << endl;
   FullPivLU<Matrix3f> lu_decomp(A);
   // lu_decomp.setThreshold(1e-5);
   cout << "The rank of A is " << lu_decomp.rank() << endl;
   cout << "Here is a matrix whose columns form a basis of the null-space of A:\n"
        << lu_decomp.kernel() << endl;
   cout << "Here is a matrix whose columns form a basis of the column-space of A:\n"
        << lu_decomp.image(A) << endl; // yes, have to pass the original A
}


/**
 * \file boost/numeric/ublasx/operation/rank.hpp
 *
 * \brief Rank of a matrix.
 *
 * The rank of a matrix is the number of linearly independent rows or columns.
 *
 * The \c rank function provides an estimate of the number of linearly
 * independent rows or columns of a matrix.
 * There are a number of ways to compute the rank of a matrix.
 * The currently adopted method is  based on the singular value decomposition
 * (SVD) which is the most time consuming, but also the most reliable.
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_RANK_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_RANK_HPP

#include <algorithm>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/eps.hpp>
#include <boost/numeric/ublasx/operation/max.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/operation/svd.hpp>
#include <boost/numeric/ublasx/operation/which.hpp>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

/**
 * \brief Estimate the rank as the number of singular values of \a A that are
 *  greater than a given tolerance.
 * \tparam MatrixExprT The type of the input matrix expression.
 * \tparam RealT The floating-point type of the tolerance.
 * \param A The input matrix expression.
 * \param tol The tolerance.
 * \return The number of singular values of \a A that are greater than \a tol.
 */
template <typename MatrixExprT, typename RealT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::size_type rank(matrix_expression<MatrixExprT> const& A, RealT tol)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type real_type;

	vector<real_type> s = svd_values(A);
	return size(which(s, ::std::bind2nd(::std::greater<real_type>(), tol)));
}

/**
 * \brief Estimate the rank as the number of singular values of \a A that are
 *  greater than the default tolerance.
 * \tparam MatrixExprT The type of the input matrix expression.
 * \param A The input matrix expression.
 * \return The number of singular values of \a A that are greater than \a tol.
 *
 * The default tolerance is
 * \f[
 * 	 \max(n,m) \|A\|_2 {\epsilon}_m
 * \f]
 * where \f$n\f$ is the number of rows of \f$A\f$, \f$m\f$ is the number of
 * columns of \f$A\f$, and \f${\epsilon}_m\f$ is the floating-point machine
 * precision.
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::size_type rank(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type real_type;

	vector<real_type> s = svd_values(A);
	real_type tol = ::std::max(num_rows(A), num_columns(A))*eps(max(s)); // note: max(s) == norm_2(A)
	return size(which(s, ::std::bind2nd(::std::greater<real_type>(), tol)));
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_RANK_HPP


/*
This program is free software; you can redistribute it and/or modify it under
the terms of the European Union Public Licence - EUPL v.1.1 as published by
the European Commission.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
for more details.

You should have received a copy of the European Union Public Licence - EUPL v.1.1
along with this program.

Further information about the European Union Public Licence - EUPL v.1.1 can
also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
STA uses interpolators based on the State Vector structures defined in "Astro-Core/statevector.h"
file. Vector and matrices operations are performed using the Eigen library.

Given a set of data points (x1 , y1 ) . . . (xn , yn ), the STA interpolators compute a
continuous interpolating function y(x) such that y(xi ) = yi

The interpolation is piecewise smooth, and its behavior at the end-points is determined by the
type of interpolation used.

*/

/*
 ------------------ Author: Guillermo Ortega ESA  -------------------------------------------
 */

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <Astro-Core/statevector.h>

#include "Interpolators.h"

using namespace sta;
using namespace Eigen;

/*
 Linear interpolation between two state vectors x0 and x1.
interval is the length of time (in seconds) between the two state vectors.
t is a value between 0 and 1 that specifies the time within the interval.

If the two known points are given by the coordinates (x_0,y_0)
and (x_1,y_1), the linear interpolant is the straight line
between these points. For a value x in the interval (x_0, x_1),
the value y along the straight line is given from the equation

    \frac{y - y_0}{x - x_0} = \frac{y_1 - y_0}{x_1 - x_0}

Solving this equation for y, which is the unknown value at x, gives

    y = y_0 + (x-x_0)\frac{y_1 - y_0}{x_1-x_0}

which is the formula for linear interpolation in the interval
(x_0,x_1). Outside this interval, the formula is identical
to linear extrapolation.

*/
sta::StateVector linearInterpolate(const sta::StateVector& x0,    // First state vector
                                         const sta::StateVector& x1,     // Second state vector
                                         double t,                       // Time within x0 an x1
                                         double interval)                // Time distance between x1 and x1
{
    sta::StateVector result;
    result.position = x0.position + t * ((x1.position - x0.position) * (1.0 / interval));
    result.velocity = x0.velocity + t * ((x1.velocity - x0.velocity) * (1.0 / interval));
    return result;
}

// Routine programmed C. Laurel
// Cubic interpolation between two state vectors v0 and v1.
// Interval is the length of time (in seconds) between the two state vectors.
// t is a value between 0 and 1 that specifies the time within the interval.
sta::StateVector cubicInterpolate(const sta::StateVector& v0,
                                         const sta::StateVector& v1,
                                         double t,
                                         double interval)
{
    double t2 = t * t;
    double t3 = t2 * t;
    Vector3d a = 2.0 * (v0.position - v1.position) + interval * (v1.velocity + v0.velocity);
    Vector3d b = 3.0 * (v1.position - v0.position) - interval * (2.0 * v0.velocity + v1.velocity);
    Vector3d c = v0.velocity * interval;
    Vector3d d = v0.position;

    sta::StateVector result;
    result.position = a * t3 + b * t2 + c * t + d;
    result.velocity = (a * (3.0 * t2) + b * (2.0 * t) + c) * (1.0 / interval);

    return result;
}


#include <iostream>
#include <boost/math/constants/constants.hpp> // pi
#include <static_math/complex.h> // constexpr complex class
#include "static_poly_io.hpp"

using std::cout;

/*** Greatest common divisor ***/
constexpr int gcd(int m, int n) {
   while (n != 0) {
      m %= n;
      if (m == 0) return n;
      n %= m;
    }
    return m;
}

/*** The Nth cyclotomic polynomial, computed with complex numbers ***/
template <int N>
constexpr static_poly<smath::complex<double>, N+1> cyclotomic() {
    static_assert(N > 0, "No nonpositive numbers please!");
    using namespace boost::math::double_constants;

    static_poly<smath::complex<double>, N+1> cyc{1};

    for (int k = 1; k <= N; ++k) {
        if (gcd(k, N) == 1) {
            static_poly<smath::complex<double>, 2> x{-smath::polar(1., 2.*k*pi / N), 1};
            cyc = detail::mul(cyc, x);
        // alas, std::polar(1., 2.*k*pi / N) is not constexpr
        // furthermore, std::cos/std::sin are not constexpr (why!?)
        }
    }
    return cyc;
}

/*** Euler's totient function... ***/
template <int N>
constexpr int euler_totient() {
    return cyclotomic<N>().degree();
}

template <int N>
struct foo {
    constexpr int val() const {
        return N;
    }
};

int main() {
    constexpr auto phi1 = cyclotomic<1>();
    constexpr auto phi2 = cyclotomic<2>();
    constexpr auto phi3 = cyclotomic<3>();
    constexpr auto phi4 = cyclotomic<4>();
    constexpr auto phi5 = cyclotomic<5>();
    constexpr auto phi6 = cyclotomic<6>();
    constexpr auto phi7 = cyclotomic<7>();
    constexpr auto phi8 = cyclotomic<8>();
    constexpr auto phi9 = cyclotomic<9>();
    constexpr auto phi35 = cyclotomic<35>();

    cout << phi1 << '\n'
         << phi2 << '\n'
         << phi3 << '\n'
         << phi4 << '\n'
         << phi5 << '\n'
         << phi6 << '\n'
         << phi7 << '\n'
         << phi8 << '\n'
         << phi9 << "\n\n"
         << phi1*phi2 << '\n'
         << phi1*phi3 << '\n'
         << phi1*phi2*phi4 << '\n'
         << phi1*phi5 << '\n'
         << phi1*phi2*phi3*phi6 << '\n'
         << phi1*phi7 << '\n'
         << phi1*phi2*phi4*phi8 << '\n'
         << phi1*phi3*phi9 << "\n\n"
         << phi35 << '\n';

    /* Use euler's totient function as a template parameter! */
    foo<euler_totient<76>()> myfoo;
    cout << myfoo.val() << '\n';

    return 0;
}


#include "RSA.h"
#include "math/Euclidean.h"
#include "Encryptor.h"
#include "Decryptor.h"
#include <exception>
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;
using namespace Crypto;

RSA::RSA(int256_t p, int256_t q, int256_t r) : p(0), q(0), m(0), phi_of_m(0), r(0), public_key(nullptr), private_key(nullptr)
{
  try
  {
    setParameters(p, q, r);
    calcKeys();
  }
  catch (std::exception &e)
  {
    //std::cout << e.what();
    throw; // Rethrow exception.
  }
}

void RSA::setParameters(int256_t p, int256_t q, int256_t r)
{
  this->p = p;
  this->q = q;
  this->r = r;
  this->m = p * q;
  this->phi_of_m = calcPhi(p, q);

  if (!isPrime(p) || !isPrime(q)) // p and q must be prime numbers
  {
    throw std::exception("[ERROR] p or q is not a prime number!");
  }
  else if ((r < m) && (r > 1) && (Euclidean::euclidean(r, phi_of_m) != 1)) // r and phi of m must be coprime
  {
    throw std::exception("[ERROR] r is not equal or less than p * q (=> m), or r and phi of m are not coprime!");
  }
}

void RSA::calcKeys()
{
  if (private_key != nullptr || public_key != nullptr)
  {
    throw std::exception("[ERROR] Keys already calculated!");
  }

  calcPublicKey();
  calcPrivateKey();
}

void RSA::calcPrivateKey()
{

  int256_t a = this->phi_of_m;
  int256_t b = this->r;

  int256_t s = 0;
  int256_t x = 0;

  //calculates the secret key
  Euclidean::extendedEuclidean(a, b, &x, &s);

  s = s < 0 ? makePositive(s, this->phi_of_m) : s;

  this->private_key = new PrivateKey{ s, this->p, this->q };
}

void RSA::calcPublicKey()
{
  this->public_key = new PublicKey{ this->r, this->m };
}

const PublicKey* RSA::getPublicKey() const
{
  return public_key;
}

const PrivateKey* RSA::getPrivateKey() const
{
  return private_key;
}

bool RSA::isPrime(int256_t numb)
{
  int it;
  for (it = 2; it < numb; it++)
  {
    if ((numb % it) == 0)
      return false;
  }

  return true;
}

int256_t RSA::calcPhi(int256_t a, int256_t b)
{
  if (!isPrime(a) || !isPrime(b))
  {
    return 0;
  }

  return (a - 1) * (b - 1);
}

int256_t RSA::makePositive(int256_t numb, int256_t mod) const
{
  int256_t tmp = numb;
  while (tmp < 0)
  {
    tmp += mod;
  }

  return tmp;
}

CryptoString RSA::encrypt(string str)
{
  Encryptor enc(public_key);
  CryptoString out = enc.encryptString(str);
  return out;
}

string RSA::decrypt(CryptoString str)
{
  Crypto::Decryptor dec(private_key);
  string res = dec.decryptString(str);
  return res;
}

CryptoChar RSA::encrypt(char ch)
{
  Encryptor enc(public_key);
  CryptoChar out = enc.encryptChar(ch);
  return out;
}

char RSA::decrypt(CryptoChar ch)
{
  Crypto::Decryptor dec(private_key);
  char res = dec.decryptChar(ch);
  return res;
}


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearfeelementmatrix.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>

namespace ElementMatrixComputation {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 4, 4> MyLinearFEElementMatrix::Eval(
    const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());
  // Matrix for returning element matrix
  Eigen::Matrix<double, 4, 4> elem_mat;

  //====================
  // Your code goes here

  // STEP 1: Case distinction depending on type of the mesh element
  if (ref_el.ToString() == "TRIA"){
      // bottom row and rightmost col are not used
      elem_mat << 2., 1., 1., 0.,
                  1., 2., 1., 0.,
                  1., 1., 2., 0.,
                  0., 0., 0., 0.;
      // multiply with size |K|
      elem_mat /= 12.;
    }
    else{
     //"QUAD":
      elem_mat << 4., 2., 1., 2.,
                  2., 4., 2., 1.,
                  1., 2., 4., 2.,
                  2., 1., 2., 4.;
      // multiply with size |K|
      elem_mat /= 36.;
  }

  // multiply with the size of the mesh element
  double K = lf::geometry::Volume(*geo_ptr);
  elem_mat *= K;

  // STEP 2: linearly add the computed -Delta u term
  lf::uscalfe::LinearFELaplaceElementMatrix emp;

  elem_mat += emp.Eval(cell);

  //====================

  return elem_mat;
}
/* SAM_LISTING_END_1 */
}  // namespace ElementMatrixComputation


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * \file register_svd.hpp
 * \brief Header for registerSVD function
 * \details Calculates rigid transformation given two sets of points. Used in
 * stereo_transform_model
 *
 * \author Kirk MacTavish, Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

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

namespace vtr {
namespace vision {

// Returns tf s.t. a = tf * b
template <unsigned int N>
bool registerSVD(const Eigen::Matrix<double, N, Eigen::Dynamic>& a,
                 const Eigen::Matrix<double, N, Eigen::Dynamic>& b,
                 Eigen::Matrix<double, N + 1, N + 1>* tf,
                 const Eigen::Array<double, 1, Eigen::Dynamic>& weights =
                     Eigen::Array<double, 1, Eigen::Dynamic>(1, 0),
                 bool scaling = false) {
  // Types
  // typedef Eigen::Matrix<double,N+1,N+1> Transform;
  typedef Eigen::Matrix<double, N, 1> Point;
  typedef Eigen::Matrix<double, N, Eigen::Dynamic> Points;
  typedef Eigen::Matrix<double, N, N> Square;

  // Check output allocation
  if (!tf) return false;

  // Are we weighting points?
  bool use_weights = weights.cols() > 0;

  // Check input (1 pt for 1D, 2 pts for 2D, 3 pts for 3D)
  if (a.cols() < N || a.cols() != b.cols() ||
      (use_weights && a.cols() != weights.cols()))
    return false;

  // Switch based on whether we're weighting the points
  // See https://igl.ethz.ch/projects/ARAP/svd_rot.pdf
  Point ca, cb;
  Points a0, b0;
  Square cov;
  if (use_weights) {
    // Weight points
    Points aw = a.array().rowwise() * weights;
    Points bw = b.array().rowwise() * weights;

    // Compute centroids
    double w_sum = weights.sum();
    ca = aw.rowwise().sum() / w_sum;
    cb = bw.rowwise().sum() / w_sum;

    // Demean
    a0 = a.colwise() - ca;
    b0 = b.colwise() - cb;

    // Covariance
    cov =
        (a0.array().rowwise() * weights).matrix() * b0.transpose();  // a0*W*b0'
  } else {
    // Compute centroids
    ca = a.rowwise().mean();
    cb = b.rowwise().mean();

    // Demean
    a0 = a.colwise() - ca;
    b0 = b.colwise() - cb;

    // Covariance
    cov = a0 * b0.transpose();
  }

  // SVD
  Eigen::JacobiSVD<Square> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Square V = svd.matrixV();
  // Normally V * U', but then we have to use R'.
  Square R = svd.matrixU() * V.transpose();

  // Check for proper right-hand rotation
  double det = R.determinant();
  if (det < 0. || use_weights) {
    // If we use the weights, we have to remove their influence here
    V.col(N - 1) *= use_weights ? det : -1.;
    // Normally V * U', but then we have to use R'.
    R = svd.matrixU() * V.transpose();
  }

  // Apply scaling if necessary
  if (scaling) {
    double scale = b0.norm() / a0.norm();
    R /= scale;
  }

  // Translation
  Point t = ca - R * cb;

  // Build final transform
  tf->row(N).setZero();
  (*tf)(N, N) = 1.;
  tf->template topLeftCorner<N, N>() = R;
  tf->template topRightCorner<N, 1>() = t;

  // Return
  return true;
}

// Explicit instantiation for common params
extern template bool registerSVD<2>(
    const Eigen::Matrix<double, 2, Eigen::Dynamic>& a,
    const Eigen::Matrix<double, 2, Eigen::Dynamic>& b,
    Eigen::Matrix<double, 2 + 1, 2 + 1>* tf,
    const Eigen::Array<double, 1, Eigen::Dynamic>& weights, bool scaling);
extern template bool registerSVD<3>(
    const Eigen::Matrix<double, 3, Eigen::Dynamic>& a,
    const Eigen::Matrix<double, 3, Eigen::Dynamic>& b,
    Eigen::Matrix<double, 3 + 1, 3 + 1>* tf,
    const Eigen::Array<double, 1, Eigen::Dynamic>& weights, bool scaling);

}  // namespace vision
}  // namespace vtr


#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>

#include <Eigen/Eigen>

#include <unsupported/Eigen/NonLinearOptimization>

#include <ros/ros.h>

struct LMFunctor
{
	// 'm' pairs of (x, f(x))
	Eigen::MatrixXf measuredValues;

	// Compute 'm' errors, one for each data point, for the given parameter values in 'x'
	int operator()(const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
	{
		// 'x' has dimensions n x 1
		// It contains the current estimates for the parameters.

		// 'fvec' has dimensions m x 1
		// It will contain the error for each data point.

		float aParam = x(0);
		float bParam = x(1);
		float cParam = x(2);

		for (int i = 0; i < values(); i++) {
			float xValue = measuredValues(i, 0);
			float yValue = measuredValues(i, 1);

			fvec(i) = yValue - (aParam * xValue * xValue + bParam * xValue + cParam);
		}
		return 0;
	}

	// Compute the jacobian of the errors
	int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
	{
		// 'x' has dimensions n x 1
		// It contains the current estimates for the parameters.

		// 'fjac' has dimensions m x n
		// It will contain the jacobian of the errors, calculated numerically in this case.

		float epsilon;
		epsilon = 1e-7f;

		for (int i = 0; i < x.size(); i++) {
			Eigen::VectorXf xPlus(x);
			xPlus(i) += epsilon;
			Eigen::VectorXf xMinus(x);
			xMinus(i) -= epsilon;

			Eigen::VectorXf fvecPlus(values());
			operator()(xPlus, fvecPlus);

			Eigen::VectorXf fvecMinus(values());
			operator()(xMinus, fvecMinus);

			Eigen::VectorXf fvecDiff(values());
			fvecDiff = (fvecPlus - fvecMinus) / (2.0f * epsilon);

			fjac.block(0, i, values(), 1) = fvecDiff;
		}

		return 0;
	}

	// Number of data points, i.e. values.
	int m;

	// Returns 'm', the number of values.
	int values() const { return m; }

	// The number of parameters, i.e. inputs.
	int n;

	// Returns 'n', the number of inputs.
	int inputs() const { return n; }

};

int main(int argc, char *argv[])
{
    // ros::init(argc, argv, "LM_node");
    // ros::start();
	// //
	// Goal
	//
	// Given a non-linear equation: f(x) = a(x^2) + b(x) + c
	// and 'm' data points (x1, f(x1)), (x2, f(x2)), ..., (xm, f(xm))
	// our goal is to estimate 'n' parameters (3 in this case: a, b, c)
	// using LM optimization.
	//

	//
	// Read values from file.
	// Each row has two numbers, for example: 5.50 223.70
	// The first number is the input value (5.50) i.e. the value of 'x'.
	// The second number is the observed output value (223.70),
	// i.e. the measured value of 'f(x)'.

	// 'm' is the number of data points.
	int m = 100;

	// Move the data into an Eigen Matrix.
	// The first column has the input values, x. The second column is the f(x) values.
    float a = 0.1;
    float b = 0.2;
    float c = 0.3;
	Eigen::MatrixXf measuredValues(m, 2);
	for (int i = 0; i < m; i++) {
		measuredValues(i, 0) = (float) rand()/RAND_MAX * 10;
		measuredValues(i, 1) = a*measuredValues(i, 0)*measuredValues(i, 0) + b*measuredValues(i, 0) + c + (float) rand()/RAND_MAX/10;
	}

	// 'n' is the number of parameters in the function.
	// f(x) = a(x^2) + b(x) + c has 3 parameters: a, b, c
	int n = 3;

	// 'x' is vector of length 'n' containing the initial values for the parameters.
	// The parameters 'x' are also referred to as the 'inputs' in the context of LM optimization.
	// The LM optimization inputs should not be confused with the x input values.
	Eigen::VectorXf x(n);
	x(0) = 0.0;             // initial value for 'a'
	x(1) = 0.0;             // initial value for 'b'
	x(2) = 0.0;             // initial value for 'c'

	//
	// Run the LM optimization
	// Create a LevenbergMarquardt object and pass it the functor.
	//

	LMFunctor functor;
	functor.measuredValues = measuredValues;
	functor.m = m;
	functor.n = n;

	Eigen::LevenbergMarquardt<LMFunctor, float> lm(functor);
	int status = lm.minimize(x);
	std::cout << "LM optimization status: " << status << std::endl;

	//
	// Results
	// The 'x' vector also contains the results of the optimization.
	//
	std::cout << "Optimization results" << std::endl;
	std::cout << "\ta: " << x(0) << std::endl;
	std::cout << "\tb: " << x(1) << std::endl;
	std::cout << "\tc: " << x(2) << std::endl;

    // ros::shutdown();
	return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Eigen>
#include <cmath>

namespace fluid {
namespace algorithm {

class SpectralShape
{

  using ArrayXd = Eigen::ArrayXd;

public:
  SpectralShape(index maxFrame) : mMagBuffer(maxFrame) {}

  void processFrame(Eigen::Ref<ArrayXd> in)
  {
    using namespace std;
    double const epsilon = std::numeric_limits<double>::epsilon();

    ArrayXd x = in.max(epsilon);
    index   size = x.size();
    double  xSum = x.sum();
    ArrayXd xSquare = x.square();
    ArrayXd lin = ArrayXd::LinSpaced(size, 0, size - 1);
    double  centroid = (x * lin).sum() / xSum;
    double  spread = (x * (lin - centroid).square()).sum() / xSum;
    double  skewness =
        (x * (lin - centroid).pow(3)).sum() / (spread * sqrt(spread) * xSum);
    double kurtosis =
        (x * (lin - centroid).pow(4)).sum() / (spread * spread * xSum);
    double flatness = exp(x.log().mean()) / x.mean();
    double rolloff = size - 1;
    double cumSum = 0;
    double target = 0.95 * xSquare.sum();
    for (index i = 0; cumSum <= target && i < size; i++)
    {
      cumSum += xSquare(i);
      if (cumSum > target)
      {
        rolloff = i - (cumSum - target) / xSquare(i);
        break;
      }
    }
    double crest = x.maxCoeff() / sqrt(x.square().mean());

    mOutputBuffer(0) = centroid;
    mOutputBuffer(1) = sqrt(spread);
    mOutputBuffer(2) = skewness;
    mOutputBuffer(3) = kurtosis;
    mOutputBuffer(4) = rolloff;
    mOutputBuffer(5) = 20 * log10(max(flatness, epsilon));
    mOutputBuffer(6) = 20 * log10(max(crest, epsilon));
  }

  void processFrame(const RealVector& input, RealVectorView output)
  {
    assert(output.size() == 7); // TODO
    ArrayXd in = _impl::asEigen<Eigen::Array>(input);
    processFrame(in);
    output = _impl::asFluid(mOutputBuffer);
  }

private:
  ArrayXd mMagBuffer;
  ArrayXd mOutputBuffer{7};
};

} // namespace algorithm
} // namespace fluid


/*
 * TPSInterpolate.hpp
 * license: http://www.boost.org/LICENSE_1_0.txt
 *  Created on: 18 Aug 2010
 *      Author: Peter Stroia-Williams
 */

#ifndef TPSINTERPOLATE_HPP_
#define TPSINTERPOLATE_HPP_

#include <vector>
#include <cmath>

#include <boost/array.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

//using namespace boost::numeric::ublas;
//using namespace boost;
//using namespace std;

double
radial_basis(double r)
{
  if(r==0.0)
    return 0.0;
  return pow(r, 2) * log(r);
}

template<typename T1, typename T2, int Dim>
  double
  radialbasis(const boost::array<T1, Dim> & v1, const boost::array<T2, Dim> & v2)
  {
    boost::numeric::ublas::vector<double> v(Dim);
    for(unsigned int i(0); i < v.size(); ++i)
    {
      v(i) = v1[i] - v2[i];
    }
    return radial_basis(norm_2(v));
  }

template<int PosDim, int ValDim, typename WorkingType = double>
  class ThinPlateSpline
  {
  public:
    boost::numeric::ublas::matrix<WorkingType> Wa;
    std::vector<boost::array<WorkingType, PosDim> > refPositions;

    ThinPlateSpline()
    {}

    ThinPlateSpline(std::vector<boost::array<WorkingType, PosDim> > positions,
        std::vector<boost::array<WorkingType, ValDim> > values)
    {
      boost::numeric::ublas::matrix<WorkingType> L;

      refPositions = positions;

      const int numPoints((int)refPositions.size());
      const int WaLength(numPoints + PosDim + 1);

      L = boost::numeric::ublas::matrix<WorkingType> (WaLength, WaLength);

      // Calculate K and store in L
      for(int i(0); i < numPoints; i++)
      {
        L(i,i) = 0.0;
        // K is symmetrical so no point in calculating things twice
        int j(i + 1);
        for(; j < numPoints; ++j)
        {

          L(i,j) = L(j,i) = radialbasis<WorkingType, WorkingType, PosDim>(refPositions[i], refPositions[j]);
        }

        // construct P and store in K
        L(j,i) = L(i,j) = 1.0;
        ++j;
        for(int posElm(0); j < WaLength; ++posElm, ++j)
          L(j,i) = L(i,j) = positions[i][posElm];
      }

      // O
      for(int i(numPoints); i < WaLength; i++)
        for(int j(numPoints); j < WaLength; j++)
          L(i,j) = 0.0;

      // Solve L^-1 Y = W^T

      typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;

      boost::numeric::ublas::matrix<WorkingType> A(L);
      pmatrix pm(A.size1());
      int res = (int)lu_factorize(A, pm);
      if(res != 0)
	  {
        ;//TODO catch this error
	  }

      boost::numeric::ublas::matrix<WorkingType> invL(boost::numeric::ublas::identity_matrix<WorkingType>(A.size1()));
      lu_substitute(A, pm, invL);

      Wa = boost::numeric::ublas::matrix<WorkingType>(WaLength, ValDim );

      boost::numeric::ublas::matrix<WorkingType> Y(WaLength, ValDim);
      int i(0);
      for(; i < numPoints; i++)
        for(int j(0); j < ValDim; ++j)
          Y(i, j) = values[i][j];

      for(; i < WaLength; i++)
        for(int j(0); j < ValDim; ++j)
          Y(i, j) = 0.0;

      Wa = prod(invL,Y);

    }

    boost::array<WorkingType, ValDim>
    interpolate(const boost::array<WorkingType, PosDim> &position) const
    {
      boost::array<WorkingType, ValDim> result;
      // Init result
      for(int j(0); j < ValDim; ++j)
        result[j] = 0;

      unsigned int i(0);
      for(; i < Wa.size1() - (PosDim+1); ++i)
      {
        for(int j(0); j < ValDim; ++j)
          result[j] += Wa(i,j) * radialbasis<WorkingType, WorkingType, PosDim>(refPositions[i], position);
      }

      for(int j(0); j < ValDim; ++j)
        result[j] += Wa(i,j);
      ++i;

      for(int k(0); k < PosDim; ++k, ++i)
      {
        for(int j(0); j < ValDim; ++j)
          result[j] += Wa(i,j) * position[k];
      }
      return result;
    }

  };

#endif /* TPSINTERPOLATE_HPP_ */