/*
Demonstrate the Eigen automatic differentiation module by training a multilayer perceptron (fully-connected feedforward neural-network).
Python3.8 and matplotlib are used for plotting the results.
Compile with:
    g++ eigen3_autodiff.cpp -o eigen3_autodiff -std=c++11 -O3 -ffast-math -I /usr/include/eigen3/ -I /usr/local/include/matplotlib-cpp.h -I /usr/include/python3.8 -l python3.8
    ./eigen3_autodiff 0.01 5  # step hidden_layer1_size hidden_layer2_size ...
*/
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/AutoDiff>
#include "matplotlibcpp.h"
namespace pyplot = matplotlibcpp;

////////////////////////////////////////////////// ALIASES

using Jet = Eigen::AutoDiffScalar<Eigen::VectorXd>;                // real number with many dual parts
using Jetvec = Eigen::Matrix<Jet, Eigen::Dynamic, 1>;              // vector of jets
using Jetmat = Eigen::Matrix<Jet, Eigen::Dynamic, Eigen::Dynamic>; // matrix of jets

////////////////////////////////////////////////// HELPERS

// Returns a random real uniform on [-1, 1]
inline double randf();

// Returns a vector of just the real parts of the given jet vector
inline Eigen::VectorXd reals(Jetvec const& jv);

// Pulls scalar data embedded in Eigen objects out to plain doubles
inline std::vector<double> extract_values(std::vector<Eigen::Matrix<double, 1, 1>> const& vec_eig);

////////////////////////////////////////////////// MAIN CLASS

class MLP {
    std::vector<int> const dims; // network dimensionalities (input_size, hidden_layer1_size, ..., output_size)
    std::vector<Jetmat> weights; // container of weight matrices
    std::vector<Jetvec> biases; // container of bias vectors
    std::vector<Jet*> params; // pointers to each individual parameter

public:
    MLP(std::vector<int> const& dims) :
        dims(dims),
        weights(dims.size()-1),
        biases(dims.size()-1) {
        // Parse the dims architecture to count up the parameters
        int n_params = 0;
        for(int layer=0; layer<weights.size(); ++layer) {
            weights[layer] = Jetmat(dims[layer+1], dims[layer]);
            biases[layer] = Jetvec(dims[layer+1]);
            n_params += weights[layer].size() + biases[layer].size();
        }
        // Initialize parameters with random reals and orthogonal basis on the duals
        int param_id = 0;
        for(Jetmat & W : weights) {
            for(int col=0; col<W.cols(); ++col) {
                for(int row=0; row<W.rows(); ++row) {
                    W(row, col) = Jet(randf(), n_params, param_id); // (value, number_of_dual_parts, which_dual_part)
                    params.push_back(&W(row, col));
                    ++param_id;
                }
            }
        }
        for(Jetvec & b : biases) {
            for(int row=0; row<b.rows(); ++row) {
                b(row) = Jet(randf(), n_params, param_id);
                params.push_back(&b(row));
                ++param_id;
            }
        }
        assert(params.size() == n_params);
    }

    ////////////////////////////////////////////////// GETTERS

    inline std::vector<int> get_dims() const {
        return dims;
    }

    inline std::vector<Jetmat> get_weights() const {
        return weights;
    }

    inline std::vector<Jetvec> get_biases() const {
        return biases;
    }

    ////////////////////////////////////////////////// WORKERS

    // Returns the sigmoid function of a given scalar jet
    inline Jet neuron(Jet const& x) const {
        return pow(Jet(1)+exp(-x), -1);
    }

    // Returns the jet vector MLP output corresponding to the given real input vector
    template <class D>
    inline Jetvec feedforward(Eigen::MatrixBase<D> const& input) const {
        Jetvec act = input.template cast<Jet>();
        for(int layer=0; layer<weights.size()-1; ++layer) {
            act = weights[layer]*act;
            act += biases[layer]; // bit of Eigen weirdness, this has to be a separate statement
            for(int i=0; i<act.size(); ++i) act(i) = neuron(act(i));
        }
        return weights[weights.size()-1]*act + biases[biases.size()-1]; // don't saturate last layer
    }

    // Calls feedforward on each input in an input set to produce the MLP's corresponding output set
    template <class V>
    inline std::vector<V> feedforward(std::vector<V> const& input_set) const {
        std::vector<V> output_set;
        for(V const& input : input_set) {
            output_set.push_back(reals(feedforward(input)));
        }
        return output_set;
    }

    // Common quadratic cost function for a single input-output pair
    template <class D>
    Jet inline cost_function(Eigen::MatrixBase<D> const& input, Eigen::MatrixBase<D> const& output) const {
        Jetvec err = output.template cast<Jet>() - feedforward(input);
        return err.dot(err);
    }

    // Cost function summed over an entire input-output pair batch
    template <class V>
    Jet inline cost_function(std::vector<V> const& input_set, std::vector<V> const& output_set) const {
        Jet cost(0);
        for(int i=0; i<input_set.size(); ++i) {
            cost += cost_function(input_set[i], output_set[i]);
        }
        return cost;
    }

    // Stochastic gradient descent
    template <class V>
    void train(std::vector<V> const& input_set, std::vector<V> const& output_set,
               double step, double tol=1e-5, int max_epoch=10000) {
        assert(input_set.size() == output_set.size());
        std::cout << "Training with step-size " << step << "..." << std::endl;
        std::vector<int> ordering;
        for(int i=0; i<input_set.size(); ++i) ordering.push_back(i);
        double last_epoch_cost = std::numeric_limits<double>::infinity();
        int epoch = 0;
        while(epoch < max_epoch) {
            double epoch_cost = 0;
            for(int i=0; i<input_set.size(); ++i) {
                Jet cost = cost_function(input_set[ordering[i]], output_set[ordering[i]]);
                epoch_cost += cost.value();
                auto dp = (-step*cost.derivatives()).eval();
                for(int i=0; i<params.size(); ++i) *params[i] += dp(i);
            }
            if(!(epoch % 100)) std::cout << "    epoch " << epoch << ": " << epoch_cost << std::endl;
            if(fabs(epoch_cost-last_epoch_cost) < tol) break;
            std::random_shuffle(ordering.begin(), ordering.end());
            last_epoch_cost = epoch_cost;
            ++epoch;
        }
        std::cout << "Finished training on epoch " << epoch << "/" << max_epoch << std::endl;
    }
};

////////////////////////////////////////////////// MAIN EXECUTABLE

int main(int argc, char** argv) {
    if(argc < 3) {
        std::cout << "Please pass in a step-size and the size of each hidden layer!" << std::endl;
        return 1;
    }

    // We will make a fixed set of data within this code for now for simplicity
    using Edouble = Eigen::Matrix<double, 1, 1>;
    std::vector<Edouble> input_set;
    std::vector<Edouble> output_set;
    for(double val=-10; val<=10; val+=0.3) input_set.push_back(Edouble(val));
    for(Edouble const& input : input_set) output_set.push_back(Edouble(5*exp(-input(0)*input(0))) +
                                                               Edouble(2.5*exp(-pow(input(0)-3, 2))) +
                                                               Edouble(0.05*(randf())));

    // Construct and train our MLP from user choices
    std::vector<int> dims;
    dims.push_back(input_set[0].size());
    for(int i=2; i<argc; ++i) {
        int dim = atoi(argv[i]);
        assert(dim > 0);
        dims.push_back(dim);
    }
    dims.push_back(output_set[0].size());
    std::cout << "MLP Layout: ";
    for(int dim : dims) std::cout << dim << ' ';
    std::cout << std::endl;
    MLP mlp(dims);
    mlp.train(input_set, output_set, atof(argv[1]));

    // Evaluate fit
    std::vector<Edouble> fine_input_set;
    for(double val=-10; val<=10; val+=0.005) fine_input_set.push_back(Edouble(val));
    auto mlp_output_set = mlp.feedforward(fine_input_set);
    std::cout << "Plotting results..." << std::endl;
    pyplot::named_plot("data", extract_values(input_set), extract_values(output_set), "g--");
    pyplot::named_plot("fit", extract_values(fine_input_set), extract_values(mlp_output_set), "k");
    pyplot::legend();
    std::cout << "Close plots to finish..." << std::endl;
    pyplot::show();

    return 0;
}

////////////////////////////////////////////////// OTHER IMPLEMENTATION DETAILS

inline double randf() {
    return 2*Eigen::MatrixXd::Random(1, 1)(0) - 1;
}

inline Eigen::VectorXd reals(Jetvec const& jv) {
    Eigen::VectorXd v(jv.size());
    for(int i=0; i<v.size(); ++i) v(i) = jv(i).value();
    return v;
}

inline std::vector<double> extract_values(std::vector<Eigen::Matrix<double, 1, 1>> const& vec_eig) {
    std::vector<double> vec(vec_eig.size());
    for(int i=0; i<vec.size(); ++i) vec[i] = vec_eig[i](0);
    return vec;
}


//
// Created by keszocze on 27.09.18.
//

#pragma once

#include <map>
#include <string>
#include <vector>

#include <cudd/cplusplus/cuddObj.hh>
#include <boost/multiprecision/cpp_int.hpp>

#include "number_representation.hpp"

