#include <gtest/gtest.h>

#include <Eigen/Core>
#include <algorithm>
#include <csim/init_ops.hpp>
#include <csim/memory_ops.hpp>
#include <csim/stat_ops.hpp>
#include <csim/update_ops.hpp>
#include <csim/update_ops_cpp.hpp>
#include <string>

#include "../util/util.hpp"

void test_single_qubit_named_gate(UINT n, std::string name,
    std::function<void(UINT, CTYPE*, ITYPE)> func, Eigen::MatrixXcd mat) {
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 2;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state_with_seed(state, dim, 0);

    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
    std::vector<UINT> indices;
    for (UINT i = 0; i < n; ++i) indices.push_back(i);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        for (UINT i = 0; i < n; ++i) {
            UINT target = indices[i];
            func(target, state, dim);
            test_state =
                get_expanded_eigen_matrix_with_identity(target, mat, n) *
                test_state;
            state_equal(state, test_state, dim, name);
        }
        std::random_shuffle(indices.begin(), indices.end());
    }
    release_quantum_state(state);
}

TEST(UpdateTest, XGate) {
    Eigen::MatrixXcd mat(2, 2);
    mat << 0, 1, 1, 0;
    test_single_qubit_named_gate(6, "XGate", X_gate, mat);
    test_single_qubit_named_gate(6, "XGate", X_gate_single_unroll, mat);
#ifdef _OPENMP
    test_single_qubit_named_gate(6, "XGate", X_gate_parallel_unroll, mat);
#endif
#ifdef _USE_SIMD
    test_single_qubit_named_gate(6, "XGate", X_gate_single_simd, mat);
#ifdef _OPENMP
    test_single_qubit_named_gate(6, "XGate", X_gate_parallel_simd, mat);
#endif
#endif
}
TEST(UpdateTest, YGate) {
    Eigen::MatrixXcd mat(2, 2);
    mat << 0, -1.i, 1.i, 0;
    test_single_qubit_named_gate(6, "YGate", Y_gate, mat);
    test_single_qubit_named_gate(6, "YGate", Y_gate_single_unroll, mat);
#ifdef _OPENMP
    test_single_qubit_named_gate(6, "YGate", Y_gate_parallel_unroll, mat);
#endif
#ifdef _USE_SIMD
    test_single_qubit_named_gate(6, "YGate", Y_gate_single_simd, mat);
#ifdef _OPENMP
    test_single_qubit_named_gate(6, "YGate", Y_gate_parallel_simd, mat);
#endif
#endif
}
TEST(UpdateTest, ZGate) {
    const UINT n = 3;
    Eigen::MatrixXcd mat(2, 2);
    mat << 1, 0, 0, -1;
    test_single_qubit_named_gate(6, "ZGate", Z_gate, mat);
    test_single_qubit_named_gate(6, "ZGate", Z_gate_single_unroll, mat);
#ifdef _OPENMP
    test_single_qubit_named_gate(6, "ZGate", Z_gate_parallel_unroll, mat);
#endif
#ifdef _USE_SIMD
    test_single_qubit_named_gate(6, "ZGate", Z_gate_single_simd, mat);
#ifdef _OPENMP
    test_single_qubit_named_gate(6, "ZGate", Z_gate_parallel_simd, mat);
#endif
#endif
}
TEST(UpdateTest, HGate) {
    const UINT n = 3;
    Eigen::MatrixXcd mat(2, 2);
    mat << 1, 1, 1, -1;
    mat /= sqrt(2.);
    test_single_qubit_named_gate(n, "HGate", H_gate, mat);
    test_single_qubit_named_gate(6, "HGate", H_gate_single_unroll, mat);
#ifdef _OPENMP
    test_single_qubit_named_gate(6, "HGate", H_gate_parallel_unroll, mat);
#endif
#ifdef _USE_SIMD
    test_single_qubit_named_gate(6, "HGate", H_gate_single_simd, mat);
#ifdef _OPENMP
    test_single_qubit_named_gate(6, "HGate", H_gate_parallel_simd, mat);
#endif
#endif
}

TEST(UpdateTest, SGate) {
    const UINT n = 3;
    Eigen::MatrixXcd mat(2, 2);
    mat << 1, 0, 0, 1.i;
    test_single_qubit_named_gate(n, "SGate", S_gate, mat);
    test_single_qubit_named_gate(n, "SGate", Sdag_gate, mat.adjoint());
}

TEST(UpdateTest, TGate) {
    const UINT n = 3;
    Eigen::MatrixXcd mat(2, 2);
    mat << 1, 0, 0, (1. + 1.i) / sqrt(2.);
    test_single_qubit_named_gate(n, "TGate", T_gate, mat);
    test_single_qubit_named_gate(n, "TGate", Tdag_gate, mat.adjoint());
}

TEST(UpdateTest, sqrtXGate) {
    const UINT n = 3;
    Eigen::MatrixXcd mat(2, 2);
    mat << 0.5 + 0.5i, 0.5 - 0.5i, 0.5 - 0.5i, 0.5 + 0.5i;
    test_single_qubit_named_gate(n, "SqrtXGate", sqrtX_gate, mat);
    test_single_qubit_named_gate(
        n, "SqrtXdagGate", sqrtXdag_gate, mat.adjoint());
}

TEST(UpdateTest, sqrtYGate) {
    const UINT n = 3;
    Eigen::MatrixXcd mat(2, 2);
    mat << 0.5 + 0.5i, -0.5 - 0.5i, 0.5 + 0.5i, 0.5 + 0.5i;
    test_single_qubit_named_gate(n, "SqrtYGate", sqrtY_gate, mat);
    test_single_qubit_named_gate(
        n, "SqrtYdagGate", sqrtYdag_gate, mat.adjoint());
}

void test_projection_gate(std::function<void(UINT, CTYPE*, ITYPE)> func,
    std::function<double(UINT, CTYPE*, ITYPE)> prob_func,
    Eigen::MatrixXcd mat) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;
    const double eps = 1e-14;
    UINT target;
    double prob;

    auto state = allocate_quantum_state(dim);
    std::vector<UINT> indices;
    for (UINT i = 0; i < n; ++i) indices.push_back(i);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        for (int i = 0; i < n; ++i) {
            target = indices[i];
            initialize_Haar_random_state(state, dim);
            Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
            for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

            // Z-projection operators
            prob = prob_func(target, state, dim);
            EXPECT_GT(prob, 1e-10);
            func(target, state, dim);
            ASSERT_NEAR(state_norm_squared(state, dim), prob, eps);
            normalize(prob, state, dim);

            test_state =
                get_expanded_eigen_matrix_with_identity(target, mat, n) *
                test_state;
            ASSERT_NEAR(test_state.squaredNorm(), prob, eps);
            test_state.normalize();
            state_equal(state, test_state, dim, "Projection gate");
        }
        std::random_shuffle(indices.begin(), indices.end());
    }
    release_quantum_state(state);
}

TEST(UpdateTest, ProjectionAndNormalizeTest) {
    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;
    test_projection_gate(P0_gate, M0_prob, P0);
    test_projection_gate(P1_gate, M1_prob, P1);
    test_projection_gate(P0_gate_single, M0_prob, P0);
    test_projection_gate(P1_gate_single, M1_prob, P1);
#ifdef _OPENMP
    test_projection_gate(P0_gate_parallel, M0_prob, P0);
    test_projection_gate(P1_gate_parallel, M1_prob, P1);
#endif
}

TEST(UpdateTest, SingleQubitRotationGateTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
    Identity << 1, 0, 0, 1;
    X << 0, 1, 1, 0;
    Y << 0, -1.i, 1.i, 0;
    Z << 1, 0, 0, -1;

    UINT target;
    double angle;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
    typedef std::tuple<std::function<void(UINT, double, CTYPE*, ITYPE)>,
        Eigen::MatrixXcd, std::string>
        testset;
    std::vector<testset> test_list;
    test_list.push_back(std::make_tuple(RX_gate, X, "Xrot"));
    test_list.push_back(std::make_tuple(RY_gate, Y, "Yrot"));
    test_list.push_back(std::make_tuple(RZ_gate, Z, "Zrot"));

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        for (auto tup : test_list) {
            target = rand_int(n);
            angle = rand_real();
            auto func = std::get<0>(tup);
            auto mat = std::get<1>(tup);
            auto name = std::get<2>(tup);
            func(target, angle, state, dim);
            test_state =
                get_expanded_eigen_matrix_with_identity(target,
                    cos(angle / 2) * Identity + 1.i * sin(angle / 2) * mat, n) *
                test_state;
            state_equal(state, test_state, dim, name);
        }
    }
    release_quantum_state(state);
}

void test_two_qubit_named_gate(UINT n, std::string name,
    std::function<void(UINT, UINT, CTYPE*, ITYPE)> func,
    std::function<Eigen::MatrixXcd(UINT, UINT, UINT)> matfunc) {
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 2;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state_with_seed(state, dim, 0);

    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
    std::vector<UINT> indices;
    for (UINT i = 0; i < n; ++i) indices.push_back(i);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        for (UINT i = 0; i + 1 < n; i += 2) {
            UINT target = indices[i];
            UINT control = indices[i + 1];
            func(control, target, state, dim);
            Eigen::MatrixXcd mat = matfunc(control, target, n);
            test_state = mat * test_state;
            state_equal(state, test_state, dim, name);
        }
        std::random_shuffle(indices.begin(), indices.end());
    }
    release_quantum_state(state);
}

TEST(UpdateTest, CNOTGate) {
    const UINT n = 4;
    test_two_qubit_named_gate(
        n, "CNOT", CNOT_gate, get_eigen_matrix_full_qubit_CNOT);
    test_two_qubit_named_gate(6, "CNOTGate", CNOT_gate_single_unroll,
        get_eigen_matrix_full_qubit_CNOT);
#ifdef _OPENMP
    test_two_qubit_named_gate(6, "CNOTGate", CNOT_gate_parallel_unroll,
        get_eigen_matrix_full_qubit_CNOT);
#endif
#ifdef _USE_SIMD
    test_two_qubit_named_gate(
        6, "CNOTGate", CNOT_gate_single_simd, get_eigen_matrix_full_qubit_CNOT);
#ifdef _OPENMP
    test_two_qubit_named_gate(6, "CNOTGate", CNOT_gate_parallel_simd,
        get_eigen_matrix_full_qubit_CNOT);
#endif
#endif
}

TEST(UpdateTest, CZGate) {
    const UINT n = 4;
    test_two_qubit_named_gate(n, "CZ", CZ_gate, get_eigen_matrix_full_qubit_CZ);
    test_two_qubit_named_gate(
        6, "CZGate", CZ_gate_single_unroll, get_eigen_matrix_full_qubit_CZ);
#ifdef _OPENMP
    test_two_qubit_named_gate(
        6, "CZGate", CZ_gate_parallel_unroll, get_eigen_matrix_full_qubit_CZ);
#endif
#ifdef _USE_SIMD
    test_two_qubit_named_gate(
        6, "CZGate", CZ_gate_single_simd, get_eigen_matrix_full_qubit_CZ);
#ifdef _OPENMP
    test_two_qubit_named_gate(
        6, "CZGate", CZ_gate_parallel_simd, get_eigen_matrix_full_qubit_CZ);
#endif
#endif
}

TEST(UpdateTest, SWAPGate) {
    const UINT n = 4;
    test_two_qubit_named_gate(
        n, "SWAP", SWAP_gate, get_eigen_matrix_full_qubit_SWAP);
    test_two_qubit_named_gate(6, "SWAPGate", SWAP_gate_single_unroll,
        get_eigen_matrix_full_qubit_SWAP);
#ifdef _OPENMP
    test_two_qubit_named_gate(6, "SWAPGate", SWAP_gate_parallel_unroll,
        get_eigen_matrix_full_qubit_SWAP);
#endif
#ifdef _USE_SIMD
    test_two_qubit_named_gate(
        6, "SWAPGate", SWAP_gate_single_simd, get_eigen_matrix_full_qubit_SWAP);
#ifdef _OPENMP
    test_two_qubit_named_gate(6, "SWAPGate", SWAP_gate_parallel_simd,
        get_eigen_matrix_full_qubit_SWAP);
#endif
#endif
}


#include "lcp_solver.hpp"

#include <Eigen/Core>

#include <constants.hpp>
#include <logger.hpp>
#include <utils/eigen_ext.hpp>

namespace ccd {
namespace opt {

