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

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

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

class DataGenerator
{

public:
    arma::mat m_L;

    DataGenerator()
    {
        m_L = generate_library();
    }

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

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

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

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

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

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

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

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


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

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

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

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

pangolin::Var<EulerAngle>* eulerAngle;

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

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

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

struct QuaternionDraw {
    Quaterniond q;
};

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

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

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

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

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

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

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

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

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

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

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

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

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

        pangolin::glDrawColouredCube();

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

        pangolin::FinishFrame();
    }

    return 0;
}


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

#include "Activation.hpp"

#include <boost/log/trivial.hpp>

#include <cmath>

namespace armnn
{

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

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

        out[i] = output;
    }
}

} //namespace armnn


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, multiply_log) {
  double a = 2.0;
  double b = 3.0;
  EXPECT_FLOAT_EQ(a * log(b), stan::math::multiply_log(a,b));

  a = 0.0;
  b = 0.0;
  EXPECT_FLOAT_EQ(0.0, stan::math::multiply_log(a,b)) <<
    "when a and b are both 0, the result should be 0";

  a = 1.0;
  b = -1.0;
  EXPECT_TRUE(std::isnan(stan::math::multiply_log(a,b))) <<
    "log(b) with b < 0 should result in NaN";
}

TEST(MathFunctions, multiply_log_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::multiply_log(2.0, nan));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::multiply_log(nan, 3.0));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::multiply_log(nan, nan));
}


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

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

using boost::multiprecision::checked_cpp_int;

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

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