namespace abo::util {

/**
 * @brief Computes the label/number of the lowest terminal level of a given BDD (forest)
 * @param bdds BDDs in the forest
 * @return Lowest level in the forest
 */
unsigned int terminal_level(const std::vector<std::vector<BDD>>& bdds);

/**
 * @brief Evaluates the sum of a and b for an adder given as a BDD forest
 * @param adder BDD forest representing an adder
 * @param a The first summand
 * @param b The second summand
 * @param bits bit width of the adder
 * @return The value of a + b
 */
long eval_adder(const std::vector<BDD>& adder, long a, long b, int bits);

/**
 * @brief Evaluates the function with the given input
 * @param function Representing an unsigned integer
 * @param input The inputs to the function (see cudd BDD.eval)
 * @return The value of function with the given input
 */
long eval(const std::vector<BDD>& function, std::vector<int> input);

/**
 * @brief count_minterms Counts the number of minterms for each node in the given BDD in percent
 * (between 0 and 1). This counts how many percent of inputs that lead to a given node as an
 * intermediate result also lead to 1.
 * @param bdd The BDD for which the minters are counted
 * @return A map containing every node in the bdd and its minterm count
 */
std::map<DdNode*, double> count_minterms(const BDD& bdd);

/**
 * @brief count_solutions For each node reachable from the given root, count the number of solutions
 * @param bdd The root node to search from
 * @return A map containing the number of solutions for each reachable node
 */
std::map<DdNode*, double> count_solutions(const BDD& bdd);

/**
 * @brief random_satisfying_input Computes a random variable assignment that satisfies the function
 * given as a bdd
 * @param bdd The function that has to be satisfied by the assignment
 * @param minterm_count The minterm count for each node of the bdd, this must be computed by
 * count_minterms called on the bdd
 * @param max_level The maximum variable level that should be present in the variable assignment
 * @return A random variable assignment satisfying bdd. The vector consists of zeros and ones,
 * integers are only used for better compatibility with cudd functions
 */
std::vector<int> random_satisfying_input(const BDD& bdd,
                                         const std::map<DdNode*, double>& minterm_count,
                                         int max_level);

//! Returns the value of the given ADD node if it is a constant node and zero otherwise
unsigned int const_ADD_value(const ADD& add);

/**
 * @brief add_terminal_values Finds all terminal values present in the given ADD and how often each
 * one is reached
 * @param add The function to compute the terminal values of
 * @return A list of terminal values and their frequencies. The first value of the pair is the
 * terminal node value (unique in the list), the second value is how often it is reached in the ADD
 */
std::vector<std::pair<double, unsigned long>> add_terminal_values(const ADD& add);

/**
 * @brief bdd_forest_to_add Computes the ADD equivalent of a function represented by a BDD forest
 * @param mgr The cudd node manager to create the ADD in
 * @param bdds The function to convert to an ADD
 * @param num_rep The number format in which the bdds represent the function
 * @return The given function as an ADD
 */
ADD bdd_forest_to_add(const Cudd& mgr, const std::vector<BDD>& bdds,
                      const NumberRepresentation num_rep
                        = NumberRepresentation::BaseTwo);

/**
 * @brief xor_difference_add Computes the bit-wise XOR between the two given functions and converts
 * the result to an ADD The conversion creates unsigned integer values
 * @param mgr The cudd node manager to create the ADD in
 * @param f The first function to compute the difference of
 * @param f_hat The second function. Must have the same length as f
 * @return The XOR difference as an ADD
 */
ADD xor_difference_add(const Cudd& mgr, const std::vector<BDD>& f,
                       const std::vector<BDD>& f_hat);

/**
 * @brief absolute_difference_add Computes the absolute difference between to functions represented
 * as BDDs and returns the result as an ADD
 * @param mgr The cudd node manager to create the ADD in
 * @param f The first function to compute the difference of
 * @param f_hat The second function. Must have the same length as f
 * @param num_rep The number representation used for the interpretation of the functions f and f_hat
 * @return A function computing the absolute difference between f and f_hat for each input
 * represented by an ADD
 */
ADD absolute_difference_add(const Cudd& mgr, const std::vector<BDD>& f,
                            const std::vector<BDD>& f_hat,
                            const NumberRepresentation num_rep);

/**
 * @brief dump_dot A helper function for calling the dump dot method of cudd for a BDD forest
 * @param mgr The cudd node manager to use
 * @param bdd The function to dump as a dot file string
 * @param innames The names of the input variables to show in the dot file. May be left empty
 * @param outnames The names for each bit of the function to show in the dot file. May be left empty
 */
void dump_dot(const Cudd& mgr, const std::vector<BDD>& bdd,
              const std::vector<std::string>& innames = std::vector<std::string>(),
              const std::vector<std::string>& outnames = std::vector<std::string>());

/**
 * @brief dump_dot A helper function for calling the dump dot method of cudd for a single BDD
 * @param mgr The cudd node manager to use
 * @param bdd The function to dump as a dot file string
 * @param inames The names of the input variables to show in the dot file. May be left empty
 * @param funname The name of the function to show in the dot file output
 */
void dump_dot(const Cudd& mgr, const BDD& bdd,
              const std::vector<std::string>& inames = std::vector<std::string>(),
              const std::string& funname = std::string());

/**
 * @brief Retrives high-child of a BDD v
 * @param mgr The Cudd object manager
 * @param v
 * @return low(v)
 */
BDD high(const Cudd& mgr, const BDD& v);

/**
 * @brief Retrives low-child of a BDD v
 * @param mgr The Cudd object manager
 * @param v
 * @return low(v)
 */
BDD low(const Cudd& mgr, const BDD& v);

/**
 * @brief Creates a full adder of two BDDs and a carry BDD
 * @param f First summand BDD
 * @param g Second summand BDD
 * @param carry_in Carry BDD
 * @return Tuple of BDDs: Summ BDD and Carry BDD
 */
std::pair<BDD, BDD> full_adder(const BDD& f, const BDD& g, const BDD& carry_in);

/**
 * @brief Creates a (vector of) BDDs that represent the difference between to given (vectors of)
 * BDDs
 *
 * The supplied BDDs are supposed to compute values represented in Two's Copmlement. The BDD
 * returned by this function computes the difference between the outputs of these BDDs, again
 * represented in Two's Complement.
 *
 * @param mgr The Cudd object manager
 * @param minuend Minuend in Two's Complement
 * @param subtrahend  Subtrahend in Two's Complement
 * @return BDD computing the difference of the outputs of the two supplied BDDs
 */
std::vector<BDD> bdd_subtract(const Cudd& mgr, const std::vector<BDD>& minuend,
                              const std::vector<BDD>& subtrahend);

/**
 * @brief Creates a (vector of) BDDs that represent the absolute difference between to given
 * (vectors of) BDDs
 *
 * The supplied BDDs are supposed to compute values represented in Two's Copmlement. The BDD
 * returned by this function computes the difference between the outputs of these BDDs. It will
 * always return an unsigned number, not a number in Two's complement.
 *
 * @param mgr The Cudd object manager
 * @param f Function in Two's Complement
 * @param g Funtion in Two's Complement
 * @param num_rep The number representation for f and g
 * @return BDD computing the absolute difference of the outputs of the two supplied BDDs
 */
std::vector<BDD> bdd_absolute_difference(const Cudd& mgr,
                                         const std::vector<BDD>& f,
                                         const std::vector<BDD>& g,
                                         const NumberRepresentation num_rep);

/**
 * @brief Creates the BDD representing the sum of two functions
 * @param mgr The Cudd object manager
 * @param f First summand
 * @param g Second summand
 * @return BDD representing f+g
 */
std::vector<BDD> bdd_add(const Cudd& mgr,
                         const std::vector<BDD>& f,
                         const std::vector<BDD>& g);

/**
 * @brief Creates BDD representing abs(f). For unsigned numbers, the original function will be
 * returned
 * @param mgr The Cudd object manager
 * @param f Function in Two's Complement
 * @param num_rep The number representation for f
 * @return BDD representing abs(f)
 */
std::vector<BDD> bdd_abs(const Cudd& mgr, const std::vector<BDD>& f,
                        const NumberRepresentation num_rep);

/**
 * @brief Shifts a function f by a number of bits left or right
 *
 * The shifted result has the same number of bits as the input, bits that are shifted above or below
 * the original function bit range will be discarded
 *
 * @param mgr The Cudd object manager
 * @param f Function to shift
 * @param bits_to_shift Number of bits to shift. Can be both positive or negative
 * @return Shifted BDD
 */
std::vector<BDD> bdd_shift(const Cudd& mgr,
                           const std::vector<BDD>& f,
                           int bits_to_shift);

/**
 * @brief Multiplies a bdd function with a constant
 * @param mgr The Cudd object manager
 * @param f Function that is to be multiplied. The function is interpreted as returning an unsigned
 * integer
 * @param factor Constant multiplication factor
 * @param num_extra_bits Additional bits used to make sure that the result can be stored (must be
 * less than sizeof(unsigned long) * 8)
 * @return BDD representing the function "factor*f" (extended by up to num_extra_bits)
 */
std::vector<BDD> bdd_multiply_constant(const Cudd& mgr,
                                       const std::vector<BDD>& f,
                                       double factor,
                                       const unsigned int num_extra_bits = 16);

/**
 * @brief Multiplies a bdd function with a constant large value
 * @param mgr The Cudd object manager
 * @param f Function that is to be multiplied. The function is interpreted as returning an unsigned
 * integer
 * @param factor Constant multiplication factor
 * @return BDD representing the function "factor*f"
 */
std::vector<BDD> bdd_multiply_constant(const Cudd& mgr,
                                       const std::vector<BDD>& f,
                                       boost::multiprecision::uint256_t factor);
/**
 * @brief Checks if an input x exists such that int(f1(x)) >= int(f2(x))
 * @param mgr The Cudd object manager
 * @param f1 Function to compare in base two (interpreted as returning an unsigned integer)
 * @param f2 Function to compare in base two (interpreted as returning an unsigned integer)
 * @return {ge, e}
 * ge is true iff there exists an x such that int(f1(x)) >= int(f2(x))
 * e is true iff there exists an x such that int(f1(x)) = int(f2(x)), but no x exists with
 * int(f1(x)) > int(f2(x))
 */
std::pair<bool, bool> exists_greater_equals(const Cudd& mgr,
                                            const std::vector<BDD>& f1,
                                            const std::vector<BDD>& f2);

/**
 * @brief Creates a BDD representing f >= g
 * @param mgr The Cudd object manager
 * @param f Function to compare in base two (interpreted as returning an unsigned integer)
 * @param g Function to compare in base two (interpreted as returning an unsigned integer)
 * @return BDD representing f >= g
 */
BDD greater_equals(const Cudd& mgr,
                   const std::vector<BDD>& f,
                   const std::vector<BDD>& g);

/**
 * @brief Creates a BDD representing f > g
 * @param mgr The Cudd object manager
 * @param f Function to compare in base two (interpreted as returning an unsigned integer)
 * @param g Function to compare in base two (interpreted as returning an unsigned integer)
 * @return BDD representing f > g
 */
BDD greater_than(const Cudd& mgr,
                const std::vector<BDD>& f,
                const std::vector<BDD>& g);

/**
 * @brief Computes a function g such that int(g(x)) = max(1, int(f(x)) for an unsigned function f
 * @param mgr The Cudd object manager
 * @param f The function to use. Is interpreted as unsigned integers
 * @return Function g as vector of BDDS of same length as f
 */
std::vector<BDD> bdd_max_one(const Cudd& mgr, const std::vector<BDD>& f);

/**
 * @brief divide Computes a function h such that int(h(x)) = int(f(x)) / int(g(x))
 * @param mgr The Cudd object manager
 * @param f int(f(x)) = 0 must not be possible, f is interpreted as an unsigned integer
 * @param g is interpreted as an unsigned integer
 * @param extra_bits
 * @return
 */
std::vector<BDD> bdd_divide(const Cudd& mgr,
                            const std::vector<BDD>& f,
                            const std::vector<BDD>& g,
                            unsigned int extra_bits);

/**
 * @brief Converts an unsigned number to a BDD function representing that value
 * @param mgr The Cudd object manager
 * @param number The value the resulting function should return
 * @return A function that returns the given number for all inputs
 */
std::vector<BDD> number_to_bdds(const Cudd& mgr,
                                const boost::multiprecision::uint256_t &number);

} // namespace abo::util


#pragma once

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

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

///////////////////////////////////////////////////////////////////////////////
// Point - Edge

template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename T = typename DerivedP::Scalar>
inline T point_edge_closest_point(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    auto e = e1 - e0;
    return (p - e0).dot(e) / e.squaredNorm();
}

///////////////////////////////////////////////////////////////////////////////
// Edge - Edge

/// Compute the barycentric coordinates of the closest points
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename T = typename DerivedEA0::Scalar>
inline Vector2<T> edge_edge_closest_point(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    auto eb_to_ea = ea0 - eb0;
    auto ea = ea1 - ea0;
    auto eb = eb1 - eb0;

    Eigen::Matrix<T, 2, 2> coefMtr;
    coefMtr(0, 0) = ea.squaredNorm();
    coefMtr(0, 1) = coefMtr(1, 0) = -eb.dot(ea);
    coefMtr(1, 1) = eb.squaredNorm();

    Vector2<T> rhs;
    rhs[0] = -eb_to_ea.dot(ea);
    rhs[1] = eb_to_ea.dot(eb);

    return coefMtr.ldlt().solve(rhs);
}

///////////////////////////////////////////////////////////////////////////////
// Point - Triangle

