#include <cmath>
#include <iostream>
#include <fstream>
#include <OpenEXR/ImathVec.h>
#include <boost/format.hpp>

/*!
 * see http://cubic.org/docs/hermite.htm
 *
 */
namespace interpolate
{
	template <typename T> void hermite(const Imath::Vec3<T>& P1,
									   const Imath::Vec3<T>& T1,
									   const Imath::Vec3<T>& P2,
									   const Imath::Vec3<T>& T2,
									   T s,
									   Imath::Vec3<T>& P)
	{
		T h1 =  2.0*std::pow(s,3.0) - 3.0*std::pow(s,2.0) + 1.0; // calculate basis function 1
		T h2 = -2.0*std::pow(s,3.0) + 3.0*std::pow(s,2.0)      ; // calculate basis function 2
		T h3 =  std::pow(s,3.0)     - 2.0*std::pow(s,2.0) + s  ; // calculate basis function 3
		T h4 =  std::pow(s,3.0)     - std::pow(s,2.0)          ; // calculate basis function 4

		P = h1*P1 + // multiply and sum all funtions
			h2*P2 + // together to build the interpolated
			h3*T1 + // point along the curve.
			h4*T2;
	}
}

// == Emacs ================
// -------------------------
// Local variables:
// tab-width: 4
// indent-tabs-mode: t
// c-basic-offset: 4
// end:
//
// == vi ===================
// -------------------------
// Format block
// ex:ts=4:sw=4:expandtab
// -------------------------


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

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

#include <iostream>

#include <boost/array.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/ring.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/util/write_dsv.hpp>

struct triangle : public boost::array<boost::geometry::model::d2::point_xy<double>, 4>
{
    inline void close()
    {
        (*this)[3] = (*this)[0];
    }
};

// Register triangle as a ring
BOOST_GEOMETRY_REGISTER_RING(triangle)

// Specializations of algorithms, where useful. If not specialized the default ones
// (for linear rings) will be used for triangle. Which is OK as long as the triangle
// is closed, that means, has 4 points (the last one being the first).
namespace boost { namespace geometry {

template<>
inline double area<triangle>(const triangle& t)
{
    /*         C
              / \
             /   \
            A-----B

           ((Bx - Ax) * (Cy - Ay)) - ((Cx - Ax) * (By - Ay))
           -------------------------------------------------
                                   2
    */

    return 0.5 * ((t[1].x() - t[0].x()) * (t[2].y() - t[0].y())
                - (t[2].x() - t[0].x()) * (t[1].y() - t[0].y()));
}

}} // namespace boost::geometry

int main()
{
    triangle t;

    t[0].x(0);
    t[0].y(0);
    t[1].x(5);
    t[1].y(0);
    t[2].x(2.5);
    t[2].y(2.5);

    t.close();

    std::cout << "Triangle: " << boost::geometry::dsv(t) << std::endl;
    std::cout << "Area: " << boost::geometry::area(t) << std::endl;

    boost::geometry::model::d2::point_xy<double> c;
    boost::geometry::centroid(t, c);
    std::cout << "Centroid: " << boost::geometry::dsv(c) << std::endl;

    return 0;
}


/*This is a simple test from the official boost documentation to assess whether boost libraries have
 * been installed correctly
 */

#include <boost/math/differentiation/autodiff.hpp>
//#include <boost/multiprecision/cpp>
#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <vector>
#include <tuple>
#include <random>
#include "ceinms2/testingUtilities.h"
#include "ceinms2/Lloyd2003Muscle.h"

namespace df = boost::math::differentiation;

template<typename T>
T fourth_power(T const &x) {
    T x4 = x * x;// retval in operator*() uses x4's memory via NRVO.
    x4 *= x4;// No copies of x4 are made within operator*=() even when squaring.
    return x4;// x4 uses y's memory in main() via NRVO.
}

bool test1() {

    constexpr unsigned Order = 5;// Highest order derivative to be calculated.
    auto const x = df::make_fvar<double, Order>(2.0);// Find derivatives at x=2.
    auto const y = fourth_power(x);
    const std::vector<double> expected{ 16., 32., 48., 48., 24., 0. };
    bool failed = false;
    for (unsigned i = 0; i <= Order; ++i) {
        std::cout << "y.derivative(" << i << ") = " << y.derivative(i) << std::endl;
        failed |= (y.derivative(i) != expected.at(i));
    }
    return failed;
}

template<typename W, typename X, typename Y, typename Z>
auto f(const W &w, const X &x, const Y &y, const Z &z) {
    using namespace std;
    return exp(w * sin(x * log(y) / z) + sqrt(w * z / (x * y))) + w * w / tan(z);
}

/*
bool test2() {
    using float50 = boost::multiprecision::cpp_bin_float_50;

    constexpr unsigned Nw = 3;// Max order of derivative to calculate for w
    constexpr unsigned Nx = 2;// Max order of derivative to calculate for x
    constexpr unsigned Ny = 4;// Max order of derivative to calculate for y
    constexpr unsigned Nz = 3;// Max order of derivative to calculate for z
    // Declare 4 independent variables together into a std::tuple.
    auto const variables = make_ftuple<float50, Nw, Nx, Ny, Nz>(11, 12, 13, 14);
    auto const &w = std::get<0>(variables);// Up to Nw derivatives at w=11
    auto const &x = std::get<1>(variables);// Up to Nx derivatives at x=12
    auto const &y = std::get<2>(variables);// Up to Ny derivatives at y=13
    auto const &z = std::get<3>(variables);// Up to Nz derivatives at z=14
    auto const v = f(w, x, y, z);
    // Calculated from Mathematica symbolic differentiation.
    float50 const answer("1976.319600747797717779881875290418720908121189218755");
    std::cout << std::setprecision(std::numeric_limits<float50>::digits10)
              << "mathematica   : " << answer << '\n'
              << "autodiff      : " << v.derivative(Nw, Nx, Ny, Nz) << '\n'
              << std::setprecision(3)
              << "relative error: " << (v.derivative(Nw, Nx, Ny, Nz) / answer - 1) << '\n';
    return 0;
}
*/

template<typename X, typename Y>
auto test_fun(const X& x, const Y& y) {
    auto val = x *x *x *x + 2 * y *y;
    return val;
}

bool test3() {
    constexpr unsigned Order = 1;// Highest order derivative to be calculated.
    auto const variables = df::make_ftuple<double, 2, 2>(2, 2);
    auto const &x = std::get<0>(variables);
    auto const &y = std::get<1>(variables);
    auto const z = test_fun(x, y);
    std::cout << z.at(0, 0) << std::endl;
    std::cout << z.at(1, 0) << std::endl;
    std::cout << z.at(0, 1) << std::endl;

    bool failed = false;
    failed |= (z.at(0, 0) != 24);
    failed |= (z.at(1, 0) != 32);
    failed |= (z.at(0, 1) != 8);

    return failed;

}

bool test4() {
    const size_t N = 1000;
    const double dt = 0.1;
    const double w = boost::math::double_constants::two_pi * 3.;
    std::vector<double> x, y;
    for (size_t i{ 0 }; i < N; ++i) {
        const double t{ i * dt };
        x.push_back(t);
        y.push_back(std::cos(w * t));
    }

    // Get derivative through object-provided method
    ceinms::CubicSpline spline(x, y);

    bool failed = false;
    for (size_t i{ 0 }; i < N; ++i) {
        const double t{ i * dt * 0.9 };
        auto yp = spline.getFirstDerivative(t);
        // get derivative using autodiff
        auto const xx = df::make_fvar<double, 1>(t);
        auto yy = spline.get(xx);
        failed |= (yy.derivative(1) != yp);
    }
    return failed;
}
/*
bool test5() {
    using boost::math::epsilon_difference;

    auto fun([](auto x) {
        if (x > 0)
            return x * x;
        else
            return x * x * x;
        });

    auto dfun([](auto x) {
        if (x > 0)
            return 2*x;
        else
            return 3 * x * x;
    });

    // Get derivative through object-provided method

    std::random_device rd;// Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd());// Standard mersenne_twister_engine seeded with rd()
    std::uniform_real_distribution<> dis(-10.0, 20.0);
    bool failed = false;
    for (int n = 0; n < 100; ++n) {
        auto const val = dis(gen);
        auto const x = df::make_fvar<double, 1>(val);
        //check the value of the two derivatives is the same, within 2 epsilons
        bool isEqual = epsilon_difference(fun(x).derivative(1),dfun(val)) < 2.0;
        std::cout << fun(x).derivative(1) << "=?" << dfun(val);
        if (isEqual)
            std::cout << " True";
        else
            std::cout << " False";
        std::cout << std::endl;
        failed |= !isEqual;
    }
    return failed;
}
*/

int main() {
    bool failed = false;
    failed |= ceinms::runTest(&test1, "Fifith order derivative");
   // failed |= test2();
    failed |= ceinms::runTest(&test3, "Derivative extraction");
    failed |= ceinms::runTest(&test4, "Cubic Spine and autodiff");
  //  failed |= test5();

    return failed;
}


#include <iostream>
#include <fstream>
#include <vector>

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

using Matrix3d = Eigen::Matrix3d;
using Vector3d = Eigen::Vector3d;

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

    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q.matrix();
}

Matrix3d qua_integration(const Matrix3d &R0, const std::vector<Vector3d> &w_vec)
{
    std::cout << "============= qua_integration =============" << std::endl;

    Eigen::Quaterniond q(R0);

    for(int i = 0; i < w_vec.size(); ++i) {
        Vector3d omega_measured_ = w_vec[i];
        constexpr double DT = 1.0;

        Eigen::Quaterniond q_new;
        Eigen::Quaterniond q_add;

        // Method 1
        // Eigen::Quaterniond q_omega;
        // q_omega.w() = 0;
        // q_omega.vec() = omega_measured_ * DT * 0.5;
        // q_add = q_omega * q;
        // q_new.w() = q.w() + q_add.w();
        // q_new.vec() = q.vec() + q_add.vec();

        // Method 2
        Eigen::Vector3d rotated = omega_measured_ * DT;
        double angle = rotated.norm();
        Eigen::Vector3d axis = rotated.normalized();
        q_add = Eigen::AngleAxisd(angle, axis);
        q_new = q * q_add;

        q = q_new;

        std::cout << "i:" << i << " w:" << omega_measured_.transpose() << "\nR:" << q.matrix() << std::endl;
    }
    return q.matrix();
}

inline Eigen::Matrix3d skewm(const Eigen::Vector3d& v)
{
    double x = v(0);
    double y = v(1);
    double z = v(2);

    Eigen::Matrix3d S;
    S << 0.0, -z, y, z, 0.0, -x, -y, x, 0.0;

    return S;
}

inline Eigen::Matrix3d exp_hat_so3(const Eigen::Vector3d& v)
{
    const double theta = v.norm();
    if (theta < 1e-10)
    {
        return Matrix3d::Identity();
    }
    const Vector3d w = v / theta;
    Matrix3d W = skewm(w);
    // NOTE(Ning): W*W = -I + w * w^T
    // NOTE(Ning): Rodrigues rotation formula
    Matrix3d SO3 = Matrix3d::Identity() + std::sin(theta) * W + (1 - std::cos(theta)) * W * W;
    return SO3;
}

