#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

#include "stiffness_matrix.hpp"

//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;

//----------------AssembleMatrixBegin----------------
//! Assemble the stiffness matrix
//! for the linear system
//!
//! @param[out] A will at the end contain the Galerkin matrix
//! @param[in] vertices a list of triangle vertices
//! @param[in] dofs a list of the dofs' indices in each triangle
template <class Matrix>
void assembleStiffnessMatrix(Matrix &A, const Eigen::MatrixXd &vertices, const Eigen::MatrixXi &dofs, const int &N) {
	const int numberOfElements = dofs.rows();
	A.resize(N, N);

	std::vector<Triplet> triplets;

	triplets.reserve(numberOfElements * 6 * 10);
	// (write your solution here)
	for (int i = 0; i < numberOfElements; ++i) {
		auto &indexSet = dofs.row(i);

		const auto &a = vertices.row(indexSet(0));
		const auto &b = vertices.row(indexSet(1));
		const auto &c = vertices.row(indexSet(2));

		Eigen::MatrixXd stiffnessMatrix;
		computeStiffnessMatrix(stiffnessMatrix, a, b, c);

		for (int n = 0; n < 6; ++n) {
			for (int m = 0; m < 6; ++m) {
				auto triplet = Triplet(indexSet(n), indexSet(m), stiffnessMatrix(n, m));
				triplets.push_back(triplet);
			}
		}
	}
	A.setFromTriplets(triplets.begin(), triplets.end());
}
//----------------AssembleMatrixEnd----------------


/**
 * @file kalman_filter.h
 * @brief Kalman Filter.
 *
 */

#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H

#include <Eigen/Dense>

namespace filter
{

class KalmanFilter
{
    public:
        /**
         * @brief Create a Kalman filter.
         *
         * @param C Output matrix
         * @param Q Process noise covariance
         * @param R Measurement noise covariance
         * @param P Estimate error covariance
         */
        KalmanFilter(
            const Eigen::MatrixXd& A,
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& Q,
            const Eigen::MatrixXd& R,
            const Eigen::MatrixXd& P
        );
        ~KalmanFilter() {};

        /**
         * @brief Initialize the filter with initial states as zero.
         *
         */
        void init();

        /**
         * @brief Initialize the filter with an estimation on the initial states.
         * @param t0 initial timestamp.
         * @param x0 estimated state of the system.
         *
         */
        void init(double t0, const Eigen::VectorXd& x0);

        /**
         * @brief An adaptive Kalman filter.
         * @brief Update the estimated state based on measured values, using the given time step and dynamics matrix.
         * @param y measurements.
         * @param dt timestamp.
         *
         */
        Eigen::VectorXd compute(const Eigen::VectorXd& y, double dt);

    private:
        // Matrices for computation.
        Eigen::MatrixXd A, C, Q, R, P, K, P0;

        // Systems dimension.
        int m; // Number of measurements.
        int n; // Number of states.

        Eigen::MatrixXd I; // Identity matrix.
        Eigen::VectorXd x_hat, x_hat_new; // Estimated states.

        // Initial timer.
        double t0;

        // Flag.
        bool initialized;
};

} // namespace controller

#endif /* KALMAN_FILTER_H */


/******************************************************************************
 * 反射衛星砲: 数学モデルユーティリティ関数
 *****************************************************************************/

#ifndef __MATHEMATIC_UTILS_HPP__
#define __MATHEMATIC_UTILS_HPP__

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

typedef std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > Vector3dSet;

// 点P1,P2の間の距離の2乗を求める
double distance2(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2);

// ベクトルX を正規化したベクトルを求める
Eigen::Vector3d normalize(const Eigen::Vector3d& X);

// ベクトルX が ベクトルa を軸として p[rad]回転したときのベクトルを求める
Eigen::Vector3d rotate(const Eigen::Vector3d& X, const Eigen::Vector3d& a, const double p);

// ベクトルX が 法線ベクトルn の面に入射したときの反射ベクトルを求める
Eigen::Vector3d reflect(const Eigen::Vector3d& X, const Eigen::Vector3d& n);

// 点の集合P={p1,p2,...} のうち、点Oに最も近い点の集合(⊆P)を求める
Vector3dSet neighoring(const Eigen::Vector3d& O, const Vector3dSet& P);

// 直線L(P,v) が楕円体E(O,R)の表面を通過,接触する座標を求める
// ただし実数解のみ
Vector3dSet intersection(const Eigen::Vector3d& P, const Eigen::Vector3d& v,
                       const Eigen::Vector3d& O, const Eigen::Vector3d& R);

// 点P が楕円体E(O,R)の中にあるかどうか調べる
bool inner_elipsoid(const Eigen::Vector3d& P, const Eigen::Vector3d& O, const Eigen::Vector3d& R);

#endif // __MATHEMATIC_UTILS_HPP__


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2014 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Non-Negative Matrix Factorization                                        *
***************************************************************************/

#ifndef NNMF_HPP
#define NNMF_HPP

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <cmath>
#include <algorithm>

#define SQRTEPS 1.490116119384766e-08

// [1] C. Boutsidis, E. Gallopoulos. SVD based initialization:A head start for nonnegative matrix factorization. Pattern Recognition 41 (2008) 1350–1362

using namespace Eigen;

class NNMF
{
public:
	enum Algorithm { ALS, MUR };

	NNMF(const int maxIterations = 200, const double threshold = 1e-4, const double tolerance = 1e-4, const bool verbose = false);

	template <class Derived>
	void compute(const MatrixBase<Derived> &dataset, const int K, const Algorithm algorithm = ALS);
	template <class Derived>
	void compute(const MatrixBase<Derived> &dataset, const MatrixXd &W0, const MatrixXd &H0, const Algorithm algorithm = ALS);

	MatrixXd W() const { return m_W; }
	MatrixXd H() const { return m_H; }

private:
	template <bool AlternatingLeastSquares = true>
	void nnmf(const MatrixXd &dataset);

	template <class Derived>
	void SVDInitialization(const MatrixBase<Derived> &dataset, const int K);
	void standard_form();

	MatrixXd m_W;
	MatrixXd m_H;
	int m_components;
	int m_maxIterations;
	double m_threshold;
	double m_tolerance;
	bool m_verbose;
};

/***************************** Implementation *****************************/

NNMF::NNMF(const int maxIterations, const double threshold, const double tolerance, const bool verbose)
: m_W(MatrixXd()), m_H(MatrixXd()), m_components(0), m_maxIterations(maxIterations), m_threshold(threshold), m_tolerance(tolerance), m_verbose(verbose)
{
}

template<class Derived>
void NNMF::compute(const MatrixBase<Derived> &dataset, const int K, const Algorithm algorithm)
{
	if (K >= dataset.cols())
	{
		m_W = dataset.derived().cast<double>();
		m_H = MatrixXd::Identity(K, K);
		return;
	}

	if (K >= dataset.rows())
	{
		m_W = MatrixXd::Identity(K, K);
		m_H = dataset.derived().cast<double>();
		return;
	}

	// Set components
	m_components = K;

	// Singular Value Decomposition initialization
	SVDInitialization(dataset, K);

	// Run Non-Negative Matrix Factorization decomposition
	if (algorithm == ALS)
		nnmf<true>(dataset.derived().cast<double>());
	else if (algorithm == MUR)
		nnmf<false>(dataset.derived().cast<double>());
	else
		nnmf<true>(dataset.derived().cast<double>());
}

template<class Derived>
void NNMF::compute(const MatrixBase<Derived> &dataset, const MatrixXd &W0, const MatrixXd &H0, const Algorithm algorithm)
{
	// Set components
	m_components = W0.cols();

	// Initialize W and H matrices
	m_W = W0;
	m_H = H0;

	// Run Non-Negative Matrix Factorization decomposition
	if (algorithm == ALS)
		nnmf<true>(dataset.derived().cast<double>());
	else if (algorithm == MUR)
		nnmf<false>(dataset.derived().cast<double>());
	else
		nnmf<true>(dataset.derived().cast<double>());
}

template <bool AlternatingLeastSquares>
void NNMF::nnmf(const MatrixXd &dataset)
{
	// Set number of elements
	const double NM = (double) dataset.size();

	// Declare previous iteration matrices and error
	MatrixXd W0 = m_W;
	MatrixXd H0 = m_H;
	double dnorm0 = 0;

	if (m_verbose)
	{
		mexPrintf("Iteration\t\tMax iterations\t\tRMSE\t\t\tRatio\t\t\tThreshold\t\tDelta\t\t\tTolerance\n");
		mexPrintf("-------------------------------------------------------------------------------------------------------------\n");
	}

	// Constant matrices
	const MatrixXd H_MUR_SQRTEPS = MatrixXd::Constant(m_components, dataset.cols(), SQRTEPS);
	const MatrixXd W_MUR_SQRTEPS = MatrixXd::Constant(dataset.rows(), m_components, SQRTEPS);

	for (int i = 0; i < m_maxIterations; ++i)
	{
		if (AlternatingLeastSquares)
		{
			// Update H
			m_H = W0.fullPivHouseholderQr().solve(dataset).cwiseMax(0);
			// Update W
			m_W = m_H.transpose().fullPivHouseholderQr().solve(dataset.transpose()).transpose().cwiseMax(0);
		}
		else
		{
			// Update H
			MatrixXd numer = W0.transpose() * dataset;
			m_H = H0.cwiseProduct(numer.cwiseQuotient(((W0.transpose() * W0) * H0) + H_MUR_SQRTEPS)).cwiseMax(0);
			// Update W
			numer = dataset * m_H.transpose();
			m_W = W0.cwiseProduct(numer.cwiseQuotient((W0 * (m_H * m_H)) + W_MUR_SQRTEPS)).cwiseMax(0);
		}

		// Compute squared error
		const ArrayXXd d = dataset - (m_W * m_H);
		const double dnorm = std::sqrt((d * d).sum() / NM);
		const double dW = ((m_W - W0).cwiseAbs() / (SQRTEPS + W0.cwiseAbs().maxCoeff())).maxCoeff();
		const double dH = ((m_H - H0).cwiseAbs() / (SQRTEPS + H0.cwiseAbs().maxCoeff())).maxCoeff();
		const double delta = std::max(dW, dH);

		if (m_verbose)
		{
			mexPrintf("%d\t\t\t\t%d\t\t\t\t\t%.2e\t\t%.2e\t\t%.2e\t\t%.2e\t\t%.2e\n", i+1, m_maxIterations, dnorm, (dnorm0 - dnorm), (m_threshold * std::max(1.0, dnorm0)), delta, m_tolerance);
			mexEvalString("drawnow");
		}

		if (i > 0)
		{
			if (delta <= m_tolerance)
			{
				standard_form();
				return;
			}
			if (dnorm0 - dnorm <= (m_threshold * std::max(1.0, dnorm0)))
			{
				standard_form();
				return;
			}
		}

		dnorm0 = dnorm;
		W0 = m_W;
		H0 = m_H;
	}
}

template <class Derived>
void NNMF::SVDInitialization(const MatrixBase<Derived> &dataset, const int K)
{
	// Allocate memory
	m_W.resize(dataset.rows(), K);
	m_H.resize(K, dataset.cols());

	// Perform SVD decomposition
	JacobiSVD<MatrixXd> svd(dataset, ComputeThinU | ComputeThinV);
	MatrixXd U = svd.matrixU();
	MatrixXd V = svd.matrixV();
	VectorXd S = svd.singularValues();

	// Retain only the largest K eigen values and eigen vectors
	S = S.segment(0, K).eval();
	U = U.block(0, 0, U.rows(), K).eval();
	V = V.block(0, 0, V.rows(), K).eval();

	// Perform initialization as suggested in [1]
	m_W.col(0) = std::sqrt(S(0)) * U.col(0);
	m_H.row(0) = std::sqrt(S(0)) * V.col(0).transpose();

	for (int i = 1; i < K; ++i)
	{
		const VectorXd Uip = U.col(i).cwiseMax(0);
		const VectorXd Uin = U.col(i).cwiseMin(0).cwiseAbs();

		const VectorXd Vip = V.col(i).cwiseMax(0);
		const VectorXd Vin = V.col(i).cwiseMin(0).cwiseAbs();

		const double UipNorm = Uip.norm();
		const double VipNorm = Vip.norm();

		const double UinNorm = Uin.norm();
		const double VinNorm = Vin.norm();

		const double mp = UipNorm * VipNorm;
		const double mn = UinNorm * VinNorm;

		VectorXd u;
		VectorXd v;
		double sigma;
		if (mp > mn)
		{
			u = Uip / UipNorm;
			v = Vip / VipNorm;
			sigma = mp;
		}
		else
		{
			u = Uin / UinNorm;
			v = Vin / VinNorm;
			sigma = mn;
		}
		m_W.col(i) = std::sqrt(S(i) * sigma) * u;
		m_H.row(i) = std::sqrt(S(i) * sigma) * v.transpose();
	}
}

void NNMF::standard_form()
{
	// Put in standard form
	RowVectorXd Hlen = m_H.cwiseProduct(m_H).rowwise().sum().cwiseSqrt();

	for (int i = 0; i < Hlen.size(); ++i)
		Hlen(i) = Hlen(i) == 0 ? 1.0 : Hlen(i);

	for (int i = 0; i < m_W.rows(); ++i)
		m_W.row(i) = m_W.row(i).cwiseProduct(Hlen);

	for (int i = 0; i < m_H.cols(); ++i)
		m_H.col(i) = m_H.col(i).cwiseQuotient(Hlen.transpose());

	const RowVectorXd magnitude = m_W.cwiseProduct(m_W).colwise().sum();
	std::vector<int> y(magnitude.size());
    int n = 0;
    std::generate(std::begin(y), std::end(y), [&] { return n++; });
	std::sort(std::begin(y), std::end(y), [&](int i1, int i2) { return magnitude(i1) > magnitude(i2); });

	const MatrixXd Wcopy = m_W;
	const MatrixXd Hcopy = m_H;
	for (int i = 0; i < magnitude.size(); ++i)
	{
		m_W.col(i) = Wcopy.col(y[i]);
		m_H.row(i) = Hcopy.row(y[i]);
	}
}

