/**
 * @file upwindfinitevolume.cc
 * @brief NPDE homework UpwindFiniteVolume code
 * @author Philipp Egg
 * @date 08.09.2020
 * @copyright Developed at ETH Zurich
 */

#include "upwindfinitevolume.h"

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

namespace UpwindFiniteVolume {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 2, 3> gradbarycoordinates(
    const Eigen::Matrix<double, 2, 3> &triangle) {
  Eigen::Matrix3d X;
  // Solve for the coefficients of the barycentric coordinate functions
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = triangle.transpose();
  return X.inverse().block<2, 3>(1, 0);
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::Vector2d computeCircumcenters(const Eigen::Vector2d &a1,
                                     const Eigen::Vector2d &a2,
                                     const Eigen::Vector2d &a3) {
#if SOLUTION

  Eigen::Vector2d mp1 = 0.5 * (a1 + a2);
  Eigen::Vector2d mp2 = 0.5 * (a2 + a3);

  Eigen::Vector2d dir1 = a2 - a1;
  Eigen::Vector2d dir2 = a3 - a2;

  Eigen::Matrix2d dir;
  dir << dir1(1), dir2(1), -dir1(0), -dir2(0);

  Eigen::Vector2d p = dir.colPivHouseholderQr().solve(mp2 - mp1);

  Eigen::Vector2d center = mp1 + p(0) * dir.col(0);

  // Barycentric coordinates from
  // Christer Ericson’s ’Real-Time Collision Detection’

  Eigen::Vector2d v0 = a2 - a1, v1 = a3 - a1, v2 = center - a1;
  double d00 = v0.dot(v0);
  double d01 = v0.dot(v1);
  double d11 = v1.dot(v1);
  double d20 = v2.dot(v0);
  double d21 = v2.dot(v1);

  double denom = d00 * d11 - d01 * d01;
  double v = (d11 * d20 - d01 * d21) / denom;
  double w = (d00 * d21 - d01 * d20) / denom;
  double u = 1.0 - v - w;

  if (v <= 0 || w <= 0 || u <= 0) {
    throw std::runtime_error("Obtused triangle!");
    return Eigen::Vector2d::Zero();
  } else {
    return center;
  }

  // Alternative:
  /*
  // Calculate the midpoint of the edges
  const Eigen::Vector2d midpoint1 = (a1 + a2) * 0.5;
  const Eigen::Vector2d midpoint2 = (a2 + a3) * 0.5;
  const Eigen::Vector2d midpoint3 = (a1 + a3) * 0.5;

  // Calculate the slope of the line perpendicular to the edges
  Eigen::Matrix<double, 2, 3> corners;
  corners.col(0) = a1;
  corners.col(1) = a2;
  corners.col(2) = a3;

  Eigen::MatrixXd barycenters = gradbarycoordinates(corners);

  // Reorder the vectors corresponding to the edges
  Eigen::Vector2d n1 = barycenters.col(2).normalized();
  Eigen::Vector2d n2 = barycenters.col(0).normalized();
  Eigen::Vector2d n3 = barycenters.col(1).normalized();

  // Check obtuse triangle
  double alpha1 = acos(n1.dot(-n3));
  double alpha2 = acos(n1.dot(-n2));
  double alpha3 = acos(n2.dot(-n3));

  double rad_90deg = M_PI / 2.0;
  if (alpha1 >= rad_90deg || alpha2 >=  rad_90deg || alpha3 >=  rad_90deg) {
    std::cout << "Obtused triangle!" << std::endl;
    std::cout << "alpha1 " << alpha1 * 180.0 / M_PI << "degree" << std::endl;
    std::cout << "alpha2 " << alpha2 * 180.0 / M_PI << "degree" << std::endl;
    std::cout << "alpha3 " << alpha3 * 180.0 / M_PI << "degree" << std::endl;
    throw std::runtime_error("Obtused triangle!");
  }

  // Compute intersection the two vectors
  // midpoint1 + n1 * t1 = midpoint2 + n2 * t2
  // (midpoint1x - midpoint2x) = t2 * n2x - t1 * n1x
  // (midpoint1y - midpoint2y) = t2 * n2y - t1 * n1y
  // solving for t1:
  double t1 = ((midpoint1[0] - midpoint2[0]) * n2[1] -
               (midpoint1[1] - midpoint2[1]) * n2[0]) /
              (n1[1] * n2[0] - n1[0] * n2[1]);

  Eigen::Vector2d intersection = midpoint1 + n1 * t1;

  return intersection;
  */
#else
  //====================
  // Your code goes here
  //====================
  return Eigen::Vector2d::Zero();
#endif
}
/* SAM_LISTING_END_2 */

}  // namespace UpwindFiniteVolume


#ifndef SE3_HPP_
#define SE3_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif
#include <math.h>

const double SMALL_EPS = 1e-10;

typedef Eigen::Matrix<double, 6, 1, Eigen::ColMajor> Vector6d;
typedef Eigen::Matrix<double, 7, 1, Eigen::ColMajor> Vector7d;

inline Eigen::Matrix3d skew(const Eigen::Vector3d&v)
{
    Eigen::Matrix3d m;
    m.fill(0.);
    m(0,1)  = -v(2);
    m(0,2)  =  v(1);
    m(1,2)  = -v(0);
    m(1,0)  =  v(2);
    m(2,0) = -v(1);
    m(2,1) = v(0);
    return m;
}

inline Eigen::Vector3d deltaR(const Eigen::Matrix3d& R)
{
    Eigen::Vector3d v;
    v(0)=R(2,1)-R(1,2);
    v(1)=R(0,2)-R(2,0);
    v(2)=R(1,0)-R(0,1);
    return v;
}

inline Eigen::Vector3d toAngleAxis(const Eigen::Quaterniond& quaterd, double* angle=NULL)
{
    Eigen::Quaterniond unit_quaternion = quaterd.normalized();
    double n = unit_quaternion.vec().norm();
    double w = unit_quaternion.w();
    double squared_w = w*w;

    double two_atan_nbyw_by_n;
    // Atan-based log thanks to
    //
    // C. Hertzberg et al.:
    // "Integrating Generic Sensor Fusion Algorithms with Sound State
    // Representation through Encapsulation of Manifolds"
    // Information Fusion, 2011

    if (n < SMALL_EPS)
    {
        // If quaternion is normalized and n=1, then w should be 1;
        // w=0 should never happen here!
        assert(fabs(w)>SMALL_EPS);

        two_atan_nbyw_by_n = 2./w - 2.*(n*n)/(w*squared_w);
    }
    else
    {
        if (fabs(w)<SMALL_EPS)
        {
            if (w>0)
            {
                two_atan_nbyw_by_n = M_PI/n;
            }
            else
            {
                two_atan_nbyw_by_n = -M_PI/n;
            }
        }
        two_atan_nbyw_by_n = 2*atan(n/w)/n;
    }
    if(angle!=NULL) *angle = two_atan_nbyw_by_n*n;
    return two_atan_nbyw_by_n * unit_quaternion.vec();
}

inline Eigen::Quaterniond toQuaterniond(const Eigen::Vector3d& v3d, double* angle = NULL)
{
    double theta = v3d.norm();
    if(angle != NULL)
        *angle = theta;
    double half_theta = 0.5*theta;

    double imag_factor;
    double real_factor = cos(half_theta);
    if(theta<SMALL_EPS)
    {
        double theta_sq = theta*theta;
        double theta_po4 = theta_sq*theta_sq;
        imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4;
    }
    else
    {
        double sin_half_theta = sin(half_theta);
        imag_factor = sin_half_theta/theta;
    }

    return Eigen::Quaterniond(real_factor,
                              imag_factor*v3d.x(),
                              imag_factor*v3d.y(),
                              imag_factor*v3d.z());
}

class SE3 {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

protected:

    Eigen::Quaterniond _r;
    Eigen::Vector3d _t;

public:
    SE3(){
        _r.setIdentity();
        _t.setZero();
    }

    SE3(const Eigen::Matrix3d& R, const Eigen::Vector3d& t):_r(Eigen::Quaterniond(R)),_t(t){
        normalizeRotation();
    }

    SE3(const Eigen::Quaterniond& q, const Eigen::Vector3d& t):_r(q),_t(t){
        normalizeRotation();
    }

    inline const Eigen::Vector3d& translation() const {return _t;}

    inline Eigen::Vector3d& translation() {return _t;}

    inline void setTranslation(const Eigen::Vector3d& t_) {_t = t_;}

    inline const Eigen::Quaterniond& rotation() const {return _r;}

    inline Eigen::Quaterniond& rotation() {return _r;}

    void setRotation(const Eigen::Quaterniond& r_) {_r=r_;}

    inline SE3 operator* (const SE3& tr2) const{
        SE3 result(*this);
        result._t += _r*tr2._t;
        result._r*=tr2._r;
        result.normalizeRotation();
        return result;
    }

    inline SE3& operator*= (const SE3& tr2){
        _t+=_r*tr2._t;
        _r*=tr2._r;
        normalizeRotation();
        return *this;
    }

    inline Eigen::Vector3d operator* (const Eigen::Vector3d& v) const {
        return _t+_r*v;
    }

    inline SE3 inverse() const{
        SE3 ret;
        ret._r=_r.conjugate();
        ret._t=ret._r*(_t*-1.);
        return ret;
    }

    inline double operator [](int i) const {
        assert(i<7);
        if (i<4)
            return _r.coeffs()[i];
        return _t[i-4];
    }

    inline Vector7d toVector() const{
        Vector7d v;
        v.head<4>() = Eigen::Vector4d(_r.coeffs());
        v.tail<3>() = _t;
        return v;
    }

    inline void fromVector(const Vector7d& v){
        _r=Eigen::Quaterniond(v[3], v[0], v[1], v[2]);
        _t=Eigen::Vector3d(v[4], v[5], v[6]);
    }

    Vector6d log() const {
        Vector6d res;

        double theta;
        res.head<3>() = toAngleAxis(_r, &theta);

        Eigen::Matrix3d Omega = skew(res.head<3>());
        Eigen::Matrix3d V_inv;
        if (theta<SMALL_EPS)
        {
            V_inv = Eigen::Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);
        }
        else
        {
            V_inv = ( Eigen::Matrix3d::Identity() - 0.5*Omega
                      + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) );
        }

        res.tail<3>() = V_inv*_t;

        return res;
    }

    Eigen::Vector3d map(const Eigen::Vector3d & xyz) const
    {
        return _r*xyz + _t;
    }

    static SE3 exp(const Vector6d & update)
    {
        Eigen::Vector3d omega(update.data());
        Eigen::Vector3d upsilon(update.data()+3);

        double theta;
        Eigen::Matrix3d Omega = skew(omega);

        Eigen::Quaterniond R = toQuaterniond(omega, &theta);
        Eigen::Matrix3d V;
        if (theta<SMALL_EPS)
        {
            V = R.matrix();
        }
        else
        {
            Eigen::Matrix3d Omega2 = Omega*Omega;

            V = (Eigen::Matrix3d::Identity()
                 + (1-cos(theta))/(theta*theta)*Omega
                 + (theta-sin(theta))/(pow(theta,3))*Omega2);
        }
        return SE3(R, V*upsilon);
    }

    Eigen::Matrix<double, 6, 6, Eigen::ColMajor> adj() const
    {
        Eigen::Matrix3d R = _r.toRotationMatrix();
        Eigen::Matrix<double, 6, 6, Eigen::ColMajor> res;
        res.block(0,0,3,3) = R;
        res.block(3,3,3,3) = R;
        res.block(3,0,3,3) = skew(_t)*R;
        res.block(0,3,3,3) = Eigen::Matrix3d::Zero(3,3);
        return res;
    }

    Eigen::Matrix<double,4,4,Eigen::ColMajor> to_homogeneous_matrix() const
    {
        Eigen::Matrix<double,4,4,Eigen::ColMajor> homogeneous_matrix;
        homogeneous_matrix.setIdentity();
        homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();
        homogeneous_matrix.col(3).head(3) = translation();

        return homogeneous_matrix;
    }

    void normalizeRotation(){
        if (_r.w()<0){
            _r.coeffs() *= -1;
        }
        _r.normalize();
    }
};

#endif // SE3_HPP_


#ifndef RICPAD_SOLVER
#define RICPAD_SOLVER

#include <iostream>
#include <vector>
#include <math.h>

#include <Eigen/Dense>

#include <differentiate.hpp>

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

namespace solver {
template<typename num_t>
class Solver {
    private:
        // Function
        const std::function<num_t(num_t)> f_;
        // Derivative of the function
        const std::function<num_t(num_t)> df_;
        // Step size
        num_t h_;
        // Tolerance of the result
        num_t tol_;

    public:
        // Construct a Solver object from the function and its derivative
        Solver(
            const std::function<num_t(num_t)> f,
            const std::function<num_t(num_t)> df
            ) :

            f_(f), df_(df)
            {
                h_   = std::sqrt(std::numeric_limits<num_t>::epsilon());
                tol_ = std::sqrt(h_);
            }

