#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

using std::cout;
using std::cin;
using std::endl;

using Eigen::MatrixXd;
using Eigen::MatrixXf;
using Eigen::Matrix;
using Eigen::VectorXd;
using Eigen::Matrix3d;
using Eigen::Matrix3f;
using Eigen::Vector3d;

#define LOGD(fmt, ...) fprintf(stdout, fmt, ##__VA_ARGS__)

float dotproduct_eigen(size_t len, float* va, float* vb)
{
    Eigen::Map<Eigen::Matrix<float, 1, Eigen::Dynamic, Eigen::RowMajor>> vva(va, len);
    Eigen::Map<Eigen::Matrix<float, 1, Eigen::Dynamic, Eigen::RowMajor>> vvb(vb, len);
    float res = vva.dot(vvb);
    return res;
}

static void matrix_add_f32_eigen(float* mA, float* mB, float* mC, const size_t M, const size_t N)
{
    using namespace Eigen;
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> eA(mA, M, N);
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> eB(mB, M, N);
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> eC(mC, M, N);
    eC = eA + eB;
}

// 例子1：创建2x2矩阵，逐元素赋值，然后输出
static void eigen_example1()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXd m(2,2); // MatrixXd是最常用的Eigen数据类型，X表示任意尺寸，d表示double。这里创建的是2x2规格的矩阵。
    m(0,0) = 3; // 追元素赋值，注意是用小括号，这应该是重载了括号操作符
    m(1,0) = 2.5;
    m(0,1) = -1;
    m(1,1) = m(1,0) + m(0,1);
    cout << m << endl; // 输出矩阵，直接std::cout即可，说明有重载<<操作符
}

// 矩阵乘以向量的例子。
static void eigen_example2()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXd m = MatrixXd::Random(3,3); //创建3x3矩阵，元素为[-1,1]之间的随机浮点数

    m = (m+MatrixXd::Constant(3, 3, 1.2)) * 50;//[-1,1] + 1.2 = [0.2, 2.2]; [0.2, 2.2]*50=[10, 110]
    // 也就是把原本在[-1,1]之间的各个元素，映射到[10, 110]之间

    cout << "m=" << endl << m << endl;

    VectorXd v(3); // 创建一个3行的列向量v

    v << 1, 2, 3; // 给列向量赋值，分别为1,2,3

    cout << "m*v=" << endl << m*v << endl; // 计算 m*v，也就是矩阵乘以向量，并输出结果
}

// eigen_example2的另一个版本：使用具体尺寸的类型，而不是MatrixXd和VectorXd
static void eigen_example2_2()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    Matrix3d m = Matrix3d::Random();
    m = (m+Matrix3d::Constant(1.2)) * 50;
    cout << "m=" << endl << m << endl;
    Vector3d v(1,2,3);
    cout << "m*v=" << endl << m*v << endl;
}

// 访问系数（元素）
static void eigen_example3()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXd m(2,2);
    m(0, 0) = 3;
    m(1, 0) = 2.5;
    m(0, 1) = -1;
    m(1, 1) = m(1, 0) + m(0, 1);
    cout << "Here is the matrix m:\n" << m << endl;

    VectorXd v(2);
    v(0) = 4;
    v(1) = v(0) - 1;
    cout << "Here is the vector v:\n" << v << endl;
}

// 用逗号表达式初始化
static void eigen_example4()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    Matrix3f m; // 声明

    // 初始化。注意：必须先声明，才能用逗号表达式初始化；定义时直接用逗号表达式初始化会报错
    // 因为这个初始化其实是通过重载<<操作符实现的
    m << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
    cout << m << endl;
}

// 输出维度信息、元素个数；动态尺寸矩阵/向量的resize
static void eigen_example5()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXd m(2, 5);
    m.resize(4, 3);
    printf("The matrix m is of size: rows=%d, cols=%d, num elements=%d\n",
        m.rows(), m.cols(), m.size());

    VectorXd v(2);
    v.resize(5);
    std::cout << "The vector v is of size " << v.size() << endl;
    printf("As a matrix, v is of size: rows=%d, cols=%d\n", v.rows(), v.cols());
}

// 动态尺寸矩阵赋值，如果尺寸不同，则自动把等号左边的矩阵resize
static void eigen_example5_2()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXf a(2, 2);
    printf("originally, a's size: row=%d, col=%d\n", a.rows(), a.cols());
    MatrixXf b(3, 3);
    a = b;
    printf("after assign, a's size: row=%d, col=%d\n", a.rows(), a.cols());
}

// eigen_example5_2的修改，从固定尺寸(3x3)的矩阵，赋值给动态维度的2x2矩阵，隐式resize
static void eigen_example5_3()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXf a(2, 2);
    printf("originally, a's size: row=%d, col=%d\n", a.rows(), a.cols());
    Matrix3f b;
    b << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
    a = b;
    printf("after assign, a's size: row=%d, col=%d\n", a.rows(), a.cols());
}

int main() {
    eigen_example1();
    eigen_example2();
    eigen_example2_2();
    eigen_example3();
    eigen_example4();
    eigen_example5();
    eigen_example5_2();
    eigen_example5_3();

    return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the running_stats object from the dlib C++
    Library.  It is a simple tool for computing basic statistics on a stream of numbers.
    In this example, we sample 100 points from the sinc function and then then compute the
    unbiased sample mean, variance, skewness, and excess kurtosis.

*/
#include <iostream>
#include <vector>
#include <dlib/statistics.h>

using namespace std;
using namespace dlib;

// Here we define the sinc function so that we may generate sample data. We compute the mean,
// variance, skewness, and excess kurtosis of this sample data.

double sinc(double x)
{
    if (x == 0)
        return 1;
    return sin(x)/x;
}

int main()
{
    running_stats<double> rs;

    double tp1 = 0;
    double tp2 = 0;

    // We first generate the data and add it sequentially to our running_stats object.  We
    // then print every fifth data point.
    for (int x = 1; x <= 100; x++)
    {
        tp1 = x/100.0;
        tp2 = sinc(pi*x/100.0);
        rs.add(tp2);

        if(x % 5 == 0)
        {
            cout << " x = " << tp1 << " sinc(x) = " << tp2 << endl;
        }
    }

    // Finally, we compute and print the mean, variance, skewness, and excess kurtosis of
    // our data.

    cout << endl;
    cout << "Mean:           " << rs.mean() << endl;
    cout << "Variance:       " << rs.variance() << endl;
    cout << "Skewness:       " << rs.skewness() << endl;
    cout << "Excess Kurtosis " << rs.ex_kurtosis() << endl;

    return 0;
}


// Copyright 2018 Hans Dembinski
//
// 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)

//[ guide_stdlib_algorithms

#include <boost/histogram.hpp>
#include <cassert>

#include <algorithm> // fill, any_of, min_element, max_element
#include <cmath>     // sqrt
#include <numeric>   // partial_sum, inner_product

int main() {
  using namespace boost::histogram;

  // make histogram that represents a probability density function (PDF)
  auto h1 = make_histogram(axis::regular<>(4, 1.0, 3.0));

  // use std::fill to set all counters to 0.25, including *flow cells
  std::fill(h1.begin(), h1.end(), 0.25);
  // reset *flow cells to zero
  h1.at(-1) = h1.at(4) = 0;

  // compute the cumulative density function (CDF), overriding cell values
  std::partial_sum(h1.begin(), h1.end(), h1.begin());

  assert(h1.at(-1) == 0.0);
  assert(h1.at(0) == 0.25);
  assert(h1.at(1) == 0.50);
  assert(h1.at(2) == 0.75);
  assert(h1.at(3) == 1.00);
  assert(h1.at(4) == 1.00);

  // use any_of to check if any cell values are smaller than 0.1,
  // and use indexed() to skip underflow and overflow cells
  auto h1_ind = indexed(h1);
  const auto any_small =
      std::any_of(h1_ind.begin(), h1_ind.end(), [](const auto& x) { return *x < 0.1; });
  assert(any_small == false); // underflow and overflow are zero, but skipped

  // find maximum element
  const auto max_it = std::max_element(h1.begin(), h1.end());
  assert(max_it == h1.end() - 2);

  // find minimum element
  const auto min_it = std::min_element(h1.begin(), h1.end());
  assert(min_it == h1.begin());

  // make second PDF
  auto h2 = make_histogram(axis::regular<>(4, 1.0, 4.0));
  h2.at(0) = 0.1;
  h2.at(1) = 0.3;
  h2.at(2) = 0.2;
  h2.at(3) = 0.4;

  // computing cosine similiarity: cos(theta) = A dot B / sqrt((A dot A) * (B dot B))
  const auto aa = std::inner_product(h1.begin(), h1.end(), h1.begin(), 0.0);
  const auto bb = std::inner_product(h2.begin(), h2.end(), h2.begin(), 0.0);
  const auto ab = std::inner_product(h1.begin(), h1.end(), h2.begin(), 0.0);
  const auto cos_sim = ab / std::sqrt(aa * bb);

  assert(std::abs(cos_sim - 0.78) < 1e-2);
}

//]


#define BOOST_TEST_MODULE test_utils

#include <boost/test/unit_test.hpp>
#include <Utils/utils.h>
#include <Utils/bshelper.h>

namespace utf = boost::unit_test;

BOOST_AUTO_TEST_SUITE(utils_boost)

    BOOST_AUTO_TEST_CASE(annual_cap1) {
        BOOST_TEST_MESSAGE("Testing annual_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 121; // (100*(1.1)^2)

        auto calculated_value = annual_capitalization(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(annual_discount1) {
        BOOST_TEST_MESSAGE("Testing annual_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 121;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 100; // (121/(1.1)^2)

        auto calculated_value = discount_annually(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(period_cap1) {
        BOOST_TEST_MESSAGE("Testing period_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int periods_per_year = 2;
        int number_of_years = 1;
        double theoretical_value = 110.25; // (100*(1.05)^2)

        auto calculated_value = period_capitalization(amount, annual_rate, periods_per_year, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(period_discount1) {
        BOOST_TEST_MESSAGE("Testing period_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 110.25;
        double annual_rate = 10.0 / 100;
        int periods_per_year = 2;
        int number_of_years = 1;
        double theoretical_value = 100; // (110.25/(1.05)^2)

        auto calculated_value = discount_by_periods(amount, annual_rate, periods_per_year, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(continuous_cap1) {
        BOOST_TEST_MESSAGE("Testing continuous_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 122.140275816; // 100 * e^(0.10*2) rounded to second

        auto calculated_value = continuous_capitalization(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));
    }

    BOOST_AUTO_TEST_CASE(continuous_discount1) {
        BOOST_TEST_MESSAGE("Testing continuous_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 122.140275816;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 100; // 122.140275816 / e^(0.10*2) rounded to second

        auto calculated_value = discount_continuously(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));
    }

    BOOST_AUTO_TEST_CASE(fwd_rate1) {
        BOOST_TEST_MESSAGE("Testing fwd_rate1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double zero_coupon_total = 0.04;
        int years_total = 2;

        double zero_coupon_partial = 0.03;
        int years_partial = 1;

        double theoretical_value = 0.05; // (100*(1.1)^2) rounded to second

        auto calculated_value = forward_rate(zero_coupon_total, years_total, zero_coupon_partial, years_partial);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(annual_to_cont1) {
        BOOST_TEST_MESSAGE("Testing annual_to_cont1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double annual_rate = 0.12;

        double theoretical_value = 0.1133286853; // ln(1.12)

        auto calculated_value = annual_to_continuous_rate(1, annual_rate);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-10));
    }

    BOOST_AUTO_TEST_CASE(cont_to_annual) {
        BOOST_TEST_MESSAGE("Testing cont_to_annual");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double continuous_rate = 0.11332868531; //(e^0.1)-1

        double theoretical_value = 0.12; // (e^0.1)-1

        auto calculated_value = continuous_to_annual_rate(1, continuous_rate);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-9));
    }

    BOOST_AUTO_TEST_CASE(total_day_count, *utf::tolerance(0.0001)) {
        BOOST_TEST_MESSAGE("Testing cont_to_annual");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double myDoubles[] = {185.0 / 360, 182.0 / 360, 182.0 / 360, 182.0 / 360};
        std::vector<double> dayCountFractionVector(myDoubles, myDoubles + sizeof(myDoubles) / sizeof(double));

        std::vector<double> calculated_values = getTotalDayCountFractionVector(dayCountFractionVector);

        double myResults[] = {0.513888888889, 1.019444444444, 1.525000000000, 2.030555555556};
        std::vector<double> expected_values(myResults, myResults + sizeof(myResults) / sizeof(double));

        BOOST_TEST(calculated_values == expected_values, boost::test_tools::per_element());

    }

    BOOST_AUTO_TEST_CASE(black_scholes_d1) {
        BOOST_TEST_MESSAGE("Testing black_scholes_d1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double asset_price = 50.0;
        double strike = 50.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double time_to_maturity = 5.0;

        double theoretical_value = 0.8251812; // (e^0.1)-1

        auto calculated_value = getD1(asset_price, strike, rate, volatility, time_to_maturity);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_d1_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-7));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_d2) {
        BOOST_TEST_MESSAGE("Testing black_scholes_d2");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double asset_price = 50.0;
        double strike = 50.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double time_to_maturity = 5.0;

        double theoretical_value = -0.5611809; // (e^0.1)-1

        auto calculated_value = getD2(asset_price, strike, rate, volatility, time_to_maturity);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_d2_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-7));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_weightAsset) {
        BOOST_TEST_MESSAGE("Testing black_scholes_weightAsset");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double asset_price = 50.0;
        double option_d1 = 0.8251812; //pre-calculated value

        double theoretical_value = 39.7682821; // (e^0.1)-1

        auto calculated_value = weightAsset(asset_price, option_d1);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-7));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_weightStrike) {
        BOOST_TEST_MESSAGE("Testing black_scholes_weightStrike");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double strike = 50.0;
        double rate = 3.66 / 100;
        double time_to_maturity = 5.0;
        double option_d2 = -0.5611809; //pre-calculated value

        double theoretical_value = 11.9642594; // (e^0.1)-1

        auto calculated_value = weightStrike(strike, option_d2, rate, time_to_maturity);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-7));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_evaluateCall) {
        BOOST_TEST_MESSAGE("Testing black_scholes_evaluateCall");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double option_strike = 50.0;
        double time_to_maturity = 5.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double asset_price = 50.0;

        double theoretical_value = 27.804023; // (e^0.1)-1

        auto calculated_value = evaluateCall(option_strike, time_to_maturity, rate, volatility, asset_price);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-6));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_evaluatePut) {
        BOOST_TEST_MESSAGE("Testing black_scholes_evaluatePut");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double option_strike = 50.0;
        double time_to_maturity = 5.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double asset_price = 50.0;

        double theoretical_value = 19.442431; // (e^0.1)-1

        auto calculated_value = evaluatePut(option_strike, time_to_maturity, rate, volatility, asset_price);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-6));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_blackScholesCall) {
        BOOST_TEST_MESSAGE("Testing black_scholes_blackScholesCall");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double option_strike = 50.0;
        double time_to_maturity = 5.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double asset_price = 50.0;

        double theoretical_value = 27.804023; // (e^0.1)-1

        auto calculated_value = blackScholes(call, option_strike, time_to_maturity, rate, volatility, asset_price);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-6));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_blackScholesPut) {
        BOOST_TEST_MESSAGE("Testing black_scholes_blackScholesPut");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double option_strike = 50.0;
        double time_to_maturity = 5.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double asset_price = 50.0;

        double theoretical_value = 19.442431; // (e^0.1)-1

        auto calculated_value = blackScholes(put, option_strike, time_to_maturity, rate, volatility, asset_price);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-6));
    }