#endif


/**
 * @file odesolve.cc
 * @brief NPDE homework ODESolve code
 * @author ?, Philippe Peter
 * @date 18.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "odesolve.h"

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

#include "../../../lecturecodes/helperfiles/polyfit.h"

namespace ODESolve {

/* SAM_LISTING_BEGIN_2 */
double TestCvpExtrapolatedEuler() {
  double conv_rate;
  double T = 1.0;
  double y0 = 0.0;
  auto f = [](double y) -> double { return 1.0 + y * y; };

  auto Psi = [&f](double h, double y0) -> double { return y0 + h * f(y0); };
  unsigned p = 1;

  // lambda corresponding to \tilde{\psi}
  auto Psi_tilde = [&Psi, &f, &p](double h, double y0) -> double {
    return PsiTilde(Psi, p, h, y0);
  };

  // exact value
  double y_ex1 = std::tan(T);

  // values for convergence study
  Eigen::ArrayXd err(11);
  Eigen::ArrayXd M(11);

  std::cout << "Error table for equidistant steps:" << std::endl;
  std::cout << "M"
            << "\t"
            << "Error" << std::endl;

  for (int i = 0; i < 11; ++i) {
    M(i) = std::pow(2, i + 2);
    double yT = OdeIntEqui(Psi_tilde, T, y0, M(i)).back();
    err(i) = std::abs(yT - y_ex1);
    std::cout << M(i) << "\t" << err(i) << std::endl;
  }
  // compute fitted rate
  Eigen::VectorXd coeffs = polyfit(M.log(), err.log(), 1);
  conv_rate = -coeffs(0);
  return conv_rate;
}

/* SAM_LISTING_BEGIN_4 */
std::pair<std::vector<double>, std::vector<double>> SolveTangentIVP() {
  auto f = [](double y) -> double { return 1.0 + y * y; };
  double y0 = 0.0;

  double T = 1.5;
  unsigned p = 1;
  double h0 = 1. / 100.;

  auto Psi = [&f](double h, double y0) -> double { return y0 + h * f(y0); };
  // run adaptive algoritm
  return OdeIntSsCtrl(Psi, p, y0, T, h0, 10e-4, 10e-6, 10e-5);

}
/* SAM_LISTING_END_4 */

}  // namespace ODESolve


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

using namespace std;
using namespace Eigen;

#define PRINT_MAT(X) cout << #X << ":\n" << X << endl

void Example1(void)
{ /*
  This Function I almost copied from following link.
  https://gist.github.com/AtsushiSakai/5175898#file-geometrysample-cpp-L23
  And here is the copy right of this function.
  FileName: GeometrySample.cpp Line 23 to Line 37
  Discription: EigenのGeometry関連の関数のサンプル
  Author: Atsushi Sakai
  Update: 2013/03/16
  */

    cout<<"====1. 2次元空間における回転行列を使用した点の回転====="<<endl;
  //変換したい点を作成
  VectorXf point_in(2), point_out(2);
  point_in<<1,1;
  PRINT_MAT(point_in);

  //回転行列の作成
  Matrix2f rot;
  rot=Rotation2Df(M_PI);  //90度反時計回りに回転
  PRINT_MAT(rot);

  //回転行列をかけて回転
  point_out=rot*point_in;
  PRINT_MAT(point_out);
}

void Example2(void)
{
  cout << "====2. 2次元空間における同時変換行列の作成===== "<< endl;
  // 同時変換行列
  Affine2f homogeneous;
  // 回転行列と並進ベクトル
  Rotation2Df rot = Rotation2Df(M_PI/6.);
  Translation2f trans(0.5, 4);

  // 同時変換行列を生成
  homogeneous = trans * rot;

  cout << "homogeneous.rotation():" << endl;
  cout << homogeneous.rotation() << endl;
  cout << "homogeneous.translation():" << endl;
  cout << homogeneous.translation() << endl;
}

void Example3(void)
{
  cout << "====3. 2次元ベクトルにおける内積と外積=====" << endl;
  Vector2d vec1(2,3);
  Vector2d vec2(4,5);
  Matrix2d m;
  m << vec1, vec2;
  auto dot = vec1.dot(vec2);
  auto cross = m.determinant();

  cout << "vec1" << endl;
  cout << vec1.transpose() << endl;
  cout << "vec2" << endl;
  cout << vec2.transpose() << endl;
  cout << "dot product" << endl;
  cout << dot << endl;
  cout << "cross product" << endl;
  cout << cross << endl;
}

void Example4(void)
{
  Matrix<double,4,4> m;// = MatrixXd::Ones(4,4);

  m <<
    1,2,3,4,
    5,6,7,8,
    9,10,11,12,
    13,14,15,16;

  cout << m << endl;

  Vector2d v1 = m.block(0,3,2,1);
  Vector2d v2 = m.block(2,0,2,1);

  cout << typeid(v1).name() << endl;

  cout << v1.transpose() << endl;
  cout << v2.transpose() << endl;
  cout << (v1 + v2).transpose() << endl;
  cout << v1.lpNorm<2>() << endl;

  cout << ((v1+v2)/2.).transpose() << endl;
  cout << ((v1+v2)/2.).lpNorm<1>() << endl;
}

class cltst{
public:
  void b(const double& a)
  { b_ = a; };

  void c(const vector<double>& c)
  { c_ = c; };

  double b(void) const
  { return b_; };

  vector<double> c(void) const
  { return c_; };

  vector<double>& c(void)
  { return c_; };

private:
  double b_;
  vector<double> c_;
};

void test(void)
{
  cltst t;
  double a = 3;
  vector<double> b{4,5,6};

  t.b(a);
  t.c(b);
  cout << t.b() << endl;
  for(size_t i=0; i<t.c().size(); i++)
    cout << t.c()[i] << ", ";
  cout << endl;

  a = 4;
  b = {7,8,9};
  cout << t.b() << endl;
  for(size_t i=0; i<t.c().size(); i++)
    cout << t.c()[i] << ", ";
  cout << endl;

  vector<double>& u = t.c();
  for(size_t i=0; i<u.size(); i++)
    cout << u[i] << ", ";
  cout << endl;
  u = {7,8,9};
  for(size_t i=0; i<t.c().size(); i++)
    cout << t.c()[i] << ", ";
  cout << endl;
}

void func(const vector<double>& a)
{
  for(auto i=a.begin(); i!=a.end(); i++)
    cout << *i << ", ";
  cout << endl;
}

void test2(void)
{
  func(vector<double>{2,3,4});
}

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

  // Example1();
  // Example2();
  // Example3();
  Example4();
  // test();
  test2();

  return 0;
}


#ifndef HELP_FUNCTIONS_HPP
#define HELP_FUNCTIONS_HPP
#include <Eigen/Sparse>
#include <vector>
#include <QImage>
typedef Eigen::SparseMatrix<double> SpMat; // declares a column-major sparse matrix type of double
typedef Eigen::Triplet<double> T;

void insertCoefficient(int id, int i, int j, double w, std::vector<T> &coeffs,
                       Eigen::VectorXd &b, const Eigen::VectorXd &boundary)
{
        int n = int(boundary.size());
        int id1 = i + j * n;
        if (i == -1 || i == n)
                b(id) -= w * boundary(j); // constrained coefficient
        else if (j == -1 || j == n)
                b(id) -= w * boundary(i); // constrained coefficient
        else
                coeffs.push_back(T(id, id1, w)); // unknown coefficient
}
void buildProblem(std::vector<T> &coefficients, Eigen::VectorXd &b, int n)
{
        b.setZero();
        Eigen::ArrayXd boundary = Eigen::ArrayXd::LinSpaced(n, 0, M_PI).sin().pow(2);
        for (int j = 0; j < n; ++j)
        {
                for (int i = 0; i < n; ++i)
                {
                        int id = i + j * n;
                        insertCoefficient(id, i - 1, j, -1, coefficients, b, boundary);
                        insertCoefficient(id, i + 1, j, -1, coefficients, b, boundary);
                        insertCoefficient(id, i, j - 1, -1, coefficients, b, boundary);
                        insertCoefficient(id, i, j + 1, -1, coefficients, b, boundary);
                        insertCoefficient(id, i, j, 4, coefficients, b, boundary);
                }
        }
}
void saveAsBitmap(const Eigen::VectorXd &x, int n, const char *filename)
{
        Eigen::Array<unsigned char, Eigen::Dynamic, Eigen::Dynamic> bits = (x * 255).cast<unsigned char>();
        QImage img(bits.data(), n, n, QImage::Format_Indexed8);
        img.setColorCount(256);
        for (int i = 0; i < 256; i++)
                img.setColor(i, qRgb(i, i, i));
        img.save(filename);
}

#endif


// Kernel PCA using the Eigen library, by Tim Nugent 2014

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

using namespace Eigen;
using namespace std;

class PCA{

public:
	PCA() : components(2), kernel_type(1), normalise(0), gamma(0.001), constant(1.0), order(2.0) {}
	explicit PCA(MatrixXd& d) : components(2), kernel_type(1), normalise(0), gamma(0.001), constant(1.0), order(2.0) {X = d;}
	void load_data(const char* data, char sep = ',');
	void set_components(const int i){components = i;};
	void set_kernel(const int i){kernel_type = i;};
	void set_normalise(const int i){normalise = i;};
	void set_gamma(const double i){gamma = i;};
	void set_constant(const double i){constant = i;};
	void set_order(const double i){order = i;};
	MatrixXd& get_transformed(){return transformed;}
	void run_pca();
	void run_kpca();
	void print();
	void write_transformed(string);
	void write_eigenvectors(string);
private:
	double kernel(const VectorXd& a, const VectorXd& b);
	MatrixXd X, Xcentered, C, K, eigenvectors, transformed;
	VectorXd eigenvalues, cumulative;
	unsigned int components, kernel_type, normalise;
	double gamma, constant, order;

};

void PCA::load_data(const char* data, char sep){

	// Read data
	unsigned int row = 0;
	ifstream file(data);
	if(file.is_open()){
		string line,token;
		while(getline(file, line)){
			stringstream tmp(line);
			unsigned int col = 0;
			while(getline(tmp, token, sep)){
				if(X.rows() < row+1){
					X.conservativeResize(row+1,X.cols());
				}
				if(X.cols() < col+1){
					X.conservativeResize(X.rows(),col+1);
				}
				X(row,col) = atof(token.c_str());
				col++;
			}
			row++;
		}
		file.close();
		Xcentered.resize(X.rows(),X.cols());
	}else{
		cout << "Failed to read file " << data << endl;
	}

}

double PCA::kernel(const VectorXd& a, const VectorXd& b){

	/*
		Kernels
		1 = RBF
		2 = Polynomial
		TODO - add some of these these:
		http://crsouza.blogspot.co.uk/2010/03/kernel-functions-for-machine-learning.html

	*/
	switch(kernel_type){
	    case 2  :
	    	return(pow(a.dot(b)+constant,order));
	    default :
	    	return(exp(-gamma*((a-b).squaredNorm())));
	}

}

void PCA::run_kpca(){

	// Fill kernel matrix
	K.resize(X.rows(),X.rows());
	for(unsigned int i = 0; i < X.rows(); i++){
		for(unsigned int j = i; j < X.rows(); j++){
			K(i,j) = K(j,i) = kernel(X.row(i),X.row(j));
			//printf("k(%i,%i) = %f\n",i,j,K(i,j));
		}
	}
	//cout << endl << K << endl;

	EigenSolver<MatrixXd> edecomp(K);
	eigenvalues = edecomp.eigenvalues().real();
	eigenvectors = edecomp.eigenvectors().real();
	cumulative.resize(eigenvalues.rows());
	vector<pair<double,VectorXd> > eigen_pairs;
	double c = 0.0;
	for(unsigned int i = 0; i < eigenvectors.cols(); i++){
		if(normalise){
			double norm = eigenvectors.col(i).norm();
			eigenvectors.col(i) /= norm;
		}
		eigen_pairs.push_back(make_pair(eigenvalues(i),eigenvectors.col(i)));
	}
	// http://stackoverflow.com/questions/5122804/sorting-with-lambda
	sort(eigen_pairs.begin(),eigen_pairs.end(), [](const pair<double,VectorXd> a, const pair<double,VectorXd> b) -> bool {return (a.first > b.first);} );
	for(unsigned int i = 0; i < eigen_pairs.size(); i++){
		eigenvalues(i) = eigen_pairs[i].first;
		c += eigenvalues(i);
		cumulative(i) = c;
		eigenvectors.col(i) = eigen_pairs[i].second;
	}
	transformed.resize(X.rows(),components);

	for(unsigned int i = 0; i < X.rows(); i++){
		for(unsigned int j = 0; j < components; j++){
			for (int k = 0; k < K.rows(); k++){
                transformed(i,j) += K(i,k) * eigenvectors(k,j);
		 	}
		}
	}

	/*
	cout << "Input data:" << endl << X << endl << endl;
	cout << "Centered data:"<< endl << Xcentered << endl << endl;
	cout << "Centered kernel matrix:" << endl << Kcentered << endl << endl;
	cout << "Eigenvalues:" << endl << eigenvalues << endl << endl;
	cout << "Eigenvectors:" << endl << eigenvectors << endl << endl;
	*/
	cout << "Sorted eigenvalues:" << endl;
	for(unsigned int i = 0; i < eigenvalues.rows(); i++){
		if(eigenvalues(i) > 0){
			cout << "PC " << i+1 << ": Eigenvalue: " << eigenvalues(i);
			printf("\t(%3.3f of variance, cumulative =  %3.3f)\n",eigenvalues(i)/eigenvalues.sum(),cumulative(i)/eigenvalues.sum());
		}
	}
	cout << endl;
	//cout << "Sorted eigenvectors:" << endl << eigenvectors << endl << endl;
	//cout << "Transformed data:" << endl << transformed << endl << endl;
}