    inline Eigen::VectorXd fischer(
        const Eigen::VectorXd& x, const Eigen::VectorXd& s);
    inline double fischer_error(const Eigen::VectorXd& phi);
    Eigen::SparseMatrix<double> jac_fischer(const Eigen::VectorXd& x,
        const Eigen::VectorXd& s,
        const Eigen::SparseMatrix<double>& A);
    inline Eigen::VectorXd grad_fischer_error(
        const Eigen::VectorXd& phi, const Eigen::SparseMatrix<double>& J);
    Eigen::VectorXd perturb(
        const Eigen::VectorXd& x, const Eigen::VectorXd& phi);
    bool is_converged(double old_err, double err, int iter);
    Eigen::VectorXd compute_search_direction(const Eigen::VectorXd& phi,
        const Eigen::SparseMatrix<double>& J,
        const Eigen::VectorXd& grad_psi,
        const int iter);
    // Armijo backtracking combined with a projected line-search
    bool projected_line_search(const Eigen::VectorXd& x,
        const Eigen::VectorXd& dir,
        const std::function<double(const Eigen::VectorXd&)>& f,
        const double fx,
        const Eigen::VectorXd& grad_fx,
        double& step_length,
        int iter,
        const double armijo_rule_coeff = 0.001);

    // Solve the LCP:
    //      s = Ax + b
    //      0 ≤ x ⟂ s ≥ 0
    // by minimizing the Fisher error:
    //      f(x) = (x² + s²) - x - s
    // using Newton's method.
    bool lcp_newton(const Eigen::MatrixXd& A_dense,
        const Eigen::VectorXd& b,
        Eigen::VectorXd& x)
    {
        // Use a sparse matrix
        const Eigen::SparseMatrix<double> A = A_dense.sparseView();

        int num_vars = b.rows();
        assert(A.rows() == num_vars);

        // x₀ = 0
        x = Eigen::VectorXd::Zero(num_vars);

        double old_err, err = std::numeric_limits<double>::infinity();
        for (int i = 0; i < Constants::FISCHER_MAX_ITER; i++) {
            Eigen::VectorXd s = A * x + b; // slack variable

            // Test all stopping criteria used
            Eigen::VectorXd phi = fischer(x, s);
            old_err = err;
            err = fischer_error(phi); // ψ

            if (is_converged(old_err, err, i)) {
                return true;
            }

            Eigen::VectorXd px = perturb(x, phi);
            Eigen::SparseMatrix<double> J = jac_fischer(px, s, A);
            Eigen::VectorXd grad_psi = grad_fischer_error(phi, J);

            // Test if we have dropped into a local minimia if so we are stuck
            if (grad_psi.norm() < Constants::FISCHER_ABS_TOL) {
                spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                             "failure='||∇ψ|| < {:g}' ||∇ψ||={:g} "
                             "failsafe=none",
                    i, Constants::FISCHER_ABS_TOL, grad_psi.norm());
                return false;
            }

            Eigen::VectorXd delta_x
                = compute_search_direction(phi, J, grad_psi, i);

            // Update iterate with result from Armijo backtracking
            double step_length = 1.0;
            bool success = projected_line_search(x, delta_x,
                [&A, &b](const Eigen::VectorXd& x) {
                    Eigen::VectorXd s = A * x + b;
                    Eigen::VectorXd phi = fischer(x, s);
                    return fischer_error(phi);
                },
                /*fx=*/err, grad_psi, step_length, i);
            if (!success) {
                delta_x = -grad_psi;
                success = projected_line_search(x, delta_x,
                    [&A, &b](const Eigen::VectorXd& x) {
                        Eigen::VectorXd s = A * x + b;
                        Eigen::VectorXd phi = fischer(x, s);
                        return fischer_error(phi);
                    },
                    /*fx=*/err, grad_psi, step_length, i);
                if (!success) {
                    return false;
                }
            }

            x += step_length * delta_x;
        }

        spdlog::warn("solver=ficher_newton_lcp "
                     "failure='too many iterations' MAX_ITER={:d}",
            Constants::FISCHER_MAX_ITER);
        return false;
    }

    /// @brief Compute the Fischer error measure.
    inline Eigen::VectorXd fischer(
        const Eigen::VectorXd& x, const Eigen::VectorXd& s)
    {
        //             _________
        // ϕ(x, s) = ⎷(x² + s²) - x - s
        //
        return (x.array().pow(2) + s.array().pow(2)).sqrt().matrix() - x - s;
    }

    inline double fischer_error(const Eigen::VectorXd& phi)
    {
        // ψ(x, s) = ½ϕ(x, s)ᵀϕ(x, s)
        return phi.squaredNorm() / 2;
    }

    Eigen::SparseMatrix<double> jac_fischer(const Eigen::VectorXd& x,
        const Eigen::VectorXd& s,
        const Eigen::SparseMatrix<double>& A)
    {
        //                ___________             _x̲ᵢ̲_+̲_s̲ᵢ̲a̲ᵢ̲_
        // ∇ϕᵢ(x, s) = ∇⎷(xᵢ² + sᵢ²) - xᵢ - sᵢ =  xᵢ² + s²    - aᵢ - 1
        Eigen::ArrayXd denom = (x.array().pow(2) + s.array().pow(2)).sqrt();
        Eigen::VectorXd p = (x.array() / denom) - 1;
        Eigen::VectorXd q = (s.array() / denom) - 1;
        return Eigen::SparseDiagonal<double>(p)
            + Eigen::SparseDiagonal<double>(q) * A;
    }

    inline Eigen::VectorXd grad_fischer_error(
        const Eigen::VectorXd& phi, const Eigen::SparseMatrix<double>& J)
    {
        // ∇ψ(x, s) = ∇ϕ(x, s)ᵀϕ(x, s) = Jᵀϕ
        return J.transpose() * phi;
    }

    /// @brief Compute the sign of the input.
    inline double sign(double x) { return (0 < x) - (x < 0); }

    Eigen::VectorXd perturb(
        const Eigen::VectorXd& x, const Eigen::VectorXd& phi)
    {
        // Bitmask for singular indices
        Eigen::ArrayXb is_singular
            = phi.array().abs() < Constants::FISCHER_SINGULAR_TOL
            && x.array().abs() < Constants::FISCHER_SINGULAR_TOL;

        // Perturbation: works on full system
        Eigen::VectorXd px = x; // perturbed x
        Eigen::VectorXd x_sign = x.unaryExpr(&sign);
        // x_sign(x_sign==0) = 1;
        x_sign += (x_sign.array() == 0).matrix().cast<double>();
        // px(S==1) = Constants::FISCHER_SINGULAR_TOL * dir(S==1);
        for (size_t i = 0; i < x.rows(); i++) {
            if (is_singular(i)) {
                px(i) = Constants::FISCHER_SINGULAR_TOL * x_sign(i);
            }
        }

        return px;
    }

    bool is_converged(double old_err, double err, int iter)
    {
        // Test relative error
        if (abs(err - old_err) / abs(old_err) < Constants::FISCHER_REL_TOL) {
            spdlog::debug("solver=fischer_newton_lcp iter={} "
                          "status=success rel_tol={} rel_err={}",
                iter, Constants::FISCHER_REL_TOL,
                abs(err - old_err) / abs(old_err));
            return true;
        }
        // Test absolute error
        if (err < Constants::FISCHER_ABS_TOL) {
            spdlog::debug("solver=fischer_newton_lcp iter={} "
                          "status=success abs_tol={} abs_err={}",
                iter, Constants::FISCHER_ABS_TOL, err);
            return true;
        }
        return false;
    }

    Eigen::VectorXd compute_search_direction(const Eigen::VectorXd& phi,
        const Eigen::SparseMatrix<double>& J,
        const Eigen::VectorXd& grad_psi,
        const int iter)
    {
        // Solve Δx = -J⁻¹ϕ
        Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
        solver.compute(J);
        Eigen::VectorXd delta_x;
        if (solver.info() == Eigen::Success) {
            delta_x = solver.solve(-phi);
            if (solver.info() != Eigen::Success) {
                spdlog::warn(
                    "solver=ficher_newton_lcp iter={:d} "
                    "failure='sparse solve for newton direction failed' "
                    "failsafe='revert to gradient descent'",
                    iter);
                delta_x = -grad_psi; // Revert to gradient descent
            }
        } else {
            spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                         "failure='sparse decomposition of the hessian failed' "
                         "failsafe='revert to gradient descent'",
                iter);
            delta_x = -grad_psi; // Revert to gradient descent
        }

        return delta_x;
    }

    // Armijo backtracking combined with a projected line-search
    bool projected_line_search(const Eigen::VectorXd& x,
        const Eigen::VectorXd& dir,
        const std::function<double(const Eigen::VectorXd&)>& f,
        const double fx,
        const Eigen::VectorXd& grad_fx,
        double& step_length,
        int iter,
        const double armijo_rule_coeff)
    {
        const double EPS = std::numeric_limits<double>::epsilon();

        // Test if the search direction is smaller than numerical precision.
        if (dir.array().abs().maxCoeff() < EPS) {
            spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                         "failure='search direction too small' max(|Δx|)={:g} "
                         "failsafe=none",
                iter, dir.array().abs().maxCoeff());
            return false;
        }

        // Test if our search direction is a 'sufficient' descent direction
        double descent_magnitude = (grad_fx.transpose() * dir)(0);
        if (descent_magnitude > -EPS * descent_magnitude) {
            spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                         "failure='search direction not descent direction' "
                         "(∇ϕ)ᵀΔx={:g} failsafe='revert to gradient descent'",
                iter, descent_magnitude);
            return false;
        }

        step_length = 1.0; // Current step length

        // Sufficent decrease parameter for Armijo backtracking line search
        double armijo_term = armijo_rule_coeff * descent_magnitude;

        Eigen::VectorXd x_k;
        do {
            x_k = (x + step_length * dir).array().max(0); // project it

            // Perform Armijo codition to see if we got a sufficient decrease
            if (f(x_k) <= fx + step_length * armijo_term) {
                return true;
            }

            step_length /= 2;
        } while ((step_length * dir).norm() >= Constants::FISCHER_SINGULAR_TOL);

        spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                     "failure='step length too small' step_length={:g}",
            iter, step_length);
        return false;
    }

} // namespace opt
} // namespace ccd


/**
 * @file   BSpline.hpp
 * @author Paul Furgale <paul.furgale@utoronto.ca>
 * @date   Fri Feb 11 13:51:57 2011
 *
 * @brief  A class to facilitate state estimation for vehicles in 3D
 *         space using B-splines.
 *
 *
 */

#ifndef _BSPLINE_HPP
#define _BSPLINE_HPP
#include <sparse_block_matrix/sparse_block_matrix.h>
#include <Eigen/Core>
#include <sm/assert_macros.hpp>
#include <vector>

namespace bsplines {
class BiVector;
}

namespace Eigen {
namespace internal {
template <>
struct functor_traits<bsplines::BiVector> {
    enum { Cost = 1, PacketAccess = false, IsRepeatable = true };
};
}  // namespace internal
}  // namespace Eigen

namespace bsplines {

class BiVector {
    enum { Cost = 1, PacketAccess = false, IsRepeatable = true };

  private:
    const int startIndex_;
    const double endValue_;
    const Eigen::VectorXd localBi_;

  public:
    BiVector(int startIndex, const Eigen::VectorXd& localBi, const double& endValue)
        : startIndex_(startIndex), endValue_(endValue), localBi_(localBi){};

