#include <cmath>
#include <fftw3.h>
#include <tuple>
#include <vector>

#include "specgram.h"

std::vector<float> window_hanning(int size) {
    std::vector<float> window;
    float scale =  2 * M_PI / (size - 1);
    for (int n = 0; n < size; n++)
        window.push_back(0.5 * (1 - cos(scale * n)));
    return window;
}

std::pair<int, int> specgram_shape(int n, int NFFT,
                                   int noverlap) {
    int stride = NFFT - noverlap;
    int time_steps = (n - noverlap) / stride;
    int n_freqs = NFFT / 2 + 1;
    return std::make_pair(n_freqs, time_steps);
}

void log_cspecgram(const int16_t* const in, int n,
                  int NFFT, int Fs,
                  int noverlap, float* out,
                  float epsilon /* default=1e-7 */) {
    cspecgram(in, n, NFFT, Fs, noverlap, out);
    auto spec_shape = specgram_shape(n, NFFT, noverlap);
    int size = spec_shape.first * spec_shape.second;
    for (int i = 0; i < size; i++)
        out[i] = log(out[i] + epsilon);
}

void cspecgram(const int16_t* const in, int n,
              int NFFT, int Fs,
              int noverlap, float* out) {

    int stride = NFFT - noverlap;
    int n_freqs;
    int time_steps;
    std::tie(n_freqs, time_steps) = specgram_shape(n, NFFT, noverlap);

    std::vector<float> window(window_hanning(NFFT));

    float *x = new float[NFFT * time_steps];

    float* in_f = new float[n];
    for (int i = 0; i < n; i++)
        in_f[i] = static_cast<float>(in[i]);

    for (int i = 0; i < time_steps; i++) {
        for (int j = 0; j < NFFT; j++) {
            x[i * NFFT + j] = window[j]  * in_f[i * stride + j];
        }
    }

    // Do an fft for each NFFT seg of x
    fftwf_complex* res = (fftwf_complex*) fftwf_malloc(
                            sizeof(fftwf_complex) * n_freqs * time_steps);

    fftwf_plan plan = fftwf_plan_many_dft_r2c(1, &NFFT,
                              time_steps, x, NULL, 1, NFFT,
                              res, NULL, 1, n_freqs,
                              FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
    fftwf_execute(plan);
    fftwf_destroy_plan(plan);

    for (int i = 0; i < time_steps; i++) {
        int s = i * n_freqs;
        for (int j = 0; j < n_freqs; j++) {
            out[s + j] = res[s + j][0] * res[s + j][0] +
                            res[s + j][1] * res[s + j][1];
        }
    }

    // Cleanup temporaries
    fftwf_free(res);
    delete[] x;
    delete[] in_f;

    // Scale output in the same style as matplotlib
    float scale = 0;
    for (float w : window)
        scale += w * w;
    scale *= Fs;
    for (int i = 0; i < time_steps; i++) {
        int s = i * n_freqs;
        out[s] /= scale;
        for (int j = 1; j < n_freqs - 1; j++) {
            out[s + j] *= (2.0 / scale);
        }
        if (NFFT % 2 == 0) {
            out[s + n_freqs - 1] /= scale;
        } else {
            out[s + n_freqs - 1] *= (2.0 / scale);
        }
    }
}

#ifdef PYTHON
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
#include <boost/python/extract.hpp>
#include <numpy/noprefix.h>

namespace py = boost::python;
namespace np = boost::python::numeric;

void specgram(np::array input, int NFFT, int Fs,
               int noverlap, np::array output) {

    py::object i_shape = input.attr("shape");
    py::object o_shape = output.attr("shape");

    if (len(i_shape) != 1 || len(o_shape) != 2)
        throw std::invalid_argument("Bad input or ouput shape");
    unsigned int n = py::extract<unsigned int>(i_shape[0]);

    int16_t* in_dat = static_cast<int16_t*>(PyArray_DATA(input.ptr()));
    float* out_dat = static_cast<float*>(PyArray_DATA(output.ptr()));
    cspecgram(in_dat, n, NFFT, Fs, noverlap, out_dat);
}

BOOST_PYTHON_MODULE(cspecgram) {
    np::array::set_module_and_type("numpy", "ndarray");
    py::def("specgram", &specgram);
}
#endif


//
// Created by Vlad Argunov on 19/10/2021.
//

#include "Regression.h"

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

Regression::Regression(std::string type) {
    type_regression = type;
}

void Regression::set_data(Eigen::MatrixXd x_values_, Eigen::VectorXd y_values_) {
    x_values = x_values_;
    y_values = y_values_;
}

void Regression::add_constant() {
    Eigen::MatrixXd x_values_with_constant;
    x_values_with_constant.resize(x_values.rows(), x_values.cols() + 1);
    x_values_with_constant.rightCols(x_values.cols()) = x_values;

    x_values_with_constant.col(0) = Eigen::VectorXd::Ones(x_values.rows());
    x_values = x_values_with_constant;

}

void Regression::print_data() {
    std::cout << "Data Matrix:\n";
    std::cout << x_values << "\n";
}

void Regression::print_responses() {
    std::cout << "Responses vector:\n";
    std::cout << y_values << "\n";
}

void Regression::solve() {
    if (type_regression == "Linear"){
        Eigen::MatrixXd computation_matrix;
        computation_matrix.resize(x_values.rows(), x_values.rows());
        computation_matrix = (x_values.transpose() * x_values).inverse();
        parameters = computation_matrix * x_values.transpose() * y_values;
    }
}

void Regression::print_parameters() {
    std::cout << "Parameters vector:\n";
    std::cout << parameters << "\n";
}


#pragma once
#ifndef MANIFOLDMATH_H
#define MANIFOLDMATH_H

#include <vector>
#include <memory>

#include <Eigen/Core>

#include <engine/Vectormath_Defines.hpp>

namespace Engine
{
    // Basically all math inside this namespace assumes that the given
    //      vectorfields contain unit vectors. They are therefore interpreted
    //      as living in a manifold which is the direct product space of N
    //      spheres.
    // The only exception are functions which interpret the vectorfield
    //      as a single 3N-dimensional vector.
    // TODO: cleanly separate these two cases!
    namespace Manifoldmath
    {
        // Get the norm of a vectorfield (interpreted as a 3N-vector)
        scalar norm(const vectorfield & vf);
        // Normalize a vectorfield (interpreted as a 3N-vector)
        void normalize(vectorfield & vf);

        // Project v1 to be parallel to v2
        //      Note: this assumes normalized vectorfields
        void project_parallel(vectorfield & vf1, const vectorfield & vf2);
        // Project v1 to be orthogonal to v2
        //      Note: this assumes normalized vectorfields
        void project_orthogonal(vectorfield & vf1, const vectorfield & vf2);
        // Invert v1's component parallel to v2
        //      Note: this assumes normalized vectorfields
        void invert_parallel(vectorfield & vf1, const vectorfield & vf2);
        // Invert v1's component orthogonal to v2
        //      Note: this assumes normalized vectorfields
        void invert_orthogonal(vectorfield & vf1, const vectorfield & vf2);
        // Project vf1's vectors into the tangent plane of vf2
        //      Note: vf2 must have normalized vectors
        void project_tangential(vectorfield & vf1, const vectorfield & vf2);

        // The tangential projector is a matrix which projects any vector into the tangent
        //      space of a vectorfield, considered to live on the direct product of N unit
        //      spheres. It is a 3N x 3N matrix.
        MatrixX tangential_projector(const vectorfield & image);

        // Calculate a matrix of orthonormal basis vectors that span the tangent space to
        //      a vectorfield, considered to live on the direct product of N unit spheres.
        //      The basis vectors will be the spherical unit vectors, except at the poles.
        //      The basis will be a 3Nx2N matrix.
        void tangent_basis_spherical(const vectorfield & vf, MatrixX & basis);

        // Calculate a matrix of orthonormal basis vectors that span the tangent space to
        //      a vectorfield, considered to live on the direct product of N unit spheres.
        //      The basis vectors will be generated from cross products with euclidean basis
        //      vectors. The basis will be a 3Nx2N matrix.
        void tangent_basis_cross(const vectorfield & vf, MatrixX & basis);

        // Calculate a matrix of orthonormal basis vectors that span the tangent space to
        //      a vectorfield, considered to live on the direct product of N unit spheres.
        //      The basis vectors will form righthanded sets with the vectors of vf.
        //      The basis will be a 3Nx2N matrix.
        void tangent_basis_righthanded(const vectorfield & vf, MatrixX & basis);

        // Calculate a matrix of orthonormal basis vectors that span the tangent space to
        //      a vectorfield, considered to live on the direct product of N unit spheres.
        //      The basis vectors will be calculated from orthonormalizations with respect
        //      to a random vector. The basis will be a 3Nx2N matrix.
        // void tangent_basis_random(const vectorfield & vf, MatrixX & basis);

        // This is the derivatives of the coordinate transformation from (unit-)spherical to cartesian
        // i.e. d_cartesian/d_spherical
        void spherical_to_cartesian_jacobian(const vectorfield & vf, MatrixX & jacobian);

        // This is the second derivatives of the coordinate transformation from (unit-)spherical to cartesian
        // i.e. d^2_cartesian/d^2_spherical
        void spherical_to_cartesian_hessian(const vectorfield & vf, MatrixX & gamma_x, MatrixX & gamma_y, MatrixX & gamma_z);

        //
        void spherical_to_cartesian_coordinate_basis(const vectorfield & vf, MatrixX & basis);

        //
        void spherical_coordinate_christoffel_symbols(const vectorfield & vf, MatrixX & gamma_theta, MatrixX & gamma_phi);

        // Calculate Hessian for a vectorfield constrained to unit length, at any extremum (i.e. where vectors || gradient)
        void hessian_bordered(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out);
        // Calculate tangential derivatives and correction terms according to the projector approach
        void hessian_projected(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out);
        // Calculate tangential derivatives and correction terms according to the projector and Weingarten map approach
        void hessian_weingarten(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out);
        // Calculate spherical derivatives using coordinate transformations via jacobians
        void hessian_spherical(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & hessian_out);
        // Calculate spherical derivates and correction terms using jacobians and christoffel symbols
        void hessian_covariant(const vectorfield & image,  const vectorfield & gradient, const MatrixX & hessian, MatrixX & hessian_out);

        // Geodesic distance between two vectorfields
        scalar dist_geodesic(const vectorfield & v1, const vectorfield & v2);

        // Calculate the "tangent" vectorfields pointing between a set of configurations
        void Tangents(std::vector<std::shared_ptr<vectorfield>> configurations, const std::vector<scalar> & energies, std::vector<vectorfield> & tangents);
    }
}

#endif


#include "lrgNormalEquationSolverStrategy.h"
#include <vector>
#include <random>
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

DataSolver::DataSolver(){}

// empty class method above

std::pair<double, double> &DataSolver::FitData(std::vector<std::pair<double, double> >&Inputs,std::pair<double, double>&Theta){

// as with the gradient solver, we need to create input matricies (the code was copied from this file so it is identical)

    VectorXd Y_e = VectorXd::Ones(Inputs.size());
    MatrixXd M_X = MatrixXd::Ones(Inputs.size(),2);

    for(int i=0; i < Inputs.size();++i){
        Y_e(i) = Inputs[i].second;
        M_X(i,0) = Inputs[i].first;
    }

// create a matrix to append our output theta to

    MatrixXd Theta_e = MatrixXd::Ones(2,1);

// here is all the linear algebra

    Theta_e =  (((M_X.transpose()*M_X).inverse())*(M_X.transpose()))*Y_e;

    Theta.first = Theta_e(0);
    Theta.second = Theta_e(1);

    return Theta;
};


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

// Author: Marc Hartung

#ifndef MTL_MATRIX_QR_GIVENS_INCLUDE
#define MTL_MATRIX_QR_GIVENS_INCLUDE

#include <cmath>
#include <utility>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>

namespace mtl { namespace matrix {

/// General Given's transformator
/**
 * Input-matrix A will be splitted in A = Q*R. Q and R are accessible from getQ() and getR().
 * \sa givens
 */
template <typename Matrix>
class qr_givens_solver {

    typedef typename Collection<Matrix>::value_type value_type;
    typedef typename Collection<Matrix>::size_type size_type;

public:

   /** \brief Constructor needs a sqare-matrix as input
     * \param MTL-Matrix type
     *
     */

    qr_givens_solver(const Matrix& IN) : R(IN), G(2,2), Q(num_cols(IN), num_rows(IN)) {
	value_type one = math::one(c);
	Q = one;
	eps = 1.0e-8;
    }

    /** \brief Returns Q as the input-matrix-type
     *
     * \return Matrix Q
     */

    Matrix& getQ() {
	return Q;
    }

    /** \brief Returns R as the input-matrix-type
     *
     * \return Matrix R
     */
    Matrix& getR() {
	return R;
    }

    /** \brief Sets the distance to zero
     *
     * \param value_type tolerance to zero
     */
    void setTolerance(value_type tol) {
	eps = tol;
    }

    /** \brief Starts QR decompostion
     */
    void calc()
    {
	value_type zero= math::zero(c);
	Matrix RIter(2, num_rows(R)), Iter(2, num_rows(R));
	for(size_type i= 0; i < R.num_cols() - 1; i++) {
	    irange r(i, num_cols(R));
 	    for(size_type j= num_rows(R)-1;j>i;j--) {
		if (std::abs(R[j][i]) > eps) {
		    set_rotation(R[i][i], R[j][i]);
		    Iter[0][r] = R[i][r];
		    Iter[1][r] = R[j][r];
		    RIter[iall][r] = G*Iter[iall][r];
		    R[i][r] = RIter[0][r];
		    R[j][r] = RIter[1][r];
		    Iter[0][iall] = Q[i][iall];
		    Iter[1][iall] = Q[j][iall];
		    RIter = G*Iter;
		    Q[i][iall] = RIter[0][iall];
		    Q[j][iall] = RIter[1][iall];
		}
		R[j][i] = zero;
	    }

	}

	for(size_type i= 0; i < num_cols(R) - 1; i++)
	    R[i+1][i] = zero;

    }

  private:

    Matrix     R, G, Q;
    value_type c, s, eps;

    template <typename T>
    inline T square(T x) const { return x * x; }

    /// Coefficients for Given's rotation
    void set_rotation(value_type a, value_type b)
    {
	using std::abs;
	using mtl::conj;

	value_type zero= math::zero(a), one= math::one(b), t;

	if ( b == zero ) {
	    c= one; s= zero;
	} else if ( abs(b) > abs(a) ) {
	    t = a / abs(b);
	    s = one/ sqrt(one + square(abs(t)));
	    c = s * t;
	    s *= (b/abs(b));
	} else {
	    t = b / abs(a);
	    c = one / sqrt(one + square(abs(t)));
	    s = c * t;
	    c = (a/abs(a))*c;
	}

	G= conj(c), conj(s),
	  -s, c;
    }

};

/// QR-Factorization of matrix A(m x n) based on Givens' rotation
template <typename Matrix>
std::pair<Matrix, Matrix>
inline qr_givens(const Matrix& A)
{
    qr_givens_solver<Matrix> solver(A);
    solver.calc();
    return std::make_pair(solver.getQ(), solver.getR());
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_QR_GIVENS_INCLUDE


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_ref_snip6

#include <boost/math/distributions/negative_binomial.hpp>
using boost::math::negative_binomial;

// Use the default rounding policy integer_round_outwards.
// Lower quantile rounded down:
double x = quantile(negative_binomial(20, 0.3), 0.05); // rounded up 27 from 27.3898
// Upper quantile rounded up:
double y = quantile(complement(negative_binomial(20, 0.3), 0.05)); // rounded down to 69 from 68.1584

//] //[/policy_ref_snip6]

#include <iostream>
using std::cout; using std::endl;

int main()
{
   cout << "quantile(negative_binomial(20, 0.3), 0.05) = "<< x <<endl
     << "quantile(complement(negative_binomial(20, 0.3), 0.05)) = " << y << endl;
}

/*
Output:

  quantile(negative_binomial(20, 0.3), 0.05) = 27
  quantile(complement(negative_binomial(20, 0.3), 0.05)) = 69
*/


// Copyright (C) 2010  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/optimization.h>
#include "optimization_test_functions.h"
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../rand.h"

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;
    using namespace dlib::test_functions;

    logger dlog("test.least_squares");

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

    void test_with_chebyquad()
    {
        print_spinner();
        {
            matrix<double,0,1> ch;

            ch = chebyquad_start(2);

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "chebyquad 2 obj: " << chebyquad(ch);
            dlog << LINFO << "chebyquad 2 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "chebyquad 2 error: " << length(ch - chebyquad_solution(2));

            DLIB_TEST(length(ch - chebyquad_solution(2)) < 1e-5);

        }
        {
            matrix<double,0,1> ch;

            ch = chebyquad_start(2);

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "LM chebyquad 2 obj: " << chebyquad(ch);
            dlog << LINFO << "LM chebyquad 2 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "LM chebyquad 2 error: " << length(ch - chebyquad_solution(2));

            DLIB_TEST(length(ch - chebyquad_solution(2)) < 1e-5);

        }

        print_spinner();
        {
            matrix<double,2,1> ch;

            ch = chebyquad_start(2);

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "chebyquad 2 obj: " << chebyquad(ch);
            dlog << LINFO << "chebyquad 2 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "chebyquad 2 error: " << length(ch - chebyquad_solution(2));

            DLIB_TEST(length(ch - chebyquad_solution(2)) < 1e-5);

        }
        print_spinner();
        {
            matrix<double,2,1> ch;

            ch = chebyquad_start(2);

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "LM chebyquad 2 obj: " << chebyquad(ch);
            dlog << LINFO << "LM chebyquad 2 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "LM chebyquad 2 error: " << length(ch - chebyquad_solution(2));

            DLIB_TEST(length(ch - chebyquad_solution(2)) < 1e-5);

        }

        print_spinner();
        {
            matrix<double,0,1> ch;

            ch = chebyquad_start(4);

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "chebyquad 4 obj: " << chebyquad(ch);
            dlog << LINFO << "chebyquad 4 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "chebyquad 4 error: " << length(ch - chebyquad_solution(4));

            DLIB_TEST(length(ch - chebyquad_solution(4)) < 1e-5);

        }
        print_spinner();
        {
            matrix<double,0,1> ch;

            ch = chebyquad_start(4);

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "LM chebyquad 4 obj: " << chebyquad(ch);
            dlog << LINFO << "LM chebyquad 4 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "LM chebyquad 4 error: " << length(ch - chebyquad_solution(4));

            DLIB_TEST(length(ch - chebyquad_solution(4)) < 1e-5);

        }

        print_spinner();
        {
            matrix<double,0,1> ch;

            ch = chebyquad_start(6);

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "chebyquad 6 obj: " << chebyquad(ch);
            dlog << LINFO << "chebyquad 6 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "chebyquad 6 error: " << length(ch - chebyquad_solution(6));

            // the ch variable contains a permutation of what is in chebyquad_solution(6).
            // Apparently there is more than one minimum?.  Just check that the objective
            // goes to zero.
            DLIB_TEST(chebyquad(ch) < 1e-10);

        }
        print_spinner();
        {
            matrix<double,0,1> ch;

            ch = chebyquad_start(6);

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "LM chebyquad 6 obj: " << chebyquad(ch);
            dlog << LINFO << "LM chebyquad 6 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "LM chebyquad 6 error: " << length(ch - chebyquad_solution(6));

            DLIB_TEST(chebyquad(ch) < 1e-10);

        }

        print_spinner();
        {
            matrix<double,0,1> ch;

            ch = chebyquad_start(8);

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "chebyquad 8 obj: " << chebyquad(ch);
            dlog << LINFO << "chebyquad 8 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "chebyquad 8 error: " << length(ch - chebyquad_solution(8));

            DLIB_TEST(length(ch - chebyquad_solution(8)) < 1e-5);

        }
        print_spinner();
        {
            matrix<double,0,1> ch;

            ch = chebyquad_start(8);

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                chebyquad_residual,
                                derivative(chebyquad_residual),
                                range(0,ch.size()-1),
                                ch);

            dlog << LINFO << "LM chebyquad 8 obj: " << chebyquad(ch);
            dlog << LINFO << "LM chebyquad 8 der: " << length(chebyquad_derivative(ch));
            dlog << LINFO << "LM chebyquad 8 error: " << length(ch - chebyquad_solution(8));

            DLIB_TEST(length(ch - chebyquad_solution(8)) < 1e-5);

        }
    }

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

    void test_with_brown()
    {
        print_spinner();
        {
            matrix<double,4,1> ch;

            ch = brown_start();

            solve_least_squares(objective_delta_stop_strategy(1e-13, 300),
                                brown_residual,
                                derivative(brown_residual),
                                range(1,20),
                                ch);

            dlog << LINFO << "brown obj: " << brown(ch);
            dlog << LINFO << "brown der: " << length(brown_derivative(ch));
            dlog << LINFO << "brown error: " << length(ch - brown_solution());

            DLIB_TEST_MSG(length(ch - brown_solution()) < 1e-5,length(ch - brown_solution()) );

        }
        print_spinner();
        {
            matrix<double,4,1> ch;

            ch = brown_start();

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                brown_residual,
                                derivative(brown_residual),
                                range(1,20),
                                ch);

            dlog << LINFO << "LM brown obj: " << brown(ch);
            dlog << LINFO << "LM brown der: " << length(brown_derivative(ch));
            dlog << LINFO << "LM brown error: " << length(ch - brown_solution());

            DLIB_TEST(length(ch - brown_solution()) < 1e-5);

        }
    }

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

// These functions are declared here because wrapping the real rosen functions in this
// way avoids triggering a bug in visual studio 2005 which prevents this code from compiling.
    double rosen_residual_double (int i, const matrix<double,2,1>& m)
    { return rosen_residual(i,m); }
    float rosen_residual_float (int i, const matrix<float,2,1>& m)
    { return rosen_residual(i,m); }

    matrix<double,2,1> rosen_residual_derivative_double (int i, const matrix<double,2,1>& m)
    { return rosen_residual_derivative(i,m); }
    matrix<float,2,1> rosen_residual_derivative_float (int i, const matrix<float,2,1>& m)
    { return rosen_residual_derivative(i,m); }

    double rosen_big_residual_double (int i, const matrix<double,2,1>& m)
    { return rosen_big_residual(i,m); }

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

    void test_with_rosen()
    {

        print_spinner();
        {
            matrix<double,2,1> ch;

            ch = rosen_start<double>();

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                rosen_residual_double,
                                rosen_residual_derivative_double,
                                range(1,20),
                                ch);

            dlog << LINFO << "rosen obj: " << rosen(ch);
            dlog << LINFO << "rosen error: " << length(ch - rosen_solution<double>());

            DLIB_TEST(length(ch - rosen_solution<double>()) < 1e-5);

        }
        print_spinner();
        {
            matrix<double,2,1> ch;

            ch = rosen_start<double>();

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                rosen_residual_double,
                                rosen_residual_derivative_double,
                                range(1,20),
                                ch);

            dlog << LINFO << "lm rosen obj: " << rosen(ch);
            dlog << LINFO << "lm rosen error: " << length(ch - rosen_solution<double>());

            DLIB_TEST(length(ch - rosen_solution<double>()) < 1e-5);

        }

        print_spinner();
        {
            matrix<double,2,1> ch;

            ch = rosen_start<double>();

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                rosen_residual_double,
                                derivative(rosen_residual_double),
                                range(1,20),
                                ch);

            dlog << LINFO << "rosen obj: " << rosen(ch);
            dlog << LINFO << "rosen error: " << length(ch - rosen_solution<double>());

            DLIB_TEST(length(ch - rosen_solution<double>()) < 1e-5);

        }
        print_spinner();
        {
            matrix<float,2,1> ch;

            ch = rosen_start<float>();

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                rosen_residual_float,
                                derivative(rosen_residual_float),
                                range(1,20),
                                ch);

            dlog << LINFO << "float rosen obj: " << rosen(ch);
            dlog << LINFO << "float rosen error: " << length(ch - rosen_solution<float>());

            DLIB_TEST(length(ch - rosen_solution<float>()) < 1e-5);

        }
        print_spinner();
        {
            matrix<float,2,1> ch;

            ch = rosen_start<float>();

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                rosen_residual_float,
                                derivative(rosen_residual_float),
                                range(1,20),
                                ch);

            dlog << LINFO << "LM float rosen obj: " << rosen(ch);
            dlog << LINFO << "LM float rosen error: " << length(ch - rosen_solution<float>());

            DLIB_TEST(length(ch - rosen_solution<float>()) < 1e-5);

        }
        print_spinner();
        {
            matrix<double,2,1> ch;

            ch = rosen_start<double>();

            solve_least_squares_lm(objective_delta_stop_strategy(1e-13, 80),
                                rosen_residual_double,
                                derivative(rosen_residual_double),
                                range(1,20),
                                ch);

            dlog << LINFO << "LM rosen obj: " << rosen(ch);
            dlog << LINFO << "LM rosen error: " << length(ch - rosen_solution<double>());

            DLIB_TEST(length(ch - rosen_solution<double>()) < 1e-5);

        }
        print_spinner();
        {
            matrix<double,2,1> ch;

            ch = rosen_big_start<double>();

            solve_least_squares(objective_delta_stop_strategy(1e-13, 80),
                                rosen_big_residual_double,
                                derivative(rosen_big_residual_double),
                                range(1,2),
                                ch);

            dlog << LINFO << "rosen big obj: " << rosen_big(ch);
            dlog << LINFO << "rosen big error: " << length(ch - rosen_big_solution<double>());

            DLIB_TEST(length(ch - rosen_big_solution<double>()) < 1e-5);

        }
    }

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

    class optimization_tester : public tester
    {
    public:
        optimization_tester (
        ) :
            tester ("test_least_squares",
                    "Runs tests on the least squares optimization component.")
        {}

        void perform_test (
        )
        {
            test_with_chebyquad();
            test_with_brown();
            test_with_rosen();
        }
    } a;

}


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2015 Adam Wulkiewicz, Lodz, Poland.