void PCA::run_pca(){

	// http://stackoverflow.com/questions/15138634/eigen-is-there-an-inbuilt-way-to-calculate-sample-covariance
	Xcentered = X.rowwise() - X.colwise().mean();
	C = (Xcentered.adjoint() * Xcentered) / double(X.rows());
	EigenSolver<MatrixXd> edecomp(C);
	eigenvalues = edecomp.eigenvalues().real();
	eigenvectors = edecomp.eigenvectors().real();
	cumulative.resize(eigenvalues.rows());
	vector<pair<double,VectorXd> > eigen_pairs;
	double c = 0.0;
	for(unsigned int i = 0; i < eigenvectors.cols(); i++){
		if(normalise){
			double norm = eigenvectors.col(i).norm();
			eigenvectors.col(i) /= norm;
		}
		eigen_pairs.push_back(make_pair(eigenvalues(i),eigenvectors.col(i)));
	}
	// http://stackoverflow.com/questions/5122804/sorting-with-lambda
	sort(eigen_pairs.begin(),eigen_pairs.end(), [](const pair<double,VectorXd> a, const pair<double,VectorXd> b) -> bool {return (a.first > b.first);} );
	for(unsigned int i = 0; i < eigen_pairs.size(); i++){
		eigenvalues(i) = eigen_pairs[i].first;
		c += eigenvalues(i);
		cumulative(i) = c;
		eigenvectors.col(i) = eigen_pairs[i].second;
	}
	transformed = Xcentered * eigenvectors;

}

void PCA::print(){

	cout << "Input data:" << endl << X << endl << endl;
	cout << "Centered data:"<< endl << Xcentered << endl << endl;
	cout << "Covariance matrix:" << endl << C << endl << endl;
	cout << "Eigenvalues:" << endl << eigenvalues << endl << endl;
	cout << "Eigenvectors:" << endl << eigenvectors << endl << endl;
	cout << "Sorted eigenvalues:" << endl;
	for(unsigned int i = 0; i < eigenvalues.rows(); i++){
		if(eigenvalues(i) > 0){
			cout << "PC " << i+1 << ": Eigenvalue: " << eigenvalues(i);
			printf("\t(%3.3f of variance, cumulative =  %3.3f)\n",eigenvalues(i)/eigenvalues.sum(),cumulative(i)/eigenvalues.sum());
		}
	}
	cout << endl;
	cout << "Sorted eigenvectors:" << endl << eigenvectors << endl << endl;
	cout << "Transformed data:" << endl << X * eigenvectors << endl << endl;
	//cout << "Transformed centred data:" << endl << transformed << endl << endl;

}

void PCA::write_transformed(string file){

	ofstream outfile(file);
	for(unsigned int i = 0; i < transformed.rows(); i++){
		for(unsigned int j = 0; j < transformed.cols(); j++){
			outfile << transformed(i,j);
			if(j != transformed.cols()-1) outfile << ",";
		}
		outfile << endl;
	}
	outfile.close();
	cout << "Written file " << file << endl;

}

void PCA::write_eigenvectors(string file){

	ofstream outfile(file);
	for(unsigned int i = 0; i < eigenvectors.rows(); i++){
		for(unsigned int j = 0; j < eigenvectors.cols(); j++){
			outfile << eigenvectors(i,j);
			if(j != eigenvectors.cols()-1) outfile << ",";
		}
		outfile << endl;
	}
	outfile.close();
	cout << "Written file " << file << endl;

}

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

	/*
	if(argc < 2){
		cout << "Usage:\n" << argv[0] << " <DATA>" << endl;
		cout << "File format:\nX1,X2, ... Xn\n";
		return(0);
	}
	*/

	PCA* P = new PCA();
	P->load_data("data/test.data");
	P->run_pca();
	cout << "Regular PCA (data/test.data):" << endl;
	P->run_pca();
	P->print();
	delete P;

	P = new PCA();
	P->load_data("data/wikipedia.data");
	cout << "Kernel PCA (data/wikipedia.data) - RBF kernel, gamma = 0.001:" << endl;
	P->run_kpca();
	P->write_eigenvectors("data/eigenvectors_RBF_data.csv");
	P->write_transformed("data/transformed_RBF_data.csv");
	cout << endl;
	delete P;

	P = new PCA();
	P->load_data("data/wikipedia.data");
	P->set_kernel(2);
	P->set_constant(1);
	P->set_order(2);
	cout << "Kernel PCA (data/wikipedia.data) - Polynomial kernel, order = 2, constant = 1:" << endl;
	P->run_kpca();
	P->write_eigenvectors("data/eigenvectors_Polynomial_data.csv");
	P->write_transformed("data/transformed_Polynomial_data.csv");
	cout << endl;
	delete P;

	return(0);

}


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

class NatCSI{
public:
	NatCSI(const std::vector<double> & t, const std::vector<double> & y):
	t_(t),
	y_(y),
	h(t.size()-1),
	c(t.size()) {
		assert ((t_.size() == y_.size()) && "size missmatch (t & y)");
		m=t.size();//n+1
		for (int i = 0; i<(m-1); i++){
			h(i)=t_[i+1]-t_[i];
			assert( ( h(i) > 0 ) && "Error: array t must be sorted!");
		}

		Eigen::SparseMatrix<double> A(m,m);
        Eigen::VectorXd b(m);

        A.reserve(3);

		A.coeffRef(0,0)=2/h(0);
		A.coeffRef(0,1)=1/h(0);
		A.coeffRef(m-1,m-1)=2/h(m-2);
		A.coeffRef(m-1,m-2)=1/h(m-2);
		double bold =(y[1]-y[0])/(h(0)*h(0));
		b(0)= 3*bold;

		for (int i=1;i<m; ++i){
			//A
			A.coeffRef(i,i-1)=1/h(i);
			A.coeffRef(i,i)=2/h(i)+2/h(i-1);
			A.coeffRef(i,i+1)=1/h(i);/// not exactly matching with the theory!
			//b
			double bnew =(y[i+1]-y[i])/(h(i)*h(i));
			b(i)= 3*(bnew+bold);
			bold=bnew;

		}
		b(m-1) =3*bold;
		A.makeCompressed();
		std::cout << A << std::endl;

		Eigen::SparseLU<Eigen::SparseMatrix<double> > lu;
		lu.compute(A);
		c = lu.solve(b);

	}

	double operator () (double x) const {
		assert( (x>=t_[0] && x<=t_[m-1]) && "Error: x is not in [t(0),t(n)]");

		auto j = ((std::lower_bound(t_.begin(), t_.end(), x)-t_.begin())-1);
		if (j==-1) j++;

		double tau = x - t_[j]/h(j);
		double tau2 = tau*tau;
		double tau3 = tau2*tau;
		return y_[j]  *  (1.-3.*tau2 + 2* tau3) +
				y_[j+1] *(3*tau2-2*tau3) +
				h(j)*c(j) *(tau-2*tau2+tau3) +
				h(j)*c(j+1)*(-tau2+tau3);
	};

private:
	std::vector<double> t_, y_;
	Eigen::VectorXd h, c;
	size_t m; //m=n+1

};

int main() {

    int n = 8, m = 100;
    std::vector<double> t;
    std::vector<double> y;
    t.resize(n);
    y.resize(n);
    for(int i = 0; i < n; ++i) {
        t[i] = (double) i / (n-1);
        y[i] = cos(t[i]);
    }

    NatCSI N(t,y);

    Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(m, 0, 1);
    for(int i = 0; i < x.size(); ++i) {
        x(i) = N(x(i));
    }
    std::cout << x << std::endl;
}

Nummerical Methods for CSE/PS8/NatCSI.cpp


#include <Eigen/Sparse>
#include <Eigen/Eigenvalues>

#include <cmath>

#include <iomanip>
#include <iostream>

#define PI  M_PI
#define PI_HALF  M_PI_2

using vector = Eigen::VectorXd;

//! \brief Golub-Welsh implementation 5.3.35
//! \param[in] n number of Gauss nodes
//! \param[out] w weights
//! \param[out] x nodes for interval [-1,1]
void golubwelsh(int n, vector & w, vector & x) {
    w.resize(n);
    x.resize(n);
    if( n == 0 ) {
        x(0) = 0;
        w(0) = 2;
    } else {
        vector b(n-1);
        Eigen::MatrixXd J = Eigen::MatrixXd::Zero(n,n);

        for(int i = 1; i < n; ++i) {
            double d = (i) / sqrt(4. * i * i - 1.);
            J(i,i-1) = d;
            J(i-1,i) = d;
        }

        Eigen::EigenSolver<Eigen::MatrixXd> eig(J);

        x = eig.eigenvalues().real();
        w = 2 * eig.eigenvectors().real().topRows<1>().cwiseProduct(eig.eigenvectors().real().topRows<1>());
    }
}

//! \brief Compute \int_a^b f(x) dx \approx \sum w_i f(x_i) (with scaling of w and x)
//! \tparam func template type for function handle f (e.g. lambda func.)
//! \param[in] f integrand
//! \param[in] w weights
//! \param[in] x nodes for interval [-1,1]
//! \param[in] a left boundary in [a,b]
//! \param[in] b right boundary in [a,b]
//! \return Approximation of integral \int_a^b f(x) dx
template <class func>
double quad(func&& f, const vector & w, const vector & x, double a, double b) {
    double I = 0;
    for(int i = 0; i < w.size(); ++i) {
        I += f( (x(i) + 1) * (b - a) / 2 + a ) * w(i);
    }
    return I * (b - a) / 2.;
}

//! \brief Compute \int_{-infty}^\infty f(x) dx using transformation x = cot(t)
//! \tparam func template type for function handle f (e.g. lambda func.)
//! \param[in] n number of Gauss points
//! \param[in] f integrand
//! \return Approximation of integral \int_{-infty}^\infty f(x) dx
template <class func>
double quadinf(int n, func&& f) {
    vector w, x;
    golubwelsh(n, w, x);
    // NOTE: no function cot available in c++, need to resort to trigonometric identities
    //! Both below are valid, the first computes two trigonometric functions
    auto ftilde = [&f] (double x) { return f(cos(x)/sin(x)) / pow(sin(x),2); };
//     auto ftilde = [&f] (double x) { double cot = tan(PI_HALF - x); return f(cot) * (1. + pow(cot,2)); };
    return quad(ftilde, w, x, 0, PI);
}

int main() {
    // Number of max Gauss pts.
    double N = 100;

    // Integrand and exact integral
    auto f = [] (double t) { return exp(-pow((t-1),2)); };
    double I_ex = sqrt(PI);

    // NOTE: We observe exponential convergence
    int sep = 12;
    std::cout << std::setw(sep) << "Nodes" << std::setw(sep) << "Quadrature" << std::setw(sep) << "Exact" << std::setw(sep) << "Error" << std::endl;
    for(int n = 1; n < N; ++n) {
        vector w, x;
        double QS = quadinf(n, f);
//         std::cout << std::setw(sep) << n << std::setw(sep) << QS << std::setw(sep) << I_ex << std::setw(sep) << std::abs(QS - I_ex) << std::endl;
        std::cout << std::setw(sep) << " " << std::abs(QS - I_ex) << std::endl;
    }
}

Nummerical Methods for CSE/solutions/solutions_ps10/quadinf.cpp


#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    double ar = 1.0, br = 2.0, cr = 1.0;
    double ae = 2.0, be = -1.0, ce = 5.0;
    int N = 100;
    double w_sigma = 1.0;
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng;  // Opencv随机数产生器

    // 生成数据
    vector<double> x_data, y_data;
    for (int i=0; i<N; i++)
    {
        double x = i / 100.0;
        x_data.push_back(x);
        double y = exp(ar*x*x + br*x + cr) + rng.gaussian(w_sigma*w_sigma);
        y_data.push_back(y);
    }

    // 开始Gauss-Newton迭代
    int iter;
    double cost=0, lastCost = 0;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    while(1)
    {
        Matrix3d H = Matrix3d::Zero();
        Vector3d b = Vector3d::Zero();
        cost = 0;

        for (int i=0; i<N; i++)
        {
            double x = x_data[i];
            double y = y_data[i];

            double error = y - exp(ae*x*x + be*x + ce);

            double de_da = - x*x*exp(ae*x*x + be*x +ce);
            double de_db = - x*exp(ae*x*x + be*x +ce);
            double de_dc = - exp(ae*x*x + be*x +ce);

            Vector3d J;
            J[0] = de_da;
            J[1] = de_db;
            J[2] = de_dc;

            H = H + J * J.transpose();
            b = b - J * error;

            cost = cost + error*error;
        }

        // 求解线性方程 Hx=b
        Vector3d dx= H.ldlt().solve(b);
        if(isnan(dx[0]))
        {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost)
        {
            cout << "cost >= lastCost, break!" << endl;
            break;
        }

        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        cout << "iter = " << iter << ", total cost = " << cost << ", \t\t update = " << dx.transpose()
             << ", \t\t estimated paras = " << ae << ", " << be << ", " << ce << endl;

        if (fabs(cost - lastCost) < 1e-8)
        {
            break;
        }

        lastCost = cost;
        iter += 1;
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2-t1);

    cout << endl << "Estimated paras a, b, c = " << ae << ", " << be << ", " << ce << endl;
    cout << endl << "Solve time cost = " << time_used.count() << "s" << endl;

    return 0;
}