BOOST_AUTO_TEST_SUITE_END()


/// @file example_geqr2.cpp
/// @author Weslley S Pereira, University of Colorado Denver, USA
//
// Copyright (c) 2022, University of Colorado Denver. All rights reserved.
//
// This file is part of <T>LAPACK.
// <T>LAPACK is free software: you can redistribute it and/or modify it under
// the terms of the BSD 3-Clause license. See the accompanying LICENSE file.

#include <iostream>

// Must be loaded in the following order
#include <plugins/tlapack_eigen.hpp>
#include <tlapack.hpp>

#include <Eigen/Dense>
#include <Eigen/Householder>

int main( int argc, char** argv )
{
    using std::size_t;
    using pair = std::pair<size_t,size_t>;
    using namespace blas;
    using namespace lapack;
    using Eigen::Matrix;

    // Constants
    const size_t m = 5;
    const size_t n = 3;

    // Input data
    Matrix<float, m, n> A {
        { 1,  2,  3},
        { 4,  5,  6},
        { 7,  8,  9},
        {10, 11, 12},
        {13, 14, 15}
    };

    // Matrices
    Matrix<float, m, n> Q = A;
    Matrix<float, n, n> R = Matrix<float, n, n>::Zero();
    Matrix<float, m, n> QtimesR = Matrix<float, m, n>::Zero();

    std::cout << "A = " << std::endl << A << std::endl << std::endl;

    // <T>LAPACK -----------------------------------------------

    std::cout << "--- <T>LAPACK: ---" << std::endl << std::endl;

    // Allocates memory
    Matrix<float, n, 1> tau;
    Matrix<float, n-1, 1> work;
    Matrix<float, n, n> orthQ;

    // Compute QR decomposision in place
    geqr2( Q, tau, work );
    // Copy the upper triangle to R
    lacpy( upper_triangle, submatrix(Q,pair{0,n},pair{0,n}), R );
    // Generate Q
    org2r( n, Q, tau, work );

    std::cout << "Q = " << std::endl << Q << std::endl;
    std::cout << std::endl;

    std::cout << "R = " << std::endl << R << std::endl;
    std::cout << std::endl;

    // Checking A = Q R
    lacpy( general_matrix, Q, QtimesR );
    trmm( Side::Right, Uplo::Upper, Op::NoTrans, Diag::NonUnit, 1.0, R, QtimesR );
    std::cout << "QR = " << std::endl << QtimesR << std::endl;
    QtimesR -= A;
    std::cout << "\\|QR - A\\|_F/\\|A\\|_F = " << std::endl << lange( frob_norm, QtimesR ) / lange( frob_norm, A ) << std::endl;
    std::cout << std::endl;

    // Checking orthogonality of Q
    orthQ = Matrix<float, n, n>::Identity();
    syrk( Uplo::Upper, Op::Trans, 1.0, Q, -1.0, orthQ );
    std::cout << "\\|Q^t Q - I\\|_F = " << std::endl << lansy( frob_norm, upper_triangle, orthQ ) << std::endl;
    std::cout << std::endl;

    // Eigen -----------------------------------------------

    std::cout << "--- Eigen: ---" << std::endl << std::endl;

    // Compute QR decomposision in place, possibly allocating memory dynamically
    Eigen::HouseholderQR<decltype(A)> qrEigen( A );
    // Generate Q
    Q = qrEigen.householderQ() * Matrix<float, m, n>::Identity();
    // Copy the upper triangle to R
    R = qrEigen.matrixQR().block(0,0,n,n).triangularView<Eigen::Upper>();

    std::cout << "Q = " << std::endl << Q << std::endl;
    std::cout << std::endl;

    std::cout << "R = " << std::endl << R << std::endl;
    std::cout << std::endl;

    // Checking A = Q R
    QtimesR = Q * R;
    std::cout << "QR = " << std::endl << QtimesR << std::endl;
    std::cout << "\\|QR - A\\|_F/\\|A\\|_F = " << (QtimesR-A).norm() / A.norm() << std::endl;
    std::cout << std::endl;

    // Checking orthogonality of Q
    orthQ = Q.transpose() * Q - Matrix<float, n, n>::Identity();
    std::cout << "\\|Q^t Q - I\\|_F = " << std::endl << orthQ.norm() << std::endl;
    std::cout << std::endl;

    return 0;
}


#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <set>
#include <map>
#include <iomanip>
#include <array>        // std::array

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <algorithm>

#include "container.h"

// for position matrix
#include <boost/numeric/ublas/matrix.hpp>

using namespace std;

// print a matrix
void print_matrix(boost::numeric::ublas::matrix<double> m)
{
    for (unsigned i = 0; i < m.size1(); ++ i)
    {
        for (unsigned j = 0; j < m.size2(); ++ j)
        {
            cout << m(i,j) << "\t";
        }
        cout << endl;
    }
}

double matrix_min(boost::numeric::ublas::matrix<double> x)
{
	double v = 1e100;
    for (int i = 0; i < x.size1(); ++ i)
        for (int j = 0; j < x.size2(); ++ j)
			if(x(i,j) < v)
				v = x(i,j);
	return v;
}

double matrix_max(boost::numeric::ublas::matrix<double> x)
{
	double v = -1e100;
    for (int i = 0; i < x.size1(); ++ i)
        for (int j = 0; j < x.size2(); ++ j)
			if(x(i,j) > v)
				v = x(i,j);
	return v;
}

// max of each column
vector<double> matrix_column_max(boost::numeric::ublas::matrix<double> x)
{
	vector<double> res;
    for (int j = 0; j < x.size2(); ++ j)
	{
		res.push_back(x(0,j));
		for (int i = 1; i < x.size1(); ++ i)
		{
			if(x(i,j) > res[j]) res[j] = x(i,j);
		}
	}
	return res;
}

// min of each column
vector<double> matrix_column_min(boost::numeric::ublas::matrix<double> x)
{
	vector<double> res;
    for (int j = 0; j < x.size2(); ++ j)
	{
		res.push_back(x(0,j));
		for (int i = 1; i < x.size1(); ++ i)
		{
			if(x(i,j) < res[j]) res[j] = x(i,j);
		}
	}
	return res;
}

// min of each column
vector<double> matrix_column_information_content(boost::numeric::ublas::matrix<double> x)
{
	vector<double> res;
    for (int j = 0; j < x.size2(); ++ j)
	{
		res.push_back(log2(x.size1()));
		for (int i = 0; i < x.size1(); ++ i)
		{
			if( x(i,j) > 0 && x(i,j)<= 1) res[j] += x(i,j) * log2(x(i,j));
			else if (x(i,j) < 0 || x(i,j)>1)
			{
				cerr << "ERROR: matrix_column_information_content, element out of range [0,1]" << endl;
				exit(1);
			}
		}
	}
	return res;
}

// positive:
// >0: only sum positive
// <0: only sum negative
// =0: both
vector<double> matrix_column_sum(boost::numeric::ublas::matrix<double> x, int positive)
{
	vector<double> res;
    for (int i = 0; i < x.size2(); ++ i) // column
	{
		res.push_back(0);
        for (int j = 0; j < x.size1(); ++ j) // row
		{
			if( positive > 0)
			{
				if (x(j,i)>0) res[i] += x(j,i);
			}
		    else if (positive <0)
			{
				if (x(j,i)<0) res[i] += x(j,i);
			}
			else
				res[i] += x(j,i);
			//cout << i << "\t" << res[i] << endl;
		}
	}
	return res;
}


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

// Copyright Jeremy W. Murphy 2015.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[polynomial_arithmetic_0
/*`First include the essential polynomial header (and others) to make the example:
*/
#include <boost/math/tools/polynomial.hpp>
//] [polynomial_arithmetic_0

#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/tools/assert.hpp>

#include <iostream>
#include <stdexcept>
#include <cmath>
#include <string>
#include <utility>

//[polynomial_arithmetic_1
/*`and some using statements are convenient:
*/

using std::string;
using std::exception;
using std::cout;
using std::abs;
using std::pair;

using namespace boost::math;
using namespace boost::math::tools; // for polynomial
using boost::lexical_cast;

//] [/polynomial_arithmetic_1]

template <typename T>
string sign_str(T const &x)
{
  return x < 0 ? "-" : "+";
}

template <typename T>
string inner_coefficient(T const &x)
{
  string result(" " + sign_str(x) + " ");
  if (abs(x) != T(1))
      result += lexical_cast<string>(abs(x));
  return result;
}

/*! Output in formula format.
For example: from a polynomial in Boost container storage  [ 10, -6, -4, 3 ]
show as human-friendly formula notation: 3x^3 - 4x^2 - 6x + 10.
*/
template <typename T>
string formula_format(polynomial<T> const &a)
{
  string result;
  if (a.size() == 0)
      result += lexical_cast<string>(T(0));
  else
  {
    // First one is a special case as it may need unary negate.
    unsigned i = a.size() - 1;
    if (a[i] < 0)
        result += "-";
    if (abs(a[i]) != T(1))
        result += lexical_cast<string>(abs(a[i]));

    if (i > 0)
    {
      result += "x";
      if (i > 1)
      {
          result += "^" + lexical_cast<string>(i);
          i--;
          for (; i != 1; i--)
              if (a[i])
                result += inner_coefficient(a[i]) + "x^" + lexical_cast<string>(i);

          if (a[i])
            result += inner_coefficient(a[i]) + "x";
      }
      i--;

      if (a[i])
        result += " " + sign_str(a[i]) + " " + lexical_cast<string>(abs(a[i]));
    }
  }
  return result;
} // string formula_format(polynomial<T> const &a)