        // Solve f = 0 for the initial value of x0
        num_t solve(num_t x0) {
            num_t desv, fx, dfx, x = x0, xold;

            desv = 2*tol_;

            while ( desv > tol_ ) {
                fx  = f_(x);
                dfx = df_(x);

                xold = x;
                x = x - fx/dfx;

                std::cout << x << "\n";

                desv = std::abs(x - xold);
            }

            return x;
        }
};

// Solver for N equations using a real type R
template <
    typename C, // complex number type
    typename R, // real number type (for tolerance parameters, etc.)
    int N       // number of equations and unknowns
>
class Solver2 {
    private:
        // Vector with the functions
        const std::vector<std::function<C(Eigen::Matrix<C,N,1>&)>> f_;
        //Eigen::Matrix<C,N,1> x0;
        // Accepted difference between two iterations
        R tol_;
        // Numerical value for the differentiation step (only relevant when
        // using numerical differentiation)
        R h_;
        // Maximum number of iterations
        int maxiter_ = 100;

    public:
        //----------------------------------------------------------------------
        // Construct a Solver2 object from a vector of functions.
        // Differentiation is performed numerically.
        Solver2(
            const std::vector<std::function<C(Eigen::Matrix<C,N,1>&)>> &f
            ) :

            f_(f) {
                //h_   = std::sqrt(std::numeric_limits<R>::epsilon());
                h_   = 1e-12;
                tol_ = 1e-8;
            };

        //----------------------------------------------------------------------
        // Setters
        void set_h(R h) {h_ = h;};
        void set_tol(R tol) {tol_ = tol;};
        void set_maxiter(int maxiter) {maxiter_ = maxiter;};

        //----------------------------------------------------------------------
        // Solve for f using x0 as initial value
        Eigen::Matrix<C,N,1> solve(
            Eigen::Matrix<C,N,1> x0
            )
        {

            std::cout << "h = " << h_ << "\n"
                << "tol = " << tol_ << "\n\n";

            using solver::differentiate;
            Eigen::Matrix<C, N, N> jacobian, inv_jacobian;
            Eigen::Matrix<C, N, 1> x(x0), xold;
            R desv = tol_ + 1;

            int niter = 0;

            C val;

            while ( desv > tol_ ) {
                for ( int i = 0; i < x.size(); i++ ) {
                    for ( int j = 0; j < x.size(); j++ ) {
                        val = differentiate<C, N>(f_[i], x, j, h_);
                        jacobian(i, j) = std::move(val);
                    }
                }

                inv_jacobian = jacobian.inverse();

                Eigen::Matrix<C, N, 1> F;

                for ( int i = 0; i < N; i++ ) F(i) = f_[i](x);
                /*
                for ( int i = 0; i < N; i++ ) {
                    std::cout << std::setprecision(15) << x(i) << " ";
                }
                std::cout << "\n";
                */
                xold = x;
                x = x - inv_jacobian * F;

                desv = (x - xold).norm();
                std::cout << "desv = " << desv << "\n";

                if ( niter++ > maxiter_ ) {
                    std::cout << "Maximum number of iterations reached.\n";
                    return x;
                }
            }

            return x;
        };

        /* Construct a Solver2 object from a function and its derivative
        Solver2(
            const std::function<R(R)> f,
            const std::function<R(R)> df
            )
        */
};

}; // namespace solver

#endif


/*
Copyright (c) 2017 InversePalindrome
InPal - MathSolver.hpp
InversePalindrome.com
*/

#pragma once

#include "Exprtk.hpp"
#include "GCD.hpp"
#include "LCM.hpp"
#include "PrimeTest.hpp"

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

#include <iostream>

template<typename T>
class MathSolver
{
public:
    MathSolver();
    MathSolver(const std::string& task);

    bool solve();

    T getValue() const;
    std::string getTask() const;

    T getVariable(const std::string& variableName);
    std::string getStringVariable(const std::string& variableName);

    T getDerivative(T& variable);
    T getSecondDerivative(T& variable);
    T getThirdDerivative(T& variable);

    T getIntegral(T& variable, T initialX, T finalX);

    void setTask(const std::string& task);

    void addVariable(const std::string& variableName, T& variable);
    void addConstant(const std::string& constantName, T& constant);
    void addStringVar(const std::string& stringVariableName, std::string& variable);
    void addFunction(const std::string& functionName, exprtk::ifunction<T>& function);
    void addCompositorFunction(const std::string& functionName, const std::vector<std::string>& parameters, const std::string& functionBody);

    void removeVariable(const std::string& variableName);
    void removeStringVar(const std::string& stringVariableName);
    void removeFunction(const std::string& functionName);

    void clearTask();
    void clearSymbols();

private:
    exprtk::parser<T> parser;
    exprtk::expression<T> expression;
    exprtk::symbol_table<T> symbolTable;
    exprtk::function_compositor<T> compositor;

    exprtk::parser_error::type error;

    std::string task;

    GCD<T> gcd;
    LCM<T> lcm;
    PrimeTest<T> primeTest;

    void loadConstants();
    void loadFunctions();
};

template<typename T>
MathSolver<T>::MathSolver() :
    MathSolver("")
{
}

template<typename T>
MathSolver<T>::MathSolver(const std::string& task) :
    parser(),
    expression(),
    symbolTable(),
    compositor(symbolTable),
    task(task),
    error()
{
    expression.register_symbol_table(symbolTable);

    loadConstants();
    loadFunctions();
}

template<typename T>
bool MathSolver<T>::solve()
{
    if (!this->parser.compile(this->task, this->expression))
    {
        for (std::size_t i = 0; i < this->parser.error_count(); ++i)
        {
            this->error = this->parser.get_error(i);

            std::cerr << "Error: " << i << " [LINE]: " << this->error.line_no << " [COL]: " << this->error.column_no << " [POS]: " << this->error.token.position
                << " Type: " << exprtk::parser_error::to_str(this->error.mode).c_str() << " Message: " << this->error.diagnostic.c_str() << "\n";
        }

        return false;
    }

    return true;
}

template<typename T>
T MathSolver<T>::getValue() const
{
    return this->expression.value();
}

template<typename T>
std::string MathSolver<T>::getTask() const
{
    return this->task;
}

template<typename T>
T MathSolver<T>::getVariable(const std::string& variableName)
{
    return this->symbolTable.get_variable(variableName)->ref();
}

template<typename T>
std::string MathSolver<T>::getStringVariable(const std::string& variableName)
{
    return this->symbolTable.get_stringvar(variableName)->value();
}

template<typename T>
T MathSolver<T>::getDerivative(T& variable)
{
    return exprtk::derivative(this->expression, variable);
}

template<typename T>
T MathSolver<T>::getSecondDerivative(T& variable)
{
    return exprtk::second_derivative(this->expression, variable);
}

template<typename T>
T MathSolver<T>::getThirdDerivative(T& variable)
{
    return exprtk::third_derivative(this->expression, variable);
}

template<typename T>
T MathSolver<T>::getIntegral(T& variable, T initialX, T finalX)
{
    return exprtk::integrate(this->expression, variable, initialX, finalX);
}

template<typename T>
void MathSolver<T>::setTask(const std::string& task)
{
    this->task = task;
}

template<typename T>
void MathSolver<T>::addVariable(const std::string& variableName, T& variable)
{
    this->symbolTable.add_variable(variableName, variable);
}

template<typename T>
void MathSolver<T>::addConstant(const std::string& constantName, T& variable)
{
    this->symbolTable.add_constant(constantName, variable);
}

template<typename T>
void MathSolver<T>::addStringVar(const std::string& stringVariableName, std::string& variable)
{
    this->symbolTable.add_stringvar(stringVariableName, variable);
}

template<typename T>
void MathSolver<T>::addFunction(const std::string& functionName, exprtk::ifunction<T>& function)
{
    this->symbolTable.add_function(functionName, function);
}

template<typename T>
void MathSolver<T>::addCompositorFunction(const std::string& functionName, const std::vector<std::string>& parameters, const std::string& functionBody)
{
    exprtk::function_compositor<T>::function function(functionName, functionBody);

    for (const auto& parameter : parameters)
    {
        function.var(parameter);
    }

    this->compositor.add(function, true);
}

template<typename T>
void MathSolver<T>::removeVariable(const std::string& variableName)
{
    this->symbolTable.remove_variable(variableName);
}

template<typename T>
void MathSolver<T>::removeStringVar(const std::string& stringVariableName)
{
    this->symbolTable.remove_stringvar(stringVariableName);
}

template<typename T>
void MathSolver<T>::removeFunction(const std::string& functionName)
{
    this->symbolTable.remove_function(functionName);
}

template<typename T>
void MathSolver<T>::clearTask()
{
    this->task.clear();
}

template<typename T>
void MathSolver<T>::clearSymbols()
{
    this->symbolTable.clear();
}

template<typename T>
void MathSolver<T>::loadConstants()
{
    this->symbolTable.add_constants();
    this->symbolTable.add_constant("e", boost::math::constants::e<long double>());
}

template<typename T>
void MathSolver<T>::loadFunctions()
{
    this->symbolTable.add_function("gcd", this->gcd);
    this->symbolTable.add_function("lcm", this->lcm);
    this->symbolTable.add_function("is_prime", this->primeTest);
}


#include <iostream>
#include <cstdio>
#include <armadillo>
#include <ctime>
#include <limits>
#include <unistd.h>

#include "comp_eig.hh"

using namespace std;
using namespace arma;

const double rmin = 1e-10;
const double rmax = 60;
const double epsilon = 1e-10;

// potential function (non-intercting)
static constexpr double V0(double r) {
  return r * r;
}

// potential function (interacting)
static constexpr double Vw(double r, double w) {
  return w * w * r * r + 1 / r;
}

// calculate maximal off-diagonal element with respect to absolute value
static double maxoff(const mat &A, size_t &i, size_t &j) {
  i = 1; j = 0; // indices of maximal lement
  double a = 0; // maximal element absolute value

  SizeMat s = size(A);

  for(size_t k = 0; k < s.n_rows; k++) {
    for(size_t l = 0; l < s.n_cols; l++) {
      if(k == l) continue;

      const double x = A(k, l);
      const double y = x * x;
      if(y > a)
        i = k, j = l, a = y;
    }
  }

  return a;
}

static void apply_rot_col(size_t k, size_t l, double t, mat &M) {
  if(k == l) return;

  const double c = 1 / sqrt( 1 + t * t );
  const double s = t * c;

  colvec kvec = c * M.col(k) - s * M.col(l);
  colvec lvec = s * M.col(k) + c * M.col(l);

  M.col(k) = kvec;
  M.col(l) = lvec;
}

static void apply_rot_row(size_t k, size_t l, double t, mat &M) {
  if(k == l) return;

  const double c = 1 / sqrt( 1 + t * t );
  const double s = t * c;

  rowvec kvec = c * M.row(k) - s * M.row(l);
  rowvec lvec = s * M.row(k) + c * M.row(l);

  M.row(k) = kvec;
  M.row(l) = lvec;
}

// solve with Jacobi's method
// A is input matrix, P is eigenvector matrix (with column vector eigenvectors), L is eigenvalue vector
static void jacobi_solve(const mat &A, mat &P, vec &L, size_t &steps, double &step_time) {
  // get N from A matrix
  const size_t N = A.n_rows;
  assert(A.n_cols == N);

  std::cout << "N=" << N << std::endl;

  // B is similar matrix to A through matrix transforms
  mat B = A;

  // S is similarity transform matrix
  mat S(size(A));

  P.eye(size(A));

  clock_t time_start, time_end;
  double time_total = 0;
  steps = 0;
  while(true) {
    // timing
    time_start = clock();

    // calculate maximal off-diagonal element with respect to absolute value
    size_t k, l;
    const double a = maxoff(B, k, l);

    // if less than epsilon, stop
    if(a < epsilon)
      break;

    // calculate sin θ and cos θ
    const double tau = (B(l, l) - B(k, k)) / (2 * B(k, l));
    const double t = (tau >= 0 ? - tau - sqrt( 1 + tau * tau ) : - tau + sqrt( 1 + tau * tau ));

    // apply similarity transform
    //   B = S.t() * B * S;
    apply_rot_col(k, l, t, B);
    apply_rot_row(k, l, t, B);

    // apply S^T to P
    //   P = P * S;
    apply_rot_col(k, l, t, P);

    // timing
    time_end = clock();
    time_total += (double)(time_end - time_start) / CLOCKS_PER_SEC;
    steps++;
  }

  // export steps and time per step
  step_time = time_total / steps;

  std::cout << "JACOBI DONE (N = " << N << ", steps = " << steps << ", time per step = " << (1000 * step_time) << "ms)" << std::endl;

  // find eigenvalues
  L.set_size(N);
  for(size_t i = 0; i < N; i++)
    L(i) = B(i, i);
}

int run_program(size_t N, double w, const std::function<double(double)> &V) {
  const double h = (rmax - rmin) / N;

  // array of ρ valuses
  vec r(N);
  for(size_t i = 0; i < N; i++)
    r(i) = rmin + h * i;

  // calculate e_i, which are all the same
  double e = - 1 / (h * h);

  // calculate d_i, which depend on V(ρ_i)
  vec d(N);
  for(size_t i = 0; i < N; i++)
    d(i) = 2 / (h * h) + V(r[i]);

  // calculate matrix A
  mat A(N, N, arma::fill::zeros);
  for(size_t i = 0; i < N; i++) {
    A(i, i) = d(i);

    if(i > 0)
      A(i, i-1) = e;
    if(i < N - 1)
      A(i, i+1) = e;
  }

  // solve with Armadillo's eig_sym
  vec eigenvalues;
  mat eigenvectors;
  eig_sym(eigenvalues, eigenvectors, A);

  // normalize w.r.t. N
  eigenvectors /= sqrt(h);

  // find lowest eigenvalue
  size_t lowest_index = 0;
  double lowest_eigenvalue = eigenvalues(0);
  for(size_t i = 1; i < N; i++) {
    double l = eigenvalues(i);
    if(l < lowest_eigenvalue) {
      lowest_index = i; lowest_eigenvalue = l;
    }
  }

  // save lowest eigenvector to file
  char filename[20];
  snprintf(filename, 20, "d-%.2lf.dat", w);
  FILE *fp = fopen(filename, "w");
  fprintf(fp, "r          u\n");
  for(size_t i = 0; i < N; i++) {
    fprintf(fp, "%-10.5g %-10.5g\n", r(i), eigenvectors(i, lowest_index));
  }
  fclose(fp);
}