// 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)

//[multi_polygon
//` Declaration and use of the Boost.Geometry model::multi_polygon, modelling the MultiPolygon Concept

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

namespace bg = boost::geometry;

int main()
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
    typedef bg::model::polygon<point_t> polygon_t; /*< Default parameters, clockwise, closed polygon. >*/
    typedef bg::model::multi_polygon<polygon_t> mpolygon_t; /*< Clockwise, closed multi_polygon. >*/

    mpolygon_t mpoly1; /*< Default-construct a multi_polygon. >*/

#if !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX) \
 && !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST)

    mpolygon_t mpoly2{{{{0.0, 0.0}, {0.0, 5.0}, {5.0, 5.0}, {5.0, 0.0}, {0.0, 0.0}},
                       {{1.0, 1.0}, {4.0, 1.0}, {4.0, 4.0}, {1.0, 4.0}, {1.0, 1.0}}},
                      {{{5.0, 5.0}, {5.0, 6.0}, {6.0, 6.0}, {6.0, 5.0}, {5.0, 5.0}}}}; /*< Construct a multi_polygon containing two polygons, using C++11 unified initialization syntax. >*/

#endif

    mpoly1.resize(2); /*< Resize a multi_polygon, store two polygons. >*/

    bg::append(mpoly1[0].outer(), point_t(0.0, 0.0)); /*< Append point to the exterior ring of the first polygon. >*/
    bg::append(mpoly1[0].outer(), point_t(0.0, 5.0));
    bg::append(mpoly1[0].outer(), point_t(5.0, 5.0));
    bg::append(mpoly1[0].outer(), point_t(5.0, 0.0));
    bg::append(mpoly1[0].outer(), point_t(0.0, 0.0));

    mpoly1[0].inners().resize(1); /*< Resize a container of interior rings of the first polygon. >*/
    bg::append(mpoly1[0].inners()[0], point_t(1.0, 1.0)); /*< Append point to the interior ring of the first polygon. >*/
    bg::append(mpoly1[0].inners()[0], point_t(4.0, 1.0));
    bg::append(mpoly1[0].inners()[0], point_t(4.0, 4.0));
    bg::append(mpoly1[0].inners()[0], point_t(1.0, 4.0));
    bg::append(mpoly1[0].inners()[0], point_t(1.0, 1.0));

    bg::append(mpoly1[1].outer(), point_t(5.0, 5.0)); /*< Append point to the exterior ring of the second polygon. >*/
    bg::append(mpoly1[1].outer(), point_t(5.0, 6.0));
    bg::append(mpoly1[1].outer(), point_t(6.0, 6.0));
    bg::append(mpoly1[1].outer(), point_t(6.0, 5.0));
    bg::append(mpoly1[1].outer(), point_t(5.0, 5.0));

    double a = bg::area(mpoly1);

    std::cout << a << std::endl;

    return 0;
}