// https://projecteuler.net/problem=56
/*
Powerful digit sum

A googol (10^100) is a massive number:
one followed by one-hundred zeros;

100^100 is almost unimaginably large:
one followed by two-hundred zeros.

Despite their size, the sum of the digits in each number is only 1.

Considering natural numbers of the form, a^b, where a, b < 100,
what is the maximum digital sum?

Solution:

*/

#include <iostream>
#include <cstdint>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>
#include <chrono>

auto digitCount(uint32_t a, uint32_t b) {
	using namespace boost;
	multiprecision::cpp_int p = a;
	p = multiprecision::pow(p, b);
	auto ans = p.str();
	uint32_t sum = 0;
	for (const auto &c : ans)
		sum += c - '0';
	return sum;
}

auto compute() {
	constexpr uint32_t limit = 100;

	uint32_t max = 0;
	for (uint32_t a = 1; a < limit; ++a)
		for (uint32_t b = 1, c; b < limit; ++b)
			if (max < (c = digitCount(a, b)))
				max = c;

	return max;
}

#ifdef _MSC_VER
	template <class T>
	inline void DoNotOptimize(const T &value) {
		__asm { lea ebx, value }
	}
#else
	template <class T>
	__attribute__((always_inline)) inline void DoNotOptimize(const T &value) {
		asm volatile("" : "+m"(const_cast<T &>(value)));
	}
#endif

int main() {
	using namespace std;
	using namespace chrono;
	auto start = high_resolution_clock::now();
	auto result = compute();
	DoNotOptimize(result);
	cout << "Done in "
		<< duration_cast<nanoseconds>(high_resolution_clock::now() - start).count() / 1e6
		<< " miliseconds." << endl;
	cout << result << endl;
}


#include <iostream>

#include <Eigen/Dense>
#include "src/rk_implementer.hpp"
#include "src/rk_solvers.hpp"

int main() {

  /**
   *  Setting global variables, the time we are integrating over
   *  and the number of integration steps we take in this time.
   *
   *  We also define the function f representing our ODE
   *  and the initial conditiond y0.
   */

  unsigned int steps = 100;
  unsigned int time = 20;

  auto f = [] (Eigen::VectorXd y) {
    Eigen::VectorXd df(2);
    df << y(0)*(3-0.7*y(1)) , -y(1)*(0.7-0.8*y(0));
    return df;
  };

  Eigen::VectorXd y0(2);
  y0 << 6,2;

  /**
   * Solve using built in solver:
   */

  std::vector<Eigen::VectorXd> y1Vec = ExplicitRKSolvers::classical4thOrderRuleIntegrator(f, time,y0, steps);

  std::cout << "The classical 4th order Runge-Kutta method gives us" << y1Vec.back().transpose() << std::endl;

  for(int i = 0; i < y1Vec.size(); i++){
    std::cout << y1Vec.at(i).transpose() << " \n";
  }

  /**
   * Solve explicitly using custom Butcher's table:
   */

  Eigen::MatrixXd A(3,3);
  A << 0, 0, 0,
       1, 0, 0,
       1.0/4,1.0/4,0;
  Eigen::VectorXd b(3);
  b << 1.0/6, 1.0/6, 2.0/3;

  ExplicitRungeKuttaIntegrator<Eigen::VectorXd> Solver(A,b);
  std::vector<Eigen::VectorXd> y2Vec = Solver.solve(f,2,y0,10);

  std::cout << "SSPRK3 3rd order Runge-Kutta method gives us = " << y2Vec[10].transpose() << std::endl;

}


#include <math.h>
#include <armadillo>

arma::mat CENTERS = {20.0, 35.0, 40.0, 45.0};
arma::mat SIGMAS = {3.0, 10.0, 5.0, 2.0};
arma::mat CHANNELS = arma::linspace(0, 99, 100);

const double EulerConstant = std::exp(1.0);

class DataGenerator
{

public:
    arma::mat m_L;

    DataGenerator()
    {
        m_L = generate_library();
    }

    arma::mat generate_library(arma::mat channels = CHANNELS,
                               arma::mat centers = CENTERS,
                               arma::mat sigmas = SIGMAS)
    {
        return generate_signal_matrix(channels, centers, sigmas);
    };

    arma::mat generate_signal(arma::mat weights, double noise = 0.00, double offset = 0.0)
    {
        arma::mat signal = get_linear_signal(weights);
        return signal + noise * arma::randn(1, signal.n_elem) + offset;
    }

    arma::mat generate_signal(arma::mat weights,
                              std::vector<int> indices,
                              double outlier_sigma_square = 10,
                              double outlier_offset = 0.0,
                              double noise = 0.00,
                              double offset = 0.00)
    {
        arma::mat signal = get_linear_signal(weights);
        std::default_random_engine generator;
        std::normal_distribution<double> distribution(outlier_offset, outlier_sigma_square);
        for (auto &&i : indices)
        {
            signal[i] += distribution(generator);
        }
        return signal + noise * arma::randn(1, signal.n_elem) + offset;
    }

    template <typename Function>
    arma::mat generate_signal(arma::mat weights, arma::mat library, Function model, double noise = 0.0, double offset = 0.0)
    {
        arma::mat signal = model(weights, library);
        return signal + noise * arma::randn(1, signal.n_elem) + offset;
    }

private:
    arma::mat generate_signal_matrix(arma::mat channels, arma::mat centers, arma::mat sigmas)
    {
        int n_components = centers.n_elem;
        arma::mat signals = arma::zeros(0, channels.n_elem);
        for (int i = 0; i < n_components; ++i)
        {
            float center = centers[i];
            float sigma = sigmas[i];
            arma::mat signal = calculate_gaussian(sigma, center, channels);
            signals = join_vert(signals, signal.t());
        };
        return signals;
    }

    arma::mat get_linear_signal(arma::mat x)
    {
        return x * m_L;
    }

    arma::mat calculate_gaussian(float sigma, float center, arma::mat x)
    {
        float scaling_factor = 1 / (sigma * sqrt(2 * M_PI));
        arma::mat exponent = -pow((x - center) / sigma, 2) / 2;
        arma::mat result = scaling_factor * pow_to_vector(EulerConstant, exponent);
        return result;
    }

    arma::mat pow_to_vector(float x, arma::mat y)
    {
        arma::mat result;
        result.copy_size(y);
        for (std::size_t i = 0; i < result.n_elem; ++i)
        {
            result[i] = std::pow(x, y[i]);
        }
        return result;
    }
};


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#include <pangolin/pangolin.h>
using namespace Eigen;
using namespace std;

struct EulerAngle {
    EulerAngle()
    {
        roll_ = 0.;
        pitch_ = 0.;
        yaw_ = 0.;
    }
public:
    double roll_;
    double pitch_;
    double yaw_;
};

std::ostream& operator<<(std::ostream& os, const EulerAngle& o)
{
    // yaw-pitch-roll => ZYX
    os << o.yaw_ << " " << o.pitch_ << " " << o.roll_;
    return os;
}

std::istream& operator>>(std::istream& is, EulerAngle& o)
{
    is >> o.yaw_;
    is >> o.pitch_;
    is >> o.roll_;
    return is;
}

pangolin::Var<EulerAngle>* eulerAngle;

struct RotationMatrix {
    Matrix3d matrix = Matrix3d::Identity();
};

ostream& operator<<(ostream& out, const RotationMatrix& r)
{
    out.setf(ios::fixed);
    Matrix3d matrix = r.matrix;
    out << '=';
    out << "[" << setprecision(2) << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"
        << "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"
        << "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";
    return out;
}

istream& operator>>(istream& in, RotationMatrix& r)
{
    return in;
}

struct QuaternionDraw {
    Quaterniond q;
};

ostream& operator<<(ostream& out, const QuaternionDraw quat)
{
    auto c = quat.q.coeffs();
    out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
    return out;
}

istream& operator>>(istream& in, const QuaternionDraw quat)
{
    return in;
}

void ResetCallback()
{
    std::cout << "You typed ctrl-r or pushed reset" << std::endl;
    EulerAngle ea;
    *eulerAngle = ea;
}

Eigen::Quaterniond euler2Quaternion(const double yaw, const double pitch, const double roll)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q;
}

int main(int argc, char** argv)
{
    const int UI_WIDTH = 500;
    pangolin::CreateWindowAndBind("visualize geometry", 1000, 500);
    glEnable(GL_DEPTH_TEST);
    // OpenGlMatrixSpec pangolin::ProjectionMatrix(int w, int h, double fu, double fv, double u0, double v0, double zNear, double zFar);
    //OpenGlMatrix 	ModelViewLookAt (double ex, double ey, double ez, double lx, double ly, double lz, AxisDirection up)
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 100),
        pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisZ));
    pangolin::View& d_cam = pangolin::CreateDisplay()
                                .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f)
                                .SetHandler(new pangolin::Handler3D(s_cam));
    // ui
    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
    eulerAngle = new pangolin::Var<EulerAngle>("ui.YawPitchRoll", EulerAngle());
    pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());
    pangolin::Var<QuaternionDraw> quaternion("ui.Quaternion", QuaternionDraw());
    pangolin::Var<std::function<void(void)>> reset("ui.Reset", ResetCallback);

    while (!pangolin::ShouldQuit()) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        // input eulerangle
        EulerAngle ea = *eulerAngle;
        cout << "yaw: " << ea.yaw_ << "\t pitch: " << ea.pitch_ << "\troll: " << ea.roll_ << endl;

        // Eulerangle to quaterniond
        Eigen::Quaterniond qd = euler2Quaternion(ea.yaw_ / 180 * M_PI, ea.pitch_ / 180 * M_PI, ea.roll_ / 180 * M_PI);

        // show quaternion
        QuaternionDraw quat;
        quat.q = qd;
        quaternion = quat;

        // quaterniond to rotationmatrix
        Matrix3d rot = qd.toRotationMatrix();

        // pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
        RotationMatrix R;
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                R.matrix(i, j) = rot(j, i);
        rotation_matrix = R;
        // transform matrix for pangolin
        Matrix<double, 4, 4> matrix;
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                matrix(i, j) = rot(j, i);

        s_cam.SetModelViewMatrix(pangolin::OpenGlMatrix(matrix));

        pangolin::glDrawColouredCube();

        // draw the original axis
        glLineWidth(3);
        glColor3f(0.8f, 0.f, 0.f);
        glBegin(GL_LINES);
        glVertex3f(0, 0, 0);
        glVertex3f(10, 0, 0);
        glColor3f(0.f, 0.8f, 0.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0, 10, 0);
        glColor3f(0.2f, 0.2f, 1.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0, 0, 10);
        glEnd();

        pangolin::FinishFrame();
    }

    return 0;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//

#include "Activation.hpp"

#include <boost/log/trivial.hpp>

#include <cmath>

namespace armnn
{

void Activation(const float* in,
               float* out,
               const TensorInfo& tensorInfo,
               ActivationFunction function,
               float a,
               float b)
{
    for (size_t i = 0; i<tensorInfo.GetNumElements(); i++)
    {
        float input = in[i];
        float output;

        // compute the result of the activation function
        switch (function)
        {
            case ActivationFunction::Linear:
            {
                output = a * input + b;
                break;
            }
            case ActivationFunction::Sigmoid:
            {
                output = 1.f / (1.f + expf(-input));
                break;
            }
            case ActivationFunction::ReLu:
            {
                output = std::max(0.f, input);
                break;
            }
            case ActivationFunction::BoundedReLu:
            {
                output = std::min(a, std::max(b, input));
                break;
            }
            case ActivationFunction::SoftReLu:
            {
                output = logf(1.0f + expf(input));
                break;
            }
            case ActivationFunction::LeakyReLu:
            {
                output = input > 0.0f ? input : (input * a);
                break;
            }
            case ActivationFunction::Abs:
            {
                output = input < 0 ? -input : input;
                break;
            }
            case ActivationFunction::Sqrt:
            {
                output = sqrtf(input);
                break;
            }
            case ActivationFunction::Square:
            {
                output = input * input;
                break;
            }
            case ActivationFunction::TanH:
            {
                output = a * tanhf(b * input);
                break;
            }
            default:
            {
                BOOST_LOG_TRIVIAL(error) << "Unsupported activation function";
                return;
            }
        }

        out[i] = output;
    }
}

} //namespace armnn


/**
 *
 * @section DESCRIPTION
 *
 * Recursive Exponentiation.
 *
 * Write a recursive function power(base, exponent) that, when invoked, returns
 *
 * baseexponent
 *
 * For example, power(3, 4) = 3 * 3 * 3 * 3. Assume that exponent is an integer
 * greater than or equal to 1. Hint: The recursion step would use the
 * relationship
 *
 * baseexponent = base * baseexponent - 1
 *
 * and the terminating condition occurs when exponent is equal to 1, because
 *
 * base1 = base
 */

#include <boost/multiprecision/cpp_int.hpp>
#include <cassert>
#include <iostream>

using boost::multiprecision::checked_cpp_int;

static checked_cpp_int
Power(const checked_cpp_int &base, const checked_cpp_int &exponent)
{
    assert(base >= 0 && exponent > 0);

    if (exponent == 1) {
        return base;
    }
    return base * Power(base, exponent - 1);
}

int main(void)
{
    using namespace std;

    checked_cpp_int base, exponent;

    cout << "Enter base: ";
    cin >> base;
    cout << "Enter exponent: ";
    cin >> exponent;
    cout \
        << "Power(" << base << ", " << exponent << ") = "
        << Power(base, exponent) << endl;
    return 0;
}