int main(int argc, char **argv) {
  const size_t N = 800;
  const double Wvalues[] = { 0.01, 0.5, 1, 5 };
  const size_t Wlen = sizeof(Wvalues) / sizeof(*Wvalues);

  // run for non-interacting case
  std::cout << "running for non-interactive case" << std::endl;
  run_program(N, 0, V0);

  // run for interacting cases
  for(size_t i = 0; i < Wlen; i++) {
    double W = Wvalues[i];
    std::cout << "running with W = " << W << std::endl;
    const auto V = [W] (double r) -> double { return Vw(r, W); };
    run_program(N, W, V);
  }
}


/*!
 * @file
 * @brief
 * @copyright alphya 2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_AUTO_DIFF_HPP
#define NYARUGA_UTIL_AUTO_DIFF_HPP

#pragma once

#include <nyaruga_util/config.hpp>

#ifdef NYARUGA_UTIL_HAS_EIGEN

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

// 二重数を用いた自動微分ライブラリ
// TODO: std::xx -> xx (using ADL)

namespace nyaruga {

namespace util {

namespace detail {

template <std::floating_point R = double>
constexpr auto make_init_value(int dim, int var_no) noexcept
{
    Eigen::Matrix<R,Eigen::Dynamic,1> tmp = Eigen::Matrix<R,Eigen::Dynamic,1>::Zero(dim);
    tmp(var_no - 1) = 1.;
    return tmp;
}

} // namespace detail

template <std::floating_point R = double>
struct var {
	using value_type = R;
	using diff_value_type = Eigen::Matrix<R,Eigen::Dynamic,1>;
	value_type val;
	diff_value_type dval;
	constexpr var() noexcept = default;
	constexpr var(value_type val_, size_t dim, size_t var_no) noexcept : val(val_), dval(detail::make_init_value(dim, var_no)) {}
	constexpr var(value_type val_, diff_value_type dval_) noexcept : val(val_), dval(dval_) {}
	friend constexpr auto operator<=>(const var&, const var&) noexcept = default;
};

template <std::floating_point R>
constexpr auto operator + (const var<R>& lhs, const var<R>& rhs) noexcept
{
	return var(lhs.val + rhs.val, static_cast<var<R>::diff_value_type>(lhs.dval + rhs.dval));
}

template <std::floating_point R>
constexpr auto operator - (const var<R>& lhs, const var<R>& rhs) noexcept
{
	return var(lhs.val - rhs.val, static_cast<var<R>::diff_value_type>(lhs.dval - rhs.dval));
}

template <std::floating_point R>
constexpr auto operator * (const var<R>& lhs, const var<R>& rhs) noexcept
{
	return var(lhs.val * rhs.val, static_cast<var<R>::diff_value_type>(rhs.val*lhs.dval + lhs.val*rhs.dval));
}

// rhs.val != 0
template <std::floating_point R>
constexpr auto operator / (const var<R>& lhs, const var<R>& rhs)
{
	return var(lhs.val / rhs.val, static_cast<var<R>::diff_value_type>((lhs.dval*rhs.val - lhs.val*rhs.dval)/std::pow(rhs.val,2)));
}

template <std::floating_point R>
constexpr auto sin(const var<R>& x) noexcept
{
	return var(std::sin(x.val), static_cast<var<R>::diff_value_type>(std::cos(x.val)*x.dval));
}

template <std::floating_point R>
constexpr auto cos(const var<R>& x) noexcept
{
	return var(std::cos(x.val), static_cast<var<R>::diff_value_type>(-x.dval*std::sin(x.val)));
}

template <std::floating_point R>
constexpr auto exp(const var<R>& x) noexcept
{
	return var(std::exp(x.val), static_cast<var<R>::diff_value_type>(x.dval*std::exp(x.val)));
}

// x.val > 0
template <std::floating_point R>
constexpr auto log(const var<R>& x)
{
	return var(std::log(x.val), static_cast<var<R>::diff_value_type>(x.dval/x.val));
}

// x.val != 0
template <std::floating_point R>
constexpr auto pow(const var<R>& x, float num)
{
	return var(std::pow(x.val,num), static_cast<var<R>::diff_value_type>(x.dval*num*std::pow(x.val, num-1)));
}

} // namespace utiil

} // namespace nyaruga

/* how to use

#include "auto_diff.hpp"

int main() {

	var x1(3.1415926/3,2,1), x2(7.,2,2), y;

	y = x1*x2 + sin(x1)*x2;
	printf("%f, %f, %f\n", y.val, y.dval(0), y.dval(1));
}

*/

#endif // #ifdef NYARUGA_UTIL_HAS_EIGEN

#endif // #ifndef NYARUGA_UTIL_AUTO_DIFF_HPP


// NTNU TDT4200 Fall 2015 Problem Set 1: MPI Intro
// permve@stud.ntnu.no

#include <boost/mpi.hpp>

#include <chrono>
#include <cmath>
#include <cstdio>
#include <iostream>
#include <stdexcept>
#include <utility>

namespace
{
    namespace config
    {
        const int master_node = 0;
        const int result_tag  = 0;
    }
}

double
sum_inverse_log_skip_multiples_of_two(
    const int node_index, const int node_count, const int start, const int stop)
{
    double sum = 0.0;

    int end_point = stop;

    double log_of_two = std::log2(2.0);
    double log_of_e   = std::log2(std::exp(1.0));

    while (true)
    {
        // Divide mid-point and start-point by 2:
        int mid_point = end_point >> 1;
        int start_point = mid_point >> 1;

        bool is_midpoint_odd = mid_point & 1;

        mid_point &= ~1;

        if (start_point < start || ((mid_point - start_point) < 2 * node_count))
        {
            // Not possible to split remaining data. Revert to plain iteration.
            const int per_node = (end_point - start) / node_count;

            int node_start = start + per_node * node_index;
            int node_end = node_start + per_node;

            if (node_index == node_count - 1)
            {
                node_end = end_point;
            }

            for (int x = node_start; x != node_end; ++x)
            {
                sum += log_of_e / std::log2(static_cast<double>(x));
            }

            return sum;
        }
        else
        {
            const int per_node = (mid_point - start_point) / node_count;

            int node_start = start_point + per_node * node_index;
            int node_end = node_start + per_node;

            if (node_index == node_count - 1)
            {
                node_end = mid_point;

                for (int x = node_start; x != node_end; ++x)
                {
                    const auto l = std::log2(static_cast<double>(x));
                    sum += log_of_e / l + log_of_e / (log_of_two + l) + log_of_e / std::log2(static_cast<double>(2 * x + 1));
                }

                if (is_midpoint_odd)
                {
                    sum += log_of_e / std::log2(static_cast<double>(2 * node_end)) + log_of_e / std::log2(static_cast<double>(2 * node_end + 1));
                }
            }
            else
            {
                for (int x = node_start; x != node_end; ++x)
                {
                    const auto l = std::log2(static_cast<double>(x));
                    sum += log_of_e / l + log_of_e / (log_of_two + l) + log_of_e / std::log2(static_cast<double>(2 * x + 1));
                }
            }

            if (end_point & 1 && node_index == 0)
            {
                sum += log_of_e / std::log2(static_cast<double>(end_point - 1));
            }

            end_point = start_point;
        }
    }
}

std::pair<int, int>
read_user_input(const int argc, const char* argv[])
{
    if (argc != 3)
    {
        throw std::runtime_error(
            "This program requires two parameters:\n"
            "the start and end specifying a range of positive integers "
            "in which start is 2 or greater, and end is greater than start.\n");
    }

    const int start = std::stoi(argv[1]);
    const int stop  = std::stoi(argv[2]);

    if (start < 2 || stop <= start)
    {
        throw std::runtime_error(
            "Start must be greater than 2 and the end must be larger than start.\n");
	}

    return std::make_pair(start, stop);
}

int
main(const int argc, const char* argv[])
{
    boost::mpi::environment  env{};
    boost::mpi::communicator world{};

    const bool is_master = (world.rank() == config::master_node);

    try
    {
        const auto start_stop = read_user_input(argc, argv);

        const auto t1 = std::chrono::system_clock::now();

        const auto this_node_sum = sum_inverse_log_skip_multiples_of_two(
            world.rank(), world.size(), start_stop.first, start_stop.second);

        if (is_master)
        {
            // Master node: gather and sum calculations for all nodes
            // Do not care about order of results; only the number of results

            double total_node_sum = this_node_sum;

            for (int i = 1; i != world.size(); ++i)
            {
                double received_node_sum;
                world.recv(boost::mpi::any_source, config::result_tag, received_node_sum);
                total_node_sum += received_node_sum;
            }

            const auto t2 = std::chrono::system_clock::now();

            const auto nanoseconds = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();

            std::printf("%ld %f\n", nanoseconds, total_node_sum);
        }
        else
        {
            // Slave node: send calculation to master
            world.send(config::master_node, config::result_tag, this_node_sum);
        }
    }
    catch (const std::exception& error)
    {
        if (is_master)
        {
            std::cerr << error.what();
        }
        return 1;
    }

    return 0;
}


#include <iostream>
#include <chrono>
#include <random>
#include <map>

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

#include "matplotlibcpp.h"

using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

double g(double x, double a, double b, double c)
{
  // return a*x*x+b*x+c;
  return std::exp(a * x * x + b * x + c);
}

int main(int argc, char **argv) {
  std::cout << "Newton \n";

  double aa = 1.0, bb = 2.0, cc = 1.0;
  double a = 2.30, b = -1.20, c = 5.50;

  double obs_sigma = 1.0;
  std::default_random_engine generator;
  std::normal_distribution<double> obs_noise_distrib(0.0, obs_sigma);

  int N = 100;
  std::vector<double> x_data(N), y_data(N);
  for (int i = 0; i < N; ++i)
  {
    x_data[i] = static_cast<double>(i) / 100.0;
    y_data[i] = g(x_data[i], aa, bb, cc) + obs_noise_distrib(generator);

  }

  // Optimize
  int max_iter = 100000;
  for (int it = 0; it < max_iter; ++it)
  {
    std::cout << "iter " << it << " : ";
    std::cout << "abc = " << a << " " << b << " " << c << "\n";
    Eigen::Vector3d J(0.0, 0.0, 0.0);
    Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
    double total_err = 0.0;
    for (int i = 0; i < N; ++i)
    {
      double x = x_data[i];
      double gx = g(x, a, b, c);
      double err = y_data[i] - gx;
      total_err += err * err;

      // Compute derivative of F(x) = 0.5 * sum(f(x)^2)
      J[0] += -x * x * gx * err;
      J[1] += -x * gx * err;
      J[2] += -gx * err;

      double fe = err * gx;
      double g2 = g(x, 2*a, 2*b, 2*c);
      double feg2 = fe-g2;

      H(0, 0) += -std::pow(x, 4) * feg2;
      H(1, 1) += -std::pow(x, 2) * feg2;
      H(2, 2) += -feg2;

      H(0, 1) += -std::pow(x, 3) * feg2;
      H(0, 2) += -std::pow(x, 2) * feg2;

      H(1, 0) += -std::pow(x, 3) * feg2;
      H(1, 2) += -x * feg2;

      H(2, 0) += -std::pow(x, 2) * feg2;
      H(2, 1) += -x * feg2;
    }

    std::cout << "J = " << J.transpose() << "\n";
    std::cout << "total error: " << total_err << "\n";

    Eigen::Vector3d delta_x = -H.inverse() * J;
    std::cout << "delta_x = " << delta_x.transpose() << "\n";

    if (delta_x.norm() < 0.0001)
      break;

    a += delta_x[0];
    b += delta_x[1];
    c += delta_x[2];
  }
  std::cout << "final = " << a << " " << b << " " << c << "\n";

  vector<double> final_y_data(N);
  for (int i = 0; i < N; ++i)
  {
    final_y_data[i] = g(x_data[i], a, b, c);
  }

  plt::scatter(x_data, y_data);
  std::map<std::string, std::string> parameters;
  parameters["c"] = "red";
  plt::scatter(x_data, final_y_data, 1.0, parameters);
  plt::show();

  return 0;
}


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

            assert(num_nodes == 3 || num_nodes == 4);

            Eigen::Matrix2d side_lengths;
            side_lengths.col(0) = vertices.col(1) - vertices.col(0);
            side_lengths.col(1) = vertices.col(2) - vertices.col(0);

            double area = side_lengths.determinant() * (num_nodes == 3 ? 0.5 : 1.);

            Eigen::Vector4d elem_vec = Eigen::Vector4d::Zero();

            for (unsigned i = 0; i < num_nodes; i++) {

                Eigen::Vector2d midpoint_cw = (vertices.col(i) + vertices.col((i - 1 + num_nodes) % num_nodes)) / 2;
                Eigen::Vector2d midpoint_ccw = (vertices.col(i) + vertices.col((i + 1 + num_nodes) % num_nodes)) / 2;

                elem_vec[i] = area / num_nodes * 0.5 * (f(midpoint_ccw) + f(midpoint_cw));
            }

            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 <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>