int main(void)
{
    using namespace std;

    checked_cpp_int base, exponent;

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


/**
 * \file boost/numeric/ublasx/operation/hild.hpp
 *
 * \brief Hilbert matrix.
 *
 * Copyright (c) 2012, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_HILB_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_HILB_HPP

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

namespace boost { namespace numeric { namespace ublasx {

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

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

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

	return H;
}

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

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_HILB_HPP


#include <armadillo>
#include <cmath>

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

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

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

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

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

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

    return POLAR;
}

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

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

    return acos(argument);
}

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

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

    return RESULT;
}

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

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

    return AMAT;
}

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

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

    return AMAT;
}

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

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

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

    return Quaternion;
}

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

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

    return Matrix_out;
}

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

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

    return Quaternion_out;
}

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

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

    return Quaternion_out;
}

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

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

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

    return Quaternion_out;
}

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

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

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

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

    return Q_out;
}

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

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

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

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

    return V_out;
}

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

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

    return c_matrix;
}

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

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

    return TM;
}

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

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

    return TM;
}

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

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

    return TM;
}

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

    return sign;
}

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

    double mroll = 0;

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

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

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

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

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

    return euler_ang;
}


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

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

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

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

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

  } // End: function potential()

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

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


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

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

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

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

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

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


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

#include "general_nonlinear_solver.h" // implements general_nonlinear_solver

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

}

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

    // Part 2: solve system
    mod_newt_sys();

}

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


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

namespace warp_perspective {

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

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

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

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

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

}


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

#include <iostream>
#include <iomanip>

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

#include <random>
#include <set>

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

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

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

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

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

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

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

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

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

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

}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

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


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

using namespace std;

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

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


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

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

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

using namespace codegenvar;

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

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

    Vec3 r1;

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

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

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

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

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

    return 0;
}


#ifndef CANNON_MATH_INTERP_H
#define CANNON_MATH_INTERP_H

/*!
 * \file cannon/math/interp.hpp
 * \brief File containing utilities for doing function interpolation.
 */

#include <vector>
#include <functional>
#include <memory>

#include <Eigen/Dense>
#include <Eigen/Sparse>

using namespace Eigen;

namespace cannon {
  namespace math {

    /*!
     * \brief Class representing the cubic spline interpolation of a set of
     * points. Following https://timodenk.com/blog/cubic-spline-interpolation/.
     */
    class CubicSpline {
      public:

        enum class BoundaryType {
          FirstDeriv,
          SecondDeriv,
          NotAKnot
        };

        CubicSpline() = delete;

        /*!
         * \brief Constructor taking data to interpolate, boundary type for the
         * spline, and values of boundary conditions. Data should be sorted in
         * increasing order of xs.
         */
        CubicSpline(const std::vector<double>& xs, const std::vector<double>& ys,
                    BoundaryType bd = BoundaryType::SecondDeriv,
                    double bd_first = 0.0, double bd_last = 0.0);
        /*!
         * \brief Constructor taking data to interpolate, boundary type for the
         * spline, and values of boundary conditions. Data should be sorted in
         * increasing order of xs.
         */
        CubicSpline(const std::vector<Vector2d>& pts,
                    BoundaryType bd = BoundaryType::SecondDeriv,
                    double bd_first = 0.0, double bd_last = 0.0);

        /*!
         * \brief Functional operator.
         */
        double operator()(double x) const;

        /*!
         * \brief Get the derivative of this spline of input order at the input
         * point.
         *
         * \param x The point to evaluate curve derivative at.
         * \param order Derivative order to evaluate.
         *
         * \returns The derivative.
         */
        double deriv(double x, unsigned int order = 1) const;

        /*!
         * \brief Get the number of polynomials defining this spline.
         *
         * \returns Number of polynomials.
         */
        inline unsigned int num_polys() const;

      private:
        /*!
         * \brief Compute coefficients for spline from data points and boundary
         * conditions.
         */
        void compute_coeffs_();

        /*!
         * \brief Compute A matrix for system of equations defining cubic
         * spline.
         *
         * \returns A matrix which will be solved to compute spline
         * coefficients.
         */
        SparseMatrix<double> construct_A_mat_() const;

        /*!
         * \brief Compute b vector for system of equations defining cubic
         * spline.
         *
         * \returns b vector which will be used to solve system of equations
         * for spline coefficients.
         */
        VectorXd construct_b_vec_() const;

        /*!
         * \brief Find the closest idx such that xs_[idx] <= x
         *
         * \param x Input value to locate
         *
         * \returns Index of closest x datum
         */
        unsigned int find_closest_x_(double x) const;

        std::vector<double> xs_; //!< X-values of data
        std::vector<double> ys_; //!< Y-values of data

        const BoundaryType bd_; //!< Type of boundary condition for this spline
        const double bd_first_; //!< First boundary condition value
        const double bd_last_; //!< Last boundary condition value

        std::vector<double> a_coeffs_; //!< Coefficients of x^3 for each polynomial
        std::vector<double> b_coeffs_; //!< Coefficients of x^2 for each polynomial
        std::vector<double> c_coeffs_; //!< Coefficients of x for each polynomial
        std::vector<double> d_coeffs_; //!< Constant coefficients for each polynomial

    };

    /*!
     * \brief Class representing a multivariate spline, defined by its
     * component coordinate splines.
     */
    class MultiSpline {
      public:

        MultiSpline() = delete;

        /*!
         * \brief Constructor taking trajectory times and coordinate points, as
         * well as boundary conditions that will be set uniformly for each
         * spline.
         */
        MultiSpline(
            const std::vector<double> &times, const std::vector<VectorXd> &coords,
            CubicSpline::BoundaryType bd = CubicSpline::BoundaryType::SecondDeriv,
            double bd_first = 0.0, double bd_last = 0.0);

        /*!
         * \brief Functor operator taking a time and returning the coordinates
         * of the multispline at that time.
         *
         * \param t Time to evaluate spline at.
         *
         * \returns Value of spline at that time.
         */
        VectorXd operator()(double t) const;

        /*!
         * \brief Compute derivative with respect to time parameterization of
         * this cubic spline.
         *
         * \param t The time to get derivative at.
         * \param order The order of derivative to compute.
         *
         * \returns The derivative.
         */
        VectorXd deriv(double t, unsigned int order = 1) const;

        /*!
         * \brief Compute a normal vector for this spline at the input time.
         *
         * \param t The time to compute a normal for.
         *
         * \returns The computed normal.
         */
        VectorXd normal(double t) const;

      private:
        std::vector<std::shared_ptr<CubicSpline>> coord_splines_; //!< Coordinate spline functions
        unsigned int dim_; //!< Dimension of spline
    };

    /*!
     * \brief Compute the Chebyshev points between the input bounds. See
     * https://en.wikipedia.org/wiki/Chebyshev_nodes
     *
     * \param num Number of points to generate
     * \param low Lower limit for points
     * \param high Upper limit for points
     *
     * \returns A vector containing the Chebyshev points.
     */
    std::vector<double> cheb_points(unsigned int num, double low=-1.0, double high=1.0);

    /*!
     * \brief Compute the Lagrange interpolant for the input function on the
     * input points.
     *
     * \param f The function to interpolate.
     * \param pts The points to interpolate on.
     *
     * \returns The Lagrange interpolating polynomial on the input points.
     */
    std::function<double(double)>
    lagrange_interp(std::function<double(double)> f, const std::vector<double>& pts);

    /*!
     * \brief Interpolate between the two input vectors.
     *
     * \param a First vector
     * \param b Second vector
     * \param t Interpolation amount. Should be between 0 and 1
     *
     * \returns Interpolated vector.
     */
    VectorXd lerp(const Ref<const VectorXd> &a, const Ref<const VectorXd> &b,
                  double t);

    /*!
     * \brief Compute the projection of the first vector onto the second.
     *
     * \param a The vector to be projected
     * \param b The vector that projection is computed with respect to.
     *
     * \returns The projected vector.
     */
    VectorXd projection(const Ref<const VectorXd> &a,
                        const Ref<const VectorXd> &b);

    /*!
     * \brief Compute the complementary projection of the first vector with
     * respect to the second.
     *
     * \param a The vector to be projected
     * \param b The vector that projection is computed with respect to.
     *
     * \returns The complementary projection.
     */
    VectorXd complementary_projection(const Ref<const VectorXd> &a,
                                      const Ref<const VectorXd> &b);
  }
}

#endif /* ifndef CANNON_MATH_INTERP_H */


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2018, Oracle and/or its affiliates
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

//[densify_strategy
//` Shows how to densify a linestring in geographic coordinate system

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/linestring.hpp>

int main()
{
    namespace bg = boost::geometry;
    typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > point_type;
    typedef bg::model::linestring<point_type> linestring_type;

    linestring_type ls;
    bg::read_wkt("LINESTRING(0 0,1 1)", ls);

    linestring_type res;

    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);
    bg::strategy::densify::geographic<> strategy(spheroid);

    boost::geometry::densify(ls, res, 50000.0, strategy);

    std::cout << "densified: " << boost::geometry::wkt(res) << std::endl;

    return 0;
}

//]

//[densify_strategy_output
/*`
Output:
[pre
densified: LINESTRING(0 0,0.249972 0.250011,0.499954 0.500017,0.749954 0.750013,1 1)
]
*/
//]


#include "ReferenceModel.hpp"
#include <Eigen/Dense>
#include <stdexcept>
#include <ros/ros.h>
#include <ros/console.h>

using namespace Eigen;
using namespace std;

ReferenceModel::ReferenceModel(void)
{
    this->dt = 0.0;
    this->nStates = 0;
    this->nInputs = 0;
    this->nOutputs = 0;
}
ReferenceModel::~ReferenceModel(void)
{

}