    double operator()(int i, int j = 0) const {
        // kill unused parameter warning
        static_cast<void>(j);
        i -= startIndex_;
        if (i < 0) {
            return endValue_;
        }
        if (i >= (localBi_.rows())) {
            return 0;
        }
        return i >= 0 ? localBi_(i) : 0;
    }
};

/**
 * @brief A class to facilitate state estimation for vehicles in 3D space using B-Splines
 *
 * NOTE by CC:
 * 1. The B-Spline definition in the class is some different to wiki, "The NURBS Book", and MTU course
 *  (a) Basic function N[i,0](u) = 1 in "The NURBS Book", B[i,1](t) = 1 in this definition
 *  (b) Basic function N[i,p](u) is a degree p polynomial in u, but B[i,k](t) is k-1 degree polynomial in this
 * definition, where k is the order of B-Spline, and k = p + 1.
 *  (c) m = n + p + 1 for "The NURBS Book", m = n + k for this. And control points(coefficient) number is n+1, knots
 * number is m+1, spline degree is p
 * 2. Some terminology in this class
 *  (a) B-Spline order: k
 *  (b) B-Spline(Polynomial) degree: k - 1
 *  (c) For clamped(nonperiodic) curve(see Ref[2]), the knots is defined(see Ref[1]) as U = {a,...,a,
 * u[p+1],...,u[m-p-1],b,...,b} = {0,...,0,u[p+1],...,u[m-p-1],1,...,1}. And the first knot and last knot has the
 * multiplicity of p+1, the valid knots is U1 = {0,u[p+1],...,u[m-p-1],1}. I denote the number of knots is m1+1, and the
 * valid number of knots is m+1. The relation meets m1 = n + p + 1 = n + k, and m = m1 - 2p
 *  (d) Valid time segment: m
 *  (e) Knots numbers: m1 + 1 = m + 2(k - 1) + 1 = m + 2k - 1
 *  (f) Coefficient matrix, the same mean of control points matrix
 *  (g) Coefficient number: n + 1
 * 3. When evaluating the B-Spline value at t, only part of control points(ie, coefficient matrix) will active in the
 * calculation, Vk = {V[i-k+1], V[i-k+2],..., V[i]}. The matrix of Vk in code is called local parameters.
 *
 * QUESTIONS:
 * 1. The class use clamped curve in previous discussion, and the knots sequences in class (knots_) should be U, but in
 * the initSpline() function, it seems that the use opened curve.
 *  ANSWER: There are some confuse in the code implementation, this class is implements the clamped curve indeed, but
 * some variable don't seems like this.
 *  (1) The knots should be U = {a,...,a,u[p+1],...,u[m-p-1],b,...,b}, where a = tMin != 0, b = tMax != 1, but in
 * initialization, the first and last k element not the same, like {a - (k-1) * dt, a - (k-2) * dt, ..., a}.
 *  (2) The method `tMin()` and `tMax()` return `a` and `b`
 *  (3) The matrix M(or the basis matrix) don't save the first and last k element because they are zero.
 *
 * I am more familiar with the definition in "The NURBS Book". So, although I have add some comment to this class, but
 * maybe they are not quite correct.
 * Some function about integral I could understand yet.
 *
 * Ref:
 *  [1] L. Piegl and W. Tiller, The NURBS Book, Second Edition. Berlin, Heidelberg: Springer-Verlag, 1997.
 *  [2] K. Qin, “General Matrix Representations for B-Splines,” The Visual Computer, vol. 16, no. 3, pp. 177–186, 2000.
 *  [3] MTU, “CS3621 Introduction to Computing with Geometry Notes.” https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/.
 */
class BSpline {
  public:
    /**
     * @brief A base class for BSpline exceptions
     */
    SM_DEFINE_EXCEPTION(Exception, std::runtime_error);

    /**
     * @brief Create a spline of the specified order. The resulting B-spline will be a series of piecewise polynomials
     * of degree splineOrder - 1.
     * @param splineOrder   The order of the spline
     */
    explicit BSpline(int splineOrder);

    /**
     * A destructor.
     */
    ~BSpline() = default;

    /**
     * @brief Get the order of the spline, k
     * @return The order of the spline, k
     */
    inline int splineOrder() const { return splineOrder_; }

    /**
     * @brief Get the degree of polynomial used by the spline, p = k - 1
     * @return The degree of polynomial used by the spline, p = k - 1
     */
    inline int polynomialDegree() const { return splineOrder_ - 1; }

    /**
     * @brief Get the number of coefficients(control points) required for a specified number of valid time segments
     * @param numTimeSegments The number of time segments required
     * @return The number of coefficients required for a specified number of valid time segments
     */
    int numCoefficientsRequired(int numTimeSegments) const;

    /**
     * @brief Get the minimum number of knots required to have at least one valid time segment
     * @return The minimum number of knots required to have at least one valid time segment
     */
    int minimumKnotsRequired() const;

    /**
     * @brief Get the number of knots required for a specified number of valid time segments
     * @param numTimeSegments The number of time segments required
     * @return The number of knots required for a specified number of valid time segments
     */
    int numKnotsRequired(int numTimeSegments) const;

    /**
     * @brief Get the number of valid time segments for a given number of knots
     * @param numKnots  The number of knots.
     * @return The number of valid time segments
     */
    int numValidTimeSegments(int numKnots) const;

    /**
     * @brief Get the number of valid time segments for the current knot sequence
     * @return The number of valid time segments for the current knot sequence
     */
    int numValidTimeSegments() const;

    /**
     * @brief Get the basic matrix active on the i-th time segment
     * @param i The index of the time segment
     * @return The basic matrix active on the i-th time segment
     */
    const Eigen::MatrixXd& basisMatrix(int i) const;

    /**
     * @brief Get the minimum time that the spline is well-defined on
     * @return The minimum time that the spline is well-defined on
     */
    const double& tMin() const;

    /**
     * @brief Get the maximum time that the spline is well-defined on. Because B-spline are defined on half-open
     * intervals, the spline curve is well defined up to but not including this time.
     * @return The maximum time that the spline is well-defined on
     */
    const double& tMax() const;

    /**
     * @brief Get the time interval that the spline is well-defined on [tMin, tMax)
     * @return The time interval that the spline is well-defined on [tMin, tMax)
     */
    std::pair<double, double> timeInterval() const;

    /**
     * @brief Get the time interval of a single spline segment
     * @param i The index of the time segment
     * @return The time interval of the i-th spline segment
     */
    std::pair<double, double> timeInterval(int i) const;

    /**
     * @brief Set the knots and coefficients of the spline, each column of the coefficient(control points) matrix is
     * interpreted as a single, vector-valued spline coefficient, and then calculate the basic matrix M in each segment
     * @param knots         A non-decreasing knot sequence
     * @param coefficients  A set of spline coefficients(control points)
     */
    void setKnotsAndCoefficients(const std::vector<double>& knots, const Eigen::MatrixXd& coefficients);

    /**
     * @brief Set the knots and coefficients of the spline, each column of the coefficient(control points) matrix is
     * interpreted as a single, vector-valued spline coefficient, and then calculate the basic matrix M in each segment
     * @param knots         A non-decreasing knot vector
     * @param coefficients  A set of spline coefficients(control points)
     */
    void setKnotVectorAndCoefficients(const Eigen::VectorXd& knots, const Eigen::MatrixXd& coefficients);

    /**
     * @brief Set the coefficient(control points) matrix
     * @param coefficients  Coefficient matrix(control points)
     */
    void setCoefficientMatrix(const Eigen::MatrixXd& coefficients);

    /**
     * @brief Sets the coefficient(control points) matrix from the stacked vector of coefficients
     * @param coefficients  The stacked vector of coefficients(control points)
     */
    void setCoefficientVector(const Eigen::VectorXd& coefficients);

    /**
     * @brief Get the knot vector
     * @return Knot vector
     */
    inline const std::vector<double>& knots() const { return knots_; }

    /**
     * @brief Get the knot vector with a column matrix
     * @return Knot vector with a column matrix
     */
    Eigen::VectorXd knotVector() const;

    /**
     * @brief Get the coefficient(control points) matrix, each column of the coefficient matrix is interpreted as a
     * single, vector-valued spline coefficient.
     * @return Coefficient(control points) matrix
     */
    inline const Eigen::MatrixXd& coefficients() const { return coefficients_; }

    /**
     * @brief Get the stacked vector of coefficients(control points) matrix
     * @return Stacked vector of coefficients (control points) matrix
     */
    Eigen::VectorXd coefficientVector();

    /**
     * @brief Get the number of total coefficients the spline currently uses
     * @return The number of total coefficients the spline currently uses
     */
    int numCoefficients() const;

    /**
     * @brief Get the length(number) of total coefficients the spline currently uses
     * @return The length(number) of total coefficients the spline currently uses
     */
    int coefficientVectorLength() const;

    /**
     * @brief This is equivalent to spline.coefficients().cols()
     * @return The number of vector-valued coefficient columns the spline currently uses
     */
    int numVvCoefficients() const;

    /**
     * @brief Get a map to a single coefficient column. This allows the user to pass around what is essentially a
     * pointer to a single column in the coefficient matrix.
     * @param i The column of the coefficient matrix to return. 0 < i < coefficients().cols() = n
     * @return A map to column i of the coefficient matrix.
     */
    Eigen::Map<const Eigen::VectorXd> vvCoefficientVector(int i) const;

    /**
     * @brief Get a map to a single coefficient column. This allows the user to pass around what is essentially a
     * pointer to a single column in the coefficient matrix.
     * @param i The column of the coefficient matrix to return. 0 < i < coefficients().cols() = n
     * @return A map to column i of the coefficient matrix.
     */
    Eigen::Map<Eigen::VectorXd> vvCoefficientVector(int i);

    /**
     * @brief Evaluate the spline curve at time t
     * @param t The time to evaluate the spline curve
     * @return The value of the spline curve at the time t
     */
    Eigen::VectorXd eval(double t) const;

    /**
     * @brief Evaluate the derivative of the spline curve at time t
     * @param t                 The time to evaluate the spline derivative
     * @param derivativeOrder   The order of the derivative. This must be >= 0
     * @return The value of the derivative of the spline curve evaluated at t
     */
    Eigen::VectorXd evalD(double t, int derivativeOrder) const;

    /**
     * @brief Evaluate the derivative of the spline curve at time t and retrieve the Jacobian of the value with respect
     * to small changed in the parameter vector(control points, coefficient matrix). The Jacobian only refers to the
     * local parameter vector (part of coefficient matrix). The indices of the local parameters with respect to the full
     * parameter vector can be retrieved using localCoefficientVectorIndices()
     * @param t                 The time to evaluate the spline derivative
     * @param derivativeOrder   The order of the derivative. This must be >= 0
     * @return The value of the derivative of the spline curve evaluated at t and the Jacobian
     */
    std::pair<Eigen::VectorXd, Eigen::MatrixXd> evalDAndJacobian(double t, int derivativeOrder) const;

    /**
     * @brief Evaluate the derivative of the spline curve at time t and retrieve the Jacobian of the value with respect
     * to small changed in the parameter vector(control points, coefficient matrix). The Jacobian only refers to the
     * local parameter vector (part of coefficient matrix)
     * @param t                     The time to evaluate the spline derivative
     * @param derivativeOrder       The order of the derivative. This must be >= 0
     * @param jacobian              A pointer to the Jacobian matrix to fill in
     * @param coefficientIndices    A pointer to an int vector that will be filled with the local coefficient indices
     * (which index is activated for calculating Jacobian matrix)
     * @return The value of the derivative of the spline curve evaluated at t
     */
    Eigen::VectorXd evalDAndJacobian(double t, int derivativeOrder, Eigen::MatrixXd* jacobian,
                                     Eigen::VectorXi* coefficientIndices) const;

    /**
     * @brief Get the local basic matrix evaluated at the time t. For vector-valued spline coefficients of dimension n1,
     * and a B-Spline of order k, this matrix will be n1 x (kn1).
     *
     * Evaluating the B-spline at time t, eval(t,O) is equivalent to evaluating Phi(t,O) * localCoefficientVector(t).
     * It's similar to the Jacobian calculation.
     *      C(u) = U * M(i) * V(i) = J * V
     * where V is the stacked vector representation of V(i)
     *
     * @param t                 The time to evaluate the local basis matrix.
     * @param derivativeOrder   The derivative order to return (0 is no derivative)
     * @return  The local basis matrix evaluated at time t
     */
    Eigen::MatrixXd Phi(double t, int derivativeOrder = 0) const;

    /**
     * @brief Get the local basic matrix evaluated at the time t. For vector-valued spline coefficients of dimension n1,
     * and a B-Spline of order k, this matrix will be n1 x (kn1).
     *
     * Evaluating the B-spline at time t, eval(t,O) is equivalent to evaluating Phi(t,O) * localCoefficientVector(t)
     *
     * @param t                 The time to evaluate the local basis matrix.
     * @param derivativeOrder   The derivative order to return (0 is no derivative)
     * @return  The local basis matrix evaluated at time t
     */
    Eigen::MatrixXd localBasisMatrix(double t, int derivativeOrder = 0) const;

    /**
     * @brief Get the local coefficient matrix evaluated at the time t. For vector-valued spline coefficients of
     * dimension n1, and a B-Spline of order k, this matrix will be n1 x k
     * @param t The time being queried
     * @return The local coefficient matrix active at time t
     */
    Eigen::MatrixXd localCoefficientMatrix(double t) const;