Matrix3d mat_integration(const Matrix3d &R0, const std::vector<Vector3d> &w_vec) {
    std::cout << "============= mat_integration =============" << std::endl;
    Eigen::Matrix3d R = R0;
    for(int i = 0; i < w_vec.size(); ++i) {
        Vector3d w = w_vec[i];
        R = R * exp_hat_so3(w);
        std::cout << "i:" << i << " w:" << w.transpose() << "\nR:" << R << std::endl;
    }

    return R;
}

void test1()
{
    std::cout << "test1" << std::endl;
    std::vector<Eigen::Vector3d> w_vec =
        {
            {0,0,0},
            {1,0,0},
            {0,1,0},
            {0,0,1},
            {1,1,1},
            {-1,-2,-3}
        };

    Matrix3d R0 = euler_to_mat(0.1,0.2,-0.1);

    mat_integration(R0, w_vec);

    qua_integration(R0, w_vec);

    std::cout << std::endl;
}

void test2()
{
    std::cout << "test2" << std::endl;

    std::vector<Eigen::Vector3d> w_vec =
    {
        {0,0,0},
        {0,0,0}
    };

    Matrix3d R0 = euler_to_mat(0.0,0.0,0.0);

    mat_integration(R0, w_vec);

    qua_integration(R0, w_vec);

    std::cout << std::endl;
}

void test3()
{
    std::cout << "test3" << std::endl;

    std::vector<Eigen::Vector3d> w_vec =
    {
        {0.01,0.01,0.01},
        {0.01,0.01,0.01},
        {0.01,0.01,0.01},
        {0.01,0.01,0.01}
    };

    Matrix3d R0 = euler_to_mat(0.0,0.0,0.0);

    mat_integration(R0, w_vec);

    qua_integration(R0, w_vec);

    std::cout << std::endl;
}

int main() {
    test1();
    test2();
    test3();
}


#include "writer.hpp"
#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <stdexcept>
#include <tuple>
#include <utility>

// Simpson integrator
inline double integrate(std::function<double(double)> f, double a, double b) {
	return (b-a) / 6 * (f(a) + 4 * f((a + b) / 2) + f(b));
}

//----------------GodunovBegin----------------
/// @param[in] N the number of grid points, NOT INCLUDING BOUNDARY POINTS
/// @param[in] T the final time at which to compute the solution
/// @param[in] f flux function, as function
/// @param[in] df derivative of flux function, as function
/// @param[in] u0 the initial conditions, as function
/// @param[out] u solution at time T
/// @param[out] X Grid Points
void Godunov(int N, double T, const std::function<double(double)> &f, const std::function<double(double)> &df, const std::function<double(double)> &u0, Eigen::VectorXd &u, Eigen::VectorXd &X) {
	// Create space discretization for interval [-2,2]
	// (write your solution here)
	u.resize(N + 1);
	X.setLinSpaced(N + 2, -2, 2);
	double dx = (X.maxCoeff() - X.minCoeff()) / (N + 1);

	// The Godunov flux
	// (write your solution here)
	// $F_{j + \frac{1}{2}}^n = \max(f(max(U_j^n, \omega)), f(min(U_{j + 1}^n, \omega)))$
	// where $\omega$ minimum of $f$
	auto F = [&f](double a, double b) -> double {
		using std::max;
		using std::min;
		// TODO: find minimum of f
		double omega{0};
		return max(f(max(a, omega)), f(min(b, omega)));
	};

	//setup vectors to store (old) solution
	// (write your solution here)
	for (int i = 0; i < N + 1; ++i) {
		u(i) = 1 / dx * integrate(u0, X(i), X(i + 1));
	}
	Eigen::VectorXd u_old{u};

	// choose dt such that if obeys CFL condition
	// (write your solution here)
	double t = 0;

	//Please uncomment the next line:
	while (t < T) {
		// Update dT according to current CFL condition
		// (write your solution here)
		// $\increment t = \frac{\increment x}{2 \max_j \abs{f'(U_j^n)}$
		double dt = std::min(T - t, dx / (2 * u.unaryExpr([&df](double x) -> double {
			                       return std::abs(df(x));
			                     }).maxCoeff()));

		// Update current time
		// (write your solution here)
		t += dt;

		// Update the internal values of u
		// (write your solution here)
		// $U_j^{n + 1} = U_j^n - \frac{\increment t}{\increment x} \left( F_{j + \frac{1}{2}}^n - F_{j - \frac{1}{2}}^n \right)$
		std::swap(u, u_old);
		for (int i = 1; i < N; ++i) {
			u(i) = u_old(i) - dt / dx * (F(u_old(i), u_old(i + 1)) - F(u_old(i - 1), u_old(i)));
		}

		// Update boundary with non-reflecting Neumann bc
		// (write your solution here)
		u(0) = u(1);
		u(N) = u(N - 1);

		//Please uncomment the next line:
	}
}
//----------------GodunovEnd----------------

//----------------convGodBegin----------------
//! Computes error for a range of cell lengths and stores them to error vectors
/// @param[in] T the final time at which to compute the solution
/// @param[in] f flux function, as function
/// @param[in] df derivative of flux function, as function
/// @param[in] u0 the initial conditions, as function
/// @param[in] uex exact solution
/// @param[in] baseName string containing name to save the computed errors and resolution
void GodunovConvergence(double T, const std::function<double(double)> &f, const std::function<double(double)> &df, const std::function<double(double)> &u0, const std::function<double(double, double)> &uex, const std::string &baseName) {
	std::vector<int>    resolutions = {100, 200, 400, 800, 1600};
	std::vector<double> L1_errors;
	std::vector<double> Linf_errors;

	for (auto &N : resolutions) {
		// find approximate solution using Godunov scheme
		// (write your solution here)
		Eigen::VectorXd u;
		Eigen::VectorXd X;
		Godunov(N, T, f, df, u0, u, X);

		// compute errors and push them bach to the corresponfing error vectors
		// (write your solution here)
		double L1_error{0};
		double Linf_error{0};
		for (int i = 0; i < N + 1; ++i) {
			double dx = X(i + 1) - X(i);
			double error = std::abs(u(i) - 1.0 / dx * integrate([&](double x) -> double {return uex(x, T);}, X(i), X(i + 1)));
			L1_error += dx * error;
			Linf_error = std::max(Linf_error, error);
		}
		L1_errors.emplace_back(L1_error);
		Linf_errors.emplace_back(Linf_error);
	}

	writeToFile(baseName + "_L1errors_Godunov.txt", L1_errors);
	writeToFile(baseName + "_Linferrors_Godunov.txt", Linf_errors);
	writeToFile(baseName + "_resolutions.txt", resolutions);
}
//----------------convGodEnd----------------

//----------------LFBegin----------------
/// @param[in] N the number of grid points, NOT INCLUDING BOUNDARY POINTS
/// @param[in] T the final time at which to compute the solution
/// @param[in] f flux function, as function
/// @param[in] df derivative of flux function, as function
/// @param[in] u0 the initial conditions, as function
/// @param[out] u solution at time T
/// @param[out] X Grid Points
void LaxFriedrichs(int N, double T, const std::function<double(double)> &f, const std::function<double(double)> &df, const std::function<double(double)> &u0, Eigen::VectorXd &u, Eigen::VectorXd &X) {
	std::ignore = df;
	// Create space discretization for interval [-2,2]
	// (write your solution here)
	double CFL = 0.5;
	X.setLinSpaced(N + 2, -2, 2);
	double dx  = (X.maxCoeff() - X.minCoeff()) / (N + 1);

	//setup vectors to store solution
	// (write your solution here)
	u.resize(N + 1);
	for (int i = 0; i < N + 1; ++i) {
		u(i) = 1 / dx * integrate(u0, X(i), X(i + 1));
	}
	Eigen::VectorXd u_old{u};

	// choose dt such that if obeys CFL condition
	// (write your solution here)
	double t  = 0;
	double dt;

	// The Lax-Friedrichs flux
	// (write your solution here)
	auto F = [&](double a, double b) -> double {
		return (f(a) + f(b)) / 2 - dx / (2 * dt) * (b - a);
	};

	//Please uncomment the next line:
	while (t < T) {
		// Update dT according to current CFL condition
		// (write your solution here)
		dt = std::min(T - t, CFL * dx / u.unaryExpr([&df](double x) -> double {
			                       return std::abs(df(x));
			                     }).maxCoeff());

		// Update current time
		// (write your solution here)
		t += dt;

		// Update the internal values of u
		// (write your solution here)
		std::swap(u, u_old);
		for (int i = 1; i < N; ++i) {
			u(i) = u_old(i) - dt / dx * (F(u_old(i), u_old(i + 1)) - F(u_old(i - 1), u_old(i)));
		}

		// Update boundary with non-reflecting Neumann bc
		// (write your solution here)
		u(0) = u(1);
		u(N) = u(N - 1);

		//Please uncomment the next line:
	}
}
//----------------LFEnd----------------

//----------------convLFBegin----------------
//! Computes error for a range of cell lengths and stores them to error vectors
/// @param[in] T the final time at which to compute the solution
/// @param[in] f flux function, as function
/// @param[in] df derivative of flux function, as function
/// @param[in] u0 the initial conditions, as function
/// @param[in] uex exact solution
/// @param[in] baseName string containing name to save the computed errors and resolution
void LFConvergence(double T, const std::function<double(double)> &f, const std::function<double(double)> &df, const std::function<double(double)> &u0, const std::function<double(double, double)> &uex, const std::string &baseName) {
	std::vector<int>    resolutions = {100, 200, 400, 800, 1600};
	std::vector<double> L1_errors;
	std::vector<double> Linf_errors;

	for (auto &N : resolutions) {
		// find approximate solution using Lax-Friedrichs scheme
		// (write your solution here)
		Eigen::VectorXd u;
		Eigen::VectorXd X;
		LaxFriedrichs(N, T, f, df, u0, u, X);

		// compute errors and push them bach to the corresponfing error vectors
		// (write your solution here)
		double L1_error{0};
		double Linf_error{0};
		for (int i = 0; i < N + 1; ++i) {
			double dx = X(i + 1) - X(i);
			double error = std::abs(u(i) - 1.0 / dx * integrate([&](double x) -> double {return uex(x, T);}, X(i), X(i + 1)));
			L1_error += dx * error;
			Linf_error = std::max(Linf_error, error);
		}
		L1_errors.emplace_back(L1_error);
		Linf_errors.emplace_back(Linf_error);
	}

	writeToFile(baseName + "_L1errors_LF.txt", L1_errors);
	writeToFile(baseName + "_Linferrors_LF.txt", Linf_errors);
	writeToFile(baseName + "_resolutions.txt", resolutions);
}
//----------------convLFEnd----------------

/* Fluxes for Burgers' equation */
double fBurgers(double u) {
	return std::pow(u, 2) / 2.;
}

double dfBurgers(double u) {
	return u;
}

/* Initial data and exact solutions for Burgers' equation  */
// i)
double U0i(double x) {
	if (x < 0.)
		return 1.;
	else
		return 0.;
}
double Uexi(double x, double t) {
	if (x < 0.5 * t)
		return 1.;
	else
		return 0.;
}

// ii)
double U0ii(double x) {
	if (x < 0.)
		return 0.;
	else
		return -2.;
}
double Uexii(double x, double t) {
	if (x < -t)
		return 0.;
	else
		return -2.;
}