void ReferenceModel::initialize(const ReferenceModel_Config_t _config)
{
    // Safety checks
    if (_config.A.cols() != _config.A.rows())
        throw invalid_argument("A is not a squre matrix!");
    if (_config.A.rows() != _config.B.rows())
        throw invalid_argument("A and B have different # of rows!");

    this->dt = _config.dt;
    this->Aref = _config.A;
    this->Bref = _config.B;
    this->nStates = this->Aref.cols();
    this->nInputs = this->Bref.cols();
    this->nOutputs = this->nStates; // full states output
    this->Cref = MatrixXd::Identity(this->nOutputs, this->nOutputs);

    // Safety checks
    if(_config.states0.size() != this->nStates)
        throw invalid_argument("Unexpected initial states size!");

    this->states0 = VectorXd::Zero(this->nStates);
    for(int i=0; i<(this->nStates); i++) {
        //ROS_INFO("initialize: %.4f\n", _config.states0(i));
        this->states0(i) = _config.states0(i);
    }
    reset();
}

void ReferenceModel::reset()
{
    this->states = VectorXd::Zero(this->nStates);
    for(int i=0; i<(this->nStates); i++) {

        this->states(i) = this->states0(i);
        //ROS_INFO("Reset: %.4f, ", this->states(i));
    }
    //ROS_INFO("\n");
}

void ReferenceModel::update(const VectorXd _inputs)
{
    // Safety check
    if(_inputs.size() != this->nInputs)
        throw invalid_argument("Unexpected inputs size!");
    //ROS_INFO("update: %.4f", _inputs(0));

    // Euler integration
    VectorXd states_dot = this->Aref * this->states + this->Bref * _inputs;
    this->states += states_dot * this->dt;
}

void ReferenceModel::get_outputs(VectorXd &_outputs)
{
    _outputs = this->Cref * this->states;
}

void ReferenceModel::get_states(VectorXd &_states)
{
    _states = VectorXd::Zero(this->nStates);
    for (int i=0; i<(this->nStates); i++) {
        _states(i) = this->states(i);
    }
    //ROS_INFO("get_states A: %.4f, %.4f", _states(0), _states(1));
    //ROS_INFO("get_states B: %.4f, %.4f", this->states(0), this->states(1));
}


/**
 * @author Eric Cousineau <eacousineau@gmail.com>, member of Dr. Aaron
 * Ames's AMBER Lab
 */
#ifndef EIGEN_UTILITIES_INTERP_UTILITIES_H
    #define EIGEN_UTILITIES_INTERP_UTILITIES_H

#include <Eigen/Dense>

#include <vector>
#include <eigen_utilities/assert_size.hpp>

namespace eigen_utilities
{

template<typename T, typename Container>
T find_lower_index(const Container &data, const T &value, bool do_saturate = false)
{
    // Assume sorted
    if (data.size() == 0)
        return -1;
    else
    {
        int index = 0;
        int last = data.size() - 1;
        if (do_saturate)
        {
            const T &min = data[0];
            const T &max = data[last];
            if (value < min)
                return -2;
            if (value > max)
                return -3; // Return this as upper
        }
        while (index + 1 < data.size())
        {
            const T &lower = data[index];
            const T &upper = data[index + 1];
            if (value >= lower && value <= upper)
                return index;
            else
                index += 1;
        }
        return -1;
    }
}

template<typename ZType>
class BilinearInterp
{
public:
    Eigen::VectorXd xs;
    Eigen::VectorXd ys;
    std::vector<std::vector<ZType> > zs;
private:
    ZType q11;
    ZType q21;
    ZType q12;
    ZType q22;
    ZType r1;
    ZType r2;
    ZType z;
public:
    void check()
    {
        common_assert(xs.size() > 0);
        common_assert(ys.size() > 0);
        common_assert_msg(zs.size() == xs.size(), "Bad tensor x-size: " << zs.size() << " != " << xs.size());
        for (int i = 0; i < zs.size(); ++i)
            common_assert_msg(zs[i].size() == ys.size(), "Bad tensor y-size at [" << i << "]: " << zs[i].size() << " != " << ys.size());
    }

    inline const ZType& eval(const Eigen::VectorXd &v, bool do_saturate = false)
    {
        assert_size_vector(v, 2);
        return eval(v(0), v(1), do_saturate);
    }

    inline const ZType& eval(double x, double y, bool do_saturate = false)
    {
//        common_assert(xs.size() > 0);
//        common_assert(ys.size() > 0);
//        assert_size_matrix(zs, xs.size(), ys.size());
        // http://en.wikipedia.org/wiki/Bilinear_interpolation
        if (do_saturate)
        {
            double xmin = xs(0);
            double xmax = xs(xs.size() - 1);
            x = std::min(std::max(x, xmin), xmax);
            double ymin = ys(0);
            double ymax = ys(ys.size() - 1);
            y = std::min(std::max(y, ymin), ymax);
        }
        int x_index = find_lower_index(xs, x);
        int y_index = find_lower_index(ys, y);
        if (x_index == -1 || y_index == -1)
            throw std::runtime_error("Bilinear interpolation values out of range");
        double xl = xs(x_index);
        double xu = xs(x_index + 1);
        double yl = ys(y_index);
        double yu = ys(y_index + 1);
        q11 = zs[x_index][y_index];
        q21 = zs[x_index + 1][y_index];
        q12 = zs[x_index][y_index + 1];
        q22 = zs[x_index + 1][y_index + 1];
        r1 = (xu - x) / (xu - xl) * q11 + (x - xl) / (xu - xl) * q21;
        r2 = (xu - x) / (xu - xl) * q12 + (x - xl) / (xu - xl) * q22;
        z = (yu - y) / (yu - yl) * r1 + (y - yl) / (yu - yl) * r2;
        return z;
    }
};

//class MatrixBilinearInterp
//{
//public:
//    Eigen::VectorXd xs;
//    Eigen::VectorXd ys;
//    std::vector<std::vector<Eigen::MatrixXd> > zs;
//private:
//    Eigen::MatrixXd Z;
//    std::vector<std::vector<BilinearInterp> > interps;
//public:
//    void checkAndInit()
//    {
//        common_assert(xs.size() > 0);
//        common_assert(ys.size() > 0);
//        common_assert_msg(zs.size() == xs.size(), "Bad tensor x-size: " << zs.size() << " != " << xs.size());
//        int rows = -1;
//        int cols = -1;
//        for (int i = 0; i < zs.size(); ++i)
//        {
//            common_assert_msg(zs[i].size() == ys.size(), "Bad tensor y-size at [" << i << "]: " << zs[i].size() << " != " << ys.size());
//            for (int j = 0; j < zs[i].size(); ++j)
//            {
//                if (rows == -1 && cols == -1)
//                    assert_size_matrix(zs[i][j], rows, cols);
//                else
//                {
//                    rows = zs[i][j].rows();
//                    cols = zs[i][j].cols();
//                    Z.resize(rows, cols);
//                }
//            }
//        }
//        std::vector<std::vector<BilinearInterp> > interp_row;
//        Eigen::MatrixXd blank(xs.size(), ys.size());
//        interp_row.assign(cols, BilinearInterp(xs, ys, blank));
//        interps.assign(rows, interp_row);
//        for (int ix = 0; ix < xs.size(); ++ix)
//        {
//            for (int iy = 0; iy < ys.size(); ++iy)
//            {
//                for (int ir = 0; ir < rows; ++ ir)
//                {
//                    for (int ij = 0; ij < cols; ++ ij)
//                    {
//                        interps[ir][ij].zs(ix, iy) = zs[ix][iy](ir, ij);
//                    }
//                }
//            }
//        }
//    }
//};

}