//]

//[multi_polygon_output
/*`
Output:
[pre
17
]
*/
//]


#ifndef InitCondLV_CC_
#define InitCondLV_CC_
/**
 * @file initcondlv.cc
 * @brief NPDE homework InitCondLV code
 * @author lfilippo, tille, jgacon, dcasati
 * @copyright Developed at ETH Zurich
 */

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

#include "../../../lecturecodes/Ode45/ode45.h"

namespace InitCondLV {

/* Compute the maps Phi(t,y0) and W(t,y0) at final time T.
 * Use initial data given by u0 and v0. */
/* SAM_LISTING_BEGIN_1 */
std::pair<Eigen::Vector2d, Eigen::Matrix2d> PhiAndW(double u0, double v0,
                                                    double T) {
  // Save the values of Phi and W at time T in PaW.first and PaW.second resp.
  std::pair<Eigen::Vector2d, Eigen::Matrix2d> PaW;

#if SOLUTION
  auto f = [](const Eigen::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.;

  // Construct ode solver with r.h.s
  Ode45<Eigen::VectorXd> O(f);
  // Set options
  O.options.rtol = 1e-14;
  O.options.atol = 1e-12;
  // Solve ODE
  auto sol = O.solve(w0, T);
  // Extract needed component
  Eigen::VectorXd wT = sol.back().first;

  PaW.first << wT(0), wT(1);
  PaW.second << wT(2), wT(4), wT(3), wT(5);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return PaW;
}
/* SAM_LISTING_END_1 */

}  // namespace InitCondLV

#endif  // #define InitCondLV_CC_


#include <boost/multiprecision/gmp.hpp>
#include <boost/format.hpp>
#include <iostream>
namespace mp = boost::multiprecision;

typedef mp::mpf_float_1000 MPFloat;

struct MPConstant {
  const char *name {};
  const char *expr {};
  MPFloat value;
};

int main() {
  const MPFloat one(1);
  const MPFloat two(2);
  const MPFloat ten(10);
  const MPFloat e = mp::exp(one);
  const MPFloat ln2 = mp::log(two);
  const MPFloat ln10 = mp::log(ten);
  const MPFloat pi = 4 * mp::atan(one);
  const MPFloat sqrtpi = mp::sqrt(pi);
  const MPFloat sqrt2 = mp::sqrt(two);

  const MPConstant constants[] = {
    {"E", "e", e},
    {"LOG2E", "log2(e)", mp::log2(e)},
    {"LOG10E", "log10(e)", mp::log10(e)},
    {"LN2", "log(2)", mp::log(two)},
    {"LN10", "log(10)", mp::log(ten)},
    {"PI", "pi", pi},
    {"2PI", "2*pi", 2 * pi},
    {"4PI", "4*pi", 4 * pi},
    {"PI_2", "pi/2", pi / 2},
    {"PI_4", "pi/4", pi / 4},
    {"1_PI", "1/pi", 1 / pi},
    {"2_PI", "2/pi", 2 / pi},
    {"4_PI", "4/pi", 4 / pi},
    {"1_SQRTPI", "1/sqrt(pi)", 1 / sqrtpi},
    {"2_SQRTPI", "2/sqrt(pi)", 2 / sqrtpi},
    {"SQRT2", "sqrt(2)", sqrt2},
    {"1_SQRT2", "1/sqrt(2)", 1 / sqrt2},
  };

  for (const MPConstant &c: constants) {
    std::cout << "#define " <<
        boost::format("K_%s%|15t|::coreutil::math_constant(%s)%|85t|// %s\n")
        % c.name % (c.value.str(36) + 'L') % c.expr;
  }

  return 0;
}


#include "project/LinearRegression.hpp"
#include <armadillo>

LinearRegression::LinearRegression(arma::mat &x,
                                   arma::vec &y){

    X = x;
    Y = y;
}

void LinearRegression::fit(bool fit_inter = true){
    fit_intercept = fit_inter;
    if(fit_intercept){
        coef = arma::solve(
                arma::join_rows(arma::ones(X.n_rows, 1), X),
                Y);
    } else {
        coef = arma::solve(X, Y);
    }
}

arma::colvec LinearRegression::predict(arma::mat &X_pred){
    if(fit_intercept) {
        return arma::join_rows(
                arma::ones(X_pred.n_rows, 1),
                X_pred)*coef;
    }
    else{
        return X_pred*coef;
    }
}


#pragma once

// C++ standard library
#include <utility>
#include <vector>

// Armadillo
#include <armadillo>

namespace mant {
  arma::uword factorial(
      const arma::uword n);