#include "NNUtil.h"

/*
 * Function shape
 *
 * @desript Display shape information of given matrix.
 */
void shape(const char *msg, Matrix<float, Dynamic, Dynamic> in)
{
  std::cout << msg << "("  << in.rows() << "," << in.cols() << ")" << std::endl;
}

/*
 * Function loge()
 *
 * @desript Calculate logf() for each items in given matrix.
 */
Matrix<float, Dynamic, Dynamic> loge(Matrix<float, Dynamic, Dynamic> in)
{
  Matrix<float, Dynamic, Dynamic> out = in;

  out = out.array() + 1e-7f;

  for(int i=0; i<in.cols(); i++)
    {
      out(0, i) = logf(out(0,i));
    }

  return out;
}

/*
 * Function cross_entropy()
 *
 * @desript Calculate Cross Entropy Loss value from 2 input matrix.
 */
float cross_entropy(Matrix<float, Dynamic, Dynamic> output, Matrix<float, Dynamic, Dynamic> expect)
{
  Matrix<float, Dynamic, Dynamic> tmp;
  float out = 0.f;
  tmp = expect.array() * loge(output).array();
  out = tmp.sum();
  return -out;
};


/*!
 * @file
 * @brief Implementation of automatic differentiation with dual numbers.
 * @copyright shijimi29431 2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

// TODO: UPDATE
// TODO: Add Doxygen Comment
// Look https://github.com/ceres-solver/ceres-solver/blob/master/include/ceres/jet.h

#ifndef SHIJIMI_MATH_AUTO_DIFF_HPP
#define SHIJIMI_MATH_AUTA_DIFF_HPP

#pragma once

#include <nyaruga_util/config.hpp>

#ifdef NYARUGA_UTIL_HAS_EIGEN

#include <cmath>
#include <shijimi/math/concepts/field.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace shijimi { namespace math {

namespace detail {

template <size_t dim, field R = double>
constexpr auto make_init_value(int var_no) noexcept
{
    Eigen::Matrix<R,dim,1> tmp = Eigen::Matrix<R,dim,1>::Zero();
    tmp(var_no - 1) = 1.;
    return tmp;
}

} // namespace detail

template <size_t dim, field R = double>
struct var {
	using value_type = R;
    static constexpr size_t dimention = dim;
	using diff_value_type = Eigen::Matrix<R,dim,1>;
	value_type val;
	diff_value_type dval;
	constexpr var() noexcept = default;
	constexpr var(value_type val_, size_t var_no) noexcept : val(val_), dval(detail::make_init_value<dim,R>(var_no)) {}
	constexpr var(const value_type& val_,const diff_value_type& dval_) noexcept : val(val_), dval(dval_) {}
	friend constexpr auto operator<=>(const var&, const var&) noexcept = default;
};

template <field R, size_t dim>
constexpr auto operator + (const var<dim,R>& lhs, const var<dim,R>& rhs) noexcept
{
	return var<dim,R>(lhs.val + rhs.val, static_cast<var<dim,R>::diff_value_type>(lhs.dval + rhs.dval));
}

template <field R, size_t dim>
constexpr auto operator - (const var<dim,R>& lhs, const var<dim,R>& rhs) noexcept
{
	return var<dim,R>(lhs.val - rhs.val, static_cast<var<dim,R>::diff_value_type>(lhs.dval - rhs.dval));
}

template <field R, size_t dim>
constexpr auto operator * (const var<dim,R>& lhs, const var<dim,R>& rhs) noexcept
{
	return var<dim,R>(lhs.val * rhs.val, static_cast<var<dim,R>::diff_value_type>(rhs.val*lhs.dval + lhs.val*rhs.dval));
}

// rhs.val != 0
template <field R, size_t dim>
constexpr auto operator / (const var<dim,R>& lhs, const var<dim,R>& rhs)
{
	return var<dim,R>(lhs.val / rhs.val, static_cast<var<dim,R>::diff_value_type>((lhs.dval*rhs.val - lhs.val*rhs.dval)/std::pow(rhs.val,2)));
}

template <field R, size_t dim>
constexpr auto sin(const var<dim,R>& x) noexcept
{
	return var<dim,R>(std::sin(x.val), static_cast<var<dim,R>::diff_value_type>(std::cos(x.val)*x.dval));
}

template <field R, size_t dim>
constexpr auto cos(const var<dim,R>& x) noexcept
{
	return var<dim,R>(std::cos(x.val), static_cast<var<dim,R>::diff_value_type>(-x.dval*std::sin(x.val)));
}

template <field R, size_t dim>
constexpr auto exp(const var<dim,R>& x) noexcept
{
	return var<dim,R>(std::exp(x.val), static_cast<var<dim,R>::diff_value_type>(x.dval*std::exp(x.val)));
}

// x.val > 0
template <field R, size_t dim>
constexpr auto log(const var<dim,R>& x)
{
	return var<dim,R>(std::log(x.val), static_cast<var<dim,R>::diff_value_type>(x.dval/x.val));
}

// x.val != 0
template <field R, size_t dim>
constexpr auto pow(const var<dim,R>& x, float num)
{
	return var<dim,R>(std::pow(x.val,num), static_cast<var<dim,R>::diff_value_type>(x.dval*num*std::pow(x.val, num-1)));
}

} // namespace math
} // namespace shijimi

#endif // #ifdef NYARUGA_UTIL_HAS_EIGEN

#endif // SHIJIMI_MATH_AUTO_DIFF_HPP


// Compile: g++ tie.cpp -o tie -lgmp
// Run: ./tie

#include <boost/multiprecision/gmp.hpp>
#include <boost/math/special_functions/gamma.hpp>

using namespace boost::multiprecision;

mpf_float fact(int int_x) {
  mpf_float_100 x = int_x;
  return boost::math::tgamma(x+1);
}

mpf_float prob(int n) {
  mpf_float::default_precision(100);
  mpf_float_100 two = 2;
  mpf_float_100 a =  fact(n);
  mpf_float_100 b =  fact(n/2);
  b *= b;
  mpf_float_100 c =  pow(two, n);
  return a / b / c;
}

#include <iostream>

int main(int argc, char const *argv[]) {
  int values[] = { 3030, 1000, 500, 200, 100, 50, 10 };
  for (int *value = values; *value; value++) {
    int n = *value;
    mpf_float p = prob(n);
    std::cout.precision(2);
    std::cout << n << " : " << std::fixed << 100*p << std::endl;
  }
  return 0;
}


#pragma once
#include <algorithm>    // std::max
#include <iostream>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <math.h>       /* log */
#include <assert.h>
#include <limits>

class DeterministicInterval {

	public:
		double lower;
		double upper;

	DeterministicInterval(double lower_, double upper_){
		lower = lower_;
		upper = upper_;
	}

	bool containsZero(){
		return ((lower<=0) && (upper>=0));
	}

	void print(){
		std::cout << "my interval is: [" << lower << "," << upper << "]" << std::endl;
	}

};

DeterministicInterval add(DeterministicInterval a, DeterministicInterval b){
	DeterministicInterval res(a.lower+b.lower,a.upper+b.upper);
	return res;
}

DeterministicInterval sub(DeterministicInterval a, DeterministicInterval b){
	DeterministicInterval res(a.lower-b.lower,a.upper-b.upper);
	return res;
}

DeterministicInterval mult(DeterministicInterval a, DeterministicInterval b){

	double p1 = a.lower * b.lower;
	double p2 = a.lower * b.upper;
	double p3 = a.upper * b.lower;
	double p4 = a.upper * b.upper;

	double min_ = std::min(std::min(p1,p2),std::min(p3,p4));
	double max_ = std::max(std::max(p1,p2),std::max(p3,p4));

	DeterministicInterval res(min_,max_);
	return res;
}

DeterministicInterval max(DeterministicInterval a, DeterministicInterval b){

	double min_ = std::max(a.lower,b.lower);
	double max_ = std::max(a.upper,b.upper);

	DeterministicInterval res(min_,max_);
	return res;
}

DeterministicInterval div(DeterministicInterval a, DeterministicInterval b){

	if (!b.containsZero()){
		DeterministicInterval inverse = DeterministicInterval(1./b.upper,1./b.lower);
		return mult(a,inverse);
	}
	else {
		DeterministicInterval res(std::numeric_limits<double>::min(),std::numeric_limits<double>::max());
	}
}

DeterministicInterval sqrt_prod(DeterministicInterval a, DeterministicInterval b){

	DeterministicInterval prod = mult(a,b);
	double min_ = std::max(prod.lower,0.);
	assert(min_>0.0);
	double max_ = std::max(prod.upper,0.);
	assert(min_<max_);
	DeterministicInterval res(std::sqrt(min_),std::sqrt(max_));
	return res;
}

DeterministicInterval log_(DeterministicInterval a){

	DeterministicInterval res(std::log(a.lower),std::log(a.upper));
	return res;
}

DeterministicInterval exp_(DeterministicInterval a){

	DeterministicInterval res(std::exp(a.lower),std::exp(a.upper));
	return res;
}

DeterministicInterval square(DeterministicInterval a){
	DeterministicInterval res = mult(a,a);
	res.lower = std::max(0.,res.lower);
	return res;
}

DeterministicInterval scale(double c, DeterministicInterval a){
	if (c>0.){
		DeterministicInterval res(a.lower*c,a.upper*c);
		return res;
	}
	else if (c<0.){
		DeterministicInterval res(a.upper*c,a.lower*c);
		return res;
	}
	else {
		DeterministicInterval res(0.,0.);
		return res;
	}
}

DeterministicInterval constant_(double c){

	DeterministicInterval res(c,c);
	return res;

}

DeterministicInterval union_(DeterministicInterval a, DeterministicInterval b){
	DeterministicInterval res(std::min(a.lower,b.lower),std::max(a.upper,b.upper));
	return res;
}


// Group B - Perpetual American Options
//
// by Scott Sidoli
//
// 6-10-19
//
// NormalDistribution.hpp
//
// Implementation of cdf and pdf for black-scholes. Utililizes boost libraries.

#ifndef NormalDistribution_hpp
#define NormalDistribution_hpp

#include <iostream>

using namespace std;
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions.hpp>
using namespace boost::math;

double N(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return cdf(myNormal, x);

}

double n(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return pdf(myNormal, x);

}

#endif


///
/// @author  Thomas Lehmann
/// @file    primes.cxx
/// @brief   generating primes
///
/// Copyright (c) 2015 Thomas Lehmann
///
/// 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.
#include <math/prime/sieve_of_eratosthenes.h>
#include <math/prime/sieve_of_eratosthenes_optimized.h>
#include <math/number.h>
#include <generator/select.h>
#include <performance/measurement.h>

#include <boost/program_options.hpp>
#include <string>
#include <memory>
#include <iomanip>
#include <iostream>
#include <cstdint>

/// @struct options
/// @brief parsed command line options
struct Options {
    /// type of filter function
    using filter_function_type = std::function<bool (const uint64_t&)>;

    uint64_t max_number;         ///! the biggest number that should be checked to be a prime.
    uint64_t start_number;       ///! starting output with first prime >= this number.
    uint64_t max_columns;        ///! number of columns for printing primes
    filter_function_type filter; ///! additional filter function
    std::string sieve;           ///! sieve algorithm

    /// default c'tor initializing defaults
    Options()
        : max_number(1000), start_number(2), max_columns(10), filter(nullptr), sieve("default") {}
};

/// Providing filter as configured.
/// @param filter_name name of the filter
/// @return concrete filter function or true function.
Options::filter_function_type resolve_filter(const std::string& filter_name) noexcept {
    if (filter_name == "pandigital") {
        std::cout << " ... applying 'pandigital' filter" << std::endl;
        return [](const uint64_t number) {return math::number<uint64_t>::is_pandigital(number);};
    }
    if (filter_name == "palindrome") {
        std::cout << " ... applying 'palindrome' filter" << std::endl;
        return [](const uint64_t number) {return math::number<uint64_t>::is_palindrome(number);};
    }
    std::cout << " ... no additional filter" << std::endl;
    return [](const uint64_t) {return true;};
}

/// @param argc number of parameters
/// @param argv array of parameters
/// @param options where to store the parsed command line options
/// @return true when all is fine and application can use the option(s).
static bool parse(int argc, char** argv, Options& options) {
    namespace po = boost::program_options;
    std::string filter_name;
    po::options_description description("Allowed options for tool 'primes'");
    description.add_options()
        ("help", "print this help")
        ("max-number", po::value<uint64_t>(&options.max_number)->default_value(1000),
         "generating primes up to this limit (default: 1000)")
        ("start-number", po::value<uint64_t>(&options.start_number)->default_value(2),
         "printing primes >= this number (default: 2)")
        ("columns", po::value<uint64_t>(&options.max_columns)->default_value(10),
         "number of columns (default: 10)")
        ("filter", po::value<std::string>(&filter_name)->default_value(""),
         "providing filter name (default: none).")
        ("sieve", po::value<std::string>(&options.sieve)->default_value("default"),
         "sieve algorithm short name (default: 'default', other is 'optimized').")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, description), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << description << std::endl;
        return false;
    }

    options.filter = resolve_filter(filter_name);

    if (options.max_columns < 1) {
        std::cout << "error: You cannot have 0 columns" << std::endl;
        return false;
    }

    if (options.max_number < 2) {
        std::cout << "No primes possible with a limit of "
                  << options.max_number << std::endl;
    }

    return true;
}