#endif // EIGEN_UTILITIES_INTERP_UTILITIES_H


#include <iostream>
#include <fstream>
#include <random>
#include <Eigen/Dense>
#include <functional>

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

using std::default_random_engine;
using std::normal_distribution;
using std::bind;

// start random number engine with fixed seed
default_random_engine re{12345};

normal_distribution<double> norm(5,2); // mean and standard deviation
auto rnorm = bind(norm, re);

int main()
{
    using namespace Eigen;

    VectorXd x1(6);
    x1 << 1, 2, 3, 4, 5, 6;
    VectorXd x2 = VectorXd::LinSpaced(6, 1, 2);
    VectorXd x3 = VectorXd::Zero(6);
    VectorXd x4 = VectorXd::Ones(6);
    VectorXd x5 = VectorXd::Constant(6, 3);
    VectorXd x6 = VectorXd::Random(6);

    double data[] = {6,5,4,3,2,1};
    Map<VectorXd> x7(data, 6);

    VectorXd x8 = x6 + x7;

    MatrixXd A1(3,3);
    A1 << 1 ,2, 3,
          4, 5, 6,
          7, 8, 9;
    MatrixXd A2 = MatrixXd::Constant(3, 4, 1);
    MatrixXd A3 = MatrixXd::Identity(3, 3);

    Map<MatrixXd> A4(data, 3, 2);

    MatrixXd A5 = A4.transpose() * A4;
    MatrixXd A6 = x7 * x7.transpose();
    MatrixXd A7 = A4.array() * A4.array();
    MatrixXd A8 = A7.array().log();
    MatrixXd A9 = A8.unaryExpr([](double x) { return exp(x); });
    MatrixXd A10 = MatrixXd::Zero(3,4).unaryExpr([](double x) { return rnorm(); });

    VectorXd x9 = A1.colwise().norm();
    VectorXd x10 = A1.rowwise().sum();

    MatrixXd A11(x1.size(), 3);
    A11 << x1, x2, x3;

    MatrixXd A12(3, x1.size());
    A12 << x1.transpose(),
          x2.transpose(),
          x3.transpose();

    JacobiSVD<MatrixXd> svd(A10, ComputeThinU | ComputeThinV);

    cout << "x1: comman initializer\n" << x1.transpose() << "\n\n";
    cout << "x2: linspace\n" << x2.transpose() << "\n\n";
    cout << "x3: zeors\n" << x3.transpose() << "\n\n";
    cout << "x4: ones\n" << x4.transpose() << "\n\n";
    cout << "x5: constant\n" << x5.transpose() << "\n\n";
    cout << "x6: rand\n" << x6.transpose() << "\n\n";
    cout << "x7: mapping\n" << x7.transpose() << "\n\n";
    cout << "x8: element-wise addition\n" << x8.transpose() << "\n\n";

    cout << "max of A1\n";
    cout << A1.maxCoeff() << "\n\n";
    cout << "x9: norm of columns of A1\n" << x9.transpose() << "\n\n";
    cout << "x10: sum of rows of A1\n" << x10.transpose() << "\n\n";

    cout << "head\n";
    cout << x1.head(3).transpose() << "\n\n";
    cout << "tail\n";
    cout << x1.tail(3).transpose() << "\n\n";
    cout << "slice\n";
    cout << x1.segment(2, 3).transpose() << "\n\n";

    cout << "Reverse\n";
    cout << x1.reverse().transpose() << "\n\n";

    cout << "Indexing vector\n";
    cout << x1(0);
    cout << "\n\n";

    cout << "A1: comma initilizer\n";
    cout << A1 << "\n\n";
    cout << "A2: constant\n";
    cout << A2 << "\n\n";
    cout << "A3: eye\n";
    cout << A3 << "\n\n";
    cout << "A4: mapping\n";
    cout << A4 << "\n\n";
    cout << "A5: matrix multiplication\n";
    cout << A5 << "\n\n";
    cout << "A6: outer product\n";
    cout << A6 << "\n\n";
    cout << "A7: element-wise multiplication\n";
    cout << A7 << "\n\n";
    cout << "A8: ufunc log\n";
    cout << A8 << "\n\n";
    cout << "A9: custom ufucn\n";
    cout << A9 << "\n\n";
    cout << "A10: custom ufunc for normal deviates\n";
    cout << A10 << "\n\n";
    cout << "A11: np.c_\n";
    cout << A11 << "\n\n";
    cout << "A12: np.r_\n";
    cout << A12 << "\n\n";

    cout << "2x2 block startign at (0,1)\n";
    cout << A1.block(0,1,2,2) << "\n\n";
    cout << "top 2 rows of A1\n";
    cout << A1.topRows(2) << "\n\n";
    cout << "bottom 2 rows of A1";
    cout << A1.bottomRows(2) << "\n\n";
    cout << "leftmost 2 cols of A1";
    cout << A1.leftCols(2) << "\n\n";
    cout << "rightmost 2 cols of A1";
    cout << A1.rightCols(2) << "\n\n";

    cout << "Diagonal elements of A1\n";
    cout << A1.diagonal() << "\n\n";
    A1.diagonal() = A1.diagonal().array().square();
    cout << "Transforming diagonal eelemtns of A1\n";
    cout << A1 << "\n\n";

    cout << "Indexing matrix\n";
    cout << A1(0,0) << "\n\n";

    cout << "singular values\n";
    cout << svd.singularValues() << "\n\n";

    cout << "U\n";
    cout << svd.matrixU() << "\n\n";

    cout << "V\n";
    cout << svd.matrixV() << "\n\n";
}