  arma::uword nchoosek(
      arma::uword n,
      const arma::uword k);

  arma::uword secondStirlingNumber(
      const arma::uword numberOfElements,
      const arma::uword numberOfParts);

  std::vector<arma::uvec> combinations(
      const arma::uword numberOfElements,
      const arma::uword cycleSize);

  std::vector<arma::uvec> multicombinations(
      const arma::uword numberOfElements,
      const arma::uword cycleSize);

  std::vector<std::pair<arma::uvec, arma::uvec>> twoSetsPartitions(
      const arma::uword numberOfElements);
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "ResizeBilinear.hpp"

#include "TensorBufferArrayView.hpp"

#include <boost/numeric/conversion/cast.hpp>

#include <cmath>
#include <algorithm>

using namespace armnnUtils;

namespace armnn
{

namespace
{

inline float Lerp(float a, float b, float w)
{
    return w * b + (1.f - w) * a;
}

}

void ResizeBilinear(const float*      in,
                    const TensorInfo& inputInfo,
                    float*            out,
                    const TensorInfo& outputInfo,
                    DataLayoutIndexed dataLayout)
{
    // We follow the definition of TensorFlow and AndroidNN: the top-left corner of a texel in the output
    // image is projected into the input image to figure out the interpolants and weights. Note that this
    // will yield different results than if projecting the centre of output texels.

    const unsigned int batchSize = inputInfo.GetShape()[0];
    const unsigned int channelCount = inputInfo.GetShape()[dataLayout.GetChannelsIndex()];

    const unsigned int inputHeight = inputInfo.GetShape()[dataLayout.GetHeightIndex()];
    const unsigned int inputWidth = inputInfo.GetShape()[dataLayout.GetWidthIndex()];
    const unsigned int outputHeight = outputInfo.GetShape()[dataLayout.GetHeightIndex()];
    const unsigned int outputWidth = outputInfo.GetShape()[dataLayout.GetWidthIndex()];

    // How much to scale pixel coordinates in the output image, to get the corresponding pixel coordinates
    // in the input image.
    const float scaleY = boost::numeric_cast<float>(inputHeight) / boost::numeric_cast<float>(outputHeight);
    const float scaleX = boost::numeric_cast<float>(inputWidth) / boost::numeric_cast<float>(outputWidth);

    TensorBufferArrayView<const float> input(inputInfo.GetShape(), in, dataLayout);
    TensorBufferArrayView<float> output(outputInfo.GetShape(), out, dataLayout);

    for (unsigned int n = 0; n < batchSize; ++n)
    {
        for (unsigned int c = 0; c < channelCount; ++c)
        {
            for (unsigned int y = 0; y < outputHeight; ++y)
            {
                // Corresponding real-valued height coordinate in input image.
                const float iy = boost::numeric_cast<float>(y) * scaleY;

                // Discrete height coordinate of top-left texel (in the 2x2 texel area used for interpolation).
                const float fiy = floorf(iy);
                const unsigned int y0 = boost::numeric_cast<unsigned int>(fiy);

                // Interpolation weight (range [0,1]).
                const float yw = iy - fiy;

                for (unsigned int x = 0; x < outputWidth; ++x)
                {
                    // Real-valued and discrete width coordinates in input image.
                    const float ix = boost::numeric_cast<float>(x) * scaleX;
                    const float fix = floorf(ix);
                    const unsigned int x0 = boost::numeric_cast<unsigned int>(fix);

                    // Interpolation weight (range [0,1]).
                    const float xw = ix - fix;

                    // Discrete width/height coordinates of texels below and to the right of (x0, y0).
                    const unsigned int x1 = std::min(x0 + 1, inputWidth - 1u);
                    const unsigned int y1 = std::min(y0 + 1, inputHeight - 1u);

                    // Interpolation
                    const float ly0 = Lerp(input.Get(n, c, y0, x0), input.Get(n, c, y0, x1), xw); // lerp along row y0.
                    const float ly1 = Lerp(input.Get(n, c, y1, x0), input.Get(n, c, y1, x1), xw); // lerp along row y1.
                    const float l = Lerp(ly0, ly1, yw);

                    output.Get(n, c, y, x) = l;
                }
            }
        }
    }
}

} //namespace armnn


//
// Copyright (c) 2019-2020 INRIA
//

#include <iostream>

#include <pinocchio/math/rotation.hpp>
#include <pinocchio/math/sincos.hpp>
#include <Eigen/Geometry>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_toRotationMatrix)
{
  using namespace pinocchio;
  const int max_tests = 1e5;
  for(int k = 0; k < max_tests; ++k)
  {
    const double theta = std::rand();
    double cos_value, sin_value;

    pinocchio::SINCOS(theta,&sin_value,&cos_value);
    Eigen::Vector3d axis(Eigen::Vector3d::Random().normalized());

    Eigen::Matrix3d rot; toRotationMatrix(axis,cos_value,sin_value,rot);
    Eigen::Matrix3d rot_ref = Eigen::AngleAxisd(theta,axis).toRotationMatrix();

    BOOST_CHECK(rot.isApprox(rot_ref));
  }
}

BOOST_AUTO_TEST_CASE(test_orthogonal_projection)
{
  using namespace pinocchio;
  const int max_tests = 1e5;

  typedef Eigen::Vector4d Vector4;
  typedef Eigen::Quaterniond Quaternion;
  typedef Eigen::Matrix3d Matrix3;

  for(int k = 0; k < max_tests; ++k)
  {
    const Vector4 vec4 = Vector4::Random();
    const Quaternion quat(vec4.normalized());

    const Matrix3 rot = quat.toRotationMatrix();

    Matrix3 rot_proj = orthogonalProjection(rot);
    BOOST_CHECK(rot.isApprox(rot_proj));
  }

  for(int k = 0; k < max_tests; ++k)
  {
    const Matrix3 mat = Matrix3::Random();

    Matrix3 rot_proj = orthogonalProjection(mat);
    BOOST_CHECK((rot_proj.transpose()*rot_proj).isIdentity());
    BOOST_CHECK(fabs(rot_proj.determinant() - 1.) <= Eigen::NumTraits<double>::dummy_precision());
  }
}

BOOST_AUTO_TEST_SUITE_END()


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

#include "math_unit_test.hpp"
#include <boost/math/tools/centered_continued_fraction.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/core/demangle.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::tools::centered_continued_fraction;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::constants::pi;

template<class Real>
void test_integral()
{
    for (int64_t i = -20; i < 20; ++i) {
        Real ii = i;
        auto cfrac = centered_continued_fraction<Real>(ii);
        auto const & a = cfrac.partial_denominators();
        CHECK_EQUAL(size_t(1), a.size());
        CHECK_EQUAL(i, a.front());
    }
}

template<class Real>
void test_halves()
{
    for (int64_t i = 0; i < 20; ++i) {
        // std::round rounds 0.5 up if i > 0, and rounds down if i < 0.
        // Should we care? These representations are not unique anyway;
        // In any case, this behavior agrees with Maple.
        Real x = i + Real(1)/Real(2);
        auto cfrac = centered_continued_fraction<Real>(x);
        auto const & a = cfrac.partial_denominators();
        CHECK_EQUAL(size_t(2), a.size());
        CHECK_EQUAL(i + 1, a.front());
        CHECK_EQUAL(int64_t(-2), a.back());
    }

    // We'll also test quarters; why not?
    for (int64_t i = -20; i < 20; ++i) {
        Real x = i + Real(1)/Real(4);
        auto cfrac = centered_continued_fraction<Real>(x);
        auto const & a = cfrac.partial_denominators();
        CHECK_EQUAL(size_t(2), a.size());
        CHECK_EQUAL(i, a.front());
        CHECK_EQUAL(int64_t(4), a.back());
    }

    for (int64_t i = -20; i < 20; ++i) {
        Real x = i + Real(1)/Real(8);
        auto cfrac = centered_continued_fraction<Real>(x);
        auto const & a = cfrac.partial_denominators();
        CHECK_EQUAL(size_t(2), a.size());
        CHECK_EQUAL(i, a.front());
        CHECK_EQUAL(int64_t(8), a.back());
    }

    for (int64_t i = -20; i < 20; ++i) {
        Real x = i + Real(3)/Real(4);
        auto cfrac = centered_continued_fraction<Real>(x);
        auto const & a = cfrac.partial_denominators();
        CHECK_EQUAL(size_t(2), a.size());
        CHECK_EQUAL(i+1, a.front());
        CHECK_EQUAL(int64_t(-4), a[1]);
    }

    for (int64_t i = -20; i < 20; ++i) {
        Real x = i + Real(7)/Real(8);
        auto cfrac = centered_continued_fraction<Real>(x);
        auto const & a = cfrac.partial_denominators();
        CHECK_EQUAL(size_t(2), a.size());
        CHECK_EQUAL(i + 1, a.front());
        CHECK_EQUAL(int64_t(-8), a[1]);
    }
}

template<typename Real>
void test_simple()
{
    std::cout << "Testing rational numbers on type " << boost::core::demangle(typeid(Real).name()) << "\n";
    {
        Real x = Real(649)/200;
        // ContinuedFraction[649/200] = [3; 4, 12, 4]
        auto cfrac = centered_continued_fraction(x);
        auto const & a = cfrac.partial_denominators();
        CHECK_EQUAL(size_t(4), a.size());
        CHECK_EQUAL(int64_t(3), a[0]);
        CHECK_EQUAL(int64_t(4), a[1]);
        CHECK_EQUAL(int64_t(12), a[2]);
        CHECK_EQUAL(int64_t(4), a[3]);
    }

    {
        Real x = Real(415)/Real(93);
        // [4; 2, 6, 7]:
        auto cfrac = centered_continued_fraction(x);
        auto const & a = cfrac.partial_denominators();
        CHECK_EQUAL(size_t(4), a.size());
        CHECK_EQUAL(int64_t(4), a[0]);
        CHECK_EQUAL(int64_t(2), a[1]);
        CHECK_EQUAL(int64_t(6), a[2]);
        CHECK_EQUAL(int64_t(7), a[3]);
    }

}

template<typename Real>
void test_khinchin()
{
    using std::sqrt;
    auto rt_cfrac = centered_continued_fraction(sqrt(static_cast<Real>(2)));
    Real K0 = rt_cfrac.khinchin_geometric_mean();
    CHECK_ULP_CLOSE(Real(2), K0, 10);
}

int main()
{
    test_integral<float>();
    test_integral<double>();
    test_integral<long double>();
    test_integral<cpp_bin_float_100>();
    test_halves<float>();
    test_halves<double>();
    test_halves<long double>();
    test_halves<cpp_bin_float_100>();
    test_simple<float>();
    test_simple<double>();
    test_simple<long double>();
    test_simple<cpp_bin_float_100>();
    test_khinchin<cpp_bin_float_100>();
    #ifdef BOOST_HAS_FLOAT128
    test_integral<float128>();
    test_halves<float128>();
    test_simple<float128>();
    test_khinchin<float128>();
    #endif
    return boost::math::test::report_errors();
}


#include <QatGenericFunctions/Variable.h>
#include <QatGenericFunctions/Parameter.h>
#include <QatGenericFunctions/Square.h>
#include <QatGenericFunctions/Sqrt.h>
#include <QatGenericFunctions/Exp.h>
#include <QatGenericFunctions/Sin.h>
#include <QatGenericFunctions/Cos.h>
#include <QatGenericFunctions/VoigtDistribution.h>
#include <QatGenericFunctions/RKIntegrator.h>
#include <QatDataAnalysis/Hist1D.h>
#include <QatDataAnalysis/Table.h>
#include <QatDataModeling/TableLikelihoodFunctional.h>
#include <QatDataModeling/MinuitMinimizer.h>
#include <QatPlotWidgets/PlotView.h>
#include <QatPlotWidgets/MultipleViewWindow.h>
#include <QatPlotting/PlotStream.h>
#include <QatPlotting/PlotHist1D.h>
#include <QatPlotting/PlotFunction1D.h>
#include <QatPlotting/PlotProfile.h>
#include <QatPlotting/PlotKey.h>
#include <QatPlotting/PlotOrbit.h>
#include <Eigen/Dense>
#include <QApplication>
#include <QMainWindow>
#include <QAction>
#include <QToolBar>
#include <QFont>
#include <cmath>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <algorithm>
#include <random>

PlotView *createPlotView(const std::string& title, const std::string& xLabel, const std::string yLabel, const PRectF& viewRange,  const double Width=1200, const double Height=800);

typedef std::vector<double> Data;
typedef std::unique_ptr<Genfun::AbsFunction> FuncPtr;
typedef std::weak_ptr<Genfun::Parameter> ParamPtr;

class PlanetOrbit
{
private:
    Genfun::RKIntegrator integrator;

public:
    Genfun::Parameter* GM;
    Genfun::Parameter* e;
    Genfun::Parameter* a;
    Genfun::Parameter* theta0;
    Genfun::Parameter* inclination;
    Genfun::Parameter* orientation;
    FuncPtr X;
    FuncPtr Y;