    /**
     * @brief Get the local coefficient vector evaluated at the time t. For vector-valued spline coefficients of
     * dimension n1, and a B-Spline of order k, this vector will be k*n1 x 1. Evaluating the B-spline at time t,
     * eval(t,O) is equivalent to evaluating Phi(t,O) * localCoefficientVector(t)
     * @param t The time being queried
     * @return The local coefficient vector active at time t
     */
    Eigen::VectorXd localCoefficientVector(double t) const;

    /**
     * @brief Get the local coefficient vector for segment i
     * @param segmentIdx The segment index, should less than number of valid time segments
     * @return The local coefficient vector active on time segment i
     */
    Eigen::VectorXd segmentCoefficientVector(int segmentIdx) const;

    /**
     * @brief Return a map to a single coefficient column.
     *
     * This allows the user to pass around what is essentially a pointer to a single column in the coefficient matrix
     * @tparam D    The dimension of vector-valued spline coefficients n1
     * @param i     The column of the coefficient matrix to return. 0 <= i < n = coefficients().cols()
     * @return  A map to column i of the coefficient matrix
     */
    template <int D>
    Eigen::Map<Eigen::Matrix<double, D, 1> > fixedSizeVvCoefficientVector(int i) {
        SM_ASSERT_EQ_DBG(Exception, D, coefficients_.rows(),
                         "Size mismatch between requested vector size and actual vector size");
        SM_ASSERT_GE_LT(Exception, i, 0, coefficients_.cols(), "Index out of range");
        return Eigen::Map<Eigen::Matrix<double, D, 1> >(&coefficients_(0, i), coefficients_.rows());
    }

    /**
     * @brief Return a map to a single coefficient column, const version.
     *
     * This allows the user to pass around what is essentially a pointer to a single column in the coefficient matrix
     * @tparam D    The dimension of vector-valued spline coefficients n1
     * @param i     The column of the coefficient matrix to return. 0 <= i < n = coefficients().cols()
     * @return  A map to column i of the coefficient matrix
     */
    template <int D>
    Eigen::Map<const Eigen::Matrix<double, D, 1> > fixedSizeVvCoefficientVector(int i) const {
        SM_ASSERT_EQ_DBG(Exception, D, coefficients_.rows(),
                         "Size mismatch between requested vector size and actual vector size");
        SM_ASSERT_GE_LT(Exception, i, 0, coefficients_.cols(), "Index out of range");
        return Eigen::Map<const Eigen::Matrix<double, D, 1> >(&coefficients_(0, i), coefficients_.rows());
    }

    /**
     * @brief Get the indices of the local coefficients active at time t. Only part of control points(coefficient
     * matrix) is activated in evaluating B-Spline at t, i.e., Vk = {V[i-k+1], V[i-k+2], ... , V[i]}, the indices is the
     * index of every element of Vk with respect to the coefficient matrix, ie, [(i-k+1) * n1, i * n1]
     * @param t The time being queried
     * @return The indices of the local coefficients active at time t
     */
    Eigen::VectorXi localCoefficientVectorIndices(double t) const;

    /**
     * @brief Get the indices of the local coefficients active on segment i
     * @param segmentIdx The segment being queried
     * @return The indices of the local coefficients active on this segment
     */
    Eigen::VectorXi segmentCoefficientVectorIndices(int segmentIdx) const;

    /**
     * @brief Get the indices of the local vector-valued coefficients active at time t. Only part of control
     * points(coefficient matrix) is activated in evaluating B-Spline at t, ie, Vk = {V[i-k+1], V[i-k+2], ... , V[i]},
     * the indices is the row index of Vj with respect to the coefficient matrix. ie, [i-k+1, i]
     * @param t The time being queried
     * @return The indices of the local vector-valued coefficients active at time t
     */
    Eigen::VectorXi localVvCoefficientVectorIndices(double t) const;

    /**
     * @brief Get the indices of the local vector-valued coefficients active on segment i
     * @param segmentIdx The segment being queried
     * @return The indices of the local vector-valued coefficients active at time t
     */
    Eigen::VectorXi segmentVvCoefficientVectorIndices(int segmentIdx) const;

    /**
     * @brief Update the local coefficient vector
     * @param t The time used to select the local coefficients.
     * @param c The local coefficient vector
     */
    void setLocalCoefficientVector(double t, const Eigen::VectorXd& c);

    /**
     * Initialize a spline from two times and two positions. The spline will be initialized to
     * have one valid time segment \f$[t_0, t_1)\f$ such that
     *  \f$\mathbf b(t_0) = \mathbf p_0\f$,
     *  \f$\mathbf b(t_1) = \mathbf p_1\f$,
     *  \f$\dot{\mathbf b}(t_0) = \frac{\mathbf{p_1} - \mathbf p_0}{t_1 - t_0}\f$, and
     *  \f$\dot{\mathbf b}(t_1) = \frac{\mathbf{p_1} - \mathbf p_0}{t_1 - t_0}\f$.
     *
     * This solve method is same as "The NURBS Book", but use the matrix representation form in Ref[2]. It construct the
     * linear system Ax = b. Where x is the control points matrix but is implemented in stacked vector, The linear
     * system include the position equation(no derivatives), if k > 2, then add the velocity(first order derivatives)
     * equation. If k > 4, then set all higher order derivatives to zero, and add it to linear system.
     *
     * @param t0 The start of the time interval.
     * @param t1 The end of the time interval
     * @param p0 The position at the start of the time interval.
     * @param p1 The position at the end of the time interval.
     */
    void initSpline(double t0, double t1, const Eigen::VectorXd& p0, const Eigen::VectorXd& p1);

    /**
     * @brief Initialize spline with <t, v> list, where t is the timestamp, v is the vector value evaluted at t. The
     * initialization using global interpolation method(see Ref[1] Section 9.2)
     *
     * The BSpline could be represented in a matrix form like below
     *      C(t) = U(t) * M * V
     * where U(t) * M is implemented in `Phi(t)`, and V is the control(coefficient) matrix. The initialize method
     * construct a linear system Ax = b, where x is the stacked control vector from V; A is U(t) * M, the size is
     * adjusted to meet the size of x; b is the interpolation value C(t).
     *
     * But for this method, the size of <t, v> is less than the knots number, so the number of equation Ax = b is not
     * enough to solve x. The author add other equations to constrain the spline, i.e., assume the second derivativate
     * of spline at each knot to zero.
     *      C''(t) = U''(t) * M * V
     * where U''(t) * M is also implemented in `Phi(t, 2)`. Then the equation is Phi * x = 0, but in implementation, the
     * author add a value `lambda` to set the weight, i.e., lambda * Phi * x = 0. In my option, the lambda could regard
     * as the weight for the whole equation systems, like the information matrix in SLAM optimization.
     *
     * Please see the difference to above equation, the argument is spline at each time, this is spline at each knot.
     *
     * @param times                 Timestamp list
     * @param interpolationPoints   Interpolation points
     * @param numSegments           Number of time segments, it's used to set the knots number.
     * @param lambda                The adjust weight for second derivatives
     */
    void initSpline2(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints, int numSegments,
                     double lambda);

    /**
     * @brief
     *
     * The method is only few difference to `initSpline2`. The second derivatives is assuming to zero in `initSpline2`
     * while the quadratic integral is assume to zero in this method. It's seems like to the bias in IMU model.
     *
     * @param times
     * @param interpolationPoints
     * @param numSegments
     * @param lambda
     * @return
     */
    void initSpline3(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints, int numSegments,
                     double lambda);

    /**
     * @brief Same to `initSpline3` but using sparse calculation
     *
     * @param times
     * @param interpolationPoints
     * @param numSegments
     * @param lambda
     * @return
     */
    void initSplineSparse(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints, int numSegments,
                          double lambda);

    void initSplineSparseKnots(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints,
                               const Eigen::VectorXd knots, double lambda);

    /**
     * Add a curve segment that interpolates the point p, ending at time t.
     *
     * If the new time corresponds with the first knot past the end of the curve,
     * the existing curve is perfectly preserved. Otherwise, the existing curve
     * will interpolate its current position at the current endpoint and the new
     * position at the new endpoint but will not necessarily match the last segment
     * exactly.
     *
     * @param t The time of the point to interpolate. This must be greater than t_max()
     * @param p The point to interpolate at time t.
     */
    void addCurveSegment(const double& t, const Eigen::VectorXd& p);

    /**
     * Add a curve segment that interpolates the point p, ending at time t.
     *
     * If the new time corresponds with the first knot past the end of the curve,
     * the existing curve is perfectly preserved. Otherwise, the existing curve
     * will interpolate its current position at the current endpoint and the new
     * position at the new endpoint but will not necessarily match the last segment
     * exactly.
     *
     * @param t The time of the point to interpolate. This must be greater than t_max()
     * @param p The point to interpolate at time t.
     * @param lambda a smoothness parameter. Higher for more smooth.
     */
    void addCurveSegment2(const double& t, const Eigen::VectorXd& p, const double& lambda);

    /**
     * @brief Removes a curve segment from the left by removing one knot and one coefficient vector.
     * After calling this function, the curve will have one fewer segment. The new minimum time will be
     * timeInterval(0).first
     */
    void removeCurveSegment();

    /**
     * @brief Evaluate the integral from t1 to t2
     *
     * NOTE, TODO: some calculation detail don't match to my option, try later
     *
     * @param t1
     * @param t2
     * @return
     */
    Eigen::VectorXd evalIntegral(double t1, double t2) const;

    /**
     * @brief Same as evalIntegral
     *
     * @param t1
     * @param t2
     * @return
     */
    inline Eigen::VectorXd evalI(double t1, double t2) const { return evalIntegral(t1, t2); }

    /**
     * Get the \f$ \mathbf V_i \f$ matrix associated with the integral over the segment.
     *
     * @param segmentIndex
     *
     * @return the \f$ \mathbf V_i \f$ matrix of size k x k
     */
    Eigen::MatrixXd Vi(int segmentIndex) const;

    // return matrix of size kn1 x kn1
    Eigen::MatrixXd Mi(int segmentIndex) const;
    // return matrix of size kn1 x kn1
    Eigen::MatrixXd Bij(int segmentIndex, int columnIndex) const;
    // return matrix of size kn1 x n1
    Eigen::MatrixXd U(double t, int derivativeOrder) const;
    // return matrix of size 1 x k
    Eigen::VectorXd u(double t, int derivativeOrder) const;
    // the index of basic matrix for input time t
    int segmentIndex(double t) const;
    // return matrix of size k x k
    Eigen::MatrixXd Dii(int segmentIndex) const;
    // return matrix of size kn1 x kn1
    Eigen::MatrixXd Di(int segmentIndex) const;

    /**
     * Get the b_i(t) for i in localVvCoefficientVectorIndices (@see #localVvCoefficientVectorIndices).
     *
     * @param t The time being queried.
     *
     * @return [b_i(t) for i in localVvCoefficientVectorIndices] of size k x 1
     *
     */
    Eigen::VectorXd getLocalBiVector(double t) const;
    void getLocalBiInto(double t, Eigen::VectorXd& ret) const;

    /**
     * Get the cumulative (tilde) b_i(t) for i in localVvCoefficientVectorIndices (@see
     * #localVvCoefficientVectorIndices).
     *
     * @param t The time being queried.
     *
     * @return [tilde b_i(t) for i in localVvCoefficientVectorIndices].
     *
     */
    Eigen::VectorXd getLocalCumulativeBiVector(double t) const;

    Eigen::CwiseNullaryOp<BiVector, Eigen::VectorXd> getBiVector(double t) const {
        return Eigen::CwiseNullaryOp<BiVector, Eigen::VectorXd>(numValidTimeSegments(), 1,
                                                                BiVector(segmentIndex(t), getLocalBiVector(t), 0));
    }

    Eigen::CwiseNullaryOp<BiVector, Eigen::VectorXd> getCumulativeBiVector(double t) const {
        return Eigen::CwiseNullaryOp<BiVector, Eigen::VectorXd>(
            numValidTimeSegments(), 1, BiVector(segmentIndex(t), getLocalCumulativeBiVector(t), 1));
    }

    Eigen::MatrixXd segmentIntegral(int segmentIdx, const Eigen::MatrixXd& W, int derivativeOrder) const;

    Eigen::MatrixXd segmentQuadraticIntegral(const Eigen::MatrixXd& W, int segmentIdx, int derivativeOrder) const;
    Eigen::MatrixXd segmentQuadraticIntegralDiag(const Eigen::VectorXd& Wdiag, int segmentIdx,
                                                 int derivativeOrder) const;
    Eigen::MatrixXd curveQuadraticIntegral(const Eigen::MatrixXd& W, int derivativeOrder) const;
    Eigen::MatrixXd curveQuadraticIntegralDiag(const Eigen::VectorXd& Wdiag, int derivativeOrder) const;

    sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> curveQuadraticIntegralSparse(const Eigen::MatrixXd& W,
                                                                                int derivativeOrder) const;
    sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> curveQuadraticIntegralDiagSparse(
        const Eigen::VectorXd& Wdiag, int derivativeOrder) const;

    void initConstantSpline(double t_min, double t_max, int numSegments, const Eigen::VectorXd& constant);

  private:
    /**
     * @brief Check the knot sequence is valid, it will throw an exception if it's not valid
     * @param knots The knot sequence to verify
     */
    void verifyKnotSequence(const std::vector<double>& knots);

    /**
     * @brief Initialize the basis matrices M based on the current knot sequence. There is one basis matrix for each
     * valid time segment defined by the spline. See Ref[2]
     *
     * Implemented using the recursive basis matrix algorithm from
     * Qin, Kaihuai, General matrix representations for B-splines, The Visual Computer (2000) 16:177–186
     */
    void initializeBasisMatrices();

    /**
     * @brief The recursive function to calculate basis matrix M.
     *           [ M_{k-1}(i) ]        [      0       ]
     *  M_k(i) = [            ] * A +  [              ] * B = M1 * A + M2 * B
     *           [     0      ]        [ [ M_{k-1}(i) ]
     *
     *  M_1(i) = [1]
     *  size of A and B: (k-1) X k
     *
     * Ref: Qin, Kaihuai, General matrix representations for B-splines, The Visual Computer (2000) 16:177–186.
     *
     * @param k The degree(order) of the matrix
     * @param i The time segment index
     * @return  Basic matrix
     */
    Eigen::MatrixXd M(int k, int i);

    /**
     * @brief A helper function to calculate d0 for producing th basic matrix M.
     *  d(0,j) = (t[i] - t[j]) / (t[j+k-1] - t[j]).
     * Defined in Qin, Kaihuai, General matrix representations for B-splines, The Visual Computer (2000) 16:177–186.
     *
     * @param k Spline degree(order)
     * @param i Time segment index
     * @param j Time segment index
     * @return  Value d0
     */
    double d0(int k, int i, int j);

    /**
     * @brief A helper function to calculate d1 for producing the M matrices.
     *  d(1,j) = (t[i+1] - t[i]) / (t[j+k-1] - t[j]).
     * Defined in Qin, Kaihuai, General matrix representations for B-splines, The Visual Computer (2000) 16:177–186.
     *
     * @param k Spline degree(order)
     * @param i Time segment index
     * @param j Time segment index
     * @return  Value d1
     */
    double d1(int k, int i, int j);

    /**
     * @brief An internal function to find the segment of the knot sequence that the time t falls in. The function
     * returns the value u = (t - t[i]) / (t[i+1] - t[i]) and the index i
     * @param t The time being queried
     * @return A pair with the first value u = (t - t[i]) / (t[i+1] - t[i]) and the second value the index i
     */
    std::pair<double, int> computeUAndTIndex(double t) const;

    /**
     * @brief An internal function to find the segment of the knot sequence that the time t falls in. The function
     * returns the width of the knot segment dt = t[i+1] - t[i] and the index i
     * @param t The time being queried
     * @return  The pair with the first value dt = t[i+1] -t[i] and the second value the index i
     */
    std::pair<double, int> computeTIndex(double t) const;

    /**
     * @brief Compute the vector U w.r.t t at given derivative order l, this is an k X 1 vector.
     *
     * At derivative order 0 (no derivative), this vector is U(t) = [1, u(t), u(t)^2, ... , u(t)^{k-1}]^T.
     * For higher derivative order n, the vector is U^{(l)}(t) = d^l U(t) / dt^{l}
     *
     * @param u                 The value u(t)
     * @param segmentIndex      The index of t
     * @param derivativeOrder   Derivative order l
     * @return The vector of dU
     */
    Eigen::VectorXd computeU(double u, int segmentIndex, int derivativeOrder) const;

    int basisMatrixIndexFromStartingKnotIndex(int startingKnotIndex) const;
    int startingKnotIndexFromBasisMatrixIndex(int basisMatrixIndex) const;
    const Eigen::MatrixXd& basisMatrixFromKnotIndex(int knotIndex) const;

  private:
    int splineOrder_;            // The order of the spline
    std::vector<double> knots_;  // The knot sequence used by the B-spline
    // The basis matrices M for each time segment, see Ref[2]. The basis matrix don't save all the matrix, only N =
    // valid time segments elements are saved, the first and last k-1 elements are 0
    std::vector<Eigen::MatrixXd> basisMatrices_;

    // The coefficient matrix(control points) used by the B-Spline. Each column can be seen as a single vector-valued
    // spline coefficient. This is stored explicitly in column major order to ensure that each column (i.e. a single
    // vector-valued spline coefficient) is stored in contiguous memory. This allows one to, for example, map a single
    // spline coefficient using the Eigen::Map type.
    // If the control points is V = [V_0, V_1,.., V_n], consider Vj is size of n1 x 1, then the size of V is
    // n1 x (n+1)
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> coefficients_;
};

}  // namespace bsplines

#endif /* _BSPLINE_HPP */


/*
BSD 2-Clause License

Copyright (c) 2019, Oscar Riveros - www.peqnp.science.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.

2. 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.

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

#include <iostream>
#include <random>

#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>

namespace mpi = boost::mpi;

typedef unsigned long long int natural;
typedef long double real;

template<class T, typename F>
class cluster {
public:
    cluster(const mpi::communicator &world, F functor) : __world(world), __functor(functor), __n(world.rank()), __m(world.size()) {}

    T operator()() {
        __n += __m;
        return __functor(__n - __m);
    }

private:
    const mpi::communicator &__world;
    const F __functor;
    T __n;
    T __m;
};

template<typename T>
T scale(const mpi::communicator &world, const T length) {
    return length / world.size() + world.size();
}

template<typename T, typename F>
std::vector<T> parallelize(const mpi::communicator &world, std::vector<T> &vs, F functor) {
    std::generate(vs.begin(), vs.end(), cluster<T, F>(world, functor));
    return vs;
}

template<typename T>
T accumulate(const mpi::communicator &world, std::vector<T> &vs, const T size) {
    real pi(0);
    mpi::all_reduce(world, 4 * std::accumulate(vs.begin(), vs.end(), real(0), std::plus<>()) / size, pi, std::plus<>());
    return pi;
}

int main() {
    mpi::environment env;
    mpi::communicator world;

    const natural size = 1000000;

    std::vector<real> vs(scale<natural>(world, size));

    std::random_device device;
    std::uniform_real_distribution<> distribution(0, 1);

    parallelize<real>(world, vs, [&](auto) {
        auto x = 2 * distribution(device) - 1;
        auto y = 2 * distribution(device) - 1;
        return std::pow(x, 2) + std::pow(y, 2) < 1;
    });

    auto pi = accumulate<real>(world, vs, size);

    if (!world.rank()) {
        std::cout << pi << std::endl;
    }

    return EXIT_SUCCESS;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE utility/random
#include <boost/test/unit_test.hpp>

#include <geneial/utility/Random.h>
using namespace geneial::utility;

BOOST_AUTO_TEST_SUITE( RandomSuite )

BOOST_AUTO_TEST_CASE( valid_values_ranges_bool )
{
    bool myRandBool = Random::generateBit();
    BOOST_CHECK(myRandBool == true || myRandBool == false);
}

#define TEST_INT_VALUERANGE_RUNS 1000
BOOST_AUTO_TEST_CASE( valid_values_ranges_int )
{
    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const int result = Random::generate<int>(0, 100);
        BOOST_CHECK(result >= 0 && result <= 100);
    }

    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const int result = Random::generate<int>(-100, 0);
        BOOST_CHECK(result >= -100 && result <= 0);
    }

    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const int result = Random::generate<int>(-100, 100);
        BOOST_CHECK(result >= -100 && result <= 100);
    }

    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const float result = Random::generate<float>(0, 100);
        BOOST_CHECK(result >= 0 && result <= 100);
    }

    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const float result = Random::generate<float>(-100, 0);
        BOOST_CHECK(result >= -100 && result <= 0);
    }

    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const float result = Random::generate<float>(-100, 100);
        BOOST_CHECK(result >= -100 && result <= 100);
    }

    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const double result = Random::generate<double>(0, 100);
        BOOST_CHECK(result >= 0 && result <= 100);
    }

    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const double result = Random::generate<double>(-100, 0);
        BOOST_CHECK(result >= -100 && result <= 0);
    }

    for (int i = 1; i < TEST_INT_VALUERANGE_RUNS; i++)
    {
        const double result = Random::generate<double>(-100, 100);
        BOOST_CHECK(result >= -100 && result <= 100);
    }
}

BOOST_AUTO_TEST_CASE( difference )
{
    double myRand1D = Random::generate<double>();
    double myRand2D = Random::generate<double>();
    BOOST_CHECK(myRand1D != myRand2D);

    float myRand1F = Random::generate<float>();
    float myRand2F = Random::generate<float>();
    std::cout << myRand1F << " - " << myRand2F;
    BOOST_CHECK(myRand1F != myRand2F);

    int myRand1I = Random::generate<int>();
    int myRand2I = Random::generate<int>();
    BOOST_CHECK(myRand1I != myRand2I);
}

//Make TEST_INT_UNIFORM_RUNS, see whether randomness divergates by TEST_INT_UNIFORM_TOLERANCE
#define TEST_INT_UNIFORM_RUNS (1e6)
#define TEST_INT_UNIFORM_SLOTS (10)
#define TEST_INT_UNIFORM_TOLERANCE (0.001f)

BOOST_AUTO_TEST_CASE( int_uniform )
{

    BOOST_TEST_MESSAGE("TEST_INT_UNIFORM_RUNS: " << TEST_INT_UNIFORM_RUNS);
    BOOST_TEST_MESSAGE("TEST_INT_UNIFORM_SLOTS: " << TEST_INT_UNIFORM_SLOTS);
    BOOST_TEST_MESSAGE("TEST_INT_UNIFORM_TOLERANCE: " << TEST_INT_UNIFORM_TOLERANCE);

    int occurrences[TEST_INT_UNIFORM_SLOTS];

    for (int i = 0; i < (TEST_INT_UNIFORM_SLOTS); i++)
    {
        occurrences[i] = 0;
    }

    for (int i = 1; i < TEST_INT_UNIFORM_RUNS; i++)
    {
        occurrences[Random::generate(0, TEST_INT_UNIFORM_SLOTS - 1)]++;
    }

    BOOST_TEST_MESSAGE("Int Distribution: ");
    for (int i = 0; i < (TEST_INT_UNIFORM_SLOTS); i++)
    {
        BOOST_TEST_MESSAGE(
                i << " " << std::string((int)100* ((float) occurrences[i] / (float) TEST_INT_UNIFORM_RUNS), '+'));
    }

    for (int i = 0; i < (TEST_INT_UNIFORM_SLOTS); i++)
    {
        BOOST_CHECK_MESSAGE(
                ((float) occurrences[i] / (float) TEST_INT_UNIFORM_RUNS) < ((float) TEST_INT_UNIFORM_RUNS / (float) TEST_INT_UNIFORM_SLOTS) / (float) TEST_INT_UNIFORM_RUNS + (float) TEST_INT_UNIFORM_TOLERANCE,
                "uniform failed for " << i << " with " << (float) occurrences[i] / (float) TEST_INT_UNIFORM_RUNS);
    }
}
#define TEST_DOUBLE_UNIFORM_RUNS 1e6
#define TEST_DOUBLE_UNIFORM_SLOTS 10
#define TEST_DOUBLE_UNIFORM_TOLERANCE 0.001f
#define TEST_DOUBLE_BOUND_TOLERANCE 0.00001f
BOOST_AUTO_TEST_CASE( double_uniform )
{

    BOOST_TEST_MESSAGE("TEST_DOUBLE_UNIFORM_RUNS: " << TEST_DOUBLE_UNIFORM_RUNS);
    BOOST_TEST_MESSAGE("TEST_DOUBLE_UNIFORM_SLOTS: " << TEST_DOUBLE_UNIFORM_SLOTS);
    BOOST_TEST_MESSAGE("TEST_DOUBLE_UNIFORM_TOLERANCE: " << TEST_DOUBLE_UNIFORM_TOLERANCE);

    int occurrences[TEST_DOUBLE_UNIFORM_SLOTS];

    for (int i = 0; i < (TEST_DOUBLE_UNIFORM_SLOTS); i++)
    {
        occurrences[i] = 0;
    }

    for (int i = 1; i < TEST_DOUBLE_UNIFORM_RUNS; i++)
    {
        const int val = (int) Random::generate<int>(0,
                TEST_DOUBLE_UNIFORM_SLOTS - TEST_DOUBLE_BOUND_TOLERANCE);
        occurrences[val]++;
    }

    BOOST_TEST_MESSAGE("Double Distribution: ");
    for (int i = 0; i < (TEST_DOUBLE_UNIFORM_SLOTS); i++)
    {
        BOOST_TEST_MESSAGE(
                i << " " << std::string((int)100* ((float) occurrences[i] / (float) TEST_DOUBLE_UNIFORM_RUNS), '+'));
    }

    for (int i = 0; i < (TEST_DOUBLE_UNIFORM_SLOTS); i++)
    {
        BOOST_CHECK_MESSAGE(
                ((float) occurrences[i] / (float) TEST_DOUBLE_UNIFORM_RUNS) < ((float) TEST_DOUBLE_UNIFORM_RUNS / (float) TEST_DOUBLE_UNIFORM_SLOTS) / (float) TEST_DOUBLE_UNIFORM_RUNS + (float) TEST_DOUBLE_UNIFORM_TOLERANCE,
                "uniform failed for " << i << " with " << (float) occurrences[i] / (float) TEST_DOUBLE_UNIFORM_RUNS);
    }
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Paul A. Bristow 2017
// Copyright John Z. Maddock 2017

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

/*! \brief Graph showing use of Lambert W function.

\details

Both Lambert W0 and W-1 branches can be shown on one graph.
But useful to have another graph for larger values of argument z.
Need two separate graphs for Lambert W0 and -1 prime because
the sensible ranges and axes are too different.

One would get too small LambertW0 in top right and W-1 in bottom left.

*/