/// Compute the barycentric coordinates of the closest point on the triangle.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename T = typename DerivedP::Scalar>
inline Vector2<T> point_triangle_closest_point(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    Eigen::Matrix<T, 2, 3> basis;
    basis.row(0) = RowVector3<T>(t1 - t0); // edge 0
    basis.row(1) = RowVector3<T>(t2 - t0); // edge 1
    Matrix2<T> A = basis * basis.transpose();
    Vector2<T> b = basis * Vector3<T>(p - t0);
    Vector2<T> x = A.ldlt().solve(b);
    assert((A * x - b).norm() < 1e-10);
    return x;
}

} // namespace ipc


/*
 * File: fast_hankel_transform.hpp
 * Created Date: 2019-09-11
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Wednesday September 25th 2019 11:38:09 am
 *
 * MIT License
 *
 * Copyright (c) 2019 Lei Pan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the 'Software'), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#ifndef FASTHANKELTRANSFORM_FASTHANKELTRANSFORM_H_
#define FASTHANKELTRANSFORM_FASTHANKELTRANSFORM_H_

#include <Eigen/Dense>
#include <complex>

class FastHankelTransform {
public:
  FastHankelTransform(int num_sample, double ux_, double uy_);
  ~FastHankelTransform();
  Eigen::VectorXd sampling();
  void set_feval(const Eigen::Ref<const Eigen::VectorXd> &feval);
  Eigen::VectorXd calculate();

private:
  double evaluate_alpha();
  double evaluate_k0(double alpha);
  void evaluate_phi();
  void evaluate_j1();
  const int num_sample_;
  const double ux_;
  const double uy_;
  double alpha_;
  double k0_;

  Eigen::VectorXd x_;
  Eigen::VectorXd f_;
  std::complex<double> *phi_;
  std::complex<double> *j1_;

  bool x_updated_ = false;
  bool f_updated_ = false;
};

#endif


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

#include <iostream>
#include <algorithm>
#include <cmath>
#include <string>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

using namespace std;

typedef mtl::compressed2D<double> m_type;

void assemble_poisson2D(m_type& A, int n)
{
    m_type dia_block(n, n), off_block(n, n);
    {
	mtl::mat::inserter<m_type> ins(dia_block);
	for (int i= 0; i < n; i++) {
	    ins[i][i] << 4.0;
	    if (i > 0) ins[i][i-1] << -1.0;
	    if (i < n-1) ins[i][i+1] << -1.0;
	}
    }
    off_block= -1.0;
    A.change_dim(n*n, n*n);
    {
	mtl::mat::inserter<m_type> ins(A);
	for (int i= 0; i < n; i++) {
	    ins[i*n][i*n] << dia_block;
	    if (i > 0) ins[i*n][i*n-n] << off_block;
	    if (i < n-1) ins[i*n][i*n+n] << off_block;
	}
    }
}

//
// Inverse iteration: is almost a literal copy from algorithm 9.1.2
//
// LinearSolver: BinaryFunction
// EigenVector: GLAS dense vector
template <class LinearSolver, class EigenVector>
double inverse_iteration( LinearSolver& solver, EigenVector& x, int m )
{
    EigenVector y( size(x), 0.0 ) ;
    double lambda = 0.0 ;
    x /= two_norm(x) ;
    for (int i=0; i<m; ++i) {
	solver( x, y ) ;
	lambda = mtl::sum(x) / mtl::sum(y) ;
	x = y / two_norm(y) ;
    }
    return lambda ;
}

//
// Inverse iteration: is almost a literal copy from algorithm 9.1.2
//
// LinearSolver: BinaryFunction
// EigenVector: GLAS dense vector
template <class LinearSolver, class Matrix, class EigenVector>
double condition( LinearSolver& solver, const Matrix& A, EigenVector& x, int m ) {
  EigenVector y( size(x) ) ;
  double lambda= 0.0 ;
  x/= two_norm(x);
  for (int i=0; i<m; ++i) {
      y= A * x;
      lambda = mtl::sum(y) / mtl::sum(x) ;
      x = y / two_norm(y) ;
  }
  return std::abs(lambda / inverse_iteration(solver, x, m));
}

template <typename Matrix>
class cg_solver
{
public:
    cg_solver(const Matrix& A) : A(A) {}

    template <typename VectorIn, typename VectorOut>
    void operator() (const VectorIn& v_in, VectorOut& v_out)
    {
	itl::pc::diagonal<Matrix>     P(A);
	itl::cyclic_iteration<double>  iter(v_in, 500, 1.e-6);

	cg(A, v_out, v_in, P, iter);
    }

private:
    const Matrix& A;
};

int main(int, char**)
{
    m_type A;
    assemble_poisson2D(A, 10);

    mtl::dense_vector<double>   x(num_rows(A));
    mtl::seed<double>           seed;
    random(x, seed);

    cg_solver<m_type> sol(A);
    double lambda= inverse_iteration(sol, x, 10);
    cout << "lambda is " << lambda << '\n';

    double cond= condition(sol, A, x, 10);
    cout << "cond is " << cond << '\n';

    return 0;
}


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * ****************************************************************************
 * Statistical Analysis.
 * ****************************************************************************
 */

#ifndef VISIONCORE_MATH_STATISTICS_HPP
#define VISIONCORE_MATH_STATISTICS_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <VisionCore/Buffers/Buffer1D.hpp>
#include <VisionCore/Buffers/Buffer2D.hpp>
#include <VisionCore/Types/Polynomial.hpp>
#include <VisionCore/Types/Gaussian.hpp>
#include <VisionCore/Math/PolarSpherical.hpp>

/**
 * @note:
 * http://rebcabin.github.io/blog/2013/01/22/covariance-matrices/
 */

namespace vc
{

namespace math
{

/**
 * Running mean, variation, std. deviation, count and min,max.
 */
template<typename T>
class RunningStats
{
public:
    typedef T Scalar;
    static constexpr int Rows = 1;
    static constexpr int Cols = 1;
    static constexpr int Dimension = Rows * Cols;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline RunningStats()
    {
        reset();
    }

    EIGEN_DEVICE_FUNC inline void reset()
    {
        cnt = 0;
        minV = std::numeric_limits<T>::max();
        maxV = -std::numeric_limits<T>::max();
        sumX = sumXX = T(0.0);
    }

    EIGEN_DEVICE_FUNC inline void operator()(const T& x)
    {
        using Eigen::numext::mini;
        using Eigen::numext::maxi;

        // process
        cnt++;

        sumX += x;
        sumXX += (x * x);

        maxV = max(maxV, x); // NOTE what about weight?
        minV = min(minV, x);
    }

    EIGEN_DEVICE_FUNC inline std::size_t count() const
    {
        return cnt;
    }

    EIGEN_DEVICE_FUNC inline T mean() const
    {
        if(cnt > 0)
        {
            return sumX / T(cnt);
        }
        else
        {
            return T(0.0);
        }
    }

    EIGEN_DEVICE_FUNC inline T variance() const
    {
        if(cnt > 0)
        {
            const T bessel_correction = T(cnt) / T(cnt - 1);
            const T cm = mean();
            const T term1 = sumXX / T(cnt);

            return bessel_correction * (term1 - (cm*cm));
        }
        else
        {
            return T(0.0);
        }
    }

    EIGEN_DEVICE_FUNC inline T stddev() const
    {
        using Eigen::numext::sqrt;
        return sqrt(variance());
    }

    EIGEN_DEVICE_FUNC inline T running_sum() const { return sumX; }

    EIGEN_DEVICE_FUNC inline T min_value() const { return minV; }
    EIGEN_DEVICE_FUNC inline T max_value() const { return maxV; }

    EIGEN_DEVICE_FUNC inline types::Gaussian<T> gaussian() const
    {
        return types::Gaussian<T>(mean(), variance());
    }
private:
    std::size_t cnt;
    T sumX, sumXX;
    T minV, maxV;
};

/**
 * Specialization for Eigen.
 */
template<typename _Scalar, int _Rows, int _Cols, int _Options>
class RunningStats<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options>>
{
public:
    typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options> VectorType;
    typedef _Scalar Scalar;
    static constexpr int Rows = _Rows;
    static constexpr int Cols = _Cols;
    static constexpr int Dimension = Rows * Cols;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline RunningStats()
    {
        reset();
    }

    EIGEN_DEVICE_FUNC inline void reset()
    {
        cnt = 0;
        minV = VectorType::Constant(std::numeric_limits<Scalar>::max());
        maxV = VectorType::Constant(-std::numeric_limits<Scalar>::max());
        sumX = VectorType::Zero();
        sumXX = VectorType::Zero();
    }

    EIGEN_DEVICE_FUNC inline void operator()(const VectorType& x)
    {
        // process
        cnt++;

        sumX += x;
        sumXX += VectorType(x.array() * x.array());

        maxV = maxV.array().cwiseMax(x.array());
        minV = minV.array().cwiseMin(x.array());
    }

    EIGEN_DEVICE_FUNC inline std::size_t count() const
    {
        return cnt;
    }

    EIGEN_DEVICE_FUNC inline VectorType mean() const
    {
        if(cnt > 0)
        {
            return sumX / Scalar(cnt);
        }
        else
        {
            return VectorType::Zero();
        }
    }

    EIGEN_DEVICE_FUNC inline VectorType variance() const
    {
        if(cnt > 0)
        {
            const Scalar bessel_correction = Scalar(cnt) / Scalar(cnt - 1);
            const VectorType cm = mean();
            const VectorType term1 = sumXX / Scalar(cnt);

            return bessel_correction * (term1 - VectorType(cm.array() * cm.array()));
        }
        else
        {
            return VectorType::Zero();
        }
    }

    EIGEN_DEVICE_FUNC inline VectorType stddev() const
    {
        return sqrt(variance().array());
    }

    EIGEN_DEVICE_FUNC inline VectorType running_sum() const { return sumX; }

    EIGEN_DEVICE_FUNC inline VectorType min_value() const { return minV; }
    EIGEN_DEVICE_FUNC inline VectorType max_value() const { return maxV; }

    EIGEN_DEVICE_FUNC inline types::Gaussian<VectorType> gaussian() const
    {
        return types::Gaussian<VectorType>(mean(), variance());
    }
private:
    std::size_t cnt;
    VectorType sumX, sumXX, minV, maxV;
};

// ******************** RUNNING MULTIVARIATE *******************************

/**
 * Multivariate analysis.
 */
template<typename T>
class MultivariateStats;

template<typename _Scalar, int Rows, int Options>
class MultivariateStats<Eigen::Matrix<_Scalar, Rows, 1, Options>>
{
public:
    typedef Eigen::Matrix<_Scalar, Rows, 1, Options> VectorType;
    typedef Eigen::Matrix<_Scalar, Rows, Rows, Options> CovarianceType;
    typedef _Scalar Scalar;
    static constexpr std::size_t Dimensions = Rows;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline MultivariateStats()
    {
        reset();
    }

    EIGEN_DEVICE_FUNC inline void reset()
    {
        cnt = 0;
        minV = VectorType::Constant(std::numeric_limits<Scalar>::max());
        maxV = VectorType::Constant(-std::numeric_limits<Scalar>::max());
        sumX = VectorType::Zero();
        sumXX = CovarianceType::Zero();
    }

    EIGEN_DEVICE_FUNC inline void operator()(const VectorType& x)
    {
        // process
        cnt++;

        sumX += x;

        sumXX += (x * x.transpose());

        maxV = maxV.array().cwiseMax(x.array());
        minV = minV.array().cwiseMin(x.array());
    }

    EIGEN_DEVICE_FUNC inline std::size_t count() const
    {
        return cnt;
    }

    EIGEN_DEVICE_FUNC inline VectorType mean() const
    {
        if(cnt > 0)
        {
            return sumX / Scalar(cnt);
        }
        else
        {
            return VectorType::Zero();
        }
    }