/***************************************
*
* LanXin TECH, All Rights Reserverd.
* Created at Thu May 16 10:27:58 2019
* Contributor: Ling Shi, Ph.D
* Email: lshi@robvision.cn
*
***************************************/

#include "common.h"

#include <Eigen/SVD>
#include <iostream>
#include <fstream>

using namespace std;

namespace lanXin {

RotMat skew(Geo3d v)
{
	RotMat rot;
	rot.setZero();

	rot(0, 1) = -v(2);
	rot(0, 2) = v(1);
	rot(1, 2) = -v(0);

	rot(1, 0) = -rot(0, 1);
	rot(2, 0) = -rot(0, 2);
	rot(2, 1) = -rot(1, 2);

	return rot;
}

Geo3d rodrigues2(const RotMat& matrix)
{
	Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
	RotMat R = svd.matrixU() * svd.matrixV().transpose();

	double rx = R(2, 1) - R(1, 2);
	double ry = R(0, 2) - R(2, 0);
	double rz = R(1, 0) - R(0, 1);

	double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
	double c = (R.trace() - 1) * 0.5;
	c = c > 1. ? 1. : c < -1. ? -1. : c;

	double theta = acos(c);

	if (s < XEPS)
	{
		double t;

		if (c > 0)
			rx = ry = rz = 0;
		else
		{
			t = (R(0, 0) + 1)*0.5;
			rx = sqrt(std::max(t, 0.0));
			t = (R(1, 1) + 1)*0.5;
			ry = sqrt(std::max(t, 0.0)) * (R(0, 1) < 0 ? -1.0 : 1.0);
			t = (R(2, 2) + 1)*0.5;
			rz = sqrt(std::max(t, 0.0)) * (R(0, 2) < 0 ? -1.0 : 1.0);

			if (fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0))
				rz = -rz;
			theta /= sqrt(rx*rx + ry*ry + rz*rz);
			rx *= theta;
			ry *= theta;
			rz *= theta;
		}
	}
	else
	{
		double vth = 1 / (2 * s);
		vth *= theta;
		rx *= vth; ry *= vth; rz *= vth;
	}
	return Eigen::Vector3d(rx, ry, rz).cast<float>();
}

GeoTransform calibrateHandEye(std::vector<GeoTransform>& vH_robot, std::vector<GeoTransform>& vH_mark, HandEyeType t)
{
	//Eigen::Matrix4f rt;
	const int n = lxmin(vH_robot.size(), vH_mark.size());
	if(n <3)
	{
		printf("At lease 3 point-pairs.\n");
		return GeoTransform();
	}

	printf("Start to calibrate with %d point-pairs.\n", n);

	std::vector<GeoTransform> vA, vB;

	for (int i = 0; i < n; i++)
	{
		for (int j = i + 1; j < n; ++j)
		{
			//if(i == 0 && j==i+1) continue;
			if (t == EyeToHand)
			{
				GeoTransform A = vH_robot[j] * vH_robot[i].inverse();
				GeoTransform B = vH_mark[j] * vH_mark[i].inverse();

				vA.push_back(A);
				vB.push_back(B);
			}
			else if (t == EyeInHand)
			{

				GeoTransform A = vH_robot[j].inverse() * vH_robot[i];
				GeoTransform B = vH_mark[j] * vH_mark[i].inverse();

				vA.push_back(A);
				vB.push_back(B);
			}
		}
	}

	GeoTransform H = sovleAXequalXB(vA, vB);

	return H;
}

Eigen::MatrixXf svdInverse(Eigen::MatrixXf  A)
{
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);//M=USV*
	float  pinvtoler = 1.e-6; //tolerance
	int row = A.rows();
	int col = A.cols();
	int k = min(row, col);
	Eigen::MatrixXf X = Eigen::MatrixXf::Zero(col, row);
	Eigen::MatrixXf singularValues_inv = svd.singularValues();//奇异值
	Eigen::MatrixXf singularValues_inv_mat = Eigen::MatrixXf::Zero(col, row);
	for (long i = 0; i < k; ++i) {
		if (singularValues_inv(i) > pinvtoler)
			singularValues_inv(i) = 1.0 / singularValues_inv(i);
		else singularValues_inv(i) = 0;
	}
	for (long i = 0; i < k; ++i)
	{
		singularValues_inv_mat(i, i) = singularValues_inv(i);
	}
	X = (svd.matrixV())*(singularValues_inv_mat)*(svd.matrixU().transpose());//X=VS+U*

	return X;
}

