//=============================================================================
// Daniel J. Greenhoe
// normed linear space R^2
//=============================================================================
//=====================================
// headers
//=====================================
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <vector>
#include <complex>
#include <Eigen/Dense>
#include "r1.h"
#include "r2.h"
#include "r2op.h"

//-----------------------------------------------------------------------------
//! List value of opair
//! \param[in]  str1 : string to print before the sequence
//! \param[in]  str2 : string to print after the sequence
//! \param[out] ptr  : output string
//-----------------------------------------------------------------------------
void opair::list(const char *str1, const char *str2, FILE *ptr) const
{
  if( strlen(str1)>0 ) fprintf(ptr,"%s",str1);
  fprintf(ptr,"(%9.6lf,%9.6lf)",getx(),gety());
  if( strlen(str2)>0 ) fprintf(ptr,"%s",str2);
}

//-----------------------------------------------------------------------------
//! \brief polar rotation coordinate <theta> of opair point (x,y)
//! \returns Return value is in the half open interval [0:2pi),
//!          -1 on error
//-----------------------------------------------------------------------------
double vectR2::theta(void) const
{
  const double x = getx();
  const double y = gety();
  if(x==0){
    if(     y == 0 ) return -1           ;
    else if(y > 0  ) return  1 * M_PI / 2;
    else             return  3 * M_PI / 2;
  }
  if(y==0){
    if(     x>0    ) return 0;
    else             return M_PI;
  }
  if( x>0 && y>0 ) return        atan( y/x); // 1st quadrant
  if( x>0 && y<0 ) return 2*M_PI-atan(-y/x); // 4th quadrant
  if( x<0 && y>0 ) return   M_PI-atan(-y/x); // 2nd quadrant
  if( x<0 && y<0 ) return   M_PI+atan( y/x); // 3rd quadrant
  else             return -1;
}

//-----------------------------------------------------------------------------
//! \brief Operator to rotate vector R2 [x,y] counter-clockwise by <phi> radians
//! \param[in]  phi: angle to rotate vector by
//-----------------------------------------------------------------------------
void vectR2::operator&=(const double phi)
{
  vectR2  p(getx(),gety());
  p = p & phi;
  put(p.getx(),p.gety());
}

//=====================================
// seqR2
//=====================================
//-----------------------------------------------------------------------------
//! \brief Constructor initializing seqR1 to 0s
//! \param[in]  M: Length of sequence
//-----------------------------------------------------------------------------
seqR2::seqR2(const long M)
{
  N  = M;
  xy = new vectR2[N];
  clear();
}

//-----------------------------------------------------------------------------
//! \brief constructor initializing seqR1 to u
//! \param[in]  M: Length of sequence
//! \param[in]  u: Initialization value
//-----------------------------------------------------------------------------
seqR2::seqR2(long M, double u)
{
  long n;
  N=M;
  xy = new vectR2[N];
  for(n=0; n<N; n++)
  {
    xy[n].put(u,u);
  }
}

//-----------------------------------------------------------------------------
//! \brief Fill the sequence with a value 0
//-----------------------------------------------------------------------------
void seqR2::clear(void)
{
  for( long n=0; n<N; n++ ) xy[n].clear();
}

//-----------------------------------------------------------------------------
//! \brief Fill the sequence with a value <u>
//-----------------------------------------------------------------------------
void seqR2::fill(double u)
{
  long n;
  for(n=0; n<N; n++) xy[n].put(u,u);
}

//-----------------------------------------------------------------------------
//! \brief Fill the sequence with (x_0, x_1, x_2, ...)
//!        where x_n = x_{n-1} + (dx,dy)
//-----------------------------------------------------------------------------
void seqR2::inc(double x0, double y0,double dx, double dy)
{
  for( long n=0; n<N; n++ )
  {
    xy[n].put(x0,y0);
    x0 += dx;
    y0 += dy;
  }
}

//-----------------------------------------------------------------------------
//! \brief Put an order pair (u,v) into the sequence x at location n
//-----------------------------------------------------------------------------
int seqR2::put(long n, double u, double v)
{
  int retval=0;
  if(n>=N){
    fprintf(stderr,"n=%ld larger than seqR1 size N=%ld\n",n,N);
    retval=-1;
    }
  else xy[n].put(u,v);
  return retval;
}

//-----------------------------------------------------------------------------
//! \brief Put an R2 vector xyz into the sequence x at location n
//-----------------------------------------------------------------------------
int seqR2::put(long n, vectR2 xya)
{
  int retval=0;
  if(n>=N){
    fprintf(stderr,"n=%ld larger than seqR1 size N=%ld\n",n,N);
    retval=-1;
    }
  else xy[n]=xya;
  return retval;
}

//-----------------------------------------------------------------------------
//! Get a single value from the sequence x at location n
//-----------------------------------------------------------------------------
vectR2 seqR2::get(const long n) const
{
  vectR2 xya(0,0);
  if(n<N)xya=xy[n];
  else   fprintf(stderr,"n=%ld larger than seqR1 size N=%ld\n",n,N);
  return xya;
}

//-----------------------------------------------------------------------------
//! \brief Get a single value from the sequence x element x at location n
//-----------------------------------------------------------------------------
double seqR2::getx(const long n) const
{
  double u=0;
  if(n<N)u=xy[n].getx();
  else   fprintf(stderr,"n=%ld larger than x seqR1 size N=%ld\n",n,N);
  return u;
}

//-----------------------------------------------------------------------------
//! \brief Get a single value from the sequence x element y at location n
//-----------------------------------------------------------------------------
double seqR2::gety(const long n) const
{
  double u=0;
  if(n<N)u=xy[n].gety();
  else   fprintf(stderr,"n=%ld larger than y seqR1 size N=%ld\n",n,N);
  return u;
}

//-----------------------------------------------------------------------------
//! \brief List contents of sequence
//-----------------------------------------------------------------------------
void seqR2::list(const long start, const long end, const char *str1, const char *str2, FILE *ptr) const
{
  long n,m;
  vectR2 x;
  if(strlen(str1)>0)fprintf(ptr,"%s",str1);
  for(n=start,m=1; n<=end; n++,m++){
    x=get(n);
    fprintf(ptr,"  ");
    x.list(ptr);
    if(m%3==0)fprintf(ptr,"\n");
    }
  if(strlen(str2)>0)fprintf(ptr,"%s",str2);
}

//-----------------------------------------------------------------------------
//! \brief List contents of seqC1 using 1 digit per element
//-----------------------------------------------------------------------------
void seqR2::list1(void) const
{
  long n,m;
  for(n=0,m=1; n<N; n++,m++){
    printf("(%2.0lf,%2.0lf)   ",getx(n),gety(n));
    if(m%5==0)printf("\n");
    }
}

//-----------------------------------------------------------------------------
//! \brief List contents of seqC1 using 1 digit per element
//-----------------------------------------------------------------------------
void seqR2::list1(long start, long end) const
{
  long n,m;
  for(n=start,m=1; n<=end; n++,m++){
    printf("(%2.0lf,%2.0lf)   ",getx(n),gety(n));
    if(m%50==0)printf("\n");
    else if(m%10==0)printf(" ");
    }
}

//-----------------------------------------------------------------------------
//! \brief Return the largest pair of values in the sequence as measured by norm()
//-----------------------------------------------------------------------------
double seqR2::norm(const long n) const
{
  vectR2 xya=get(n);
  return xya.norm();
}

//-----------------------------------------------------------------------------
//! \brief Return the largest pair of values in the sequence as measured by norm()
//-----------------------------------------------------------------------------
vectR2 seqR2::max(const int verbose) const
{
  long n;
  double maxnorm=0;
  long   maxn=0;
  vectR2  maxpair;
  for(n=0; n<N; n++)if(norm(n)>maxnorm){maxnorm=norm(n); maxn=n;}
  maxpair=get(maxn);
  if(verbose)
  {
    for(n=0; n<N; n++)
    {
      if(norm(n)>=(maxnorm*0.999))
        printf("max=(%lf,%lf) at n=%ld\n",maxpair.getx(),maxpair.gety(),n);
    }
  }
  return maxpair;
}

//=====================================
// external operations
//=====================================
//-----------------------------------------------------------------------------
//! \brief Operator: return p+q
//-----------------------------------------------------------------------------
vectR2 operator+(const vectR2 p, const vectR2 q)
{
  const Eigen::Map< const Eigen::Vector2d > a( p.getdata() );
  const Eigen::Map< const Eigen::Vector2d > b( q.getdata() );
  const Eigen::Vector2d c = a + b;
  const vectR2 r( c(0), c(1) );
  return r;
}

//-----------------------------------------------------------------------------
//! \brief Operator: return p-q
//-----------------------------------------------------------------------------
vectR2 operator-(const vectR2 p, const vectR2 q)
{
  const Eigen::Map< const Eigen::Vector2d > a( p.getdata() );
  const Eigen::Map< const Eigen::Vector2d > b( q.getdata() );
  const Eigen::Vector2d c = a - b;
  const vectR2 r( c(0), c(1) );
  return r;
}

//-----------------------------------------------------------------------------
//! \brief Operator: return -p
//-----------------------------------------------------------------------------
vectR2 operator-(const vectR2 p)
{
  const Eigen::Map< const Eigen::Vector2d > a( p.getdata() );
  const Eigen::Vector2d b = -a;
  const vectR2 c( b(0), b(1) );
  return c;
}

//-----------------------------------------------------------------------------
//!  \brief operator: return <p> rotated counter-clockwise by <phi> radians
//!  \details
//!    https://stackoverflow.com/questions/17036818/
//!    https://eigen.tuxfamily.org/dox/group__TopicStorageOrders.html
//!    https://stackoverflow.com/questions/28722899/
//-----------------------------------------------------------------------------
vectR2 operator&(const vectR2 p, const double phi)
{
  double cosphi, sinphi;
  if(phi==0){ cosphi = 1;        sinphi = 0;        }
  else      { cosphi = cos(phi); sinphi = sin(phi); }
  const std::vector<double> rr = { cosphi, -sinphi,
                                   sinphi,  cosphi  };
  const Eigen::Matrix<double, 2, 2, Eigen::RowMajor> R(rr.data());
//printf("     _                    _\n");
//printf("R = | %9.6lf %9.6lf  |\n", R(0,0), R(0,1) );
//printf("    |_%9.6lf %9.6lf _|\n", R(1,0), R(1,1) );
//const Eigen::Map< const Eigen::Vector2d > x( p.pairxy.data() );
  const Eigen::Map< const Eigen::Vector2d > x( p.getdata() );
  const Eigen::Vector2d y = R * x;
  const vectR2 q( y(0), y(1) );
  return q;
}

//-----------------------------------------------------------------------------
//! \brief Return the angle theta in radians between the two vectors induced by
//!        the points <p> and <q> in the plane R^2
//! \returns On SUCCESS return theta in the closed interval [0:M_PI];
//!          On ERROR   exit with value EXIT_FAILURE
//-----------------------------------------------------------------------------
double pqtheta(const vectR2 p, const vectR2 q)
{
  const double rp=p.mag(), rq=q.mag();
  double y;
  if(rp==0) return -1;
  if(rq==0) return -2;
  y = (p^q)/(rp*rq);
  if( y > +1.0 )
  {
    if(y-1 < 1e-13 ) y = 1.0;
    else{
      fprintf(stderr,"\nERROR using pqtheta(vectR2 p, vectR2 q): (p^q)/(rp*rq)=%.12lf>+1\n",y);
      exit(EXIT_FAILURE);
      }
  }
  if(y<(-1.0))
  {
    if(y+1> -1e-13 ) y = -1.0;
    else{
      fprintf(stderr,"\nERROR using pqtheta(vectR2 p, vectR2 q): (p^q)/(rp*rq)=%.12lf<-1\n",y);
      exit(EXIT_FAILURE);
      }
  }
  const double theta = acos(y);
  return theta;
}


#include <cmath>
#include "../matplotlibcpp.h"
namespace plt = matplotlibcpp;

#ifdef WITH_EIGEN
  #include <Eigen/Dense>
#endif

typedef std::vector<std::vector<double>> nested_vectors;

template <typename Matrix>
void get_data(Matrix& x, Matrix& y, Matrix& z) {
  // check the sizes
  const unsigned ncols = x.cols(),
                 nrows = x.rows();
  assert(ncols == y.cols() && ncols == z.cols());
  assert(nrows == y.rows() && nrows == z.rows());

  // write data
  const double a = -5,
               b = 5;
  const double col_incr = (b - a) / ncols,
               row_incr = (b - a) / nrows;
  for (unsigned i = 0; i < nrows; ++i) {
    for (unsigned j = 0; j < ncols; ++j) {
      x(i, j) = i;
      y(i, j) = j;
      z(i, j) = ::std::sin(::std::hypot(a + i * col_incr, a + j * row_incr));
    }
  }
}