/**
 * \file boost/numeric/ublasx/operation/hild.hpp
 *
 * \brief Hilbert matrix.
 *
 * Copyright (c) 2012, 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_HILB_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_HILB_HPP

#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace boost { namespace numeric { namespace ublasx {

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

template <typename T>
BOOST_UBLAS_INLINE
matrix<T> hilb(::std::size_t n)
{
	matrix<T> H(n,n);

	for (::std::size_t r = 0; r < n; ++r)
	{
		for (::std::size_t c = 0; c < n; ++c)
		{
			//H(r,c) = T(1.0)/((r+1)+(c+1)-T(1.0));
			H(r,c) = T(1.0)/(r+c+T(1.0));
		}
	}

	return H;
}

BOOST_UBLAS_INLINE
matrix<double> hilb(::std::size_t n, int=0)
{
	return hilb<double>(n);
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_HILB_HPP


#include <armadillo>
#include <cmath>

#include <armadillo>
#include "mathutils/matrix_tool.hh"
#include "mathutils/numerical_constants.hh"

arma::vec3 pol_from_cart(arma::vec3 in)
{
    double d = 0.0;
    double azimuth = 0.0;
    double elevation = 0.0;
    double denom;
    arma::vec3 POLAR;

    double v1 = in(0);
    double v2 = in(1);
    double v3 = in(2);

    d = norm(in);
    azimuth = atan2(v2, v1);

    denom = sqrt(v1 * v1 + v2 * v2);
    if (denom > 0.) {
        elevation = atan2(-v3, denom);
    } else {
        if (v3 > 0)
            elevation = -PI / 2.;
        if (v3 < 0)
            elevation = PI / 2.;
        if (v3 == 0)
            elevation = 0.;
    }

    POLAR(0) = d;
    POLAR(1) = azimuth;
    POLAR(2) = elevation;

    return POLAR;
}

///////////////////////////////////////////////////////////////////////////////
// Returns the angle between two 3x1 vectors
// Example: theta=angle(VEC1,VEC2);
// 010824 Created by Peter H Zipfel
///////////////////////////////////////////////////////////////////////////////
double angle(arma::vec3 VEC1, arma::vec3 VEC2)
{
    double argument;
    double scalar = dot(VEC1, VEC2);
    double abs1 = norm(VEC1);
    double abs2 = norm(VEC2);

    double dum = abs1 * abs2;
    if (abs1 * abs2 > arma::datum::eps)
        argument = scalar / dum;
    else
        argument = 1.;
    if (argument > 1.)
        argument = 1.;
    if (argument < -1.)
        argument = -1.;

    return acos(argument);
}

///////////////////////////////////////////////////////////////////////////////
// Return skew symmetric matrix of a Vector3
//            | 0 -c  b|        |a|
//            | c  0 -a| <--    |b|
//            |-b  a  0|        |c|
//
// Example: MAT = VEC.skew_sym();
///////////////////////////////////////////////////////////////////////////////
arma::mat33 skew_sym(arma::vec3 vec)
{
    arma::mat33 RESULT;

    RESULT(0, 0) = 0;
    RESULT(1, 0) = vec(2);
    RESULT(2, 0) = -vec(1);
    RESULT(0, 1) = -vec(2);
    RESULT(1, 1) = 0;
    RESULT(2, 1) = vec(0);
    RESULT(0, 2) = vec(1);
    RESULT(1, 2) = -vec(0);
    RESULT(2, 2) = 0;

    return RESULT;
}

////////////////////////////////////////////////////////////////////////////////
// Returns the T.M. of the psivg -> thtvg sequence
//
// 010628 Created by Peter H Zipfel
// 170121 Create Armadillo Version by soncyang
////////////////////////////////////////////////////////////////////////////////
arma::mat33 build_psivg_thtvg_TM(const double &psivg, const double &thtvg)
{
    arma::mat33 AMAT;

    AMAT(0, 2) = -sin(thtvg);
    AMAT(1, 0) = -sin(psivg);
    AMAT(1, 1) = cos(psivg);
    AMAT(2, 2) = cos(thtvg);
    AMAT(0, 0) = (AMAT(2, 2) * AMAT(1, 1));
    AMAT(0, 1) = (-AMAT(2, 2) * AMAT(1, 0));
    AMAT(2, 0) = (-AMAT(0, 2) * AMAT(1, 1));
    AMAT(2, 1) = (AMAT(0, 2) * AMAT(1, 0));
    AMAT(1, 2) = 0.0;

    return AMAT;
}

////////////////////////////////////////////////////////////////////////////////
// Returns the T.M. of the psi->tht->phi sequence
// Euler angle transformation matrix of flight mechanics
//
// 011126 Created by Peter H Zipfel
// 170121 Add Armadillo version
////////////////////////////////////////////////////////////////////////////////
arma::mat33 build_psi_tht_phi_TM(const double &psi, const double &tht, const double &phi)
{
    double spsi = sin(psi);
    double cpsi = cos(psi);
    double stht = sin(tht);
    double ctht = cos(tht);
    double sphi = sin(phi);
    double cphi = cos(phi);

    arma::mat33 AMAT;
    AMAT(0, 0) = cpsi * ctht;
    AMAT(1, 0) = cpsi * stht * sphi - spsi * cphi;
    AMAT(2, 0) = cpsi * stht * cphi + spsi * sphi;
    AMAT(0, 1) = spsi * ctht;
    AMAT(1, 1) = spsi * stht * sphi + cpsi * cphi;
    AMAT(2, 1) = spsi * stht * cphi - cpsi * sphi;
    AMAT(0, 2) = -stht;
    AMAT(1, 2) = ctht * sphi;
    AMAT(2, 2) = ctht * cphi;

    return AMAT;
}

arma::vec4 Matrix2Quaternion(arma::mat33 Matrix_in)
{
    arma::vec4 q_square;
    double q_square_max;
    int j;
    arma::vec4 Quaternion;
    Matrix_in = trans(Matrix_in);
    q_square[0] = fabs(1.0 + Matrix_in(0, 0) + Matrix_in(1, 1) + Matrix_in(2, 2));
    q_square[1] = fabs(1.0 + Matrix_in(0, 0) - Matrix_in(1, 1) - Matrix_in(2, 2));
    q_square[2] = fabs(1.0 - Matrix_in(0, 0) + Matrix_in(1, 1) - Matrix_in(2, 2));
    q_square[3] = fabs(1.0 - Matrix_in(0, 0) - Matrix_in(1, 1) + Matrix_in(2, 2));

    q_square_max = q_square.max();
    j = q_square.index_max();

    switch (j) {
    case 0:
        Quaternion(0) = 0.5 * sqrt(q_square_max);
        Quaternion(1) = 0.25 * (Matrix_in(2, 1) - Matrix_in(1, 2)) / Quaternion[0];
        Quaternion(2) = 0.25 * (Matrix_in(0, 2) - Matrix_in(2, 0)) / Quaternion[0];
        Quaternion(3) = 0.25 * (Matrix_in(1, 0) - Matrix_in(0, 1)) / Quaternion[0];
        break;
    case 1:
        Quaternion(1) = 0.5 * sqrt(q_square_max);
        Quaternion(0) = 0.25 * (Matrix_in(2, 1) - Matrix_in(1, 2)) / Quaternion[1];
        Quaternion(2) = 0.25 * (Matrix_in(1, 0) + Matrix_in(0, 1)) / Quaternion[1];
        Quaternion(3) = 0.25 * (Matrix_in(0, 2) + Matrix_in(2, 0)) / Quaternion[1];
        break;
    case 2:
        Quaternion(2) = 0.5 * sqrt(q_square_max);
        Quaternion(0) = 0.25 * (Matrix_in(0, 2) - Matrix_in(2, 0)) / Quaternion[2];
        Quaternion(1) = 0.25 * (Matrix_in(1, 0) + Matrix_in(0, 1)) / Quaternion[2];
        Quaternion(3) = 0.25 * (Matrix_in(2, 1) + Matrix_in(1, 2)) / Quaternion[2];
        break;
    case 3:
        Quaternion(3) = 0.5 * sqrt(q_square_max);
        Quaternion(0) = 0.25 * (Matrix_in(1, 0) - Matrix_in(0, 1)) / Quaternion[3];
        Quaternion(1) = 0.25 * (Matrix_in(2, 0) + Matrix_in(0, 2)) / Quaternion[3];
        Quaternion(2) = 0.25 * (Matrix_in(2, 1) + Matrix_in(1, 2)) / Quaternion[3];
        break;
    }

    return Quaternion;
}

arma::mat33 Quaternion2Matrix(arma::vec4 Quaternion_in)
{
    arma::mat33 Matrix_out;

    Matrix_out(0, 0) = 2. * (Quaternion_in(0) * Quaternion_in(0) + Quaternion_in(1) * Quaternion_in(1)) - 1.;
    Matrix_out(0, 1) = 2. * (Quaternion_in(1) * Quaternion_in(2) + Quaternion_in(0) * Quaternion_in(3));
    Matrix_out(0, 2) = 2. * (Quaternion_in(1) * Quaternion_in(3) - Quaternion_in(0) * Quaternion_in(2));
    Matrix_out(1, 0) = 2. * (Quaternion_in(1) * Quaternion_in(2) - Quaternion_in(0) * Quaternion_in(3));
    Matrix_out(1, 1) = 2. * (Quaternion_in(0) * Quaternion_in(0) + Quaternion_in(2) * Quaternion_in(2)) - 1.;
    Matrix_out(1, 2) = 2. * (Quaternion_in(2) * Quaternion_in(3) + Quaternion_in(0) * Quaternion_in(1));
    Matrix_out(2, 0) = 2. * (Quaternion_in(1) * Quaternion_in(3) + Quaternion_in(0) * Quaternion_in(2));
    Matrix_out(2, 1) = 2. * (Quaternion_in(2) * Quaternion_in(3) - Quaternion_in(0) * Quaternion_in(1));
    Matrix_out(2, 2) = 2. * (Quaternion_in(0) * Quaternion_in(0) + Quaternion_in(3) * Quaternion_in(3)) - 1.;

    return Matrix_out;
}

arma::vec4 Quaternion_conjugate(arma::vec4 Quaternion_in)
{
    arma::vec4 Quaternion_out;

    Quaternion_out(0) = Quaternion_in(0);
    Quaternion_out(1) = -Quaternion_in(1);
    Quaternion_out(2) = -Quaternion_in(2);
    Quaternion_out(3) = -Quaternion_in(3);

    return Quaternion_out;
}

arma::vec4 Quaternion_cross(arma::vec4 Quaternion_in1, arma::vec4 Quaternion_in2)
{
    arma::vec4 Quaternion_out;

    Quaternion_out(0) = Quaternion_in1(0) * Quaternion_in2(0) - Quaternion_in1(1) * Quaternion_in2(1) - Quaternion_in1(2) * Quaternion_in2(2) - Quaternion_in1(3) * Quaternion_in2(3);
    Quaternion_out(1) = Quaternion_in1(1) * Quaternion_in2(0) + Quaternion_in1(0) * Quaternion_in2(1) - Quaternion_in1(3) * Quaternion_in2(2) + Quaternion_in1(2) * Quaternion_in2(3);
    Quaternion_out(2) = Quaternion_in1(2) * Quaternion_in2(0) + Quaternion_in1(3) * Quaternion_in2(1) + Quaternion_in1(0) * Quaternion_in2(2) - Quaternion_in1(1) * Quaternion_in2(3);
    Quaternion_out(3) = Quaternion_in1(3) * Quaternion_in2(0) - Quaternion_in1(2) * Quaternion_in2(1) + Quaternion_in1(1) * Quaternion_in2(2) + Quaternion_in1(0) * Quaternion_in2(3);

    return Quaternion_out;
}

void Quaternion2Euler(arma::vec4 Quaternion_in, double &Roll, double &Pitch, double &Yaw)
{
    Roll = atan2(2.0 * (Quaternion_in(0) * Quaternion_in(1) + Quaternion_in(2) * Quaternion_in(3)), 1.0 - 2.0 * (Quaternion_in(1) * Quaternion_in(1) + Quaternion_in(2) * Quaternion_in(2)));
    Pitch = asin(2.0 * (Quaternion_in(0) * Quaternion_in(2) - Quaternion_in(3) * Quaternion_in(1)));
    Yaw = atan2(2.0 * (Quaternion_in(0) * Quaternion_in(3) + Quaternion_in(1) * Quaternion_in(2)), 1.0 - 2.0 * (Quaternion_in(2) * Quaternion_in(2) + Quaternion_in(3) * Quaternion_in(3)));
}

arma::vec4 Euler2Quaternion(double Roll, double Pitch, double Yaw)
{
    arma::vec4 Quaternion_out;
    double cr = cos(Roll * 0.5);
    double sr = sin(Roll * 0.5);
    double cp = cos(Pitch * 0.5);
    double sp = sin(Pitch * 0.5);
    double cy = cos(Yaw * 0.5);
    double sy = sin(Yaw * 0.5);

    Quaternion_out(0) = cy * cr * cp + sy * sr * sp;
    Quaternion_out(1) = cy * sr * cp - sy * cr * sp;
    Quaternion_out(2) = cy * cr * sp + sy * sr * cp;
    Quaternion_out(3) = sy * cr * cp - cy * sr * sp;

    return Quaternion_out;
}

int QuaternionMultiply(arma::vec4 &Q_out, arma::vec4 const &Q_in1, arma::vec4 const &Q_in2)
{
    //Q_in1.print("Qin1:%5.5lf");
    //Q_in2.print("Qin2:%5.5lf");
    Q_out(0) = Q_in1(0) * Q_in2(0) - Q_in1(1) * Q_in2(1) - Q_in1(2) * Q_in2(2) - Q_in1(3) * Q_in2(3);
    Q_out(1) = Q_in1(0) * Q_in2(1) + Q_in1(1) * Q_in2(0) + Q_in1(2) * Q_in2(3) - Q_in1(3) * Q_in2(2);
    Q_out(2) = Q_in1(0) * Q_in2(2) - Q_in1(1) * Q_in2(3) + Q_in1(2) * Q_in2(0) + Q_in1(3) * Q_in2(1);
    Q_out(3) = Q_in1(0) * Q_in2(3) + Q_in1(1) * Q_in2(2) - Q_in1(2) * Q_in2(1) + Q_in1(3) * Q_in2(0);
    //Q_out.print("Qout2:%5.5lf");
    return 0;
}

arma::vec4 QuaternionInverse(arma::vec4 Q_in)
{
    return Quaternion_conjugate(Q_in) / sqrt(Q_in(0) * Q_in(0) + Q_in(1) * Q_in(1) + Q_in(2) * Q_in(2) + Q_in(3) * Q_in(3));
}

arma::vec4 QuaternionTranspose(arma::vec4 Q_in)
{
    arma::vec4 Q_out;

    Q_out(0) = -Q_in(0);
    Q_out(1) = Q_in(1);
    Q_out(2) = Q_in(2);
    Q_out(3) = Q_in(3);

    return Q_out;
}

arma::vec3 QuaternionRotation(arma::vec4 Q_in, arma::vec3 V_in)
{
    arma::vec4 Q_temp;
    arma::vec3 V_out;

    Q_temp(0) = 0.0;
    Q_temp(1) = V_in(0);
    Q_temp(2) = V_in(1);
    Q_temp(3) = V_in(2);

    Q_temp = Quaternion_cross(Quaternion_conjugate(Q_in), Quaternion_cross(Q_temp, Q_in));

    V_out(0) = Q_temp(1);
    V_out(1) = Q_temp(2);
    V_out(2) = Q_temp(3);

    return V_out;
}

arma::mat33 cross_matrix(arma::vec3 in)
{
    arma::mat33 c_matrix;

    c_matrix(0, 0) = 0.0;
    c_matrix(0, 1) = -in(2);
    c_matrix(0, 2) = in(1);
    c_matrix(1, 0) = in(2);
    c_matrix(1, 1) = 0.0;
    c_matrix(1, 2) = -in(0);
    c_matrix(2, 0) = -in(1);
    c_matrix(2, 1) = in(0);
    c_matrix(2, 2) = 0.0;

    return c_matrix;
}

arma::mat33 TMX(double ang)
{
    arma::mat33 TM;

    TM(0, 0) = 1.0;
    TM(0, 1) = 0.0;
    TM(0, 2) = 0.0;
    TM(1, 0) = 0.0;
    TM(1, 1) = cos(ang);
    TM(1, 2) = sin(ang);
    TM(2, 0) = 0.0;
    TM(2, 1) = -sin(ang);
    TM(2, 2) = cos(ang);

    return TM;
}

arma::mat33 TMY(double ang)
{
    arma::mat33 TM;

    TM(0, 0) = cos(ang);
    TM(0, 1) = 0.0;
    TM(0, 2) = -sin(ang);
    TM(1, 0) = 0.0;
    TM(1, 1) = 1.0;
    TM(1, 2) = 0.0;
    TM(2, 0) = sin(ang);
    TM(2, 1) = 0.0;
    TM(2, 2) = cos(ang);

    return TM;
}

arma::mat33 TMZ(double ang)
{
    arma::mat33 TM;

    TM(0, 0) = cos(ang);
    TM(0, 1) = sin(ang);
    TM(0, 2) = 0.0;
    TM(1, 0) = -sin(ang);
    TM(1, 1) = cos(ang);
    TM(1, 2) = 0.0;
    TM(2, 0) = 0.0;
    TM(2, 1) = 0.0;
    TM(2, 2) = 1.0;

    return TM;
}

///////////////////////////////////////////////////////////////////////////////
// Returns the sign of the variable
// Example: value_signed=value*sign(variable)
// 010824 Created by Peter H Zipfel
///////////////////////////////////////////////////////////////////////////////
int sign(const double &variable)
{
    int sign = 0;
    if (variable < 0.)
        sign = -1;
    if (variable >= 0.)
        sign = 1;

    return sign;
}

arma::vec3 euler_angle(arma::mat33 TBD_in)
{
    double psibdc(0), thtbdc(0), phibdc(0);
    double cthtbd(0);

    double mroll = 0;

    double tbd13 = TBD_in(0, 2);
    double tbd11 = TBD_in(0, 0);
    double tbd33 = TBD_in(2, 2);
    double tbd12 = TBD_in(0, 1);
    double tbd23 = TBD_in(1, 2);

    arma::vec3 euler_ang;
    // *geodetic Euler angles
    // computed pitch angle: 'thtbdc'
    // note: when |tbd13| >= 1, thtbdc = +- pi/2, but cos(thtbdc) is
    // forced to be a small positive number to prevent division by zero
    if (fabs(tbd13) < 1) {
        thtbdc = asin(-tbd13);
        cthtbd = cos(thtbdc);
    } else {
        thtbdc = PI / 2 * sign(-tbd13);
        cthtbd = EPS;
    }
    // computed yaw angle: 'psibdc'
    double cpsi = tbd11 / cthtbd;
    if (fabs(cpsi) > 1)
        cpsi = 1 * sign(cpsi);
    psibdc = acos(cpsi) * sign(tbd12);

    // computed roll angle: 'phibdc'
    double cphi = tbd33 / cthtbd;
    if (fabs(cphi) > 1)
        cphi = 1 * sign(cphi);

    // selecting the Euler roll angle of flight mechanics (not for thtbdc=90 or
    // =-90deg)
    if (mroll == 0 || mroll == 1)
        // roll feedback for right side up
        phibdc = acos(cphi) * sign(tbd23);
    else if (mroll == 2)
        // roll feedback for inverted flight
        phibdc = acos(-cphi) * sign(-tbd23);

    euler_ang(0) = phibdc;
    euler_ang(1) = thtbdc;
    euler_ang(2) = psibdc;

    return euler_ang;
}


//  Diagonalizing tridiagonal Toeplitz matrix  with Lapack functions
//  Compile as c++ -O3 -o Tridiag.x TridiagToeplitz.cpp -larmadillo -llapack -lblas
//  Here we add the harmonic oscillator to the tridiagonal Toeplitz matrix and
//  use Richardson's extrapolation scheme to obtain results to high precision
#include <cmath>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <armadillo>

using namespace  std;
using namespace  arma;
double potential(double);
void PolynomialInterpolation(vec, vec, int, double, double *, double *);
// Begin of main program

int main(int argc, char* argv[])
{
  int       i, j, StartDim, Dim;
  double    RMin, RMax, Step, DiagConst, NondiagConst;
  double error, FinalE;
  RMin = 0.0; RMax = 10.0; StartDim =400;
  vec StepSizes = zeros<vec>(5);
  mat EigvaluesInterpol = zeros<mat>(5,20);
  for (int interpolation = 0; interpolation < 5; interpolation++){
    if (interpolation == 1) {
      Dim = StartDim;
    }
    else{
      Dim = (interpolation-1)*2*StartDim;
    }
    mat Hamiltonian = zeros<mat>(Dim,Dim);
    // Integration step length
    Step    = RMax/Dim;
    DiagConst = 2.0 / (Step*Step);
    NondiagConst =  -1.0 / (Step*Step);
    // local memory for r and the potential w[r]
    double r;
    vec Potential = zeros<vec>(Dim);
    for(i = 0; i < Dim; i++) {
      r = RMin + (i+1) * Step;
      Potential[i] = potential(r);
    }
    StepSizes(interpolation) = Step;
    // Setting up tridiagonal matrix and diagonalization using Armadillo
    Hamiltonian(0,0) = DiagConst+Potential(0);
    Hamiltonian(0,1) = NondiagConst;
    for(i = 1; i < Dim-1; i++) {
      Hamiltonian(i,i-1)    = NondiagConst;
      Hamiltonian(i,i)    = DiagConst+Potential(i);
      Hamiltonian(i,i+1)    = NondiagConst;
    }
    Hamiltonian(Dim-1,Dim-2) = NondiagConst;
    Hamiltonian(Dim-1,Dim-1) = DiagConst+Potential(Dim-1);
    // diagonalize and obtain eigenvalues
    vec Eigval(Dim);
    eig_sym(Eigval, Hamiltonian);
    for (int l = 0; l < 20; l++){
      //      EigvaluesInterpol(interpolation,l) = Eigval(l);
      vec EigSend(interpolation);
           for (int k = 0; k < interpolation; k++) EigSend(k) = EigvaluesInterpol(k,l);
      if (interpolation > 0 ){
	  PolynomialInterpolation(StepSizes, EigSend, interpolation, 0.0, &FinalE, &error);
	  cout << setiosflags(ios::showpoint | ios::uppercase);
	cout <<"Eigenvalue = " << setw(15) << l << endl;
	cout << setw(15) << setprecision(8) << FinalE << endl;
      }
    }
  }
  return 0;
 }  //  end of main function

  /*
    The function potential()
    calculates and return the value of the
    potential for a given argument x.
    The potential here is for the hydrogen atom
  */

  double potential(double x)
  {
    return x*x;

  } // End: function potential()

  void PolynomialInterpolation(vec xa, vec ya, int n, double x, double *y, double *dy)
  {
    int i,m,ns=1;
    double den,dif,dift,ho,hp,w;

    dif=fabs(x-xa(0));
    vec c = zeros<vec>(n);
    vec d = zeros<vec>(n);
    for (i=0;i < n;i++) {
      if ( (dift=fabs(x-xa(i))) < dif) {
	ns=i;
	dif=dift;
      }
      c(i)=ya(i);
      d(i)=ya(i);
    }
    *y=ya(ns--);
    for (m=0; m< n;m++) {
      for (i = 0;i < n-m;i++) {
	ho=xa(i)-x;
	hp=xa(i+m)-x;
	w=c(i+1)-d(i);
	//			if ( (den=ho-hp) == 0.0) nrerror("Error ");
	den=w/den;
	d(i)=hp*den;
	c(i)=ho*den;
      }
      *y += (*dy=(2*ns < (n-m) ? c(ns+1) : d(ns--)));
    }
  }