int main()
{
  cout << "Example: Polynomial arithmetic.\n\n";

  try
  {
//[polynomial_arithmetic_2
/*`Store the coefficients in a convenient way to access them,
then create some polynomials using construction from an iterator range,
and finally output in a 'pretty' formula format.

[tip Although we might conventionally write a polynomial from left to right
in descending order of degree, Boost.Math stores in [*ascending order of degree].]

  Read/write for humans:    3x^3 - 4x^2 - 6x + 10
  Boost polynomial storage: [ 10, -6, -4, 3 ]
*/
  std::array<double, 4> const d3a = {{10, -6, -4, 3}};
  polynomial<double> const a(d3a.begin(), d3a.end());

  // With C++11 and later, you can also use initializer_list construction.
  polynomial<double> const b{{-2.0, 1.0}};

  // formula_format() converts from Boost storage to human notation.
  cout << "a = " << formula_format(a)
  << "\nb = " << formula_format(b) << "\n\n";

//] [/polynomial_arithmetic_2]

//[polynomial_arithmetic_3
  // Now we can do arithmetic with the usual infix operators: + - * / and %.
  polynomial<double> s = a + b;
  cout << "a + b = " << formula_format(s) << "\n";
  polynomial<double> d = a - b;
  cout << "a - b = " << formula_format(d) << "\n";
  polynomial<double> p = a * b;
  cout << "a * b = " << formula_format(p) << "\n";
  polynomial<double> q = a / b;
  cout << "a / b = " << formula_format(q) << "\n";
  polynomial<double> r = a % b;
  cout << "a % b = " << formula_format(r) << "\n";
//] [/polynomial_arithmetic_3]

//[polynomial_arithmetic_4
/*`
Division is a special case where you can calculate two for the price of one.

Actually, quotient and remainder are always calculated together due to the nature
of the algorithm: the infix operators return one result and throw the other
away.

If you are doing a lot of division and want both the quotient and remainder, then
you don't want to do twice the work necessary.

In that case you can call the underlying function, [^quotient_remainder],
to get both results together as a pair.
*/
  pair< polynomial<double>, polynomial<double> > result;
  result = quotient_remainder(a, b);
// Reassure ourselves that the result is the same.
  BOOST_MATH_ASSERT(result.first == q);
  BOOST_MATH_ASSERT(result.second == r);
//] [/polynomial_arithmetic_4]
//[polynomial_arithmetic_5
  /*
We can use the right and left shift operators to add and remove a factor of x.
This has the same semantics as left and right shift for integers where it is a
factor of 2. x is the smallest prime factor of a polynomial as is 2 for integers.
*/
    cout << "Right and left shift operators.\n";
    cout << "\n" << formula_format(p) << "\n";
    cout << "... right shift by 1 ...\n";
    p >>= 1;
    cout << formula_format(p) << "\n";
    cout << "... left shift by 2 ...\n";
    p <<= 2;
    cout << formula_format(p) << "\n";

/*
We can also give a meaning to odd and even for a polynomial that is consistent
with these operations: a polynomial is odd if it has a non-zero constant value,
even otherwise. That is:
    x^2 + 1     odd
    x^2         even
   */
    cout << std::boolalpha;
    cout << "\nPrint whether a polynomial is odd.\n";
    cout << formula_format(s) << "   odd? " << odd(s) << "\n";
    // We cheekily use the internal details to subtract the constant, making it even.
    s -= s.data().front();
    cout << formula_format(s) << "   odd? " << odd(s) << "\n";
    // And of course you can check if it is even:
    cout << formula_format(s) << "   even? " << even(s) << "\n";

    //] [/polynomial_arithmetic_5]
    //[polynomial_arithmetic_6]
    /* For performance and convenience, we can test whether a polynomial is zero
     * by implicitly converting to bool with the same semantics as int.    */
    polynomial<double> zero; // Default construction is 0.
    cout << "zero: " << (zero ? "not zero" : "zero") << "\n";
    cout << "r: " << (r ? "not zero" : "zero") << "\n";
    /* We can also set a polynomial to zero without needing a another zero
     * polynomial to assign to it. */
    r.set_zero();
    cout << "r: " << (r ? "not zero" : "zero") << "\n";
    //] [/polynomial_arithmetic_6]
}
catch (exception const &e)
{
  cout << "\nMessage from thrown exception was:\n   " << e.what() << "\n";
}
return 0;
} // int main()

/*
//[polynomial_output_1

a = 3x^3 - 4x^2 - 6x + 10
b = x - 2

//] [/polynomial_output_1]

//[polynomial_output_2

a + b = 3x^3 - 4x^2 - 5x + 8
a - b = 3x^3 - 4x^2 - 7x + 12
a * b = 3x^4 - 10x^3 + 2x^2 + 22x - 20
a / b = 3x^2 + 2x - 2
a % b = 6

//] [/polynomial_output_2]

*/


//------------------------------------------------------------------------------
// \file BooleanAlgebra_tests.cpp
// \ref https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_2-429h.pdf
//------------------------------------------------------------------------------
#include "Cpp/Utilities/SuperBitSet.h"
#include "Utilities/ToHexString.h"

#include <boost/test/unit_test.hpp>
#include <cmath>

using Cpp::Utilities::SuperBitSet;
using Utilities::ToHexString;