    PlanetOrbit()
    {
        Genfun::Variable theta;
        Genfun::Sin qSin;
        Genfun::Cos qCos;
        Genfun::Sqrt qSqrt;

        GM = integrator.createControlParameter("GM", 0.0002959, 0);
        e = integrator.createControlParameter("eccentricity", 0, 0, 1);
        a = integrator.createControlParameter("semimajor axis", 1);
        theta0 = integrator.createControlParameter("Theta0", 0, -2*M_PI, 2*M_PI);
        orientation = new Genfun::Parameter("Orientation", 0, -2*M_PI, 2*M_PI);
        inclination = new Genfun::Parameter("Inclination", 0, 0, M_PI);

        Genfun::GENFUNCTION r = (*a) * (1 - (*e)*(*e)) / (1 + (*e) * qCos(theta - (*theta0)));
        Genfun::GENFUNCTION DTheta = qSqrt((*GM) * (*a) * (1 - (*e)*(*e))) / (r*r);

        integrator.addDiffEquation(&DTheta);

        Genfun::GENFUNCTION Theta = *integrator.getFunction(theta);
        Genfun::GENFUNCTION Radius = (*a) * (1 - (*e) * (*e)) / (1 + (*e) * qCos(Theta - (*theta0)));

        X = FuncPtr((Radius * qCos(Theta - (*theta0) + (*orientation))).clone());
        Y = FuncPtr((Radius * qSin(Theta - (*theta0) + (*orientation))).clone());
    }

    ~PlanetOrbit()
    {
        delete orientation;
    }
};

const double MAX_ANGLE = 3 * M_PI;

double Time2Radian(const double hour, const double minute, const double second)
{
    const double Hour2Radian = 2 * M_PI / 24;
    const double Minute2Radian = Hour2Radian / 60;
    const double Second2Radian = Minute2Radian/ 60;
    double angle = Hour2Radian*std::abs(hour) + Minute2Radian*std::abs(minute) + Second2Radian*std::abs(second);
    return std::signbit(hour) ? -angle : angle;
}

double Degree2Radian(const double degree, const double minute, const double second)
{
    const double Degree2Radian = 2 * M_PI / 360;
    const double Minute2Radian = Degree2Radian / 60;
    const double Second2Radian = Minute2Radian / 60;
    double angle = Degree2Radian*std::abs(degree) + Minute2Radian*std::abs(minute) + Second2Radian*std::abs(second);
    return std::signbit(degree) ? -angle : angle;
}

void readData(const std::string& file, Data& TIME, Data& RA, Data& DEC)
{
    std::ifstream inFile(file);
    std::string day, time;

    for (size_t i = 1; inFile >> day >> time; i++)
    {
        TIME.push_back(i);

        double hour, minute, second;
        inFile >> hour >> minute >> second;
        RA.push_back(Time2Radian(hour, minute, second));
        inFile >> hour >> minute >> second;
        DEC.push_back(Degree2Radian(hour, minute, second));
    }
}

Data refineData(const Data& rawData, const double lowerBound, const double upperBound);
Hist1D binData(const Data& data);
Table tableData(const Data& data);

const std::string dataFileName = "Mars01.dat";
const bool verbose = true;

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

    QToolBar *toolBar = window.addToolBar("Tools");
    QAction *quitAction = toolBar->addAction("Quit (q)");
    quitAction->setShortcut(QKeySequence("q"));
	QObject::connect(quitAction, SIGNAL(triggered()), &app, SLOT(quit()));

    PlanetOrbit Earth;
    Earth.e->setValue(0.8);
    Earth.theta0->setValue(M_PI/3);
    Earth.orientation->setValue(-M_PI/3);
    PlotOrbit earthOrbit(*(Earth.X), *(Earth.Y), 0, 360);
    PlotView *earthView = createPlotView("Earth", "X", "Y", PRectF(-2, 2, -2, 2), 800, 800);
    earthView->add(&earthOrbit);
    window.add(earthView, "Earth");

    Data TIME, RA, DEC;
    readData(dataFileName, TIME, RA, DEC);

    std::cout << "RA @ 0: " << RA[0] << std::endl;
    std::cout << "DEC @ 0: " << DEC[0] << std::endl;

    PlotView *RAView = createPlotView("Right Ascension", "Days", "", PRectF(-10, 760, -0.5, 2*M_PI + 0.5));
    window.add(RAView, "RA");

    PlotProfile RAProfile;
    for (size_t i = 0; i < TIME.size(); i++)
    {
        RAProfile.addPoint(TIME[i],RA[i]);
    }
    {
        PlotProfile::Properties prop;
        prop.pen.setWidth(1);
        RAProfile.setProperties(prop);
    }
    RAView->add(&RAProfile);

    PlotView *DECView = createPlotView("Declination", "Days", "", PRectF(-10, 760, -1, 1));
    window.add(DECView, "DEC");

    PlotProfile DECProfile;
    for (size_t i = 0; i < TIME.size(); i++)
    {
        DECProfile.addPoint(TIME[i], DEC[i]);
    }
    {
        PlotProfile::Properties prop;
        prop.pen.setWidth(1);
        DECProfile.setProperties(prop);
    }
    DECView->add(&DECProfile);

	window.show();
	app.exec();