template <>
void get_data(nested_vectors& x, nested_vectors& y, nested_vectors& z) {
    for (double i = -5; i <= 5;  i += 0.25) {
        std::vector<double> x_row, y_row, z_row;
        for (double j = -5; j <= 5; j += 0.25) {
            x_row.push_back(i);
            y_row.push_back(j);
            z_row.push_back(::std::sin(::std::hypot(i, j)));
        }
        x.push_back(x_row);
        y.push_back(y_row);
        z.push_back(z_row);
    }
}

int main() {
    std::vector<std::vector<double>> x, y, z;
    get_data(x, y, z);
    plt::plot_surface(x, y, z);
    plt::show();

    #ifdef WITH_EIGEN
      const unsigned n = 100;  // resolution of hypot function
      Eigen::MatrixXd X(n,n), Y(n,n), Z(n,n);
      get_data(X, Y, Z);
      plt::plot_surface(X, Y,   Z);
      plt::show();
    #endif
}


#include <Eigen/SVD> // Eigen is required

template<int R, int C>
static inline void pseudoInverse(float A[], float B[])
{
	typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor | Eigen::AutoAlign, R, C> t_A;

	t_A mA(R, C);

	for (int i = 0; i < R; i++)
	{
		for (int j = 0; j < C; j++)
		{
			mA(i, j) = A[i + j * R];
		}
	}

	Eigen::JacobiSVD<t_A, Eigen::ColPivHouseholderQRPreconditioner> svd(mA, Eigen::ComputeThinU | Eigen::ComputeThinV);

	auto U = svd.matrixU();
	auto S = svd.singularValues();
	auto V = svd.matrixV();

	float Vt[C * C];

	for (int i = 0; i < C; i++)
	{
		float s = S(i);

		if (s != 0.f)
			s = 1.f / s;

		for (int j = 0; j < C; j++)
		{
			Vt[j * C + i] = V(j, i) * s;
		}
	}

	for (int i = 0; i < C; i++)
	{
		for (int j = 0; j < R; j++)
		{
			float v = 0.f;

			for (int k = 0; k < C; k++)
			{
				v += Vt[i * C + k] * U(j, k);
			}

			B[i * R + j] = v;
		}
	}
}

void svd_routine_49_2(float A[], float B[])
{
	pseudoInverse<49, 2>(A, B);
}

void svd_routine_121_8(float A[], float B[])
{
	pseudoInverse<121, 8>(A, B);
}


/**
 * @author      : adamlamson (adamlamson@LamsonMacbookPro)
 * @file        : two_step_prob
 * @created     : Wednesday Jan 29, 2020 14:05:35 MST
 */

#ifndef TWO_STEP_PROB_HPP

#define TWO_STEP_PROB_HPP
#include <boost/math/tools/roots.hpp>
#include <cassert>
#include <cmath>

// Solve the for the time that maximizes the probability of two events occuring
template <class T>
class MaxTimeOfTwoStepProbFunctor {
  public:
    // Constructor just stores values find root of function.
    MaxTimeOfTwoStepProbFunctor(T const &rate1, T const &rate2,
                                T const &total_time)
        : k_ab(rate1), k_bc(rate2), tot_time(total_time) {}

    T operator()(T const &t) {
        T fx = k_ab * (exp(k_bc * (tot_time - t)) - 1.) -
               k_bc * (exp(k_ab * t) - 1.);
        return fx;
    }

  private:
    T k_ab;
    T k_bc;
    T tot_time;
};

template <class T>
T max_time_of_two_step_prob(T const &rate1, T const &rate2,
                            T const &total_time) {

    T guess = .5 * total_time;
    T factor = 2; // How big steps to take when searching.

    const boost::uintmax_t maxit = 20; // Limit to maximum iterations.
    boost::uintmax_t it =
        maxit; // Initally our chosen max iterations, but updated with actual.
    bool is_rising =
        false; // So if guess is too low, then try increasing guess.
    int digits = std::numeric_limits<T>::digits; // Maximum possible binary
                                                 // digits accuracy for type T.
    // Some fraction of digits is used to control how accurate o try to make the
    // result.
    int get_digits =
        digits - 3; // We have to have a non-zero interval at each step, so
                    // maximum accuracy is digits - 1.  But we also have to
                    // allow for inaccuracy in f(x), otherwise the last few
                    // iterations just thrash around.
    boost::math::tools::eps_tolerance<T> rel_tol(
        get_digits); // Set the tolerance.
    std::pair<T, T> t = boost::math::tools::bracket_and_solve_root(
        MaxTimeOfTwoStepProbFunctor<T>(rate1, rate2, total_time), guess, factor,
        is_rising, rel_tol, it);
    return t.first + (t.second - t.first) * .5;
};

template <class T>
T two_step_prob(T const &rate1, T const &rate2, T const &total_time,
                T const &t) {
    assert(total_time >= t);
    T prob = (1. - exp(-1. * rate1 * t)) *
             (1. - exp(-1. * rate2 * (total_time - t)));
    return prob;
};

template <class T>
T two_step_max_prob(T const &rate1, T const &rate2, T const &total_time) {
    T t_max = max_time_of_two_step_prob(rate1, rate2, total_time);
    T prob = two_step_prob(rate1, rate2, total_time, t_max);
    return prob;
};

#endif /* end of include guard TWO_STEP_PROB_HPP */


#pragma once
#include <iostream>
#include <random>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#define EPSILON 1e-6f
#define DELTA 1e-4f
#define PI 3.14159265358979323846f
#define PI_INV 0.31830988618f

namespace mathext {
    std::default_random_engine random_generator;

    std::vector<float> unif(float a, float b, int N = 1)
    {
        std::vector<float> res;
        std::uniform_real_distribution<float> dis(a, b);

        for (int i = 0; i < N; i++) {
            res.push_back(dis(random_generator));
        }
        return res;
    }

    Eigen::Vector2f disk(float r)
    {
        std::vector<float> u = unif(0.0f, 1.0f, 2);

        return Eigen::Vector2f(r * sqrt(u[0]), 2 * PI * u[1]);
    }

    Eigen::Matrix3f rot_from_two_vectors(Eigen::Vector3f src, Eigen::Vector3f des)
    {
        return Eigen::Quaternionf::FromTwoVectors(src, des).toRotationMatrix();
    }

    Eigen::Matrix3f rot_align_normal(Eigen::Vector3f n)
    {
        return rot_from_two_vectors(Eigen::Vector3f(0.0f, 0.0f, 1.0f), n);
    }

    Eigen::Vector3f reflect(Eigen::Vector3f v1, Eigen::Vector3f n)
    {
        return 2 * n.dot(v1) * n - v1;
    }

    Eigen::Vector3f refract(Eigen::Vector3f v1, Eigen::Vector3f n, float ior = 1.0f)
    {
        float cos_theta_i = v1.dot(n);
        float eta = 1 / ior;
        if (cos_theta_i < 0) {
            eta = 1 / eta;
            n *= -1;
            cos_theta_i *= -1;
        }
        float c = sqrt(1 - eta * eta * (1 - cos_theta_i * cos_theta_i));
        return c < 0 ? Eigen::Vector3f(0, 0, 0) : (eta * (-v1) + (eta * cos_theta_i - c) * n);
    }