BOOST_AUTO_TEST_SUITE(Utilities)
BOOST_AUTO_TEST_SUITE(BooleanAlgebra_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateBitwiseOr)
{
  // cf. https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_2-429h.pdf
  // pp. 16
  // A | B = 1 when either A = 1 or B = 1
  {
    SuperBitSet<2> bits {"01"};
    BOOST_TEST_REQUIRE(bits.to_string() == "01");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 1);
    SuperBitSet<2> rhs_0 {0};
    BOOST_TEST_REQUIRE(rhs_0.to_string() == "00");
    BOOST_TEST_REQUIRE(rhs_0.to_ulong() == 0);
    BOOST_TEST((bits | rhs_0) == bits);
    BOOST_TEST((bits | rhs_0).to_string() == "01");
    SuperBitSet<2> rhs_1 {3};
    BOOST_TEST_REQUIRE(rhs_1.to_string() == "11");
    BOOST_TEST_REQUIRE(rhs_1.to_ulong() == 3);
    BOOST_TEST((bits | rhs_1) == rhs_1);
    BOOST_TEST((bits | rhs_1).to_string() == "11");
  }
  // Further example:
  {
    SuperBitSet<4> bits {"0101"};
    BOOST_TEST_REQUIRE(bits.to_string() == "0101");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 5);
    SuperBitSet<4> rhs {3};
    BOOST_TEST_REQUIRE(rhs.to_string() == "0011");
    BOOST_TEST_REQUIRE(rhs.to_ulong() == 3);
    SuperBitSet<4> expected_result {7};
    BOOST_TEST_REQUIRE(expected_result.to_string() == "0111");
    BOOST_TEST_REQUIRE(expected_result.to_ulong() == 7);
    BOOST_TEST((bits | rhs) == expected_result);
    BOOST_TEST((bits | rhs).to_string() == "0111");
  }

  // cf. https://en.wikipedia.org/wiki/Bitwise_operation OR
  // bitwise OR may be used to set to 1 selected bits of register described
  // above. e.g. fourth bit of 0010 may be set by performing bitwise OR with
  // pattern with only 4th bit set.
  {
    SuperBitSet<4> bits {"0010"};
    BOOST_TEST_REQUIRE(bits.to_string() == "0010");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 2);
    SuperBitSet<4> rhs {8};
    BOOST_TEST_REQUIRE(rhs.to_string() == "1000");
    BOOST_TEST_REQUIRE(rhs.to_ulong() == 8);
    SuperBitSet<4> expected_result {10};
    BOOST_TEST_REQUIRE(expected_result.to_string() == "1010");
    BOOST_TEST_REQUIRE(expected_result.to_ulong() == 10);
    BOOST_TEST((bits | rhs) == expected_result);
    BOOST_TEST((bits | rhs).to_string() == "1010");
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateBitwiseAnd)
{
  // cf. https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_2-429h.pdf
  // pp. 16
  // A & B = 1 when both A = 1 and B = 1
  {
    SuperBitSet<2> bits {"01"};
    BOOST_TEST_REQUIRE(bits.to_string() == "01");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 1);
    SuperBitSet<2> rhs_0 {0};
    BOOST_TEST_REQUIRE(rhs_0.to_string() == "00");
    BOOST_TEST_REQUIRE(rhs_0.to_ulong() == 0);
    BOOST_TEST((bits & rhs_0) == rhs_0);
    BOOST_TEST((bits & rhs_0).to_string() == "00");
    SuperBitSet<2> rhs_1 {3};
    BOOST_TEST_REQUIRE(rhs_1.to_string() == "11");
    BOOST_TEST_REQUIRE(rhs_1.to_ulong() == 3);
    BOOST_TEST((bits & rhs_1) == bits);
    BOOST_TEST((bits & rhs_1).to_string() == "01");
  }
  // Further example:
  {
    SuperBitSet<4> bits {"0101"};
    BOOST_TEST_REQUIRE(bits.to_string() == "0101");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 5);
    SuperBitSet<4> rhs {3};
    BOOST_TEST_REQUIRE(rhs.to_string() == "0011");
    BOOST_TEST_REQUIRE(rhs.to_ulong() == 3);
    SuperBitSet<4> expected_result {1};
    BOOST_TEST_REQUIRE(expected_result.to_string() == "0001");
    BOOST_TEST_REQUIRE(expected_result.to_ulong() == 1);
    BOOST_TEST((bits & rhs) == expected_result);
    BOOST_TEST((bits & rhs).to_string() == "0001");
  }

  // cf. https://en.wikipedia.org/wiki/Bitwise_operation AND
  // Thus, if both bits in compared position are 1, bit in resulting binary
  // representation is (1 x 1 = 1); otherwise result is 0 (1 x 0 = 0 and
  // 0 x 0 = 0).
  // Operation may be used to determine whether particular bit is set (1) or
  // clear (0).
  // This is often called bit masking (by analogy, use of masking tape covers,
  // or masks, portions that are not of interest)
  {
    SuperBitSet<4> bits {"0011"};
    BOOST_TEST_REQUIRE(bits.to_string() == "0011");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 3);
    SuperBitSet<4> rhs {2};
    BOOST_TEST_REQUIRE(rhs.to_string() == "0010");
    BOOST_TEST_REQUIRE(rhs.to_ulong() == 2);
    SuperBitSet<4> expected_result {2};
    BOOST_TEST_REQUIRE(expected_result.to_string() == "0010");
    BOOST_TEST_REQUIRE(expected_result.to_ulong() == 2);
    BOOST_TEST((bits & rhs) == expected_result);
    BOOST_TEST((bits & rhs).to_string() == "0010");

    BOOST_TEST(!(bits & rhs).all());
    BOOST_TEST((bits & rhs).any());
    BOOST_TEST(!(bits & rhs).none());
  }
  // bitwise AND may be used to clear selected bits (or flags) of a register in
  // which each bit represents an individual Boolean state.
  // This technique is an efficient way to store a number of Boolean values
  // using as little memory as possible.
  {
    SuperBitSet<4> bits {"0110"};
    BOOST_TEST_REQUIRE(bits.to_string() == "0110");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 6);

    // 3rd. flag may be cleared with pattern that has a 0 only in 3rd. bit.
    SuperBitSet<4> rhs {11};
    BOOST_TEST_REQUIRE(rhs.to_string() == "1011");
    BOOST_TEST_REQUIRE(rhs.to_ulong() == 11);
    SuperBitSet<4> expected_result {2};
    BOOST_TEST_REQUIRE(expected_result.to_string() == "0010");
    BOOST_TEST_REQUIRE(expected_result.to_ulong() == 2);
    BOOST_TEST((bits & rhs) == expected_result);
    BOOST_TEST((bits & rhs).to_string() == "0010");

    BOOST_TEST(!(bits & rhs).all());
    BOOST_TEST((bits & rhs).any());
    BOOST_TEST(!(bits & rhs).none());
  }

  // Also, easy to check parity (even or odd?) of binary number by checking
  // value of lowest valued bit.

  {
    SuperBitSet<4> bits {"0110"};
    BOOST_TEST_REQUIRE(bits.to_string() == "0110");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 6);
    SuperBitSet<4> rhs {0001};
    BOOST_TEST_REQUIRE(rhs.to_string() == "0001");
    BOOST_TEST_REQUIRE(rhs.to_ulong() == 1);
    SuperBitSet<4> expected_result {0};
    BOOST_TEST_REQUIRE(expected_result.to_string() == "0000");
    BOOST_TEST_REQUIRE(expected_result.to_ulong() == 0);
    BOOST_TEST((bits & rhs) == expected_result);
    BOOST_TEST((bits & rhs).to_string() == "0000");

    BOOST_TEST(!(bits & rhs).all());
    BOOST_TEST(!(bits & rhs).any());
    BOOST_TEST((bits & rhs).none());
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateBitwiseXor)
{
  // cf. https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_2-429h.pdf
  // pp. 16
  // A ^ B = 1 when either A = 1 or B = 1, but not both
  {
    SuperBitSet<2> bits {"01"};
    BOOST_TEST_REQUIRE(bits.to_string() == "01");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 1);
    SuperBitSet<2> rhs_0 {0};
    BOOST_TEST_REQUIRE(rhs_0.to_string() == "00");
    BOOST_TEST_REQUIRE(rhs_0.to_ulong() == 0);
    BOOST_TEST((bits ^ rhs_0) == bits);
    BOOST_TEST((bits ^ rhs_0).to_string() == "01");
    SuperBitSet<2> rhs_1 {3};
    BOOST_TEST_REQUIRE(rhs_1.to_string() == "11");
    BOOST_TEST_REQUIRE(rhs_1.to_ulong() == 3);
    BOOST_TEST((bits ^ rhs_1) == SuperBitSet<2>{"10"});
    BOOST_TEST((bits ^ rhs_1).to_string() == "10");
  }
  // Further example:
  {
    SuperBitSet<4> bits {"0101"};
    BOOST_TEST_REQUIRE(bits.to_string() == "0101");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 5);
    SuperBitSet<4> rhs {3};
    BOOST_TEST_REQUIRE(rhs.to_string() == "0011");
    BOOST_TEST_REQUIRE(rhs.to_ulong() == 3);
    SuperBitSet<4> expected_result {6};
    BOOST_TEST_REQUIRE(expected_result.to_string() == "0110");
    BOOST_TEST_REQUIRE(expected_result.to_ulong() == 6);
    BOOST_TEST((bits ^ rhs) == expected_result);
    BOOST_TEST((bits ^ rhs).to_string() == "0110");
  }

  // cf. https://en.wikipedia.org/wiki/Bitwise_operation XOR
  // bitwise XOR may be used to invert selected bits in a register (also called
  // toggle or flip). Any bit may be toggled by XORing with 1.
  {
    SuperBitSet<4> bits {"0010"};
    BOOST_TEST_REQUIRE(bits.to_string() == "0010");
    BOOST_TEST_REQUIRE(bits.to_ulong() == 2);
    SuperBitSet<4> rhs {10};
    BOOST_TEST_REQUIRE(rhs.to_string() == "1010");
    BOOST_TEST_REQUIRE(rhs.to_ulong() == 10);
    SuperBitSet<4> expected_result {8};
    BOOST_TEST_REQUIRE(expected_result.to_string() == "1000");
    BOOST_TEST_REQUIRE(expected_result.to_ulong() == 8);
    BOOST_TEST((bits ^ rhs) == expected_result);
    BOOST_TEST((bits ^ rhs).to_string() == "1000");
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
// NOT is also called the complement, or negation
BOOST_AUTO_TEST_CASE(DemonstrateBitwiseNot)
{
  {
    SuperBitSet<1> bits0 {"0"};
    BOOST_TEST_REQUIRE(bits0.to_string() == "0");
    BOOST_TEST((~bits0).to_string() == "1");

    SuperBitSet<1> bits1 {"1"};
    BOOST_TEST_REQUIRE(bits1.to_string() == "1");
    BOOST_TEST((~bits1).to_string() == "0");
  }

  // bitwise complement = two's complement of the value - 1. If 2's complement
  // arithmetic used, then NOT x = -x - 1
  // TODO: explore and understand the previous statement.
  {
    SuperBitSet<4> bits {7};
    BOOST_TEST(bits.to_string() == "0111");
    BOOST_TEST(bits.to_ulong() == 7);
    BOOST_TEST((~bits).to_string() == "1000");
    BOOST_TEST((~bits).to_ulong() == 8);
  }
  {
    SuperBitSet<8> bits {171};
    BOOST_TEST(bits.to_string() == "10101011");
    BOOST_TEST(bits.to_ulong() == 171);
    BOOST_TEST((~bits).to_string() == "01010100");
    BOOST_TEST((~bits).to_ulong() == 84);
  }
  // For unsigned ints, bitwise complement of number is "mirror reflection" of
  // number across half-way point of unsigned int's range;
  // e.g. for 8-bit unsigned int, NOT x = 255 - x, "flips" increasing range from
  // 0 to 255, to decreasing range from 255 to 0.

  {
    // cf. https://stackoverflow.com/questions/5040920/converting-from-signed-char-to-unsigned-char-and-back-again
    SuperBitSet<8> bits {static_cast<uint8_t>(static_cast<signed char>(-7))};
    BOOST_TEST(bits.to_string() == "11111001");
    BOOST_TEST(pow(2, 7) == 128);
    BOOST_TEST(pow(2, 6) == 64);
    BOOST_TEST(pow(2, 5) == 32);
    BOOST_TEST(pow(2, 4) == 16);
    BOOST_TEST(pow(2, 3) == 8);

    BOOST_TEST(
      pow(2, 0) + pow(2, 3) + pow(2, 4) + pow(2, 5) + pow(2, 6) == 121);
    BOOST_TEST(
      -pow(2,7) + pow(2, 0) + pow(2, 3) + pow(2, 4) + pow(2, 5) + pow(2, 6) ==
        -7);

    BOOST_TEST((~bits).to_string() == "00000110");
    BOOST_TEST((~bits).to_ulong() == 6);
  }
  {
    SuperBitSet<8> bits {0xff};
    BOOST_TEST(bits.to_string() == "11111111");
    BOOST_TEST(bits.to_ulong() == 255);
    BOOST_TEST((~bits).to_string() == "00000000");
    BOOST_TEST((~bits).to_ulong() == 0);
  }
  {
    SuperBitSet<8> bits {static_cast<uint8_t>(static_cast<signed char>(-128))};
    BOOST_TEST(bits.to_string() == "10000000");
    BOOST_TEST(bits.to_ulong() == 128);

    BOOST_TEST((~bits).to_string() == "01111111");
    BOOST_TEST((~bits).to_ulong() == 127);
  }
  {
    SuperBitSet<8> bits {static_cast<uint8_t>(static_cast<signed char>(-1))};
    BOOST_TEST(bits.to_string() == "11111111");
    BOOST_TEST(bits.to_ulong() == 255);

    BOOST_TEST((~bits).to_string() == "00000000");
    BOOST_TEST((~bits).to_ulong() == 0);
  }
}

BOOST_AUTO_TEST_SUITE_END() // BooleanAlgebra_tests
BOOST_AUTO_TEST_SUITE_END() // Utilities


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

#ifdef _MSC_VER
#  pragma warning(disable: 4512) // assignment operator could not be generated.
#  pragma warning(disable: 4510) // default constructor could not be generated.
#  pragma warning(disable: 4610) // can never be instantiated - user defined constructor required.
#endif

#include <iostream>
#include <iomanip>
#include <boost/math/distributions/students_t.hpp>

void two_samples_t_test_equal_sd(
        double Sm1,
        double Sd1,
        unsigned Sn1,
        double Sm2,
        double Sd2,
        unsigned Sn2,
        double alpha)
{
   //
   // Sm1 = Sample Mean 1.
   // Sd1 = Sample Standard Deviation 1.
   // Sn1 = Sample Size 1.
   // Sm2 = Sample Mean 2.
   // Sd2 = Sample Standard Deviation 2.
   // Sn2 = Sample Size 2.
   // alpha = Significance Level.
   //
   // A Students t test applied to two sets of data.
   // We are testing the null hypothesis that the two
   // samples have the same mean and that any difference
   // if due to chance.
   // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda353.htm
   //
   using namespace std;
   using namespace boost::math;

   // Print header:
   cout <<
      "_______________________________________________\n"
      "Student t test for two samples (equal variances)\n"
      "_______________________________________________\n\n";
   cout << setprecision(5);
   cout << setw(55) << left << "Number of Observations (Sample 1)" << "=  " << Sn1 << "\n";
   cout << setw(55) << left << "Sample 1 Mean" << "=  " << Sm1 << "\n";
   cout << setw(55) << left << "Sample 1 Standard Deviation" << "=  " << Sd1 << "\n";
   cout << setw(55) << left << "Number of Observations (Sample 2)" << "=  " << Sn2 << "\n";
   cout << setw(55) << left << "Sample 2 Mean" << "=  " << Sm2 << "\n";
   cout << setw(55) << left << "Sample 2 Standard Deviation" << "=  " << Sd2 << "\n";
   //
   // Now we can calculate and output some stats:
   //
   // Degrees of freedom:
   double v = Sn1 + Sn2 - 2;
   cout << setw(55) << left << "Degrees of Freedom" << "=  " << v << "\n";
   // Pooled variance:
   double sp = sqrt(((Sn1-1) * Sd1 * Sd1 + (Sn2-1) * Sd2 * Sd2) / v);
   cout << setw(55) << left << "Pooled Standard Deviation" << "=  " << v << "\n";
   // t-statistic:
   double t_stat = (Sm1 - Sm2) / (sp * sqrt(1.0 / Sn1 + 1.0 / Sn2));
   cout << setw(55) << left << "T Statistic" << "=  " << t_stat << "\n";
   //
   // Define our distribution, and get the probability:
   //
   students_t dist(v);
   double q = cdf(complement(dist, fabs(t_stat)));
   cout << setw(55) << left << "Probability that difference is due to chance" << "=  "
      << setprecision(3) << scientific << 2 * q << "\n\n";
   //
   // Finally print out results of alternative hypothesis:
   //
   cout << setw(55) << left <<
      "Results for Alternative Hypothesis and alpha" << "=  "
      << setprecision(4) << fixed << alpha << "\n\n";
   cout << "Alternative Hypothesis              Conclusion\n";
   cout << "Sample 1 Mean != Sample 2 Mean       " ;
   if(q < alpha / 2)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Sample 1 Mean <  Sample 2 Mean       ";
   if(cdf(dist, t_stat) < alpha)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Sample 1 Mean >  Sample 2 Mean       ";
   if(cdf(complement(dist, t_stat)) < alpha)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << endl << endl;
}

void two_samples_t_test_unequal_sd(
        double Sm1,
        double Sd1,
        unsigned Sn1,
        double Sm2,
        double Sd2,
        unsigned Sn2,
        double alpha)
{
   //
   // Sm1 = Sample Mean 1.
   // Sd1 = Sample Standard Deviation 1.
   // Sn1 = Sample Size 1.
   // Sm2 = Sample Mean 2.
   // Sd2 = Sample Standard Deviation 2.
   // Sn2 = Sample Size 2.
   // alpha = Significance Level.
   //
   // A Students t test applied to two sets of data.
   // We are testing the null hypothesis that the two
   // samples have the same mean and that any difference
   // if due to chance.
   // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda353.htm
   //
   using namespace std;
   using namespace boost::math;

   // Print header:
   cout <<
      "_________________________________________________\n"
      "Student t test for two samples (unequal variances)\n"
      "_________________________________________________\n\n";
   cout << setprecision(5);
   cout << setw(55) << left << "Number of Observations (Sample 1)" << "=  " << Sn1 << "\n";
   cout << setw(55) << left << "Sample 1 Mean" << "=  " << Sm1 << "\n";
   cout << setw(55) << left << "Sample 1 Standard Deviation" << "=  " << Sd1 << "\n";
   cout << setw(55) << left << "Number of Observations (Sample 2)" << "=  " << Sn2 << "\n";
   cout << setw(55) << left << "Sample 2 Mean" << "=  " << Sm2 << "\n";
   cout << setw(55) << left << "Sample 2 Standard Deviation" << "=  " << Sd2 << "\n";
   //
   // Now we can calculate and output some stats:
   //
   // Degrees of freedom:
   double v = Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2;
   v *= v;
   double t1 = Sd1 * Sd1 / Sn1;
   t1 *= t1;
   t1 /=  (Sn1 - 1);
   double t2 = Sd2 * Sd2 / Sn2;
   t2 *= t2;
   t2 /= (Sn2 - 1);
   v /= (t1 + t2);
   cout << setw(55) << left << "Degrees of Freedom" << "=  " << v << "\n";
   // t-statistic:
   double t_stat = (Sm1 - Sm2) / sqrt(Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2);
   cout << setw(55) << left << "T Statistic" << "=  " << t_stat << "\n";
   //
   // Define our distribution, and get the probability:
   //
   students_t dist(v);
   double q = cdf(complement(dist, fabs(t_stat)));
   cout << setw(55) << left << "Probability that difference is due to chance" << "=  "
      << setprecision(3) << scientific << 2 * q << "\n\n";
   //
   // Finally print out results of alternative hypothesis:
   //
   cout << setw(55) << left <<
      "Results for Alternative Hypothesis and alpha" << "=  "
      << setprecision(4) << fixed << alpha << "\n\n";
   cout << "Alternative Hypothesis              Conclusion\n";
   cout << "Sample 1 Mean != Sample 2 Mean       " ;
   if(q < alpha / 2)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Sample 1 Mean <  Sample 2 Mean       ";
   if(cdf(dist, t_stat) < alpha)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Sample 1 Mean >  Sample 2 Mean       ";
   if(cdf(complement(dist, t_stat)) < alpha)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << endl << endl;
}

int main()
{
   //
   // Run tests for Car Mileage sample data
   // http://www.itl.nist.gov/div898/handbook/eda/section3/eda3531.htm
   // from the NIST website http://www.itl.nist.gov.  The data compares
   // miles per gallon of US cars with miles per gallon of Japanese cars.
   //
   two_samples_t_test_equal_sd(20.14458, 6.414700, 249, 30.48101, 6.107710, 79, 0.05);
   two_samples_t_test_unequal_sd(20.14458, 6.414700, 249, 30.48101, 6.107710, 79, 0.05);

   return 0;
} // int main()

/*
Output is

------ Build started: Project: students_t_two_samples, Configuration: Debug Win32 ------
Compiling...
students_t_two_samples.cpp
Linking...
Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\students_t_two_samples.exe"
_______________________________________________
Student t test for two samples (equal variances)
_______________________________________________

Number of Observations (Sample 1)                      =  249
Sample 1 Mean                                          =  20.145
Sample 1 Standard Deviation                            =  6.4147
Number of Observations (Sample 2)                      =  79
Sample 2 Mean                                          =  30.481
Sample 2 Standard Deviation                            =  6.1077
Degrees of Freedom                                     =  326
Pooled Standard Deviation                              =  326
T Statistic                                            =  -12.621
Probability that difference is due to chance           =  5.273e-030

Results for Alternative Hypothesis and alpha           =  0.0500

Alternative Hypothesis              Conclusion
Sample 1 Mean != Sample 2 Mean       NOT REJECTED
Sample 1 Mean <  Sample 2 Mean       NOT REJECTED
Sample 1 Mean >  Sample 2 Mean       REJECTED

_________________________________________________
Student t test for two samples (unequal variances)
_________________________________________________

Number of Observations (Sample 1)                      =  249
Sample 1 Mean                                          =  20.14458
Sample 1 Standard Deviation                            =  6.41470
Number of Observations (Sample 2)                      =  79
Sample 2 Mean                                          =  30.48101
Sample 2 Standard Deviation                            =  6.10771
Degrees of Freedom                                     =  136.87499
T Statistic                                            =  -12.94627
Probability that difference is due to chance           =  1.571e-025

Results for Alternative Hypothesis and alpha           =  0.0500

Alternative Hypothesis              Conclusion
Sample 1 Mean != Sample 2 Mean       NOT REJECTED
Sample 1 Mean <  Sample 2 Mean       NOT REJECTED
Sample 1 Mean >  Sample 2 Mean       REJECTED

Build Time 0:03
Build log was saved at "file://i:\boost-06-05-03-1300\libs\math\test\Math_test\students_t_two_samples\Debug\BuildLog.htm"
students_t_two_samples - 0 error(s), 0 warning(s)
========== Build: 1 succeeded, 0 failed, 0 up-to-date, 0 skipped ==========

*/


/**
 * @file nonlinschroedingerequation_main.cc
 * @brief NPDE homework NonLinSchroedingerEquation code
 * @author Oliver Rietmann
 * @date 22.04.2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <complex>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>
#include <utility>

#include "nonlinschroedingerequation.h"
#include "propagator.h"

int main() {
  /* SAM_LISTING_BEGIN_9 */
  // Load mesh and initalize FE space and DOF handler
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  const lf::io::GmshReader reader(
      std::move(mesh_factory), CURRENT_SOURCE_DIR "/../meshes/square_64.msh");
  auto mesh_p = reader.mesh();
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());

  // Mass matrix
  lf::assemble::COOMatrix<double> D_COO(N_dofs, N_dofs);
  NonLinSchroedingerEquation::MassElementMatrixProvider mass_emp;
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, mass_emp, D_COO);
  Eigen::SparseMatrix<double> D = D_COO.makeSparse();
  Eigen::SparseMatrix<std::complex<double>> M = std::complex<double>(0, 1) * D;

  // Stiffness matrix
  lf::assemble::COOMatrix<double> A_COO(N_dofs, N_dofs);
  lf::uscalfe::LinearFELaplaceElementMatrix stiffness_emp;
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, stiffness_emp, A_COO);
  Eigen::SparseMatrix<double> A = A_COO.makeSparse();

  // Prepare timestepping
  int timesteps = 100;
  double T = 1.0;
  double tau = T / timesteps;