    return 0;
}

PlotView *createPlotView(const std::string& title, const std::string& xLabel, const std::string yLabel, const PRectF& viewRange, const double Width, const double Height)
{
    PlotView *view_ptr = new PlotView(viewRange);
    view_ptr->setFixedWidth(Width);
    view_ptr->setFixedHeight(Height);

    PlotStream titleStream(view_ptr->titleTextEdit());
    titleStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << title
                << PlotStream::EndP();

    PlotStream xLabelStream(view_ptr->xLabelTextEdit());
    xLabelStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << xLabel
                << PlotStream::EndP();

    PlotStream yLabelStream(view_ptr->yLabelTextEdit());
    yLabelStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << yLabel
                << PlotStream::EndP();

    return view_ptr;
}


// Implementing the class that is defined in the header file: PerpetualAmericanOption.hpp
//
// (c) Sudhansh Dua

#include "PerpetualAmericanOption.hpp"
#include <string>
#include <boost/math/distributions.hpp>
#include <cmath>
#include <vector>

using namespace std;
using namespace boost::math;

double PerpetualAmericanOption::CallPrice() const
{
	return ::PerpetualCall(S, K, r, sig, b);
}

double PerpetualAmericanOption::PutPrice() const
{
	return ::PerpetualPut(S, K, r, sig, b);
}

void PerpetualAmericanOption::init()		// Initialising all the default values
{
	//	Default values
	r = 0.03;
	sig = 0.2;
	K = 105;
	S = 100;				//	Default stock price
	b = r;					//	Black - Scholes(1973) stock option model : b = r

	type = "C";				//	Call option as the default
}

void PerpetualAmericanOption::copy(const PerpetualAmericanOption& option)
{
	r = option.r;
	sig = option.sig;
	K = option.K;
	b = option.b;
	type = option.type;
	S = option.S;
}

//	Constructors and destructor
//	Default Constructor
PerpetualAmericanOption::PerpetualAmericanOption() : Option()
{
	init();
}

//	Copy constructor
PerpetualAmericanOption::PerpetualAmericanOption(const PerpetualAmericanOption& option) : Option(option)
{
	copy(option);
}

//	Constructor that accepts values
PerpetualAmericanOption::PerpetualAmericanOption(const double& S1, const double& K1, const double& r1,
	const double& sig1, const double& b1, const string type1) : Option(), S(S1), K(K1), r(r1), sig(sig1), b(b1), type(type1) {}

//	Destructor
PerpetualAmericanOption::~PerpetualAmericanOption() {}

//	Assignment Operator
PerpetualAmericanOption& PerpetualAmericanOption::operator = (const PerpetualAmericanOption& option)
{
	if (this == &option)
	{
		return *this;		//	Self-assignment check!
	}
	Option::operator = (option);
	copy(option);
	return *this;
}

// Functions that calculate the option price
double PerpetualAmericanOption::Price() const
{
	if (type == "C")
	{
		return CallPrice();
	}
	else
	{
		return PutPrice();
	}
}

// Modifier functions
void PerpetualAmericanOption::toggle()				//	Change the option type
{
	type = ((type == "C") ? "P" : "C");
}

// Global functions
double PerpetualCall(const double S, const double K, const double r, const double sig, const double b)
{
	double y1 = 0.5 - (b / (sig * sig)) + sqrt(pow(((b / (sig * sig)) - 0.5), 2) + (2 * r / (sig * sig)));

	if (y1 == 1.0)
	{
		return S;
	}
	double C = (K / (y1 - 1)) * pow(((y1 - 1) / y1) * (S / K), y1);

	return C;
}

double PerpetualPut(const double S, const double K, const double r, const double sig, const double b)
{
	double y2 = 0.5 - (b / (sig * sig)) - sqrt(pow(((b / (sig * sig)) - 0.5), 2) + (2 * r / (sig * sig)));

	if (y2 == 1.0)
	{
		return S;
	}
	double P = (K / (1 - y2)) * pow(((y2 - 1) / y2) * (S / K), y2);

	return P;
}


#include "homography/compute_homography_model.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/IterativeLinearSolvers>

#include "homography/compute_homography_error.hpp"

using namespace Eigen;
using namespace cv;

HomographyModel ComputeHomographyModel::operator()(vector<HomographyMeasurement> m) {
  MatrixXd A = MatrixXd::Zero(2*m.size(), 8);
  VectorXd b = VectorXd::Zero(2*m.size());

  // normalize the points
  double m_x=0, m_y=0, d=0;
  double m_xp=0, m_yp=0, dp=0;

  // calculate translation
  for(size_t i = 0; i < m.size(); i++) {
    HomographyMeasurement& measurement = m[i];
    float &x = measurement.src.x, &y = measurement.src.y, &xp = measurement.dst.x, &yp = measurement.dst.y;
    m_x += x;
    m_y += y;
    m_xp += xp;
    m_yp += yp;
  }
  m_x /= m.size(), m_y /= m.size(), m_xp /= m.size(), m_yp /= m.size();

  // calculate distance
  for(size_t i = 0; i < m.size(); i++) {
    HomographyMeasurement& measurement = m[i];
    float &x = measurement.src.x, &y = measurement.src.y, &xp = measurement.dst.x, &yp = measurement.dst.y;
    d += sqrt((x-m_x)*(x-m_x) + (y-m_y)*(y-m_y));
    dp += sqrt((xp-m_xp)*(xp-m_xp) + (yp-m_yp)*(yp-m_yp));
  }
  d /= sqrt(2)*m.size(), dp /= sqrt(2)*m.size();

  // transform points
  for(size_t i = 0; i < m.size(); i++) {
    HomographyMeasurement& measurement = m[i];
    float &x = measurement.src.x, &y = measurement.src.y, &xp = measurement.dst.x, &yp = measurement.dst.y;
    x = (x-m_x)/d;
    y = (y-m_y)/d;
    xp = (xp-m_xp)/dp;
    yp = (yp-m_yp)/dp;
  }

  Matrix3d T = Matrix3d::Zero();
  Matrix3d Tpi = Matrix3d::Zero();
  Matrix3d S = Matrix3d::Zero();
  Matrix3d Spi = Matrix3d::Zero();

  T(0,2) = -m_x;
  T(1,2) = -m_y;
  T(0,0) = T(1,1) = T(2,2) = 1;
  S(0,0) = S(1,1) = 1/d;
  S(2,2) = 1;

  Tpi(0,2) = +m_xp;
  Tpi(1,2) = +m_yp;
  Tpi(0,0) = Tpi(1,1) = Tpi(2,2) = 1;
  Spi(0,0) = Spi(1,1) = dp;
  Spi(2,2) = 1;

  for(size_t i = 0; i < m.size(); i++) {
    const HomographyMeasurement& measurement = m[i];
    double x = measurement.src.x, y = measurement.src.y, xp = measurement.dst.x, yp = measurement.dst.y;

    A(2*i,0) = x;
    A(2*i,1) = y;
    A(2*i,2) = 1;
    A(2*i,6) = -x*xp;
    A(2*i,7) = -y*xp;
    // A(2*i,8) = -xp;

    A(2*i+1,3) = x;
    A(2*i+1,4) = y;
    A(2*i+1,5) = 1;
    A(2*i+1,6) = -x*yp;
    A(2*i+1,7) = -y*yp;
    // A(2*i+1,8) = -yp;

    b(2*i) = xp;
    b(2*i+1) = yp;
  }
  FullPivHouseholderQR<MatrixXd> solver(A);
  auto h = solver.solve(b);

  Matrix3d H;

  H(0,0) = h(0);
  H(0,1) = h(1);
  H(0,2) = h(2);
  H(1,0) = h(3);
  H(1,1) = h(4);
  H(1,2) = h(5);
  H(2,0) = h(6);
  H(2,1) = h(7);
  H(2,2) = 1;

  H = Tpi*Spi*H*S*T;

  return HomographyModel(H/H(2,2));
};


#pragma once
#ifndef PARAMETERIZATION_HPP
#define PARAMETERIZATION_HPP
#include <math.h>
#include <vector>
#include <set>
#include <iostream>
#include <list>
#include <string>
#include <map>
#include <utility>
#include <sstream>

#include <Eigen/Core>
#include <Eigen/Sparse>

#include "./lib/Mesh_Library/Mesh/mesh.h"
#include "./lib/Mesh_Library/Mesh/iterators.h"
#include "./lib/mesh_library/Geometry/Point.h"
#include "./lib/mesh_library/Geometry/Point2.H"
#define M_PI 3.14159
using std::vector;
using std::set;
using namespace MeshLib;
using namespace Eigen;

//Computes the harmonic weight for the edge.
float HarmonicWeight(CEdge* e) {
	// length of edge shared by the two triangles.
	float c = e->GetLength();
	CHalfEdge* he = e->halfedge(0);

	auto a1_p1 = he->he_next()->vertex()->point();
	auto a1_p2 = he->he_next()->he_next()->vertex()->point();
	float a1 = CPoint::distance(a1_p1, a1_p2);

	auto b1_p1 = he->he_next()->he_next()->vertex()->point();
	auto b1_p2 = he->he_next()->he_next()->he_next()->vertex()->point();
	float b1 = CPoint::distance(b1_p1, b1_p2);

	he = e->halfedge(1);

	auto a2_p1 = he->he_next()->vertex()->point();
	auto a2_p2 = he->he_next()->he_next()->vertex()->point();
	float a2 = CPoint::distance(a2_p1, a2_p2);

	auto b2_p1 = he->he_next()->he_next()->vertex()->point();
	auto b2_p2 = he->he_next()->he_next()->he_next()->vertex()->point();
	float b2 = CPoint::distance(b2_p1, b2_p2);

	float cos_u = (b1*b1 + a1*a1 - c*c) / (2.0 * b1 * a1);
	float cos_v = (b2*b2 + a2*a2 - c*c) / (2.0 *b2 *a2);

	float sin_u = sqrt(abs(1.0 - cos_u*cos_u));
	float sin_v = sqrt(abs(1.0 - cos_v*cos_v));

	float weight;
	weight = ((cos_u / sin_u) + (cos_v / sin_v)) * 0.5;

	return weight;

}

void uvMap(CMesh* mesh, std::vector<float>* outUvs) {

	std::list<CVertex*> mvs = mesh->vertices();
	std::list<CFace*> mfs = mesh->faces();
	std::list<CEdge*> mes = mesh->edges();
	std::list<CHalfEdge*> mhes = mesh->halfedges();

	std::vector<CVertex*> verts{ std::begin(mvs), std::end(mvs)};
	std::vector<CFace*> faces {std::begin(mfs), std::end(mfs)};
	std::vector<CEdge*> edges { std::begin(mes), std::end(mes)};

	const int N = verts.size();

	std::list<CHalfEdge*>::iterator firstBoundary = mesh->EndHalfEdges();
	std::list<CHalfEdge*>::iterator currentBoundary = mesh->EndHalfEdges();

	for(std::list<CHalfEdge*>::iterator hiter = mesh->halfedges().begin(); hiter != mesh->halfedges().end(); hiter++ )
	{
		CHalfEdge* item = *hiter;
		if(mesh->isBoundary(hiter)) {
			firstBoundary = hiter;
			break;
		}
	}

	std::map<int, CVertex*> vertexMap;

	for (std::vector<CVertex*>::iterator it = verts.begin(); it != verts.end(); ++it) {
		CVertex* item = *it;
		item->set_index(vertexMap.size());
		vertexMap.insert(std::make_pair(vertexMap.size(), item));

	}

	std::vector<CHalfEdge*> boundaryEdges;
	std::vector<CVertex*> boundaryVertices;
	set<int> boundarySet;
	vector<float> edgeLengths;
	float totalEdgeLength = 0.0;
	std::list<CHalfEdge*>::iterator previousBoundary = firstBoundary;
	currentBoundary = firstBoundary;
	int testi = 0;

	CVertex* prevItem;
	for (std::vector<CVertex*>::iterator it = verts.begin(); it != verts.end(); ++it) {
		CVertex* item = *it;
		if (item->boundary()) {
			if (testi == 0) {
				testi++;
				prevItem = item;
				continue;
			}
			boundaryVertices.push_back(item);
			auto halfedge = item->halfedge();
			boundaryEdges.push_back(halfedge);
			boundarySet.insert(item->index());
			edgeLengths.push_back(totalEdgeLength);
			auto bo_p1 = prevItem->point();
			auto bo_p2 = item->point();
			totalEdgeLength += CPoint::distance(bo_p1, bo_p2);
			testi++;
			prevItem = item;
		}
	}

	Eigen::VectorXd bx(N);
	Eigen::VectorXd by(N);

	for(int i =0; i<N;i++)
	{
		bx[i] = 0.0f;
		by[i] = 0.0f;
	}

	for (int i=0; i < boundaryVertices.size(); i++) {
		CVertex* v = boundaryVertices[i];
		double theta = (edgeLengths[i]/totalEdgeLength)*2.0f * M_PI;
		bx[v->index()] = cos(theta);
		by[v->index()] = sin(theta);
	}

	typedef Eigen::Triplet<double> Triplet;
	typedef Eigen::SparseMatrix<double> SparseMatrix;
	SparseMatrix W(N, N);

	vector<Triplet> triplets;
	vector<double> diag;
	diag.resize(N, 0);

	for (std::vector<CEdge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
		CEdge* item = *it;
		if (mesh->isBoundary(item)) {
			continue;
		}
		CVertex* v0 = item->halfedge(0)->vertex();
		CVertex* v1 = item->halfedge(1)->vertex();

		int i0 = v0->index();
		int i1 = v1->index();

		float weight = HarmonicWeight(item);

		if (boundarySet.count(i0) == 0) {
			triplets.push_back(Triplet(i0, i1, weight));
		}
		if (boundarySet.count(i1) == 0) {
			triplets.push_back(Triplet(i1, i0, weight));
		}

		diag[i0] -= weight;
		diag[i1] -= weight;
	}

	for (int i = 0; i < diag.size(); i++) {
		if(boundarySet.count(i) > 0) {

			triplets.push_back(Triplet(i, i, 1.0));
		}
		else {
			triplets.push_back(Triplet(i, i, diag[i]));
		}
	}
	W.setFromTriplets(triplets.begin(), triplets.end());
	Eigen::SparseLU<SparseMatrix > solver;
	solver.compute(W);
	if (solver.info() != Eigen::Success) {
		printf("ERROR: found no decomposition of sparse matrix");
		assert(true);
	}

	Eigen::VectorXd x(N);
	Eigen::VectorXd y(N);
	x = solver.solve(bx);
	y = solver.solve(by);

	std::vector<CPoint2> uvpoints;
	for (int i = 0; i < N; i++) {
		outUvs->push_back(x[i]);
		outUvs->push_back(y[i]);

		CPoint2 cp(x[i], y[i]);
		uvpoints.push_back(cp);
		verts[i]->set_uv(cp);

		std::ostringstream attrs;
		attrs << verts[i]->string() << " uv=(" << std::to_string(cp[0]) << " " << std::to_string(cp[1]) << ")";
		verts[i]->set_string(attrs.str());
	}

}

void Parameterization(std::string meshfile, std::string outmeshfile) {
	CMesh mesh;
	std::vector<float> outUvs;
	mesh.read_m(meshfile.c_str());
	uvMap(&mesh, &outUvs);
	mesh.write_m(outmeshfile.c_str());

	return;
}

#endif


/**
 * @brief Linear Discriminant Analysis with Eigen
 *
 * Dead simple, blazing fast LDA, \cite friedman2001elements
 *
 * @file lda.hpp
 * @author François-David Collin <Francois-David.Collin@umontpellier.fr>
 * @date 2018-08-31
 */
#pragma once

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

using namespace Eigen;
using namespace std;

typedef Matrix<size_t, Dynamic, 1> VectorXs;

/**
 * @brief Computes lda with Trevor/hastie algorithm
 *
 * @param x data
 * @param y labels from 0 to K
 * @param Ld computes matrix whoses columns are Ld vectors
 */
template<class Derived>
std::vector<int> lda(const MatrixBase<Derived> &x,
         const VectorXs &y,
         MatrixXd& Ld)
{
    auto n = x.rows();
    auto K = y.maxCoeff() + 1;
    auto p = x.cols();

    // M = Centroids
    MatrixXd M = MatrixXd::Zero(K, p);
    VectorXd m = VectorXd::Zero(p);
    VectorXs d = VectorXs::Zero(K);

    for (auto i = 0; i < n; i++)
    {
        auto c = y[i];
        auto r = x.row(i);
        d[c]++;
        M.row(c) += r;
        m += r;
    }

    for (auto c = 0; c < K; c++)
    {
        M.row(c) /= static_cast<double>(d[c]);
    }

    // m = Global mean
    m /= static_cast<double>(n);

    // D is x centered data and sorted by classes
    MatrixXd D(n, p);

    // W = Within-class covariance matrix
    MatrixXd Wraw = MatrixXd::Zero(p, p);
    size_t slicepos = 0;
    for (auto c = 0; c < K; c++)
    {
        // Dc is a single class subrange of x
        MatrixXd Dc = x.block(slicepos, 0, d[c], p);
        // Centering
        Dc.rowwise() -= M.row(c);
        Wraw += Dc.transpose() * Dc;
        slicepos += d[c];
    }
    Wraw /= static_cast<double>(n - K);
    auto validvars = std::vector<int>();
    for(auto i = 0; i< x.cols(); i++) {
        if (std::abs(Wraw(i,i)) >= 1.0e-8) validvars.push_back(i);
    }
    auto W = Wraw(validvars,validvars);
    // [4,4]((0.265008,0.0927211,0.167514,0.0384014),(0.0927211,0.115388,0.0552435,0.0327102),(0.167514,0.0552435,0.185188,0.0426653),(0.0384014,0.0327102,0.0426653,0.0418816))

    // Calculate pseudo-inverse square root for W with SVD
    // https://math.stackexchange.com/a/1176942
    JacobiSVD<MatrixXd> svd(W,ComputeFullU|ComputeFullV);
    double tolerance = std::numeric_limits<double>::epsilon() * std::max(W.cols(), W.rows()) * svd.singularValues().array().abs().maxCoeff();
    auto W12 = svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse().sqrt(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
    // SelfAdjointEigenSolver<MatrixXd> svd(W);
    // if (svd.info() != Success) {
    //     throw std::runtime_error("LDA's first solver failed");
    // }
    //auto W12 = svd.matrixU() * svd.eigenvalues().array().inverse().sqrt().matrix().asDiagonal() * svd.matrixU().transpose();
//    auto W12 = svd.operatorInverseSqrt();
    MatrixXd Mvalid = M(all,validvars);
    MatrixXd Mstar = Mvalid * W12;
    VectorXd mstar = VectorXd::Zero(validvars.size());
    for (auto c = 0; c < K; c++)
        mstar += Mstar.row(c);
    mstar /= static_cast<double>(K);

    Mstar.rowwise() -= mstar.transpose();
    auto Bstar = Mstar.transpose() * Mstar / static_cast<double>(K - 1);

    // We get the eigenvectors of B* via SVD
    JacobiSVD<MatrixXd> svd2(Bstar,ComputeThinU);
    MatrixXd Vl = W12 * svd2.matrixU();
    Ld = Vl.leftCols(K-1);
    return validvars;
}


// Group E - Excel Visualization
//
// by Scott Sidoli
//
// 6-15-19
//
// NormalDistribution.cpp
//
// Normal distribution functionality.

#ifndef NormalDistribution_hpp
#define NormalDistribution_hpp

#include <iostream>

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

double N(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return cdf(myNormal, x);

}

double n(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return pdf(myNormal, x);

}

#endif


#include <boost/numeric/ublas/matrix.hpp>
#include <iomanip>
#include <iostream>

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

// A custom, more pretty way to print matrices
std::ostream &operator<<(std::ostream &Str, matrix<double> const &v)
{
	for (int i = 0; i < v.size1(); i++)
	{
		Str << ((i == 0) ? "[[" : " [");
		for (int j = 0; j < v.size2(); j++)
		{
			Str << setw(4);
			Str << v(i, j);
			Str << setw(0);
		}
		Str << ((i == v.size1() - 1) ? "]]" : "]");
		if (i != v.size1() - 1)
			Str << endl;
	}
	return Str;
}

int main()
{
	matrix<double> A(2, 2);
	A(0, 0) = 1;
	A(0, 1) = 2;
	A(1, 0) = 3;
	A(1, 1) = 4;

	matrix<double> B(2, 2);
	B(0, 0) = -1;
	B(0, 1) = 0;
	B(1, 0) = 0;
	B(1, 1) = -1;

	matrix<double> C = prod(A, B);

	cout << "Multiplication of " << endl
		 << A << endl;
	cout << "and " << endl
		 << B << endl;
	cout << "is" << endl;
	cout << C << endl;
	return 0;
}


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

// from numpy import random
using Eigen::MatrixXd; // Objeto que me crea una matriz DINAMICA
using namespace std;

int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  cout << m << endl;
}


// This file is part of:
// gnss-sim: A GNSS Signal Simulator
//
// Copyright (c) 2020 Damien Dusha
// SPDX-License-Identifier: MIT
//
// Derived from https://github.com/osqzss/gps-sdr-sim (MIT Licence):
// Copyright (c) 2015-2020 Takuji Ebinuma

#include "geodesy.h"
#include "gpssim.h"
#include "gps_math.h"

#include <Eigen/Core>

#include <cmath>

GeodeticPosition xyz2llh(const Eigen::Vector3d &ecef)
{
	constexpr double a = WGS84_RADIUS;
	constexpr double e = WGS84_ECCENTRICITY;
	constexpr double e2 = e*e;

    constexpr double eps = 1.0e-3;
	if (ecef.norm() < eps)
	{
        // Invalid ECEF vector
        return GeodeticPosition::FromRadians(0.0, 0.0, -a);
	}

	const double x = ecef.x();
	const double y = ecef.y();
	const double z = ecef.z();

	const double rho2 = x*x + y*y;
	double dz = e2*z;

    double n, zdz, nh;
	while (1)
	{
		zdz = z + dz;
		nh = std::sqrt(rho2 + zdz*zdz);
		const double slat = zdz / nh;
		n = a / std::sqrt(1.0-e2*slat*slat);
		const double dz_new = n*e2*slat;

		if (std::fabs(dz-dz_new) < eps)
			break;

		dz = dz_new;
	}

    const double lat_rad = std::atan2(zdz, std::sqrt(rho2));
    const double lon_rad = std::atan2(y, x);
    const double height_m = nh - n;

    return GeodeticPosition::FromRadians(lat_rad, lon_rad, height_m);
}

Eigen::Vector3d llh2xyz(const GeodeticPosition &llh)
{
	constexpr double a = WGS84_RADIUS;
	constexpr double e = WGS84_ECCENTRICITY;
	constexpr double e2 = e*e;

	const double clat = std::cos(llh.latitude_rad());
	const double slat = std::sin(llh.latitude_rad());
	const double clon = std::cos(llh.longitude_rad());
	const double slon = std::sin(llh.longitude_rad());
	const double d = e*slat;

	const double n = a / std::sqrt(1.0-d*d);
	const double nph = n + llh.height_m();

	const double tmp = nph*clat;
	const double x = tmp*clon;
	const double y = tmp*slon;
	const double z = ((1.0-e2)*n + llh.height_m())*slat;

	return Eigen::Vector3d(x, y, z);
}

void ltcmat(const GeodeticPosition &llh, double t[3][3])
{
	const double slat = std::sin(llh.latitude_rad());
	const double clat = std::cos(llh.latitude_rad());
	const double slon = std::sin(llh.longitude_rad());
	const double clon = std::cos(llh.longitude_rad());

	t[0][0] = -slat*clon;
	t[0][1] = -slat*slon;
	t[0][2] = clat;
	t[1][0] = -slon;
	t[1][1] = clon;
	t[1][2] = 0.0;
	t[2][0] = clat*clon;
	t[2][1] = clat*slon;
	t[2][2] = slat;
}

void ecef2neu(const double *xyz, double t[3][3], double *neu)
{
	neu[0] = t[0][0]*xyz[0] + t[0][1]*xyz[1] + t[0][2]*xyz[2];
	neu[1] = t[1][0]*xyz[0] + t[1][1]*xyz[1] + t[1][2]*xyz[2];
	neu[2] = t[2][0]*xyz[0] + t[2][1]*xyz[1] + t[2][2]*xyz[2];

	return;
}

AzimuthElevation neu2azel(const double *neu)
{
	double azimuth_rad = std::atan2(neu[1],neu[0]);
	if (azimuth_rad < 0.0)
		azimuth_rad += 2.0*PI;

	const double ne = std::hypot(neu[0], neu[1]);
	const double elevation_rad = std::atan2(neu[2], ne);

    return AzimuthElevation::FromRadians(azimuth_rad, elevation_rad);
}


#include "nombre_modulaire.h"
#include "numerique.h"

#include <boost/multiprecision/integer.hpp>

std::ostream &operator<<(std::ostream &os, const nombre_modulaire &a) {
    os << a.value() << "[" << a.modulo() << "]";
    return os;
}

std::pair<size_t, nombre_modulaire> nombre_modulaire::factoriel2(size_t modulo, size_t n) {
    nombre_modulaire resultat(modulo, 1);
    size_t exposant = 0;
    bool sgn = false;
    while (n > 0) {
        for (size_t i = n % modulo; i > 1; --i)
            resultat *= i;
        n /= modulo;
        sgn ^= (n % 2 == 1);
        exposant += n;
    }
    return std::make_pair(exposant, sgn ? -resultat : resultat);
}

void nombre_modulaire::meme_mod(const nombre_modulaire &n1, const nombre_modulaire &n2) {
    if (n1._modulo != n2._modulo) {
        throw std::domain_error("Modulo different !");
    }
}

void nombre_modulaire::meme_mod(const nombre_modulaire &n) const {
    meme_mod(*this, n);
}

nombre_modulaire::nombre_modulaire(const nombre_modulaire &n) : _modulo(n._modulo), _value(n._value) {
}

nombre_modulaire nombre_modulaire::puissance(nombre_modulaire a, size_t n) {
    nombre_modulaire result(a._modulo, 1);
    while (n > 0) {
        if (n % 2)
            result *= a;
        a *= a;
        n /= 2;
    }
    return result;
}

nombre_modulaire nombre_modulaire::puissance(size_t a, size_t n, size_t modulo) {
    nombre_modulaire result(modulo, 1);
    nombre_modulaire p(modulo, a);
    while (n > 0) {
        if (n % 2)
            result *= p;
        p *= a;
        n /= 2;
    }
    return result;
}

unsigned long long nombre_modulaire::value() const { return _value; }

size_t nombre_modulaire::modulo() const { return _modulo; }

nombre_modulaire nombre_modulaire::operator-() const {
    return nombre_modulaire(_modulo, _modulo - value());
}

nombre_modulaire &nombre_modulaire::operator=(const nombre_modulaire &op) {
    // meme_mod(op);

    _modulo = op._modulo;
    _value = op._value;
    return *this;
}

nombre_modulaire &nombre_modulaire::operator+=(const nombre_modulaire &op) {
    meme_mod(op);

    _value += op._value;
    if (_value >= _modulo) _value -= _modulo;
    return *this;
}

nombre_modulaire &nombre_modulaire::operator-=(const nombre_modulaire &op) {
    meme_mod(op);

    _value = _value < op._value ? _modulo + _value - op._value : _value - op._value;
    return *this;
}

nombre_modulaire &nombre_modulaire::operator*=(const nombre_modulaire &op) {
    meme_mod(op);

    uint128_t c = 0;
    boost::multiprecision::multiply(c, _value, op._value);
    _value = boost::multiprecision::integer_modulus(c, _modulo);

    return *this;
}

nombre_modulaire &nombre_modulaire::operator/=(const nombre_modulaire &op) {
    meme_mod(op);
    return operator*=(puissance(op, _modulo - 2));
}

nombre_modulaire nombre_modulaire::operator+(const nombre_modulaire &op) const {
    nombre_modulaire result(*this);
    result.operator+=(op);
    return result;
}

nombre_modulaire nombre_modulaire::operator-(const nombre_modulaire &op) const {
    nombre_modulaire result(*this);
    result.operator-=(op);
    return result;
}

nombre_modulaire nombre_modulaire::operator*(const nombre_modulaire &op) const {
    nombre_modulaire result(*this);
    result.operator*=(op);
    return result;
}

nombre_modulaire nombre_modulaire::operator/(const nombre_modulaire &op) const {
    nombre_modulaire result(*this);
    result.operator/=(op);
    return result;
}

nombre_modulaire nombre_modulaire::factoriel(size_t modulo, size_t n) {
    auto[exposant, resultat] = factoriel2(modulo, n);
    if (exposant > 0) {
        return nombre_modulaire(modulo, 0);
    } else {
        return resultat;
    }
}

nombre_modulaire nombre_modulaire::arrangement(size_t modulo, size_t n, size_t k) {
    auto fn = factoriel2(modulo, n);
    auto fnk = factoriel2(modulo, n - k);
    if (fn.first - fnk.first > 0)
        return nombre_modulaire(modulo, 0);
    return fn.second / fnk.second;
}

nombre_modulaire nombre_modulaire::coefficient_binomial(size_t modulo, size_t n, size_t k) {
    auto fn = factoriel2(modulo, n);
    auto fk = factoriel2(modulo, k);
    auto fnk = factoriel2(modulo, n - k);
    if (fn.first - fk.first - fnk.first > 0)
        return nombre_modulaire(modulo, 0);
    return fn.second / (fk.second * fnk.second);
}


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <math.h>
#include <algorithm>    // std::swap

#include "isam_plane3d.h"

namespace isam {

//copied from pop_up_wall  rays is 3*n, each column is a ray staring from origin  plane is (4，1） parameters, compute intersection  output is 3*n
void ray_plane_interact(const Eigen::MatrixXd &rays,const Eigen::Vector4d &plane,Eigen::MatrixXd &intersections)
{
    Eigen::VectorXd frac=-plane[3]/(plane.head(3).transpose()*rays).array();   //n*1
    intersections= frac.transpose().replicate<3,1>().array() * rays.array();
}
// copied from pop_up_wall (update_plane_equation_from_seg_fast), don't want to depend on that. no ground. ground_seg_ray=invK*groundlines  3*2n, precomputed
// output is not quaternion normized
void get_wall_plane_equation(const Eigen::MatrixXd& ground_seg_ray, const Eigen::Matrix4d& transToWorld, Eigen::MatrixXd& all_planes_sensor_out)
{
    if (ground_seg_ray.cols()>0)
    {
	Eigen::Vector4d ground_plane_world(0,0,-1,0);  //plane parameters for ground in world(footprint), treated as column vector
	Eigen::Vector4d ground_plane_sensor = transToWorld.transpose()*ground_plane_world;  // using a new pose to project

	Eigen::MatrixXd ground_seg3d_sensor; // 3*2n
	ray_plane_interact(ground_seg_ray,ground_plane_sensor,ground_seg3d_sensor);

	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> temp;
	temp = ground_seg3d_sensor.transpose();
	temp.resize(ground_seg3d_sensor.cols()/2,6); // n * 6
	ground_seg3d_sensor = temp;

	int num_seg = ground_seg3d_sensor.rows();
	all_planes_sensor_out.resize(num_seg,4);

	for (int seg=0;seg<num_seg;seg++)
	{
	    Eigen::Vector3d partwall_line3d_sensor_bg=ground_seg3d_sensor.row(seg).head(3);
	    Eigen::Vector3d partwall_line3d_sensor_en=ground_seg3d_sensor.row(seg).tail(3);
	    Eigen::Vector3d temp1=partwall_line3d_sensor_en-partwall_line3d_sensor_bg;
	    Eigen::Vector3d temp2=ground_plane_sensor.head(3);
	    Eigen::Vector3d partwall_normal_sensor=temp1.cross(temp2);
	    double dist=-partwall_normal_sensor.transpose()*partwall_line3d_sensor_bg;
	    Eigen::Vector4d partwall_plane_sensor;   partwall_plane_sensor<<partwall_normal_sensor,dist;

	    all_planes_sensor_out.row(seg)=partwall_plane_sensor;
	}
    }
    else
    {
	all_planes_sensor_out.resize(0,4);
    }
}

}


#include <frovedis.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // dot product of two std::vectors'
    std::vector<float> x = {1,2,3,4};
    std::vector<float> y = {1,2,3,4};
    auto d = dot<float> (x,y); // d = x.y
    BOOST_CHECK (d == 30);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> cm (
           make_rowmajor_matrix_local_load<float>("./sample_4x4"));