// iii)
double U0iii(double x) {
	if (x < 0.)
		return 0.;
	else
		return 1.;
}
double Uexiii(double x, double t) {
	if (x < 0)
		return 0.;
	else if (x < t && t < 2)
		return x / t;
	else
		return 1.;
}

// iv)
double U0iv(double x) {
	if (0. < x && x < 1.)
		return 1.;
	else
		return 0.;
}
double Uexiv(double x, double t) {
	if (x <= 0)
		return 0.;
	else if ((x <= t && t <= 2) || (x < std::sqrt(2 * t) && t > 2))
		return x / t;
	else if (t <= 2 && t < x && (x < 1 + t / 2.))
		return 1.;
	else
		return 0.;
}

/* Flux for Buckley-Leverett equation*/
double fBL(double u) {
	return std::pow(u, 2) / (std::pow(u, 2) + std::pow(1 - u, 2));
}

double dfBL(double u) {
	return (2 * u * (u * u + (1 - u) * (1 - u)) - u * u * (2 * u - 2 * (1 - u))) / std::pow(u * u + (1 - u) * (1 - u), 2);
}

/* Initial data for Buckley-Leverett */
double U0BL(double x) {
	if (x < 0.)
		return 0.1;
	else
		return 0.9;
}

int main(int, char **) {
	double T = 0.8;
	int    N = 400;

	Eigen::VectorXd u, X;
	// Test for Burgers with initiald data i
	Godunov(N, T, fBurgers, dfBurgers, U0i, u, X);
	writeToFile("uBi_G.txt", u);
	LaxFriedrichs(N, T, fBurgers, dfBurgers, U0i, u, X);
	writeToFile("uBi_LF.txt", u);

	Godunov(N, T, fBurgers, dfBurgers, U0ii, u, X);
	writeToFile("uBii_G.txt", u);
	LaxFriedrichs(N, T, fBurgers, dfBurgers, U0ii, u, X);
	writeToFile("uBii_LF.txt", u);

	Godunov(N, T, fBurgers, dfBurgers, U0iii, u, X);
	writeToFile("uBiii_G.txt", u);
	LaxFriedrichs(N, T, fBurgers, dfBurgers, U0iii, u, X);
	writeToFile("uBiii_LF.txt", u);

	Godunov(N, T, fBurgers, dfBurgers, U0iv, u, X);
	writeToFile("uBiv_G.txt", u);
	LaxFriedrichs(N, T, fBurgers, dfBurgers, U0iv, u, X);
	writeToFile("uBiv_LF.txt", u);

	// compute convergence for Burgers with initial data i
	GodunovConvergence(T, fBurgers, dfBurgers, U0i, Uexi, "Burgers1");
	LFConvergence(T, fBurgers, dfBurgers, U0i, Uexi, "Burgers1");

	// compute convergence for Burgers with initial data ii
	GodunovConvergence(T, fBurgers, dfBurgers, U0ii, Uexii, "Burgers2");
	LFConvergence(T, fBurgers, dfBurgers, U0ii, Uexii, "Burgers2");

	// compute convergence for Burgers with initial data iii
	GodunovConvergence(T, fBurgers, dfBurgers, U0iii, Uexiii, "Burgers3");
	LFConvergence(T, fBurgers, dfBurgers, U0iii, Uexiii, "Burgers3");

	// compute convergence for Burgers with initial data iv
	GodunovConvergence(T, fBurgers, dfBurgers, U0iv, Uexiv, "Burgers4");
	LFConvergence(T, fBurgers, dfBurgers, U0iv, Uexiv, "Burgers4");

	// Test for Buckley-Leverett
	Godunov(N, T, fBL, dfBL, U0BL, u, X);
	writeToFile("uBL_G.txt", u);
	LaxFriedrichs(N, T, fBL, dfBL, U0BL, u, X);
	writeToFile("uBL_LF.txt", u);
}


#pragma once
#include "H1_norm.hpp"
#include "L2_norm.hpp"
#include "fem_solve.hpp"
#include "writer.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <functional>
#include <igl/readMESH.h>
#include <vector>

//----------------convBegin----------------
//! Compute the L2 and H1-error for different meshes using quadratic FEM
//!
//! @param baseMeshName the basename for the mesh (eg. Square).
//!                     the method will automatically append _<n>.stl
//!
//! @param maxLevel the max mesh level to use. Will read up to and including
//!                 baseMeshName_<maxLevel>.stl
//!
//! @param f the function f as in the exercise (LHS)
//! @param exactSol the exact solution (found by eg. hand computation or googling).
//! @param exactSol_grad the gradient of the exact solution.
void convergenceAnalysis(const std::string &baseMeshName, int maxLevel, const std::function<double(double, double)> f, const std::function<double(double, double)> exactSol, const std::function<Eigen::Vector2d(double, double)> exactSol_grad) {
	std::vector<double> differences_L2;
	std::vector<double> differences_H1;
	std::vector<int>    numberOfDegreesOfFreedom;
	for (int i = 0; i <= maxLevel; i++) {
		Vector          u;
		Eigen::MatrixXd vertices;
		Eigen::MatrixXi triangles;
		Eigen::MatrixXi tetrahedra;

		std::stringstream basenameSS;
		basenameSS << baseMeshName << "_" << i;
		std::string basename = basenameSS.str();

		igl::readMESH(std::string(NPDE_DATA_PATH)
		                  + basename
		                  + ".mesh",
		              vertices,
		              tetrahedra,
		              triangles);

		// Initialize quadratic Dofs
		QDofs quadraticDofs(vertices, triangles);
		// get dofs
		Eigen::MatrixXi dofs;
		quadraticDofs.get_dofs(dofs);

		std::cout << "Computing convergence. At: " << basename << std::endl;
		// solve finite element system
		// (write your solution here)
		numberOfDegreesOfFreedom.push_back(solveFiniteElement(u, quadraticDofs, f));

		//compute L2-error and save it in the corresponding vector
		// (write your solution here)
		differences_L2.push_back(computeL2Difference(vertices, dofs, u, exactSol));

		//compute H1-error and save it in the corresponding vector
		// (write your solution here)
		differences_H1.push_back(computeH1Difference(vertices, dofs, u, exactSol_grad));

		// store number of dofs in vector
		// (write your solution here)

		writeToFile(basename + "_values.txt", u);
		writeMatrixToFile(basename + "_vertices.txt", vertices);
		writeMatrixToFile(basename + "_triangles.txt", triangles);
	}

	std::stringstream errorL2Filename;
	errorL2Filename << baseMeshName << "_errors.txt";

	std::stringstream errorH1Filename;
	errorH1Filename << baseMeshName << "_errorsH1.txt";

	writeToFile(errorL2Filename.str(), differences_L2);
	writeToFile(errorH1Filename.str(), differences_H1);
	writeToFile(baseMeshName + "_resolutions.txt", numberOfDegreesOfFreedom);
}
//----------------convEnd----------------


/*
 * test.cpp

 *
 *  Created on: Oct 14, 2017
 *      Author: nmsutton
 */

#include "test.h"
//#include <iostream>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

/*
 * uncomment out below lines to get plotting working
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
 */

test::test()
{

}

test::~test()
{
}

/* we solve the simple ODE x' = 3/(2t^2) + x/(2t)
 * with initial condition x(1) = 0.
 * Analytic solution is x(t) = sqrt(t) - 1/t
 */

void test::rhs( const double x , double &dxdt , const double t )
{
    dxdt = 3.0/(2.0*t*t) + x/(2.0*t);
}

void test::write_cout( const double &x , const double t )
{
    cout << t << '\t' << x << endl;
    //x_data.push_back(x[0]);
}

// state_type = double
typedef runge_kutta_dopri5< double > stepper_type;

void test::run_test()
{
    double x = 0.0;
    runge_kutta_dopri5< double > rk2;
    //integrate_adaptive( make_controlled( 1E-12 , 1E-12 , stepper_type() ) , rhs , x , 1.0 , 10.0 , 0.1 , write_cout );
	double t = 1.0;
	const double dt = 0.1;//0.0025;
	int time_span = 90;

	//integrate_const( rk2 , rhs , x , 1.0 , 10.0 , 0.1, write_cout );
	for (int i = 0; i < time_span; i++) {
		t += dt;
		x_data.push_back(x);
		//y_data.push_back(0);
		integrate_const( rk2 , rhs , x , t , (t+dt) , dt);//, write_cout );
	}
	/*
	 * uncomment out below lines to get plotting working
    plt::plot(x_data);
    plt::show();
    */
}


#include <cstdlib>
#include <tuple>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>

namespace LinearRegression
{
	//h(X₁,X₂)=θ₁X₁+θ₂X₂
	//θ₁=10,θ₂=150,X₂=1
	std::tuple<Eigen::MatrixX2d, Eigen::VectorXd> RandomGenterateTrainSet(size_t counts)
	{
		Eigen::MatrixX2d train_input(counts, 2);
		Eigen::VectorXd train_output(counts);
		for (size_t i = 0; i < counts; i++)
		{
			float X₁ = std::rand() % 100 + 20, X₂ = 1;
			train_input(i, 0) = X₁;
			train_input(i, 1) = X₂;
			float θ₁ = 10, θ₂ = 150;
			float error = std::rand() % 11 - 5;
			float hx = θ₁ * X₁ + θ₂ * X₂ + error;
			train_output[i] = hx;
		}
		return { train_input/*.normalized()*/, train_output };
	}

	//              i      i
	//1/2m * ∑(h(x) - y(x) )²
	double LossFunction(const Eigen::MatrixXd& model, const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output)
	{
		auto hx_sub_jx = train_output - train_input * model;
		return hx_sub_jx.array().pow(2).sum() / (train_input.rows() * 2);
	}

	//                            i      i     i
	//θ = θ - α * 1/m * ∑(h(x) - y(x) ) * x
	// j    j                                  j
	void GradientDescent(Eigen::MatrixXd& model, const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output, const Eigen::VectorXd& learning_rate, double limit)
	{
		size_t batch_size = 0;
		Eigen::MatrixXd update = Eigen::MatrixXd::Zero(model.rows(), model.cols());
		do
		{
			batch_size++;
			Eigen::MatrixXd last_update;
			for (size_t train_index = 0; train_index < train_input.rows(); train_index++)
			{
				auto hx_sub_yx = train_input * model - train_output;
				//std::cout << hx_sub_yx << "\n\n";
				Eigen::MatrixXd duplicate_line(model.cols(), train_input.cols());
				auto&& reference = duplicate_line << train_input.row(train_index);
				for (size_t line = 0; line < model.cols() - 1; line++)
				{
					reference, train_input.row(train_index);
				}
				last_update = update;
				update = learning_rate.array() * (1.0 / train_input.rows() * hx_sub_yx.row(train_index) * duplicate_line).transpose().array();
				//std::cout << duplicate_line << "\n\n";
				//std::cout << update << "\n\n";
				model -= update;
				std::cout << model << "\n\n";
			}

			if (last_update.minCoeff() != 0)
			{
				//non-convergence
				if ((update.array().abs() > last_update.array().abs()).sum())
					break;
				//convergence
				if ((update.array() / last_update.array()).abs().maxCoeff() < limit)
					break;
			}
		} while (true);
		std::cout << "batch_size:" << batch_size << "\n\n";
	}