// Prepare inital data
  const double PI = 3.14159265358979323846;
  auto u0 = [PI](Eigen::Vector2d x) -> double {
    return 4.0 * std::cos(PI * x(0)) * std::cos(PI * x(1));
  };
  lf::mesh::utils::MeshFunctionGlobal mf_u0{u0};
  Eigen::VectorXcd mu = lf::fe::NodalProjection(*fe_space, mf_u0);

  // Prepare split-step propagator for full step $\tau$
  NonLinSchroedingerEquation::SplitStepPropagator splitStepPropagator(A, M,
                                                                      tau);

  // Arrays for storing "energies" contributing to the Hamiltonian
  Eigen::VectorXd norm(timesteps + 1);
  Eigen::VectorXd E_kin(timesteps + 1);
  Eigen::VectorXd E_int(timesteps + 1);
  // Timestepping
  for (int j = 0; j < timesteps; ++j) {
    // Compute norm and energy along the solution
    norm(j) = NonLinSchroedingerEquation::Norm(mu, D);
    E_kin(j) = NonLinSchroedingerEquation::KineticEnergy(mu, A);
    E_int(j) = NonLinSchroedingerEquation::InteractionEnergy(mu, D);
    // Timestep tau according to Strang splitting
    mu = splitStepPropagator(mu);
  }
  norm(timesteps) = NonLinSchroedingerEquation::Norm(mu, D);
  E_kin(timesteps) = NonLinSchroedingerEquation::KineticEnergy(mu, A);
  E_int(timesteps) = NonLinSchroedingerEquation::InteractionEnergy(mu, D);

  // Timegrid
  Eigen::VectorXd t = Eigen::VectorXd::LinSpaced(timesteps + 1, 0.0, T);

  // Nice output format
  const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");

  // Write norm to file
  std::ofstream norm_csv;
  norm_csv.open("norm.csv");
  norm_csv << t.transpose().format(CSVFormat) << std::endl;
  norm_csv << norm.transpose().format(CSVFormat) << std::endl;
  norm_csv.close();
  /* SAM_LISTING_END_9 */

  // Call python script to plot norm
  std::cout << "Generated " CURRENT_BINARY_DIR "/norm.csv" << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_norm.py " CURRENT_BINARY_DIR
              "/norm.csv " CURRENT_BINARY_DIR "/norm.eps");

  // Write energies to file
  std::ofstream energies_csv;
  energies_csv.open("energies.csv");
  energies_csv << t.transpose().format(CSVFormat) << std::endl;
  energies_csv << E_kin.transpose().format(CSVFormat) << std::endl;
  energies_csv << E_int.transpose().format(CSVFormat) << std::endl;
  energies_csv.close();

  // Call python script to plot energies
  std::cout << "Generated " CURRENT_BINARY_DIR "/energies.csv" << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR
              "/plot_energies.py " CURRENT_BINARY_DIR
              "/energies.csv " CURRENT_BINARY_DIR "/energies.eps");

  // Write entry-wise squared modulus of $\mu$ to .vtk file
  std::cout << "Generated " CURRENT_BINARY_DIR "/solution.vtk" << std::endl;
  lf::io::VtkWriter vtk_writer(mesh_p, "solution.vtk");
  Eigen::VectorXd mu_abs2 = mu.cwiseAbs2();
  lf::fe::MeshFunctionFE mu_abs2_mf(fe_space, mu_abs2);
  vtk_writer.WritePointData("mu_abs2", mu_abs2_mf);

  return 0;
}


//-----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax gaussian_elimination
#include <boost/test/unit_test.hpp>

#include <mitrax/gaussian_elimination.hpp>
#include <mitrax/operator.hpp>

#include <cmath>

using boost::typeindex::type_id;
using boost::typeindex::type_id_runtime;
using namespace mitrax;
using namespace mitrax::literals;

constexpr auto ref1 = make_matrix< float >(3_DS, {
	{1, 2, 3},
	{1, 1, 1},
	{3, 3, 1}
});

constexpr auto ref2 = make_matrix< float >(3_DS, {
	{1, 2, 3},
	{4, 5, 6},
	{7, 8, 9}
});

constexpr auto ref3 = make_matrix< float >(3_DS, {
	{-1,  2, 0},
	{ 1,  0, 1},
	{ 2, -4, 0}
});

constexpr auto ref4 = make_matrix< float >(3_DS, {
	{1, 2, 3},
	{4, 5, 6},
	{0, 0, 0}
});

template < typename M, col_t C, row_t R >
constexpr bool near_null(
	matrix< M, C, R > const& m,
	value_type_t< M > const& threshold
){
	for(auto& v: m) if(threshold < v) return false;
	return true;
}

template < typename T, typename U >
constexpr bool equal(T const& a, U const& b){
	return std::abs(a - b) < 0.00001;
}

BOOST_AUTO_TEST_SUITE(suite_gaussian_elimination)

BOOST_AUTO_TEST_CASE(test_upper_triangular_matrix){
	auto m = upper_triangular_matrix(ref1);

	BOOST_TEST((
		m.cols() == 3_CS &&
		m.rows() == 3_RS &&
		m(0_c, 0_r) ==  1 &&
		m(1_c, 0_r) ==  2 &&
		m(2_c, 0_r) ==  3 &&
		m(0_c, 1_r) ==  0 &&
		m(1_c, 1_r) == -1 &&
		m(2_c, 1_r) == -2 &&
		m(0_c, 2_r) ==  0 &&
		m(1_c, 2_r) ==  0 &&
		m(2_c, 2_r) == -2
	));
}

BOOST_AUTO_TEST_CASE(test_matrix_kernel_3x3_1){
	auto const v = matrix_kernel(ref2);

	BOOST_TEST((
		v.cols() == 1_CS &&
		v.rows() == 3_RS &&
		v[0_d] ==  1 &&
		v[1_d] == -2 &&
		v[2_d] ==  1
	));
}

BOOST_AUTO_TEST_CASE(test_matrix_kernel_3x3_2){
	auto const v = matrix_kernel(ref3);

	BOOST_TEST((
		v.cols() == 1_CS &&
		v.rows() == 3_RS &&
		v[0_d] == -1 &&
		v[1_d] == -0.5 &&
		v[2_d] ==  1
	));
}

BOOST_AUTO_TEST_CASE(test_matrix_kernel_3x3_3){
	auto const v = matrix_kernel(ref4);

	BOOST_TEST((
		v.cols() == 1_CS &&
		v.rows() == 3_RS &&
		v[0_d] ==  1 &&
		v[1_d] == -2 &&
		v[2_d] ==  1
	));
}

BOOST_AUTO_TEST_CASE(test_matrix_kernel_numeric){
	for(size_t i = 0; i < 10; ++i){
		auto m = make_matrix_v< double >(dims(dim_t(i + 1)));
		size_t j = 5;
		for(auto& v: m) v = ++j;

		auto k = matrix_kernel(m);

		BOOST_TEST(near_null(m * k, 0));
	}
}

BOOST_AUTO_TEST_CASE(test_gaussian_elimination){
	constexpr auto m = make_matrix< float >(3_DS, {
		{ 1  , -0.2, -0.2},
		{-0.4,  0.8, -0.1},
		{ 0  , -0.5,  0.9}
	});

	constexpr auto v = make_vector< float >(3_RS, {7, 12.5, 16.5});

	auto res = gaussian_elimination(m, v);

	BOOST_TEST((
		res.cols() == 1_CS &&
		res.rows() == 3_RS &&
		res[0_d] == 20 &&
		res[1_d] == 30 &&
		res[2_d] == 35
	));
}

BOOST_AUTO_TEST_CASE(test_inverse_2x2){
	constexpr auto m = make_matrix< float >(2_DS, {
		{2, 5},
		{1, 3}
	});

	auto i = inverse(m);

	BOOST_TEST((
		i.cols() == 2_CS &&
		i.rows() == 2_RS &&
		i(0_c, 0_r) ==  3 &&
		i(1_c, 0_r) == -5 &&
		i(0_c, 1_r) == -1 &&
		i(1_c, 1_r) ==  2
	));
}

BOOST_AUTO_TEST_CASE(test_inverse_3x3_1){
	constexpr auto m = make_matrix< float >(3_DS, {
		{1, 2, 0},
		{2, 4, 1},
		{2, 1, 0}
	});

	auto i = inverse(m) * 3;

	BOOST_TEST((
		i.cols() == 3_CS &&
		i.rows() == 3_RS &&
		equal(i(0_c, 0_r), -1) &&
		equal(i(1_c, 0_r),  0) &&
		equal(i(2_c, 0_r),  2) &&
		equal(i(0_c, 1_r),  2) &&
		equal(i(1_c, 1_r),  0) &&
		equal(i(2_c, 1_r), -1) &&
		equal(i(0_c, 2_r), -6) &&
		equal(i(1_c, 2_r),  3) &&
		equal(i(2_c, 2_r),  0)
	));
}

BOOST_AUTO_TEST_CASE(test_inverse_3x3_2){
	constexpr auto m = make_matrix< float >(3_DS, {
		{ 2, -1,  0},
		{-1,  2, -1},
		{ 0, -1,  2}
	});

	auto i = inverse(m) * 4;

	BOOST_TEST((
		i.cols() == 3_CS &&
		i.rows() == 3_RS &&
		equal(i(0_c, 0_r), 3) &&
		equal(i(1_c, 0_r), 2) &&
		equal(i(2_c, 0_r), 1) &&
		equal(i(0_c, 1_r), 2) &&
		equal(i(1_c, 1_r), 4) &&
		equal(i(2_c, 1_r), 2) &&
		equal(i(0_c, 2_r), 1) &&
		equal(i(1_c, 2_r), 2) &&
		equal(i(2_c, 2_r), 3)
	));
}