/// @return sieve algorithm depending on command line option
template <typename T>
std::unique_ptr<math::prime::sieve_interface<T>> create_sieve(const Options& options) noexcept {
    if (options.sieve == "optimized") {
        return std::unique_ptr<math::prime::sieve_interface<T>>(
            new math::prime::sieve_of_eratosthenes_optimized<T>(options.max_number));
    }

    return std::unique_ptr<math::prime::sieve_interface<T>>(
        new math::prime::sieve_of_eratosthenes<T>(options.max_number));
}

/// Simple example demonstrating how to generate primes.
///
/// @param argc number of parameters
/// @param argv array of parameters
/// @return 0 when succeeded, 1 when failed or when used the help
int main(int argc, char** argv) {
    std::cout << "prime tool (version " << VERSION << ")" << std::endl;

    Options options;
    // parsing command line options
    if (!parse(argc, argv, options)) {
        return 1;
    }

    std::cout << " ... creating sieve up to max. number: " << options.max_number << std::endl;
    /// initializing maximum 'size' of sieve
    auto sieve = create_sieve<std::vector<bool>>(options);

    const auto sieve_duration = performance::measure<std::milli>([&sieve]() {
        /// calculating the primes and none primes
        sieve->calculate();
    });

    std::cout << " ... collecting primes" << std::endl;
    std::cout << std::endl;

    /// printing out all primes
    constexpr auto step = static_cast<uint64_t>(1);
    const auto primes = generator::select(options.start_number, options.max_number, step)
           .where([&sieve](const int n){return sieve->is_prime(n);})
           .where(options.filter)
           .to_vector();

    const auto width = math::digits<int>::count(*(primes.end()-1)) + 1;

    auto column = static_cast<uint64_t>(0);
    for (const auto& prime: primes) {
        std::cout << std::setw(width) << prime;
        ++column;
        if (column % options.max_columns == 0) {
            std::cout << std::endl;
        }
    }
    std::cout << std::endl << std::endl;
    std::cout << " ... " << primes.size() << " primes found." << std::endl;
    std::cout << " ... Sieve calculation took " << sieve_duration << "ms." << std::endl;
    return 0;
}


//compute_returns.cpp

#include<string>

#include <algorithm>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

#include "pca.h"
#include "compute_returns_eigen.h"

using namespace boost::accumulators;

Vec computeRiskReturn(const Vec& assetReturns) {
	accumulator_set<double, features<tag::mean, tag::variance> > acc;
	acc = std::for_each(assetReturns.begin(), assetReturns.end(), acc);

	Vec tmp;
	tmp.push_back(boost::accumulators::mean(acc)); //boost mean
	tmp.push_back(boost::accumulators::variance(acc)); //boost sigma

	return tmp;
}

ComputeReturn::ComputeReturn(unsigned int _period, unsigned int _k)
:period(_period),k(_k),logRtns(false)
{
   	mPrices.clear(), mAssetReturns.clear(), mMeanReturns.clear();
   	mPrices.resize(1,Vec(1)), mAssetReturns.resize(1,Vec(1)), VarCov.resize(0,0);
}

ComputeReturn::ComputeReturn(const Vec& _prices,
				  	  	  	 unsigned int _period,
				  	  	  	 unsigned int _k,
							 bool _logRtns)
:period(_period),k(_k),logRtns(_logRtns)
{
	mPrices.clear(), mAssetReturns.clear(), mMeanReturns.clear();

	size_t n(_prices.size());

	mPrices.resize(1, Vec(n));
	mPrices[0] = _prices;

	//Compute rtns
	mAssetReturns.resize(1, Vec(n));
	logRtns ? this->_geometricReturns() : this->_arithmetricReturns();

	//Compute average return and volatility
	Vec tmp = computeRiskReturn(mAssetReturns[0]);

	mMeanReturns.push_back(tmp[0]);

	VarCov.resize(1,1);

    VarCov(0,0) = tmp[1];
}

ComputeReturn::ComputeReturn(const Mat& _prices,
				  			 unsigned int _period,
				  			 unsigned int _k ,
                  			 bool _logRtns)
:period(_period),k(_k),logRtns(_logRtns)
{
	mPrices.clear(), mAssetReturns.clear(), mMeanReturns.clear();

	size_t m(_prices.size());
	size_t n(_prices[0].size());

	mPrices.resize(m, Vec(n));
	mPrices = _prices;

    mAssetReturns.resize(m, Vec(n));
	//Compute rtns
	for(size_t i = 0;i < m;++i)
		logRtns ? _geometricReturns(i) : _arithmetricReturns(i);

	//1. average rtn
	for(size_t i = 0;i < m;++i){

		accumulator_set<double, features<tag::mean> > acc;
		acc = std::for_each(mAssetReturns[i].begin(), mAssetReturns[i].end(), acc);

		mMeanReturns.push_back(boost::accumulators::mean(acc)); //boost mean
	}

	//Compute Var-Cov matrix

	VarCov.resize(m,m);

	for(size_t i = 0;i <m;++i){
		for(size_t j = 0;j <m;++j){

			//2. compute covariance
			Vec _tmp;

			double q = mAssetReturns[i].size() < mAssetReturns[j].size() ? mAssetReturns[i].size() : mAssetReturns[j].size();

			for(size_t p = 0;p < q;++p)
				_tmp.push_back((mAssetReturns[i][p] - mMeanReturns[i]) * (mAssetReturns[j][p] - mMeanReturns[j]));

			accumulator_set<double, features<tag::mean> > acc;
			acc = std::for_each(_tmp.begin(), _tmp.end(), acc);

			VarCov(i,j) = boost::accumulators::mean(acc); //boost mean

		}
	}
}

ComputeReturn::ComputeReturn(const ComputeReturn& other):

    period(other.period), k(other.k), logRtns(other.logRtns),
	mPrices(other.mPrices), mAssetReturns(other.mAssetReturns),
	mMeanReturns(other.mMeanReturns), VarCov(other.VarCov)
{}

void ComputeReturn::arithmetricReturns(const Vec& _prices)
{
    mPrices.resize(1, Vec(_prices.size()));

	mPrices[0] = _prices;
	_arithmetricReturns();
}

void ComputeReturn::geometricReturns(const Vec& _prices)
{
    mPrices.resize(1, Vec(_prices.size()));

	mPrices[0] = _prices;
	_geometricReturns();
}

void ComputeReturn::arithmetricReturns(const Mat& _prices){

    mPrices = _prices;

    for(size_t i = 0;i < mPrices.size();++i)
        _arithmetricReturns(i);
}

void ComputeReturn::geometricReturns(const Mat& _prices){

    mPrices = _prices;

    for(size_t i = 0;i < mPrices.size();++i)
		 _geometricReturns(i);
}

void ComputeReturn::setPeriod(unsigned int _period){

	period = _period;

	for(size_t i = 0;i < mPrices.size();++i){
		logRtns ? _geometricReturns(i) : _arithmetricReturns(i);
	}
}

void ComputeReturn::setWindow(unsigned int _k){

	k = _k;

	for(size_t i = 0;i < mPrices.size();++i){
		logRtns ? _geometricReturns(i) : _arithmetricReturns(i);
	}
}

Vec ComputeReturn::getReturns(size_t p) const {return mAssetReturns[p];}

double ComputeReturn::getMeanReturn(size_t p) const {return mMeanReturns[p];}

double ComputeReturn::getStdDev(size_t p) const {return sqrt(VarCov(p,p));}

Vec ComputeReturn::getRollingMean(size_t p) const {

	Vec RollingMeanReturns;

	for(unsigned int i = 0;i < mAssetReturns[p].size() - k;++i){

        Vec _assetReturns;

        for(unsigned int j = i; j < i + k; ++j)
            _assetReturns.push_back(mAssetReturns[p][j]);

		RollingMeanReturns.push_back(computeRiskReturn(_assetReturns)[0]);
	}

	return RollingMeanReturns;
}

Vec ComputeReturn::getRollingStdDev(size_t p) const {

	Vec RollingSigmastminus1;

	for(unsigned int i = 0;i < mAssetReturns[p].size() - k;++i){

    	Vec _assetReturns;

        for(unsigned int j = i; j < i + k; ++j)
        	_assetReturns.push_back(mAssetReturns[p][j]);

		RollingSigmastminus1.push_back(sqrt(computeRiskReturn(_assetReturns)[1]));
	}

	return RollingSigmastminus1;
}

Eigen::MatrixXd ComputeReturn::getCorrelMat(){

	size_t m = VarCov.cols();

	Eigen::MatrixXd CorrelMat(m,m);

    for(size_t i = 0;i < m;++i)
        for(size_t j = 0;j < m;++j)
            CorrelMat(i,j) = VarCov(i,j)/sqrt(VarCov(i,i) * VarCov(j,j));

    return CorrelMat;
}

void ComputeReturn::setReturns(const Mat& _mAssetReturns){

	mAssetReturns = _mAssetReturns;
}

void ComputeReturn::correlReweightedRtns(const Eigen::MatrixXd& Chat){

	Eigen::MatrixXd C = this->getCorrelMat();

    Eigen::MatrixXd A( C.llt().matrixL() );

    Eigen::MatrixXd Ahat( Chat.llt().matrixL() );

	size_t m = mAssetReturns.size();
	size_t n = mAssetReturns[0].size();

    //Fill correl matrix to Eigen matrix
    MatrixXd R(m,n);
    for(size_t i = 0;i <m;++i)
        for(size_t j = 0;j <n;++j)
            R(i,j) = mAssetReturns[i][j];

    MatrixXd Rhat(n,m);
    Rhat = Ahat * A.inverse() * R;

	//Back to std::vector
    for(size_t i = 0;i <m;++i)
        for(size_t j = 0;j <n;++j)
             mAssetReturns[i][j] = Rhat(i,j);

}

Eigen::MatrixXd ComputeReturn::computePC(){

    // convert to req format
    vector<float> vec;

	size_t n = VarCov.rows();
	size_t m = VarCov.cols();

    for(size_t i = 0;i < n;++i)
        for(size_t j = 0;j < m;++j)
            vec.push_back(VarCov(i,j));

	std::shared_ptr<Pca> pca(new Pca());

  	int init_result = pca->Calculate(vec, n, m);//, true, true, false);
  	if(init_result == 1) cout << "correl matrix not positive definite" << endl;

    vector<float> scores = pca->scores(); //Rotated data

  	unsigned int kaiser = pca->kaiser(); //Kaiser criterion 99%

    unsigned int nrows = pca->nrows();

	Eigen::MatrixXd PC(nrows, kaiser);

	for(size_t i = 0;i < nrows;++i)
		for(size_t j = 0;j < kaiser;++j)
			PC(i,j) = scores[j + kaiser*i];

	return PC;
}


#include "rng.h"
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
//#include <eigen3/Eigen/Dense>
//#include <eigen3/Eigen/Cholesky>
#include <ctime>
#include <iostream>

// random number generator engine
boost::mt19937 rng_g(std::time(0)) ;
boost::normal_distribution<double> normal_dist ;
boost::variate_generator< boost::mt19937, boost::normal_distribution<double> > var_gen( rng_g, normal_dist ) ;
boost::uniform_01<> uni_dist ;
//bool seeded = false ;

//void seed_rng()
//{
//    std::cout << "seeding rng" << std::endl ;
//    rng_g.seed( std::time(0) ) ;
//    seeded = true;
//}

double randn()
{
//    if (!seeded)
//        seed_rng() ;
    return var_gen() ;
}

double randu01()
{
//    if (!seeded)
//        seed_rng() ;
    return uni_dist(rng_g) ;
}

void randmvn3(double* mean, double* cov, int n, double* results){
    // compute cholesky decomposition of covariance matrix
    double L11 = sqrt(cov[0]) ;
    double L21 = cov[1]/L11 ;
    double L22 = sqrt(cov[4]-pow(L21,2)) ;
    double L31 = cov[2]/L11 ;
    double L32 = (cov[5]-L31*L21)/L22 ;
    double L33 = sqrt(cov[8] - pow(L31,2) - pow(L32,2)) ;

    // multiply uncorrelated normal random samples by decomposition to produce
    // correlated samples, and add mean
    for ( int i = 0 ; i < n ; i++ ){
        double x1 = randn() ;
        double x2 = randn() ;
        double x3 = randn() ;
        results[i] = x1*L11 + mean[0];
        results[i+n] = x1*L21 + x2*L22 + mean[1] ;
        results[i+2*n] = x1*L31 + x2*L32 + x3*L33 + mean[2] ;
    }
}


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef std::vector<vecteur> matrice;

ENREGISTRER_PROBLEME(81, "Path sum: two ways") {
    // In the 5 by 5 matrix below, the minimal path sum from the top left to the bottom right, by
    // only moving to the right and down, is indicated in bold red and is equal to 2427.
    //
    //              131 673 234 103  18
    //              201  96 342 965 150
    //              630 803 746 422 111
    //              537 699 497 121 956
    //              805 732 524  37 331
    //
    // Find the minimal path sum, in matrix.txt (right click and "Save Link/Target As..."), a 31K
    // text file containing a 80 by 80 matrix, from the top left to the bottom right by only moving
    // right and down.
    matrice graphe;
    std::ifstream ifs("data/p081_matrix.txt");
    std::string ligne;
    while (ifs >> ligne) {
        std::vector<std::string> v;
        boost::split(v, ligne, boost::is_any_of(","));
        vecteur l;
        for (const auto &s: v) {
            l.push_back(std::stoull(s));
        }
        graphe.push_back(std::move(l));
    }

    const nombre taille = graphe.size();
    matrice chemin(taille, vecteur(taille, 0));
    for (size_t i = 0; i < taille; ++i)
        for (size_t j = 0; j < taille; ++j) {
            if (i == 0 && j == 0)
                chemin[i][j] = graphe[i][j];
            else if (i == 0)
                chemin[i][j] = graphe[i][j] + chemin[i][j - 1];
            else if (j == 0)
                chemin[i][j] = graphe[i][j] + chemin[i - 1][j];
            else
                chemin[i][j] = graphe[i][j] + std::min(chemin[i][j - 1], chemin[i - 1][j]);

        }
    return std::to_string(chemin.back().back());
}


// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "sysid/analysis/OLS.h"

#include <tuple>
#include <vector>

#include <Eigen/Cholesky>
#include <Eigen/Core>

using namespace sysid;

std::tuple<std::vector<double>, double> sysid::OLS(
    const std::vector<double>& data, size_t independentVariables) {
  // Perform some quick sanity checks regarding the size of the vector.
  assert(data.size() % (independentVariables + 1) == 0);

  // The linear model can be written as follows:
  // y = Xβ + u, where y is the dependent observed variable, X is the matrix
  // of independent variables, β is a vector of coefficients, and u is a
  // vector of residuals.

  // We want to minimize u^2 = u'u = (y - Xβ)'(y - Xβ).
  // β = (X'X)^-1 (X'y)

  // Get the number of elements.
  size_t n = data.size() / (independentVariables + 1);

  // Create new variables to make things more readable.
  size_t rows = n;
  size_t cols = independentVariables;  // X
  size_t strd = independentVariables + 1;

  // Create y and X matrices.
  Eigen::Map<const Eigen::MatrixXd, 0, Eigen::Stride<1, Eigen::Dynamic>> y(
      data.data() + 0, rows, 1, Eigen::Stride<1, Eigen::Dynamic>(1, strd));

  Eigen::Map<const Eigen::MatrixXd, 0, Eigen::Stride<1, Eigen::Dynamic>> X(
      data.data() + 1, rows, cols, Eigen::Stride<1, Eigen::Dynamic>(1, strd));

  // Calculate b = β that minimizes u'u.
  Eigen::MatrixXd b = (X.transpose() * X).llt().solve(X.transpose() * y);

  // We will now calculate r^2 or the coefficient of determination, which
  // tells us how much of the total variation (variation in y) can be
  // explained by the regression model.

  // We will first calculate the sum of the squares of the error, or the
  // variation in error (SSE).
  double SSE = (y - X * b).squaredNorm();

  // Now we will calculate the total variation in y, known as SSTO.
  double SSTO = ((y.transpose() * y) - (1 / n) * (y.transpose() * y)).value();

  double rSquared = (SSTO - SSE) / SSTO;
  double adjRSquared = 1 - (1 - rSquared) * ((n - 1.0) / (n - 3));

  return {{b.data(), b.data() + b.rows()}, adjRSquared};
}


//
// Created by Jakub Tyrcha on 13/11/2017.
//

#include "utilities.h"
#include <vector>
#include <numeric>
#include <boost/hana/functional/compose.hpp>
namespace hana = boost::hana;

bool is_prime(size_t i) {
    for(size_t j = 2; j*j <= i; ++j) {
        if((i % j) == 0) {
            return false;
        }
    }
    return true;
}

size_t next_prime(size_t v) {
    while(!is_prime(v++)) {}
    return --v;
}

std::vector<size_t> generate_prime_size() {
    std::vector<size_t> sizes;
    sizes.resize(50);
    std::iota(sizes.begin(), sizes.end(), 0);
    std::transform(sizes.begin(), sizes.end(), sizes.begin(),
                   hana::compose([](size_t v){
                       return next_prime(v);
                   },[](size_t v){
                       constexpr size_t exp_b = 10;
                       if(v < exp_b) {
                           return 2 << v;
                       }
                       else {
                           return (2 << (exp_b + (v - exp_b) / 2))
                                  + ((v % 2) ? 2 << (exp_b + (v - exp_b) / 2 - 1) : 0);
                       }
                   })
    );
    return sizes;
}

void print_prime_tables() {
    auto sizes = generate_prime_size();

    printf("{ ");
    size_t i=1;
    for(auto s : sizes) {
        printf("%lu, ", s); if(((i++) % 10) == 0) printf("\n");
    }
    printf("};");

    printf("\n");

    printf("{ ");
    for(auto s : sizes) {
        printf("case %lu: return hash %% %lu;\n", s, s);
    }
    printf("};");
}


#ifndef RLSS_INTERNAL_SVM_HPP
#define RLSS_INTERNAL_SVM_HPP
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <qp_wrappers/problem.hpp>
#include <qp_wrappers/cplex.hpp>
#include <rlss/internal/Util.hpp>
#include <absl/strings/str_cat.h>

namespace rlss {

namespace internal {

/*
* Calculate the svm hyperplane between two set of points f and s
* such that for all points p \in f, np + d < 0 where n is the normal
* of the hyperplane and d is the offset of the hyperplane.
*/
template<typename T, unsigned int DIM>
Hyperplane<T, DIM> svm(
    const StdVectorVectorDIM<T,DIM>& f,
    const StdVectorVectorDIM<T,DIM>& s) {

    QPWrappers::Problem<T> svm_qp(DIM + 1);
    Matrix<T> Q(DIM+1, DIM+1);
    Q.setIdentity();
    Q *= 2;
    Q(DIM, DIM) = 0;

    svm_qp.add_Q(Q);

    for(const VectorDIM<T,DIM>& pt : f) {
        Row<T> coeff(DIM+1);
        coeff.setZero();

        for(unsigned int d = 0; d < DIM; d++) {
            coeff(d) = pt(d);
        }
        coeff(DIM) = 1;

        svm_qp.add_constraint(coeff, std::numeric_limits<T>::lowest(), -1);
    }

    for(const VectorDIM<T,DIM>& pt : s) {
        Row<T> coeff(DIM+1);
        coeff.setZero();

        for(unsigned int d = 0; d < DIM; d++) {
            coeff(d) = pt(d);
        }
        coeff(DIM) = 1;

        svm_qp.add_constraint(coeff, 1, std::numeric_limits<T>::max());
    }

    QPWrappers::RLSS_SVM_QP_SOLVER::Engine<T> solver;
    solver.setFeasibilityTolerance(1e-8);
    Vector<T> result(DIM+1);
    auto ret = solver.init(svm_qp, result);
//    debug_message("svm optimization return value is ", ret);
    Hyperplane<T, DIM> hp;

    if(ret == QPWrappers::OptReturnType::Optimal) {
        for(unsigned int d = 0; d < DIM; d++) {
            hp.normal()(d) = result(d);
        }
        hp.offset() = result(DIM);
    } else {
        // cplex seems more reliable for svm
        QPWrappers::CPLEX::Engine<T> solver;
        solver.setFeasibilityTolerance(1e-8);
        auto ret = solver.init(svm_qp, result);
//        debug_message("svm optimization CPLEX return value is ", ret);
        if(ret == QPWrappers::OptReturnType::Optimal) {
            for(unsigned int d = 0; d < DIM; d++) {
                hp.normal()(d) = result(d);
            }
            hp.offset() = result(DIM);
        } else {
            debug_message("svm failed");
            for(const auto& vec: f) {
                debug_message("f", vec.transpose());
            }
            for(const auto& vec: s) {
                debug_message("s", vec.transpose());
            }
            throw std::runtime_error(
                absl::StrCat(
                        "svm not feasible"
                )
            );
        }
    }

    return hp;
}

} // namespace internal
} // namespace rlss
#endif // RLSS_INTERNAL_SVM_HPP


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//% Code implementing the paper "Accelerated Quadratic Proxy for Geometric Optimization", SIGGRAPH 2016.
//% Disclaimer: The code is provided as-is for academic use only and without any guarantees.
//%             Please contact the author to report any bugs.
//% Written by Shahar Kovalsky (http://www.wisdom.weizmann.ac.il/~shaharko/)
//%            Meirav Galun (http://www.wisdom.weizmann.ac.il/~/meirav/)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

#include "mex.h"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "mexHelpers.cpp"
#include <svd3x3.h>

using namespace Eigen;
using namespace igl;

void projBlockRotation2x2(VectorXd &pA, int dim)
{
	int block_size = dim*dim;
	int num_blocks = pA.size() / block_size;
	Map<MatrixXd> currA(pA.data(), dim, dim);
	Vector2d b;

	// project
	for (int ii = 0; ii < num_blocks; ii++)
	{
		// get current block
		new (&currA) Map<MatrixXd>(pA.data() + ii*block_size, dim, dim);
		// closest similarity
		b << 0.5*(currA(0, 0) + currA(1, 1)), 0.5*(currA(0, 1) - currA(1, 0)); // first row of B
		// closest rotation
		b = b / b.norm();
		currA << b(0), b(1), -b(1), b(0);
	}
}

void projBlockRotation3x3(VectorXd &pA, int dim)
{
	int block_size = dim*dim;
	int num_blocks = pA.size() / block_size;

	Matrix3f currAf, R;
	Matrix3f U, V;
	Vector3f s;
	Map<Matrix3d> currA(pA.data());

	// project
	for (int ii = 0; ii < num_blocks; ii++)
	{
		// get current block
		new (&currA) Map<MatrixXd>(pA.data() + ii*block_size, dim, dim);
		//
		currAf = currA.cast<float>(); // double -> single
		svd3x3(currAf, U, s, V); // svd
		R = U * V.transpose(); // polar
		currA = R.cast<double>();
	}
}

void mexFunction(int nlhs, mxArray *plhs[],
	int nrhs, const mxArray*prhs[])

{
	// assign input
	int A_rows = mxGetM(prhs[0]); // # rows of A
	int A_cols = mxGetN(prhs[0]); // # cols of A
	double *dim;
	const Map<VectorXd> A(mxGetPr(prhs[0]), A_rows, A_cols);
	dim = mxGetPr(prhs[1]);

	// init output
	VectorXd pA(A_rows);
	pA = A;

	// project
	if (*dim == 2)
		projBlockRotation2x2(pA, *dim);
	else if (*dim == 3)
		projBlockRotation3x3(pA, *dim);
	else
		mexErrMsgIdAndTxt("MATLAB:wrong_dimension", "dim must be either 2 or 3");

	// assign outputs
	mapDenseMatrixToMex(pA, &(plhs[0]));

}


/**
 * @file gps.cpp
 * @author WARG
 *
 * @section LICENSE
 *
 *  Copyright (c) 2015-2017, Waterloo Aerial Robotics Group (WARG)
 *  All rights reserved.
 *
 *  This software is licensed under a modified version of the BSD 3 clause license
 *  that should have been included with this software in a file called COPYING.txt
 *  Otherwise it is available at:
 *  https://raw.githubusercontent.com/UWARG/computer-vision/master/COPYING.txt
 */

#include <iomanip>
#include <boost/log/trivial.hpp>
#include "frame.h"

#define RAD2DEG(rad) ((rad)*180.0/M_PI)
#define DEG2RAD(deg) ((deg)*M_PI/180.0)
#define EARTH_RADIUS 6371000

//Based on the GPS location of the image, calculates the
//GPS location of a certain pixel in the image.
bool get_gps(cv::Point2d point, Frame* f, cv::Point2d* returnResult){
    if (f == NULL) {
        BOOST_LOG_TRIVIAL(error) << "Frame is null";
        return false;
    }
    BOOST_LOG_TRIVIAL(trace) << "get_gps(" << point << ", " << f->get_id() << ")";

    const Metadata* m = f->get_metadata();
    cv::Mat img = f->get_img();
    int h = img.cols;
    int w = img.rows;

    if (w <= 0 || h <= 0){
        BOOST_LOG_TRIVIAL(error) << "Invalid frame size w:" << w << " h:" << h;
        return false;
    }

    cv::Point2d imgCenter(w/2, h/2);

    //(0,0) is in the center of the image
    cv::Point2d biasedPoint = point - imgCenter;

    double altitude = m->altitude;
    double heading = m->heading;
    double latitude = m->lat;
    double longitude = m->lon;

    BOOST_LOG_TRIVIAL(trace) << "Camera FOV: " << f->get_camera().get_fov();

    BOOST_LOG_TRIVIAL(trace) << "Dist from Center (pixels: " << biasedPoint;

    double cameraXEdge = altitude * tan(DEG2RAD(f->get_camera().get_fov().width/2)); //meters from center of photo to edge
    double cameraYEdge = altitude * tan(DEG2RAD(f->get_camera().get_fov().height/2)); //meters from center of photo to edge

    BOOST_LOG_TRIVIAL(trace) << "X Edge: " << cameraXEdge << " Y Edge: " << cameraYEdge;

    //Rotation Matrix - Heading
    //Note: The '-heading' compensates for the fact that directional heading is
    //a clockwise quantity, but cos(theta) assumes theta is a counterclockwise
    //quantity.
    double realX = cos(DEG2RAD(-heading)) * biasedPoint.x/(w/2)*cameraXEdge - sin(DEG2RAD(-heading)) * biasedPoint.y/(h/2)*cameraYEdge;
    double realY = sin(DEG2RAD(-heading)) * biasedPoint.x/(w/2)*cameraXEdge + cos(DEG2RAD(-heading)) * biasedPoint.y/(h/2)*cameraYEdge;

    BOOST_LOG_TRIVIAL(trace) << "Real X: " << realX << " Real Y: " << realY;
    BOOST_LOG_TRIVIAL(trace) << "Cos:" << cos(DEG2RAD(-heading));
    BOOST_LOG_TRIVIAL(trace) << "Sin:" << sin(DEG2RAD(-heading));
    BOOST_LOG_TRIVIAL(trace) << "X:" << biasedPoint.x/(w/2)*cameraXEdge;
    BOOST_LOG_TRIVIAL(trace) << "Y:" << biasedPoint.y/(h/2)*cameraYEdge;

    double lon = RAD2DEG(realX/EARTH_RADIUS)/cos(DEG2RAD(latitude)) + longitude;
    double lat = RAD2DEG(realY/EARTH_RADIUS) + latitude;

    BOOST_LOG_TRIVIAL(trace) << "Distance from centre: " << RAD2DEG(realY/EARTH_RADIUS) << " " << RAD2DEG(realX/EARTH_RADIUS)/cos(DEG2RAD(latitude));

    BOOST_LOG_TRIVIAL(trace) << std::setprecision(10) << "Result: " << lat << " " << lon;

    *returnResult = cv::Point2d(lat,lon);
    return true;
}