    // checking dot() operation
    auto row1 = make_row_vector<float> (cm,1);
    auto row2 = make_row_vector<float> (cm,2);

    // checking whether the dot operation successfully taken place
    auto r = dot<float>(row1, row2); // r = row1 . row2
    BOOST_CHECK (r == 2);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.
// 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/extensions/nsphere/nsphere.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/io/wkt/read.hpp>

typedef bg::model::d2::point_xy<double> point_type;
typedef bg::model::box<point_type> box_type;
typedef bg::model::nsphere<point_type, double> circle_type;

template <typename Geometry>
void test_circle(Geometry const& geometry, std::string const& wkt_geometry, bool expected)
{
    circle_type circle(point_type(1.0, 1.0), 3.0);

    bool detected = bg::disjoint(geometry, circle);

    BOOST_CHECK_MESSAGE(detected == expected,
        "disjoint: " << wkt_geometry
        << " in circle (1,1) with radius 3"
        << " -> Expected: " << expected
        << " detected: " << detected);
}

template <typename Geometry>
void test_circle(std::string const& wkt_geometry, bool expected)
{
    Geometry geometry;
    bg::read_wkt(wkt_geometry, geometry);

    test_circle<Geometry>(geometry, wkt_geometry, expected);
}

int test_main( int , char* [] )
{
    test_circle<point_type>("POINT(2 1)", false);
    test_circle<point_type>("POINT(12 1)", true);

    test_circle<box_type>("BOX(2 1, 4 4)", false);
    test_circle<box_type>("BOX(-3 -3, -2 -2)", true);

    test_circle<circle_type>(circle_type(point_type(4, 4), 2), "CIRCLE(4 4, 2)", false);
    test_circle<circle_type>(circle_type(point_type(4, 4), 0.5), "CIRCLE(4 4, 0.5)", true);

    return 0;
}