	void GradientDescent(Eigen::MatrixXd& model, const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output, const Eigen::VectorXd& learning_rate, size_t batch_size)
	{
		for (size_t i = 0; i < batch_size; i++)
		{
			for (size_t train_index = 0; train_index < train_input.rows(); train_index++)
			{
				auto hx_sub_yx = train_input * model - train_output;
				//std::cout << hx_sub_yx << "\n\n";
				Eigen::MatrixXd duplicate_line(model.cols(), train_input.cols());
				auto&& reference = duplicate_line << train_input.row(train_index);
				for (size_t line = 0; line < model.cols() - 1; line++)
				{
					reference, train_input.row(train_index);
				}
				Eigen::MatrixXd update = learning_rate.array() * (1.0 / train_input.rows() * hx_sub_yx.row(train_index) * duplicate_line).transpose().array();
				//std::cout << duplicate_line << "\n\n";
				//std::cout << update << "\n\n";
				model -= update;
				std::cout << model << "\n\n";
			}
		}
	}

	//  T   -1    T
	//(X *X)   * X  * y
	//\note This matrix must be invertible, otherwise the result is undefined.
	Eigen::MatrixXd NormalEquation(const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output)
	{
		if ((train_input.transpose() * train_input).fullPivLu().isInvertible())
		{
			return (train_input.transpose() * train_input).inverse() * train_input.transpose() * train_output;
		}
		else
		{
			std::cout << "Not invertible!" << std::endl;
			return {};
		}
	}
}

int main_()
{
	std::cout << std::boolalpha;

	auto [train_input, train_output] = LinearRegression::RandomGenterateTrainSet(100);
	//std::cout << train_input << std::endl;
	//std::cout << train_output << std::endl;

	Eigen::MatrixXd model = Eigen::Vector2d(1, 200);
	Eigen::Vector2d learning_rate(0.005, 1);
	double limit = 0.05;
	size_t batch_size = 25;
	LinearRegression::GradientDescent(model, train_input, train_output, learning_rate, limit/*batch_size*/);
	std::cout << LinearRegression::NormalEquation(train_input, train_output) << std::endl;
	return 0;
}


#ifndef GRAVITY_HPP
# define GRAVITY_HPP

#include <Eigen/Dense>

#include <cmath>

/** @brief Parameters for gravity model */
class GravBody {
protected:
  double mu;         /* (m3/s2) gravitational constant of planet */
  double eq_radius;  /* (m)     mean equatorial radius of planet */
public:
  GravBody(const double& mu_, const double& r_eq)
    : mu(mu_),
      eq_radius(r_eq)
  { }

  /** @brief Compute gravitational acceleration and gravity gradient
   *         (change in acceleration with respect to change in position)
   *
   * This function treats the planet as a point mass.
   *
   * @param[in]  r_pcpf   position in planet-centered, planet-fixed
   *                      coordinates; should have same units as
   *                      params->mu (meters, usually)
   * @param[out] a_pcpf   acceleration due to gravity in PCPF frame
   * @param[out] da_dr    partial of change in acceleration with respect
   *                      to change in position
   */
  void accel(const Eigen::Vector3d& r_pcpf,
	     Eigen::Vector3d& a_pcpf,
	     Eigen::Matrix3d& da_dr) const {
    double r2 = r_pcpf.squaredNorm();
    double r3 = r2 * sqrt(r2);
    double mu_over_r3 = mu / r3;

    // Compute acceleration vector
    a_pcpf = r_pcpf * -mu_over_r3;

    // Compute gravity gradient
    da_dr = (r_pcpf * r_pcpf.transpose() * 3.0 / r2 - Eigen::Matrix3d::Identity()) * mu_over_r3;
  }
};

#endif


#pragma once

#include <cstdio>
#include <dlib/optimization.h>
#include <cmath>
#include <iostream>

#define DIM 3

using namespace dlib;

class CovIntersection
{
public:
	void loadData(matrix<double, DIM, DIM> covA, matrix<double, DIM, DIM> covB, matrix<double, DIM, 1> posA, matrix<double, DIM, 1> posB)
	{
		CA = covA;
		CB = covB;

		ca = posA;
		cb = posB;
	}

	static double function(double x)
	{
		// double value = sum(diag(inv(inv(CA) + inv(CB) - inv(x*CA + (1-x)*CB))));
		return sum(diag(inv(inv(CA) + inv(CB) - inv(x*CA + (1 - x)*CB))));
		// return value;
	}

	static matrix<double, DIM, DIM> CA, CB;
	static matrix<double, DIM, 1> ca, cb;

	void optimize()
	{
		minValue = find_min_single_variable(&CovIntersection::function, starting_point, begin, end, eps, max_iter, initial_search_radius);
		minX = starting_point;
	}

	void computeFusedValues()
	{
		covFused = inv(inv(CA) + inv(CB) - inv(minX*CA + (1 - minX)*CB));

		matrix <double, DIM, DIM> KICI, LICI;
		KICI = covFused * (inv(CA) - minX * inv(minX*CA + (1 - minX)*CB));
		LICI = covFused * (inv(CB) - (1 - minX) * inv(minX*CA + (1 - minX)*CB));

		poseFused = KICI * ca + LICI * cb;
	}

	double minValue;
	double minX;

	matrix <double, DIM, DIM> covFused;
	matrix <double, DIM, 1> poseFused;

private:
	const double begin = 0.0;
	const double end = 1.0;
	double starting_point = 0.0;
	const double eps = 1e-3;
	const long max_iter = 100;
	const double initial_search_radius = 0.01;
	// print variables
};

matrix <double, DIM, DIM> CovIntersection::CA;
matrix <double, DIM, DIM> CovIntersection::CB;
matrix <double, DIM, 1> CovIntersection::ca;
matrix <double, DIM, 1> CovIntersection::cb;


#include <Eigen/Dense>
#include <cmath>
#include "nv_core.h"
#include "nv_num.h"
#include "nv_ml_gaussian.h"

// ガウス分布

float nv_gaussian_log_predict(const nv_cov_t *cov, const nv_matrix_t *x, int xm)
{
	Eigen::VectorXf X = Eigen::Map<Eigen::VectorXf>(&x->v[xm*x->n], x->n) - Eigen::Map<Eigen::VectorXf>(cov->u->v, x->n);
	Eigen::Map<Eigen::MatrixXf> Sigma(cov->cov->v, x->n, x->n);
	return log(1 / sqrt(pow(2 * acos(-1), x->n) * Sigma.determinant())) - X.dot(Sigma.llt().solve(X)) / 2;
}


#include "ode45.hpp"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Sparse>

using namespace Eigen;
using namespace std;

// Compute the maps Phi and W at time T, for initial data given by u0 and v0.
pair<Vector2d,Matrix2d> PhiAndW(double u0, double v0, double T) {

    auto f = [] (const VectorXd & w) {
        Eigen::VectorXd temp(6);
        temp(0) = (2. - w(1))*w(0);
        temp(1) = (w(0) - 1.)*w(1);
        temp(2) = (2. - w(1))*w(2) - w(0)*w(3);
        temp(3) = w(1)*w(2) + (w(0) - 1.)*w(3);
        temp(4) = (2. - w(1))*w(4) - w(0)*w(5);
        temp(5) = w(1)*w(4) + (w(0) - 1.)*w(5);
        return temp;
    };

    Eigen::VectorXd w0(6);
    w0 << u0, v0, 1., 0, 0, 1.;

    ode45<Eigen::VectorXd> O(f);
    O.options.rtol = 1e-14;
    O.options.atol = 1e-12;
    auto sol = O.solve(w0, T);
    VectorXd wT = sol.back().first;

    pair<Vector2d,Matrix2d> PaW;
    PaW.first << wT(0), wT(1);
    PaW.second << wT(2), wT(4), wT(3), wT(5);
    return PaW;
}

// Apply the Newton method to find initial data giving solutions with period equal to 5.
int main(){
    Vector2d y;
    y << 3, 2;
    double T = 5;
    pair<Vector2d,Matrix2d> PaW = PhiAndW(y(0), y(1), T);
    Vector2d F = PaW.first - y;
    Matrix2d DF;

    while (F.norm() > 1e-5) {
        PaW = PhiAndW(y(0), y(1), T);
        F = PaW.first - y;
        DF = PaW.second - MatrixXd::Identity(2,2);
        y = y - DF.lu().solve(F);
    }

    cout << "The obtained initial condition is: " << endl << y << endl;
    PaW = PhiAndW(y(0), y(1), 100);

    cout << "y(100) = " << endl << PaW.first << endl;
}

Nummerical Methods for CSE/PS13/solutions_ps13/LV.cpp


//
//  main.cpp
//  Exercise 5
//
//  Created by Zhehao Li on 2020/4/17.
//  Copyright © 2020 Zhehao Li. All rights reserved.
//
//  Exercise 5 Statistical Functions

#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions.hpp>             // For non-member functions of distributions

#include <vector>
#include <iostream>
using namespace std;

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

    using namespace boost::math;

    // 1. Create exponential distribution object
    double scaleParameter = 0.5;
    exponential_distribution<> myExponential(scaleParameter);          // Default type is 'double'
    cout << "Mean: " << mean(myExponential) << ", standard deviation: " << standard_deviation(myExponential) << endl;

    // 1.1 Distributional properties
    double x = 3.6;
    cout << "pdf: " << pdf(myExponential, x) << endl;
    cout << "cdf: " << cdf(myExponential, x) << endl;

    // 1.2 Choose precision
    cout.precision(10); // Number of values behind the comma

    // 1.3 Other properties
    cout << "\n*** Poisson Distribution ***\n";
    cout << "mean: "                    << mean(myExponential)              << endl;
    cout << "variance: "                << variance(myExponential)          << endl;
    cout << "median: "                  << median(myExponential)            << endl;
    cout << "mode: "                    << mode(myExponential)              << endl;
    cout << "kurtosis excess: "         << kurtosis_excess(myExponential)   << endl;
    cout << "kurtosis: "                << kurtosis(myExponential)          << endl;

    // 2. Poisson distribution
    double lmbda = 3.0;                                 // Mean
    poisson_distribution<> myPoisson(lmbda);            // Default type is 'double'

    double val = 13.0;
    cout << "Exponential pdf: " << pdf(myPoisson, val) << endl;
    cout << "Exponential cdf: " << cdf(myPoisson, val) << endl;

    vector<double> pdfList;
    vector<double> cdfList;

    double start = 0.0;
    double end = 10.0;
    long   N = 30;        // Number of subdivisions

    val = 0.0;
    double h = (end - start) / double(N);

    // Push the pdf and cdf value into vectors
    for (long j = 1; j <= N; ++j){
        pdfList.push_back(pdf(myPoisson, val));
        cdfList.push_back(cdf(myPoisson, val));

        val += h;
    }

    cout << "\n********\n" << endl;

    // Print the vector of pdf
    cout << "Poisson pdf:" << endl;
    for (long j = 0; j < pdfList.size(); ++j){
        cout << pdfList[j] << ", ";
    }

    cout << "\n********\n" << endl;

    // Print the vector of cdf
    cout << "Poisson cdf:" << endl;
    for (long j = 0; j < cdfList.size(); ++j){
        cout << cdfList[j] << ", ";
    }

    return 0;
}