GeoTransform sovleAXequalXB(std::vector<GeoTransform>& vA, std::vector<GeoTransform>& vB)
{
	GeoTransform H;
	H.setIdentity();
	if (vA.size() != vB.size())
	{
		printf("A and B must be same size.\n");
		return H;
	}

	const int n = vA.size();

	RotMat R_a, R_b;
	Geo3d r_a, r_b;

	Eigen::MatrixXf A(n*3, 3);
	Eigen::MatrixXf b(n*3, 1);
	A.setZero();
	b.setZero();

	for (int i = 0; i < n; ++i)
	{
		R_a = vA[i].linear();
		R_b = vB[i].linear();

		Geo3d rod_a = rodrigues2(R_a);
		Geo3d rod_b = rodrigues2(R_b);

		float theta_a = rod_a.norm();
		float theta_b = rod_b.norm();

		rod_a /= theta_a;
		rod_b /= theta_b;

		Geo3d P_a = 2*sin(theta_a/2)*rod_a;
		Geo3d P_b = 2*sin(theta_b/2)*rod_b;

		Eigen::Matrix3f rot = skew(Geo3d(P_b+P_a));
		Geo3d v = P_b - P_a;
		//for (int row = 3 * i; row < 3 * i + 3; ++row)
		//{
		//	for (int col = 0; col < 3; ++col)
		//	{
		//		A(row, col) = rot(row - 3 * i, col);
		//	}

		//	b(row) = v(row - 3 * i);
		//}

		//cout << A.middleRows(3 * i, 3 * i + 3) << endl;
		//cout << rot << endl;
		A.middleRows(3 * i, 3) = rot;
		b.middleRows(3*i, 3) = v;
	}
	//Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);

	// 3 by 3*n
	Eigen::MatrixXf pinA = svdInverse(A);

	// 3 by 1 = 3 by 3*n multi 3*n by 1
	Geo3d H_ba_prime = pinA * b;

	Geo3d H_ba = 2 * H_ba_prime / sqrt(1 + lxsq(H_ba_prime.norm()));

	// 1 by 3
	Eigen::MatrixXf H_ba_Trs = H_ba.transpose();

	RotMat R_ba = (1 - lxsq(H_ba.norm()) / 2) * RotMat::Identity()
		+ 0.5 * (H_ba * H_ba_Trs + sqrt(4 - lxsq(H_ba.norm()))*skew(H_ba));

	A.setZero();
	b.setZero();
	for (int i = 0; i < n; ++i)
	{
		RotMat AA = vA[i].linear() - RotMat::Identity();
		Geo3d bb = R_ba * vB[i].translation() - vA[i].translation();
		//for (int row = 3 * i; row < 3 * i + 3; ++row)
		//{
		//	for (int col = 0; col < 3; ++col)
		//	{
		//		A(row, col) = AA(row - 3 * i, col);
		//	}

		//	b(row) = bb(row - 3 * i);
		//}
		A.middleRows(3 * i, 3) = AA;
		b.middleRows(3 * i, 3) = bb;
	}
	pinA = svdInverse(A);
	Geo3d t_ba = pinA * b;
	H.linear() = R_ba;
	H.translation() = t_ba;

	// check
	for(int i = 0; i<n; ++i)
	{
		GeoTransform AX = vA[i] * H;
		GeoTransform XB = H * vB[i];

		Geo3d angles1 = AX.linear().eulerAngles(0, 1, 2);
		Geo3d angles2 = XB.linear().eulerAngles(0, 1, 2);
		// cout << i << " Dist Error: " << (AX.translation() - XB.translation()).norm()
		// 	<< ". Rotation Error: " << (angles1 - angles2).transpose() << endl;
	}

	std::string fname("H.txt");
	printf("Calibration Finished. Saved at %s.\n", fname.c_str());

	cout << "H: \n" << H.matrix() << endl;
	ofstream out(fname);
	out <<"# Calibrated At " << fname  << endl;
	out << H.matrix() << endl;
	out.close();

	return H;
}

} /* End of namespace lanXin */


// TestNormal.cpp
//
// First program test the Boost statistics library.
//
// Look at the Normal distribution because it is important.
// And gamma distribution
//
// 2008-6-27 DD initial code
// 2011-11-9 DD for QN course
//
// (C) Datasim Education BV 2009-2011
//

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

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

int main()
{
	// Don't forget to tell compiler which namespace
	using namespace boost::math;

	normal_distribution<> myNormal(1.0, 10.0); // Default type is 'double'
	cout << "Mean: " << mean(myNormal) << ", standard deviation: " << standard_deviation(myNormal) << endl;

	// Distributional properties
	double x = 10.25;

	cout << "pdf: " << pdf(myNormal, x) << endl;
	cout << "cdf: " << cdf(myNormal, x) << endl;

	// Choose another data type and now a N(0,1) variate
	normal_distribution<float> myNormal2;
	cout << "Mean: " << mean(myNormal2) << ", standard deviation: " << standard_deviation(myNormal2) << endl;

	cout << "pdf: " << pdf(myNormal2, x) << endl;
	cout << "cdf: " << cdf(myNormal2, x) << endl;

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

	// Other properties
	cout << "\n***normal distribution: \n";
	cout << "mean: " << mean(myNormal) << endl;
	cout << "variance: " << variance(myNormal) << endl;
	cout << "median: " << median(myNormal) << endl;
	cout << "mode: " << mode(myNormal) << endl;
	cout << "kurtosis excess: " << kurtosis_excess(myNormal) << endl;
	cout << "kurtosis: " << kurtosis(myNormal) << endl;
	cout << "characteristic function: " << chf(myNormal, x) << endl;
	cout << "hazard: " << hazard(myNormal, x) << endl;

	// Gamma distribution
	double alpha = 3.0; // Shape parameter, k
	double beta = 0.5;	// Scale parameter, theta
	gamma_distribution<double> myGamma(alpha, beta);

	double val = 13.0;
	cout << endl <<  "pdf: " << pdf(myGamma, val) << endl;
	cout << "cdf: " << cdf(myGamma, 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);

	for (long j = 1; j <= N; ++j)
	{
		pdfList.push_back(pdf(myGamma, val));
		cdfList.push_back(cdf(myGamma, val));

		val += h;
	}

	for (long j = 0; j < pdfList.size(); ++j)
	{
		cout << pdfList[j] << ", ";

	}

	cout << "***" << endl;

	for (long j = 0; j < cdfList.size(); ++j)
	{
		cout << cdfList[j] << ", ";

	}

	return 0;
}