BOOST_AUTO_TEST_CASE(test_inverse_3x3_3){
	constexpr auto m = make_matrix< float >(3_DS, {
		{ 2, -1,  0},
		{ 1,  2, -2},
		{ 0, -1,  1}
	});

	auto i = inverse(m);

	BOOST_TEST((
		i.cols() == 3_CS &&
		i.rows() == 3_RS &&
		equal(i(0_c, 0_r),  0) &&
		equal(i(1_c, 0_r),  1) &&
		equal(i(2_c, 0_r),  2) &&
		equal(i(0_c, 1_r), -1) &&
		equal(i(1_c, 1_r),  2) &&
		equal(i(2_c, 1_r),  4) &&
		equal(i(0_c, 2_r), -1) &&
		equal(i(1_c, 2_r),  2) &&
		equal(i(2_c, 2_r),  5)
	));
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Dense>

#include "root_finding.hpp"
#include "types.hpp"

namespace convexnmf {

namespace norms {

inline Scalar L1_Infinity_MixedNorm(const MatrixRef &A) {
    return A.cwiseAbs().rowwise().maxCoeff().sum();
}

inline Scalar L1Norm(const MatrixRef &A) { return A.cwiseAbs().sum(); }

inline Scalar LInfinity_1_MixedNorm(const MatrixRef &A) {
    return A.cwiseAbs().rowwise().sum().maxCoeff();
}

} // namespace norms

// Returns a vector which is the projection of v onto the simplex
// with sum sum_target
Vector SimplexProjection(const VectorRef &v, Scalar sum_target);

Vector L1BallProjection(const VectorRef &v, Scalar radius);
Vector L2BallProjection(const VectorRef &v, Scalar radius);

// Computes the proximal operator
// u = argmin_B 0.5 ||A - B||_F^2 + \lambda ||B||_{1, \infty}
// This separates into computing the l_\infty proximal operator
// for each row, which, in turn, is equivalent to projection onto
// the l1 ball of radius \theta.
// (TODO) Add reference
Matrix L1InfinityProximalOperator(const MatrixRef &A, Scalar lambda);

// Computes the L1infinity projection, or
// u = argmin_B ||A - B||_F^2 subject to ||B||_{1,\infty} <= radius
// using the previous initial guess can be used.
Matrix L1InfinityProjection(const MatrixRef &A, Scalar radius, Scalar root_tolerance = 1.0e-10);

} // namespace convexnmf


#ifndef _COCONUT_PULP_MATH_QUATERNION_HPP_
#define _COCONUT_PULP_MATH_QUATERNION_HPP_

#include <boost/operators.hpp>

#include "ScalarEqual.hpp"
#include "Vector.hpp"

namespace coconut {
namespace pulp {
namespace math {

template <class ScalarType, class ScalarEqualityFunc = ScalarEqual<ScalarType>>
class Quaternion :
	boost::equality_comparable<Quaternion<ScalarType, ScalarEqualityFunc>,
	boost::additive<Quaternion<ScalarType, ScalarEqualityFunc>,
	boost::multipliable<Quaternion<ScalarType, ScalarEqualityFunc>,
	boost::multiplicative<Quaternion<ScalarType, ScalarEqualityFunc>, ScalarType
	>>>>
{
public:

	using Scalar = ScalarType;

	using ScalarPart = Scalar;

	using VectorPart = Vector<Scalar, 3, ScalarEqualityFunc>;

	// --- CONSTRUCTORS AND OPERATORS

	constexpr Quaternion(ScalarPart s, VectorPart v) :
		elements_(v.x(), v.y(), v.z(), s)
	{
	}

	friend std::ostream& operator<<(std::ostream& os, const Quaternion& q) {
		return os << q.s() << " + " << q.v();
	}

	friend bool operator==(const Quaternion& lhs, const Quaternion& rhs) noexcept {
		return lhs.elements_ == rhs.elements_;
	}

	Quaternion& operator*=(const Quaternion& other) noexcept {
		const auto s1 = s();
		const auto s2 = other.s();
		const auto v1 = v();
		const auto v2 = other.v();

		const auto s = s1 * s2 - ::dot(v1, v2);
		const auto v = s1 * v2 + s2 * v1 + cross(v1, v2);

		*this = Quaternion(s, v);

		return *this;
	}

	Quaternion& operator+=(const Quaternion& other) noexcept {
		elements_ += other.elements_;
		return *this;
	}

	Quaternion& operator-=(const Quaternion& other) noexcept {
		elements_ -= other.elements_;
		return *this;
	}

	Quaternion& operator*=(const Scalar& s) noexcept {
		elements_ *= s;
		return *this;
	}

	Quaternion& operator/=(const Scalar& s) noexcept {
		elements_ /= s;
		return *this;
	}

	// --- QUATERNION-SPECIFIC OPERATIONS

	constexpr Quaternion conjugate() const noexcept {
		return Quaternion(s(), -v());
	}

	Quaternion& normalise() noexcept {
		const auto n = norm();
		if (n > Scalar(0)) {
			*this /= n;
		}
		return *this;
	}

	Quaternion normalised() const noexcept {
		auto result = *this;
		return result.normalise();
	}

	Scalar norm() const noexcept {
		return elements_.length();
	}

	Scalar normSq() const noexcept {
		return elements_.lengthSq();
	}

	Quaternion inverse() const noexcept {
		const auto n = normSq();
		assert(!ScalarEqualityFunc()(n, Scalar(0)));
		return conjugate() / n;
	}

	Scalar dot(const Quaternion& other) const noexcept {
		return elements_.dot(other.elements_);
	}

	// --- ACCESSORS

	constexpr const ScalarPart s() const noexcept {
		return elements_.w();
	}

	ScalarPart& s() noexcept {
		return elements_.w();
	}

	const VectorPart v() const noexcept { // TODO: return view
		return elements_.xyz();
	}

	const Scalar x() const noexcept {
		return elements_.x();
	}

	Scalar& x() noexcept {
		return elements_.x();
	}

	const Scalar y() const noexcept {
		return elements_.y();
	}

	Scalar& y() noexcept {
		return elements_.y();
	}

	const Scalar z() const noexcept {
		return elements_.z();
	}

	Scalar& z() noexcept {
		return elements_.z();
	}

	const Scalar w() const noexcept {
		return elements_.w();
	}

	Scalar& w() noexcept {
		return elements_.w();
	}

private:

	using Elements = Vector<Scalar, 4, ScalarEqualityFunc>;

	Elements elements_;

};

template <class S, class SEF>
S dot(const Quaternion<S, SEF>& lhs, const Quaternion<S, SEF>& rhs) noexcept {
	return lhs.dot(rhs);
}

using Quat = Quaternion<float>;

} // namespace math

using math::Quaternion;
using math::Quat;

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_MATH_QUATERNION_HPP_ */


#include <iostream>
#include <filesystem>
#include <Eigen/Eigenvalues>
#include <fstream>
#include "SpectraAnalysisLinearElements.h"
#include "../PhysicalModel/ExternalForces.h"

SpectraAnalysisLinearElements::SpectraAnalysisLinearElements(const SimParameters& params, Eigen::VectorXd& q0, Eigen::VectorXd& v0, const LinearElements& model)
{
	params_ = params;
	q0_ = q0;
	v0_ = v0;
	numSpectras_ = params.numSpectra;

	if (numSpectras_ > q0.size())
		numSpectras_ = q0.size();

	model_ = model;

	initialization();
}

void SpectraAnalysisLinearElements::initialization()
{
	// form mass matrix and its inverse
	std::vector<Eigen::Triplet<double>> T, T1;
	for (int i = 0; i < model_.massVec_.size(); i++)
	{
		T.push_back({ i, i, model_.massVec_(i) });
		T1.push_back({ i, i, 1.0 / model_.massVec_(i) });
	}

	massMat_.resize(q0_.size(), q0_.size());
	massInvMat_.resize(q0_.size(), q0_.size());

	massMat_.setFromTriplets(T.begin(), T.end());
	massInvMat_.setFromTriplets(T1.begin(), T1.end());

	// compute the eigen modes
	model_.computeHessian(q0_, K_);

	Eigen::VectorXd g;
	model_.computeGradient(q0_, g);
	b_ = g - K_ * q0_;

	// check the energy
	Eigen::VectorXd testQ = q0_;
	testQ.setZero();
	double E0 = model_.computeEnergy(testQ);
	double E = model_.computeEnergy(q0_);
	double E1 = 0.5 * q0_.dot(K_ * q0_) + q0_.dot(b_) + E0;

	std::cout << "E = " << E << ", E1 = " << E1 << ", error = " << E1 - E << std::endl;

	testQ.setRandom();
	E = model_.computeEnergy(testQ);
	E1 = 0.5 * testQ.dot(K_ * testQ) + testQ.dot(b_) + E0;
	std::cout << "E = " << E << ", E1 = " << E1 << ", error = " << E1 - E << std::endl;

	if (numSpectras_ < q0_.size())
	{
		Spectra::SparseSymMatProd<double> opK(K_);
		Spectra::SparseCholesky<double> opM(massMat_);

		Spectra::SymGEigsSolver<Spectra::SparseSymMatProd<double>, Spectra::SparseCholesky<double>, Spectra::GEigsMode::Cholesky> eigs(opK, opM, numSpectras_, (2 * numSpectras_ > q0_.size()) ? q0_.size() : 2 * numSpectras_);

		eigs.init();
		int nconv = eigs.compute(Spectra::SortRule::LargestMagn);
		if (eigs.info() == Spectra::CompInfo::Successful)
		{
			std::cout << eigs.eigenvalues() << std::endl;
			eigenValues_ = eigs.eigenvalues();
			eigenVecs_ = eigs.eigenvectors();
		}
		else
		{
			std::cerr << "error in t computing the eigen values of the M^{-1} K " << std::endl;
			exit(1);
		}
	}

	else
	{
		int halfNum = numSpectras_ / 2;
		Spectra::SparseSymMatProd<double> opK(K_);
		Spectra::SparseCholesky<double> opM(massMat_);

		Spectra::SymGEigsSolver<Spectra::SparseSymMatProd<double>, Spectra::SparseCholesky<double>, Spectra::GEigsMode::Cholesky> eigs(opK, opM, halfNum, numSpectras_);

		eigs.init();
		eigs.compute(Spectra::SortRule::LargestMagn);

		int leftNum = numSpectras_ - halfNum;

		using OpType = Spectra::SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
		using BOpType = Spectra::SparseSymMatProd<double>;
		OpType op(K_, massMat_);
		BOpType Bop(massMat_);

		Spectra::SymGEigsShiftSolver<OpType, BOpType, Spectra::GEigsMode::ShiftInvert> eigs1(op, Bop, leftNum, numSpectras_, 0.0);
		eigs1.init();
		eigs1.compute(Spectra::SortRule::LargestMagn);

		if (eigs.info() == Spectra::CompInfo::Successful && eigs1.info() == Spectra::CompInfo::Successful)
		{
			std::cout << eigs.eigenvalues() << std::endl;
			std::cout << eigs1.eigenvalues() << std::endl;

			eigenValues_.resize(numSpectras_);
			eigenValues_.segment(0, halfNum) = eigs.eigenvalues();
			eigenValues_.segment(halfNum, leftNum) = eigs1.eigenvalues();

			eigenVecs_.resize(q0_.size(), numSpectras_);
			eigenVecs_.block(0, 0, q0_.size(), halfNum) = eigs.eigenvectors();
			eigenVecs_.block(0, halfNum, q0_.size(), leftNum) = eigs1.eigenvectors();
		}
		else
		{
			// super slow when size of q is large
			Eigen::GeneralizedEigenSolver<Eigen::MatrixXd> ges;
			ges.compute(K_.toDense(), massMat_.toDense());
			eigenValues_ = ges.eigenvalues().real();
			eigenVecs_ = ges.eigenvectors().real();
		}

	}

	// check the othonormality
	Eigen::MatrixXd idMat = Eigen::MatrixXd::Identity(numSpectras_, numSpectras_);
	std::cout << "error: " << (eigenVecs_.transpose() * massMat_ * eigenVecs_ - idMat).norm() << std::endl;

	// compute initial alphas and betas
	curAlphaBeta_.resize(numSpectras_);
	preAlphaBeta_.resize(numSpectras_);
	initialAlphaBeta_.resize(numSpectras_);
	curAlphaBetaTheo_.resize(numSpectras_);

	for (int i = 0; i < numSpectras_; i++)
	{
		double alpha = eigenVecs_.col(i).dot(massMat_ * q0_);
		double beta = eigenVecs_.col(i).dot(massMat_ * v0_);
		initialAlphaBeta_[i] << alpha, beta;
		preAlphaBeta_[i] = initialAlphaBeta_[i];
		curAlphaBeta_[i] = initialAlphaBeta_[i];
		curAlphaBetaTheo_[i] = initialAlphaBeta_[i];
	}

	// compute the constant part
	/*cis_.resize(numSpectras_);

	for (int i = 0; i < numSpectras_; i++)
	{
		double value = eigenVecs_.col(i).dot(b_);
		cis_[i] = value;
	}*/

	// current time
	curTime_ = 0;

	updateCis();
}

void SpectraAnalysisLinearElements::updateCis()
{
	Eigen::VectorXd extForce = ExternalForces::externalForce(q0_, curTime_, params_.impulseMag, params_.impulsePow);
	Eigen::VectorXd c = b_ - extForce;

	// compute the constant part
	cis_.resize(numSpectras_);

	for (int i = 0; i < numSpectras_; i++)
	{
		double value = eigenVecs_.col(i).dot(c);
		cis_[i] = value;
	}
}

void SpectraAnalysisLinearElements::updateAlphasBetas()
{
	double h = params_.timeStep;
	updateCis();
	for (int i = 0; i < numSpectras_; i++)
	{
		if (params_.integrator == SimParameters::TI_IMPLICIT_EULER)
		{
			Eigen::Matrix2d A;
			A << 1, -h, eigenValues_[i] * h, 1;
			Eigen::Vector2d consVec;
			consVec << 0, -h * cis_[i];

			Eigen::Matrix2d Ainv = A.inverse();

			preAlphaBeta_[i] = curAlphaBeta_[i];
			curAlphaBeta_[i] = Ainv * (preAlphaBeta_[i] + consVec);
		}
		else if (params_.integrator == SimParameters::TI_NEWMARK)
		{
			Eigen::Matrix2d A;
			double NM_beta = params_.NM_beta;
			A << 1 + h * h * eigenValues_[i] * NM_beta, 0,
				eigenValues_[i] * h / 2.0, 1;
			Eigen::Vector2d consVec;

			Eigen::Matrix2d A1;
			A1 << 1 - h * h * eigenValues_[i] * (0.5 - NM_beta), h,
				-eigenValues_[i] * h / 2.0, 1;

			consVec << -h * h * cis_[i] / 2, -params_.timeStep * cis_[i];

			Eigen::Matrix2d Ainv = A.inverse();

			preAlphaBeta_[i] = curAlphaBeta_[i];
			curAlphaBeta_[i] = Ainv * (A1 * preAlphaBeta_[i] + consVec);
		}
		else if (params_.integrator == SimParameters::TI_TR_BDF2)
		{
			Eigen::Vector2d alphabeta;
			double gamma = params_.TRBDF2_gamma;
			double gamma2 = (1 - 2 * gamma) / (2 - 2 * gamma);
			double gamma3 = (1 - gamma2) / (2 * gamma);

			Eigen::Matrix2d A, A1, A2, Ainv;
			Eigen::Vector2d consVec;

			A << 1, -gamma * h, eigenValues_[i] * gamma* h, 1;
			Ainv = A.inverse();

			A1 << 1, gamma * h, -eigenValues_[i] * gamma* h, 1;

			consVec << 0, -2 * gamma * h * cis_[i];
			alphabeta = Ainv * (A1 * curAlphaBeta_[i] + consVec);

			A << 1, -gamma2 * h, eigenValues_[i] * gamma2 * h, 1;
			Ainv = A.inverse();
			consVec << 0, -gamma2 * h * cis_[i];

			A1 << gamma3, 0, 0, gamma3;
			A2 << 1 - gamma3, 0, 0, 1 - gamma3;

			preAlphaBeta_[i] = curAlphaBeta_[i];
			curAlphaBeta_[i] = Ainv * (A1 * alphabeta + A2 * preAlphaBeta_[i] + consVec);
		}
		else if (params_.integrator == SimParameters::TI_BDF2)
		{
			if (curTime_ == 0)	// use IE for the first step
			{
				Eigen::Matrix2d A;
				A << 1, -h, eigenValues_[i] * h, 1;
				Eigen::Vector2d consVec;
				consVec << 0, -h * cis_[i];

				Eigen::Matrix2d Ainv = A.inverse();

				preAlphaBeta_[i] = curAlphaBeta_[i];
				curAlphaBeta_[i] = Ainv * (preAlphaBeta_[i] + consVec);
			}
			else
			{
				Eigen::Vector2d alphabeta;

				Eigen::Matrix2d A, A1, A2, Ainv;
				Eigen::Vector2d consVec;

				A << 1, -2.0 / 3.0 * h, eigenValues_[i] * 2.0 / 3.0 * h, 1;
				Ainv = A.inverse();
				consVec << 0, -2.0 / 3.0 * h * cis_[i];

				A1 << 4.0 / 3.0, 0, 0, 4.0 / 3.0;
				A2 << -1.0 / 3.0, 0, 0, -1.0 / 3.0;

				alphabeta = Ainv * (A1 * curAlphaBeta_[i] + A2 * preAlphaBeta_[i] + consVec);

				preAlphaBeta_[i] = curAlphaBeta_[i];
				curAlphaBeta_[i] = alphabeta;
			}
		}
	}
	curTime_ += h;

	// compute the theoretical alpha beta
	for (int i = 0; i < numSpectras_; i++)
	{
		curAlphaBetaTheo_[i](0) = (initialAlphaBeta_[i](0) + cis_[i] / eigenValues_[i]) * std::cos(std::sqrt(eigenValues_[i]) * curTime_) - cis_[i] / eigenValues_[i];
		curAlphaBetaTheo_[i](1) = (initialAlphaBeta_[i](0) + cis_[i] / eigenValues_[i]) * std::sin(std::sqrt(eigenValues_[i]) * curTime_) * std::sqrt(eigenValues_[i]);
	}
}

void SpectraAnalysisLinearElements::getCurPosVel(Eigen::VectorXd& pos, Eigen::VectorXd& vel)
{
	if (curAlphaBeta_.size() != numSpectras_)
	{
		std::cerr << "mismatch in alpha beta vector size and number of spectras." << std::endl;
		exit(1);
	}

	pos.setZero(eigenVecs_.rows());
	vel.setZero(eigenVecs_.rows());

	for (int i = 0; i < numSpectras_; i++)
	{
		pos += curAlphaBeta_[i](0) * eigenVecs_.col(i);
		vel += curAlphaBeta_[i](1) * eigenVecs_.col(i);
	}
}

void SpectraAnalysisLinearElements::getTheoPosVel(Eigen::VectorXd& pos, Eigen::VectorXd& vel)
{
	if (curAlphaBetaTheo_.size() != numSpectras_)
	{
		std::cerr << "mismatch in alpha beta vector size and number of spectras." << std::endl;
		exit(1);
	}

	pos.setZero(eigenVecs_.rows());
	vel.setZero(eigenVecs_.rows());

	for (int i = 0; i < numSpectras_; i++)
	{
		pos += curAlphaBetaTheo_[i](0) * eigenVecs_.col(i);
		vel += curAlphaBetaTheo_[i](1) * eigenVecs_.col(i);
	}
}

void SpectraAnalysisLinearElements::saveInfo(std::string outputFolder)
{
	if (!std::filesystem::exists(outputFolder))
	{
		std::cout << "create directory: " << outputFolder << std::endl;
		if (!std::filesystem::create_directories(outputFolder))
		{
			std::cout << "create folder failed." << outputFolder << std::endl;
			exit(1);
		}
	}

	std::string evalsFileName = outputFolder + "evals.txt";
    if(curTime_ == 0)
    {
        std::ofstream  efs;
        efs.open(evalsFileName, std::ofstream::out);
        for (int i = 0; i < numSpectras_; i++)
        {
            efs << eigenValues_[i] << std::endl;
        }
    }

	for (int i = 0; i < numSpectras_; i++)
	{
		std::string alphafileName = outputFolder + "alpha_" + std::to_string(i) + ".txt";
		std::string theoAlphafileName = outputFolder + "alpha_theo_" + std::to_string(i) + ".txt";

		std::string betafileName = outputFolder + "beta_" + std::to_string(i) + ".txt";
		std::string theoBetafileName = outputFolder + "beta_theo_" + std::to_string(i) + ".txt";

		std::string extFfileName = outputFolder + "c_" + std::to_string(i) + ".txt";

		std::ofstream afs, atfs, bfs, btfs, cfs;

		if (curTime_ == 0)
		{
			afs.open(alphafileName, std::ofstream::out);
			atfs.open(theoAlphafileName, std::ofstream::out);

			bfs.open(betafileName, std::ofstream::out);
			btfs.open(theoBetafileName, std::ofstream::out);

			cfs.open(extFfileName, std::ofstream::out);
		}
		else
		{
			afs.open(alphafileName, std::ofstream::out | std::ofstream::app);
			atfs.open(theoAlphafileName, std::ofstream::out | std::ofstream::app);

			bfs.open(betafileName, std::ofstream::out | std::ofstream::app);
			btfs.open(theoBetafileName, std::ofstream::out | std::ofstream::app);

			cfs.open(extFfileName, std::ofstream::out | std::ofstream::app);
		}

		afs << curAlphaBeta_[i](0) << std::endl;
		atfs << curAlphaBetaTheo_[i](0) << std::endl;

		bfs << curAlphaBeta_[i](1) << std::endl;
		btfs << curAlphaBetaTheo_[i](1) << std::endl;

		cfs << cis_[i] << std::endl;
	}
}


#include <Eigen/Dense>
#include <iostream>
#include <mplot++/mplot++.h>
#include <tuple>

namespace ei = Eigen;
namespace mp = mplotpp;

template<class T>
std::tuple<Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>,
           Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>>
meshgrid(const Eigen::Array<T, Eigen::Dynamic, 1>& x,
         const Eigen::Array<T, Eigen::Dynamic, 1>& y)
{
  Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> X(y.size(), x.size());
  Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> Y(y.size(), x.size());
  for (ssize_t i = 0; i < X.rows(); ++i) {
    X.row(i) = x;
  }
  for (ssize_t j = 0; j < Y.cols(); ++j) {
    Y.col(j) = y;
  }

  return std::make_tuple(X, Y);
}

int
main()
{
  ei::ArrayXd x = mp::arange(-1.0, 1.01, 1.0);
  ei::ArrayXd y = mp::arange(-2.0, 2.01, 1.0);
  std::cout << "x = " << x.transpose() << std::endl;
  std::cout << "y = " << y.transpose() << std::endl;

  {
    ei::ArrayXXd X(y.size(), x.size());
    for (ssize_t i = 0; i < X.rows(); ++i) {
      X.row(i) = x;
    }
    std::cout << "X =\n" << X << std::endl;

    ei::ArrayXXd Y(y.size(), x.size());
    for (ssize_t j = 0; j < Y.cols(); ++j) {
      Y.col(j) = y;
    }
    std::cout << "Y =\n" << Y << std::endl;
  }

  {
    auto [X, Y] = meshgrid(x, y);
    std::cout << "X =\n" << X << std::endl;
    std::cout << "Y =\n" << Y << std::endl;
    std::cout << "X + 1 =\n" << (X + 1) << std::endl;
    auto Z1 = (-X.pow(2) - Y.pow(2)).exp();
    std::cout << "Z1 =\n" << Z1 << std::endl;
    auto Z2 = (-(X - 1).pow(2) - (Y - 1).pow(2)).exp();
    std::cout << "Z2 =\n" << Z2 << std::endl;
    auto Z = (Z1 - Z2) * 2;
    std::cout << "Z =\n" << Z << std::endl;
  }
}


#include <tdp/testing/testing.h>
#include <iostream>
#include <Eigen/Dense>
#include <tdp/manifold/SO3.h>
#include <tdp/manifold/SO3mat.h>
#include <tdp/eigen/dense.h>

using namespace tdp;

TEST(SO3, log) {
  const float eps = 1e-4;

  for (size_t i=0; i<1000; ++i) {
    Eigen::Vector2f rand = Eigen::Vector2f::Random();
    SO3f Rx0 = SO3f::Rx(ToRad(rand(0)));
    SO3matf Rx0_ = SO3matf::Rx(ToRad(rand(0)));

    ASSERT_TRUE(IsAppox(Rx0.matrix(), Rx0_.matrix(), eps));
    ASSERT_NEAR(fabs(rand(0)), ToDeg(SO3f::Log_(Rx0).norm()), eps);
    ASSERT_TRUE(rand(0)<0.? SO3f::Log_(Rx0)(0) < 0. : SO3f::Log_(Rx0)(0) >= 0. );

  }

  for (size_t i=0; i<1000; ++i) {
    SO3f R0 = SO3f::Random();
    Eigen::Vector3f x0 = SO3f::Log_(R0);
    Eigen::Vector3f x1 = SO3mat<float>::Log_(R0.matrix());

    if (!IsAppox(x0,x1,eps)) {
      std::cout << R0.matrix() << std::endl;
      tdp::Vector3fda axis;
      float angle;
      R0.ToAxisAngle(axis, angle);
      std::cout << axis.transpose() << " angle " << angle
        << " " << ToDeg(angle) << std::endl;
      std::cout << ToDeg(x0.norm()) << " " << ToDeg(x1.norm()) << std::endl;
      std::cout << R0 << std::endl;
    }
    EXPECT_TRUE(IsAppox(x0,x1,eps));
//    ASSERT_NEAR(x0.norm(),x1.norm(),eps);
  }
}

TEST(SO3, exp) {

  const float eps = 1e-5;
  for (size_t i=0; i<1000; ++i) {
    Eigen::Vector3f x = 1e-3*Eigen::Vector3f::Random();
    Eigen::Matrix3f R0 = SO3f::Exp_(x).matrix();
    Eigen::Matrix3f R1 = SO3mat<float>::Exp_(x).matrix();

    ASSERT_TRUE(IsAppox(R0,R1,eps));

    Eigen::Vector3f x0 = SO3f::Log_(R0);
    Eigen::Vector3f x1 = SO3mat<float>::Log_(R1);

    if (!IsAppox(x0,x1,eps)) {
      std::cout << R0 << std::endl << R1 << std::endl;
      std::cout << "so3:  " << x.transpose() << std::endl;
      std::cout << "Quat: " << SO3f::Exp_(x) << std::endl;
    }

    ASSERT_TRUE(IsAppox(x0,x1,eps));
    ASSERT_TRUE(IsAppox(x,x1,eps));
    ASSERT_TRUE(IsAppox(x0,x,eps));

  }
}

TEST(SO3, Rz) {
  const float eps = 1e-4;
  Eigen::Matrix3f R0t = Eigen::Matrix3f::Identity();
  SO3f R0 = SO3f::Rz(0.);
  ASSERT_TRUE(IsAppox(R0.matrix(), R0t, eps));

  SO3f R0_1;
  for (size_t i=0; i<36; ++i)
    R0_1 = SO3f::Rz(ToRad(10.)) * R0_1;
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));

  R0_1 = SO3f();
  for (size_t i=0; i<360; ++i)
    R0_1 = SO3f::Rz(ToRad(1.)) * R0_1;
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));

  R0_1 = SO3f();
  for (size_t i=0; i<3600; ++i)
    R0_1 = SO3f::Rz(ToRad(0.1)) * R0_1;
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));
}