#ifndef LQR_CONTROLLER_HPP
#define LQR_CONTROLLER_HPP

#include <armadillo>
#include <cmath>
class LQRController {

public:

    arma::mat A;
    arma::mat B;
    arma::mat Q;
    arma::mat R;
    arma::mat P;
    arma::mat Klqr;

    int maxIter;
    float eps = 1e-3;

    LQRController(const arma::mat& _A, const arma::mat& _B,
                const arma::mat& _Q, const arma::mat& _R,
                int _maxIter = 2000, float _eps = 1e-3) {

        maxIter = _maxIter;
        eps = _eps;

        compute_LQR_gain(_A, _B, _Q, _R);
    }

    void compute_LQR_gain(const arma::mat& _A, const arma::mat& _B, const arma::mat& _Q, const arma::mat& _R) {

        A = _A;
        B = _B;
        Q = _Q;
        R = _R;
        P = _Q;

        arma::mat Pold = P;
        arma::mat delta;

        for (int i = 0; i < maxIter; i++) {
            P = A.t() * P * A - (A.t() * P * B) * arma::inv(R + B.t() * P * B) * B.t() * P * A + Q;
            delta = Pold - P;
            if (std::abs(delta.max()) < eps) {
                break;
            }
            Pold = P;
        }

        Klqr = arma::inv(R + B.t() * P * B) * B.t() * P * A;
    }

    arma::vec get_control(const arma::vec & x) {
        return -Klqr * x;
    }

};

#endif


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of tikhonov class.
 */
#include "num_collect/regularization/tikhonov.h"

#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "eigen_approx.h"
#include "num_prob_collect/regularization/blur_sine.h"

TEST_CASE("num_collect::regularization::tikhonov") {
    using coeff_type = Eigen::MatrixXd;
    using data_type = Eigen::VectorXd;

    SECTION("solve") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 30;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());
        Eigen::VectorXd solution;
        tikhonov.solve(0.0, solution);

        REQUIRE_THAT(solution, eigen_approx(prob.solution()));
    }

    SECTION("solve with different parameters") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 30;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        constexpr double param_small = 1e-2;
        Eigen::VectorXd solution_small;
        tikhonov.solve(param_small, solution_small);

        constexpr double param_large = 1e+2;
        Eigen::VectorXd solution_large;
        tikhonov.solve(param_large, solution_large);

        REQUIRE(solution_large.squaredNorm() < solution_small.squaredNorm());
    }

    SECTION("singular_values") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 30;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        REQUIRE(tikhonov.singular_values().size() == solution_size);
        for (num_collect::index_type i = 0; i < solution_size; ++i) {
            INFO("i = " << i);
            REQUIRE(tikhonov.singular_values()(i) > 0.0);
        }
    }

    SECTION("calculate norms") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 10;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        constexpr double param = 1.0;
        Eigen::VectorXd solution;
        tikhonov.solve(param, solution);

        constexpr double rel_tol = 1e-6;

        SECTION("residual_norm") {
            const double expected =
                (prob.coeff() * solution - prob.data()).squaredNorm();
            REQUIRE_THAT(tikhonov.residual_norm(param),
                Catch::Matchers::WithinRel(expected, rel_tol));
        }

        SECTION("regularization term") {
            const double expected = solution.squaredNorm();
            REQUIRE_THAT(tikhonov.regularization_term(param),
                Catch::Matchers::WithinRel(expected, rel_tol));
        }
    }

    SECTION("calculate the first-order derivatives of norms") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 10;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        constexpr double param = 1.0;
        Eigen::VectorXd solution;
        tikhonov.solve(param, solution);

        constexpr double param_diff = param * 1e-3;
        constexpr double param_plus = param + param_diff;
        Eigen::VectorXd solution_plus;
        tikhonov.solve(param_plus, solution_plus);

        constexpr double rel_tol = 1e-2;

        SECTION("first_derivative_of_residual_norm") {
            const double expected =
                ((prob.coeff() * solution_plus - prob.data()).squaredNorm() -
                    (prob.coeff() * solution - prob.data()).squaredNorm()) /
                param_diff;
            REQUIRE_THAT(tikhonov.first_derivative_of_residual_norm(param),
                Catch::Matchers::WithinRel(expected, rel_tol));
        }

        SECTION("first_derivative_of_regularization_term") {
            const double expected =
                (solution_plus.squaredNorm() - solution.squaredNorm()) /
                param_diff;
            REQUIRE_THAT(
                tikhonov.first_derivative_of_regularization_term(param),
                Catch::Matchers::WithinRel(expected, rel_tol));
        }
    }

    SECTION("calculate the second-order derivatives of norms") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 10;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        constexpr double param = 1.0;
        Eigen::VectorXd solution;
        tikhonov.solve(param, solution);

        constexpr double param_diff = param * 1e-3;
        constexpr double param_plus = param + param_diff;
        Eigen::VectorXd solution_plus;
        tikhonov.solve(param_plus, solution_plus);

        constexpr double param_minus = param - param_diff;
        Eigen::VectorXd solution_minus;
        tikhonov.solve(param_minus, solution_minus);

        constexpr double rel_tol = 1e-2;

        SECTION("second_derivative_of_residual_norm") {
            const double expected =
                ((prob.coeff() * solution_plus - prob.data()).squaredNorm() -
                    2.0 *
                        (prob.coeff() * solution - prob.data()).squaredNorm() +
                    (prob.coeff() * solution_minus - prob.data())
                        .squaredNorm()) /
                (param_diff * param_diff);
            REQUIRE_THAT(tikhonov.second_derivative_of_residual_norm(param),
                Catch::Matchers::WithinRel(expected, rel_tol));
        }

        SECTION("second_derivative_of_regularization_term") {
            const double expected =
                (solution_plus.squaredNorm() - 2.0 * solution.squaredNorm() +
                    solution_minus.squaredNorm()) /
                (param_diff * param_diff);
            REQUIRE_THAT(
                tikhonov.second_derivative_of_regularization_term(param),
                Catch::Matchers::WithinRel(expected, rel_tol));
        }
    }

    SECTION("sum_of_filter_factor") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 10;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        constexpr double param = 1.0;

        const double expected = (tikhonov.singular_values().array().square() /
            (tikhonov.singular_values().array().square() + param))
                                    .sum();
        REQUIRE_THAT(tikhonov.sum_of_filter_factor(param),
            Catch::Matchers::WithinRel(expected));
    }

    SECTION("data_size") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 10;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        REQUIRE(tikhonov.data_size() == data_size);
    }

    SECTION("param_search_region") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 10;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        const double max_singular_value = tikhonov.singular_values().maxCoeff();
        const double squared_max_singular_value =
            max_singular_value * max_singular_value;
        const auto [min_param, max_param] = tikhonov.param_search_region();
        REQUIRE(min_param < squared_max_singular_value);
        REQUIRE(max_param > squared_max_singular_value);
    }
}


#include "utils.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/integer.hpp>
using namespace std;
using Bint = boost::multiprecision::cpp_int;

string FermatFactorizer_cppfunc(string s){
    Bint n(s);
    if(n%2 == 0){
        return "2";
    }
    Bint x = sqrt(n);
    if(pow(x,2)==n){
        return x.str();
    }
    x+=1;
    Bint y = sqrt(pow(x,2)-n);
    Bint w = pow(x,2)-n-pow(y,2);
    for(;;){
        if(w==0){
            Bint retval = x-y;
            return retval.str();
        }
        else if(w>0){
            y+=1;
        }
        else{
            x+=1;
        }
        w = pow(x,2)-n-pow(y,2);
    }
}


#include <fstream>
#include <cmath>
#include <boost/config.hpp>
#include <boost/version.hpp>

// CGAL headers
#include <CGAL/Cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Min_circle_2.h>
#include <CGAL/Min_circle_2_traits_2.h>
#include <CGAL/Min_ellipse_2.h>
#include <CGAL/Min_ellipse_2_traits_2.h>
#include <CGAL/convex_hull_2.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/min_quadrilateral_2.h>
#include <CGAL/rectangular_p_center_2.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsEllipseItem>
#include <QGraphicsRectItem>

// GraphicsView items and event filters (input classes)

#include <CGAL/Qt/PointsGraphicsItem.h>
#include <CGAL/Qt/PolygonGraphicsItem.h>

// for viewportsBbox
#include <CGAL/Qt/utility.h>

#include <CGAL/Qt/GraphicsViewPolylineInput.h>

// the two base classes
#include "ui_Bounding_volumes.h"
#include <CGAL/Qt/DemosMainWindow.h>

#include "Ellipse.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Vector_2 Vector_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;

typedef CGAL::Polygon_2<K> Polygon_2;