#include <boost/math/special_functions/lambert_w.hpp>
using boost::math::lambert_w0;
using boost::math::lambert_wm1;
using boost::math::lambert_w0_prime;
using boost::math::lambert_wm1_prime;

#include <boost/math/special_functions.hpp>
using boost::math::isfinite;
#include <boost/svg_plot/svg_2d_plot.hpp>
using namespace boost::svg;
#include <boost/svg_plot/show_2d_settings.hpp>
using boost::svg::show_2d_plot_settings;

#include <iostream>
// using std::cout;
// using std::endl;
#include <exception>
#include <stdexcept>
#include <string>
#include <array>
#include <vector>
#include <utility>
using std::pair;
#include <map>
using std::map;
#include <set>
using std::multiset;
#include <limits>
using std::numeric_limits;
#include <cmath> //

  /*!
  */
int main()
{
  try
  {
    std::cout << "Lambert W graph example." << std::endl;

//[lambert_w_graph_1
//] [/lambert_w_graph_1]
    {
      std::map<const double, double> wm1s;   // Lambert W-1 branch values.
      std::map<const double, double> w0s;   // Lambert W0 branch values.

      std::cout.precision(std::numeric_limits<double>::max_digits10);

      int count = 0;
      for (double z = -0.36787944117144232159552377016146086744581113103176804; z < 2.8; z += 0.001)
      {
        double w0 = lambert_w0(z);
        w0s[z] = w0;
   //     std::cout << "z " << z << ", w = " << w0 << std::endl;
        count++;
      }
      std::cout << "points " << count << std::endl;

      count = 0;
      for (double z = -0.3678794411714423215955237701614608727; z < -0.001; z += 0.001)
      {
        double wm1 = lambert_wm1(z);
        wm1s[z] = wm1;
        count++;
      }
      std::cout << "points " << count << std::endl;

      svg_2d_plot data_plot;
      data_plot.title("Lambert W function.")
        .x_size(400)
        .y_size(300)
        .legend_on(true)
        .legend_lines(true)
        .x_label("z")
        .y_label("W")
        .x_range(-1, 3.)
        .y_range(-4., +1.)
        .x_major_interval(1.)
        .y_major_interval(1.)
        .x_major_grid_on(true)
        .y_major_grid_on(true)
        //.x_values_on(true)
        //.y_values_on(true)
        .y_values_rotation(horizontal)
        //.plot_window_on(true)
        .x_values_precision(3)
        .y_values_precision(3)
        .coord_precision(4) // Needed to avoid stepping on curves.
        .copyright_holder("Paul A. Bristow")
        .copyright_date("2018")
        //.background_border_color(black);
        ;
      data_plot.plot(w0s, "W0 branch").line_color(red).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plot.plot(wm1s, "W-1 branch").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plot.write("./lambert_w_graph");

      show_2d_plot_settings(data_plot); // For plot diagnosis only.

    } // small z Lambert W

    {  // bigger argument z Lambert W

      std::map<const double, double> w0s_big;   // Lambert W0 branch values for large z and W.
      std::map<const double, double> wm1s_big;   // Lambert W-1 branch values for small z and large -W.
      int count = 0;
      for (double z = -0.3678794411714423215955237701614608727; z < 10000.; z += 50.)
      {
        double w0 = lambert_w0(z);
        w0s_big[z] = w0;
        count++;
      }
      std::cout << "points " << count << std::endl;

      count = 0;
      for (double z = -0.3678794411714423215955237701614608727; z < -0.001; z += 0.001)
      {
        double wm1 = lambert_wm1(z);
        wm1s_big[z] = wm1;
        count++;
      }
     std::cout << "Lambert W0 large z argument points = " << count << std::endl;

     svg_2d_plot data_plot2;
     data_plot2.title("Lambert W0 function for larger z.")
      .x_size(400)
      .y_size(300)
      .legend_on(false)
      .x_label("z")
      .y_label("W")
      //.x_label_on(true)
      //.y_label_on(true)
      //.xy_values_on(false)
      .x_range(-1, 10000.)
      .y_range(-1., +8.)
      .x_major_interval(2000.)
      .y_major_interval(1.)
      .x_major_grid_on(true)
      .y_major_grid_on(true)
      //.x_values_on(true)
      //.y_values_on(true)
      .y_values_rotation(horizontal)
      //.plot_window_on(true)
      .x_values_precision(3)
      .y_values_precision(3)
      .coord_precision(4) // Needed to avoid stepping on curves.
      .copyright_holder("Paul A. Bristow")
      .copyright_date("2018")
      //.background_border_color(black);
    ;

    data_plot2.plot(w0s_big, "W0 branch").line_color(red).shape(none).line_on(true).bezier_on(false).line_width(1);
    // data_plot2.plot(wm1s_big, "W-1 branch").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
    // This wouldn't show anything useful.
    data_plot2.write("./lambert_w_graph_big_w");
   } // Big argument z Lambert W

    { //  Lambert W0 Derivative plots

    //  std::map<const double, double> wm1ps;   // Lambert W-1 prime branch values.
      std::map<const double, double> w0ps;   // Lambert W0 prime branch values.

      std::cout.precision(std::numeric_limits<double>::max_digits10);

      int count = 0;
      for (double z = -0.36; z < 3.; z += 0.001)
      {
        double w0p = lambert_w0_prime(z);
        w0ps[z] = w0p;
        // std::cout << "z " << z << ", w0 = " << w0 << std::endl;
        count++;
      }
      std::cout << "points " << count << std::endl;

      //count = 0;
      //for (double z = -0.36; z < -0.1; z += 0.001)
      //{
      //  double wm1p = lambert_wm1_prime(z);
      //  std::cout << "z " << z << ", w-1 = " << wm1p << std::endl;
      //  wm1ps[z] = wm1p;
      //  count++;
      //}
      //std::cout << "points " << count << std::endl;

      svg_2d_plot data_plotp;
      data_plotp.title("Lambert W0 prime function.")
        .x_size(400)
        .y_size(300)
        .legend_on(false)
        .x_label("z")
        .y_label("W0'")
        .x_range(-0.3, +1.)
        .y_range(0., +5.)
        .x_major_interval(0.2)
        .y_major_interval(2.)
        .x_major_grid_on(true)
        .y_major_grid_on(true)
        .y_values_rotation(horizontal)
        .x_values_precision(3)
        .y_values_precision(3)
        .coord_precision(4) // Needed to avoid stepping on curves.
        .copyright_holder("Paul A. Bristow")
        .copyright_date("2018")
        ;

      // derivative of N[productlog(0, x), 55]  at x=0 to 10
      // Plot[D[N[ProductLog[0, x], 55], x], {x, 0, 10}]
      // Plot[ProductLog[x]/(x + x ProductLog[x]), {x, 0, 10}]
      data_plotp.plot(w0ps, "W0 prime branch").line_color(red).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plotp.write("./lambert_w0_prime_graph");
  } // Lambert W0 Derivative plots

    { //  Lambert Wm1 Derivative plots

    std::map<const double, double> wm1ps;   // Lambert W-1 prime branch values.

    std::cout.precision(std::numeric_limits<double>::max_digits10);

    int count = 0;
    for (double z = -0.3678; z < -0.00001; z += 0.001)
    {
      double wm1p = lambert_wm1_prime(z);
      // std::cout << "z " << z << ", w-1 = " << wm1p << std::endl;
      wm1ps[z] = wm1p;
      count++;
    }
    std::cout << "Lambert W-1 prime points = " << count << std::endl;

    svg_2d_plot data_plotp;
    data_plotp.title("Lambert W-1 prime function.")
      .x_size(400)
      .y_size(300)
      .legend_on(false)
      .x_label("z")
      .y_label("W-1'")
      .x_range(-0.4, +0.01)
      .x_major_interval(0.1)
      .y_range(-20., -5.)
      .y_major_interval(5.)
      .x_major_grid_on(true)
      .y_major_grid_on(true)
      .y_values_rotation(horizontal)
      .x_values_precision(3)
      .y_values_precision(3)
      .coord_precision(4) // Needed to avoid stepping on curves.
      .copyright_holder("Paul A. Bristow")
      .copyright_date("2018")
      ;

      // derivative of N[productlog(0, x), 55]  at x=0 to 10
      // Plot[D[N[ProductLog[0, x], 55], x], {x, 0, 10}]
      // Plot[ProductLog[x]/(x + x ProductLog[x]), {x, 0, 10}]
      data_plotp.plot(wm1ps, "W-1 prime branch").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plotp.write("./lambert_wm1_prime_graph");
    } // Lambert W-1 prime graph
 } // try
  catch (std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
}  // int main()

   /*

   //[lambert_w_graph_1_output

   //] [/lambert_w_graph_1_output]
   */


//  Copyright 2015 Stephan Menzel. 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)

#include <tools/Random.hpp>

#include <boost/thread.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/bind.hpp>
#include <boost/ref.hpp>

#include <iostream>

struct door {
	door() : m_win(false), m_open(false) {};
	bool m_win;
	bool m_open;
};

enum strategy {
	STUBBORN = 0,  // will stick to original choice upon Monty revealing the goat
	MINDCHANGER,   // will always change his mind to the remaining door
	UNDECIDED      // reacts randomly
};

//! \return win or loose
bool round(const strategy n_strategy) {

	struct door doors[3];

	// First assign the door to win the car
	doors[moose::tools::urand(2)].m_win = true;

	// Now our player selects a door randomly
	unsigned int choice = moose::tools::urand(2);

	// Monty selects another random door with a goat to open
	unsigned int idx = 0;
	do {
		idx = moose::tools::urand(2);
	} while (doors[idx].m_win || (idx == choice));
						// we need a random index that looses
						// and was not the player's choice
	doors[idx].m_open = true;

	switch (n_strategy) {
		case STUBBORN:
			// player does not change his mind but sticks with his choice
			break;
		case UNDECIDED:
			// undecided player has a 50% chance to change his mind, otherwise
			// bail too and stick with his choice
			if (moose::tools::urand(100) < 50) {
				break;
			}
		case MINDCHANGER:
			// player always changes his mind and changes
			// his choice to the remaining door
			for (unsigned int i = 0; i < 3; ++i) {
				if (!doors[i].m_open && (i != choice)) {
					choice = i;
					break;
				}
			}
			break;
	}

	// player does not change his mind but sticks with his choice
	return doors[choice].m_win;
}

// return ratio of wins in percent
void player(double &n_result, const strategy n_strategy, const unsigned int n_rounds) {

	using namespace boost::accumulators;

	// accumulate all the results
	accumulator_set<double, stats<tag::mean> > acc;

	// play as many rounds and accumulate wins and loosses
	for (unsigned int i = 0; i < n_rounds; ++i) {

		bool result = round(n_strategy);
		acc(result ? 100.0 : 0.0);
	}

	n_result = mean(acc);
}

int main(int argc, char **argv) {

	unsigned int rounds = 1000000;

	double stubborn_wins = 0.0;
	double mindchanger_wins = 0.0;
	double undecided_wins = 0.0;

	std::cout << "players going to work..." << std::endl;

	boost::thread sp = boost::thread(boost::bind(&player, boost::ref(stubborn_wins),    STUBBORN,    rounds));
	boost::thread mp = boost::thread(boost::bind(&player, boost::ref(mindchanger_wins), MINDCHANGER, rounds));
	boost::thread up = boost::thread(boost::bind(&player, boost::ref(undecided_wins),   UNDECIDED,   rounds));

	sp.join();
	mp.join();
	up.join();

	std::cout << "Stubborn player won " << stubborn_wins << " percent of his " << rounds << " rounds." << std::endl;
	std::cout << "Mindchanger player won " << mindchanger_wins << " percent of his " << rounds << " rounds." << std::endl;
	std::cout << "Undecided player won " << undecided_wins << " percent of his " << rounds << " rounds." << std::endl;
}


#pragma once

#include "globla.hpp"

#include <Eigen/Eigen>

struct Plane3D {
    /* Plane normal. */
    vec3 normal;
    /* Plane center. */
    vec3 center;

    /* Points on plane. */
    std::vector<vec3> points;
    /* Points on plane's border. */
    std::vector<vec3> hull;

    /* Default constructor */
    Plane3D();
    /* Construct plane with normal vector and center point */
    Plane3D(vec3 const &normal, vec3 const &center);
    /* Construct plane with parameter `theta`, all points p(x, y, z) in plane
     * satisfies:
     *  \theta_0 + \theta_1 x + \theta_2 y = z.
     */
    Plane3D(Eigen::Vector3d const &theta);

    /* Get distance from `point` to this plane. */
    flt dist(vec3 const &point) const;
    /* Get projection of `point` on this plane. */
    vec3 project(vec3 const &from) const;
    /* Get the bases of this plane */
    std::pair<vec3, vec3> get_base() const;
    /* Get 2D coordinate of `point` (inside this plane). */
    vec2 get_coord_2d(vec3 const &point) const;
    /* Get 3D location of given coordinate (3d vector) */
    vec3 get_coord_3d(vec2 const &coord) const;
    /* Compute plane border, need member `points` to be non-empty. */
    bool compute_hull();
    /* Compute `normal` and `center` based on the points from this `plane` 's
     * hull.
     */
    bool compute_params();

    bool dump_off(std::string const &filename) const;
};

// Author: Blurgy <gy@blurgy.xyz>
// Date:   Mar 24 2021, 14:08 [CST]


#include <array>
#include <chrono>
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include "bicycle/whipple.h"
#include "parameters.h"

namespace {
    const double fs = 200; // sample rate [Hz]
    const double dt = 1.0/fs; // sample time [s]
    const double v0 = 4.0; // forward speed [m/s]
    const size_t N = 1000; // length of simulation in samples

    std::array<model::BicycleWhipple::state_t, N> continuous_time_system_state_n;
    std::array<model::BicycleWhipple::state_t, N> continuous_time_system_state_0;
    std::array<model::BicycleWhipple::state_t, N> discrete_time_system_state_n;
    std::array<model::BicycleWhipple::state_t, N> discrete_time_system_state_0;
} // namespace

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

    model::BicycleWhipple bicycle(v0, dt);

    std::chrono::time_point<std::chrono::system_clock> cont_start, cont_stop;
    std::chrono::time_point<std::chrono::system_clock> disc_start, disc_stop;

    cont_start = std::chrono::system_clock::now();
    bicycle.set_v_dt(v0, 0);
    cont_stop = std::chrono::system_clock::now();

    disc_start = std::chrono::system_clock::now();
    bicycle.set_v_dt(v0, dt);
    disc_stop = std::chrono::system_clock::now();

    std::chrono::duration<double> cont_time = cont_stop - cont_start;
    std::chrono::duration<double> disc_time = disc_stop - disc_start;
    disc_time -= cont_time;
    std::cout << "time for continuous state space computation: " <<
        std::chrono::duration_cast<std::chrono::microseconds>(cont_time).count() <<
        " us" << std::endl;
    std::cout << "(additional) time for discrete state space computation: " <<
        std::chrono::duration_cast<std::chrono::microseconds>(disc_time).count() <<
        " us" << std::endl;

    std::cout << "M: " << std::endl << bicycle.M() << std::endl;
    std::cout << "C1: " << std::endl << bicycle.C1() << std::endl;
    std::cout << "K0: " << std::endl << bicycle.K0() << std::endl;
    std::cout << "K2: " << std::endl << bicycle.K2() << std::endl << std::endl;

    std::cout << "for v = " << bicycle.v() << " m/s" << std::endl;
    std::cout << "A: " << std::endl << bicycle.A() << std::endl;
    std::cout << "B: " << std::endl << bicycle.B() << std::endl << std::endl;

    std::cout << "for fs = "  << fs << "  Hz" << std::endl;
    std::cout << "Ad: " << std::endl << bicycle.Ad() << std::endl;
    std::cout << "Bd: " << std::endl << bicycle.Bd() << std::endl << std::endl;

    model::BicycleWhipple::state_t x, x0;
    x0 << 0, 0, 10, 10, 0; // define in degrees
    x0 *= constants::as_radians;

    std::cout << "initial state: [" << x0.transpose() << "]' rad" << std::endl;
    std::cout << "states are: [yaw angle, roll angle, steer angle, roll rate, steer rate]'" << std::endl << std::endl;

    std::cout << "simulating (no input) continuous time system at constant speed..." << std::endl;
    x = x0;
    for (auto& state: continuous_time_system_state_n) {
        state = bicycle.integrate_state(x, model::BicycleWhipple::input_t::Zero(), dt);
        x = state;
    }

    std::cout << "simulating (zero input) continuous time system at constant speed..." << std::endl;
    cont_start = std::chrono::system_clock::now();
    x = x0;
    for (auto& state: continuous_time_system_state_0) {
        state = bicycle.integrate_state(x, model::BicycleWhipple::input_t::Zero(), dt);
        x = state;
    }
    cont_stop = std::chrono::system_clock::now();

    std::cout << "simulating (no input) discrete time system at constant speed..." << std::endl;
    x = x0;
    for (auto& state: discrete_time_system_state_n) {
        state = bicycle.update_state(x);
        x = state;
    }

    std::cout << "simulating (zero input) discrete time system at constant speed..." << std::endl;
    disc_start = std::chrono::system_clock::now();
    x = x0;
    for (auto& state: discrete_time_system_state_0) {
        state = bicycle.update_state(x, model::BicycleWhipple::input_t::Zero());
        x = state;
    }
    disc_stop = std::chrono::system_clock::now();

    std::cout << std::endl;
    std::cout << "state at end of simulation (" << N << " steps)" << std::endl;
    std::cout << "continuous time (no input):   " << continuous_time_system_state_n.back().transpose() << std::endl;
    std::cout << "continuous time (zero input): " << continuous_time_system_state_0.back().transpose() << std::endl;
    std::cout << "discrete time (no input):     " << discrete_time_system_state_n.back().transpose() << std::endl;
    std::cout << "discrete time (zero input):   " << discrete_time_system_state_0.back().transpose() << std::endl;
    std::cout << std::endl;

    cont_time = cont_stop - cont_start;
    disc_time = disc_stop - disc_start;
    std::cout << "simulation time (zero input form)" << std::endl;
    std::cout << "continuous: Tc = " <<
        std::chrono::duration_cast<std::chrono::microseconds>(cont_time).count() <<
        " us" << std::endl;
    std::cout << "discrete: Td = " <<
        std::chrono::duration_cast<std::chrono::microseconds>(disc_time).count() <<
        " us" << std::endl;
    std::cout << "Tc - Td = " <<
        std::chrono::duration_cast<std::chrono::microseconds>(cont_time - disc_time).count() <<
        " us" << std::endl;

    return EXIT_SUCCESS;
}