TEST(SO3, Ry) {
  const float eps = 1e-4;
  Eigen::Matrix3f R0t = Eigen::Matrix3f::Identity();
  SO3f R0 = SO3f::Ry(0.);
  ASSERT_TRUE(IsAppox(R0.matrix(), R0t, eps));

  SO3f R0_1;
  for (size_t i=0; i<36; ++i)
    R0_1 *= SO3f::Ry(ToRad(10.));
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));

  R0_1 = SO3f();
  for (size_t i=0; i<360; ++i)
    R0_1 *= SO3f::Ry(ToRad(1.));
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));

  R0_1 = SO3f();
  for (size_t i=0; i<3600; ++i)
    R0_1 *= SO3f::Ry(ToRad(0.1));
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));
}

TEST(SO3, Rx) {
  const float eps = 1e-4;
  Eigen::Matrix3f R0t = Eigen::Matrix3f::Identity();
  SO3f R0 = SO3f::Rx(0.);
  ASSERT_TRUE(IsAppox(R0.matrix(), R0t, eps));

  ASSERT_TRUE(IsAppox(SO3f::Rx(ToRad(10.)).matrix(),
        SO3mat<float>::Rx(ToRad(10.)).matrix(),eps));
  SO3f R0_1;
  for (size_t i=0; i<36; ++i)
    R0_1 *= SO3f::Rx(ToRad(10));
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));

  R0_1 = SO3f();
  for (size_t i=0; i<360; ++i)
    R0_1 *= SO3f::Rx(ToRad(1.));
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));

  R0_1 = SO3f();
  for (size_t i=0; i<3600; ++i)
    R0_1 *= SO3f::Rx(ToRad(0.1));
  ASSERT_TRUE(IsAppox(R0_1.matrix(), R0t, eps));
}