typedef CGAL::Min_circle_2<CGAL::Min_circle_2_traits_2<K> > Min_circle;
typedef CGAL::Min_ellipse_2<CGAL::Min_ellipse_2_traits_2<K> > Min_ellipse;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Bounding_volumes
{
  Q_OBJECT

private:
  Polygon_2 convex_hull, min_rectangle, min_parallelogram;
  Min_circle mc;
  Min_ellipse me;
  QGraphicsScene scene;

  std::vector<Point_2> points;
  CGAL::Qt::PointsGraphicsItem<std::vector<Point_2> > * pgi;
  CGAL::Qt::PolygonGraphicsItem<Polygon_2> * convex_hull_gi;
  CGAL::Qt::PolygonGraphicsItem<Polygon_2> * min_rectangle_gi;
  CGAL::Qt::PolygonGraphicsItem<Polygon_2> * min_parallelogram_gi;
  QGraphicsEllipseItem *cgi, *egi;

  const std::size_t P;
  QGraphicsRectItem *p_center[3];
  Iso_rectangle_2 p_center_iso_rectangle[3];
  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;

public:
  MainWindow();

public Q_SLOTS:

  void update();

  void update_from_points();

  void processInput(CGAL::Object o);

  void on_actionShowMinCircle_toggled(bool checked);

  void on_actionShowMinEllipse_toggled(bool checked);

  void on_actionShowMinRectangle_toggled(bool checked);

  void on_actionShowMinParallelogram_toggled(bool checked);

  void on_actionShowConvexHull_toggled(bool checked);

  void on_actionShowPCenter_toggled(bool checked);

  void on_actionInsertPoint_toggled(bool checked);

  void on_actionInsertRandomPoints_triggered();

  void on_actionLoadPoints_triggered();

  void on_actionSavePoints_triggered();

  void on_actionClear_triggered();

  void on_actionRecenter_triggered();

  virtual void open(QString fileName);

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow(), P(3)
{
  setupUi(this);

  QObject::connect(this, SIGNAL(changed()), this, SLOT(update()));

  // Add a GraphicItem for the Min_circle
  cgi = new QGraphicsEllipseItem;
  cgi->setPen(QPen(Qt::red, 0, Qt::SolidLine));
  cgi->hide();
  scene.addItem(cgi);

  egi = new QGraphicsEllipseItem;
  egi->setPen(QPen(Qt::magenta, 0, Qt::SolidLine));
  egi->hide();
  scene.addItem(egi);

  for(std::size_t i =0; i < P; i++){
    p_center[i] = new QGraphicsRectItem;
    p_center[i]->setPen(QPen(Qt::cyan, 0, Qt::SolidLine));
    p_center[i]->hide();
    scene.addItem(p_center[i]);
  }

  // Graphics Item for the input point set
  pgi = new CGAL::Qt::PointsGraphicsItem<std::vector<Point_2> >(&points);

  QObject::connect(this, SIGNAL(changed()),
		   pgi, SLOT(modelChanged()));
  pgi->setVerticesPen(QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(pgi);

  // Graphics Item for the convex hull
  convex_hull_gi = new CGAL::Qt::PolygonGraphicsItem<Polygon_2>(&convex_hull);

  QObject::connect(this, SIGNAL(changed()),
		   convex_hull_gi, SLOT(modelChanged()));
  convex_hull_gi->setEdgesPen(QPen(Qt::black, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(convex_hull_gi);

  // Graphics Item for the min rectangle
  min_rectangle_gi = new CGAL::Qt::PolygonGraphicsItem<Polygon_2>(&min_rectangle);

  QObject::connect(this, SIGNAL(changed()),
		   min_rectangle_gi, SLOT(modelChanged()));
  min_rectangle_gi->setEdgesPen(QPen(Qt::green, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(min_rectangle_gi);

  // Graphics Item for the min parallelogram
  min_parallelogram_gi = new CGAL::Qt::PolygonGraphicsItem<Polygon_2>(&min_parallelogram);

  QObject::connect(this, SIGNAL(changed()),
		   min_parallelogram_gi, SLOT(modelChanged()));
  min_parallelogram_gi->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(min_parallelogram_gi);

  // Setup input handlers. They get events before the scene gets them
  // and the input they generate is passed to the triangulation with
  // the signal/slot mechanism
  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 1);

  scene.installEventFilter(pi);

  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
		   this, SLOT(processInput(CGAL::Object)));

  //
  // Manual handling of actions
  //

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
		   this, SLOT(close()));

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Turn the vertical axis upside down
  this->graphicsView->matrix().scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Bounding_volumes.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
	  this, SLOT(open(QString)));
}

void
MainWindow::update()
{
  if(this->actionShowConvexHull->isChecked()){
    convex_hull_gi->show();
  }else {
    convex_hull_gi->hide();
  }

  if(this->actionShowMinRectangle->isChecked()){
    min_rectangle_gi->show();
  }else {
    min_rectangle_gi->hide();
  }

  if(this->actionShowMinParallelogram->isChecked()){
    min_parallelogram_gi->show();
  }else {
    min_parallelogram_gi->hide();
  }

  CGAL::Qt::Converter<K> convert;

  if(this->actionShowPCenter->isChecked() && convex_hull.size()>=3){
    for(std::size_t i=0; i< P; i++){
      p_center[i]->setRect(convert(p_center_iso_rectangle[i]));
      p_center[i]->show();
    }
  }

  if (mc.is_degenerate() || (! this->actionShowMinCircle->isChecked())){
    cgi->hide();
  } else {
    K::Circle_2 c;
    if (mc.number_of_support_points() == 2)
      c = K::Circle_2(mc.support_point(0), mc.support_point(1));
    else
      c = K::Circle_2(mc.support_point(0), mc.support_point(1), mc.support_point(2));

    cgi->setRect(convert(c.bbox()));
    cgi->show();
  }

  if (me.is_degenerate()  || (! this->actionShowMinEllipse->isChecked()) ){
    egi->hide();
  } else {
    if (me.number_of_support_points() == 2) {
    } else {
      Ellipse_2<K> e(me);
      double half_width = sqrt(e.va() * e.va());
      double half_height = sqrt(e.vb() * e.vb());
      double angle = std::atan2( e.va().y(), e.va().x() ) * 180.0/CGAL_PI;
      Vector_2 wh(half_width, half_height);

      Iso_rectangle_2 isor(e.center()+ wh, e.center()-wh);
      egi->setRect(convert(isor));
      // Rotate an item 45 degrees around (x, y).
      double x = e.center().x();
      double y = e.center().y();
      egi->setTransform(QTransform().translate(x, y).rotate(angle).translate(-x, -y));
      egi->show();
    }
  }
}

void
MainWindow::update_from_points()
{
    convex_hull.clear();
    CGAL::convex_hull_2(points.begin(), points.end(), std::back_inserter(convex_hull));

    min_rectangle.clear();
    CGAL::min_rectangle_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_rectangle));

    min_parallelogram.clear();
    CGAL::min_parallelogram_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_parallelogram));

    std::vector<Point_2> center;
    double radius;

    CGAL::rectangular_p_center_2 (points.begin(), points.end(), std::back_inserter(center), radius, static_cast<int>(P));
    Vector_2 rvec(radius, radius);

    for(std::size_t i = 0; i < center.size(); i++){
      p_center_iso_rectangle[i] = Iso_rectangle_2(center[i]-rvec, center[i]+rvec);
    }
}

void
MainWindow::processInput(CGAL::Object o)
{
  std::list<Point_2> input;
  if(CGAL::assign(input, o)){
    Point_2 p = input.front();

    mc.insert(p);
    me.insert(p);
    points.push_back(p);

    convex_hull.push_back(p);
    Polygon_2 tmp;
    CGAL::convex_hull_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(tmp));
    convex_hull = tmp;

    min_rectangle.clear();
    CGAL::min_rectangle_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_rectangle));

    min_parallelogram.clear();
    CGAL::min_parallelogram_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_parallelogram));

    std::vector<Point_2> center;
    double radius;
    if (points.size()>=P){
      CGAL::rectangular_p_center_2 (points.begin(), points.end(), std::back_inserter(center), radius, static_cast<int>(P));
      Vector_2 rvec(radius, radius);

      for(std::size_t i=0; i < center.size(); i++){
        p_center_iso_rectangle[i] = Iso_rectangle_2(center[i]-rvec, center[i]+rvec);
      }
    }
  }
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */
void
MainWindow::on_actionInsertPoint_toggled(bool checked)
{
  if(checked){
    scene.installEventFilter(pi);
  } else {
    scene.removeEventFilter(pi);
  }
}