    float power_heuristic(int n_f, float p_f, int n_g, float p_g, float beta = 2)
    {
        float c_f = (float)n_f * p_f, c_g = (float)n_g * p_g;
        return pow(c_f, beta) / (pow(c_f, beta) + pow(c_g, beta));
    }
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

int main(int argc, char **argv)
{
  // exploit namespaces to shorten code
  using namespace boost::numeric::ublas;
  using std::cout;
  using std::endl;

  // declare three 3x3 matrices of complex<long double> elements
  matrix<std::complex<long double> > m(3, 3), n(3, 3), o(3, 3);

  // iterate over 3x3 matrix entries
  // r : row index
  // c : column index
  for (unsigned r = 0; r < m.size1(); r++) {
    for (unsigned c = 0; c < m.size2(); c++) {
      // enumerated matrix entries
      m(r,c) = 3 * r + c;

      // complex numbers designating rows and cols
      n(r,c) = r + c * 1i;

      // elementwise square of n
      o(r,c) = std::pow(n(r,c), 2);
    }
  }

  // print to screen as demonstration
  cout << "m:" << endl;
  cout << m << endl;
  cout << endl << "n:" << endl;
  cout << n << endl;
  cout << endl << "o:" << endl;
  cout << n << endl;
  cout << endl << "m + n:" << endl;
  cout << m + n << endl;
  cout << endl << "m * n:" << endl;
  cout << prod(m, n) << endl;
  cout << endl << "n * n - o:" << endl;
  cout << prod(n, n) - o << endl;
}


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

//----------------H1Begin----------------
//! Computes the H^1 differences between
//! u1 (considered as coefficients for Quadratic FEM) and u2grad.
double computeH1Difference(const Eigen::MatrixXd &vertices,
                           const Eigen::MatrixXi &dofs,
                           const Eigen::VectorXd &u1,
                           const std::function<Eigen::Vector2d(double, double)> &u2grad) {
	const int numberOfElements = dofs.rows();

	double error = 0;
	for (int i = 0; i < numberOfElements; ++i) {
		auto &idSet = dofs.row(i);

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

		auto            coordinateTransform = makeCoordinateTransform(b - a, c - a);
		auto            volumeFactor        = std::abs(coordinateTransform.determinant());
		Eigen::Matrix2d elementMap          = coordinateTransform.inverse().transpose();

		// (write your solution here)
		auto f = [&](double x, double y) -> double {
			Eigen::Vector2d transformedPoint = coordinateTransform * Eigen::Vector2d(x, y) + a.transpose();

			Eigen::Vector2d approximate_grad = Eigen::Vector2d(0, 0);
			for (int j = 0; j < 6; ++j) {
				approximate_grad += u1(idSet(j)) * elementMap * gradientShapefun(j, x, y);
			}
			Eigen::Vector2d diff = u2grad(transformedPoint.x(), transformedPoint.y()) - approximate_grad;

			return diff.dot(diff) * volumeFactor;
		};

		error += integrate(f);
	}

	return std::sqrt(error);
}
//----------------H1End----------------


// 2.62     Nov 10, 05  hyperparameter autosearch: observations weighted by inverse stddev
// 3.02     Sep 01, 11  watch for denormal or infinite limits during autosearch
// MAS      Jan 02, 15  changed first step direction if hyperparameter is very small

#define  _USE_MATH_DEFINES
#include <cmath>
#include <stdexcept>
#include <limits>
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/LU>

//#include "tnt_array2d.h"
//#include "tnt_array2d_utils.h"
//#include "jama_lu.h"
//
//#include "logging.h"

#include "HParSearch.h"

//observations weighted by inverse stddev
QuadrCoefs QuadrLogFit( const map<double,UniModalSearch::MS> & y_by_x )
{
    unsigned n = y_by_x.size();

    //prepare for case weights: inverse of stddev; beware of zero stddev
    double minstddev=numeric_limits<double>::max();
    for (map<double,UniModalSearch::MS>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end(); itr++) {
        if (0 < itr->second.s && itr->second.s < minstddev) {
        	minstddev = itr->second.s;
        }
    }
    const double zeroadjust = minstddev<numeric_limits<double>::max() ? 100.0/minstddev //non-zeroes present
        : 1.0; //all weights will be equal

    Eigen::MatrixXd X( n, 3 ); //nRows, nCols
    Eigen::MatrixXd XTW( 3, n ); //nRows, nCols
    Eigen::VectorXd Y( n );
    int i = 0;
    for (map<double,UniModalSearch::MS>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end();
        itr++, i++) {
        double weight = itr->second.s>0 ? 1/itr->second.s : zeroadjust;
        X(i,0) = XTW(0,i) = 1.0;
        X(i,1) = XTW(1,i) = log( itr->first );
        X(i,2) = XTW(2,i) = log( itr->first ) * log( itr->first );
        for (int j=0; j < 3; j++) { //for no weighting, just skip this
            XTW(j,i) *= weight;
        }
        Y(i) = itr->second.m;
    }

    Eigen::MatrixXd XTX =  XTW * X;
    Eigen::VectorXd XTY = XTW * Y;
    Eigen::VectorXd b_hat = XTX.fullPivLu().solve(XTY);

    // TODO Check numerical accuracy and throw error

    QuadrCoefs ret;
    ret.c0 = b_hat(0);
    ret.c1 = b_hat(1);
    ret.c2 = b_hat(2);
    return ret;
}

StepValue UniModalSearch::step() //recommend: do/not next step, and the next x value
{
    StepValue ret(true,0,0);
    switch( y_by_x.size() ) {
    case 0: ret.second = 1; break;
    case 1:
        if (y_by_x.begin()->first < m_first_cut)
            ret.second = y_by_x.begin()->first * m_stdstep;
        else
            ret.second = y_by_x.begin()->first / m_stdstep;
            //we divide here because step with more penalty is safer numerically
        break;
    case 2:
        if( y_by_x.begin()->second.m > y_by_x.rbegin()->second.m )
            ret.second = y_by_x.begin()->first / m_stdstep;
        else
            ret.second = y_by_x.rbegin()->first * m_stdstep;
        break;
    default: // 3 or more
		if( y_by_x.begin()->first==best->first ) {  //max is at the left - move to the left
            ret.second = best->first / m_stdstep;
			if( !(best->first > numeric_limits<double>::denorm_min()) ) { //inf or nan
				ret.first = false;
			    ret.expected = best->second.m;
			}
		}else if( y_by_x.rbegin()->first==best->first ) { //max is at the right - move to the right
            ret.second = best->first * m_stdstep;
			if( !(best->first < numeric_limits<double>::infinity()) ) { //inf or nan
				ret.first = false;
			    ret.expected = best->second.m;
			}
		}else { //max is 'bracketed'
            QuadrCoefs coefs = QuadrLogFit( y_by_x );
            double log_argmax = - coefs.c1 / coefs.c2 / 2;
            double expected_max = - coefs.c1*coefs.c1 / coefs.c2 / 4 + coefs.c0;

			double maxval = best->second.m;
            if( 0==maxval )  ret.first=false;
            else if( (expected_max-maxval)/fabs(maxval) < m_stop_by_y )  ret.first=false;
            else if( fabs(log_argmax-log(best->first)) < m_stop_by_x )  ret.first=false;
            //double deriv = 2*coefs.c2*best->first + coefs.c1;    cout<<"\nderiv  "<<deriv;
            //ret.first = fabs(deriv) > stop_by_y;

            ret.second = exp( log_argmax );
            ret.expected = expected_max;
            //map<double,double>::const_iterator left = best; left--;
            //map<double,double>::const_iterator right = best; right++;
//            std::cout
//            //Log(6)
//            <<"\nSearch step "<<ret.second<<" stop_by_y "<<((expected_max-maxval)/fabs(maxval))
//                <<" stop_by_x "<<(fabs(log_argmax-log(best->first))) << endl;
        }
    }
    return ret;
}

//void UniModalSearch::dump(std::ostream& stream) const {
//    int i = 0;
//    for (map<double,UniModalSearch::MS>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end();
//        itr++, i++) {
//    	stream << "search[ " << itr->first << " ] = " << itr->second << std::endl;
//    }
//}

std::ostream& operator<< (std::ostream& stream, const UniModalSearch& search) {
//	search.dump(stream);

    for (map<double,UniModalSearch::MS>::const_iterator itr=search.y_by_x.begin(); itr!=search.y_by_x.end();
        itr++) {
    	stream << "search[ " << itr->first << " ] = " << itr->second.m
    			<< "(" << itr->second.s << ")" << std::endl;
    }

	return stream;
}

#ifdef UNITTEST_
#include <iostream>
void main() {
    map<double,double> y_by_x;
    //example llkl.d001145.b prepared in R
    y_by_x[ 2.000000e+04 ] = -408.199;
    y_by_x[ 2.002884e+03 ] = -328.581;
    y_by_x[ 2.000000e+02 ] = -281.788;
    y_by_x[ 2.002884e+01 ] = -229.966;
    y_by_x[ 2.000000e+00 ] = -184.908;
    y_by_x[ 2.002884e-01 ] = -150.792;
    y_by_x[ 2.000000e-02 ] = -132.081;
    y_by_x[ 2.002884e-03 ] = -127.708;
    y_by_x[ 2.000000e-04 ] = -146.397;
    y_by_x[ 2.002884e-05 ] = -183.208;
    QuadrCoefs c = QuadrLogFit( y_by_x );
    //R results:
    // (Intercept)     logvar2      logvar
    // -171.030466   -1.186039  -12.662184
    cout<<"c0="<<c.c0<<"  c1="<<c.c1<<"  c2="<<c.c2<<endl;

    UniModalSearch s;
    for( map<double,double>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end(); itr++ )
        s.tried( itr->first, itr->second );
    //R result: 0.004805411
    cout<<"step="<<s.step().second<<endl;
}
#endif //UNITTEST_

/*
    Copyright (c) 2002, 2003, 2004, 2005, 2006, 2007, Rutgers University, New Brunswick, NJ, USA.

    Permission is hereby granted, free of charge, to any person obtaining
    a copy of this software and associated documentation files (the
    "Software"), to deal in the Software without restriction, including
    without limitation the rights to use, copy, modify, merge, publish,
    distribute, sublicense, and/or sell copies of the Software, and to
    permit persons to whom the Software is furnished to do so, subject to
    the following conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
    BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
    ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
    CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.

    Except as contained in this notice, the name(s) of the above
    copyright holders, DIMACS, and the software authors shall not be used
    in advertising or otherwise to promote the sale, use or other
    dealings in this Software without prior written authorization.
*/


#include <iostream>
#include <armadillo>
#define PRINT 1

using namespace std;
using namespace arma;

int main()
{
  // User input
  arma::mat input;
  input << 1 << 3 << 2 << 4 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << endr
        << 5 << 7 << 6 << 8 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << endr;
  input = input.t();

  size_t size = 4; // input channels
  size_t upscale_factor = 2;
  size_t height = 2;
  size_t width = 2;

  // Forward()
  size_t batchSize = input.n_cols;
  size_t size_out = size / std::pow(upscale_factor, 2); // output channels
  size_t output_height = height * upscale_factor;
  size_t output_width = width * upscale_factor;

  arma::mat output;
  output.zeros(output_height * output_width * size_out, batchSize );

  for(size_t n = 0; n < batchSize; n++)
  {
    arma::mat inputImage = input.col(n);
    arma::mat outputImage = output.col(n);
    arma::cube inputTemp(const_cast<arma::mat&>(inputImage).memptr(), height, width, size, false, false);
    arma::cube outputTemp(const_cast<arma::mat&>(outputImage).memptr(), output_height, output_width, size_out, false, false);

    for (size_t c = 0; c < size_out ; c++)
    {
      for (size_t h = 0; h < output_height; h++)
      {
        for (size_t w = 0; w < output_width; w++)
        {
          size_t height_index = h / upscale_factor;
          size_t width_index = w / upscale_factor;
          size_t channel_index = (upscale_factor * (h % upscale_factor)) + (w % upscale_factor) + (c * std::pow(upscale_factor, 2));
          outputTemp(w, h, c) = inputTemp(width_index, height_index, channel_index);
        }
      }
    }

    output.col(n) = outputImage;

    if(PRINT)
    {
      cout << "Image " << n << " calculations: " << endl;
      cout << "-----------------------------------" << endl;
      cout << "-----------------------------------" << endl;
      cout << "INPUT for Pixel Shuffle: " << endl;
      cout << inputTemp << endl;
      cout << "-----------------------------------" << endl;
      cout << "OUTPUT from Pixel Shuffle: " << endl;
      cout << outputTemp << endl;
      cout << "-----------------------------------" << endl;
    }

  }

  // this is gy for Pixel Shuffle layer simulated as a tensor filled with an arbitrary sequence of values.
  arma::mat gy;
  gy << 1 << 5 << 9 << 13 << 2 << 6 << 10 << 14 << 3 << 7 << 11 << 15 << 4 << 8 << 12 << 16 << endr
    << 17 << 21 << 25 << 29 << 18 << 22 << 26 << 30 << 19 << 23 << 27 << 31 << 20 << 24 << 28 << 32 << endr;
  gy = gy.t();

  // Backward()
  arma::mat g;
  g.zeros(arma::size(input));

  for(size_t n = 0; n < batchSize; n++)
  {
    arma::mat gyImage = gy.col(n);
    arma::mat gImage = g.col(n);
    arma::cube gyTemp(const_cast<arma::mat&>(gyImage).memptr(), output_height, output_width, size_out, false, false);
    arma::cube gTemp(const_cast<arma::mat&>(gImage).memptr(), height, width, size, false, false);

    for (size_t c = 0; c < size_out ; c++)
    {
      for (size_t h = 0; h < output_height; h++)
      {
        for (size_t w = 0; w < output_width; w++)
        {
          size_t height_index = h / upscale_factor;
          size_t width_index = w / upscale_factor;
          size_t channel_index = (upscale_factor * (h % upscale_factor)) + (w % upscale_factor) + (c * std::pow(upscale_factor, 2));
          gTemp(width_index, height_index, channel_index) = gyTemp(w, h, c);
        }
      }
    }

    g.col(n) = gImage;

    if (PRINT)
    {
      cout << "Image " << n << " calculations: " << endl;
      cout << "-----------------------------------" << endl;
      cout << "Hypothetical gy for Pixel Shuffle: " << endl;
      cout << gyTemp << endl;
      cout << "-----------------------------------" << endl;
      cout << "g for Pixel Shuffle: " << endl;
      cout << gTemp << endl;
      cout << "-----------------------------------" << endl;
    }
  }

  return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include <cmath>
#include <string>
#include <fstream>
#include <bits/stdc++.h>

template <typename T>
void print(T x)
{
    std::ofstream output;
    output.open("output/output.txt", std::ios_base::app); // std::ofstream::trunc);
    std::cout << x << std::endl;
    output.close();
}

double trapezRule(Eigen::VectorXd function(Eigen::VectorXd), double x_min, double x_max, double h)
{
    double result = 100;
    double old_result = 0;
    int iterator = 0;

    do
    {
        h = h / 2;
        iterator++;
        Eigen::VectorXd deltaX = Eigen::VectorXd::LinSpaced((x_max - x_min) / h + 1, x_min, x_max);
        Eigen::VectorXd f = function(deltaX);
        Eigen::VectorXd result_vec = f * h;
        old_result = result;
        result = 0.5 * (result_vec(0) + result_vec(result_vec.size() - 1)) + result_vec(Eigen::seq(1, result_vec.size() - 2)).sum();

    } while ((abs(result - old_result) > 1e-6) & (iterator <= 30));
    print("Iterationen:");
    print(iterator);
    return result;
}

Eigen::VectorXd f1a(Eigen::VectorXd x)
{
    return Eigen::exp(x.array()) / x.array();
}

Eigen::VectorXd f1b(Eigen::VectorXd x)
{
    return Eigen::exp(-x.array()) / Eigen::sqrt(x.array());
}

Eigen::VectorXd f1c(Eigen::VectorXd x)
{
    return Eigen::sin(x.array()) / x.array(); //sin(x) / x;
}

double f3a(double x)
{
    return (x * x) - 2;
}

double bisection(double function(double), double a, double b, double c)
{
    if ((a < b) & (b < c) & (function(b) < function(a)) & (function(b) < function(c)))
    {
        int iteration = 0;
        do
        {
            iteration++;

            if (abs(b - a) > abs(c - b))
            {
                double a_new = a;
                double b_new = (a + b) / 2;
                double c_new = b;
                if (function(b_new) < function(c_new))
                {
                    a = a_new;
                    b = b_new;
                    c = c_new;
                }
                else
                {
                    a = b_new;
                    b = c_new;
                    c = c;
                }
            }
            else
            {
                double a_new = b;
                double b_new = (b + c) / 2;
                double c_new = c;
                if (function(b_new) < function(a_new))
                {
                    a = a_new;
                    b = b_new;
                    c = c_new;
                }
                else
                {
                    a = a;
                    b = a_new;
                    c = b_new;
                }
            }
        } while ((abs(a - c) > 1e-9) & (iteration < 100));
        print("Iterationen: ");
        print(iteration);
        print("a - c = ");
        print(abs(a - c));
    }
    else
    {
        print("First requirement of bisection is not fullfiled.");
    }
    return a;
}

double fDot(double function(double), double x0)
{
    double deltaX = (x0 + 1) / 1000;
    // Eigen::VectorXd deltaX = Eigen::VectorXd::LinSpaced((x(x.size()-1) -x(0) ) / diff_x + 1, x(0), x(x.size()-1));
    return (function(x0 + deltaX) - function(x0 - deltaX)) / (2 * deltaX);
}

double fDDot(double function(double), double x0)
{
    double deltaX = (x0 + 1) / 1000;
    // Eigen::VectorXd deltaX = Eigen::VectorXd::LinSpaced((x(x.size()-1) -x(0) ) / diff_x + 1, x(0), x(x.size()-1));
    return (function(x0 + deltaX) - 2 * function(x0) + function(x0 - deltaX)) / (deltaX * deltaX);
}

double newton(double function(double), double x0)
{
    double x_new = x0;
    double x_old = 0;
    double iteration = 0;
    do
    {
        iteration++;
        x_old = x_new;
        x_new = x_old - fDot(function, x_old) / fDDot(function, x_old);
    } while ((abs(x_new - x_old) > 1e-9) & (iteration < 100));
    print("Iterationen: ");
    print(iteration);
    print("x_new - x_old = ");
    print(abs(x_new - x_old));
    return x_new;
}

int main()
{
    for (int m = 3; m < 5; m++)
    {
        int j = pow(2, m);
        int N = 10;
        int l = N;
        std::complex<double> complex_(0.0, 1.0);
        Eigen::VectorXcd F_j(j);
        Eigen::VectorXcd f_l(l);
        Eigen::MatrixXcd omega(j, l);

        for (int i = 0; i < l; i++)
        {
            f_l(i) = std::sqrt(1 + i);
        }

        for (int i = 0; i < l; i++)
        {
            for (int k = 0; k < j; k++)
            {
                omega(k, i) = std::exp(k * i / N * 2 * M_PI * complex_);
            }
        }

        F_j = omega * f_l;
        Eigen::VectorXd F_j_real = F_j.real();
        print(F_j_real);
    }

    // // Ex. 1
    // // a)

    // print("Exercise a): ");
    // double zero = 1e-6;
    // print("Zero is:");
    // print(zero);
    // double i1 = trapezRule(f1a, -1, -zero, 0.5) + trapezRule(f1a, zero, 1, 0.5);
    // print("Result I1: ");
    // print(i1);

    // // b) see WolframAlpha for x_max
    // print("Exercise b): ");
    // double infty = 100.;
    // print("Infinity is:");
    // print(infty);
    // double epsilon = 0.001;
    // print("Epsilon is:");
    // print(epsilon);
    // zero = 1e-11;
    // print("Zero is:");
    // print(zero);
    // double i2 = trapezRule(f1b, zero, epsilon, 0.0005) + trapezRule(f1b, epsilon, infty, 0.5);
    // print("Result I2: ");
    // print(i2);

    // // c)
    // print("Exercise c): ");
    // infty = 1000000.;
    // print("Infinity is:");
    // print(infty);
    // zero = 1e-6;
    // print("Zero is:");
    // print(zero);
    // double i3 = trapezRule(f1c, -infty, -zero, 0.5) + trapezRule(f1c, zero, infty, 0.5);
    // print("Result I3: ");
    // print(i3);

    // // Ex. 3
    // // a)
    // double a = -0.5;
    // double b = -0.1;
    // double c = 2.0;
    // double result_bisec = bisection(f3a, a, b, c);

    // // b)
    // double x0 = 1;
    // double result_newton = newton(f3a, x0);
    // print("Bisec - Newton:");
    // print(abs(result_bisec - result_newton));

    return 0;
}


/*
  Solves the one-particle Schrodinger equation
  for a potential specified in function
  potential(). This example is for the harmonic oscillator in 3d
*/
#include <cmath>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <armadillo>

using namespace  std;
using namespace  arma;

double potential(double);
void output(double, double, int, vec& );

// Begin of main program

int main(int argc, char* argv[])
{
  int       i, j, Dim, lOrbital;
  double    RMin, RMax, Step, DiagConst, NondiagConst, OrbitalFactor;
  // With spherical coordinates RMin = 0 always
  RMin = 0.0;

  RMax = 8.0;  lOrbital = 0;  Dim =2000;
  mat Hamiltonian = zeros<mat>(Dim,Dim);
  // Integration step length
  Step    = RMax/ Dim;
  DiagConst = 2.0 / (Step*Step);
  NondiagConst =  -1.0 / (Step*Step);
  OrbitalFactor = lOrbital * (lOrbital + 1.0);

  // local memory for r and the potential w[r]
  vec r(Dim); vec w(Dim);
  for(i = 0; i < Dim; i++) {
    r(i) = RMin + (i+1) * Step;
    w(i) = potential(r(i)) + OrbitalFactor/(r(i) * r(i));
  }

  // Setting up tridiagonal matrix and brute diagonalization using Armadillo
  Hamiltonian(0,0) = DiagConst + w(0);
  Hamiltonian(0,1) = NondiagConst;
  for(i = 1; i < Dim-1; i++) {
    Hamiltonian(i,i-1)    = NondiagConst;
    Hamiltonian(i,i)    = DiagConst + w(i);
    Hamiltonian(i,i+1)    = NondiagConst;
  }
  Hamiltonian(Dim-1,Dim-2) = NondiagConst;
  Hamiltonian(Dim-1,Dim-1) = DiagConst + w(Dim-1);
  // diagonalize and obtain eigenvalues

  vec Eigval(Dim);
  eig_sym(Eigval, Hamiltonian);
  output(RMin , RMax, Dim, Eigval);

  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 output(double RMin , double RMax, int Dim, vec& d)
{
  int i;
  cout << "RESULTS:" << endl;
  cout << setiosflags(ios::showpoint | ios::uppercase);
  cout <<"Rmin = " << setw(15) << setprecision(8) << RMin << endl;
  cout <<"Rmax = " << setw(15) << setprecision(8) << RMax << endl;
  cout <<"Number of steps = " << setw(15) << Dim << endl;
  cout << "Five lowest eigenvalues:" << endl;
  for(i = 0; i < 5; i++) {
    cout << setw(15) << setprecision(8) << d[i] << endl;
  }
}  // end of function output

//   vec q1 = randu<vec>(100);


#ifndef UTIL_H
#define UTIL_H
#include <cmath>
#include <cassert>
#include <boost/operators.hpp>

template<class T> class Maybe {
    T value;
    bool has_value;
public:
    Maybe() : value(), has_value(false) {}
    Maybe(T val) : value(val), has_value(true) {}

    operator bool() {
        return has_value;
    }

    T val() {
        assert(has_value);
        return value;
    }
};

void log(const char* s) {
    printf("%s\n", s);
    fflush(stdout);
}

inline double sq(double val) {
    return val*val;
}

const double EPS = 1e-6;

inline bool almost_zero(double val) {
    return fabs(val) < EPS;
}

struct Vector : boost::additive<Vector>, boost::multiplicative2<Vector, double>, boost::equality_comparable<Vector> {
    double coord[3];
    Vector() : coord{0, 0, 0} {}
    Vector(double x, double y, double z) : coord{x, y, z} {}

    double length() const {
        return sqrt(sq(coord[0]) + sq(coord[1]) + sq(coord[2]));
    }

    bool operator==(const Vector& other) {
        for (int i = 0; i < 3; ++i) {
            if (fabs(coord[i] - other.coord[i]) > EPS) {
                return false;
            }
        }
        return true;
    }

    Vector& operator+=(const Vector& other) {
        for (int i = 0; i < 3; ++i) {
            coord[i] += other.coord[i];
        }
        return *this;
    }

    Vector& operator-=(const Vector& other) {
        for (int i = 0; i < 3; ++i) {
            coord[i] -= other.coord[i];
        }
        return *this;
    }

    Vector& operator*=(double factor) {
        for (int i = 0; i < 3; ++i) {
            coord[i] *= factor;
        }
        return *this;
    }

    Vector& operator/=(double factor) {
        for (int i = 0; i < 3; ++i) {
            coord[i] /= factor;
        }
        return *this;
    }

    Vector normalized() const {
        return *this / length();
    }

    friend Vector operator*(Vector a, Vector b) {
        return Vector(a.coord[0]*b.coord[0], a.coord[1]*b.coord[1], a.coord[2]*b.coord[2]);
    }

    friend double dot(Vector a, Vector b) {
        double ans = 0;
        for (int i = 0; i < 3; ++i) {
            ans += a.coord[i]*b.coord[i];
        }
        return ans;
    }

    friend Vector vec(Vector a, Vector b) {
        return Vector(a.coord[1]*b.coord[2]-a.coord[2]*b.coord[1], -(a.coord[0]*b.coord[2]-a.coord[2]*b.coord[0]), a.coord[0]*b.coord[1]-a.coord[1]*b.coord[0]);
    }

    friend double angle(Vector a, Vector b) {
        return dot(a, b) / a.length() / b.length();
    }
};

inline double sq(Vector v) {
    return dot(v, v);
}

typedef Vector Point;

#endif


/**
 * \file SinusGeneratorFilter.cpp
 */

#include <ATK/Tools/SinusGeneratorFilter.h>

#include <cmath>

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

namespace ATK
{
  template<typename DataType_>
  SinusGeneratorFilter<DataType_>::SinusGeneratorFilter()
  :Parent(0, 2)
  {
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_frequency(DataType_ frequency)
  {
    if(frequency <= 0)
    {
      throw std::out_of_range("Frequency must be strictly positive");
    }
    this->frequency = frequency;
    this->frequ_cos = std::cos(2 * boost::math::constants::pi<DataType_>() * frequency / output_sampling_rate);
    this->frequ_sin = std::sin(2 * boost::math::constants::pi<DataType_>() * frequency / output_sampling_rate);
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_frequency() const
  {
    return frequency;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::full_setup()
  {
    Parent::full_setup();
    cos = 1;
    sin = 0;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::process_impl(gsl::index size) const
  {
    DataType* ATK_RESTRICT output_cos = outputs[0];
    DataType* ATK_RESTRICT output_sin = outputs[1];

    for(gsl::index i = 0; i < size; ++i)
    {
      auto new_cos = cos * frequ_cos - sin * frequ_sin;
      auto new_sin = cos * frequ_sin + sin * frequ_cos;
      auto norm = (new_cos * new_cos + new_sin * new_sin);

      cos = new_cos / norm;
      sin = new_sin / norm;

      output_cos[i] = volume * cos + offset;
      output_sin[i] = volume * sin + offset;
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class SinusGeneratorFilter<float>;
#endif
  template class SinusGeneratorFilter<double>;
}


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

#include "mylinearloadvector.h"

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

#include <Eigen/Core>
#include <functional>

namespace ElementMatrixComputation {

namespace {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector4d computeLoadVector(
    const Eigen::MatrixXd &vertices,
    std::function<double(const Eigen::Vector2d &)> f) {
  // Number of nodes of the element: triangles = 3, rectangles = 4
  const int num_nodes = vertices.cols();
  // Vector for returning element vector
  Eigen::Vector4d elem_vec = Eigen::Vector4d::Zero();

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

  Eigen::MatrixXd midpoints(2,num_nodes);

  double area;
  switch(num_nodes){
    case 3: {
      area = 0.5*(vertices(0,1)-vertices(0,0)*(vertices(1,2)-vertices(1,0))-
                  vertices(0,2)-vertices(0,0)*(vertices(1,1)-vertices(1,0))
              );
      midpoints<<
        vertices(0,0)+vertices(0,1),
        vertices(0,1)+vertices(0,2),
        vertices(0,2)+vertices(0,0),
        vertices(1,0)+vertices(1,1),
        vertices(1,1)+vertices(1,2),
        vertices(1,2)+vertices(1,0);
      break;
    }
    case 4: {
      area = (vertices(0,1)-vertices(0,0))*(vertices(1,3)-vertices(1,0));
      midpoints<<
        vertices(0,0)+vertices(0,1),
        vertices(0,1)+vertices(0,2),
        vertices(0,2)+vertices(0,3),
        vertices(0,3)+vertices(0,0),
        vertices(1,0)+vertices(1,1),
        vertices(1,1)+vertices(1,2),
        vertices(1,2)+vertices(1,3),
        vertices(1,3)+vertices(1,0);
      break;
    }
  }
  midpoints *= 0.5;

  Eigen::VectorXd fvals = Eigen::VectorXd::Zero(num_nodes);
  for (int i=0;i<num_nodes;i++){
    fvals[i] = f(midpoints.col(i));
  }

  for (int i=0;i<num_nodes;i++){
    elem_vec[i%num_nodes] += 0.5*fvals[i];
    elem_vec[(i+1)%num_nodes] += 0.5*fvals[i];
  }
  elem_vec *= area/(double)num_nodes;
  return elem_vec;
}
/* SAM_LISTING_END_1 */

}  // namespace

Eigen::Vector4d MyLinearLoadVector::Eval(const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};
  const lf::base::size_type num_nodes{ref_el.NumNodes()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();

  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());

  return computeLoadVector(vertices, f_);
}

}  // namespace ElementMatrixComputation


#include <eigen3/Eigen/Core>
#include <Eigen/LU>
#include <vector>

#include "nntree/dataset.h"

namespace nntree {
    namespace core {

// TODO(equivalence1) memory leaks here, but we will rewrite it anyways
        void LeastSquares(Tensor<double>& x, Tensor<double>& y, Tensor<double>& res) {
            namespace E = Eigen;

            assert(x.Ndim() == 2);
            assert(y.Ndim() == 2);

            E::Map<E::MatrixXd, 0, E::Stride<E::Dynamic, E::Dynamic>>
                mxX_t(x.Data(),
                      x.Ncols(),
                      x.Nrows(),
                      E::Stride<E::Dynamic, E::Dynamic>(x.Strides()[0] / sizeof(double),
                                                        x.Strides()[1] / sizeof(double)));
            auto mxX = mxX_t.transpose();
            E::Map<E::MatrixXd, 0, E::Stride<E::Dynamic, E::Dynamic>>
                mxY_t(y.Data(),
                      y.Ncols(),
                      y.Nrows(),
                      E::Stride<E::Dynamic, E::Dynamic>(y.Strides()[0] / sizeof(double),
                                                        y.Strides()[1] / sizeof(double)));
            auto mxY = mxY_t.transpose();

            auto solution = (mxX.transpose() * mxX).inverse() * mxX.transpose() * mxY;
            auto solution_t = solution.transpose();

            auto result = new double[solution_t.rows() * solution_t.cols()];
            assert(result != nullptr);
            E::Map<E::MatrixXd> resultMap(result, solution_t.rows(), solution_t.cols());
            resultMap = solution_t;

            std::vector<uint64_t> shape({(uint64_t) solution.rows(),
                                         (uint64_t) solution.cols()});
            std::vector<uint64_t> strides({(uint64_t) resultMap.outerStride() * sizeof(double),
                                           (uint64_t) resultMap.innerStride() * sizeof(double)});
            res.FromMem(resultMap.data(), shape, strides, true);
        }

// TODO(equivalence1) reference is not const
        void LeastSquares(core::DataSet<double, double>& ds, Tensor<double>& res) {
            auto& x = ds.GetInput();
            auto& y = ds.GetOutput();
            assert(y.Nrows() == x.Nrows());
            LeastSquares(x, y, res);
        }

    }
}


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

#ifndef DLTV_PROBLEM_HPP_
#define DLTV_PROBLEM_HPP_

#include <Eigen/Dense>
#include <cstdint>

template<std::size_t _nPts = 100>
struct DltvOcpDi
{
  static_assert(_nPts > 1, "Number of trajectory points must be > 1.");

  // Must be defined
  constexpr static std::size_t nPts = _nPts;
  constexpr static std::size_t nx = 2;
  constexpr static std::size_t nu = 1;

  // Time step
  constexpr static double dt = 0.01;

  // Custom types
  using state_t = Eigen::Matrix<double, nx, 1>;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using A_t = Eigen::Matrix<double, nx, nx>;
  using B_t = Eigen::Matrix<double, nx, nu>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  // Constants
  const A_t A = (A_t() << 1., 0.01, 0., 1.).finished();
  const B_t B = (B_t() << 5.0e-5, 0.01).finished();
  const state_t E = (state_t() << 0., 0.).finished();
  const Q_t Q = (Q_t() << 10., 0., 0., 0.1 ).finished();
  const R_t R = (R_t() << 0.01).finished();
  const Q_t QT = (Q_t() << 1., 0., 0., 1.).finished();

  void get_x0(Eigen::Ref<state_t> x0) const
  {
    x0 << 1., 0.;
  }

  void get_T(std::size_t k, double & t) const
  {
    t = static_cast<double>(k) * dt;
  }

  void get_state_lb(std::size_t, Eigen::Ref<state_t> state_lb) const
  {
    state_lb <<
      -1000.,
      -0.5;
  }

  void get_state_ub(std::size_t, Eigen::Ref<state_t> state_ub) const
  {
    state_ub <<
      1000.,
      0.5;
  }

  void get_input_lb(std::size_t, Eigen::Ref<input_t> input_lb) const
  {
    input_lb << -1;
  }

  void get_input_ub(std::size_t, Eigen::Ref<input_t> input_ub) const
  {
    input_ub << 1;
  }

  const A_t & get_A(std::size_t) const
  {
    return A;
  }

  const B_t & get_B(std::size_t) const
  {
    return B;
  }

  const state_t & get_E(std::size_t) const
  {
    return E;
  }

  const Q_t & get_Q(std::size_t) const
  {
    return Q;
  }

  const R_t & get_R(std::size_t) const
  {
    return R;
  }

  const Q_t & get_QT() const
  {
    return QT;
  }

  state_t get_xldot(double) const {return state_t::Zero();}
};

#endif  // DLTV_PROBLEM_HPP_


#include "spectral_clustering.h"
#include <Eigen/Eigenvalues>
#include <iostream>
#include <ctime>

using namespace std;

//these functions are defined later in this file
void repair_clusters(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, Eigen::MatrixXi& sF, Eigen::VectorXi& sFC);
std::vector<std::vector<int> > spectral_clustering(const Eigen::MatrixXd& A, int K);
std::vector<std::vector<int> > kmeans_cluster(const Eigen::MatrixXd& data, int K);

/*
Spectral clustering of the faces
- use similarity matrix and face connectivity to determine the clusters

Input:

K: number of clusters
V, F: the mesh
similarity: a |F|*|F| symmetric matrix

Output:

sF: A face index matrix of size |F|*3. Faces in the same cluster are in consecutive rows of sF
sFC: A K*1 vector. Each element indicates the size of each cluster stored in sF
*/

void spectral_clustering
(int K, const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, const Eigen::MatrixXd& similarity,
 Eigen::MatrixXi& sF, Eigen::VectorXi& sFC)
{

	//Requirements
	// - similar faces are clustered together based on the similarity matrix
	// - faces in a cluster must be contiguous and form a single connected component

  //dummy segmentation, replace the following code
  int cluster_size=F.rows()/K;
  sFC=Eigen::VectorXi::Constant(K,cluster_size);
  sFC[K-1]=F.rows()-cluster_size*(K-1);
  sF=F;

  //YOUR CODE HERE

  //make sure that faces in the same cluster are connected
  repair_clusters(V,F,sF,sFC);
}

/// repair clusters so that each cluster contains a compact connected component
void repair_clusters
(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, Eigen::MatrixXi& sF, Eigen::VectorXi& sFC)
{
  //YOUR CODE HERE
}

/**
* Spectral clustering into K clusters
*
* @param A 		the affinity matrix
* @param K    the number of clusters
*
* @return		   ordered set of data row indices for each cluster
*/

std::vector<std::vector<int> > spectral_clustering(const Eigen::MatrixXd& A, int K)
{
	// compute normalized laplacian
  // using Ng, Jordan and Weiss, "On Spectral Clustering: Analysis and an Algorithm", NIPS 2002
  Eigen::VectorXd S=A.colwise().sum().array().pow(-0.5);
  Eigen::MatrixXd D=S.asDiagonal();
  Eigen::MatrixXd I=Eigen::MatrixXd::Identity(A.rows(),A.cols());
  Eigen::MatrixXd L = I - (D* A *D);

  //solve, eigenvalues are sorted in increasing order
  cout<<"- Solving Eigen Decomposition"<<endl;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(L);
	Eigen::VectorXd E = es.eigenvalues();
	Eigen::MatrixXd V = es.eigenvectors();

  // keep only the k-principal components (Eigen's eigenvectors are colwise)
  V = V.block(0,1,V.rows(),K);

  //normalize V
  Eigen::VectorXd norm = V.array().pow(2).matrix().rowwise().sum();
  for (unsigned int k = 0; k < K; ++k) V.col(k) = V.col(k).array() / norm.array();

  return kmeans_cluster(V, K);
}

/**
 * kmeans clustering
 * based on kmeans matlab script by Ian T Nabney (1996-2001)
 *
 * @param data 	affinity matrix
 * @oaram ncentres	number of clusters
 * @return		ordered set of data row indices for each cluster
 * 			    	(order is based on distance to cluster centre)
 */
std::vector<std::vector<int> >
kmeans_cluster(const Eigen::MatrixXd& data, int ncentres)
{
  std::srand(std::time(nullptr));
  //std::srand(0); //use this to debug so you get the same results

	int ndims = data.cols();
	int ndata = data.rows();

	Eigen::MatrixXd centres = Eigen::MatrixXd::Zero(ncentres, ndims);
	Eigen::MatrixXd old_centres;

	std::vector<int> rands;
	for (int i=0; i < ncentres; i++)
  {
		//randomly initialise centers
		bool flag;
		do {
			flag = false;
			int randIndex = std::rand()%ndata;
			//make sure same row not chosen twice
			for (unsigned int j=0; j < rands.size(); ++j) {
				if (randIndex == rands[j]) {
					flag = true;
					break;
				}
			}
			if (!flag) {
				centres.row(i) = data.row(randIndex);
				rands.push_back(randIndex);
			}
		} while (flag);
	}
	Eigen::MatrixXd id = Eigen::MatrixXd::Identity(ncentres, ncentres);
	//maps vectors to centres.
	Eigen::MatrixXd post(ndata, ncentres);

	Eigen::VectorXd minvals(ndata);

	double old_e = 0;
	int niters = 100;
	for (int n=0; n < niters; n++) {
		//Save old centres to check for termination
		old_centres = centres;

		// Calculate posteriors based on existing centres
		Eigen::MatrixXd d2(ndata, ncentres);
		for (int j = 0; j < ncentres; j++) {
			for (int k=0; k < ndata; k++) {
				d2(k,j) = (data.row(k)-centres.row(j)).squaredNorm();
			}
		}

		int r,c;
		// Assign each point to nearest centre
		for (int k=0; k < ndata; k++) {
			//get centre index (c)
			minvals[k] = d2.row(k).minCoeff(&r, &c);
			//set centre
			post.row(k) = id.row(c);
		}

		Eigen::VectorXd num_points = post.colwise().sum();
		// Adjust the centres based on new posteriors
		for (int j = 0; j < ncentres; j++) {
			if (num_points[j] > 0) {
				Eigen::MatrixXd s = Eigen::MatrixXd::Zero(1,ndims);
				for (int k=0; k<ndata; k++) {
					if (post(k,j) == 1) {
						s += data.row(k);
					}
				}
				centres.row(j) = s / num_points[j];
			}
		}

		// Error value is total squared distance from cluster centres
		double e = minvals.sum();
		double ediff = fabs(old_e - e);
		double cdiff = (centres-old_centres).array().abs().maxCoeff();
		//std::cout << "Cycle " << n << " Error " << e << " Movement " << cdiff << ", " << ediff << std::endl;

		if (n > 1) {
			//Test for termination
			if (cdiff < 0.0000000001 && ediff < 0.0000000001) {
				break;
			}
		}
		old_e = e;
	}

	//------- finished kmeans ---------

	//find the item closest to the centre for each cluster
	std::vector<std::vector<int> > clustered_items;
	for (int j=0; j < ncentres; j++) {
		//put data into map (multimap because minvals[k] could be the same for multiple units)
		std::multimap<double,int> cluster;
		for (int k=0; k < ndata; k++) {
			if (post(k,j) == 1) {
				cluster.insert(std::make_pair(minvals[k], k));
			}
		}
		//extract data from map
		std::vector<int> units;
		//the map will be sorted based on the key (the minval) so just loop through it
		//to get set of data indices sorted on the minval
		for (std::multimap<double,int>::iterator it = cluster.begin(); it != cluster.end(); it++) {
			units.push_back(it->second);
		}

		clustered_items.push_back(units);
	}
	//return the ordered set of item indices for each cluster centre
	return clustered_items;
}


#include <graphlab.hpp>
#include <string>
#include <iostream>
#include <vector>
#include <cmath>
#include <numeric>
#include <limits>
#include <graphlab/engine/warp_engine.hpp>
#include <graphlab/engine/warp_parfor_all_vertices.hpp>
#include <unsupported/Eigen/MatrixFunctions>

#include <Eigen/Dense>

int verbose;
std::vector<double> Hleft;
//#include <functional>

/*

  Total Subgraph Centrality.

  For a graph G with adjacency matrix A, TSC(G) = exp(A)*b, where 1 is
  the ones vector.

  We're going to implement this with an Arnoldi solver, following Saad
  (1992).

  The algorithm works like this:
  Choose a maximum iteration m.  Then make matrices V and H:
  b = ones(A.nodecount)
  V[0] = b/||b||
  for j in 0..m:
      w = A*V[j]
      for i in 0..j:
         H[i,j] = (w,V[i])
	 w = w - H[i,j] * V[i]
      H[j+1,j] = ||w||
      V[j+1] =w/||w||

  Then TSC = exp(A)*b ~= (V * exp(H) / ||b||)[:,0].  Stop when
  successive approximations converge, or we run out of steps.

  We still have that matrix exponential, but it's small and dense, and
  there's an implementation in Eigen.

*/

class node {
public:
std::vector<double> V;
double w;
  double TSC;
  double prev;
  node(): w(0.0),TSC(0.0),prev(0.0){};
  void load(graphlab::iarchive& infile) {
    infile>>V>>w>>TSC>>prev;
  }
  void save(graphlab::oarchive& outfile) const {
    outfile<<V<<w<<TSC<<prev;
  }

};

class edge {
public:
  double weight;
  edge(): weight(1.0) {} ;
  edge(double weight) : weight(weight) {};

  void save(graphlab::oarchive& outfile) const {
    outfile<<weight;
  }

  void load(graphlab::iarchive& infile) {
    infile>>weight;
  }

};

typedef node vertex_data_type;
typedef edge edge_data_type;
typedef graphlab::distributed_graph<vertex_data_type, edge_data_type> graph_type;
typedef graphlab::warp::warp_engine<graph_type> engine_type;

// This is just a little class to be used to find the maximum change in TSC values.
class max_finder{
public:
  double data;
  max_finder& operator+=(const max_finder& other){
    if (this->data <other.data){
      this->data = other.data;
    }
    return *this;
  }

  max_finder(): data(std::numeric_limits<double>::max()) {};
  max_finder(double x): data(x) {};
  void load(graphlab::iarchive& infile) {
    infile>>data;
  }
  void save(graphlab::oarchive& outfile) const {
    outfile<<data;
  }

};

// These three functions compute the A*w step of the
// Arnoldi iteration.
double arnoldi_map(graph_type::edge_type e,
		   graph_type::vertex_type v){
  return v.data().V.back();

}
void arnoldi_combine(double& v1, const double& v2) {
  v1 += v2;
}
void AVj_to_w(graph_type::vertex_type& v) {
  v.data().w = graphlab::warp::map_reduce_neighborhood(v,
						       graphlab::IN_EDGES,
						       arnoldi_map,
						       arnoldi_combine);
  return;
}

// Pushes the current w onto the V matrix
void w_to_v(graph_type::vertex_type& v){
  v.data().V.push_back(v.data().w);
}
// Scales w by a constant factor.  This is meant to be called through transform_vertices
// with a boost::bind to take care of the argument list
void scale_w(graph_type::vertex_type& v, double scale_factor){
  v.data().w /= scale_factor;
}

// Dumps the TSC to stdout
void print_TSC(graph_type::vertex_type& v){
  std::cout<<v.id()<<" " <<v.data().TSC<<std::endl;
}

// For debugging.
void print_w(graph_type::vertex_type& v){
  std::cout<<v.id()<<" " <<v.data().w<<" "<<v.data().V.size()<<" "<<v.data().V.back()<<std::endl;
}

// Make initial vector if we want a column from exp(A)
void initialize_column(graph_type::vertex_type& v, int i,int m) {
  if (v.id()==i){
    v.data().w = 1.0;
  } else{
    v.data().w = 0.0;
  }
  v.data().V.reserve(m);

}
// Make initial vector if we want the TSC
void initialize_TSC(graph_type::vertex_type& v, int m) {
  v.data().w = 1.0/sqrt((double)m);
  v.data().V.reserve(m);
}

// ||w||**2
double sum_w(const graph_type::vertex_type&  v){
  return v.data().w*v.data().w;
}

// w*V[i].  Call via boost::bind to select the i.
double w_dot_V(const graph_type::vertex_type&  v, int i){
  return v.data().w * v.data().V[i];
}

// w - (h,w)*V[i].  Call via boost::bind to set i, hdot.
void w_minus_hdot(graph_type::vertex_type& v, int i, double hdot) {
  v.data().w -= hdot * v.data().V[i];
}

// TSC = V*Hleft
void accumulate_hleft(graph_type::vertex_type&v){
  v.data().prev = v.data().TSC;
  v.data().TSC = 0;
  for(int j=0;j<Hleft.size();j++){
    v.data().TSC += v.data().V[j] * Hleft[j];
  }
  if (verbose){
    if (v.id()<10){
      std::cout<<"TSC "<<v.id()<<" "<<Hleft.size()<<" "<<v.data().TSC<<" "<<v.data().prev<< " "<<(v.data().TSC-v.data().prev)/(1e-15+v.data().TSC) <<std::endl;
    }
  }
  return;
}

// sum ((TSC-prevTSC)/TSC)
double total_error(const graph_type::vertex_type& v){
  return fabs((v.data().TSC-v.data().prev)/(1e-15+v.data().TSC));
}

// max ((TSC-prevTSC)/TSC)
max_finder max_error(const graph_type::vertex_type& v){
  return max_finder(fabs((v.data().TSC-v.data().prev)/(1e-15+v.data().TSC)));
}

int main(int argc, char** argv) {
  graphlab::command_line_options clopts("Total Subgraph Centrality");
  graphlab::distributed_control dc;
  std::string infile;
  std::string format = "tsv";
  int verbose=0;
  int m=100;
  int column = -1;
  clopts.attach_option("graph",infile,"Input graph.");
  clopts.attach_option("format",format,"Input format. Default tsv.");
  clopts.attach_option("m",m,"Maximum number of orthogonal vectors to approximate with. Default 100.");
  clopts.attach_option("column",column,"Column of exponential to calculate (instead of row-sum)");
  clopts.attach_option("verbose",verbose,"Verbosity level.");
  if(!clopts.parse(argc, argv)) {
    dc.cout() << "Error in parsing command line arguments." << std::endl;
    return EXIT_FAILURE;
  }
  if (infile==""){
    dc.cout() <<"ERROR: Must specify --graph! \n";
    return EXIT_FAILURE;
  }
  graph_type graph(dc,clopts);
  graph.load_format(infile, format);
  graph.finalize();
  if (m>graph.num_vertices()) {
    m = graph.num_vertices();
  }
  engine_type engine(dc,graph,clopts);

  engine.signal_all();
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(m+1,m+1);
  Hleft.resize(m);
  for(int i=0;i<m;i++) { Hleft[i] = 0.0;}
  double beta;
  if (column>=0) {
    if (column>graph.num_vertices()){
      column = 0;
    }
    graph.transform_vertices(boost::bind(initialize_column,_1,column,m));
    beta = 1.0;
  } else {
    graph.transform_vertices(boost::bind(initialize_TSC,_1,m));
    beta = sqrt(m);
  }

  // The first column of V is just w
  graph.transform_vertices(w_to_v);

  for(int j=0;j<m;j++) {
    graphlab::warp::parfor_all_vertices(graph,AVj_to_w);
    // We should be able to move this loop inside the map-reduce and transform steps
    for(int i=0;i<=j;i++) {
      H(i,j)= graph.map_reduce_vertices<double>(boost::bind(w_dot_V,_1,i));
      graph.transform_vertices(boost::bind(w_minus_hdot,_1,i,H(i,j)));
    }
    H(j+1,j)= sqrt(graph.map_reduce_vertices<double>(sum_w));
    // If we're in this case, it means we have a spanning set and we shouldn't
    // prep for the next iteration.  Otherwise, make the data we'll need next time.
    if ( ( !std::isnan(H(j+1,j))) && (H(j+1,j) > 0) ) {
      graph.transform_vertices(boost::bind(scale_w, _1, H(j+1,j)));
      graph.transform_vertices(w_to_v);
    }

    if (j>0) {
      // Have we converged?
      Eigen::MatrixXd EH(H);
      EH = EH.exp();
      Hleft.resize(j+1);

      for(int i=0;i<=j;i++) { Hleft[i] = EH(i,0) * beta;}
      graph.transform_vertices(accumulate_hleft);
      if (j>1) {
 	max_finder largest_error= graph.map_reduce_vertices<max_finder>(max_error);
	double all_error = graph.map_reduce_vertices<double>(total_error);
	if (verbose){
	  std::cerr<<"ARNOLDI STEP FINISHED "<<j<<std::endl;
	  std::cout<<"MAX ERROR: "<<largest_error.data<<" TOTAL ERROR: "<<all_error<<std::endl;
	}
	if (largest_error.data < 1e-15) { break; };
	if (all_error < 1e-15){  break; }
      }
    }
    // If we know we are in the last iteration, break
    if (std::isnan(H(j+1,j))){ break;}
    if (fabs(H(j+1,j))<1e-15){ break;}

  }
  graph.transform_vertices(print_TSC);

}


/******************************************************************************

  Basic N-dimensional topology.

  Copyright (c) 2013
  Dzmitry Hlindzich <hlindzich@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef TOPOLOGY_HPP_79A57FE0_9EFD_11E2_9E96_0800200C9A66_
#define TOPOLOGY_HPP_79A57FE0_9EFD_11E2_9E96_0800200C9A66_

#include <vector>
#include <utility>
#include <set>
#include <boost/static_assert.hpp>

#include "bo/core/vector.hpp"

namespace bo {
namespace math {

// Basic N-dimensional hyperrectangle (N-orthotope) geometry.
template <typename RealType, std::size_t N>
class OrthotopeTopology
{
private:
    BOOST_STATIC_ASSERT(N > 0);

public:
    typedef Vector<RealType, N> Point;
    typedef std::pair<Point, Point> Edge;
    typedef std::vector<Edge> Edges;

    // Returns all the edges of a unit N-orthotope.
    static Edges edges()
    {
        // Initialize the source of the edge traverse.
        PointSet in_vertices;
        in_vertices.insert(Point(0));

        // Traverse the edges.
        Edges e;
        traverse_edges(in_vertices, e);

        return e;
    }

    // Returns the number of edges in a N-orthotope.
    static std::size_t edge_count()
    {
        return (1 << (N - 1)) * N;
    }

    // Returns the number of vertices in a N-orthotope.
    static std::size_t vertex_count()
    {
        return 1 << N;
    }

    // Returns the number of facets ((N-1)-dimensional faces) in a N-othotope.
    static std::size_t facet_count()
    {
        return 2 * N;
    }

private:

    typedef std::set<Point> PointSet;

    // Recursively traverses the edges of a N-orthotope starting from the given set
    // of vertices in ascending order and pushes the edges into the given container.
    static void traverse_edges(const PointSet &in_vertices, Edges &edges)
    {
        if (in_vertices.size() == 1 && *in_vertices.begin() == Point(1))
            return;

        PointSet out_vertices;

        for (typename PointSet::const_iterator it = in_vertices.begin(); it != in_vertices.end(); ++it)
        {
            Point edge_begin = *it;

            // Find all adjacent vertices for the current one.
            for (std::size_t k = 0; k < N; ++k)
            {
                if (edge_begin[k] == 1)
                    continue;

                // An adjacent vertex differs only in one dimension.
                Point edge_end = edge_begin;
                edge_end[k] = 1;

                // Insert the edge into the collection.
                edges.push_back(Edge(edge_begin, edge_end));

                // Save the end vertex.
                out_vertices.insert(edge_end);
            }
        }

        traverse_edges(out_vertices, edges);
    }

};

} // namespace math
} // namespace bo

#endif // TOPOLOGY_HPP_79A57FE0_9EFD_11E2_9E96_0800200C9A66_


// Copyright 2020, Madhur Chauhan

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

// This is an example to calculate Reciprocal Fibonacci Constant (A079586 in the OEIS)
// compile with flags: -std=c++11 -lmpfr

//[fibonacci_eg

#include <boost/math/special_functions/fibonacci.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <iomanip>
#include <iostream>

int main() {
    using Real = boost::multiprecision::mpfr_float_1000;
    boost::math::fibonacci_generator<Real> gen;
    gen.set(1); // start producing values from 1st fibonacci number
    Real ans = 0;
    const int ITR = 1000;
    for (int i = 0; i < ITR; ++i) {
        ans += 1.0 / gen();
    }
    std::cout << std::setprecision(1000) << "Reciprocal fibonacci constant after "
              << ITR << " iterations is: " << ans << std::endl;
}

//]


#include "LogisticRegression.h"
#include "TrainingType.h"
#include <armadillo>
#include <assert.h>
#include <iostream>
#include <math.h>

using namespace arma;

LogisticRegression::LogisticRegression(mat &x, vec &y, double regPara)
    : x{x}, y{y}, regPara{regPara}, trained{false} {
  assert(x.n_rows == y.n_rows);

  // Create bias column and append at the end of  x
  mat bias = ones<mat>(this->ExampleNumber(), 1);
  this->x.insert_cols(0, bias);
}

LogisticRegression::~LogisticRegression() {}

void LogisticRegression::AddData(mat &extraX, vec &extraY) {
  assert(extraX.n_rows == extraY.n_rows);
  // Add 1 because x has a bias column
  assert((extraX.n_cols + 1) == this->x.n_cols);

  this->trained = false;
  // Add Bias column to latest added input
  mat bias = ones<mat>(extraX.n_rows, 1);
  mat inputX = extraX;
  inputX.insert_cols(0, bias);
  this->x.insert_rows(this->x.n_rows, inputX);
  this->y.insert_rows(this->y.n_rows, extraY);
}

void LogisticRegression::Train(TrainingType Type, double alpha,
                               unsigned int iters) {
  /*if (Type == normalEquation) {
    this->NormalEquation();
  } else*/
  if (Type == gradientDescent) {
    this->GradientDescent(alpha, iters);
  } else {
    std::cerr << "Invalid training type" << std::endl;
  }
}

uword LogisticRegression::ExampleNumber() { return this->x.n_rows; }

double LogisticRegression::Probablity(vec &x) {
  if (!this->trained) {
    std::cerr << "This model hasn't been trained" << std::endl;
    return 0.0;
  }
  vec bias = vec("1");
  vec input = x;
  input.insert_rows(0, bias);
  double prob = (input.t() * this->theta).eval()(0, 0);
  return prob;
}

double LogisticRegression::Predict(vec &x) {
  double prob = this->Probablity(x);
  if (prob >= probabilityThreshold) {
    return 1;
  } else {
    return 0;
  }
}

arma::mat LogisticRegression::SigmoidFunction(arma::mat inputX) {
  //--     1      --//
  //-- ---------- --//
  //-- 1 + e^(-inpuX) --//
  return 1 / (1 + exp(-inputX));
}

double LogisticRegression::SelfCost() { return this->Cost(this->x); }

double LogisticRegression::Cost(mat &inputX) {
  this->InitializeTheta();
  //--                    h = g(X Theta)                   --//
  //--J(Theta) = 1/m * (-y^T log(h) - (1-y)^T log(1-h)) +lambda/2m theta^2--//
  assert(inputX.n_cols == this->theta.n_rows);
  vec h = this->SigmoidFunction(inputX * this->theta);
  vec ve = (-this->y.t() * log(h)) - ((1 - y).t() * log(1 - h));
  vec thetaWithoutFirst = this->theta;
  thetaWithoutFirst[0] = 0;

  return (1 / (float)this->ExampleNumber() * ve +
          this->regPara / (double)this->ExampleNumber() * 2 *
              thetaWithoutFirst.t() * thetaWithoutFirst)
      .eval()(0, 0);
}

vec LogisticRegression::CostDerivative() {
  vec h = this->SigmoidFunction(this->x * this->theta);
  vec deriv = this->x.t() * (h - this->y);
  vec thetaWithoutFirst = this->theta;
  thetaWithoutFirst[0] = 0;
  return 1 / (double)this->ExampleNumber() * deriv +
         this->regPara / (double)this->ExampleNumber() * thetaWithoutFirst;
}

void LogisticRegression::GradientDescent(double alpha, unsigned int iters) {
  this->InitializeTheta();
  for (unsigned int i = 0; i < iters; i++) {
    this->theta = this->theta - (alpha * this->CostDerivative());
  }

  this->trained = true;
}

void LogisticRegression::InitializeTheta() {
  if (this->trained != true || this->theta.n_rows != this->x.n_cols) {
    // Initialize Theta
    this->theta = zeros<vec>(this->x.n_cols);
  }
}


#include <iostream>
#include <numeric>
#include <type.hpp>

#include <Eigen/Eigen>

#include "FEM/FEM1DApp.hpp"
#include "imgui/implot.h"
#include "Visualization/Visualizer.h"

#include <numeric>

using Linear = PolynomialFEMApp<1>;
using Quadratic = PolynomialFEMApp<2>;

class FEM1DVisualizer :public Visualizer
{
protected:

	void evaluate()
	{
		int segement_ = segemnt;

		if (segement_ % 2 != 0)
		{
			segement_ += 1;
		}

		//Homework 2
		//auto rhs = [](Float x) {return  -4 - x + Power(x, 2) - Power(x, 3) + Power(x, 4) + Cos(x) - 2 * x * Cos(x) - 2 * Sin(x); };
		//auto a = [](Float x) {return sin(x) + 2; };
		//auto c = [](Float x) {return x * x + 1; };
		auto rhs = [](Float x) {return  2 * cos(1 - x) * cos(x) + 2 * sin(1 - x) * sin(x); };
		auto d = [this](Float x) {return 1; };
		auto b = [](Float x) {return 0;	};
		auto c = [](Float x) {return 0; };

		Interval interval(0.0, 1.0);

		interval.SetPartitionCount(segement_);

		StaticFEM1DMG mg_linear(rhs, d, b, c, interval);
		Linear cg_linear(rhs, d, b, c, interval);
		mg_linear.evaluate();
		cg_linear.evaluate();
		for (int i = 0; i < Length; ++i)
		{
			cg_val[i] = cg_linear.Value(1.0 / (Length - 1) * i);
			mg_val[i] = mg_linear.Value(1.0 / (Length - 1) * i);

			cg_diff[i] = cg_val[i] - precise_val[i];
			mg_diff[i] = mg_val[i] - precise_val[i];
		}
	}

	void Control_UI();
	void draw(bool* p_open) override;

	std::vector<Point2d> points;
	bool updated = true;
	int segemnt = 16;
	float epsilon = 1E-7;

	void error(std::vector<float>& ref, std::vector<float>& eval, Float& L_1, Float& L_2, Float& L_inf)
	{
		assert(ref.size() == eval.size());

		std::vector<Float> minus(ref.size());

		for (int i = 0; i < ref.size(); ++i)
		{
			minus[i] = abs(ref[i] - eval[i]);
		}

		L_inf = *std::max_element(minus.begin(), minus.end(), [](Float a, Float b) {return a < b; });
		L_2 = sqrt(std::accumulate(minus.begin(), minus.end(), static_cast<Float>(0), [](Float r, Float a) {return r + a * a; }) / Float(ref.size()));
		L_1 = std::accumulate(minus.begin(), minus.end(), static_cast<Float>(0), [](Float r, Float a) {return r + a; }) / Float(ref.size());
	}

public:
	void CalcAccurateRst()
	{
		Float h = 1.0 / (Length - 1);
		for (int i = 0; i < Length; ++i)
		{
			xs[i] = i * h;

			auto accurate_func = [this](Float x) {return sin(x) * sin(1 - x); };

			precise_val[i] = accurate_func(xs[i]);
		}
	}

	FEM1DVisualizer() {
		CalcAccurateRst();
		segemnt = 16;
		Float L1, L2, L_inf;
		do
		{
			//std::cout << "Segmentations: " << segemnt << std::endl;
			evaluate();

			error(precise_val, mg_val, L1, L2, L_inf);

			using std::cout;
			using std::endl;

			//if (segemnt == 16)
			//{
			//	cout << segemnt << '&' << L1 << '&' << '-' << '&' << L2 << '&' << '-' << '&' << L_inf << '&' << '-' << "\\\\" << endl;
			//}
			//else
			//	cout << segemnt << '&' << L1 << '&' <<- log2(L1 / linear_L1.back()) << '&' << L2 << '&' << -log2(L2 / mg_L2.back()) << '&' << L_inf << '&' << -log2(L_inf / mg_Linf.back()) << "\\\\" << endl;

			pointcount.push_back(segemnt);
			mg_L1.push_back(L1);
			mg_L2.push_back(L2);
			mg_Linf.push_back(L_inf);
			error(precise_val, cg_val, L1, L2, L_inf);

			//if (segemnt == 16)
			//{
			//	cout << segemnt << '&' << L1 << '&' << '-' << '&' << L2 << '&' << '-' << '&' << L_inf << '&' << '-' << "\\\\" << endl;
			//}
			//else
			//	cout << segemnt << '&' << L1 << '&' << -log2(L1 / cg_L1.back()) << '&' << L2 << '&' << -log2(L2 / cg_L2.back()) << '&' << L_inf << '&' << -log2(L_inf / cg_Linf.back()) << "\\\\" << endl;

			cg_L1.push_back(L1);
			cg_L2.push_back(L2);
			cg_Linf.push_back(L_inf);

			segemnt *= 2;
		} while (segemnt != 8192);
		segemnt = 16;

		evaluate();
	}
	const size_t Length = 10001;

	std::vector<float> xs = std::vector<float>(Length);
	std::vector<float> precise_val = std::vector<float>(Length);
	std::vector<float> cg_val = std::vector<float>(Length);
	std::vector<float> mg_val = std::vector<float>(Length);
	std::vector<float> cg_diff = std::vector<float>(Length);
	std::vector<float> mg_diff = std::vector<float>(Length);

	std::vector<float> pointcount;
	std::vector<float> mg_L1;
	std::vector<float> mg_L2;
	std::vector<float> mg_Linf;

	std::vector<float> cg_L1;
	std::vector<float> cg_L2;
	std::vector<float> cg_Linf;
};

static inline ImVec2 operator-(const ImVec2& lhs, const ImVec2& rhs) { return ImVec2(lhs.x - rhs.x, lhs.y - rhs.y); }

void FEM1DVisualizer::Control_UI()
{
	if (ImGui::SliderInt("Number of segments", &segemnt, 3, 200))
	{
		segemnt = segemnt < 2 ? 2 : segemnt;
		evaluate();
	}
	if (ImGui::SliderFloat("Epsilon", &epsilon, 1E-7, 1E-1, "%.8f", ImGuiSliderFlags_Logarithmic))
	{
		evaluate();
		CalcAccurateRst();
	}
}

void FEM1DVisualizer::draw(bool* p_open)
{
	if (ImGui::BeginTabBar("FEM 1D App")) {
		if (ImGui::BeginTabItem("FEM1D"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				ImPlot::PlotLine("Precise solution", &xs[0], &precise_val[0], Length);
				ImPlot::PlotLine("FEM Conjugate Gradient", &xs[0], &cg_val[0], Length);
				ImPlot::PlotLine("FEM Multigrid", &xs[0], &mg_val[0], Length);

				ImPlot::EndPlot();
			}
			Control_UI();
			ImGui::EndTabItem();
		}
		if (ImGui::BeginTabItem("FEM1D difference"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				ImPlot::PlotLine("FEM diff Multigrid", &xs[0], &mg_diff[0], Length);
				ImPlot::PlotLine("FEM diff Conjugate Gradient", &xs[0], &cg_diff[0], Length);

				ImPlot::EndPlot();
			}
			Control_UI();
			ImGui::EndTabItem();
		}

		if (ImGui::BeginTabItem("Error Plot"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus, ImPlotAxisFlags_LogScale, ImPlotAxisFlags_LogScale)) {
				ImPlot::PlotLine("Multigrid L1    Error", &pointcount[0], &mg_L1[0], pointcount.size());
				ImPlot::PlotLine("Multigrid L2    Error", &pointcount[0], &mg_L2[0], pointcount.size());
				ImPlot::PlotLine("Multigrid L_inf Error", &pointcount[0], &mg_Linf[0], pointcount.size());

				ImPlot::PlotLine("Conjugate Gradient L1    Error", &pointcount[0], &cg_L1[0], pointcount.size());
				ImPlot::PlotLine("Conjugate Gradient L2    Error", &pointcount[0], &cg_L2[0], pointcount.size());
				ImPlot::PlotLine("Conjugate Gradient L_inf Error", &pointcount[0], &cg_Linf[0], pointcount.size());

				ImPlot::EndPlot();
			}

			ImGui::EndTabItem();
		}
		ImGui::EndTabBar();
	}

	ImGui::End();
}

int main()
{
	FEM1DVisualizer visualizer;
	visualizer.RenderLoop();
}


#include <Eigen/Dense>
extern "C"
{
#include "induct.h"
}

//#include <cmath>

/*Starts by applying QR decomposition using the householder transform and then applies
SVD to obtain the recompressed U and V matrices.
The Eigen Library is used to assist in computing QR and SVD.*/
int SVD_QR(double **U, double **V, int m, int n, int rank, double tol)
{
	using namespace Eigen;

	MatrixXd U2(m, rank);
	MatrixXd V2(n, rank);
	MatrixXd Q_u(m, rank);
	MatrixXd R_u(rank, rank);
	MatrixXd Q_v(n, rank);
	MatrixXd R_v(rank, rank);

	int new_rank = 0;

	for (int i = 0; i < rank; i++)
	{
		for (int j = 0; j < m; j++)
		{
			U2(j, i) = U[i][j];
		}
		for (int j = 0; j < n; j++)
		{
			V2(j, i) = V[i][j];
		}
	}

	HouseholderQR<MatrixXd> qr(U2);
	Q_u = qr.householderQ()* MatrixXd::Identity(m, rank);

	R_u = Q_u.transpose()*U2;

	HouseholderQR<MatrixXd> qr2(V2);
	//Q_v = V2.colPivHouseholderQr().householderQ().setLength(V2.colPivHouseholderQr().nonzeroPivots());
	Q_v = qr2.householderQ()*MatrixXd::Identity(n, rank);

	//R_v = V2.colPivHouseholderQr().matrixR().topLeftCorner(rank, rank).template triangularView<Upper>();
	R_v = Q_v.transpose()*V2;

	JacobiSVD<MatrixXd> svd(R_u*R_v.transpose(), ComputeFullU | ComputeFullV);
	//BDCSVD<MatrixXd> svd(R_u.block(0, 0, rank, rank)*R_v.transpose().block(0, 0, rank, rank), ComputeFullU | ComputeFullV);

	for (int i = 0; i < rank; i++)
	{
		if (svd.singularValues()(i) >= tol*svd.singularValues()(0))
		{
			++new_rank;
		}
		else
		{
			for (int j = 0; j < m; j++)
			{
				U[i][j] = 0;
			}
			for (int j = 0; j < n; j++)
			{
				V[i][j] = 0;
			}
		}
	}

	double temp_U = 0;
	double temp_V = 0;
	MatrixXd U_temp(new_rank, new_rank);

	for (int i = 0; i < new_rank; i++)
	{
		for (int j = 0; j < new_rank; j++)
		{
			U_temp(i, j) = svd.matrixU()(j, i)*svd.singularValues()(i);//check
		}
	}

	for (int i = 0; i < m; i++)
	{
		for (int k = 0; k < new_rank; k++)
		{

			for (int j = 0; j < new_rank; j++)
			{
				temp_U += Q_u(i, j)*U_temp(k, j);
			}
			U[k][i] = temp_U;
			temp_U = 0;
		}
	}

	for (int i = 0; i < n; i++)
	{
		for (int k = 0; k < new_rank; k++)
		{

			for (int j = 0; j < new_rank; j++)
			{
				temp_V += Q_v(i, j)*svd.matrixV()(j, k);
			}
			V[k][i] = temp_V;
			temp_V = 0;
		}
	}
	return new_rank;
}


//! [nbody-all]
#include <chrono>
#include <vector>
#include <iostream>
#include <cmath>

#include <boost/simd/pack.hpp>
#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/sqrt.hpp>

#include <boost/simd/memory/allocator.hpp>

typedef float T;
namespace bs = boost::simd;
using pack_t = bs::pack<T>;

struct particles
{
  std::vector<T, bs::allocator<T>> x, y, z, m, vx, vy, vz;
  std::size_t size_;

  particles(std::size_t size) : size_(size)
                              , x(size + pack_t::static_size)
                              , y(size + pack_t::static_size)
                              , z(size + pack_t::static_size)
                              , m(size + pack_t::static_size)
                              , vx(size + pack_t::static_size)
                              , vy(size + pack_t::static_size)
                              , vz(size + pack_t::static_size)
  {}

  std::size_t size() const {return size_;}
};

//! [nbody-simd]
void nbody_step(particles & ps)
{
  pack_t grav_con {6.67408e-11};
  pack_t epsilon {0.00125f};
  for(std::size_t i = 0 ; i <ps.size() ; i += pack_t::static_size)
  {
    pack_t ax{0};
    pack_t ay{0};
    pack_t az{0};

    pack_t pix{&ps.x[i]};
    pack_t piy{&ps.y[i]};
    pack_t piz{&ps.z[i]};

    pack_t pim {&ps.m[ i ]};

    for(std::size_t j = i + 1 ; j <ps.size() ; ++j)
    {
      auto pjx = bs::load<pack_t>(&ps.x[ j ]);
      auto pjy = bs::load<pack_t>(&ps.y[ j ]);
      auto pjz = bs::load<pack_t>(&ps.z[ j ]);

      auto dx = pjx - pix;
      auto dy = pjy - piy;
      auto dz = pjz - piz;

      auto inorm = grav_con / bs::sqrt(dx * dx + dy * dy + dz * dz + epsilon);
      auto inorm3 = inorm * inorm *inorm;

      auto fi = bs::load<pack_t>(&ps.m[ j ]) * inorm3;
      auto fj = pim * inorm3;

      ax += dx * fi;
      ay += dy * fi;
      az += dz * fi;

      auto pjvx = bs::load<pack_t>(&ps.vx[ j ]);
      pjvx -= dx * fj;
      bs::store(pjvx, &ps.vx[ j ]);
      auto pjvy = bs::load<pack_t>(&ps.vy[ j ]);
      pjvy -= dy * fj;
      bs::store(pjvy, &ps.vy[ j ]);
      auto pjvz = bs::load<pack_t>(&ps.vz[ j ]);
      pjvz -= dz * fj;
      bs::store(pjvz, &ps.vz[ j ]);
    }

    pack_t pivx {&ps.vx[ i ]};
    pack_t pivy {&ps.vy[ i ]};
    pack_t pivz {&ps.vz[ i ]};

    pivx += ax;
    pivy += ay;
    pivz += az;

    pix += pivx;
    piy += pivy;
    piz += pivz;

    bs::aligned_store(pivx, &ps.vx[ i ]);
    bs::aligned_store(pivy, &ps.vy[ i ]);
    bs::aligned_store(pivz, &ps.vz[ i ]);

    bs::aligned_store(pix, &ps.x[ i ]);
    bs::aligned_store(piy, &ps.y[ i ]);
    bs::aligned_store(piz, &ps.z[ i ]);
  }
}
//! [nbody-simd]

//! [nbody-scalar]
void nbody_step_scalar(particles & ps)
{
  T epsilon = 0.00125f;
  for(std::size_t i = 0 ; i <ps.size() ; ++i)
  {
    T ax{0};
    T ay{0};
    T az{0};

    T pix = ps.x[ i ];
    T piy = ps.y[ i ];
    T piz = ps.z[ i ];

    T pim = ps.m[ i ];

    for(std::size_t j = i + 1 ; j <ps.size() ; ++j)
    {
      T pjx = ps.x[ j ];
      T pjy = ps.y[ j ];
      T pjz = ps.z[ j ];

      T dx = pjx - pix;
      T dy = pjy - piy;
      T dz = pjz - piz;

      T inorm = 6.667408e-11 / std::sqrt(dx * dx + dy * dy + dz * dz + epsilon);

      T fi = ps.m[ j ] * inorm * inorm * inorm;
      T fj = pim * inorm * inorm * inorm;

      ax += dx * fi;
      ay += dy * fi;
      az += dz * fi;

      ps.vx[ j ] -= dx * fj;
      ps.vy[ j ] -= dy * fj;
      ps.vz[ j ] -= dz * fj;
    }

    T pivx = ps.vx[ i ];
    T pivy = ps.vy[ i ];
    T pivz = ps.vz[ i ];

    pivx += ax;
    pivy += ay;
    pivz += az;

    pix += pivx;
    piy += pivy;
    piz += pivz;

    ps.vx[ i ] = pivx;
    ps.vy[ i ] = pivy;
    ps.vz[ i ] = pivz;

    ps.x[ i ]  = pix;
    ps.y[ i ]  = piy;
    ps.z[ i ]  = piz;
  }
}
//! [nbody-scalar]

int main(int argc, char * argv[])
{
  std::size_t size  = 4096
    , steps = 10;

  if(argc != 3)
  {
    std::cerr <<"E: Incorrect number of arguments, using " <<size <<" particles and " <<steps <<" steps." <<std::endl;
  }
  else {
    size  = std::stoll(argv[ 1 ], 0, 10);
    steps = std::stoll(argv[ 2 ], 0, 10);
  }

  particles ps_scalar(size);
  particles ps_simd(size);

  for(std::size_t i = 0 ; i <size ; ++i)
  {
    ps_scalar.x[ i ] = (0.5f + i - size / 2) * 10.0f;
    ps_scalar.y[ i ] = 0.0f;
    ps_scalar.z[ i ] = 0.0f;
    ps_scalar.m[ i ] = 1.0f;
    ps_simd.x[ i ] = (0.5f + i - size / 2) * 10.0f;
    ps_simd.y[ i ] = 0.0f;
    ps_simd.z[ i ] = 0.0f;
    ps_simd.m[ i ] = 1.0f;
  }

  auto start = std::chrono::system_clock::now();

  for(std::size_t step = 0 ; step <steps ; ++step)
  {
    nbody_step_scalar(ps_scalar);
  }

  auto stop = std::chrono::system_clock::now();

  auto duration = stop - start;

  std::cout <<" Time scalar: " <<std::chrono::duration_cast<std::chrono::milliseconds>(duration).count() <<std::endl;

  start = std::chrono::system_clock::now();

  for(std::size_t step = 0 ; step <steps ; ++step)
  {
    nbody_step(ps_simd);
  }

  stop = std::chrono::system_clock::now();

  duration = stop - start;

  std::cout <<" Time SIMD: " <<std::chrono::duration_cast<std::chrono::milliseconds>(duration).count() <<std::endl;

  if(size < 64)
  {
    for(std::size_t i = 0 ; i <ps_scalar.size() ; ++i)
    {
      std::cout <<"(" <<ps_scalar.x[ i ] <<", " <<ps_scalar.y[ i ] << ", " <<ps_scalar.z[ i ] <<")" <<std::endl;
      std::cout <<"(" <<ps_simd.x[ i ] <<", " <<ps_simd.y[ i ] << ", " <<ps_simd.z[ i ] <<")" <<std::endl;
    }
  }
  return 0;
}
//! [nbody-all]


#include <iostream>
#include <cmath>

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <chrono>
using namespace std::chrono;
using namespace std;

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

// TO-DO: 搞清楚四元数的顺序问题，明天去填下。

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    // 构造函数初始化
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    // 保留三位小数！
    cout .precision(3);
    // 将旋转向量用矩阵的形式表征出来
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 将旋转向量形式转换成旋转矩阵形式
    cout<<"rotation matrix =\n"<<rotation_vector.toRotationMatrix() <<endl;
    rotation_matrix = rotation_vector.toRotationMatrix();

    // 定义了一个待旋转的向量
    Eigen::Vector3d v ( 1,0,0 );
    // 这里应该是重载<<的时候存在顺序的问题，因此需要加上transpose来正确显示想要展示的内容
    cout<<"(1,0,0) before rotation = "<<v.transpose()<<endl;
    // 用 AngleAxis 可以进行坐标变换
    // 里面应该是重载了*运算符
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵，效果一样，同理重载
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    // 这里的2,1,0分别代指ZYX，表示先后以这三个为轴旋转
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧式变换，包括了旋转与平移
    // 欧氏变换矩阵使用 Eigen::Isometry（等距）
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();    // 虽然称为3d，实质上是4＊4的矩阵
    // 通过调用成员函数的方式来赋值
    T.rotate ( rotation_vector );                         // 按照rotation_vector进行旋转
    // pretranslate函数用于inital position setting
    // Applies on the right the translation matrix represented by the vector
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );         // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    Eigen::Matrix3d rotation_T = T.rotation();
    cout<<"---- T ---- rotation matrix  =\n"<<rotation_T <<endl;

    Eigen::Quaterniond q_T = Eigen::Quaterniond ( rotation_T );
    cout<<"---- T ---- quaternion = \n"<<q_T.coeffs() <<endl;

    Eigen::Vector3d translation_T = T.translation();
    cout<<"---- T ---- translation = \n"<<translation_T.transpose() <<endl;

    // 用变换矩阵进行坐标变换，v是（1，0，0）
    Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t，理解！
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;         // 结果正确

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然，也就是用角轴转成四元数
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部，顺序不一样

    Eigen::Quaterniond q1(0.924, 0, 0, 0.383); // 赋值的时候是w,x,y,z
    cout<<"quaternion1 = \n"<<q1.coeffs() <<endl;

    // 也可以把旋转矩阵赋给它，结果一样！
    Eigen::Quaterniond q2 = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q2.coeffs() <<endl;

    cout<<"--- rotation matrix =\n"<<q2.toRotationMatrix() <<endl;

    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl; //结果正确

    auto t0 = std::chrono::high_resolution_clock::now();
    Eigen::Quaterniond quat = Eigen::Quaterniond ( T.rotation() );
    Eigen::Vector3d pos = T.translation();
    std::cout <<"time is " << std::chrono::duration_cast<std::chrono::nanoseconds> (std::chrono::high_resolution_clock::now() - t0).count()<< '\n';

    Eigen::Matrix4d T_m = Eigen::Matrix4d::Identity();
    T_m.block<3, 3>(0, 0) = rotation_matrix;
    T_m.block<3, 1>(0, 3) = Eigen::Vector3d ( 1,3,4 );
    cout << "Transform matrix = \n" << T_m <<endl;
    cout << "Rotation matrix= \n" << T_m.block<3, 3>(0, 0) << endl;
    cout << "Translation matrix= \n" << T_m.block<3, 1>(0, 3) << endl;

    // Test if Quaterniond only takes normalized rotation matrix
    cout << "Rotation matrix= \n" << rotation_matrix << endl;
    rotation_matrix *= 2.0;
    cout << "Rotation matrix= \n" << rotation_matrix << endl;
    double scale = std::sqrt((rotation_matrix.transpose() * rotation_matrix)(0, 0));
    Eigen::Matrix3d ret = (1. / scale) * rotation_matrix;
    Eigen::Matrix3d rotation_matrix_quat = Eigen::Quaterniond ( ret ).normalized().toRotationMatrix();;
    cout << "Rotation matrix= \n" << rotation_matrix_quat << endl;

    return 0;
}