#pragma once
#include "constant_function.hpp"
#include "coordinate_transform.hpp"
#include "grad_shape.hpp"
#include "integrate.hpp"
#include "shape.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>

//----------------compMatrixBegin----------------
//! Evaluate the stiffness matrix on the triangle spanned by
//! the points (a, b, c).
//!
//! Here, the stiffness matrix A is a 3x3 matrix
//!
//! $$A_{ij} = \int_{K} ( sigma(x,y) \nabla \lambda_i^K(x, y) \cdot  \nabla \lambda_j^K(x, y)\; + r \lambda_i^K(x,y)\lambda_j^K(x,y) )dV$$
//!
//! where $K$ is the triangle spanned by (a, b, c).
//!
//! @param[out] stiffnessMatrix should be a 3x3 matrix
//!                        At the end, will contain the integrals above.
//!
//! @param[in] a the first corner of the triangle
//! @param[in] b the second corner of the triangle
//! @param[in] c the third corner of the triangle
//! @param[in] sigma the function sigma as in the exercise
//! @param[in] r the parameter r as in the exercise
template <class MatrixType, class Point>
void computeStiffnessMatrix(MatrixType & stiffnessMatrix,
                            const Point &a,
                            const Point &b,
                            const Point &c,
                            const std::function<double(double, double)> &sigma = constantFunction,
                            const double r = 0) {
	Eigen::Matrix2d coordinateTransform = makeCoordinateTransform(b - a, c - a);
	double          volumeFactor        = std::abs(coordinateTransform.determinant());

	Eigen::Matrix2d elementMap = coordinateTransform.inverse().transpose();
	// (write your solution here)
	stiffnessMatrix.resize(3, 3);

	auto PhiK = [&](const Eigen::Vector2d &x) -> Eigen::Vector2d {
		return coordinateTransform * x + a.transpose();
	};

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			auto f = [&](double x, double y) -> double {
				Eigen::Vector2d transformedPoint = PhiK(Eigen::Vector2d(x,y));

				return (sigma(transformedPoint.x(), transformedPoint.y()) * (elementMap * gradientLambda(i, x, y)).dot(elementMap * gradientLambda(j, x, y)) + r * lambda(i, x, y) * lambda(j, x, y)) * volumeFactor;
			};
			stiffnessMatrix(i, j) = integrate(f);
		}
	}
}
//----------------compMatrixEnd----------------


#include <Eigen/Dense>
#include <utility>
#include <iostream>
#include <vector>
#include <cmath>
#include <iomanip>

#include "general_nonlinear_solver.h" // implements general_nonlinear_solver

//! \brief Implements a single step of the modified newton
//! \tparam argument type of argument to function f: such as double or vector etc...
//! \tparam function type for the function f, likely a lambda function
//! \tparam jacobian type for the jacobian df, likely a lambda function
//! \param[in] x previous value to use in step, also initial guess if needed
//! \param[out] x_next next step x_{k+1} of modified Newton
//! \param[in] f function handle for f(x) = 0
//! \param[in] df function handle for jacobian df of f
template <typename argument, class function, class jacobian>
void mod_newt_step_scalar(const argument & x, argument & x_next, function&& f, jacobian&& df) {
    argument y = x + f(x) / df(x);
    x_next = y - f(y) / df(x);
}