double gps_dist(cv::Point2d p1, cv::Point2d p2) {
    double dLat = DEG2RAD(p2.x-p1.x);
    double dLon = DEG2RAD(p2.y-p1.y);

    double rLat1 = DEG2RAD(p1.x);
    double rLat2 = DEG2RAD(p2.x);

    double a = sin(dLat/2) * sin(dLat/2) +
          sin(dLon/2) * sin(dLon/2) * cos(rLat1) * cos(rLat2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    return EARTH_RADIUS * c;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd X = MatrixXd::Random(5,5);
MatrixXd A = X + X.transpose();
cout << "Here is a random symmetric 5x5 matrix:" << endl << A << endl << endl;
Tridiagonalization<MatrixXd> triOfA(A);
MatrixXd Q = triOfA.matrixQ();
cout << "The orthogonal matrix Q is:" << endl << Q << endl;
MatrixXd T = triOfA.matrixT();
cout << "The tridiagonal matrix T is:" << endl << T << endl << endl;
cout << "Q * T * Q^T = " << endl << Q * T * Q.transpose() << endl;

  return 0;
}


#include "render/mesh/MeshFactory.h"
#include "math/Defines.h"

#include <cmath>

#include <boost/format.hpp>

namespace epsilon
{
	ParametricData Parametric::Sphere(int slices, int stacks)
	{
		ParametricData data;
		float theta, phi;
		int v = 0;
		int next = 0;

		// Generate Vertices
		for (int x = 0; x < (slices+1); x++)
		{
			theta = x * PI / slices;
			for( int y = 0; y < stacks; y++)
			{
				phi = y * TWOPI / stacks;
				float px = sin(theta) * cos(phi);
				float py = cos(theta);
				float pz = -sin(theta) * sin(phi);
				data.vertices.push_back( Vector3( px, py, pz) );
				data.normals.push_back( Vector3( px, py, pz).Normalised() );
			}
		}

		// Generate Faces/Indices
		for (int x = 0; x < slices; x++ )
		{
			for (int y = 0; y < stacks; y++ )
			{
				next = ( y + 1 ) % stacks;
				// first triangle
				data.indices.push_back( v + y + stacks );
				data.indices.push_back( v + next );
				data.indices.push_back( v + y );

				// second triangle
				data.indices.push_back( v + y + stacks );
				data.indices.push_back( v + next + stacks);
				data.indices.push_back( v + next );
			}
			v += stacks;
		}

		return data;
	}

	ParametricData Parametric::Plane(int rows, int columns)
	{
		ParametricData data;

		float c, r;
		int tl, tr, bl, br;
		int v = 0;

		// Generate Vertices
		for ( int x = 0; x < (columns + 1); x++ )
		{
			c = x * ( 1.0f / columns );
			for ( int y = 0; y < (rows + 1); y++ )
			{
				r = y * ( 1.0f / rows );
				data.vertices.push_back( Vector3( (-0.5f + c),
												  0.0f,
												  (-0.5f + r) ) );
			}
		}

		// Generate Normals
		for ( int n = 0; n < ((rows + 1) * (columns + 1)); n++ )
		{
			//data.normals.push_back(Vector3::UP);
			data.normals.push_back(Vector3(0.0f, 1.0f, 0.0f));
			data.colours.push_back(Vector4(1.0f, 1.0f, 1.0f, 1.0f));
		}

		// Generate Faces/Indices
		for ( int x = 0; x < columns; x++ )
		{
			for ( int y = 0; y < rows; y++ )
			{
				bl = v + x + y;
				tl = bl + 1;
				tr = tl + rows + 1;
				br = tr - 1;
				data.indices.push_back(bl);
				data.indices.push_back(tl);
				data.indices.push_back(tr);

				data.indices.push_back(bl);
				data.indices.push_back(tr);
				data.indices.push_back(br);
			}
			v += rows;
		}

		// Generate Texture Coordinates
		for (int x = 0; x < (columns + 1); x++ )
		{
			c = x * ( 1.0f / columns);
			for ( int y = 0; y < (rows + 1); y++ )
			{
				r = y * ( 1.0f / rows );
				data.texCoords.push_back(Vector2(c, r));
			}
		}

		return data;
	}

	MeshFactory::MeshFactory(void)
	{
	}

	MeshFactory::~MeshFactory(void)
	{
	}
	Mesh::Ptr MeshFactory::GenerateGrid(int size, int resolution)
	{
		Mesh::Ptr newGrid = Mesh::Create(GL_LINES);

        newGrid->SetMeshType("GRID");
        newGrid->SetMeshParameters(str(format("size=%d|resolution=%d") % size % resolution));

		VerticesAttrib::List verts;
		//ColourAttrib::List
		float halfWidth = size / 2.0f;

		// if the resolution doesn't evenly divide into the size
		if ( (size % resolution) > 0.0f )
		{
			float diff = (float)(size % resolution);

			// modify the size so that the resolution evenly fits
			halfWidth -= diff / 2.0f;
		}

		// Impossible to have a grid smaller than its resolution
		if ( size > resolution )
		{
			float resStep =  (float)resolution;// / halfWidth;

			// 'Vertical' Lines first
			for ( float x = -halfWidth; x < halfWidth; x += resStep)
			{
				verts.push_back(Vector3(x,0,-halfWidth));
				verts.push_back(Vector3(x,0,halfWidth));
			}

			// 'Horizontal' lines
			for ( float z = -halfWidth; z < halfWidth; z += resStep)
			{
				verts.push_back(Vector3(-halfWidth,0,z));
				verts.push_back(Vector3(halfWidth,0,z));
			}

			// Border
			// TL -> TR
			verts.push_back(Vector3(-halfWidth,0,halfWidth));
			verts.push_back(Vector3(halfWidth,0,halfWidth));
			// TR - BR
			verts.push_back(Vector3(halfWidth,0,halfWidth));
			verts.push_back(Vector3(halfWidth,0,-halfWidth));
			// BR -> BL
			verts.push_back(Vector3(halfWidth,0,-halfWidth));
			verts.push_back(Vector3(-halfWidth,0,-halfWidth));
			// BL -> TL
			verts.push_back(Vector3(-halfWidth,0,-halfWidth));
			verts.push_back(Vector3(-halfWidth,0,halfWidth));
		}

		newGrid->VertexData()
			   ->SetVertices(verts);
			   //->BuildBuffers();

		return newGrid;
	}

	Mesh::Ptr MeshFactory::GenerateCube()
	{
		Mesh::Ptr newCube =  Mesh::Create();

        newCube->SetMeshType("CUBE");

		VerticesAttrib::List verts;
		NormalAttrib::List norms;
		//TexCoordAttrib::List texCoords;
		VertexIndicesBuffer::List faces;

		verts.push_back(Vector3(-0.5, -0.5, -0.5));
        norms.push_back(Vector3(-0.5, -0.5, -0.5));

        verts.push_back(Vector3(-0.5, -0.5, 0.5));
        norms.push_back(Vector3(-0.5, -0.5, 0.5));

        verts.push_back(Vector3(-0.5, 0.5, -0.5));
        norms.push_back(Vector3(-0.5, 0.5, -0.5));

        verts.push_back(Vector3(-0.5, 0.5, 0.5));
        norms.push_back(Vector3(-0.5, 0.5, 0.5));

        verts.push_back(Vector3(0.5, -0.5, -0.5));
        norms.push_back(Vector3(0.5, -0.5, -0.5));

        verts.push_back(Vector3(0.5, -0.5, 0.5));
        norms.push_back(Vector3(0.5, -0.5, 0.5));

        verts.push_back(Vector3(0.5, 0.5, -0.5));
        norms.push_back(Vector3(0.5, 0.5, -0.5));

        verts.push_back(Vector3(0.5, 0.5, 0.5));
        norms.push_back(Vector3(0.5, 0.5, 0.5));

		// left
		faces.push_back(0);
		faces.push_back(1);
		faces.push_back(3);
		faces.push_back(3);
		faces.push_back(2);
		faces.push_back(0);

		// right
		faces.push_back(4);
		faces.push_back(6);
		faces.push_back(7);
		faces.push_back(7);
		faces.push_back(5);
		faces.push_back(4);

		// front
		faces.push_back(7);
		faces.push_back(3);
		faces.push_back(1);
		faces.push_back(1);
		faces.push_back(5);
		faces.push_back(7);

		// back
		faces.push_back(0);
		faces.push_back(2);
		faces.push_back(6);
		faces.push_back(6);
		faces.push_back(4);
		faces.push_back(0);

		// top
		faces.push_back(3);
		faces.push_back(7);
		faces.push_back(2);
		faces.push_back(7);
		faces.push_back(6);
		faces.push_back(2);

		// bottom
		faces.push_back(5);
		faces.push_back(1);
		faces.push_back(4);
		faces.push_back(1);
		faces.push_back(0);
		faces.push_back(4);

		newCube->VertexData()
			   ->SetVertices(verts)
               ->SetNormals(norms)
			   ->SetIndices(faces);

		return newCube;
	}

	Mesh::Ptr MeshFactory::GenerateWireCube()
	{
		Mesh::Ptr newCube = Mesh::Create(GL_LINES);

		VerticesAttrib::List verts;

		verts.push_back(Vector3(-0.5, -0.5, -0.5));
		verts.push_back(Vector3(-0.5, -0.5, 0.5));

		verts.push_back(Vector3(-0.5, -0.5, 0.5));
		verts.push_back(Vector3( 0.5, -0.5, 0.5));

		verts.push_back(Vector3(-0.5, 0.5, -0.5));
		verts.push_back(Vector3(-0.5, 0.5, 0.5));

		verts.push_back(Vector3(-0.5, 0.5, 0.5));
		verts.push_back(Vector3( 0.5, 0.5, 0.5));

		verts.push_back(Vector3(0.5, -0.5, -0.5));
		verts.push_back(Vector3(0.5, -0.5, 0.5));

		verts.push_back(Vector3(-0.5, 0.5, -0.5));
		verts.push_back(Vector3( 0.5, 0.5, -0.5));

		verts.push_back(Vector3(0.5, 0.5, -0.5));
		verts.push_back(Vector3(0.5, 0.5, 0.5));

		verts.push_back(Vector3(-0.5, -0.5, -0.5));
		verts.push_back(Vector3(0.5,  -0.5, -0.5));

		// Corner Verticals
		verts.push_back(Vector3(-0.5, -0.5, -0.5));
		verts.push_back(Vector3(-0.5,  0.5, -0.5));

		verts.push_back(Vector3(0.5, -0.5, -0.5));
		verts.push_back(Vector3(0.5,  0.5, -0.5));

		verts.push_back(Vector3(0.5, -0.5, 0.5));
		verts.push_back(Vector3(0.5, 0.5,  0.5));

		verts.push_back(Vector3(-0.5, -0.5, 0.5));
		verts.push_back(Vector3(-0.5,  0.5, 0.5));

		newCube->VertexData()
			->SetVertices(verts);

		return newCube;
	}

	Mesh::Ptr MeshFactory::GenerateSphere(int slices, int stacks)
	{
		VerticesAttrib::List verts;
		NormalAttrib::List norms;
		ColourAttrib::List colours;

		TexCoordAttrib::List texCoords;
		VertexIndicesBuffer::List faces;

		// This is used purely for normal generation as it requires random access
		VecVec raverts;

		Mesh::Ptr newSphere = Mesh::Create();

        newSphere->SetMeshType("SPHERE");
        newSphere->SetMeshParameters(str(format("slices=%d|stacks=%d") % slices % stacks));

		int vi, next;
		float x, y, z, u, v, theta, phi, a;

		// Calculate Verts and Tex coords
		for ( int i = 0; i < (slices + 1); i++ )
		{
			theta = i * PI / stacks;
			for ( int j = 0; j < stacks; j++ )
			{
				phi = j * 2.0f * PI / stacks;

				// vertices
				x = std::sin(theta) * std::cos(phi);
				y = std::cos(theta);
				z = -std::sin(theta) * std::sin(phi);
				verts.push_back( Vector3(x, y, z) );

                norms.push_back( Vector3(x, y, z) );

				colours.push_back( Vector4(1.0f) );

				raverts.push_back( Vector3(x, y, z) );
				// Tex Coord
				v = std::acos(z) / PI;

                a = x / (std::sin(PI*(v)));

                if (y >= 0)
                {
					u = std::acos( a ) / TWOPI;
                }
				else
                {
					u = (PI + std::acos( a ) ) / TWOPI;
                }
				texCoords.push_back( Vector2(u, v) );
			}
		}

		// Calculate Faces
		vi = 0;
		for ( int i = 0; i < slices; i++ )
		{
			for ( int j = 0; j < stacks; j++ )
			{
				next = (j + 1) % stacks;

				faces.push_back( vi + j + stacks);
				faces.push_back( vi + next);
				faces.push_back( vi + j );

				faces.push_back( vi + j + stacks);
				faces.push_back( vi + next + stacks);
				faces.push_back( vi + next);
			}
			vi += stacks;
		}

		//norms = MeshFactory::GenerateNormals(raverts, faces);

		//newSphere->SetMeshData(verts, norms, texCoords, faces);
		newSphere->VertexData()
				 ->SetVertices(verts)
				 ->SetNormals(norms)
				 ->SetColours(colours)
				 ->SetTexCoords(texCoords)
				 ->SetIndices(faces);
				 //->BuildBuffers();

		return newSphere;
	}

	Mesh::Ptr MeshFactory::GenerateWireSphere()
	{
		Mesh::Ptr newSphere = Mesh::Create(GL_LINES);
		VerticesAttrib::List verts;

		ParametricData data;
		int slices = 8;
		int stacks = 8;
		float theta, phi;

		Vector3 last;
		Vector3 now;
		Vector3 first;
		// Generate Vertices

		for (int x = 0; x < (slices + 1); x++)
		{
			theta = x * PI / slices;
			for (int y = 0; y < stacks; y++)
			{
				phi = y * TWOPI / stacks;
				float px = sin(theta) * cos(phi);
				float py = cos(theta);
				float pz = -sin(theta) * sin(phi);

				now = Vector3(px, py, pz);

				if (y == 0)
				{
					first = now;
				}
				else if ( y % 2 == 0)
				{
					verts.push_back(last);
					verts.push_back(now);
				}
				else if (y == (stacks - 1))
				{
					verts.push_back(now);
					verts.push_back(first);
				}
				verts.push_back(now);

				last = now;
			}
		}

		newSphere->VertexData()
				 ->SetVertices(verts);

		return newSphere;
	}

	Mesh::Ptr MeshFactory::GeneratePlane(int widthSegments, int heightSegments)
	{
		Mesh::Ptr newPlane = Mesh::Create();

		newPlane->SetMeshType("PLANE");
		newPlane->SetMeshParameters(str(format("width_segs=%d|height_segs=%d") % widthSegments % heightSegments));

		ParametricData planeData = Parametric::Plane(heightSegments, widthSegments);

		newPlane->VertexData()
				->SetVertices(planeData.vertices)
				->SetNormals(planeData.normals)
				->SetColours(planeData.colours)
				->SetTexCoords(planeData.texCoords)
				->SetIndices(planeData.indices);
				//->BuildBuffers();

		return newPlane;
	}

	Mesh::Ptr MeshFactory::GenerateTriangle()
	{
		// Create a triangle
		VerticesAttrib::List verts;
		NormalAttrib::List norms;
		ColourAttrib::List colours;
		TexCoordAttrib::List tc;
		VertexIndicesBuffer::List inds;

		Mesh::Ptr newTriangle = Mesh::Create();

		verts.push_back(Vector3(-0.5,-0.5,0));
		verts.push_back(Vector3(0,0.5,0));
		verts.push_back(Vector3(0.5,-0.5,0));

		norms.push_back(Vector3(0,0,-1));
		norms.push_back(Vector3(0,0,-1));
		norms.push_back(Vector3(0,0,-1));

		colours.push_back(Vector4(1,0,0,1));
		colours.push_back(Vector4(0,1,0,1));
		colours.push_back(Vector4(0,0,1,1));

		tc.push_back(Vector2(0,0));
		tc.push_back(Vector2(0,0));
		tc.push_back(Vector2(0,0));

		inds.push_back(0);
		inds.push_back(1);
		inds.push_back(2);

		//newTriangle->SetMeshData(verts, norms, colours, tc, inds);
		newTriangle->VertexData()
				   ->SetVertices(verts)
				   ->SetNormals(norms)
				   ->SetTexCoords(tc)
				   ->SetIndices(inds);
				   //->BuildBuffers();

		return newTriangle;
	}

	Mesh::Ptr MeshFactory::GenerateIcoHedron()
	{
		return Mesh::Create();
	}

	Mesh::Ptr MeshFactory::GenerateOctohedron()
	{
		return Mesh::Create();
	}

	NormalAttrib::List MeshFactory::GenerateNormals(VerticesAttrib::List verts, VertexIndicesBuffer::List indices)
	{
		NormalAttrib::List norms;// = VecVec(verts.size());
		std::vector<VecVec> vertNorms = std::vector<VecVec>(verts.size());
		VerticesAttrib::List faceNormals;
		Vector3 faceNormal, v0, v1, v2, a, b;
		int vec0, vec1, vec2;
		bool found;

		// Generate face normals
		for (VertexIndicesBuffer::List::iterator ind = indices.begin(); ind != indices.end(); ind++ )
		{
			vec0 = *ind++;
			vec1 = *ind++;
			vec2 = *ind++;

			v0 = verts[vec0];
			v1 = verts[vec1];
			v2 = verts[vec2];

			a = v0 - v1;
			b = v2 - v1;
			b.Cross(a).Normalise();

			// add face normal to the vec positions list in vec0, vec1, vec2
			found = false;
			for (VecVec::iterator vnIt = vertNorms[vec0].begin(); vnIt != vertNorms[vec0].end(); vnIt++ )
			{
				if ( *vnIt == b )
				{
					found = true;
					break;
				}
			}

			if ( !found )
			{
				vertNorms[vec0].push_back(b);
			}

			found = false;
			for (VecVec::iterator vnIt = vertNorms[vec1].begin(); vnIt != vertNorms[vec1].end(); vnIt++ )
			{
				if ( *vnIt == b )
				{
					found = true;
					break;
				}
			}

			if ( !found )
			{
				vertNorms[vec1].push_back(b);
			}

			found = false;
			for (VecVec::iterator vnIt = vertNorms[vec2].begin(); vnIt != vertNorms[vec2].end(); vnIt++ )
			{
				if ( *vnIt == b )
				{
					found = true;
					break;
				}
			}

			if ( !found )
			{
				vertNorms[vec2].push_back(b);
			}
		}

		// Calculate the average for all of the face normals for each of the vectors
		for ( std::vector<VecVec>::iterator vnIt = vertNorms.begin(); vnIt != vertNorms.end(); vnIt++ )
		{
			faceNormal = Vector3();

			for (VecVec::iterator vnFnIt = (*vnIt).begin(); vnFnIt != (*vnIt).end(); vnFnIt++ )
			{
				faceNormal += *vnFnIt;
			}
			faceNormal.Normalise();
			norms.push_back(faceNormal);
		}

		return norms;

		 /*normals = list( MeshUtilities.face_normal(vertices, face) for face in faces )

            # Normal Generation

            # Calculate the normals once for each unique vertex and after calcs done,
            # rebuild the vertex list
            vertex_normals = []

            # for each vertex
            for v_inc in range(len(vertices)):
                face_normals = []
                vert_norm = Vector3()

                # for each face
                for f_inc in range(len(faces)):

                    # if the face contains the current vertex
                    if v_inc in faces[f_inc]:
                        # and the faces normal isn't already stored i.e. ignore co-planar face normals
                        if normals[f_inc] not in face_normals:
                            # Store it
                            face_normals.append(normals[f_inc])

                # Calculate the average for all of the face normals found

                for f_norm in face_normals:
                    vert_norm += f_norm
                vert_norm.normalize()

                vertex_normals.append(vert_norm)

            for vi in range(len(self._vert_index)):
                v_ind = self._vert_index[vi]
                self._normals.append(vertex_normals[v_ind])*/
	}

}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Real_timer.h>

#include <CGAL/compute_average_spacing.h>
#include <CGAL/grid_simplify_point_set.h>
#include <CGAL/jet_smooth_point_set.h>

#include <boost/lexical_cast.hpp>

#include <vector>
#include <fstream>

// Types
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::FT FT;
typedef Kernel::Point_3 Point;
typedef CGAL::Random_points_on_sphere_3<Point> Generator;

// Concurrency
#ifdef CGAL_LINKED_WITH_TBB
typedef CGAL::Parallel_tag Concurrency_tag;
#else
typedef CGAL::Sequential_tag Concurrency_tag;
#endif

// instance of std::function<bool(double)>
struct Progress_to_std_cerr_callback
{
  mutable std::size_t nb;
  CGAL::Real_timer timer;
  double t_start;
  mutable double t_latest;
  const std::string name;

  Progress_to_std_cerr_callback (const char* name)
    : name (name)
  {
    timer.start();
    t_start = timer.time();
    t_latest = t_start;
  }

  bool operator()(double advancement) const
  {
    // Avoid calling time() at every single iteration, which could
    // impact performances very badly
    ++ nb;
    if (advancement != 1 && nb % 100 != 0)
      return true;

    double t = timer.time();
    if (advancement == 1 || (t - t_latest) > 0.1) // Update every 1/10th of second
    {
      std::cerr << "\r" // Return at the beginning of same line and overwrite
                << name << ": " << int(advancement * 100) << "%";

      if (advancement == 1)
        std::cerr << std::endl;
      t_latest = t;
    }

    return true;
  }
};

int main (int argc, char* argv[])
{
  int N = (argc > 1) ? boost::lexical_cast<int>(argv[1]) : 1000;

  // Generate N points on a sphere of radius 100.
  std::vector<Point> points;
  points.reserve (N);
  Generator generator(100.);
  std::copy_n (generator, N, std::back_inserter(points));

  // Compute average spacing
  FT average_spacing = CGAL::compute_average_spacing<Concurrency_tag>
    (points, 6,
     CGAL::parameters::callback
     (Progress_to_std_cerr_callback("Computing average spacing")));

  // Simplify on a grid with a size of twice the average spacing
  points.erase(CGAL::grid_simplify_point_set
               (points, 2. * average_spacing,
                CGAL::parameters::callback
                (Progress_to_std_cerr_callback("Grid simplification"))),
               points.end());

  // Smooth simplified point set
  CGAL::jet_smooth_point_set<Concurrency_tag>
    (points, 6,
     CGAL::parameters::callback
     (Progress_to_std_cerr_callback("Jet smoothing")));

  return EXIT_SUCCESS;
}


#include "penrose.h"
#include <Eigen/Dense>
#include "../../external_libs/quickhull/QuickHull.hpp"

namespace quacry{
using Vec2 = glm::vec2;
using Vec3 = glm::vec3;
using Vec4 = glm::vec4;
using Mat4 = glm::mat4;
using Vec5f = Eigen::Matrix<float,5,1>;
using Mat5f = Eigen::Matrix<float,5,5>;
using namespace kipod::MeshModels;

auto PenroseRotation() -> Mat5f
{
    Mat5f g;
    double c = std::sqrt(2./5);
    double t = 2./5 * 3.1415926535;
    double u = 1./std::sqrt(2.);
    auto si = [=](int i){ return (float)sin(i*t); };
    auto co = [=](int i){ return (float)cos(i*t); };
    for(int j = 0; j<5; ++j)
        g.col(j) = c*Vec5f(co(j), si(j), co(2*j), si(2*j), u);

    return g;
}

auto PenroseInternalPolytope(Mat5f g, const Vec5f& gamma) -> std::pair<std::vector<Vec3>,std::vector<unsigned int>>
{
    LOG_INFO("Creating Internal Penrose Polytope from data g={} and gamma={}", g,gamma);

    using namespace quickhull;
    QuickHull<float> qh;
    auto vertices = std::vector<Vec3>();
    auto g_penrose = PenroseRotation();
    g = g*g_penrose;
    auto unit_cube = std::vector<Vec5f>();
    auto s = [](int i, int j){ return (i & (1 << j)) == 0 ? -0.5f : 0.5f; };
    LOG_INFO("Creating 5-dim Unitcube:");
    for(int i = 0; i<32; ++i){
       Vec5f v = { s(i,0), s(i,1),s(i,2),s(i,3),s(i,4)};
       unit_cube.push_back(v);
       LOG_INFO("{}th vector = {}", i, v.transpose());
    }
    for(auto& v : unit_cube) v = g*(v + gamma);
    for(const auto& v : unit_cube) vertices.emplace_back(v[2],v[3],v[4]);
    auto hull = qh.getConvexHull((float*)vertices.data(), vertices.size(), true, false);
    auto hull_vertices = std::vector<glm::vec3>();
    auto indices = std::vector<unsigned int>();
    for(auto& v : hull.getVertexBuffer()) hull_vertices.emplace_back(v.x,v.y,v.z);
    for(auto& v : hull.getIndexBuffer()) indices.push_back(v);
    return std::make_pair(hull_vertices, indices);
}

auto Penrose() -> Quasicrystal23
{
    auto [v,i] = PenroseInternalPolytope(Mat5f::Identity(), Vec5f::Constant(0.25f));
    auto penrose = Quasicrystal23("Penrose", PenroseRotation(), MeshModel(v,i), {-2,2, -2,2, -2,2, -2,2, -2,2});

    return penrose;
}
}