    EIGEN_DEVICE_FUNC inline CovarianceType covariance() const
    {
        if(cnt > 0)
        {
            const Scalar bessel_correction = Scalar(cnt) / Scalar(cnt - 1);
            const VectorType m = mean();
            return bessel_correction * ( (sumXX / Scalar(cnt)) - (m * m.transpose()) );
        }
        else
        {
            return CovarianceType::Zero();
        }
    }

    EIGEN_DEVICE_FUNC inline VectorType running_sum() const { return sumX; }

    EIGEN_DEVICE_FUNC inline VectorType min_value() const { return minV; }
    EIGEN_DEVICE_FUNC inline VectorType max_value() const { return maxV; }

    EIGEN_DEVICE_FUNC inline types::MultivariateGaussian<Scalar,Rows> gaussian() const
    {
        return types::MultivariateGaussian<Scalar,Rows>(mean(), covariance());
    }
private:
    std::size_t cnt;
    VectorType sumX, minV, maxV;
    CovarianceType sumXX;
};

// ******************** ALLAN VARIANCE *******************************
// From: http://www.leapsecond.com/tools/adev_lib.c

template<typename InputIterator>
typename std::iterator_traits<InputIterator>::value_type allanDeviation(std::size_t tau,
                                                                        InputIterator itbegin,
                                                                        InputIterator itend,
                                                                        bool isoverlapping = false,
                                                                        std::size_t* terms = nullptr)
{
    typedef typename std::iterator_traits<InputIterator>::value_type T;
    using namespace std;

    std::size_t n = 0;
    T sum = T(0.0);
    const std::size_t stride = isoverlapping ? 1 : tau;

    for ( ; (itbegin + 2 * tau) < itend ; itbegin += stride)
    {
        const T v = *(itbegin + 2 * tau) - T(2.0) * *(itbegin + tau) + *itbegin;
        sum += v * v;
        n += 1;
    }

    sum /= T(2.0);

    if(terms != nullptr) { *terms = n; }

    if (n < 3) { return T(0.0); }

    return sqrt(sum / n) / tau;
}

template<typename InputIterator>
typename std::iterator_traits<InputIterator>::value_type hadamardDeviation(std::size_t tau,
                                                                           InputIterator itbegin,
                                                                           InputIterator itend,
                                                                           bool isoverlapping = false,
                                                                           std::size_t* terms = nullptr)
{
    typedef typename std::iterator_traits<InputIterator>::value_type T;
    using namespace std;

    std::size_t n = 0;
    T sum = T(0.0);
    const std::size_t stride = isoverlapping ? 1 : tau;

    for ( ; (itbegin + 3 * tau) < itend; itbegin += stride)
    {
        const T v = *(itbegin + 3 * tau) - T(3.0) * *(itbegin + 2 * tau) + T(3.0) * *(itbegin + tau) - *itbegin;
        sum += v * v;
        n += 1;
    }

    sum /= T(6.0);

    if(terms != nullptr) { *terms = n; }

    if (n < 3) { return T(0.0); }

    return sqrt(sum / n) / tau;
}

template<typename InputIterator>
typename std::iterator_traits<InputIterator>::value_type modifiedAllanDeviation(std::size_t tau,
                                                                                InputIterator itbegin,
                                                                                InputIterator itend,
                                                                                std::size_t* terms = nullptr)
{
    typedef typename std::iterator_traits<InputIterator>::value_type T;
    using namespace std;

    std::size_t n = 0;
    T sum = T(0.0), v = T(0.0);

    InputIterator orgbegin = itbegin;

    for ( ; (itbegin + 2 * tau) < itend && (std::size_t)std::distance(orgbegin, itbegin) < tau; ++itbegin)
    {
        v += *(itbegin + 2 * tau) - T(2.0) * *(itbegin + tau) + *itbegin;
    }

    sum += v * v;
    n += 1;

    itbegin = orgbegin;

    for ( ; (itbegin + 3 * tau) < itend; ++itbegin)
    {
        v += *(itbegin + 3 * tau) - T(3.0) * *(itbegin + 2 * tau) + T(3.0) * *(itbegin + tau) - *itbegin;
        sum += v * v;
        n += 1;
    }

    sum /= T(2.0) * T(tau) * T(tau);

    if(terms != nullptr) { *terms = n; }
    if(n < 3) { return T(0.0); }

    return sqrt(sum / n) / tau;
}

template<typename InputIterator>
typename std::iterator_traits<InputIterator>::value_type timeDeviation(std::size_t tau,
                                                                       InputIterator itbegin,
                                                                       InputIterator itend,
                                                                       std::size_t* terms = nullptr)
{
    typedef typename std::iterator_traits<InputIterator>::value_type T;
    using namespace std;
    return T(tau) * modifiedAllanDeviation(tau, itbegin, itend, terms) / sqrt(3.0);
}

// ******************** PEARSON PRODUCT MOMENT *******************************

/**
 * Calculate r_xy from data.
 */
template<typename T>
class PearsonProductMoment;

template<typename Derived, typename BaseType>
class PearsonProductMomentBase
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC void reset()
    {
        stats_x.reset();
        stats_x2.reset();
        stats_y.reset();
        stats_y2.reset();
        stats_xy.reset();
    }

    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsX() const { return stats_x; }
    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsY() const { return stats_y; }
    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsX2() const { return stats_x2; }
    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsY2() const { return stats_y2; }
    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsXY() const { return stats_xy; }
protected:
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsX() { return stats_x; }
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsY() { return stats_y; }
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsX2() { return stats_x2; }
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsY2() { return stats_y2; }
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsXY() { return stats_xy; }

private:
    RunningStats<BaseType> stats_x;
    RunningStats<BaseType> stats_y;
    RunningStats<BaseType> stats_xy;
    RunningStats<BaseType> stats_x2;
    RunningStats<BaseType> stats_y2;
};

template<typename T>
class PearsonProductMoment : public PearsonProductMomentBase<PearsonProductMoment<T>, T>
{
    typedef PearsonProductMomentBase<PearsonProductMoment<T>, T> Base;
public:
    typedef T BaseType;
    typedef types::Polynomial<BaseType,1> PolynomialT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline PearsonProductMoment()
    {
        Base::reset();
    }

    EIGEN_DEVICE_FUNC void operator()(const T& x, const T& y)
    {
        Base::statsX()(x);
        Base::statsX2()(x*x);
        Base::statsY()(y);
        Base::statsY2()(y*y);
        Base::statsXY()(x*y);
    }

    EIGEN_DEVICE_FUNC BaseType result() const
    {
        using Eigen::numext::sqrt;
        return (Base::statsXY().mean() - (Base::statsX().mean() * Base::statsY().mean())) /
                sqrt( ( Base::statsX2().mean() - ( Base::statsX().mean() * Base::statsX().mean() ) ) *
                      ( Base::statsY2().mean() - ( Base::statsY().mean() * Base::statsY().mean() ) ) );
    }

    EIGEN_DEVICE_FUNC inline BaseType coefficientOfDetermination() const
    {
        BaseType r = result();
        return r*r;
    }

    EIGEN_DEVICE_FUNC inline PolynomialT lineEquation() const
    {
        PolynomialT ret;
        BaseType b = result() * ( Base::statsY().stddev() / Base::statsX().stddev() );
        BaseType a = Base::statsY().mean() - b * Base::statsX().mean();
        ret.coeff() << a , b;
        return ret;
    }
};

template<typename _Scalar, int _Rows, int _Cols>
class PearsonProductMoment<Eigen::Matrix<_Scalar, _Rows, _Cols> > :
  public PearsonProductMomentBase<PearsonProductMoment<Eigen::Matrix<_Scalar, _Rows, _Cols> > , Eigen::Matrix<_Scalar, _Rows, _Cols>>
{
    typedef PearsonProductMomentBase<PearsonProductMoment<Eigen::Matrix<_Scalar, _Rows, _Cols> > , Eigen::Matrix<_Scalar, _Rows, _Cols>> Base;
public:
    typedef Eigen::Matrix<_Scalar, _Rows, _Cols> BaseType;
    typedef types::Polynomial<BaseType,1> PolynomialT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline PearsonProductMoment()
    {
        Base::reset();
    }

    EIGEN_DEVICE_FUNC void operator()(const BaseType& x, const BaseType& y)
    {
        Base::statsX()(x);
        Base::statsX2()(x.array() * x.array());
        Base::statsY()(y);
        Base::statsY2()(y.array() * y.array());
        Base::statsXY()(x.array() * y.array());
    }

    EIGEN_DEVICE_FUNC BaseType result() const
    {
        using Eigen::numext::sqrt;

        return (Base::statsXY().mean().array() - (Base::statsX().mean().array() * Base::statsY().mean().array())) /
                sqrt( ( Base::statsX2().mean().array() - ( Base::statsX().mean().array() * Base::statsX().mean().array() ) ) *
                      ( Base::statsY2().mean().array() - ( Base::statsY().mean().array() * Base::statsY().mean().array() ) ) );
    }

    EIGEN_DEVICE_FUNC inline BaseType coefficientOfDetermination() const
    {
        BaseType r = result();
        return r.array() * r.array();
    }

    EIGEN_DEVICE_FUNC inline PolynomialT lineEquation()
    {
        PolynomialT ret;
        BaseType b = result().array() * ( Base::statsY().stddev().array() / Base::statsX().stddev().array() );
        BaseType a = Base::statsY().mean().array() - b.array() * Base::statsX().mean().array();
        ret.coeff() << a , b;
        return ret;
    }
};

}

}

#endif // VISIONCORE_MATH_STATISTICS_HPP


//  boost sinc.hpp header file

//  (C) Copyright Hubert Holin 2001. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_SINC_HPP
#define BOOST_SINC_HPP

#include <cmath>
#include <limits>
#include <string>
#include <stdexcept>

#include <boost/config.hpp>

// These are the the "Sinus Cardinal" functions.

namespace boost
{
    namespace math
    {
#if        defined(__GNUC__) && (__GNUC__ < 3)
        // gcc 2.x ignores function scope using declarations,
        // put them in the scope of the enclosing namespace instead:

        using    ::std::abs;
        using    ::std::sqrt;
        using    ::std::sin;

        using    ::std::numeric_limits;
#endif    /* defined(__GNUC__) && (__GNUC__ < 3) */

        // This is the "Sinus Cardinal" of index Pi.