void
MainWindow::on_actionShowMinCircle_toggled(bool checked)
{
  cgi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowMinEllipse_toggled(bool checked)
{
  egi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowMinRectangle_toggled(bool checked)
{
  min_rectangle_gi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowMinParallelogram_toggled(bool checked)
{
  min_parallelogram_gi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowConvexHull_toggled(bool checked)
{
  convex_hull_gi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowPCenter_toggled(bool checked)
{
  for(std::size_t i =0; i < P; i++){
    p_center[i]->setVisible(checked);
  }
  Q_EMIT( changed());
}

void
MainWindow::on_actionClear_triggered()
{
  mc.clear();
  me.clear();
  points.clear();
  convex_hull.clear();
  min_rectangle.clear();
  min_parallelogram.clear();
  for(std::size_t i=0; i < P;i++){
    p_center[i]->hide();
  }
  Q_EMIT( changed());
}

void
MainWindow::on_actionInsertRandomPoints_triggered()
{
  QRectF rect = CGAL::Qt::viewportsBbox(&scene);
  CGAL::Qt::Converter<K> convert;
  Iso_rectangle_2 isor = convert(rect);
  CGAL::Random_points_in_iso_rectangle_2<Point_2> pg((isor.min)(), (isor.max)());
  bool ok = false;

  const int number_of_points =
    QInputDialog::getInt(this,
                             tr("Number of random points"),
                             tr("Enter number of random points"),
			     100,
			     0,
			     (std::numeric_limits<int>::max)(),
			     1,
			     &ok);

  if(!ok) {
    return;
  }

  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  for(int i = 0; i < number_of_points; ++i){
    Point_2 p = *pg++;
    mc.insert(p);
    me.insert(p);
    points.push_back(p);
  }

  update_from_points();

  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadPoints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
						  tr("Open Points file"),
                                                  ".",
                                                  tr("CGAL files (*.pts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.WKT *.wkt);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    CGAL::read_multi_point_WKT(ifs, points);
    for(K::Point_2 p : points)
    {
      mc.insert(p);
      me.insert(p);
    }
#endif
  }
  else
  {
    K::Point_2 p;
    while(ifs >> p) {
      mc.insert(p);
      me.insert(p);
      points.push_back(p);
    }
  }
  update_from_points();

  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  actionRecenter->trigger();
  Q_EMIT( changed());

}

void
MainWindow::on_actionSavePoints_triggered()
{
  QString fileName = QFileDialog::getSaveFileName(this,
						  tr("Save points"),
                                                  ".",
                                                  tr("CGAL files (*.pts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.WKT *.wkt);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    std::ofstream ofs(qPrintable(fileName));
    if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
    {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      std::vector<K::Point_2> out_pts;
      out_pts.reserve(std::distance(mc.points_begin(),
                                    mc.points_end()));
      for(Min_circle::Point_iterator pit = mc.points_begin();
          pit != mc.points_end(); ++pit)
        out_pts.push_back(*pit);
      CGAL::write_multi_point_WKT(ofs, out_pts);
#endif
    }
    else
    {
      for(Min_circle::Point_iterator
          vit = mc.points_begin(),
          end = mc.points_end();
          vit!= end; ++vit)
      {
        ofs << *vit << std::endl;
      }
    }
  }
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(cgi->boundingRect());
  this->graphicsView->fitInView(cgi->boundingRect(), Qt::KeepAspectRatio);
}

#include "Bounding_volumes.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Bounding_volumes demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}


#ifndef QUADRATIC_FORM_HPP
#define QUADRATIC_FORM_HPP

#include <Eigen/Dense>

// quadraticform in three variables
struct QuadraticForm{
    // x1, x2, x3
    // A*x1*x1 + B*x1*x2 + C*x2*x2 + D*x1*x3 + E*x2*x3 + F*x3*x3 = 0
    double A, B, C, D, E, F;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
    Eigen::Matrix3d mat;

    QuadraticForm(double a, double b, double c, double d, double e, double f) :
        A(a), B(b), C(c), D(d), E(e), F(f) {
        mat << A, B/2, D/2,
               B/2, C, E/2,
               D/2, E/2, F;
        eigensolver = Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>(mat);
    }

    QuadraticForm& operator*=(double k) {
        A*=k; B*=k; C*=k; D*=k; E*=k; F*=k;
        Eigen::Matrix3d _mat;
        _mat << A, B/2, D/2,
                B/2, C, E/2,
                D/2, E/2, F;
        mat = _mat;
        eigensolver = Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>(mat);
        return *this;
    }

    double evaluate(const Eigen::Vector3d& vec) {
        return vec.transpose()*mat*vec;
    }

};

#endif // QUADRATIC_FORM_HPP


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use the general purpose non-linear
    optimization routines from the dlib C++ Library.

    The library provides implementations of many popular algorithms such as L-BFGS
    and BOBYQA.  These algorithms allow you to find the minimum or maximum of a
    function of many input variables.  This example walks though a few of the ways
    you might put these routines to use.

*/

#include <dlib/optimization.h>
#include <dlib/global_optimization.h>
#include <iostream>

using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------

// In dlib, most of the general purpose solvers optimize functions that take a
// column vector as input and return a double.  So here we make a typedef for a
// variable length column vector of doubles.  This is the type we will use to
// represent the input to our objective functions which we will be minimizing.
typedef matrix<double,0,1> column_vector;

// ----------------------------------------------------------------------------------------
// Below we create a few functions.  When you get down into main() you will see that
// we can use the optimization algorithms to find the minimums of these functions.
// ----------------------------------------------------------------------------------------

double rosen (const column_vector& m)
/*
    This function computes what is known as Rosenbrock's function.  It is
    a function of two input variables and has a global minimum at (1,1).
    So when we use this function to test out the optimization algorithms
    we will see that the minimum found is indeed at the point (1,1).
*/
{
    const double x = m(0);
    const double y = m(1);

    // compute Rosenbrock's function and return the result
    return 100.0*pow(y - x*x,2) + pow(1 - x,2);
}

// This is a helper function used while optimizing the rosen() function.
const column_vector rosen_derivative (const column_vector& m)
/*!
    ensures
        - returns the gradient vector for the rosen function
!*/
{
    const double x = m(0);
    const double y = m(1);

    // make us a column vector of length 2
    column_vector res(2);

    // now compute the gradient vector
    res(0) = -400*x*(y-x*x) - 2*(1-x); // derivative of rosen() with respect to x
    res(1) = 200*(y-x*x);              // derivative of rosen() with respect to y
    return res;
}

// This function computes the Hessian matrix for the rosen() fuction.  This is
// the matrix of second derivatives.
matrix<double> rosen_hessian (const column_vector& m)
{
    const double x = m(0);
    const double y = m(1);

    matrix<double> res(2,2);

    // now compute the second derivatives
    res(0,0) = 1200*x*x - 400*y + 2; // second derivative with respect to x
    res(1,0) = res(0,1) = -400*x;   // derivative with respect to x and y
    res(1,1) = 200;                 // second derivative with respect to y
    return res;
}

// ----------------------------------------------------------------------------------------

class rosen_model
{
    /*!
        This object is a "function model" which can be used with the
        find_min_trust_region() routine.
    !*/

public:
    typedef ::column_vector column_vector;
    typedef matrix<double> general_matrix;

    double operator() (
        const column_vector& x
    ) const { return rosen(x); }

    void get_derivative_and_hessian (
        const column_vector& x,
        column_vector& der,
        general_matrix& hess
    ) const
    {
        der = rosen_derivative(x);
        hess = rosen_hessian(x);
    }
};

// ----------------------------------------------------------------------------------------

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_optimization_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
try
{
    // Set the starting point to (4,8).  This is the point the optimization algorithm
    // will start out from and it will move it closer and closer to the function's
    // minimum point.   So generally you want to try and compute a good guess that is
    // somewhat near the actual optimum value.
    column_vector starting_point = {4, 8};

    // The first example below finds the minimum of the rosen() function and uses the
    // analytical derivative computed by rosen_derivative().  Since it is very easy to
    // make a mistake while coding a function like rosen_derivative() it is a good idea
    // to compare your derivative function against a numerical approximation and see if
    // the results are similar.  If they are very different then you probably made a
    // mistake.  So the first thing we do is compare the results at a test point:
    cout << "Difference between analytic derivative and numerical approximation of derivative: "
         << length(derivative(rosen)(starting_point) - rosen_derivative(starting_point)) << endl;

    cout << "Find the minimum of the rosen function()" << endl;
    // Now we use the find_min() function to find the minimum point.  The first argument
    // to this routine is the search strategy we want to use.  The second argument is the
    // stopping strategy.  Below I'm using the objective_delta_stop_strategy which just
    // says that the search should stop when the change in the function being optimized
    // is small enough.

    // The other arguments to find_min() are the function to be minimized, its derivative,
    // then the starting point, and the last is an acceptable minimum value of the rosen()
    // function.  That is, if the algorithm finds any inputs to rosen() that gives an output
    // value <= -1 then it will stop immediately.  Usually you supply a number smaller than
    // the actual global minimum.  So since the smallest output of the rosen function is 0
    // we just put -1 here which effectively causes this last argument to be disregarded.

    find_min(bfgs_search_strategy(),  // Use BFGS search algorithm
             objective_delta_stop_strategy(1e-7), // Stop when the change in rosen() is less than 1e-7
             rosen, rosen_derivative, starting_point, -1);
    // Once the function ends the starting_point vector will contain the optimum point
    // of (1,1).
    cout << "rosen solution:\n" << starting_point << endl;

    // Now let's try doing it again with a different starting point and the version
    // of find_min() that doesn't require you to supply a derivative function.
    // This version will compute a numerical approximation of the derivative since
    // we didn't supply one to it.
    starting_point = {-94, 5.2};
    find_min_using_approximate_derivatives(bfgs_search_strategy(),
                                           objective_delta_stop_strategy(1e-7),
                                           rosen, starting_point, -1);
    // Again the correct minimum point is found and stored in starting_point
    cout << "rosen solution:\n" << starting_point << endl;

    // Here we repeat the same thing as above but this time using the L-BFGS
    // algorithm.  L-BFGS is very similar to the BFGS algorithm, however, BFGS
    // uses O(N^2) memory where N is the size of the starting_point vector.
    // The L-BFGS algorithm however uses only O(N) memory.  So if you have a
    // function of a huge number of variables the L-BFGS algorithm is probably
    // a better choice.
    starting_point = {0.8, 1.3};
    find_min(lbfgs_search_strategy(10),  // The 10 here is basically a measure of how much memory L-BFGS will use.
             objective_delta_stop_strategy(1e-7).be_verbose(),  // Adding be_verbose() causes a message to be
                                                                // printed for each iteration of optimization.
             rosen, rosen_derivative, starting_point, -1);

    cout << endl << "rosen solution: \n" << starting_point << endl;

    starting_point = {-94, 5.2};
    find_min_using_approximate_derivatives(lbfgs_search_strategy(10),
                                           objective_delta_stop_strategy(1e-7),
                                           rosen, starting_point, -1);
    cout << "rosen solution: \n"<< starting_point << endl;

    // dlib also supports solving functions subject to bounds constraints on
    // the variables.  So for example, if you wanted to find the minimizer
    // of the rosen function where both input variables were in the range
    // 0.1 to 0.8 you would do it like this:
    starting_point = {0.1, 0.1}; // Start with a valid point inside the constraint box.
    find_min_box_constrained(lbfgs_search_strategy(10),
                             objective_delta_stop_strategy(1e-9),
                             rosen, rosen_derivative, starting_point, 0.1, 0.8);
    // Here we put the same [0.1 0.8] range constraint on each variable, however, you
    // can put different bounds on each variable by passing in column vectors of
    // constraints for the last two arguments rather than scalars.

    cout << endl << "constrained rosen solution: \n" << starting_point << endl;

    // You can also use an approximate derivative like so:
    starting_point = {0.1, 0.1};
    find_min_box_constrained(bfgs_search_strategy(),
                             objective_delta_stop_strategy(1e-9),
                             rosen, derivative(rosen), starting_point, 0.1, 0.8);
    cout << endl << "constrained rosen solution: \n" << starting_point << endl;

    // In many cases, it is useful if we also provide second derivative information
    // to the optimizers.  Two examples of how we can do that are shown below.
    starting_point = {0.8, 1.3};
    find_min(newton_search_strategy(rosen_hessian),
             objective_delta_stop_strategy(1e-7),
             rosen,
             rosen_derivative,
             starting_point,
             -1);
    cout << "rosen solution: \n"<< starting_point << endl;

    // We can also use find_min_trust_region(), which is also a method which uses
    // second derivatives.  For some kinds of non-convex function it may be more
    // reliable than using a newton_search_strategy with find_min().
    starting_point = {0.8, 1.3};
    find_min_trust_region(objective_delta_stop_strategy(1e-7),
                          rosen_model(),
                          starting_point,
                          10 // initial trust region radius
    );
    cout << "rosen solution: \n"<< starting_point << endl;

    // Next, let's try the BOBYQA algorithm.  This is a technique specially
    // designed to minimize a function in the absence of derivative information.
    // Generally speaking, it is the method of choice if derivatives are not available
    // and the function you are optimizing is smooth and has only one local optima.  As
    // an example, consider the be_like_target function defined below:
    column_vector target = {3, 5, 1, 7};
    auto be_like_target = [&](const column_vector& x) {
        return mean(squared(x-target));
    };
    starting_point = {-4,5,99,3};
    find_min_bobyqa(be_like_target,
                    starting_point,
                    9,    // number of interpolation points
                    uniform_matrix<double>(4,1, -1e100),  // lower bound constraint
                    uniform_matrix<double>(4,1, 1e100),   // upper bound constraint
                    10,    // initial trust region radius
                    1e-6,  // stopping trust region radius
                    100    // max number of objective function evaluations
    );
    cout << "be_like_target solution:\n" << starting_point << endl;

    // Finally, let's try the find_min_global() routine.  Like find_min_bobyqa(),
    // this technique is specially designed to minimize a function in the absence
    // of derivative information.  However, it is also designed to handle
    // functions with many local optima.  Where BOBYQA would get stuck at the
    // nearest local optima, find_min_global() won't.  find_min_global() uses a
    // global optimization method based on a combination of non-parametric global
    // function modeling and BOBYQA style quadratic trust region modeling to
    // efficiently find a global minimizer.  It usually does a good job with a
    // relatively small number of calls to the function being optimized.
    //
    // You also don't have to give it a starting point or set any parameters,
    // other than defining bounds constraints.  This makes it the method of
    // choice for derivative free optimization in the presence of multiple local
    // optima.  Its API also allows you to define functions that take a
    // column_vector as shown above or to explicitly use named doubles as
    // arguments, which we do here.
    auto complex_holder_table = [](double x0, double x1)
    {
        // This function is a version of the well known Holder table test
        // function, which is a function containing a bunch of local optima.
        // Here we make it even more difficult by adding more local optima
        // and also a bunch of discontinuities.

        // add discontinuities
        double sign = 1;
        for (double j = -4; j < 9; j += 0.5)
        {
            if (j < x0 && x0 < j+0.5)
                x0 += sign*0.25;
            sign *= -1;
        }
        // Holder table function tilted towards 10,10 and with additional
        // high frequency terms to add more local optima.
        return -( std::abs(sin(x0)*cos(x1)*exp(std::abs(1-std::sqrt(x0*x0+x1*x1)/pi))) -(x0+x1)/10 - sin(x0*10)*cos(x1*10));
    };

    // To optimize this difficult function all we need to do is call
    // find_min_global()
    auto result = find_min_global(complex_holder_table,
                                  {-10,-10}, // lower bounds
                                  {10,10}, // upper bounds
                                  std::chrono::milliseconds(500) // run this long
                                  );

    cout.precision(9);
    // These cout statements will show that find_min_global() found the
    // globally optimal solution to 9 digits of precision:
    cout << "complex holder table function solution y (should be -21.9210397): " << result.y << endl;
    cout << "complex holder table function solution x:\n" << result.x << endl;
}
catch (std::exception& e)
{
    cout << e.what() << endl;
}


#ifndef MLT_MODELS_CLASSIFIERS_PERCEPTRON_HPP
#define MLT_MODELS_CLASSIFIERS_PERCEPTRON_HPP

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

#include "linear_classifier.hpp"
#include "../../utils/linear_algebra.hpp"

namespace mlt {
namespace models {
namespace classifiers {
template <class Rng>
class Perceptron : public LinearClassifier<Perceptron<Rng>> {
public:
	template <class R, class = enable_if<is_same<decay_t<R>, Rng>::value>>
	Perceptron(size_t epochs,  bool shuffle, double learning_rate, bool fit_intercept, R&& rng) : LinearClassifier(fit_intercept),
		_epochs(epochs), _shuffle(shuffle), _learning_rate(learning_rate), _rng(forward<R>(rng)) {}

	Self& fit(Features input, Target classes, bool cold_start = true) {
		auto n_classes = classes.maxCoeff() + 1;
		auto n_features = input.rows();
		auto n_samples = input.cols();

		MatrixXd current_coeffs = _fitted && !cold_start && num_classes() == n_classes ?
		                                coefficients() :
		                                (MatrixXd::Zero(n_classes, n_features + (fit_intercept() ? 1 : 0)) * 0.005);

		MatrixXd input_prime(input.rows() + (fit_intercept() ? 1 : 0), n_samples);
		input_prime.topRows(input.rows()) << input;

		if (fit_intercept()) {
			input_prime.bottomRows<1>() = VectorXd::Ones(n_samples);
		}

		vector<int> idxs;
		if (_shuffle) {
			idxs = vector<int>(n_samples);
			for (auto i = 0; i < n_samples; i++) {
				idxs[i] = i;
			}
		}

		for (auto epoch = 0; epoch < _epochs; epoch++) {
			if (_shuffle) {
				shuffle(idxs.begin(), idxs.end(), _rng);
			}

			for (auto i = 0; i < n_samples; i++) {
				auto idx = _shuffle ? idxs[i] : i;

				auto f = input.col(idx).eval();
				auto t = classes(idx);
				auto y = max_row(_apply_linear_transformation(f, current_coeffs));

				if (t != y) {
					auto f_t = f.transpose().eval();
					current_coeffs.block(y, 0, 1, n_features) -= _learning_rate * f_t;
					current_coeffs.block(t, 0, 1, n_features) += _learning_rate * f_t;

					if (fit_intercept()) {
						current_coeffs(y, n_classes) -= 1;
						current_coeffs(t, n_classes) += 1;
					}
				}
			}
		}

		_set_coefficients(current_coeffs);

		return *this;
	}

protected:
	size_t _epochs;
	bool _shuffle;
	double _learning_rate;
	Rng& _rng;
};

	template <class R = default_random_engine>
	auto create_perceptron(size_t epoch = 100, bool shuffle = true, double learning_rate = 0.001, bool fit_intercept = true, R&& rng = default_random_engine()) {
		return Perceptron<R>(epoch, shuffle, learning_rate, fit_intercept, forward<R>(rng));
	}
}
}
}
#endif


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2014 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Principal Component Analysis                                             *
***************************************************************************/

#ifndef PRINCIPALCOMPONENTANALYSIS_HPP
#define PRINCIPALCOMPONENTANALYSIS_HPP

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <omp.h>

using namespace Eigen;

class PrincipalComponentAnalysis
{
public:
    PrincipalComponentAnalysis();

    template <class Derived>
    void compute(const MatrixBase<Derived> &dataset, const bool zscores = false);
    template <class Derived>
    MatrixXd dimensionalityReductionComponents(const MatrixBase<Derived> &dataset, const unsigned int components);
    template <class Derived>
    MatrixXd dimensionalityReductionVarianceExplained(const MatrixBase<Derived> &dataset, const double varianceExplained, const unsigned int minComponents = 0, const unsigned int maxComponents = 0);
    template <class Derived>
    Matrix<typename Derived::Scalar, Dynamic, Dynamic> filteringComponents(const MatrixBase<Derived> &dataset, const unsigned int components);
    template <class Derived>
    Matrix<typename Derived::Scalar, Dynamic, Dynamic> filteringVarianceExplained(const MatrixBase<Derived> &dataset, const double varianceExplained, const unsigned int minComponents = 0, const unsigned int maxComponents = 0);

    MatrixXd scores() const { return m_scores; }
    MatrixXd coefficients() const { return m_coefficients; }
    VectorXd latent() const { return m_latent; }
    VectorXd explained() const { return m_explained; }
    RowVectorXd mu() const { return m_mu; }
    unsigned int components() const { return m_components; }

private:
    MatrixXd m_scores;
    MatrixXd m_coefficients;
    VectorXd m_latent;
    VectorXd m_explained;
    RowVectorXd m_mu;
    unsigned int m_components;
    SelfAdjointEigenSolver<MatrixXd> m_solver;
};

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

PrincipalComponentAnalysis::PrincipalComponentAnalysis()
: m_scores(MatrixXd()), m_coefficients(MatrixXd()), m_latent(VectorXd()), m_explained(VectorXd()), m_mu(RowVectorXd()), m_components(0)
{
}

template<class Derived>
void PrincipalComponentAnalysis::compute(const MatrixBase<Derived> &dataset, const bool zscores)
{
    // Check extreme case
    if (dataset.rows() == 1)
    {
        m_latent = VectorXd::Ones(1);
        m_explained = m_latent / m_latent.sum();
        m_coefficients = MatrixXd::Ones(dataset.cols(), 1);
        m_scores = dataset.template cast<double>() * m_coefficients;
        return;
    }

    // Subtract mean of each variable
    m_mu = dataset.template cast<double>().colwise().mean();
    MatrixXd centered = dataset.template cast<double>().rowwise() - m_mu;

    // Standarize std if required
    if (zscores)
    {
        RowVectorXd stdScaling = (centered.cwiseProduct(centered).colwise().sum() / (double) (dataset.rows() - 1)).cwiseSqrt();
        #pragma omp parallel for
        for (int i = 0; i < centered.rows(); ++i)
            centered.row(i) = centered.row(i).cwiseQuotient(stdScaling);
    }

    // Compute the covariance matrix.
    MatrixXd covarianceMatrix = (centered.transpose() * centered) / (double)(dataset.rows() - 1);

    // Compute Singular Value Decomposition
    m_solver.compute(covarianceMatrix);

    // Store results
    m_latent = m_solver.eigenvalues();
    m_explained = m_latent / m_latent.sum();
    m_coefficients = m_solver.eigenvectors();
    m_scores = centered * m_coefficients;
}

template<class Derived>
MatrixXd PrincipalComponentAnalysis::dimensionalityReductionComponents(const MatrixBase<Derived> &dataset, const unsigned int components)
{
    // Compute PCA
    compute(dataset);

    // Get the dataset in the new components dimensional space
    m_components = components;
    return m_scores.rightCols(components);
}

template<typename  Derived>
MatrixXd PrincipalComponentAnalysis::dimensionalityReductionVarianceExplained(const MatrixBase<Derived> &dataset, const double varianceExplained, const unsigned int minComponents, const unsigned int maxComponents)
{
    // Compute PCA
    compute(dataset);

    // Get components to explain at least <varianceExplained>
    m_components = 0;
    double cumsum = 0;
    for (int i = m_explained.size() - 1; i >= 0; --i)
    {
        m_components++;
        cumsum += m_explained(i);
        if (cumsum > varianceExplained)
            break;
    }
    if (minComponents != 0)
        m_components = m_components < minComponents ? minComponents : m_components;
    if (maxComponents != 0)
        m_components = m_components > maxComponents ? maxComponents : m_components;

    // Get the dataset in the new components dimensional space
    return m_scores.rightCols(m_components);
}

template <typename  Derived>
Matrix<typename Derived::Scalar, Dynamic, Dynamic> PrincipalComponentAnalysis::filteringComponents(const MatrixBase<Derived> &dataset, const unsigned int components)
{
    // Compute PCA
    compute(dataset);

    // Rebuild dataset with components
    m_components = components;
    MatrixXd filteredDataset = (m_scores.rightCols(m_components) * m_coefficients.rightCols(m_components).transpose());
    filteredDataset = filteredDataset.rowwise() + m_mu;

    return filteredDataset.cast<Derived::Scalar>();
}

template <typename  Derived>
Matrix<typename Derived::Scalar, Dynamic, Dynamic> PrincipalComponentAnalysis::filteringVarianceExplained(const MatrixBase<Derived> &dataset, const double varianceExplained, const unsigned int minComponents, const unsigned int maxComponents)
{
    // Compute PCA
    compute(dataset);

    // Get components to explain at least <varianceExplained>
    m_components = 0;
    double cumsum = 0;
    for (int i = m_explained.size() - 1; i >= 0; i--)
    {
        m_components++;
        cumsum += m_explained(i);
        if (cumsum > varianceExplained)
            break;
    }
    if (minComponents != 0)
        m_components = m_components < minComponents ? minComponents : m_components;
    if (maxComponents != 0)
        m_components = m_components > maxComponents ? maxComponents : m_components;

    // Rebuild dataset with components
    MatrixXd filteredDataset = m_scores.rightCols(m_components) * m_coefficients.rightCols(m_components).transpose();
    filteredDataset = filteredDataset.rowwise() + m_mu;

    return filteredDataset.cast<typename Derived::Scalar>();
}

#endif


#include "LinearRegression.h"
#include "TrainingType.h"
#include <armadillo>
#include <assert.h>
#include <iostream>

using namespace arma;

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

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

LinearRegression::~LinearRegression() {}

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

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

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

void LinearRegression::NormalEquation() {
  mat xtx = (this->x.t() * this->x);
  mat L = eye<mat>(xtx.n_rows, xtx.n_cols);
  L[0] = 0;
  // Check if xtx is full-rank matrix
  if (rank(xtx) == xtx.n_rows || this->regPara > 0) {
    this->theta = pinv(xtx - (this->regPara * L)) * this->x.t() * this->y;
    this->trained = true;
  } else {
    std::cerr << "you have to regularize your data set" << std::endl;
  }
}

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

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

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

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

double LinearRegression::Cost(mat &inputX) {
  this->InitializeTheta();
  //--J(Theta) = 1/2m * (X Theta - y)^T (X Theta - y) + lambda theta^2--//
  assert(inputX.n_cols == this->theta.n_rows);
  vec ve = (inputX * this->theta) - this->y;
  vec thetaWithoutFirst = this->theta;
  thetaWithoutFirst[0] = 0;
  return (((float)1 / 2) * this->ExampleNumber() * ve.t() * ve +
          this->regPara * thetaWithoutFirst.t() * thetaWithoutFirst)
      .eval()(0, 0);
}

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

  this->trained = true;
}

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


#include <Eigen/Core>
#include <iostream>
#include <LBFGS.h>

using Eigen::VectorXd;
using Eigen::MatrixXd;
using namespace LBFGSpp;

double foo(const VectorXd& x, VectorXd& grad)
{
    const int n = x.size();
    VectorXd d(n);
    for(int i = 0; i < n; i++)
        d[i] = i;

    double f = (x - d).squaredNorm();
    grad.noalias() = 2.0 * (x - d);
    return f;
}

int main()
{
    const int n = 10;
    LBFGSParam<double> param;
    LBFGSSolver<double> solver(param);

    VectorXd x = VectorXd::Zero(n);
    double fx;
    int niter = solver.minimize(foo, x, fx);

    std::cout << niter << " iterations" << std::endl;
    std::cout << "x = \n" << x.transpose() << std::endl;
    std::cout << "f(x) = " << fx << std::endl;

    return 0;
}