//! \brief Implements a single step of the modified newton
//! \tparam argument type of argument to function f: such as double or vector etc...
//! \tparam function type for the function f, likely a lambda function
//! \tparam jacobian type for the jacobian df, likely a lambda function
//! \param[in] x previous value to use in step, also initial guess if needed
//! \param[out] x_next next step x_{k+1} of modified Newton
//! \param[in] f function handle for f(x) = 0
//! \param[in] df function handle for jacobian df of f
template <typename argument, class function, class jacobian>
void mod_newt_step_system(const argument & x, argument & x_next, function&& f, jacobian&& df) {
    auto lu = df(x).lu();
    // reusing LU decomposition in the latter
    // the constructor performs LU decomposition in case argument = Vector
    argument y = x + lu.solve(f(x));
    x_next = y - lu.solve(f(y));
}

//! \brief Solve a scalar non-linear eq. with the modified Newton
void mod_newt_ord() {
    // Setting up values, functions and jacobian
    const double a = 0.123;
    auto f = [&a] (double x) { return atan(x) - a; }; // function, must see a
    auto df = [] (double x) { return 1. / (x*x+1.); };

    const double x_ex = tan(a); // exact solution

    // store solution and error at each time step
    std::vector<double> sol;
    std::vector<double> err;

    // Compute error and push back to err, used in general_nonlinear solver as breaking condition errf(x) < eps
    auto errf = [&err, x_ex] (double & x) { double e =  std::abs(x - x_ex); err.push_back(e); return e; };

    // Perform convergence study with Modified newton for scalar
    std::cout << std::endl << "*** Modified Newton method (scalar) ***" << std::endl << std::endl;
    std::cout << "Exact: " << x_ex << std::endl;
    // Initial guess and compute initial error
    double x_scalar = 5;
    sol.push_back(x_scalar);
    errf(x_scalar);
    // Lambda performing the next step, used to define a proper function handle to be passed to general_nonlinear_solver
    auto newt_scalar_step = [&sol, &f, &df] (double x, double & x_new) { mod_newt_step_scalar(x, x_new, f, df); sol.push_back(x_new); };
    // Actually perform the solution
    general_nonlinear_solver(newt_scalar_step, x_scalar, errf);
    // Print solution (final)
    std::cout << std::endl << "x^*_scalar = " << std::endl << x_scalar << std::endl;

    // Print table of solutions, errors and EOOC
    auto space = std::setw(15);
    std::cout << std::endl << space << "sol." << space << "err." << space << "order" << std::endl;
    for(unsigned i = 0; i < sol.size(); ++ i) {
        std::cout << space << sol.at(i) << space << err.at(i);
        if(i >= 3) std::cout << space << (log(err.at(i)) - log(err.at(i-1))) / (log(err.at(i-1)) - log(err.at(i-2)));
        std::cout << std::endl;
    }
}

//! \brief Solve a system non-linear eq. with the modified Newton
void mod_newt_sys() {
    //// Setup initial values
    using Vector = Eigen::VectorXd;
    using Matrix = Eigen::MatrixXd;

    Matrix A = Matrix::Random(4,4);
    Vector c = Vector::Random(4);

    A = (A*A.transpose()).eval(); // make sure A is symmetric
    c = c.cwiseAbs(); // make sure c is > 0 uniformly

    // Handler for function and jacobian in standard format. Must see matrix A and vector c
    auto F = [&A,&c] (const Vector & x) { Vector tmp =  A*x + c.cwiseProduct(x.array().exp().matrix()).eval(); return tmp; };
    std::function<Matrix(const Vector &)> dF = [&A, &c] (const Vector & x) { Matrix C = A; Vector temp = c.cwiseProduct(x.array().exp().matrix()); C += temp.asDiagonal(); return C; };

    // Container for errors
    std::vector<double> err;
    // Define lambda for breaking condition, which also stores the error of the previous step. Must see err
    auto rerr = [&err] (Vector & x) { if(err.size() > 0) return err.back(); else return 1.; };

    std::cout << std::endl << "*** Modified Newton method (system) ***" << std::endl << std::endl;
    Vector x_system = Vector::Zero(c.size()); // Initial guess

    // Refactor the step st. is compatible with general_nonlinear_solver,  must see F; dF and error vector
    auto newt_system_step = [&F, &dF, &err] (const Vector & x, Vector & x_new) { mod_newt_step_system(x, x_new, F, dF); double e = (x - x_new).norm()/ x_new.norm(); err.push_back(e); };
    // Actually performs computations
    general_nonlinear_solver(newt_system_step, x_system, rerr);

    // Sanity check
    std::cout << std::endl << "x^*_system = " << std::endl << x_system << std::endl;
    std::cout << std::endl << "F(x^*_system) = " << std::endl << F(x_system) << std::endl;

}

int main() {
    // Part 1: solve scalar
    mod_newt_ord();

    // Part 2: solve system
    mod_newt_sys();

}

Nummerical Methods for CSE/PS5/solutions_ps5/modnewt.cpp


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#include <Eigen/Core>

namespace warp_perspective {

Eigen::MatrixXf get_2D_to_3D(int width, int height) {
  Eigen::MatrixXf A1(4,3);
  A1 << 1, 0,  -1*(float)width/2,
        0, 1,  -1*(float)height/2,
        0, 0,         1,
        0, 0,         1;
  return A1;
}

Eigen::MatrixXf get_rotation(float theta, float phi, float gamma) {
  Eigen::Matrix4f Rx, Ry, Rz, R;
  Rx << 1,               0,                0, 0,
        0, std::cos(theta), -1*std::sin(theta), 0,
        0, std::sin(theta),  std::cos(theta), 0,
        0,               0,                0, 1;
  Ry << std::cos(phi), 0, -1*std::sin(phi), 0,
                    0, 1,              0, 0,
        std::sin(phi), 0,  std::cos(phi), 0,
                    0, 0,              0, 1;
  Rz << std::cos(gamma), -1*std::sin(gamma), 0, 0,
        std::sin(gamma),  std::cos(gamma), 0, 0,
                      0,                0, 1, 0,
                      0,                0, 0, 1;
  R = (Rx * Ry) * Rz;
  return R;
}

Eigen::MatrixXf get_translation(int dx, int dy, int dz) {
  Eigen::Matrix4f T;
  T << 1, 0, 0, dx,
       0, 1, 0, dy,
       0, 0, 1, dz,
       0, 0, 0, 1;
  return T;
}

Eigen::MatrixXf get_3D_to_2D(float focal, int width, int height) {
  Eigen::MatrixXf A2(3,4);
  A2 << focal,     0,  (float)width/2, 0,
            0, focal, (float)height/2, 0,
            0,     0,               1, 0;
  return A2;
}

Eigen::Matrix<float, 3, 3> get_transformation_matrix(
  int width, int height,
  float theta, float phi, float gamma,
  int dx, int dy, int dz,
  float focal) {
  Eigen::MatrixXf A1  = get_2D_to_3D(width, height);
  Eigen::Matrix4f R   = get_rotation(theta, phi, gamma);
  Eigen::Matrix4f T   = get_translation(dx, dy, dz);
  Eigen::MatrixXf A2  = get_3D_to_2D(focal, width, height);
  return (A2 * (T * (R * A1)));
}

}


#include "writer.hpp"
#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <stdexcept>
//! Vector type
typedef Eigen::VectorXd Vector;

/// Uses forward Euler and upwind finite differences to compute u from time 0 to time T
/// Propagation of information is assumed to be from left to right
///
/// @param[out] u solution at all time steps up to time T, each column corresponding to a time step (including the initial condition as first column)
/// @param[out] time the time levels
/// @param[in] u0 the initial conditions, as column vector
/// @param[in] dt the time step size
/// @param[in] T the final time at which to compute the solution (which we assume to be a multiple of dt)
/// @param[in] N the number of grid points, INCLUDING BOUNDARY POINTS
/// @param[in] a the advection velocity
///
void SimpleUpwindFD(Eigen::MatrixXd &u, Vector &time, const Vector u0, double dt, double T, int N, const std::function<double(double)> &a) {
	const unsigned int nsteps = round(T / dt);
	const double       dx     = 1. / (N - 1);
	u.resize(N, nsteps + 1);
	time.resize(nsteps + 1);

	// (write your solution here)
	u.col(0) = u0;
	time(0)  = 0;
	for (unsigned int t = 0; t < nsteps - 1; ++t) {
		time(t + 1) = (t + 1) * dt;
		for (int x = 1; x < N - 2; ++x) {
			u(x, t + 1) = u(x, t) - dt * (std::max(a(x / dx), 0.0) * (u(x, t) - u(x - 1, t)) / dx - std::min(a(x / dx), 0.0) * (u(x + 1, t) - u(x, t)) / dx);
		}
		u(0, t + 1)     = u(N - 2, t + 1);
		u(N - 1, t + 1) = u(1, t + 1);
	}
}

/// Uses forward Euler and upwind finite differences to compute u from time 0 to time T
///
/// @param[out] u solution at all time steps up to time T, each column corresponding to a time step (including the initial condition as first column)
/// @param[out] time the time levels
/// @param[in] u0 the initial conditions, as column vector
/// @param[in] dt the time step size
/// @param[in] T the final time at which to compute the solution (which we assume to be a multiple of dt)
/// @param[in] N the number of grid points, INCLUDING BOUNDARY POINTS
/// @param[in] a the advection velocity
///

void UpwindFD(Eigen::MatrixXd &u, Vector &time, const Vector u0, double dt, double T, int N, const std::function<double(double)> &a) {
	//const unsigned int nsteps = round(T / dt);
	//const double       dx     = 1. / (N - 1);
	//u.resize(N, nsteps + 1);
	//time.resize(nsteps + 1);

	// (write your solution here)
	SimpleUpwindFD(u, time, u0, dt, T, N, a);
}

///
/// Uses forward Euler and centered finite differences to compute u from time 0 to time T
///
/// @param[out] u solution at all time steps up to time T, each column corresponding to a time step (including the initial condition as first column)
/// @param[out] time the time levels
/// @param[in] u0 the initial conditions, as column vector
/// @param[in] dt the time step size
/// @param[in] T the final time at which to compute the solution (which we assume to be a multiple of dt)
/// @param[in] N the number of grid points, INCLUDING BOUNDARY POINTS
/// @param[in] a the advection velocity
///

void CenteredFD(Eigen::MatrixXd &u, Vector &time, const Vector u0, double dt, double T, int N, const std::function<double(double)> &a) {
	const unsigned int nsteps = round(T / dt);
	const double       dx     = 1. / (N - 1);
	u.resize(N, nsteps + 1);
	time.resize(nsteps + 1);

	/* Initialize u */
	u.col(0) << u0;
	time[0] = 0;
	// (write your solution here)
	for (unsigned int t = 0; t < nsteps; ++t) {
		for (int x = 1; x < N - 1; ++x) {
			u(x, t + 1) = u(x, t) / dt - a(x / dx) * (u(x + 1, t) - u(x - 1, t)) / (2 * dx);
		}
		u(0, t + 1)     = u(N - 2, t + 1);
		u(N - 1, t + 1) = u(1, t + 1);
	}
}

/* Initial condition: rectangle */
double U0(double x) {
	if (x < 0.25 || x > 0.75)
		return 0;
	else
		return 2.;
}

int main(int, char **) {
	double T  = 2.;
	double dt = 0.002; // Change this for timestep comparison
	int    N  = 101;

	auto a = [](double x) {
		return std::sin(2 * M_PI * x);
	};

	Vector u0(N);
	double h = 1. / (N - 1);
	/* Initialize u0 */
	for (int i = 0; i < u0.size(); i++)
		u0[i]    = U0(h * (double) i);

	Eigen::MatrixXd u_simpleupwind;
	Vector          time_simpleupwind;
	SimpleUpwindFD(u_simpleupwind, time_simpleupwind, u0, dt, T, N, a);
	writeToFile("time_simpleupwind.txt", time_simpleupwind);
	writeMatrixToFile("u_simpleupwind.txt", u_simpleupwind);

	Eigen::MatrixXd u_upwind;
	Vector          time_upwind;
	UpwindFD(u_upwind, time_upwind, u0, dt, T, N, a);
	writeToFile("time_upwind.txt", time_upwind);
	writeMatrixToFile("u_upwind.txt", u_upwind);

	Eigen::MatrixXd u_centered;
	Vector          time_centered;
	CenteredFD(u_centered, time_centered, u0, dt, T, N, a);
	writeToFile("time_centered.txt", time_centered);
	writeMatrixToFile("u_centered.txt", u_centered);
}


//
//  main.cpp
//  Ransac
//
//  Created by JiangZhigang on 2020/11/29.
//  Copyright © 2020 JiangZhigang. All rights reserved.
//

#include <iostream>
#include <iomanip>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Dense>

#include <random>
#include <set>

std::vector<cv::Point2f> generate_data(float k, float b, int n, float line_noise_level = 10, float random_noise_count = 200, bool add_ellipse = false){
    std::default_random_engine e(static_cast<unsigned int>(time(nullptr)));
    std::uniform_int_distribution<> u(-line_noise_level, line_noise_level);

    std::vector<cv::Point2f> points;
    for(int i=-n; i<n; i++){
        points.push_back(cv::Point2f(i, k*i + b + u(e)));

        if(!add_ellipse) continue;
        float a_ = 300, b_ = 200;
        float q = 1 - i * i / (a_ * a_);
        if(q > 0){
            float y = sqrt(b_*b_ * q);
            float r = atan(k);
            float c = cos(r);
            float s = sin(r);

            auto p1 = cv::Point2f(i, y);
            p1 = cv::Point2f(p1.x * c + p1.y * (-s), p1.x * s + p1.y * c + b);
            points.push_back(p1);
            auto p2 = cv::Point2f(i, -y);
            p2 = cv::Point2f(p2.x * c + p2.y * (-s), p2.x * s + p2.y * c + b);
            points.push_back(p2);
        }
    }

    std::uniform_int_distribution<> u_(-n, n);
    for(int i=0; i<random_noise_count; i++){
        points.push_back(cv::Point2f(u_(e), u_(e)));
    }
    std::shuffle(points.begin(), points.end(), e);
    return points;
}