        template<typename T>
        inline T    sinc_pi(const T x)
        {
#ifdef    BOOST_NO_STDC_NAMESPACE
            using    ::abs;
            using    ::sin;
            using    ::sqrt;
#else    /* BOOST_NO_STDC_NAMESPACE */
            using    ::std::abs;
            using    ::std::sin;
            using    ::std::sqrt;
#endif    /* BOOST_NO_STDC_NAMESPACE */

            using    ::std::numeric_limits;

            static T const    taylor_0_bound = numeric_limits<T>::epsilon();
            static T const    taylor_2_bound = sqrt(taylor_0_bound);
            static T const    taylor_n_bound = sqrt(taylor_2_bound);

            if    (abs(x) >= taylor_n_bound)
            {
                return(sin(x)/x);
            }
            else
            {
                // approximation by taylor series in x at 0 up to order 0
                T    result = static_cast<T>(1);

                if    (abs(x) >= taylor_0_bound)
                {
                    T    x2 = x*x;

                    // approximation by taylor series in x at 0 up to order 2
                    result -= x2/static_cast<T>(6);

                    if    (abs(x) >= taylor_2_bound)
                    {
                        // approximation by taylor series in x at 0 up to order 4
                        result += (x2*x2)/static_cast<T>(120);
                    }
                }

                return(result);
            }
        }

#ifdef    BOOST_NO_TEMPLATE_TEMPLATES
#else    /* BOOST_NO_TEMPLATE_TEMPLATES */
        template<typename T, template<typename> class U>
        inline U<T>    sinc_pi(const U<T> x)
        {
#ifdef    BOOST_NO_STDC_NAMESPACE
            using    ::abs;
            using    ::sin;
            using    ::sqrt;
#else    /* BOOST_NO_STDC_NAMESPACE */
            using    ::std::abs;
            using    ::std::sin;
            using    ::std::sqrt;
#endif    /* BOOST_NO_STDC_NAMESPACE */

            using    ::std::numeric_limits;

            static T const    taylor_0_bound = numeric_limits<T>::epsilon();
            static T const    taylor_2_bound = sqrt(taylor_0_bound);
            static T const    taylor_n_bound = sqrt(taylor_2_bound);

            if    (abs(x) >= taylor_n_bound)
            {
                return(sin(x)/x);
            }
            else
            {
                // approximation by taylor series in x at 0 up to order 0
                U<T>    result = static_cast< U<T> >(1);

                if    (abs(x) >= taylor_0_bound)
                {
                    U<T>    x2 = x*x;

                    // approximation by taylor series in x at 0 up to order 2
                    result -= x2/static_cast<T>(6);

                    if    (abs(x) >= taylor_2_bound)
                    {
                        // approximation by taylor series in x at 0 up to order 4
                        result += (x2*x2)/static_cast<T>(120);
                    }
                }

                return(result);
            }
        }
#endif    /* BOOST_NO_TEMPLATE_TEMPLATES */
    }
}

#endif /* BOOST_SINC_HPP */


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

#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/io/gmsh_reader.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/refinement/refinement.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <cmath>
#include <iostream>
#include <memory>
#include <vector>

#include "upwindfinitevolume.h"

int main() {
  /* SAM_LISTING_BEGIN_1 */
#if SOLUTION
  // Read in mesh
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory),
                            CURRENT_SOURCE_DIR "/mesh.msh");

  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader.mesh();

  double eps = 1e-6;

  auto v = [](Eigen::Vector2d x) -> Eigen::Vector2d {
    return Eigen::Vector2d(1, 0.5 - x[1]);
  };

  auto u = [](Eigen::Vector2d x) -> double {
    return std::sin(4 * M_PI * x[1]) * std::exp(-x[0]);
  };

  auto f = [&u, eps](Eigen::Vector2d x) -> double {
    return eps * (1.0 - 16.0 * M_PI * M_PI) * u(x) - 2 * u(x) +
           (0.5 - x[1]) * 4 * M_PI * std::cos(4.0 * M_PI * x[1]) *
               std::exp(-x[0]);
  };

  // Convergence study
  auto mesh_seq_p{
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh_p, 6)};
  int num_meshes = mesh_seq_p->NumLevels();

  std::vector<double> vec_l2error;
  std::vector<double> vec_Ndofs;
  for (int level = 0; level < num_meshes; ++level) {
    // Get the current mesh
    auto cur_mesh = mesh_seq_p->getMesh(level);

    // Create a DOF Hander for the current mesh
    const lf::assemble::UniformFEDofHandler cur_dofh(
        cur_mesh, {{lf::base::RefEl::kPoint(), 1},
                   {lf::base::RefEl::kSegment(), 0},
                   {lf::base::RefEl::kTria(), 0},
                   {lf::base::RefEl::kQuad(), 0}});
    int N_dofs = cur_dofh.NumDofs();

    lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);
    UpwindFiniteVolume::ElementMatrixProvider my_mat_provider(v, eps);
    lf::assemble::AssembleMatrixLocally(0, cur_dofh, cur_dofh, my_mat_provider,
                                        A);

    Eigen::VectorXd phi(N_dofs);
    phi.setZero();
    UpwindFiniteVolume::ElementVectorProvider my_vec_provider(f);
    lf::assemble::AssembleVectorLocally(0, cur_dofh, my_vec_provider, phi);

    // Dirichlet boundary condition
    auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(cur_mesh, 2)};
    auto my_selector = [&cur_dofh, &u, &bd_flags](unsigned int dof_idx) {
      if (bd_flags(cur_dofh.Entity(dof_idx))) {
        const lf::mesh::Entity &entity{cur_dofh.Entity(dof_idx)};
        const lf::geometry::Geometry *geo_p = entity.Geometry();
        const Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);
        Eigen::Vector2d corner = corners.col(0);
        return std::make_pair(true, u(corner));
      } else {
        return std::make_pair(false, 42.0);
      }
    };
    lf::assemble::FixFlaggedSolutionComponents<double>(my_selector, A, phi);

    const Eigen::SparseMatrix<double> A_crs = A.makeSparse();

    // Numerical Solution
    Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
    solver.compute(A_crs);
    if (solver.info() != Eigen::Success) {
      printf("Decomposition Failed.\n");
    }
    Eigen::VectorXd sol_vec = solver.solve(phi);

    // Exact Solution
    Eigen::VectorXd exact_sol(N_dofs);
    for (int i = 0; i < N_dofs; ++i) {
      const lf::mesh::Entity &entity{cur_dofh.Entity(i)};
      const lf::geometry::Geometry *geo_p = entity.Geometry();
      const Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);
      exact_sol[i] = u(corners.col(0));
    }

    // L2 Error
    double l2_error = (sol_vec - exact_sol).norm() / N_dofs;
    vec_l2error.push_back(l2_error);
    vec_Ndofs.push_back(N_dofs);
  }

  for (int i = 0; i < vec_l2error.size(); ++i) {
    if (i > 0) {
      double conv_rate =
          (std::log(vec_l2error.at(i - 1)) - std::log(vec_l2error.at(i))) /
          (std::log(vec_Ndofs.at(i)) - std::log(vec_Ndofs.at(i - 1)));
      std::cout << "N_dofs: " << vec_Ndofs.at(i)
                << "; L2 Error: " << vec_l2error.at(i)
                << "; Conv. rate: " << conv_rate << std::endl;
    } else {
      std::cout << "N_dofs: " << vec_Ndofs.at(i)
                << "; L2 Error: " << vec_l2error.at(i) << std::endl;
    }
  }
  /* SAM_LISTING_END_1 */

#else
  //====================
  // Your code goes here
  //====================
#endif
  return 0;
}


#pragma once
#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>

namespace ppl {
namespace math {

inline size_t padded_length(size_t N)
{
    return std::pow(2, std::ceil(std::log(N)/std::log(2.)));
}

/**
 * Computes autocorrelation of x where each column of x
 * is a component of a process and hence each row is a time point.
 * More mathematically, x(i,...) is the process value at time i.
 *
 * For more detail, see:
 * https://lingpipe-blog.com/2012/06/08/autocorrelation-fft-kiss-eigen/
 * https://github.com/stan-dev/math/blob/41e548e19da5675121c245b535d4019c8bbd754b/stan/math/prim/mat/fun/autocorrelation.hpp
 *
 * @tparam  T   underlying value type (usually double)
 * @param   x   process matrix
 */
template <class T>
inline auto autocorrelation(const Eigen::MatrixBase<T>& x)
{
    using scalar_t = typename T::Scalar;
    using complex_t = std::complex<scalar_t>;

    size_t n_rows = x.rows();
    size_t padded_len = 2*padded_length(n_rows);

    Eigen::Matrix<scalar_t, Eigen::Dynamic, Eigen::Dynamic> out(x.rows(), x.cols());

    Eigen::FFT<scalar_t> fft;

    for (int i = 0; i < x.cols(); ++i) {

        // create centered copy of x
        Eigen::Matrix<scalar_t, Eigen::Dynamic, 1> x_cent(padded_len);
        x_cent.setZero();
        x_cent.head(n_rows) = x.col(i).array() - x.col(i).mean();

        // FFT
        Eigen::Matrix<complex_t, Eigen::Dynamic, 1> freq(padded_len);
        fft.fwd(freq, x_cent);

        // compute complex-norm element-wise
        freq = freq.array().abs2();

        // inverse FFT and trim to shape of x
        Eigen::Matrix<complex_t, Eigen::Dynamic, 1> ifreq(padded_len);
        fft.inv(ifreq, freq);

        // get autocorrelation by normalizing by variance
        out.col(i) = ifreq.head(n_rows).real() / (n_rows * n_rows * 2.);
        out.col(i) /= out(0,i);

    }

    return out;
}

} // namespace math
} // namespace ppl


#include "adapter_creator.hpp"
#include "experiment_helpers.hpp"
#include <Eigen/SVD>
#include <iostream>

void create_random_pose(opengv::rotation_t & rotation, opengv::translation_t & position, const double & noise, const double & outlierFraction, const size_t & numberPoints, const int & numberCameras,opengv::bearingVectors_t & bearingVectors, std::vector<int> & camCorrespondences, opengv::points_t & points, opengv::translations_t & camOffsets, opengv::rotations_t & camRotations){

  //create a random camera-system
  opengv::generateRandomCameraSystem( numberCameras, camOffsets, camRotations );

  //derive correspondences based on random point-cloud
  Eigen::MatrixXd gt(3,numberPoints);
  opengv::generateRandom2D3DCorrespondences(position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction, bearingVectors, points, camCorrespondences, gt );

  //print the experiment characteristics
  opengv::printExperimentCharacteristics(
				 position, rotation, noise, outlierFraction );
}

void choose_best_transformation(const opengv::transformations_t &solutions, const opengv::transformation_t & ref, opengv::transformation_t & transformation_method){
  double error = 1e7;
  int index = 0;
  for(int i = 0; i < solutions.size(); ++i){
    if(error > (ref - solutions[i] ).norm()){
      error = (ref - solutions[i] ).norm();
      index = i;
    }
  }
  transformation_method = solutions[index];
 }

void choose_best_essential_matrix(const opengv::essentials_t & solutions, const opengv::essential_t & ref, opengv::essential_t & essential_matrix){
  if(solutions.size() == 1){
    essential_matrix = solutions[0];
  }
  double error = 1e7;
  int index = 0;
  for(int i = 0; i < solutions.size(); ++i){
    if(error > (ref - solutions[i]).norm()){
      error = (ref - solutions[i]).norm();
      index = i;
    }
  }
  essential_matrix = solutions[index];
}

opengv::essential_t calculate_essential_matrix(const opengv::translation_t & position, const opengv::rotation_t & rotation)
{
  //E transforms vectors from vp 2 to 1: x_1^T * E * x_2 = 0
  //and E = (t)_skew*R
  Eigen::Matrix3d t_skew = Eigen::Matrix3d::Zero();
  t_skew(0,1) = -position[2];
  t_skew(0,2) = position[1];
  t_skew(1,0) = position[2];
  t_skew(1,2) = -position[0];
  t_skew(2,0) = -position[1];
  t_skew(2,1) = position[0];
  opengv::essential_t E = t_skew * rotation;
  std::cout << "the random essential matrix is:" << std::endl;
  std::cout << E << std::endl;
  return E;
}