Level_8/V. An Introduction to the Boost C++ Libraries/05b - Normal Distribution/TestNormalDistribution.cpp


/**
 *  @file    diffusion.hpp
 *  @brief   Collections of diffusion problem, definitions.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef DIFFUSION_H
#define DIFFUSION_H

#include <string>
#include <Eigen/Core>
#include "spdlog/spdlog.h"
#include "numerical/fdm/fdm.hpp"

namespace bench{
namespace diffusion{

typedef Eigen::VectorXd Vec;
typedef Eigen::ArrayXd Arr;

/**
* Reference solution for the 1D diffusion problem a, defined as:
*
* \f[
*    \frac{\partial u}{\partial t} = -\alpha \frac{\partial^2 u}{\partial x^2}
*                                    + f(x,t)\quad x \in (0, L),~t \in (0, T]
* \f]
*
* with Dirichlet boundary condition \f$u(0, t) = 0\f$ and  \f$ u(L, t) = 0\f$
* for \f$t>0\f$.
*
* The diffusion coefficient, \f$ \alpha(L, t) = 1\f$ is constant and uniform.
*
* We define the source term and initial condition with a reference
* (manufactured) solution that respects the boundary conditions at the
* extremities of the interval:
*
* \f[
*    u(x, t) = 5tx*\left(L-x\right)
* \f]
*
* We get the source term substituting the manufactured solution in the
* diffusion equation, i.e.:
*
* \f[
*    f(x, t) = 5x\left(L-x\right)+10\alpha t
* \f]
*
* The initial condition is simply set to the manufactured solution at
* \f$t=0\f$:
*
* \f[
*    u(x, 0) = u_0 = 0
* \f]
*
* @param x The spatial mesh node.
* @param t The temporal mesh node.
* @param L The length of the interval.
* @return The reference solution.
*/
template<typename T>
T reference_diffusion_a(T x, T t, T L){
	return 5.0 * t * x * (L - x);
}

/**
* Computes the 1D diffusion problem defined in @ref reference_diffusion_a()
* using the finite difference method. Here we solve the problem using the
* \f$\Theta\f$-scheme in time and a centered difference scheme in space, i.e.
*
* \f[
*    u_i^{n+1} - \Theta\left(\alpha\left(
*        \frac{u_{i-1}^{n+1}-2u_{i}^{n+1}+u_{i+1}^{n+1}}{\Delta x^2}
*        \right)\right) = \left(1-\Theta\right)\left(\alpha\left(
*        \frac{u_{i-1}^{n}-2u_{i}^{n}+u_{i+1}^{n}}{\Delta x^2}
*        \right)\right) + \Theta\Delta t f_i^{n+1}+\left(1-\Theta\right)f_i^n
*        + u_i^n
* \f]
*
* For \f$\Theta=0\f$ we have the forward Euler scheme in time, for
* \f$\Theta=1\f$ we have the backward Euler scheme in time and for
* \f$\Theta=1/2\f$ we have the Crank-Nicolson scheme in time.
*
* In matrix form we have:
*
* \f[
*    \mathbf{A}\mathbf{u} = \mathbf{b}
* \f]
*
* Using the Fourrier coefficient \f$F=\alpha\Delta t/\Delta x^2\f$, the matrix
* entries are:
*
* \f[
*    A_{i,i-1}=-F\Theta,~A{i, i}=1+2F\Theta,~A_{i, i+1}=-F\Theta
* \f].
*
* The vector \f$\mathbf{u}\f$ represents the unknown at the discrete time
* \f$t=t^{n+1}\f$.
*
* The RHS can be written as:
*
* \f[
*    b_i = u_i^n + F\left(1-\Theta\right)u_{i+1}^n-2u_i^n+u_{i-1}^n +
*        \Delta t \Theta f_i^{n+1} + \Delta t \left(1-\Theta\right)f_i^n
* \f]
*
* For more information see pp. 181-183 of:
*
* R. J. Leveque, Finite Difference Methods for Ordinary and Partial
* Differential Equations: Steady-State and Time-Dependent Problems, SIAM, 2007.
*
* @return The solution.
* @see numerical::fdm::Parameters
* @see numerical::fdm::Mesh
* @see numerical::fdm::SparseSolver
*/
bool fdm_diffusion_a();

class FDMDiffusionA : public numerical::fdm::Problem<double>{
public:
    FDMDiffusionA(numerical::fdm::Parameters<double>* p):
        numerical::fdm::Problem<double>(p){
        }
    // double alpha(double x, double y, double z, double t);

};

}  // namespace diffusion
}  // namespace bench

#endif  // DIFFUSION_H


#include "RegressionEigen.hpp"
#include "RegressionGSL.hpp"
#include <Math/GeneralMath.hpp>
#include <Util/VecD.hpp>

//#define EIGEN_USE_MKL_ALL
//#define EIGEN_USE_BLAS // Enables openblas64
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
using namespace EnjoLib;