void draw_data(cv::Mat& board, const std::vector<cv::Point2f>& points){
    int h = board.cols, w =  board.rows;
    for(auto const& point : points){
        cv::Point2f p = cv::Point2f(h/2+point.x, w/2-point.y);//origin change center
        cv::circle(board, p, 1, cv::Scalar(255, 255, 255), -1);
    }
}

void draw_line(cv::Mat& board, float k, float b, cv::Scalar color){
    int h = board.cols, w =  board.rows;
    cv::Point2f p1 = cv::Point2f(-h, -w * k + b);
    p1 = cv::Point2f(h/2+p1.x, w/2-p1.y);
    cv::Point2f p2 = cv::Point2f(h, w * k + b);
    p2 = cv::Point2f(h/2+p2.x, w/2-p2.y);
    cv::line(board, p1, p2, color, 1);
}

float calc_error(const cv::Point2f& point, float k, float b){
    // distance of point to line
    float A = k, B = -1, C = b;
    return abs(A * point.x + B * point.y + C) / sqrt(A * A + B * B);
}

float calc_error_y2(const cv::Point2f& point, float k, float b){
    return pow(abs(b + point.x * k - point.y), 2);
}

double calc_loss(const std::vector<cv::Point2f>& points, double w1, double w2){
    // MSE
    double sse = 0;
    for (auto const& point: points) {
        double se = calc_error_y2(point, w2, w1);
        sse += se;
    }
    double mes = sse / (2 * points.size());
    return mes;

}

/// use the Eigen libary to implement the least square method
/// @param points a set of observed data points
/// @param k_ k of model parameters which best fit the data
/// @param b_ b of model parameters which best fit the data
/// @return whether a good model has been found
bool least_squares(const std::vector<cv::Point2f>& points, float& k_, float& b_){
    Eigen::MatrixXd A(points.size(), 2);
    Eigen::VectorXd b(points.size());

    int i = 0;
    for(auto const& point : points){
        A.row(i) = Eigen::Vector2d(point.x, 1);
        b(i) = point.y;
        i++;
    }
    // Eigen provides multiple solver
    // Eigen::MatrixXd solution = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    Eigen::VectorXd solution = A.colPivHouseholderQr().solve(b);

    if(solution.size()  == 2){
        k_ = solution(0);
        b_ = solution(1);
        return true;
    }
    return false;
}

/// use the Eigen libary to implement the least square method
/// @param points a set of observed data points
/// @param k_ k of model parameters which best fit the data
/// @param b_ b of model parameters which best fit the data
/// @return whether a good model has been found
bool least_squares_b(const std::vector<cv::Point2f>& points, float& k_, float& b_){
    Eigen::MatrixXd A(points.size(), 2);
    Eigen::VectorXd b(points.size());

    int i = 0;
    for(auto const& point : points){
        A.row(i) = Eigen::Vector2d(point.x, 1);
        b(i) = point.y;
        i++;
    }

    Eigen::MatrixXd ATA = A.transpose() * A;
    auto rank =  Eigen::FullPivLU<Eigen::MatrixXd>(ATA).rank();
    if(rank < ATA.rows()){
        std::cout << "rank of A^TA is " << rank << ", can't directly get inverse" << std::endl;
        return false;
    }

    Eigen::VectorXd solution = ATA.inverse() * A.transpose() * b;
    if(solution.size()  == 2){
        k_ = solution(0);
        b_ = solution(1);
        return true;
    }
    return false;
}

/// implement the least square method from [there](https://blog.csdn.net/pl20140910/article/details/51926886)
/// @param points a set of observed data points
/// @param k_ k of model parameters which best fit the data
/// @param b_ b of model parameters which best fit the data
/// @return whether a good model has been found
bool least_squares_a(const std::vector<cv::Point2f>& points, float& k_, float& b_){
    int n = (int)points.size();
    float A=0,B=0,C=0,D=0;
    for(auto const& point : points){
        A += point.x * point.x;
        B += point.x;
        C += point.x * point.y;
        D += point.y;
    }

    float temp = (n*A - B*B);
    if(temp){
        k_ = (n*C - B*D) / temp;
        b_ = (A*D - B*C) / temp;
        return true;
    }else{
        return false;
    }
}

/// standard RANSAC algorithm from [wikipedia](https://en.wikipedia.org/wiki/Random_sample_consensus)
/// @param data a set of observed data points
/// @param k_ k of model parameters which best fit the data
/// @param b_ b of model parameters which best fit the data
/// @param n the minimum number of data values required to fit the model
/// @param k the maximum number of iterations allowed in the algorithm
/// @param t a threshold value for determining when a data point fits a model
/// @param d the number of close data values required to assert that a model fits well to data
/// @return whether a good model has been found
bool ransac(const std::vector<cv::Point2f>& data, float& k_, float& b_, int n = 10, int k = 1000, float t = 10, int d = 10){
    int c = (int)data.size();
    std::default_random_engine e(static_cast<unsigned int>(time(nullptr)));
    std::uniform_int_distribution<> u(0, c-1);

    float best_error = FLT_MAX;
    float best_model_k = 0, best_model_b = 0;

    std::vector<int> best_indexs;
    int it = 0;
    while (it++ < k) {
        // n randomly selected values from data
        int maybe_it = 0;
        std::vector<bool> visited(c, false);
        std::vector<cv::Point2f> maybe_inliers;
        while(true && maybe_it++ < k ){
            int i = u(e);
            if(visited[i]) continue;

            maybe_inliers.push_back(data[i]);
            visited[i] = true;

            if(maybe_inliers.size() == n) break;
        }
        if(maybe_inliers.empty()) return false;

        // model parameters fitted to maybeinliers
        float maybe_model_k = 0, maybe_model_b = 0;
        least_squares(maybe_inliers, maybe_model_k, maybe_model_b);
        std::vector<cv::Point2f> also_inliers;

        for(int i = 0; i < c; i++){
            if(visited[i]) continue;// every point in data not in maybeinliers
            float error = calc_error(data[i], maybe_model_k, maybe_model_b);
            if(error > t) continue;//point fits maybemodel with an error smaller than t
            also_inliers.push_back(data[i]);//add point to alsoinliers
        }
//        std::cout << also_inliers.size() << std::endl;
        if(also_inliers.size() <= d) continue;//the number of elements in alsoinliers is > d

        // this implies that we may have found a good model
        // now test how good it is

        // parameters fitted to all points in maybeinliers and alsoinliers
        also_inliers.insert(also_inliers.end(), maybe_inliers.begin(), maybe_inliers.end());
        float better_model_k = 0, better_model_b = 0;
        least_squares(also_inliers, better_model_k, better_model_b);

        // measure of how well model fits these points
        float this_error = 0;
        for(int i = 0; i < also_inliers.size(); i++){
            this_error += calc_error(also_inliers[i], better_model_k, better_model_b);
        }
        this_error = this_error/also_inliers.size();

        bool better = this_error < best_error;
        if(better){
            best_model_k = better_model_k;
            best_model_b = better_model_b;
            best_error = this_error;
        }
    }

    k_ = best_model_k;
    b_ = best_model_b;
    return best_error != FLT_MAX;
}

/// gradient descent
/// @param data a set of observed data points
/// @param k_ k of model parameters which best fit the data
/// @param b_ b of model parameters which best fit the data
/// @param alpha learing rate
/// @param k the maximum number of iterations allowed in the algorithm
/// @param min_loss  the loss of close data values required to assert that a model fits well to data
bool gradient_descent(const std::vector<cv::Point2f>& data, float& k_, float& b_, double alpha = 0.1, int k = 1000, double min_loss = 0.001){
    int c = (int)data.size();
    double loss = FLT_MAX;
    double loss_pre = loss;

    int it = 0;
    double w1 = 0, w2 = 0;
    do {
        // the partial derivative of w1,w2
        double g1 = 0, g2 = 0;
        for(int i=0; i<c; i++){
            g1 = g1 + w1 + w2 * data[i].x - data[i].y;
            g2 = g2+ (w1  + w2 * data[i].x  - data[i].y) * data[i].x;
        }

        w1 = w1 - alpha * g1 / c;// update w1
        w2 = w2 - alpha * g2 / c;// update w2

        loss_pre = loss;
        loss = calc_loss(data, w1, w2);

    }while(abs(loss - loss_pre) >= min_loss && it++ < k);

    // std::cout<<"gradient_descent loss it:" << it << " loss:"<< loss << std::endl;
    k_ = w2;
    b_ = w1;
    return loss != FLT_MAX && !isinf(loss);
}

int main(int argc, const char * argv[]) {
    std::cout << std::fixed << std::setprecision(8);

    float k = 0.5;
    float b = 20;
    cv::Mat board = cv::Mat(800, 800, CV_8UC3);

    std::vector<cv::Point2f> points = generate_data(k, b, 300, 10, 400, true);

    std::cout<< "original   k:" << k << " b:" << b << std::endl;
    draw_data(board, points);
    draw_line(board, k, b, cv::Scalar(255,255,255));

    float k_, b_;
//    if(least_squares_a(points, k_, b_)){
//        std::cout<< "least squares a:" << k_ << " b:" << b_ << std::endl;
//        draw_line(board, k_, b_, cv::Scalar(0,255,0));
//    }
//
//    if(least_squares_b(points, k_, b_)){
//        std::cout<< "least squares b:" << k_ << " b:" << b_ << std::endl;
//        draw_line(board, k_, b_, cv::Scalar(0,255,0));
//    }

    if(least_squares(points, k_, b_)){
          std::cout<< "least squares:" << k_ << " b:" << b_ << std::endl;
          draw_line(board, k_, b_, cv::Scalar(0,255,0));
      }

    if(ransac(points, k_, b_, 10, 1000, 50, 100)){
        std::cout<< "ransac k:" << k_ << "b:" << b_ << std::endl;
        draw_line(board, k_, b_, cv::Scalar(0,0,255));
    }

    if(gradient_descent(points, k_, b_, 0.00004, 1e8, 1e-8)){
        std::cout<< "gradient_descent k:" << k_ << "b:" << b_ << std::endl;
        draw_line(board, k_, b_, cv::Scalar(0,255,255));
    }

    cv::imshow("board", board);
    cv::waitKey();
    return 0;
}


/*
 * =====================================================================================
 *
 *       Filename:  1.cpp
 *
 *    Description: Demo Program for Eigen C++
 *    Library.
 *
 *    Version:  1.0
 *        Created:  08/28/2021 03:09:00 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Dhruva Gole (181030017), goledhruva@gmail.com
 *   Organization:  VJTI
 *
 * =====================================================================================
 */

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

int main()
{
	Eigen::MatrixXd m(3,3);
	Eigen::Matrix3d a;
	a << 24, -4, -4,
	  	 -4, 24, -4,
		 -4, -4,  4;
  m << a.inverse();
  std::cout << "Matrix a = \n" << a << std::endl;
  std::cout << "Addition of two matrices: a + (inverse a)\n" << a+m << std::endl;
  std::cout << "And product is:\n" << a*m << std::endl;
  std::cout << "The Determinant of a is:\n" << a.determinant() << std::endl;
  return 0;
}


#include<iostream>
#include <Eigen/Dense>
#include <math.h>
#include <vector>
#include <Eigen/QR>

using namespace std;

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

   Eigen:: MatrixXf m =  Eigen::MatrixXf::Random(3,7);
std::cout << "Here is the matrix m:" << std::endl << m << std::endl;
 Eigen::JacobiSVD< Eigen:: MatrixXf> svd(m,Eigen::ComputeThinU |  Eigen::ComputeThinV);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its left singular vectors are the columns of the thin U matrix:" << endl << svd.matrixU() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
 Eigen::Vector3f rhs(1, 0, 0);
cout << "Now consider this rhs vector:" << endl << rhs << endl;
cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(rhs) << endl;
    return 0;
}


#include <codegenvar/Eigen>
#include <Eigen/Geometry>
#include <iostream>

template<class T, int n>
using Vec = Eigen::Matrix<T, n, 1>;

// https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
template <class T>
void rotate_rodrigues(const Vec<T, 3>& k, const T& theta, const Vec<T, 3>& v, Vec<T, 3>& v_rot)
{
    v_rot = v*cos(theta) + k.cross(v)*sin(theta) + k*k.dot(v)*(1-cos(theta));
}

using namespace codegenvar;

bool equals(const Vec3& a, const Vec3& b)
{
    for(int i = 0; i<3; i++)
        if (!a(i).equals(b(i)))
            return false;
    return true;
}

// main:
int main()
{
    Symbol angle("theta");
    Vec3 axis = namedVector<3>("k"), point = namedVector<3>("v");

    Vec3 r1;

    // Use Rodrigues' rotation formula to calculate the rotation of the point:
    rotate_rodrigues(axis, angle, point, r1);
    std::cout << "Rodrigues:\n" << r1 << std::endl;

    // Use Eigen to calculate the rotation of the point:
    Vec3 r = Eigen::AngleAxis<Symbol>(angle, axis)*point;
    std::cout << "\nEigen:\n"  << r << std::endl;

    // Check for mathematical equality:
    std::cout << "\nThe two expressions are "
              << (equals(r1, r) ? "equal" : "not equal")
              << "." << std::endl;

    // If we convert all expressions to their expanded form, we can see that they are identical.
    std::cout << "\nExpanded:\n";

    std::cout << "\nRodrigues:\n" << expanded(r1) << std::endl;
    std::cout << "\nEigen:\n"  << expanded(r) << std::endl;

    return 0;
}