opengv::transformation_t calculate_transformation_from_essential_matrix(const opengv::essential_t & essential_matrix, const opengv::rotation_t &ref_rotation, const opengv::translation_t & ref_translation){
  //Obtain matrices U and V
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(essential_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  Eigen::Matrix3d sing_values = Eigen::Matrix3d::Zero(3,3);
  sing_values(0,0) = svd.singularValues()(0,0);
  sing_values(1,1) = svd.singularValues()(1,0);

  double detU = U.determinant();
  if (detU < 0){
    U = -U;
  }

  double detV = V.determinant();
  if (detV < 0){
    V = -V;
  }

  Eigen::Matrix3d Rz = Eigen::Matrix3d::Zero(3,3);
  Rz(0,1) = -1;Rz(1,0) = 1; Rz(2,2) = 1;

  opengv::rotation_t r = U * Rz.transpose() * V.transpose();
  Eigen::MatrixXd skew_t = U * Rz * sing_values * U.transpose();
  Eigen::MatrixXd t = Eigen::MatrixXd::Zero(3,1);
  t(0,0) = skew_t(2,1);t(1,0) = skew_t(0,2); t(2,0) = skew_t(1,0);

  if ( (r - ref_rotation).norm() > 1e-6){
    r = U * Rz * V.transpose();
  }
  if( (t - ref_translation).norm() > 1e-6){
    skew_t = U * Rz.transpose() * sing_values * U.transpose();
    t(0,0) = skew_t(2,1);t(1,0) = skew_t(0,2); t(2,0) = skew_t(1,0);
  }

  opengv::transformation_t result;
  result.block<3,3>(0,0) = r;
  result.block<3,1>(0,3) = t;
  return result;
}


#include <Eigen/StdVector>

#include <pcl/kdtree/kdtree_flann.h>
#include <week_scanmatching/icp/icp.h>
#include <Eigen/Core>

namespace scanmatcher
{
Result ICP::scanmatch(const pcl::PointCloud<pcl::PointXYZ>& input, const pcl::PointCloud<pcl::PointXYZ>& target,
                      const Eigen::Isometry3d& prediction)
{
  // Transformation is sR(p_i) + t
  Eigen::Isometry3d total_transform = Eigen::Isometry3d::Identity();
  double error = std::numeric_limits<double>::infinity();

  // TODO(Oswin) Parametrize this
  int max_iterations = 500;
  double threshold = 1e-5;
  double correspondance_outlier_threshold = 0.5;

  int num_input = input.points.size();

  Eigen::Matrix3Xd aligned_input(3, num_input);
  for (int i = 0; i < num_input; i++)
  {
    aligned_input.col(i) = input[i].getVector3fMap().cast<double>();
  }

  Eigen::Matrix3Xd target_corresp(3, num_input);

  // Construct KD tree for target for finding closest points
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  *target_ptr = target;
  kdtree.setInputCloud(target_ptr);

  for (int i = 0; i < max_iterations; i++)
  {
    // TODO(Oswin) Make this a KD tree
    std::vector<int> inlier_indices{};
    for (int j = 0; j < num_input; j++)
    {
      pcl::PointXYZ pcl_point;
      pcl_point.getVector3fMap() = aligned_input.col(j).cast<float>();
      std::vector<int> closest_idx;
      std::vector<float> distance;
      kdtree.nearestKSearch(pcl_point, 1, closest_idx, distance);
      auto closest_point = target[closest_idx.front()];
      target_corresp.col(j) = closest_point.getVector3fMap().cast<double>();

      if (std::sqrt(distance.front()) < correspondance_outlier_threshold)
      {
        inlier_indices.emplace_back(j);
      }
    }

    int num_inliers = inlier_indices.size();
    if (num_inliers < 4)
    {
      return { Eigen::Affine3d::Identity(), std::numeric_limits<double>::infinity() };
    }
    Eigen::Matrix3Xd inlier_input(3, num_inliers);
    Eigen::Matrix3Xd inlier_corresp(3, num_inliers);

    for (int idx = 0; idx < num_inliers; idx++)
    {
      int inlier_index = inlier_indices[idx];
      inlier_input.col(idx) = aligned_input.col(inlier_index);
      inlier_corresp.col(idx) = target_corresp.col(inlier_index);
    }

    Eigen::VectorXd weights = Eigen::VectorXd::Ones(num_inliers);
    Eigen::Isometry3d transform = getRigidTransform(weights, inlier_input, inlier_corresp);
    total_transform = transform * total_transform;

    error = 0.0;
    for (int j = 0; j < num_input; j++)
    {
      aligned_input.col(j) = transform * aligned_input.col(j);
      error += (target_corresp.col(j) - aligned_input.col(j)).squaredNorm();
    }
    error /= num_input;

    if (error < threshold)
    {
      break;
    }
  }

  Eigen::Affine3d affine_transform{ total_transform };
  return { affine_transform, error };
}

Eigen::Isometry3d ICP::getRigidTransform(const Eigen::VectorXd& weights, const Eigen::Matrix3Xd& input,
                                         const Eigen::Matrix3Xd& target) const
{
  const int dims = input.cols();

  // 1: Compute weighted centroids of input and target
  const double weights_sum_inverse = 1.0 / weights.sum();
  const Eigen::Vector3d input_centroid =
      (input.array().rowwise() * weights.array().transpose()).rowwise().sum() * weights_sum_inverse;
  const Eigen::Vector3d target_centroid =
      (target.array().rowwise() * weights.array().transpose()).rowwise().sum() * weights_sum_inverse;

  // 2: Subtract centroids from input and target
  const Eigen::Matrix3Xd input_prime = input.colwise() - input_centroid;
  const Eigen::Matrix3Xd target_prime = target.colwise() - target_centroid;

  // 3: Compute S = X W Y^T
  const Eigen::MatrixXd s = target_prime * weights.asDiagonal() * input_prime.transpose();
  const Eigen::JacobiSVD<Eigen::MatrixXd> svd(s, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::Matrix3d rotation_matrix = svd.matrixV() * svd.matrixU().transpose();

  if (rotation_matrix.determinant() < 0)
  {
    Eigen::MatrixXd altered_v = svd.matrixV();
    altered_v.col(dims) *= -1;
    rotation_matrix = altered_v * svd.matrixU().transpose();
  }

  Eigen::Isometry3d transform{ rotation_matrix.transpose() };
  transform.translation() = target_centroid - rotation_matrix * input_centroid;

  return transform;
}
}  // namespace scanmatcher


#include "RasRandomNumber.h"

#include <chrono> // for seconds, milliseconds, nanoseconds, picoseconds
#include <Eigen/Cholesky>
using namespace Eigen;

unsigned RasRandomNumber::ras_now_nanoseconds(void)
{
    std::chrono::nanoseconds ns = std::chrono::duration_cast< std::chrono::nanoseconds >(std::chrono::system_clock::now().time_since_epoch());
    return unsigned(ns.count() % 100000000)+std::rand();
}

std::vector<std::vector<double> > RasRandomNumber::ras_mvnorm(unsigned long int n, std::vector<double> &mu, std::vector<std::vector<double> > &corr, unsigned seed)
{
    if (seed==0) seed=ras_now_nanoseconds();
    std::default_random_engine generator(seed);
    std::normal_distribution<double> distribution(0.0,1.0);

    // chol is cholesky decomposition, should be square
    int ncol=(int)corr.size();

    MatrixXd A(ncol,ncol);
    for (int i=0; i<ncol; i++)
        for (int j=0; j<ncol; j++)
            A(i,j)=corr[i][j];

    MatrixXd U = A.llt().matrixU();
    std::vector<std::vector<double> > chol(ncol,std::vector<double> (ncol,0) );
    for (int i=0; i<ncol; i++)
        for (int j=0; j<ncol; j++)
            chol[i][j]=U(i,j);

    // initialization
    std::vector<std::vector<double> > ret(n,std::vector<double> (ncol,0));
    std::vector<std::vector<double> > z  (n,std::vector<double> (ncol,0));

    for (unsigned long int i=0; i<n; i++){
        for (int j=0; j<ncol; j++){
            z[i][j] = distribution(generator); // standard normal rv
        }
    }
    std::vector<std::vector<double> > C= RasMatrix::ras_prod_mat(z,chol);
    for (unsigned long int i=0; i<n; i++){
        for (int j=0; j<ncol; j++){
            ret[i][j]=mu[j]+C[i][j];
        }
    }
    return ret;
}

std::vector<int> RasRandomNumber::ras_rpois(unsigned long int n, double lam, unsigned seed)
{
    if (seed==0) seed=ras_now_nanoseconds();
    std::default_random_engine generator(seed);
    std::poisson_distribution<int> distribution(lam);

    std::vector<int> number(n);
    for (unsigned long int i=0; i<n; i++)
        number[i] = distribution(generator);
    return number;
}

int RasRandomNumber::ras_rpois(double lam, unsigned seed)
{
    //    std::default_random_engine generator(time(0));
    if (seed==0) seed=ras_now_nanoseconds();
    std::default_random_engine generator(seed);
    std::poisson_distribution<int> distribution(lam);

    int number = distribution(generator);
    return number;
}

double RasRandomNumber::ras_uniform(unsigned seed)
{
    if (seed==0) seed=ras_now_nanoseconds();
    static std::default_random_engine re(seed);
    static std::uniform_real_distribution<double> Dist(0,1);
    return Dist(re);
}

std::vector<unsigned long int> RasRandomNumber::ras_SampleWithoutReplacement(unsigned long int populationSize, unsigned long int sampleSize, unsigned seed)
{
    if (seed==0) seed=ras_now_nanoseconds();
    static std::default_random_engine re(seed);
    static std::uniform_real_distribution<double> Dist(0,1);

    // Use Knuth's variable names
    std::vector<unsigned long int> samples(sampleSize);
    int n = sampleSize;
    int N = populationSize;

    int t = 0; // total input records dealt with
    int m = 0; // number of items selected so far
    double u;

    while (m < n)
    {
        u = Dist(re); // call a uniform(0,1) random number generator

        if ( (N - t)*u >= n - m )
        {
            t++;
        }
        else
        {
            samples[m] = t;
            t++; m++;
        }
    }
    return samples;
}


#pragma once
// gauss_jordan.hpp: Gauss-Jordan matrix inversion
//
// Copyright (C) 2017-2019 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-BLAS project, which is released under an MIT Open Source license.

#include <boost/numeric/mtl/mtl.hpp>

namespace sw {
namespace hprblas {

template<typename Matrix>
Matrix GaussJordanInversion(const Matrix& A) {
	using Scalar = typename mtl::Collection<Matrix>::value_type;

	size_t m = A.num_rows();
	size_t n = A.num_cols();
	Matrix a(n, m), inv(m, n);
	a = A; // you need a deep copy
	inv = Scalar(1);

	// Performing elementary operations
	for (unsigned i = 0; i < m; ++i) {
		if (a[i][i] == 0) {
			unsigned c = 1;
			while (a[i + c][i] == 0 && (i + c) < n)	++c;
			if ((i + c) == n) break;

			for (unsigned j = i, k = 0; k < n; ++k) {
				std::swap(a[j][k], a[j + c][k]);
				std::cerr << "TBD" << std::endl; // need to create a permutation matrix
			}
		}
		// transform to diagonal matrix
		for (unsigned j = 0; j < m; j++) {
			if (i != j) {
				Scalar scale = a[j][i] / a[i][i];
				for (unsigned k = 0; k < n; ++k) {
					a[j][k] = a[j][k] - a[i][k] * scale;
					inv[j][k] = inv[j][k] - inv[i][k] * scale;
				}
			}
			//std::cout << i << "," << j << std::endl;
			//sw::hprblas::printMatrix(std::cout, "a", a);
			//sw::hprblas::printMatrix(std::cout, "inv", inv);
		}
	}
	// transform to identity matrix
	for (unsigned i = 0; i < m; ++i) {
		Scalar normalize = a[i][i];
		a[i][i] = Scalar(1);
		for (unsigned j = 0; j < n; ++j) {
			inv[i][j] /= normalize;
		}
	}
	//printMatrix(std::cout, "conversion", a);
	return inv;
}

} // namespace hprblas
} // namespace sw


///////////////////////////////////////////////////////////////////////////////
//      Copyright Christopher Kormanyos 2012 - 2015, 2020.
// 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)
//

// This example uses Boost.Multiprecision to implement
// a high-precision Gauss-Laguerre quadrature integration.
// The quadrature is used to calculate the airy_ai(x) function
// for real-valued x on the positive axis with x.ge.1.

// In this way, the integral representation could be seen
// as part of a scheme to calculate real-valued Airy functions
// on the positive axis for medium to large argument.
// A Taylor series or hypergeometric function (not part
// of this example) could be used for smaller arguments.

// This example has been tested with decimal digits counts
// ranging from 21...301, by adjusting the parameter
// local::my_digits10 at compile time.

// The quadrature integral representaion of airy_ai(x) used
// in this example can be found in:

// A. Gil, J. Segura, N.M. Temme, "Numerical Methods for Special
// Functions" (SIAM Press 2007), Sect. 5.3.3, in particular Eq. 5.110,
// page 145. Subsequently, Gil et al's book cites the another work:
// W. Gautschi, "Computation of Bessel and Airy functions and of
// related Gaussian quadrature formulae", BIT, 42 (2002), pp. 110-118.

#include <cmath>
#include <cstdint>
#include <functional>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <sstream>
#include <tuple>
#include <vector>

#include <boost/cstdfloat.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/noncopyable.hpp>

namespace gauss { namespace laguerre {

namespace util {

void progress_bar(std::ostream& os, const float percent);

void progress_bar(std::ostream& os, const float percent)
{
  std::stringstream strstrm;

  strstrm.precision(1);

  strstrm << std::fixed << percent << "%";

  os << strstrm.str() << "\r";

  os.flush();
}

}

namespace detail
{
  template<typename T>
  class laguerre_l_object BOOST_FINAL
  {
  public:
    laguerre_l_object(const int n, const T a) noexcept
      : order(n),
        alpha(a),
        p1   (0),
        d2   (0) { }

    laguerre_l_object& operator=(const laguerre_l_object& other)
    {
      if(this != other)
      {
        order = other.order;
        alpha = other.alpha;
        p1    = other.p1;
        d2    = other.d2;
      }

      return *this;
    }

    T operator()(const T& x) const noexcept
    {
      // Calculate (via forward recursion):
      // * the value of the Laguerre function L(n, alpha, x), called (p2),
      // * the value of the derivative of the Laguerre function (d2),
      // * and the value of the corresponding Laguerre function of
      //   previous order (p1).

      // Return the value of the function (p2) in order to be used as a
      // function object with Boost.Math root-finding. Store the values
      // of the Laguerre function derivative (d2) and the Laguerre function
      // of previous order (p1) in class members for later use.

        p1 = T(0);
      T p2 = T(1);
        d2 = T(0);

      T j_plus_alpha = alpha;
      T two_j_plus_one_plus_alpha_minus_x = (1 + alpha) - x;

      const T my_two = 2;

      for(int j = 0; j < order; ++j)
      {
        const T p0(p1);

        // Set the value of the previous Laguerre function.
        p1 = p2;

        // Use a recurrence relation to compute the value of the Laguerre function.
        p2 = ((two_j_plus_one_plus_alpha_minus_x * p1) - (j_plus_alpha * p0)) / (j + 1);

        ++j_plus_alpha;
        two_j_plus_one_plus_alpha_minus_x += my_two;
      }

      // Set the value of the derivative of the Laguerre function.
      d2 = ((p2 * order) - (j_plus_alpha * p1)) / x;

      // Return the value of the Laguerre function.
      return p2;
    }

    const T previous  () const noexcept { return p1; }
    const T derivative() const noexcept { return d2; }

    static bool root_tolerance(const T& a, const T& b) noexcept
    {
      using std::fabs;

      // The relative tolerance here is: ((a - b) * 2) / (a + b).
      return ((fabs(a - b) * 2) < (fabs(a + b) * std::numeric_limits<T>::epsilon()));
    }

  private:
    const   int order;
    const   T   alpha;
    mutable T   p1;
    mutable T   d2;
  };

  template<typename T>
  class abscissas_and_weights : private boost::noncopyable
  {
  public:
    abscissas_and_weights(const int n, const T a) : order(n),
                                                    alpha(a),
                                                    xi   (),
                                                    wi   ()
    {
      if(alpha < -20.0F)
      {
        // Users can decide to perform different range checking.
        std::cout << "Range error: the order of the Laguerre function must exceed -20.0."
                  << std::endl;
      }
      else
      {
        calculate();
      }
    }

    const std::vector<T>& abscissa_n() const noexcept { return xi; }
    const std::vector<T>& weight_n  () const noexcept { return wi; }

  private:
    const int order;
    const T   alpha;

    std::vector<T> xi;
    std::vector<T> wi;

    abscissas_and_weights() : order(),
                              alpha(),
                              xi   (),
                              wi   () { }

    void calculate()
    {
      using std::abs;

      std::cout << "Finding the approximate roots..." << std::endl;

      std::vector<std::tuple<T, T>> root_estimates;

      root_estimates.reserve(static_cast<typename std::vector<std::tuple<T, T>>::size_type>(order));

      const laguerre_l_object<T> laguerre_root_object(order, alpha);

      // Set the initial values of the step size and the running step
      // to be used for finding the estimate of the first root.
      T step_size  = 0.01F;
      T step       = step_size;

      T first_laguerre_root = 0.0F;

      if(alpha < -1.0F)
      {
        // Iteratively step through the Laguerre function using a
        // small step-size in order to find a rough estimate of
        // the first zero.

        const bool this_laguerre_value_is_negative = (laguerre_root_object(T(0)) < 0);

        constexpr int j_max = 10000;

        int j = 0;

        while((j < j_max) && (this_laguerre_value_is_negative != (laguerre_root_object(step) < 0)))
        {
          // Increment the step size until the sign of the Laguerre function
          // switches. This indicates a zero-crossing, signalling the next root.
          step += step_size;

          ++j;
        }
      }
      else
      {
        // Calculate an estimate of the 1st root of a generalized Laguerre
        // function using either a Taylor series or an expansion in Bessel
        // function zeros. The Bessel function zeros expansion is from Tricomi.

        // Here, we obtain an estimate of the first zero of cyl_bessel_j(alpha, x).

        T j_alpha_m1;

        if(alpha < 1.4F)
        {
          // For small alpha, use a short series obtained from Mathematica(R).
          // Series[BesselJZero[v, 1], {v, 0, 3}]
          // N[%, 12]
          j_alpha_m1 = (((          T(0.09748661784476F)
                          * alpha - T(0.17549359276115F))
                          * alpha + T(1.54288974259931F))
                          * alpha + T(2.40482555769577F));
        }
        else
        {
          // For larger alpha, use the first line of Eqs. 10.21.40 in the NIST Handbook.
          const T alpha_pow_third(boost::math::cbrt(alpha));
          const T alpha_pow_minus_two_thirds(T(1) / (alpha_pow_third * alpha_pow_third));

          j_alpha_m1 = alpha * (((((                             + T(0.043F)
                                    * alpha_pow_minus_two_thirds - T(0.0908F))
                                    * alpha_pow_minus_two_thirds - T(0.00397F))
                                    * alpha_pow_minus_two_thirds + T(1.033150F))
                                    * alpha_pow_minus_two_thirds + T(1.8557571F))
                                    * alpha_pow_minus_two_thirds + T(1.0F));
        }

        const T vf             = (  T(order * 4U)
                                  + T(alpha * 2U)
                                  + T(2U));
        const T vf2            = vf * vf;
        const T j_alpha_m1_sqr = j_alpha_m1 * j_alpha_m1;

        first_laguerre_root = (j_alpha_m1_sqr * (   -T(0.6666666666667F)
                                                 + ((T(0.6666666666667F) * alpha) * alpha)
                                                 +  (T(0.3333333333333F) * j_alpha_m1_sqr) + vf2)) / (vf2 * vf);
      }

      bool this_laguerre_value_is_negative = (laguerre_root_object(T(0)) < 0);

      // Re-set the initial value of the step-size based on the
      // estimate of the first root.
      step_size = first_laguerre_root / 2;
      step      = step_size;

      // Step through the Laguerre function using a step-size
      // of dynamic width in order to find the zero crossings
      // of the Laguerre function, providing rough estimates
      // of the roots. Refine the brackets with a few bisection
      // steps, and store the results as bracketed root estimates.

      while(root_estimates.size() < static_cast<std::size_t>(order))
      {
        // Increment the step size until the sign of the Laguerre function
        // switches. This indicates a zero-crossing, signalling the next root.
        step += step_size;

        if(this_laguerre_value_is_negative != (laguerre_root_object(step) < 0))
        {
          // We have found the next zero-crossing.

          // Change the running sign of the Laguerre function.
          this_laguerre_value_is_negative = (!this_laguerre_value_is_negative);

          // We have found the first zero-crossing. Put a loose bracket around
          // the root using a window. Here, we know that the first root lies
          // between (x - step_size) < root < x.

          // Before storing the approximate root, perform a couple of
          // bisection steps in order to tighten up the root bracket.
          std::uintmax_t a_couple_of_iterations = 4U;

          const std::pair<T, T>
            root_estimate_bracket = boost::math::tools::bisect(laguerre_root_object,
                                                               step - step_size,
                                                               step,
                                                               laguerre_l_object<T>::root_tolerance,
                                                               a_couple_of_iterations);

          static_cast<void>(a_couple_of_iterations);

          // Store the refined root estimate as a bracketed range in a tuple.
          root_estimates.push_back(std::tuple<T, T>(std::get<0>(root_estimate_bracket),
                                                    std::get<1>(root_estimate_bracket)));

          if(    (root_estimates.size() == 1U)
             || ((root_estimates.size() % 8U) == 0U)
             ||  (root_estimates.size() == static_cast<std::size_t>(order)))
          {
            const float progress = (100.0F * static_cast<float>(root_estimates.size())) / static_cast<float>(order);

            std::cout << "root_estimates.size(): "
                      << root_estimates.size()
                      << ", "
                      ;

            util::progress_bar(std::cout, progress);
          }

          if(root_estimates.size() >= static_cast<std::size_t>(2U))
          {
            // Determine the next step size. This is based on the distance between
            // the previous two roots, whereby the estimates of the previous roots
            // are computed by taking the average of the lower and upper range of
            // the root-estimate bracket.

            const T r0 = (  std::get<0>(*(root_estimates.crbegin() + 1U))
                          + std::get<1>(*(root_estimates.crbegin() + 1U))) / 2;

            const T r1 = (  std::get<0>(*root_estimates.crbegin())
                          + std::get<1>(*root_estimates.crbegin())) / 2;

            const T distance_between_previous_roots = r1 - r0;

            step_size = distance_between_previous_roots / 3;
          }
        }
      }

      const T norm_g =
        ((alpha == 0) ? T(-1)
                      : -boost::math::tgamma(alpha + order) / boost::math::factorial<T>(order - 1));

      xi.reserve(root_estimates.size());
      wi.reserve(root_estimates.size());

      std::cout << std::endl;

      // Calculate the abscissas and weights to full precision.
      for(std::size_t i = static_cast<std::size_t>(0U); i < root_estimates.size(); ++i)
      {
        if(   ((i % 8U) == 0U)
           || ( i == root_estimates.size() - 1U))
        {
          const float progress = (100.0F * static_cast<float>(i + 1U)) / static_cast<float>(root_estimates.size());

          std::cout << "Calculating abscissas and weights. Processed "
                    << (i + 1U)
                    << ", "
                    ;

          util::progress_bar(std::cout, progress);
        }

        // Calculate the abscissas using iterative root-finding.

        // Select the maximum allowed iterations, being at least 20.
        // The determination of the maximum allowed iterations is
        // based on the number of decimal digits in the numerical
        // type T.
        constexpr int local_math_tools_digits10 =
          static_cast<int>(static_cast<boost::float_least32_t>(boost::math::tools::digits<T>()) * BOOST_FLOAT32_C(0.301));

        const std::uintmax_t number_of_iterations_allowed = (std::max)(20, local_math_tools_digits10 / 2);

        std::uintmax_t number_of_iterations_used = number_of_iterations_allowed;

        // Perform the root-finding using ACM TOMS 748 from Boost.Math.
        const std::pair<T, T>
          laguerre_root_bracket = boost::math::tools::toms748_solve(laguerre_root_object,
                                                                    std::get<0>(root_estimates.at(i)),
                                                                    std::get<1>(root_estimates.at(i)),
                                                                    laguerre_l_object<T>::root_tolerance,
                                                                    number_of_iterations_used);

        static_cast<void>(number_of_iterations_used);

        // Compute the Laguerre root as the average of the values from
        // the solved root bracket.
        const T laguerre_root = (  std::get<0>(laguerre_root_bracket)
                                 + std::get<1>(laguerre_root_bracket)) / 2;

        // Calculate the weight for this Laguerre root. Here, we calculate
        // the derivative of the Laguerre function and the value of the
        // previous Laguerre function on the x-axis at the value of this
        // Laguerre root.
        static_cast<void>(laguerre_root_object(laguerre_root));

        // Store the abscissa and weight for this index.
        xi.push_back(laguerre_root);
        wi.push_back(norm_g / ((laguerre_root_object.derivative() * order) * laguerre_root_object.previous()));
      }

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

  template<typename T>
  struct airy_ai_object BOOST_FINAL
  {
  public:
    airy_ai_object(const T& x)
      : my_x     (x),
        my_zeta  (((sqrt(x) * x) * 2) / 3),
        my_factor(make_factor(my_zeta)) { }

    T operator()(const T& t) const
    {
      using std::sqrt;

      return my_factor / sqrt(boost::math::cbrt(2 + (t / my_zeta)));
    }

  private:
    const T my_x;
    const T my_zeta;
    const T my_factor;

    airy_ai_object() : my_x     (),
                       my_zeta  (),
                       my_factor() { }

    static T make_factor(const T& z)
    {
      using std::exp;
      using std::sqrt;

      return 1 / ((sqrt(boost::math::constants::pi<T>()) * sqrt(boost::math::cbrt(z * 48))) * (exp(z) * boost::math::tgamma(T(5) / 6)));
    }
  };
} // namespace detail

} } // namespace gauss::laguerre

// A float_type is created to handle the desired number of decimal digits from `cpp_dec_float` without using __expression_templates.
struct local
{
  static constexpr unsigned int my_digits10 = 101U;

  typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<my_digits10>,
                                        boost::multiprecision::et_off>
  float_type;
};

static_assert(local::my_digits10 > 20U,
                        "Error: This example is intended to have more than 20 decimal digits");

int main()
{
  // Use Gauss-Laguerre quadrature integration to compute airy_ai(x / 7)
  // with 7 <= x <= 120 and where x is incremented in steps of 1.

  // During development of this example, we have empirically found
  // the numbers of Gauss-Laguerre coefficients needed for convergence
  // when using various counts of base-10 digits.

  // Let's calibrate, for instance, the number of coefficients needed
  // at the point x = 1.

  // Empirical data lead to:
  // Fit[{{21.0, 3.5}, {51.0, 11.1}, {101.0, 22.5}, {201.0, 46.8}}, {1, d, d^2}, d]
  // FullSimplify[%]
  // -1.28301 + (0.235487 + 0.0000178915 d) d

  // We need significantly more coefficients at smaller real values than are needed
  // at larger real values because the slope derivative of airy_ai(x) gets more
  // steep as x approaches zero.

  // This Gauss-Laguerre quadrature is designed for airy_ai(x) with real-valued x >= 1.

  constexpr boost::float_least32_t d = static_cast<boost::float_least32_t>(std::numeric_limits<local::float_type>::digits10);

  constexpr boost::float_least32_t laguerre_order_factor = -1.28301F + ((0.235487F + (0.0000178915F * d)) * d);

  constexpr int laguerre_order = static_cast<int>(laguerre_order_factor * d);

  std::cout << "std::numeric_limits<local::float_type>::digits10: " << std::numeric_limits<local::float_type>::digits10 << std::endl;
  std::cout << "laguerre_order: " << laguerre_order << std::endl;

  typedef gauss::laguerre::detail::abscissas_and_weights<local::float_type> abscissas_and_weights_type;

  const abscissas_and_weights_type the_abscissas_and_weights(laguerre_order, local::float_type(-1) / 6);

  bool result_is_ok = true;

  for(std::uint32_t u = 7U; u < 121U; ++u)
  {
    const local::float_type x = local::float_type(u) / 7;

    typedef gauss::laguerre::detail::airy_ai_object<local::float_type> airy_ai_object_type;

    const airy_ai_object_type the_airy_ai_object(x);

    const local::float_type airy_ai_value =
      std::inner_product(the_abscissas_and_weights.abscissa_n().cbegin(),
                         the_abscissas_and_weights.abscissa_n().cend(),
                         the_abscissas_and_weights.weight_n().cbegin(),
                         local::float_type(0U),
                         std::plus<local::float_type>(),
                         [&the_airy_ai_object](const local::float_type& this_abscissa,
                                               const local::float_type& this_weight) -> local::float_type
                         {
                           return the_airy_ai_object(this_abscissa) * this_weight;
                         });

    static const local::float_type one_third = 1.0F / local::float_type(3U);

    static const local::float_type one_over_pi_times_one_over_sqrt_three =
      sqrt(one_third) * boost::math::constants::one_div_pi<local::float_type>();

    const local::float_type sqrt_x = sqrt(x);

    const local::float_type airy_ai_control =
       (sqrt_x * one_over_pi_times_one_over_sqrt_three)
      * boost::math::cyl_bessel_k(one_third, ((2.0F * x) * sqrt_x) * one_third);

    std::cout << std::setprecision(std::numeric_limits<local::float_type>::digits10)
              << "airy_ai_value  : "
              << airy_ai_value
              << std::endl;

    std::cout << std::setprecision(std::numeric_limits<local::float_type>::digits10)
              << "airy_ai_control: "
              << airy_ai_control
              << std::endl;

    const local::float_type delta = fabs(1.0F - (airy_ai_control / airy_ai_value));

    static const local::float_type tol("1E-" + boost::lexical_cast<std::string>(std::numeric_limits<local::float_type>::digits10 - 7U));

    result_is_ok &= (delta < tol);
  }

  std::cout << std::endl
            << "Total... result_is_ok: "
            << std::boolalpha
            << result_is_ok
            << std::endl;
} // int main()

/*

Partial output:

//[gauss_laguerre_quadrature_output_1

std::numeric_limits<local::float_type>::digits10: 101
laguerre_order: 2291

Finding the approximate roots...
root_estimates.size(): 1, 0.0%
root_estimates.size(): 8, 0.3%
root_estimates.size(): 16, 0.7%
...
root_estimates.size(): 2288, 99.9%
root_estimates.size(): 2291, 100.0%

Calculating abscissas and weights. Processed 1, 0.0%
Calculating abscissas and weights. Processed 9, 0.4%
...
Calculating abscissas and weights. Processed 2289, 99.9%
Calculating abscissas and weights. Processed 2291, 100.0%
//] [/gauss_laguerre_quadrature_output_1]

//[gauss_laguerre_quadrature_output_2

airy_ai_value  : 0.13529241631288141552414742351546630617494414298833070600910205475763353480226572366348710990874867334
airy_ai_control: 0.13529241631288141552414742351546630617494414298833070600910205475763353480226572366348710990874868323
airy_ai_value  : 0.11392302126009621102904231059693500086750049240884734708541630001378825889924647699516200868335286103
airy_ai_control: 0.1139230212600962110290423105969350008675004924088473470854163000137882588992464769951620086833528582
...
airy_ai_value  : 3.8990420982303275013276114626640705170145070824317976771461533035231088620152288641360519429331427451e-22
airy_ai_control: 3.8990420982303275013276114626640705170145070824317976771461533035231088620152288641360519429331426481e-22

Total... result_is_ok: true

//] [/gauss_laguerre_quadrature_output_2]

*/


//
// Created by Amir Masoud Abdol on 2020-04-11
//

#include "TestStrategy.h"

#include <boost/math/distributions/fisher_f.hpp>

using namespace sam;
using boost::math::fisher_f;

void FTest::run(Experiment *experiment) {

  static ResultType res;

  // The first group is always the control group
  for (int i{experiment->setup.nd()}, d{0}; i < experiment->setup.ng();
       ++i, ++d %= experiment->setup.nd()) {

    res =
        f_test((*experiment)[d].stddev_, (*experiment)[d].nobs_,
               (*experiment)[i].stddev_, (*experiment)[i].nobs_, params.alpha);

    (*experiment)[i].stats_ = res.fstat;
    (*experiment)[i].pvalue_ = res.pvalue;
    (*experiment)[i].sig_ = res.sig;
  }
}

FTest::ResultType FTest::f_test(float Sd1,   // Sample 1 std deviation
                                unsigned Sn1, // Sample 1 size
                                float Sd2,   // Sample 2 std deviation
                                unsigned Sn2, // Sample 2 size
                                float alpha) // Significance level
{
  // F-statistic:
  double f_stats = (Sd1 / Sd2);

  //
  // Finally define our distribution, and get the probability:
  //
  fisher_f dist(Sn1 - 1, Sn2 - 1);
  double p = cdf(dist, f_stats);

  double ucv = quantile(complement(dist, alpha));
  double ucv2 = quantile(complement(dist, alpha / 2));
  double lcv = quantile(dist, alpha);
  double lcv2 = quantile(dist, alpha / 2);

  //
  // Finally print out results of null and alternative hypothesis:
  //
  if ((ucv2 < f_stats) || (lcv2 > f_stats)) { // Alternative "NOT REJECTED"
    return {.fstat = static_cast<float>(f_stats), .df1 = Sn1 - 1, .df2 = Sn2 - 1, .pvalue = static_cast<float>(p), .sig = true};
  }

  if (lcv > f_stats) {// Alternative "NOT REJECTED"
    return {.fstat = static_cast<float>(f_stats), .df1 = Sn1 - 1, .df2 = Sn2 - 1, .pvalue = static_cast<float>(p), .sig = true};
  }

  if (ucv < f_stats) { // Alternative "NOT REJECTED"
    return {.fstat = static_cast<float>(f_stats), .df1 = Sn1 - 1, .df2 = Sn2 - 1, .pvalue = static_cast<float>(p), .sig = true};
  }

  // Alternative "Rejected"
  return {.fstat = static_cast<float>(f_stats), .df1 = Sn1 - 1, .df2 = Sn2 - 1, .pvalue = static_cast<float>(p), .sig = false};

}