/**
 * @file quic_svd_test.cpp
 * @author Siddharth Agrawal
 *
 * Test file for QUIC-SVD class.
 */

#include <mlpack/core.hpp>
#include <mlpack/methods/quic_svd/quic_svd.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(QUICSVDTest);

using namespace mlpack;

/**
 * The reconstruction error of the obtained SVD should be small.
 */
BOOST_AUTO_TEST_CASE(QUICSVDReconstructionError)
{
  // Load the dataset.
  arma::mat dataset;
  data::Load("test_data_3_1000.csv", dataset);

  // Obtain the SVD using default parameters.
  arma::mat u, v, sigma;
  svd::QUIC_SVD quicsvd(dataset, u, v, sigma);

  // Reconstruct the matrix using the SVD.
  arma::mat reconstruct;
  reconstruct = u * sigma * v.t();

  // The relative reconstruction error should be small.
  double relativeError = arma::norm(dataset - reconstruct, "frob") /
                         arma::norm(dataset, "frob");
  BOOST_REQUIRE_SMALL(relativeError, 1e-5);
}

/**
 * The singular value error of the obtained SVD should be small.
 */
BOOST_AUTO_TEST_CASE(QUICSVDSigularValueError)
{
  arma::mat U = arma::randn<arma::mat>(3, 20);
  arma::mat V = arma::randn<arma::mat>(10, 3);

  arma::mat R;
  arma::qr_econ(U, R, U);
  arma::qr_econ(V, R, V);

  arma::mat s = arma::diagmat(arma::vec("1 0.1 0.01"));

  arma::mat data = arma::trans(U * arma::diagmat(s) * V.t());

  arma::vec s1, s3;
  arma::mat U1, U2, V1, V2, s2;

  // Obtain the SVD using default parameters.
  arma::svd_econ(U1, s1, V1, data);
  svd::QUIC_SVD quicsvd(data, U1, V1, s2);

  s3 = arma::diagvec(s2);
  s1 = s1.subvec(0, s3.n_elem - 1);

  // The sigular value error should be small.
  double error = arma::norm(s1 - s3);
  BOOST_REQUIRE_SMALL(error, 0.05);
}

BOOST_AUTO_TEST_SUITE_END();


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

// For IO
#include <stdlib.h>
#include <math.h>
#include <cmath>
#include <iostream>
#include <unistd.h>
#include <vector>

// Plotting library
#include <boost/tuple/tuple.hpp>
#include "gnuplot-iostream.h"
#include <utility>

// For measuring elapsed time
#include <chrono>

// For Random Float Generator
#include <time.h>

// For object under test
#include "detector.h"
#include "qam-modulator.h"
#include "common.h"
#include "fftw3.h"

/**
* Test NYQUIST MODULATOR
*
*/
BOOST_AUTO_TEST_SUITE(DetectorTest)