RegressionEigen::RegressionEigen(){}
RegressionEigen::~RegressionEigen(){}

/*
VecD RegressionEigen::polynomialfit(int degree, const VecD & dx, const VecD & dy) const
{
    return RegressionGSL().polynomialfit(degree, dx, dy);
}
*/

VecD RegressionEigen::polynomialfit(int degree, const VecD & dx, const VecD & dy) const
{
    VecD coeffs(degree);
    const int obs = dx.size();
    //VecD weights(obs);
    MatrixXd A(obs, degree);
   //cout << "Here is the matrix A:\n" << A << endl;
    VectorXd b(obs);
    //Map<VectorXd> b(dy.data(), obs, 1);
    //VecD dycopy = dy;
    //Map<VectorXd> b(dycopy.data(), obs);
   const GeneralMath gmat;

    for(int i=0; i < obs; ++i)
    {
        b(i) = dy[i];

        //gsl_vector_set(y.vec, i, dy[i]);
        //gsl_vector_set(w.vec, i, weights[i]);
        //gsl_vector_set(w.vec, i, i);        // More weight to latest
        //gsl_vector_set(w.vec, i, obs - i);  // More weight to initial
        //gsl_vector_set(w.vec, i, 1);      // Disabled different weights
        for(int j=0; j < degree; ++j)
        {
            //gsl_matrix_set(X.mat, i, j, pow(dx[i], j));
            A(i, j) = gmat.PowInt(dx[i], j);
            //A(i, j) = dx[i];
        }
    }

    /*
    const auto & eigRes = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);
       cout << "The least-squares solution is:\n"
        << eigRes << endl;
      */
    //const MatrixXd & aTrans = A.transpose();
	//const auto & f = (aTrans*A).inverse()*aTrans*b;
	const auto & f = (A.transpose()*A).inverse()*A.transpose()*b; // Faster

	// Faster!
	//const auto & f = (A.transpose()*A).inverse()*A.transpose()*b;
	//const auto & ff = A.colPivHouseholderQr().solve(b);
	//const auto & ff = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);

	//cout << "Matrix f" << endl;
	//cout << ff << endl;

    /* store result ... */
    for(int i=0; i < degree; ++i)
    {
        const double coeff = f(i, 0);
        //cout << "Coeff " << i << " = " << coeff << endl;;
        coeffs[i] = coeff;
    }
    return coeffs; /* we do not "analyse" the result (cov matrix mainly)
          to know if the fit is "good" */
    // http://stats.stackexchange.com/questions/151018/how-does-the-covariance-matrix-of-a-fit-get-computed
}


#include "knn.h"
#include <Eigen/Dense>
#include <iostream>

int Predict1toMaxNeighbors(
    double *train_inputs_ptr,
    double *train_label_ptr,
    int nrow, int ncol, int max_neighbors,
    double *test_input_ptr, //ncol
    //inputs above, outputs below
    double *test_prediction_ptr //max neighbors
){
  Eigen::VectorXd distance_vec(nrow);
  Eigen::Map< Eigen::MatrixXd > train_inputs_mat(train_inputs_ptr, nrow, ncol);
  Eigen::Map< Eigen::VectorXd > test_input_vec(test_input_ptr, ncol);
  Eigen::VectorXd diff_vec(ncol);
  Eigen::VectorXi sorted_index_vec(nrow);
  //Compute the distance
  for(int i=0;i<nrow;i++){
    distance_vec(i) = (train_inputs_mat.row(i).transpose()-test_input_vec).lpNorm<1>();
    //distance_vec(i) = diff_vec.lpNorm<1>();//computes L1 norm
    //distance_vec(i) = diff_vec.norm(); //Computes L2 norm
    sorted_index_vec(i) = i; //initialze sort vec
  }

  std::sort(sorted_index_vec.data(),
            sorted_index_vec.data()+sorted_index_vec.size(),
            [&distance_vec](int left, int right){             //&distance_vec lets you use distance_vec in this func
              return distance_vec(left) < distance_vec(right);
            });
  //std:: cout << sorted_index_vec << std::endl << std:: endl;
  double total_labels = 0.0;
  // First element to max neighbors
  for(int k=0; k<max_neighbors; k++){
    int row = sorted_index_vec(k);
    int neighbors = k+1;
    total_labels+= train_label_ptr[row];
    test_prediction_ptr[k]= total_labels/neighbors;
  }
  return 0;
}

int knn(
    const double *train_inputs_ptr, //n_observations x n_features (Matrix)
    const double *train_label_ptr,  //n_observations (vector)
    const double *test_input_ptr,   //n_features (vector)
    const int n_observations, const int n_features, const int max_neighbors,
    double *test_predictions_ptr    //max_neighbors (vector)
){
  Eigen::Map< Eigen::MatrixXd > train_inputs_mat((double*)train_inputs_ptr, n_observations,n_features);
  Eigen::Map< Eigen::VectorXd > test_input_vec((double*)test_input_ptr,n_features);
  Eigen::VectorXd dist_vec(n_observations);
  Eigen::VectorXi sorted_index_vec(n_observations);
  for (int i=0; i<n_observations;i++){
    dist_vec(i) = (train_inputs_mat.row(i).transpose() - test_input_vec).norm();
    sorted_index_vec(i) = i;
  }
  std::sort(
    sorted_index_vec.data(),
    sorted_index_vec.data()+sorted_index_vec.size(),
    [&dist_vec](int left, int right){
      return dist_vec(left) < dist_vec(right);
    }
  );
  double total = 0.0;
  for(int model_i=0; model_i<max_neighbors; model_i++){
    int neighbors = model_i +1;
    int row_i = sorted_index_vec(model_i);
    total+=train_label_ptr[row_i];
    test_predictions_ptr[model_i] = total/neighbors;
  }
  return 0;
}