TEST(SO3, composition) {

  const float eps = 1e-5;
  for (size_t i=0; i<1000; ++i) {
    SO3f Rw0 = SO3f::Random();
    SO3f Rw1 = SO3f::Random();
    Eigen::Matrix3f Rw0mat = Rw0.matrix();
    Eigen::Matrix3f Rw1mat = Rw1.matrix();

    SO3f R01 = Rw0.Inverse() * Rw1;
    Eigen::Matrix3f R01mat = Rw0mat.transpose()*Rw1mat;
    ASSERT_TRUE(R01mat.isApprox(R01.matrix(),eps));

    SO3f Rw0w1 = Rw0 * Rw1;
    Eigen::Matrix3f Rw0w1mat = Rw0mat*Rw1mat;
    ASSERT_TRUE(Rw0w1mat.isApprox(Rw0w1.matrix(),eps));
  }

  SO3f Rw0;
  Eigen::Matrix3f Rw0mat = Eigen::Matrix3f::Identity();
  for (size_t i=0; i<1000; ++i) {
    Eigen::Vector3f x0 = 1e-3*Eigen::Vector3f::Random();
    Rw0 = Rw0 * SO3f::Exp_(x0);
    Rw0mat = Rw0mat * SO3mat<float>::Exp_(x0).matrix();
    ASSERT_TRUE(Rw0mat.isApprox(Rw0.matrix(),eps));
  }

}

TEST(SO3, opt) {

  SO3d R;
  std::cout << R << std::endl;

  double theta = 15.*M_PI/180.;
  Eigen::Matrix3d Rmu_;
  Rmu_ << 1, 0, 0,
         0, cos(theta), sin(theta),
         0, -sin(theta), cos(theta);
  SO3d Rmu(Rmu_);

  std::cout << Rmu << std::endl;
//  std::cout << R+Rmu << std::endl;
  std::cout << R << std::endl;
//  std::cout << Rmu+R << std::endl;
//  std::cout << R-Rmu << std::endl;

//  Eigen::Vector3d w = R-Rmu;
//  std::cout << Rmu.Exp(w) << std::endl;

//  std::cout << Rmu-R << std::endl;

  double delta = 0.1;
  double f_prev = 1e99;
  double f = (Rmu.Inverse() * R).matrix().trace();
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100; ++it) {
    Eigen::Matrix3d J = -0.5*((R.Inverse() * Rmu).matrix() - (Rmu.Inverse() * R).matrix());
    Eigen::Vector3d Jw = SO3d::vee(J);
//    R = R.Exp(-delta*Jw);
    R *= SO3d::Exp_(-delta*Jw);
//    std::cout << Jw << std::endl;
    f_prev = f;
    f = (Rmu.Inverse() * R).matrix().trace();
    std::cout << "f=" << f << " df/f=" << (f_prev - f)/f
      << std::endl;
  }
  std::cout << Rmu << std::endl;
  std::cout << R << std::endl;
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


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

TEST(MathFunctions, binary_log_loss) {
  EXPECT_FLOAT_EQ(0.0, stan::math::binary_log_loss(0,0.0));
  EXPECT_FLOAT_EQ(0.0, stan::math::binary_log_loss(1,1.0));
  EXPECT_FLOAT_EQ(-log(0.5), stan::math::binary_log_loss(0,0.5));
  EXPECT_FLOAT_EQ(-log(0.5), stan::math::binary_log_loss(1,0.5));
  EXPECT_FLOAT_EQ(-log(0.75), stan::math::binary_log_loss(0,0.25));
  EXPECT_FLOAT_EQ(-log(0.75), stan::math::binary_log_loss(1,0.75));
}

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

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::binary_log_loss(0, nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::binary_log_loss(1, nan));
}


// c
#include <cmath>

// std
#include <iostream>

// 3rd party
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <gtest/gtest.h>

#include <opencv2/opencv.hpp>

// local
#include "OccupancyGrid/occgrid.hpp"
#include "OccupancyGrid/forward_sensor_model.h"

namespace bconst = boost::math::constants;
namespace brand = boost::random;

#undef DEBUGDRAW

double log_gaussian1d(double x, double mu, double sigma) {
    return (-0.5 * (x - mu)*(x - mu) / (sigma*sigma)) + log(sigma * bconst::root_two_pi<double>());
}

double gaussian1d(double x, double mu, double sigma) {
    // ignore the scaling factor
    return exp(-0.5 * (x - mu)*(x - mu) / (sigma*sigma));//  / (sigma * bconst::root_two_pi<double>());
}

//double probability_single_observation_given_map_and_pose(
double log_odds_observation_given_map_and_pose(
    const Observation2D& observation,
    OccupancyGrid2D<double, int>& map,
    double& expected_range)
{
    double total_angle = observation.ptheta;
    cv::Vec2d direction(cos(total_angle), sin(total_angle));
    cv::Vec2d position(observation.px, observation.py);
    // if (std::isnan(direction(0))) {
    //     printf("Robot Angle : %f, angle of obs: %f\n", robot_angle, angle_of_observation);
    //     throw std::logic_error("direction(0) is nan");
    // }
    //
    assert(! std::isnan(direction(1)));
    assert(! std::isnan(direction(0)));
    cv::Vec2d final_pos;
    bool reflectance;
    expected_range = map.ray_trace(observation.px, observation.py,
        observation.ptheta, LASER_MAX_RANGE, final_pos, reflectance);
    //if (expected_range == LASER_MAX_RANGE) {
        // laser didn't strike any wall
        // but that doesn't matter, because our input observations also
        // contain saturated readings instead of providing some special value.
    //}
    double sigma = NOISE_VARIANCE * expected_range; // noise increases with distance
#ifdef DEBUG
      printf("Sigma:%f\n", sigma);
      printf("Expected range:%f\n", expected_range);
      printf("Observed range:%f\n", observation.range);
#endif
    double gaussian_lodds = log_gaussian1d(observation.range, expected_range, sigma);
    return gaussian_lodds;
}

class ForwardSensorModelTest : public ::testing::Test {
  protected:
    OccupancyGrid2D<double, int> map;
    boost::mt19937 gen;
    double range_observation;
    boost::normal_distribution<> norm_dist;;
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > norm_rand;;

    ForwardSensorModelTest():
      map(-2.5, -1.5, 0.5, 0.5, 12, 6),
      gen(),
      range_observation(sqrt(2.5*2.5 + 0.5*0.5)),
      norm_dist(0, NOISE_VARIANCE * range_observation),
      norm_rand(gen, norm_dist)
    { }

    virtual void SetUp() {
        map.og_ = cv::Scalar(map.FREE);
        map.og_.at<uint8_t>(10, 4) = map.OCCUPIED;
        map.og_.at<uint8_t>(10, 1) = map.OCCUPIED;
        map.og_.at<uint8_t>(11, 2) = map.OCCUPIED;
        map.og_.at<uint8_t>(11, 3) = map.OCCUPIED;
        map.og_.at<uint8_t>(1, 2) = map.OCCUPIED;
        map.og_.at<uint8_t>(1, 3) = map.OCCUPIED;
        range_observation = sqrt(2.5*2.5 + 0.5*0.5);
    }
};

TEST_F(ForwardSensorModelTest, test1) {
    double noise = norm_rand();
    range_observation += noise;
    double robot_angle = atan2(0.5, 2.5);
    Observation2D observation(0, 0, robot_angle, range_observation);
    double exp_range;
    double lodds =
      log_odds_observation_given_map_and_pose(
          observation,
          map,
          exp_range);

    ASSERT_NEAR(
        log_gaussian1d(noise, 0, NOISE_VARIANCE * (range_observation - noise)),
        lodds,
        0.002);
}

//double probability_observation_given_map_and_all_poses(
double log_odds_observation_given_map_and_all_poses(
    const std::vector<Observation2D>& observations,
    OccupancyGrid2D<double, int>& map)
{
    std::vector<double> lodds_vector;
    lodds_vector.reserve(observations.size());
#ifdef DEBUGDRAW
    int scan_count = 0;
    double last_px = observations[0].px;
    double last_px = observations[1].py;
    std::vector<double> angles;
    std::vector<double> ranges;
    std::vector<double> exp_ranges;
#endif
    for (std::vector<Observation2D>::const_iterator it = observations.begin();
        it != observations.end();
        ++it) {
        double expr;
        lodds_vector.push_back(
            log_odds_observation_given_map_and_pose(*it, map, expr));
#ifdef DEBUGDRAW
        // when position of the robot changes
        // draw lasers observations collected till now.
        if (last_px != (*it).px && last_py != (*it).py) {
            cv::Vec2d position(last_px, last_py);
            last_px = (*it).px;
            last_py = (*it).py;
            map.draw_and_show_lasers(position, 0, &angles[0],
                &exp_ranges[0],
                &ranges[0],
                scan_count);
            // reset
            scan_count = 0;
            angles.clear();
            ranges.clear();
            exp_ranges.clear();
        }
        scan_count++;
        angles.push_back((*it).ptheta);
        ranges.push_back((*it).range);
        exp_ranges.push_back(expr);
#endif
    }

    double lodds = 0;
    for (size_t i = 0; i < observations.size(); i ++) {
        lodds += lodds_vector[i];
    }
    return lodds;
}


// Exercise 5.1.4 - Random Number Generation
//
// by Scott Sidoli
//
// 5-30-19
//
// Main.cpp
//
// In this exercise, we experiment with a random number generator. We create a discrete uniform
// distribution with values ranging from 1 to 6 inclusively. We keep track of the count and
// use this to get the frequency. We ask the user to determine the number of trials and then
// run the simulation.

#include <boost\random.hpp>
#include <boost\random\detail\const_mod.hpp>
#include <ctime>
#include <map>
#include <iostream>

using namespace std;

int main()
{
	// Throwing dice.
	// Mersenne Twister.
	boost::random::mt19937 myRng;

	// Set the seed.
	myRng.seed(static_cast<boost::uint32_t> (std::time(0)));

	// Uniform in range [1,6]
	boost::random::uniform_int_distribution<int> six(1, 6);

	map<int, long> statistics;				// Structure to hold outcome + frequency
	int outcome;							// Current outcome

	// Setting the number of trials
	cout << "How many trials? ";
	int n;
	cin >> n;

	// Initializing the count
	for (int i = 0; i < 6; ++i)
		statistics[i + 1] = 0;

	// Dice throw
	for (int i = 0; i < n; ++i)
	{
		outcome = six(myRng);
		statistics[outcome]++;
	}

	// Setting the precision of the output
	cout.precision(6);

	cout << endl;

	// Desired print statement
	for (int i = 0; i < 6; ++i)
		cout << "Trial " << i + 1 << " has " << 100.0 * double(statistics[i + 1]) / double(n) << "% outcomes" << endl;

	return 0;
}

Scott_Sidoli Level 8 HW Submission/Section 5.1/Exercise514/Exercise514/Main.cpp