/**
*
*/
BOOST_AUTO_TEST_CASE(CorrelatorTest)
{
    printf("\nTesting Correlator...\n");

    size_t nPoints = 512;
    size_t symbolSize = nPoints*2;
    size_t prefixSize = (int) (symbolSize / 8);
    size_t symbolSizeWithPrefx = symbolSize + prefixSize;
    size_t pilotToneStep = 8;
    size_t bitsPerSymbol = 2;
    double pilotToneAmplitude = 2.0;
    size_t energyDispersalSeed = 10;
    size_t nAvaiableifftPoints = (nPoints - (int)(nPoints/pilotToneStep));
    size_t nMaxEncodedBytes = (int)((nAvaiableifftPoints *  bitsPerSymbol)  / 8);
    size_t nData = nMaxEncodedBytes;

    // Setup random float generator
    srand( (unsigned)time( NULL ) );

    //double modulatorOutput[symbolSizeWithPrefx];
    DoubleVec modulatorOutput;
    modulatorOutput.resize(symbolSizeWithPrefx);

    // Create rx signal array to capable of holding 20 upsampled symbols with prefix
    size_t rxSignalSize = (symbolSizeWithPrefx * 10);
    size_t rxLastAllowedIndex = (symbolSizeWithPrefx * 9);

    DoubleVec rxSignal(rxSignalSize);
    DoubleVec corellatorOutput(rxSignalSize);
    ByteVec txBytes(nData);

    // Initialize Encoder objects
    QamModulator qam(nPoints, pilotToneStep, pilotToneAmplitude, energyDispersalSeed, bitsPerSymbol);
    ofdmFFT ifft(nPoints, FFTW_BACKWARD, pilotToneStep);
    NyquistModulator nyquistModulator(nPoints, ifft.out);

    // Initialize Decoder objects
    ofdmFFT fft(nPoints, FFTW_FORWARD, pilotToneStep);
    NyquistModulator nyquistDemodulator(nPoints, fft.in);
    Detector detector(nPoints, prefixSize, &fft, &nyquistDemodulator);

    // Generate Random Bytes
    for(size_t i = 0; i < nData; i++)
    {
        txBytes[i] = rand() % 255;
    }

    // Generate random index value for symbol start
    size_t  symbolStart = rand() % rxLastAllowedIndex;
    printf("Randomly Generated Symbol Start = %lu\n",symbolStart);

    // Encode one symbol
    qam.Modulate(txBytes, (DoubleVec &) ifft.in, nData);
    ifft.ComputeTransform( (fftw_complex *) &modulatorOutput[prefixSize]);
    nyquistModulator.Modulate( modulatorOutput, prefixSize);
    AddCyclicPrefix(modulatorOutput, symbolSize , prefixSize);
    // Check Prefix Has been added correctly
    for(size_t i = 0 ; i < prefixSize; i++)
    {
        //modulatorOutput[i] = modulatorOutput[symbolSize+i];

        if(modulatorOutput[i] != modulatorOutput[symbolSize + i])
        {
            printf("Missmatch symbolwithprefix[%lu]  = %f, modulatorOutput[%lu] = %f \n" , i , modulatorOutput[i], (symbolSize + i) , modulatorOutput[(symbolSize + i)]);
        }
    }

    // Copy the symbol with prefix to Rx signal array
    std::copy(modulatorOutput.begin(), modulatorOutput.begin()+symbolSizeWithPrefx, rxSignal.begin()+symbolStart);

    // Check if the symbol with prefix has been copied correctly
    for(size_t i = 0 ; i < symbolSizeWithPrefx ; i++)
    {
        if(rxSignal[symbolStart+i] != modulatorOutput[i])
        {
            printf("Missmatch rxSignal[%lu]  = %f, symbolwithprefix[%lu] = %f \n" , symbolStart+i , rxSignal[symbolStart+i], i , modulatorOutput[i]);
        }
    }

    auto start = std::chrono::steady_clock::now();
    for(size_t i = 0; i < rxLastAllowedIndex; i++)
    {
        corellatorOutput[i] = detector.ExecuteCorrelator(rxSignal, i);
    }
    auto end = std::chrono::steady_clock::now();

    std::cout << "Cross-Corellator elapsed time: "
    << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
    << " ns" << std::endl;

    /*
    // Plot modulator output, symbol
    std::vector<double> modOutput(symbolSizeWithPrefx);
    for(uint32_t i = 0; i < symbolSizeWithPrefx; i++)
    {
        modOutput.at(i) = modulatorOutput[i];
    }

    // Plot Corellator output
    std::vector<double> corOutput(rxLastAllowedIndex);
    // Copy Correlator output to vector
    for(uint32_t i = 0; i < rxLastAllowedIndex; i++)
    {
        // Square Result to reduce noise
        corOutput.at(i) = corellatorOutput[i] * corellatorOutput[i];
    }

    Gnuplot gp;
    gp << "plot '-' with line title 'Correlation'\n";
    gp.send1d(corOutput);

    Gnuplot gp1;
    gp1 << "plot '-' with line title 'Symbol with prefix'\n";
    gp1.send1d(modOutput);
    */

    // Find Peak in correlation
    size_t peakIndex = 0;
    double max = 0.0;
    start = std::chrono::steady_clock::now();
    for (size_t i = 0; i < corellatorOutput.size(); i++)
    {
        if(corellatorOutput[i] > max)
        {
            max = corellatorOutput[i];
            peakIndex = i;
        }
    }
    end = std::chrono::steady_clock::now();

    std::cout << "Peak Search elapsed time: "
    << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
    << " ns" << std::endl;

    // Check if the correlator max output is where the symbol was inserted
    BOOST_CHECK_MESSAGE( (peakIndex == symbolStart),
    "Symbol start has not been detected correctly, The max correlation occurs at index =  " << peakIndex );

}

BOOST_AUTO_TEST_CASE(SymbolStartTest)
{
    printf("\nTesting Symbol Start Search...\n");

    size_t nPoints = 512;
    size_t symbolSize = nPoints*2;
    size_t prefixSize = (int) (symbolSize / 8);
    size_t symbolSizeWithPrefx = symbolSize + prefixSize;
    size_t pilotToneStep = 8;
    double pilotToneAmplitude = 2.0;
    size_t energyDispersalSeed = 10;
    size_t bitsPerSymbol = 2;

    // Setup random float generator
    srand( (unsigned)time( NULL ) );

    // Create rx signal array to capable of holding 20 upsampled symbols with prefix
    size_t  rxSignalSize = (symbolSizeWithPrefx * 10);
    size_t  rxLastAllowedIndex = (symbolSizeWithPrefx * 9);

    // Generate random index value for symbol start
    size_t  symbolStart = rand() % rxLastAllowedIndex;
    printf("Randomly Generated Symbol Start = %lu\n",symbolStart);

    size_t nAvaiableifftPoints = (nPoints - (int)(nPoints/pilotToneStep));
    size_t nMaxEncodedBytes = (int)((nAvaiableifftPoints *  bitsPerSymbol)  / 8);
    size_t nData = nMaxEncodedBytes;

    ByteVec txBytes(nData);
    DoubleVec EncoderOutput(symbolSizeWithPrefx);
    DoubleVec rxSignal(rxSignalSize);

    // Initialize Encoder objects
    QamModulator qam(nPoints, pilotToneStep, pilotToneAmplitude, energyDispersalSeed, bitsPerSymbol);
    ofdmFFT ifft(nPoints, FFTW_BACKWARD, pilotToneStep);
    NyquistModulator nyquistModulator(nPoints, ifft.out);

    // Initialize Decoder objects
    ofdmFFT fft(nPoints, FFTW_FORWARD, pilotToneStep);
    NyquistModulator nyquistDemodulator(nPoints, fft.in);
    Detector detector(nPoints, prefixSize, &fft, &nyquistDemodulator);

    // Randomly fill ifft input
    for(size_t i = 0; i < nData; i++)
    {
        txBytes[i] = rand() % 255;
    }

    // Encode one symbol
    // (fftw_complex *) &output[GetSettings().cyclicPrefixSize]);
    qam.Modulate(txBytes, (DoubleVec &) ifft.in, nData);
    ifft.ComputeTransform( (fftw_complex *) &EncoderOutput[prefixSize]);
    nyquistModulator.Modulate( EncoderOutput, prefixSize);
    AddCyclicPrefix(EncoderOutput, symbolSize , prefixSize);

    // Copy the symbol with prefix to Rx signal array
    std::copy(EncoderOutput.begin(), EncoderOutput.begin()+symbolSizeWithPrefx, rxSignal.begin()+symbolStart);
    // Check if the symbol with prefix has been copied correctly
    for(size_t i = 0 ; i < symbolSizeWithPrefx ; i++)
    {
        if(rxSignal[symbolStart+i] != EncoderOutput[i])
        {
            printf("Missmatch rxSignal[%lu]  = %f, symbolwithprefix[%lu] = %f \n" , symbolStart+i , rxSignal[symbolStart+i], i , EncoderOutput[i]);
        }
    }

    size_t CoarseSearchIndex = 0;
    auto start = std::chrono::steady_clock::now();
    CoarseSearchIndex = detector.CoarseSearch(rxSignal);
    auto end = std::chrono::steady_clock::now();

    std::cout << "Coarse Search elapsed time: "
    << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
    << " ns" << std::endl;

    // Check if the correlator max output is where the symbol was inserted
    BOOST_CHECK_MESSAGE( (CoarseSearchIndex == symbolStart),
    "Symbol start has not been detected correctly, The peak occurs at index: " << CoarseSearchIndex );

    printf("Coarse Search Index = %lu\n", CoarseSearchIndex);

    // Skip the prefix
    size_t symbolStartIndex = CoarseSearchIndex + prefixSize;
    printf("Symbol Start Index = %lu\n", symbolStartIndex);
    // Randomly change the Coarse Start by up to +/- 9 indexes
    size_t symbolStartOffsetIndex = symbolStartIndex + (rand() % 19 + (-9));

    start = std::chrono::steady_clock::now();
    size_t FineSearchSymbolIndex = detector.FineSearch(rxSignal, symbolStartOffsetIndex, nData);
    end = std::chrono::steady_clock::now();

    std::cout << "Fine Search elapsed time: "
    << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
    << " ns" << std::endl;

    printf("Fine Search Index = %lu\n", FineSearchSymbolIndex);

    // Check if the fine search corresponds to symbol start
    BOOST_CHECK_MESSAGE( (FineSearchSymbolIndex == symbolStart+prefixSize),
    "Symbol start has not been detected correctly, The lowest img sum occurs at start index: " << FineSearchSymbolIndex );

}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

int main(int argc, char** argv)
{

  // This program illustrates a simple use of boyer_myrvold_planar_embedding
  // as a simple yes/no test for planarity.

  using namespace boost;

  typedef adjacency_list<vecS,
                         vecS,
                         undirectedS,
                         property<vertex_index_t, int>
                         > graph;

  graph K_4(4);
  add_edge(0, 1, K_4);
  add_edge(0, 2, K_4);
  add_edge(0, 3, K_4);
  add_edge(1, 2, K_4);
  add_edge(1, 3, K_4);
  add_edge(2, 3, K_4);

  if (boyer_myrvold_planarity_test(K_4))
    std::cout << "K_4 is planar." << std::endl;
  else
    std::cout << "ERROR! K_4 should have been recognized as planar!"
          << std::endl;

  graph K_5(5);
  add_edge(0, 1, K_5);
  add_edge(0, 2, K_5);
  add_edge(0, 3, K_5);
  add_edge(0, 4, K_5);
  add_edge(1, 2, K_5);
  add_edge(1, 3, K_5);
  add_edge(1, 4, K_5);
  add_edge(2, 3, K_5);
  add_edge(2, 4, K_5);

  // We've almost created a K_5 - it's missing one edge - so it should still
  // be planar at this point.

  if (boyer_myrvold_planarity_test(K_5))
    std::cout << "K_5 (minus an edge) is planar." << std::endl;
  else
    std::cout << "ERROR! K_5 with one edge missing should"
              << " have been recognized as planar!" << std::endl;

  // Now add the final edge...
  add_edge(3, 4, K_5);

  if (boyer_myrvold_planarity_test(K_5))
    std::cout << "ERROR! K_5 was recognized as planar!" << std::endl;
  else
    std::cout << "K_5 is not planar." << std::endl;

  return 0;
}


// size.cpp : Calculates the size (or dimensions) of a Matrix
//            Example: size(A)
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
// Author: James Quinlan
//
// This file is part of the HPRBLAS project, which is released under an MIT Open Source license.

// COMMON LIBRARIES
#include <iostream>
#include <hprblas>

// #include <boost/numeric/mtl/mtl.hpp>  // not needed for size, using for other tests.  remove in production

// DEPENDENCIES
#include <matpak/rowsto.hpp>
#include <matpak/size.hpp>
#include <boost/numeric/mtl/mtl.hpp>

#include <generators/matrix_generators.hpp>

// Selects posits or floats
#define USE_POSIT 0

int main ()
{
    // COMMON NAMESPACES
	using namespace std;
	using namespace mtl; using mtl::iall;
	using namespace sw::unum;
	using namespace sw::hprblas;
	using namespace sw::hprblas::matpak;

    cout << setprecision(5);

#if USE_POSIT
    	constexpr size_t nbits = 32;
		constexpr size_t es = 2;
		using Scalar = posit<nbits, es>;
		using Matrix = mtl::mat::dense2D< Scalar >;
		cout << "\nUsing POSIT<" << nbits << "," <<  es << ">\n" <<  endl;
#else
		using Scalar = double;
		using Matrix = mtl::mat::dense2D< Scalar >;
#endif

		Matrix A = rowsto< Matrix >(7,7);   //
		cout << "Matrix A = \n" << A << endl;
		cout <<  "Size A = " << size(A) << endl;

	//	Matrix B = uniform_rand<Matrix>(6,6);
	//	cout << "Matrix B = \n" << B << endl;

	 //submatrix from matrix per irange
    using mtl::irange;
    irange row(2, 4), col(1, 7);
    dense2D<double> B1= A[row][col];
	 std::cout << "B1 is\n" << B1 << "\n";

	return 0;
}