#ifndef BURST_BENCHMARK_UTILITY_RANDOM_URD_ORDER_STATISTIC_DISTRIBUTION_HPP
#define BURST_BENCHMARK_UTILITY_RANDOM_URD_ORDER_STATISTIC_DISTRIBUTION_HPP

#include <boost/math/special_functions/beta.hpp>

#include <cstddef>
#include <random>
#include <tuple>

namespace utility
{
    template <typename RealType>
    class urd_order_statistic_distribution;

    template <typename RealType>
    class urd_order_statistic_distribution_param
    {
    public:
        using distribution_type = urd_order_statistic_distribution<RealType>;

        urd_order_statistic_distribution_param
        (
            std::size_t n,
            std::size_t k,
            RealType a,
            RealType b
        ):
            m_n(n),
            m_k(k),
            m_a(a),
            m_b(b)
        {
        }

        std::size_t n () const
        {
            return m_n;
        }

        std::size_t k () const
        {
            return m_k;
        }

        RealType a () const
        {
            return m_a;
        }

        RealType b () const
        {
            return m_b;
        }

        friend bool
            operator ==
            (
                const urd_order_statistic_distribution_param & left,
                const urd_order_statistic_distribution_param & right
            )
        {
            return
                std::tie(left.m_n, left.m_k, left.m_a, left.m_b) ==
                std::tie(right.m_n, right.m_k, right.m_a, right.m_b);
        }

        friend bool
            operator !=
            (
                const urd_order_statistic_distribution_param & left,
                const urd_order_statistic_distribution_param & right
            )
        {
            return !(left == right);
        }

    private:
        std::size_t m_n;
        std::size_t m_k;
        RealType m_a;
        RealType m_b;
    };

    /*!
        \brief
            Распределение порядковых статистик выборки из стандартного непрерывного равномерного
            распределения.

        \details
            "urd" означает "uniform random distribution".
            https://ru.wikipedia.org/wiki/Порядковая_статистика#Пример
    */
    template <typename RealType = double>
    class urd_order_statistic_distribution
    {
    public:
        using result_type = RealType;
        using param_type = urd_order_statistic_distribution_param<result_type>;

        /*!
            \brief
                Создание распределения с параметрами

            \param n
                Размер выборки.
            \param k
                Номер порядковой статистики.
            \param a
                Минимальное значение равномерного распределения.
            \param b
                Максимальное значение равномерного распределения.
         */
        urd_order_statistic_distribution
        (
            std::size_t n,
            std::size_t k,
            result_type a,
            result_type b
        ):
            m_param(n, k, a, b)
        {
        }

        explicit urd_order_statistic_distribution (const param_type & p):
            m_param(p)
        {
        }

        void reset ()
        {
        }

        template <typename URNG>
        result_type operator () (URNG & g)
        {
            return (*this)(g, m_param);
        }

        template <typename URNG>
        result_type operator () (URNG & g, const param_type & p)
        {
            auto ibeta = m_uniform(g);
            auto beta = boost::math::ibeta_inv(p.k(), p.n() - p.k() + 1, ibeta);
            return beta * (p.b() - p.a()) + p.a();
        }

        std::size_t n () const
        {
            return m_param.n();
        }

        std::size_t k () const
        {
            return m_param.k();
        }

        result_type a () const
        {
            return m_param.a();
        }

        result_type b () const
        {
            return m_param.b();
        }

        param_type param () const
        {
            return m_param;
        }

        void param (const param_type & p)
        {
            m_param = p;
        }

        result_type min () const
        {
            return a();
        }

        result_type max () const
        {
            return b();
        }

        friend bool
            operator ==
            (
                const urd_order_statistic_distribution & left,
                const urd_order_statistic_distribution& right
            )
        {
            return left.m_param == right.m_param;
        }

        friend bool
            operator !=
            (
                const urd_order_statistic_distribution & left,
                const urd_order_statistic_distribution& right
            )
        {
            return !(left == right);
        }

    private:
        using uniform_real_distribution_type = std::uniform_real_distribution<result_type>;

        param_type m_param;
        uniform_real_distribution_type m_uniform{0, 1};
    };
}

#endif // BURST_BENCHMARK_UTILITY_RANDOM_URD_ORDER_STATISTIC_DISTRIBUTION_HPP


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main()
{
    /* Constructor */

    arma::mat input;
    /*
    input << 1 << 446 << 42 << arma::endr
          << 2 <<  16 << 63 << arma::endr
          << 3 <<  13 << 63 << arma::endr
          << 4 <<  21 << 21 << arma::endr
          << 1 <<  13 << 11 << arma::endr
          << 32 << 45 << 42 << arma::endr
          << 22 << 16 << 63 << arma::endr
          << 32 << 13 << 42 << arma::endr;
    */
    input << 1  << 19  << arma::endr
          << 2  << 20  << arma::endr
          << 3  << 21  << arma::endr
          << 4  << 22  << arma::endr
          << 5  << 23  << arma:: endr
          << 6  << 24  << arma::endr
          << 7  << 25  << arma::endr
          << 8  << 26  << arma:: endr
          << 9  << 27  << arma::endr
          << 10 << 28  << arma::endr
          << 11 << 29  << arma:: endr
          << 12 << 30  << arma::endr
          << 13 << 31  << arma::endr
          << 14 << 32  << arma:: endr
          << 15 << 33  << arma::endr
          << 16 << 34  << arma::endr
          << 17 << 35  << arma:: endr
          << 18 << 36  << arma::endr;
    cout << "-----------------------------------" << endl;
    cout << "Input shape : " << input.n_rows << " " << input.n_cols << endl;
    cout << "-----------------------------------" << endl;

    const size_t size = 3; // number of channels
    const double eps = 1e-5;
    const double momentum = 0.1;
    arma::mat weights, runningMean, runningVariance, gamma, beta;
    weights.set_size(size + size, 1); // (size + size, 1)
    runningMean.zeros(size, 1); // (size, 1)
    runningVariance.ones(size, 1); // (size, 1)

    /* Reset() */

    gamma = arma::mat(weights.memptr(), size, 1, false, false);  // (size, 1)
    beta = arma::mat(weights.memptr() + gamma.n_elem, size, 1, false, false); // (size, 1)
    gamma.fill(1.0);
    beta.fill(0.0);

    /* Forward */

    // Step-0 : Preparation of temporary cubes for input and output
    const size_t batchSize = input.n_cols;
    const size_t inputSize = input.n_rows / size; // (inputWidth * inputHeight)
    arma::mat output;
    output.set_size(arma::size(input));
    arma::cube inputTemp(const_cast<arma::mat&>(input).memptr(),input.n_rows / size, size, batchSize, false, false);
    arma::cube outputTemp(const_cast<arma::mat&>(output).memptr(),input.n_rows / size, size, input.n_cols, false, false);
    outputTemp = inputTemp; // n_rows = inputSize, n_cols = size, n_slices = batchSize // (4, 2, 3)

    cout << "N     : " << batchSize << endl;
    cout << "C     : " << size << endl;
    cout << "H x W : " << inputSize << endl;
    cout << "-----------------------------------" << endl;
    cout << "Input Cube - " << endl << "each slice is an image of the batch of "<< batchSize << " images" << endl << "each column of a slice is one of the " << size << " channels of the image, HxW is flattened into this single column" << endl;
    cout << "-----------------------------------" << endl;
    inputTemp.print();
    cout << "-----------------------------------" << endl;

    // PURE FORWARD FOR INSTANCE NORM

    arma::cube mean(1, size, batchSize);
    arma::cube variance(1, size, batchSize);
    for (size_t s = 0; s < inputTemp.n_slices; s++)
    {
        arma::mat& currentInputSlice = inputTemp.slice(s);
        arma::mat& currentOutputSlice = outputTemp.slice(s);

        // Step -1 :  Calculate mean and variance
        mean.slice(s) = arma::mean(currentInputSlice,0);
        variance.slice(s) = arma::var(currentInputSlice, 1, 0);

        // Step 2 : Normalisation
        currentOutputSlice -= arma::repmat(mean.slice(s), input.n_rows / size, 1);
        currentOutputSlice /= arma::sqrt(arma::repmat(variance.slice(s), input.n_rows / size, 1) + eps);

        // Step 3 : Scaling
        currentOutputSlice %= arma::repmat(gamma.t(), input.n_rows / size, 1);
        currentOutputSlice += arma::repmat(beta.t(), input.n_rows / size, 1);
    }
    cout << "Input Mean : " << endl;
    cout << mean << endl;
    cout << "-----------------------------------" << endl;
    cout << "Input Variance : " << endl;
    cout << variance << endl;
    cout << "-----------------------------------" << endl;
    cout << "Output Cube - " << endl << "each slice is an image of the batch of "<< batchSize << " images" << endl << "each column of a slice is one of the " << size << " channels of the image, HxW is flattened into this single column" << endl;
    cout << "-----------------------------------" << endl;
    outputTemp.print();
    cout << "-----------------------------------" << endl;

    cout << "Output shape : " << output.n_rows << " " << output.n_cols << endl;
    cout << "-----------------------------------" << endl;

    return 0;
}


#ifndef STAN_MATH_FWD_SCAL_FUN_INV_SQRT_HPP
#define STAN_MATH_FWD_SCAL_FUN_INV_SQRT_HPP

#include <stan/math/fwd/core.hpp>

#include <boost/math/tools/promotion.hpp>

namespace stan {
  namespace math {

    template <typename T>
    inline
    fvar<T>
    inv_sqrt(const fvar<T>& x) {
      using std::sqrt;
      T sqrt_x(sqrt(x.val_));
      return fvar<T>(1 / sqrt_x, -0.5 * x.d_ / (x.val_ * sqrt_x));
    }
  }
}
#endif


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Barycentric_coordinates_2/Mean_value_coordinates_2.h>
#include <boost/math/special_functions/fpclassify.hpp>

template<typename Kernel>
void test_mv_special_points() {

  using FT      = typename Kernel::FT;
  using Point_2 = typename Kernel::Point_2;

  const std::vector<Point_2> vertices = {
    Point_2(0, 0),
    Point_2(1, 1),
    Point_2(FT(7) / FT(4), FT(1) / FT(2)),
    Point_2(FT(7) / FT(4), FT(5) / FT(2)),
    Point_2(1, 2),
    Point_2(0, 3),
    Point_2(FT(1) / FT(2), FT(3) / FT(2))
  };

  const Point_2 queries[11] = {
    Point_2(FT(1) + (FT(1) / FT(std::pow(10.0, 300.0))), FT(2) - (FT(1) / FT(std::pow(10.0, 300.0)))),
    Point_2(FT(1) + (FT(1) / FT(std::pow(10.0, 300.0))), FT(1) + (FT(1) / FT(std::pow(10.0, 300.0)))),
    Point_2(1, FT(3) / FT(2)),
    Point_2(FT(5) / FT(4), FT(5) / FT(4)),
    Point_2(FT(5) / FT(4), FT(7) / FT(4)),
    Point_2(FT(3) / FT(2), FT(3) / FT(2)),

    Point_2(FT(7) / FT(4) - (FT(1) / FT(std::pow(10.0, 300.0))), FT(7) / FT(4) - (FT(1) / FT(std::pow(10.0, 300.0)))),
    Point_2(FT(7) / FT(4) - (FT(1) / FT(std::pow(10.0, 300.0))), FT(5) / FT(4) + (FT(1) / FT(std::pow(10.0, 300.0)))),

    Point_2(FT(3) / FT(4) - (FT(1) / FT(std::pow(10.0, 300.0))), FT(3) / FT(4) + (FT(1) / FT(std::pow(10.0, 300.0)))),
    Point_2(FT(3) / FT(4) - (FT(1) / FT(std::pow(10.0, 300.0))), FT(9) / FT(4) - (FT(1) / FT(std::pow(10.0, 300.0)))),
    Point_2(FT(1) / FT(2) + (FT(1) / FT(std::pow(10.0, 300.0))), FT(3) / FT(2))
  };

  std::size_t count = 0;
  const FT epsilon = FT(1) / FT(1000000000000000);

  std::vector<FT> coordinates;
  for (std::size_t i = 0; i < 11; ++i) {
    CGAL::Barycentric_coordinates::mean_value_coordinates_2(
      vertices, queries[i], std::back_inserter(coordinates));

    assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 0])));
    assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 0])));

    assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 1])));
    assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 1])));

    assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 2])));
    assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 2])));

    assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 3])));
    assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 3])));

    assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 4])));
    assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 4])));

    assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 5])));
    assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 5])));

    assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 6])));
    assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 6])));

    const FT coordinate_sum =
      coordinates[count + 0] +
      coordinates[count + 1] +
      coordinates[count + 2] +
      coordinates[count + 3] +
      coordinates[count + 4] +
      coordinates[count + 5] +
      coordinates[count + 6] ;
    const Point_2 linear_combination(
      vertices[0].x() * coordinates[count + 0] +
      vertices[1].x() * coordinates[count + 1] +
      vertices[2].x() * coordinates[count + 2] +
      vertices[3].x() * coordinates[count + 3] +
      vertices[4].x() * coordinates[count + 4] +
      vertices[5].x() * coordinates[count + 5] +
      vertices[6].x() * coordinates[count + 6] ,
      vertices[0].y() * coordinates[count + 0] +
      vertices[1].y() * coordinates[count + 1] +
      vertices[2].y() * coordinates[count + 2] +
      vertices[3].y() * coordinates[count + 3] +
      vertices[4].y() * coordinates[count + 4] +
      vertices[5].y() * coordinates[count + 5] +
      vertices[6].y() * coordinates[count + 6] );
    const Point_2 difference(
      linear_combination.x() - queries[i].x(),
      linear_combination.y() - queries[i].y());
    assert(
      CGAL::abs(coordinate_sum - FT(1)) < epsilon &&
      CGAL::abs(difference.x()) < epsilon &&
      CGAL::abs(difference.y()) < epsilon );
    count += 7;
  }
}

int main() {

  test_mv_special_points< CGAL::Simple_cartesian<double> >();
  test_mv_special_points< CGAL::Exact_predicates_inexact_constructions_kernel >();

  std::cout << "test_mv_special_points: PASSED" << std::endl;
  return EXIT_SUCCESS;
}


// TMA_calculator.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include "pch.h"
#include <iostream>
#include <math.h>
#include <vector>
#include <thread>
#include <dlib/matrix.h>
#include <dlib/optimization.h>
#include <dlib/global_optimization.h>
#include <wchar.h>
#include <locale.h>
#include <io.h>
#include <cstdio>
#include <cwchar>
#include <fcntl.h>

using namespace std;

vector<double> bearing = vector<double>(20, 0.0); //target bearing
vector<double> bearing_noisy = vector<double>(20, 0.0); //target bearing with random inaccuracies
vector<double> recording_time = vector<double>(20, 0.0);

//ownship coordinate (m,n)
vector<double> m = vector<double>(20, 0.0);
vector<double> n = vector<double>(20, 0.0);

unsigned int _j = 2; //iteration index
double own_ship_hdg = 0.0; //ownship heading
double last_travel_distance = 0.0; //ownship travel straight distance from last position
double last_travel_direction = 0.0; //ownship travel straight direction from last position

typedef dlib::matrix<double, 0, 1> column_vector;

// Simple upper and lower limiter
double limit(double input, double lower_limit, double upper_limit)
{
    if (input > upper_limit)
    {
        return upper_limit;
    }
    else if (input < lower_limit)
    {
        return lower_limit;
    }
    else
    {
        return input;
    }
}

vector<double> calculate_last_brg(const double &L1_distance, const double &spd, const double &crs)
{
    vector<double> out = vector<double>(3, 0.0);
    double u = spd * sin(crs * deg_to_rad); //target speed component on x axis
    double v = spd * cos(crs * deg_to_rad); //target speed component on y axis

    //target coordinate (a,b) at first bearing
    double a = L1_distance * sin(bearing[0] * deg_to_rad); //L1_distance = target distance at first bearing
    double b = L1_distance * cos(bearing[0] * deg_to_rad);

    //target coordinate (x,y) at last bearing
    double x = a + u * recording_time[_j];
    double y = b + v * recording_time[_j];

    out[0] = x;
    out[1] = y;
    out[2] = sqrt(pow(y - n[_j], 2) + pow(x - m[_j], 2));

    return out;
}

vector<double> calculate_last_brg_noisy(const double &L1_distance, const double &spd, const double &crs)
{
    vector<double> out = vector<double>(3, 0.0);
    double u = spd * sin(crs * deg_to_rad); //target speed component on x axis
    double v = spd * cos(crs * deg_to_rad); //target speed component on y axis

    //target coordinate (a,b) at first bearing
    double a = L1_distance * sin(bearing_noisy[0] * deg_to_rad); //L1_distance = target distance at first bearing
    double b = L1_distance * cos(bearing_noisy[0] * deg_to_rad);

    //target coordinate (x,y) at last bearing
    double x = a + u * recording_time[_j];
    double y = b + v * recording_time[_j];

    out[0] = x;
    out[1] = y;
    out[2] = sqrt(pow(y - n[_j], 2) + pow(x - m[_j], 2));

    return out;
}

int main()
{
    _setmode(_fileno(stdout), _O_U16TEXT); //support for chinese characters

    double optimize_L1_distance;
    double optimize_spd;
    double optimize_current_distance;
    double optimize_x;
    double optimize_y;

    //target function to minimize using BFGS algorithm
    auto target_function = [](const column_vector& mStartingPoint)
    {
        const double L1_distance = mStartingPoint(0);
        const double spd = mStartingPoint(1);
        const double crs = mStartingPoint(2);

        double u = spd * sin(crs * deg_to_rad);
        double v = spd * cos(crs * deg_to_rad);
        double a = L1_distance * sin(bearing[0] * deg_to_rad);
        double b = L1_distance * cos(bearing[0] * deg_to_rad);

        double total_error = 0.0;

        for (unsigned int i = 0; i < _j + 1; i++)
        {
            double x = a + u * recording_time[i];
            double y = b + v * recording_time[i];
            double line_error = (y - n[i]) * sin(bearing[i] * deg_to_rad) - (x - m[i]) * cos(bearing[i] * deg_to_rad);
            total_error += pow(line_error, 2);
        }

        //double penalty_for_spd = pow(limit(0.5 - spd, 0.0, 999999.0) * 100.0, 2) + pow(limit(spd - 6.0, 0.0, 999999.0) * 100.0, 2); //set speed limit: from 0.5 to 6.0 m/s
        //double penalty_for_range = pow(limit(500 - L1_distance, 0.0, 999999.0) * 0.1, 2) + pow(limit(L1_distance - 10000.0, 0.0, 999999.0) * 0.1, 2); //set limit for target distance at t1: from 500m to 10km

        return total_error;
    };

    auto target_function_noisy = [](const column_vector& mStartingPoint)
    {
        const double L1_distance = mStartingPoint(0);
        const double spd = mStartingPoint(1);
        const double crs = mStartingPoint(2);

        double u = spd * sin(crs * deg_to_rad);
        double v = spd * cos(crs * deg_to_rad);
        double a = L1_distance * sin(bearing_noisy[0] * deg_to_rad);
        double b = L1_distance * cos(bearing_noisy[0] * deg_to_rad);

        double total_error = 0.0;

        for (unsigned int i = 0; i < _j + 1; i++)
        {
            double x = a + u * recording_time[i];
            double y = b + v * recording_time[i];
            double line_error = (y - n[i]) * sin(bearing_noisy[i] * deg_to_rad) - (x - m[i]) * cos(bearing_noisy[i] * deg_to_rad);
            total_error += pow(line_error, 2);
        }
        return total_error;
    };

#ifdef _CHINESE
    std::wcout << L"说明：本舰自上一次所在位置的移动方向 = 从上一个观测点指向当前位置的绝对方位，可在海图中对两个观测点连线获得。v0.3版本加入误差分布计算，对观测到的方位角施加-0.5到0.5度范围内的随机误差，并进行1000次循环计算误差分布概率。" << endl;
    std::wcout << endl;

    std::wcout << L"时间点time t1 = 0 (sec)" << endl;

    std::wcout << L"本舰航向 (deg): ";
    std::cin >> own_ship_hdg;
    std::wcout << endl;

    std::wcout << L"敌舰相对方位角 (deg): ";
    std::cin >> bearing[0];
    std::wcout << endl;
    bearing[0] += own_ship_hdg;

    std::wcout << L"本舰自上一次所在位置的移动方向 = 0 (deg)" << endl;
    std::wcout << L"本舰距上一次所在位置的直线距离 = 0 (meter)" << endl;

    std::wcout << L"*******************************" << endl;

    std::wcout << L"时间点time t2 (sec): ";
    std::cin >> recording_time[1];
    std::wcout << endl;

    std::wcout << L"本舰航向 (deg): ";
    std::cin >> own_ship_hdg;
    std::wcout << endl;

    std::wcout << L"敌舰相对方位角 (deg): ";
    std::cin >> bearing[1];
    std::wcout << endl;
    bearing[1] += own_ship_hdg;

    std::wcout << L"本舰自上一次所在位置的移动方向 (deg): ";
    std::cin >> last_travel_direction;
    std::wcout << endl;

    std::wcout << L"本舰距上一次所在位置的直线距离 (meter): ";
    std::cin >> last_travel_distance;
    std::wcout << endl;
#else
    std::wcout << "time t1 = 0 (sec)" << endl;

    std::wcout << "ownship heading at t1 (deg): ";
    std::cin >> own_ship_hdg;
    std::wcout << endl;

    std::wcout << "target relative bearing at t1 (deg): ";
    std::cin >> bearing[0];
    std::wcout << endl;
    bearing[0] += own_ship_hdg;

    std::wcout << "true bearing from ownship last observation position to current position = 0 (deg)" << endl;
    std::wcout << "straight distance from ownship last observation position to current position = 0 (meter)" << endl;

    std::wcout << "*******************************" << endl;

    std::wcout << "time t2 (sec): ";
    std::cin >> recording_time[1];
    std::wcout << endl;

    std::wcout << "ownship heading at t2: ";
    std::cin >> own_ship_hdg;
    std::wcout << endl;

    std::wcout << "target relative bearing at t2 (deg): ";
    std::cin >> bearing[1];
    std::wcout << endl;
    bearing[1] += own_ship_hdg;

    std::wcout << "true bearing from ownship last observation position to current position (deg): ";
    std::cin >> last_travel_direction;
    std::wcout << endl;

    std::wcout << "straight distance from ownship last observation position to current position (meter): ";
    std::cin >> last_travel_distance;
    std::wcout << endl;
#endif

    m[1] = last_travel_distance * sin(last_travel_direction * deg_to_rad);
    n[1] = last_travel_distance * cos(last_travel_direction * deg_to_rad);

    //start input iteration
    for (unsigned int j = 2; j < 20; j++)
    {
        _j = j;

#ifdef _CHINESE
        std::wcout << L"*******************************" << endl;

        std::wcout << L"时间点time t" << j + 1 << L" (sec): ";
        std::cin >> recording_time[j];
        std::wcout << endl;

        std::wcout << L"本舰航向 (deg): ";
        std::cin >> own_ship_hdg;
        std::wcout << endl;

        std::wcout << L"敌舰相对方位角 (deg): ";
        std::cin >> bearing[j];
        std::wcout << endl;
        bearing[j] += own_ship_hdg;

        std::wcout << L"本舰自上一次所在位置的移动方向 (deg): ";
        std::cin >> last_travel_direction;
        std::wcout << endl;

        std::wcout << L"本舰距上一次所在位置的直线距离 (meter): ";
        std::cin >> last_travel_distance;
        std::wcout << endl;
#else
        std::wcout << "*******************************" << endl;

        std::wcout << "time t" << j + 1 << " (sec): ";
        std::cin >> recording_time[j];
        std::wcout << endl;

        std::wcout << "ownship heading at t" << j + 1 << ": ";
        std::cin >> own_ship_hdg;
        std::wcout << endl;

        std::wcout << "target relative bearing at t" << j + 1 << " (deg): ";
        std::cin >> bearing[j];
        std::wcout << endl;
        bearing[j] += own_ship_hdg;

        std::wcout << "true bearing from ownship last observation position to current position (deg): ";
        std::cin >> last_travel_direction;
        std::wcout << endl;

        std::wcout << "straight distance from ownship last observation position to current position (meter): ";
        std::cin >> last_travel_distance;
        std::wcout << endl;
#endif

        m[j] = m[j - 1] + last_travel_distance * sin(last_travel_direction * deg_to_rad);
        n[j] = n[j - 1] + last_travel_distance * cos(last_travel_direction * deg_to_rad);

        std::wcout << endl;
        column_vector starting_point = { 1000.0,1.0,0.0 };
        vector<double> optimal_crs;

        //multiple starting point for BFGS algorithm to find for multiple local minimal. (We need to keep all possible results for TMA)

        for (double L1_distance = 1000.0; L1_distance <= 10000.0; L1_distance += 500.0)
        {
            for (double spd = 1.0; spd < 10.0; spd += 2.0)
            {
                for (double crs = 0.0; crs <= 360.0; crs += 60.0)
                {
                    starting_point = { L1_distance,spd,crs };

                    dlib::find_min_using_approximate_derivatives(dlib::bfgs_search_strategy(), dlib::objective_delta_stop_strategy(1e-7), target_function, starting_point, -1);
                    vector<double> last_brg = calculate_last_brg(starting_point(0), starting_point(1), starting_point(2));

                    //adjust course result within 0-360 range
                    while (starting_point(2) < 0)
                    {
                        starting_point(2) += 360;
                    }

                    while (starting_point(2) >= 360)
                    {
                        starting_point(2) -= 360;
                    }

                    starting_point(2) = round(starting_point(2) * 100.0) / 100.0;

                    if (starting_point(0) > 1.0 && starting_point(1) > 0.0 && find(optimal_crs.begin(), optimal_crs.end(), starting_point(2)) == optimal_crs.end()) {
                        optimal_crs.push_back(starting_point(2));
                        optimize_L1_distance = starting_point(0);
                        optimize_spd = starting_point(1);
                        optimize_current_distance = last_brg[2];
                        optimize_x = last_brg[0];
                        optimize_y = last_brg[1];

                        if (abs(m[j]) < 0.1 && abs(n[j]) < 0.1)
                        {
#ifdef _CHINESE
                            std::wcout << L"敌舰航向target true course: " << starting_point(2) << L"deg" << endl;
#else
                            std::wcout << "target true course: " << starting_point(2) << "deg" << endl;
#endif
                        }
                        else
                        {
#ifdef _CHINESE
                            std::wcout << L"敌舰航向target true course: " << starting_point(2) << L"deg, 速度speed: " << optimize_spd * ms_to_kts << L"knots, 距离distance: " << optimize_current_distance << L"m" << endl;
#else
                            std::wcout << "target true course: " << starting_point(2) << "deg, speed: " << optimize_spd * ms_to_kts << "knots, distance: " << optimize_current_distance << "m" << endl;
#endif
                        }
                    }
                }
            }
        }

        //error analysis
        if (optimal_crs.size() > 0)
        {
            double last_opt_crs = optimal_crs[optimal_crs.size() - 1];
            int total_error_count = 0;
            int error_within_75m = 0;
            int error_within_150m = 0;
            int error_within_300m = 0;
            int error_within_5deg = 0;
            int error_within_10deg = 0;
            int error_within_20deg = 0;
            int error_within_05kts = 0;
            int error_within_1kts = 0;
            int error_within_2kts = 0;

            for (double L1_distance = optimize_L1_distance; L1_distance <= optimize_L1_distance + 1.0; L1_distance += 0.1)
            {
                for (double spd = optimize_spd; spd <= optimize_spd + 0.1; spd += 0.01)
                {
                    for (double crs = last_opt_crs; crs <= last_opt_crs + 1.0; crs += 0.1)
                    {
                        starting_point = { L1_distance,spd,crs };

                        for (unsigned int k = 0; k < _j + 1; k++)
                        {
                            bearing_noisy[k] = bearing[k] + (rand() % 11 - 5) / 10.0;
                        }

                        dlib::find_min_using_approximate_derivatives(dlib::bfgs_search_strategy(), dlib::objective_delta_stop_strategy(1e-7), target_function_noisy, starting_point, -1);
                        vector<double> last_brg = calculate_last_brg_noisy(starting_point(0), starting_point(1), starting_point(2));

                        //adjust course result within 0-360 range
                        while (starting_point(2) < 0)
                        {
                            starting_point(2) += 360;
                        }

                        while (starting_point(2) >= 360)
                        {
                            starting_point(2) -= 360;
                        }

                        if (starting_point(0) > 1.0 && starting_point(1) > 0.0) {
                            double last_x = last_brg[0];
                            double last_y = last_brg[1];
                            double distance_error = sqrt(pow(last_x - optimize_x, 2) + pow(last_y - optimize_y, 2));
                            double course_error = min(abs(starting_point(2) - last_opt_crs), 360.0 - abs(starting_point(2) - last_opt_crs));
                            double spd_error = abs(starting_point(1) - optimize_spd);
                            if (distance_error < 75.0)
                            {
                                error_within_75m += 1;
                            }

                            if (distance_error < 150.0)
                            {
                                error_within_150m += 1;
                            }

                            if (distance_error < 300.0)
                            {
                                error_within_300m += 1;
                            }

                            if (course_error < 5.0)
                            {
                                error_within_5deg += 1;
                            }

                            if (course_error < 10.0)
                            {
                                error_within_10deg += 1;
                            }

                            if (course_error < 20.0)
                            {
                                error_within_20deg += 1;
                            }

                            if (spd_error < 0.5)
                            {
                                error_within_05kts += 1;
                            }

                            if (spd_error < 1.0)
                            {
                                error_within_1kts += 1;
                            }

                            if (spd_error < 2.0)
                            {
                                error_within_2kts += 1;
                            }

                            total_error_count += 1;
                        }
                    }
                }
            }
            double error_prob_75m = error_within_75m * 100.0 / total_error_count;
            double error_prob_150m = error_within_150m * 100.0 / total_error_count;
            double error_prob_300m = error_within_300m * 100.0 / total_error_count;
            double error_prob_5deg = error_within_5deg * 100.0 / total_error_count;
            double error_prob_10deg = error_within_10deg * 100.0 / total_error_count;
            double error_prob_20deg = error_within_20deg * 100.0 / total_error_count;
            double error_prob_05kts = error_within_05kts * 100.0 / total_error_count;
            double error_prob_1kts = error_within_1kts * 100.0 / total_error_count;
            double error_prob_2kts = error_within_2kts * 100.0 / total_error_count;
            std::wcout << endl;

#ifdef _CHINESE
            std::wcout << L"航向误差分布位于5度以内的概率probability of target course error within 5deg: " << error_prob_5deg << " %" << endl;
            std::wcout << L"航向误差分布位于10度以内的概率probability of target course error within 10deg: " << error_prob_10deg << " %" << endl;
            std::wcout << L"航向误差分布位于20度以内的概率probability of target course error within 20deg: " << error_prob_20deg << " %" << endl;
#else
            std::wcout << "probability of target course error within 5deg: " << error_prob_5deg << " %" << endl;
            std::wcout << "probability of target course error within 10deg: " << error_prob_10deg << " %" << endl;
            std::wcout << "probability of target course error within 20deg: " << error_prob_20deg << " %" << endl;
#endif
            std::wcout << endl;
            if (abs(m[j]) < 0.1 && abs(n[j]) < 0.1)
            {
                std::wcout << j + 1 << L" bearings when stationary can only get course solution." << endl;
            }
            else
            {
#ifdef _CHINESE
                std::wcout << L"位置误差分布位于75米以内的概率probability of target positional error within 75m: " << error_prob_75m << " %" << endl;
                std::wcout << L"位置误差分布位于150米以内的概率probability of target positional error within 150m: " << error_prob_150m << " %" << endl;
                std::wcout << L"位置误差分布位于300米以内的概率probability of target positional error within 300m: " << error_prob_300m << " %" << endl;
                std::wcout << endl;
                std::wcout << "速度误差分布位于0.5节以内的概率probability of target speed error within 0.5kts: " << error_prob_05kts << " %" << endl;
                std::wcout << "速度误差分布位于1节以内的概率probability of target speed error within 1kts: " << error_prob_1kts << " %" << endl;
                std::wcout << "速度误差分布位于2节以内的概率probability of target speed error within 2kts: " << error_prob_2kts << " %" << endl;
#else
                std::wcout << "probability of target positional error within 75m: " << error_prob_75m << " %" << endl;
                std::wcout << "probability of target positional error within 150m: " << error_prob_150m << " %" << endl;
                std::wcout << "probability of target positional error within 300m: " << error_prob_300m << " %" << endl;
                std::wcout << endl;
                std::wcout << "probability of target speed error within 0.5kts: " << error_prob_05kts << " %" << endl;
                std::wcout << "probability of target speed error within 1kts: " << error_prob_1kts << " %" << endl;
                std::wcout << "probability of target speed error within 2kts: " << error_prob_2kts << " %" << endl;
#endif
            }
            std::wcout << endl;
        }
        else
        {
#ifdef _CHINESE
            std::wcout << L"无解！" << endl;
#else
        std::wcout << L"No solution found!" << endl;
#endif
        }
    }
}


/* test_exponential_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/exponential_distribution.hpp>

#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::exponential_distribution<>
#define BOOST_RANDOM_ARG1 lambda
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0.0

#define BOOST_RANDOM_TEST2_PARAMS (1000.0)
#define BOOST_RANDOM_TEST2_MIN 0.0

#include "test_distribution.ipp"


/*
 * Copyright 2009-2018 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE symmetric_matrix_test
#include <boost/test/unit_test.hpp>
#include <votca/xtp/symmetric_matrix.h>
#include <iostream>

using namespace votca::xtp;

BOOST_AUTO_TEST_SUITE(symmetric_matrix_test)

BOOST_AUTO_TEST_CASE(Constructor_test) {
  int dim=3;
Eigen::MatrixXd test=Eigen::MatrixXd::Random(dim,dim);
 Eigen::MatrixXd trans=test.transpose();
test+=trans;
Symmetric_Matrix sym=Symmetric_Matrix(test);

Eigen::MatrixXd back=Eigen::MatrixXd::Zero(dim,dim);

sym.AddtoEigenMatrix(back);

bool check_matrices=back.isApprox(test,0.000001);
if(!check_matrices){
   std::cout<<sym<<std::endl;
  std::cout<<test<<std::endl;
  std::cout<<back<<std::endl;
}
BOOST_CHECK_EQUAL(check_matrices, 1);
}

BOOST_AUTO_TEST_CASE(Add_test) {

   int dim=3;
Eigen::MatrixXd test=Eigen::MatrixXd::Random(dim,dim);
 Eigen::MatrixXd trans=test.transpose();
test+=trans;
Symmetric_Matrix sym=Symmetric_Matrix(test);

Eigen::MatrixXd rand=Eigen::MatrixXd::Random(dim,dim);

Eigen::MatrixXd result=rand+2*test;

sym.AddtoEigenMatrix(rand,2.0);

bool check_matrices2=rand.isApprox(result,0.000001);
if(!check_matrices2){
  std::cout<<test<<std::endl;
  std::cout<<sym<<std::endl;
  std::cout<<rand<<std::endl;
  std::cout<<result<<std::endl;
}
BOOST_CHECK_EQUAL(check_matrices2, 1);

}

BOOST_AUTO_TEST_CASE(FullMatrix_test) {

int dim=3;
Eigen::MatrixXd test=Eigen::MatrixXd::Random(dim,dim);
 Eigen::MatrixXd trans=test.transpose();
test+=trans;
Symmetric_Matrix sym=Symmetric_Matrix(test);
Eigen::MatrixXd result=sym.FullMatrix();
bool check_matrices=test.isApprox(result,0.000001);

BOOST_CHECK_EQUAL(check_matrices, 1);
}

BOOST_AUTO_TEST_CASE(TraceofProd_test) {

   int dim=3;
Eigen::MatrixXd test=Eigen::MatrixXd::Random(dim,dim);
 Eigen::MatrixXd trans=test.transpose();
test+=trans;
Symmetric_Matrix sym1=Symmetric_Matrix(test);

Eigen::MatrixXd test2=Eigen::MatrixXd::Random(dim,dim);
 Eigen::MatrixXd trans2=test2.transpose();
test2+=trans2;
Symmetric_Matrix sym2=Symmetric_Matrix(test2);

double ref=test2.cwiseProduct(test).sum();
double result=sym1.TraceofProd(sym2);

bool check=std::abs(ref-result)<1e-7;
if(!check){
  std::cout<<"Ref: "<<ref<<" Sym: "<<result<<std::endl;
}

BOOST_CHECK_EQUAL(check, 1);

}

BOOST_AUTO_TEST_SUITE_END()


/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 * Copyright (C) 2018-2021 University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */

/**
 * @file   test-mp2p_error_terms_jacobians.cpp
 * @brief  Unit tests for Jacobians of error terms
 * @author Francisco Jose Mañas Alvarez, Jose Luis Blanco Claraco
 * @date   Apr 10, 2020
 */

#include <mp2p_icp/errorTerms.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/math/num_jacobian.h>  // finite difference method
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/random.h>

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

using namespace mrpt;  // for the "_deg" suffix
using namespace mrpt::math;
using namespace mrpt::poses;

auto& rnd = mrpt::random::getRandomGenerator();

static double normald(const double sigma)
{
    return rnd.drawGaussian1D_normalized() * sigma;
}
static float normalf(const float sigma)
{
    return rnd.drawGaussian1D_normalized() * sigma;
}

// ===========================================================================
//  Test: error_point2point
// ===========================================================================

static void test_Jacob_error_point2point()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mrpt::tfest::TMatchingPair pair;
    pair.global = {normalf(20), normalf(20), normalf(20)};
    pair.local  = {normalf(20), normalf(20), normalf(20)};

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 3, 12> J1;
    // const mrpt::math::CVectorFixed<double, 3> error = // (Ignored here)
    mp2p_icp::error_point2point(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<3>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<3>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_point2point(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_point2line
// ===========================================================================

static void test_Jacob_error_point2line()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mp2p_icp::point_line_pair_t pair;

    pair.ln_global.pBase.x = normalf(20);
    pair.ln_global.pBase.y = normalf(20);
    pair.ln_global.pBase.z = normalf(20);
    pair.ln_global.director =
        mrpt::math::TPoint3D(normald(1), normald(1), normald(1)).unitarize();

    pair.pt_local.x = normalf(10);
    pair.pt_local.y = normalf(10);
    pair.pt_local.z = normalf(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 3, 12> J1;

    mp2p_icp::error_point2line(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-5);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<3>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<3>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_point2line(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "relativePose: " << p
                  << "\n"
                     "numJacob:"
                  << numJacob.asEigen()
                  << "\n"
                     "jacob   :"
                  << jacob.asEigen()
                  << "\n"
                     "diff    :"
                  << (numJacob - jacob)
                  << "\n"
                     "J1      :"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_point2plane
// ===========================================================================

static void test_Jacob_error_point2plane()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mp2p_icp::point_plane_pair_t pair;

    pair.pl_global.centroid.x     = normalf(20);
    pair.pl_global.centroid.y     = normalf(20);
    pair.pl_global.centroid.z     = normalf(20);
    pair.pl_global.plane.coefs[0] = normald(20);
    pair.pl_global.plane.coefs[1] = normald(20);
    pair.pl_global.plane.coefs[2] = normald(20);

    pair.pt_local.x = normalf(10);
    pair.pt_local.y = normalf(10);
    pair.pt_local.z = normalf(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 3, 12> J1;

    mp2p_icp::error_point2plane(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<3>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<3>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_point2plane(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_line2line
// ===========================================================================

static void test_Jacob_error_line2line()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mp2p_icp::matched_line_t pair;

    pair.ln_global.pBase.x     = normalf(10);
    pair.ln_global.pBase.y     = normalf(10);
    pair.ln_global.pBase.z     = normalf(10);
    pair.ln_global.director[0] = normald(10);
    pair.ln_global.director[1] = normald(10);
    pair.ln_global.director[2] = normald(10);

    pair.ln_local.pBase.x     = normalf(10);
    pair.ln_local.pBase.y     = normalf(10);
    pair.ln_local.pBase.z     = normalf(10);
    pair.ln_local.director[0] = normald(10);
    pair.ln_local.director[1] = normald(10);
    pair.ln_local.director[2] = normald(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 4, 12> J1;

    mp2p_icp::error_line2line(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<4>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<4>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_line2line(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_plane2plane
// ===========================================================================

static void test_Jacob_error_plane2plane()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mp2p_icp::matched_plane_t pair;

    pair.p_global.centroid.x     = normalf(20);
    pair.p_global.centroid.y     = normalf(20);
    pair.p_global.centroid.z     = normalf(20);
    pair.p_global.plane.coefs[0] = normald(20);
    pair.p_global.plane.coefs[1] = normald(20);
    pair.p_global.plane.coefs[2] = normald(20);

    pair.p_local.centroid.x     = normalf(10);
    pair.p_local.centroid.y     = normalf(10);
    pair.p_local.centroid.z     = normalf(10);
    pair.p_local.plane.coefs[0] = normald(10);
    pair.p_local.plane.coefs[1] = normald(10);
    pair.p_local.plane.coefs[2] = normald(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 3, 12> J1;

    mp2p_icp::error_plane2plane(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<3>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<3>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_plane2plane(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_line2line
// ===========================================================================

static void test_error_line2line()
{
    const CPose3D p = CPose3D(
        // x y z
        1, 0.5, 0.1,
        // Yaw pitch roll
        0, 0, 0);

    mp2p_icp::matched_line_t pair;

    pair.ln_global.pBase.x     = 0;
    pair.ln_global.pBase.y     = 1;
    pair.ln_global.pBase.z     = -4;
    pair.ln_global.director[0] = 0.4364;
    pair.ln_global.director[1] = 0.8729;
    pair.ln_global.director[2] = -0.2182;

    pair.ln_local.pBase.x     = 2;
    pair.ln_local.pBase.y     = 1;
    pair.ln_local.pBase.z     = -0.5;
    pair.ln_local.director[0] = 0.2357;
    pair.ln_local.director[1] = 0.2357;
    pair.ln_local.director[2] = 0.9428;

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 4, 12> J1;

    mrpt::math::CVectorFixedDouble<4> ref_error;
    ref_error[0] = 0.0517;
    ref_error[1] = 0.2007;
    ref_error[2] = 0.6372;
    ref_error[3] = -1.1610;

#if 0
    mrpt::math::CVectorFixedDouble<4> error =
        mp2p_icp::error_line2line(pair, p, J1);

    std::cout << "\nResultado: \n"
              << error << "\nRecta A:\n"
              << pair.ln_global << "\nRecta B:\n"
              << pair.ln_local << "\n";
#endif

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<4>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<4>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_line2line(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }
#if 0
    std::cout << "numJacob:\n"
              << numJacob.asEigen() << "\njacob:\n"
              << jacob.asEigen() << "\nDiff:\n"
              << (numJacob - jacob) << "\nJ1:\n"
              << J1.asEigen() << "\ndDexp_de:\n"
              << dDexpe_de.asEigen() << "\n";
#endif
}

static void test_against_ground_truth_error_point2line()
{
    const CPose3D p = CPose3D::Identity();

    mp2p_icp::point_line_pair_t pair;

    pair.ln_global.pBase.x  = 10;
    pair.ln_global.pBase.y  = 11;
    pair.ln_global.pBase.z  = 12;
    pair.ln_global.director = mrpt::math::TPoint3D(0, 0, 1).unitarize();

    pair.pt_local.x = 10;
    pair.pt_local.y = 11;
    pair.pt_local.z = -1.0;

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 3, 12> J1;

    CVectorFixedDouble<3> err = mp2p_icp::error_point2line(pair, p, J1);
    ASSERT_NEAR_(err[0], 0.0, 1e-6);
}

int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
{
    try
    {
        // test Jacobians:
        // ----------------------------------------------
        rnd.randomize(1234);  // for reproducible tests

        // Run for many different random conditions:
        for (int i = 0; i < 1000; i++)
        {
            test_Jacob_error_point2point();
            test_Jacob_error_point2line();
            test_Jacob_error_point2plane();
            test_Jacob_error_plane2plane();
            // TODO: Fix this one:
            // test_Jacob_error_line2line();

            test_error_line2line();
        }

        // Test for known fixed conditions:
        // ----------------------------------------------
        test_against_ground_truth_error_point2line();
    }
    catch (std::exception& e)
    {
        std::cerr << mrpt::exception_to_str(e) << "\n";
        return 1;
    }
}


#include <iostream>
#include <utility>

#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::multiprecision::cpp_bin_float_single;
using boost::multiprecision::cpp_bin_float_double;

typedef boost::multiprecision::cpp_bin_float_single f32;
typedef boost::multiprecision::cpp_bin_float_double f64;

int main(void)
{
#if BUGTEST_PRECISION == 32
    //f32 pi32 = boost::math::constants::pi<f32, boost::math::policies::policy<boost::math::policies::digits2<32> > >();
    f32 pi32 = boost::math::constants::pi<f32>();
    std::cout << pi32 << std::endl;
#elif BUGTEST_PRECISION == 64
    //f64 pi64 = boost::math::constants::pi<f64, boost::math::policies::policy<boost::math::policies::digits2<64> > >();
    f64 pi64 = boost::math::constants::pi<f64>();
    std::cout << pi64 << std::endl;
#else
#error Set BUGTEST_PRECISION to 32 or 64.
#endif
    return 0;
}


/* Boost check_gmp.cpp test file

 Copyright 2009 Karsten Ahnert
 Copyright 2009 Mario Mulansky

 This file tests the odeint library with the gmp arbitrary precision types

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

#define BOOST_TEST_MODULE odeint_gmp

#include <iostream>

#include <gmpxx.h>

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

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint.hpp>
//#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

namespace mpl = boost::mpl;

const int precision = 1024;

typedef mpf_class value_type;
typedef mpf_class state_type;

//provide min, max and pow functions for mpf types - required for controlled steppers
value_type min( const value_type a , const value_type b )
{
    if( a<b ) return a;
    else return b;
}
value_type max( const value_type a , const value_type b )
{
    if( a>b ) return a;
    else return b;
}
value_type pow( const value_type a , const value_type b )
{
    // do calculation in double precision
    return value_type( std::pow( a.get_d() , b.get_d() ) );
}

//provide vector_space reduce:

namespace boost { namespace numeric { namespace odeint {

template<>
struct vector_space_reduce< state_type >
{
  template< class Op >
  state_type operator()( state_type x , Op op , state_type init ) const
  {
      init = op( init , x );
      return init;
  }
};

} } }

void constant_system( const state_type &x , state_type &dxdt , value_type t )
{
    dxdt = value_type( 1.0 , precision );
}

/* check runge kutta stepers */
typedef mpl::vector<
    euler< state_type , value_type , state_type , value_type , vector_space_algebra > ,
    modified_midpoint< state_type , value_type , state_type , value_type , vector_space_algebra > ,
    runge_kutta4< state_type , value_type , state_type , value_type , vector_space_algebra > ,
    runge_kutta4_classic< state_type , value_type , state_type , value_type , vector_space_algebra > ,
    runge_kutta_cash_karp54_classic< state_type , value_type , state_type , value_type , vector_space_algebra > ,
    runge_kutta_cash_karp54< state_type , value_type , state_type , value_type , vector_space_algebra > ,
    runge_kutta_dopri5< state_type , value_type , state_type , value_type , vector_space_algebra > ,
    runge_kutta_fehlberg78< state_type , value_type , state_type , value_type , vector_space_algebra >
    > stepper_types;

template< class Stepper >
struct perform_runge_kutta_test {

    void operator()( void )
    {
        /* We have to specify the desired precision in advance! */
        mpf_set_default_prec( precision );

        mpf_t eps_ , unity;
        mpf_init( eps_ ); mpf_init( unity );
        mpf_set_d( unity , 1.0 );
        mpf_div_2exp( eps_ , unity , precision-1 ); // 2^(-precision+1) : smallest number that can be represented with used precision
        value_type eps( eps_ );

        Stepper stepper;
        state_type x;
        x = 0.0;

        stepper.do_step( constant_system , x , 0.0 , 0.1 );

        BOOST_MESSAGE( eps );
        BOOST_CHECK_MESSAGE( abs( x - value_type( 0.1 , precision ) ) < eps , x - 0.1 );
    }
};

BOOST_AUTO_TEST_CASE_TEMPLATE( runge_kutta_stepper_test , Stepper , stepper_types )
{
    perform_runge_kutta_test< Stepper > tester;
    tester();
}

/* check controlled steppers */
typedef mpl::vector<
    controlled_runge_kutta< runge_kutta_cash_karp54_classic< state_type , value_type , state_type , value_type , vector_space_algebra > > ,
    controlled_runge_kutta< runge_kutta_dopri5< state_type , value_type , state_type , value_type , vector_space_algebra > > ,
    controlled_runge_kutta< runge_kutta_fehlberg78< state_type , value_type , state_type , value_type , vector_space_algebra > > ,
    bulirsch_stoer< state_type , value_type , state_type , value_type , vector_space_algebra >
    > controlled_stepper_types;

template< class Stepper >
struct perform_controlled_test {

    void operator()( void )
    {
        mpf_set_default_prec( precision );

        mpf_t eps_ , unity;
        mpf_init( eps_ ); mpf_init( unity );
        mpf_set_d( unity , 1.0 );
        mpf_div_2exp( eps_ , unity , precision-1 ); // 2^(-precision+1) : smallest number that can be represented with used precision
        value_type eps( eps_ );

        Stepper stepper;
        state_type x;
        x = 0.0;

        value_type t(0.0);
        value_type dt(0.1);

        stepper.try_step( constant_system , x , t , dt );

        BOOST_MESSAGE( eps );
        BOOST_CHECK_MESSAGE( abs( x - value_type( 0.1 , precision ) ) < eps , x - 0.1 );
    }
};

BOOST_AUTO_TEST_CASE_TEMPLATE( controlled_stepper_test , Stepper , controlled_stepper_types )
{
    perform_controlled_test< Stepper > tester;
    tester();
}


#ifndef TURBOTRACK_HPP
#define TURBOTRACK_HPP

#include <Eigen/Dense>

namespace turbotrack {

enum class TrackballType {
	// chen_et_al, not implemented yet
	shoemake,
	holroyd
};

// See equation 33 in Henriksen et al.
Eigen::Vector3f shoemake_projection(Eigen::Vector2f const &mouse, float radius);

// See equation 46 in Henriksen et al.
Eigen::Vector3f holroyd_projection(Eigen::Vector2f const &mouse, float radius);

Eigen::Quaternionf mouse_move(Eigen::Vector2f const &old_pos,
                              Eigen::Vector2f const &new_pos,
                              float radius = 1.0,
                              TrackballType type = TrackballType::holroyd);

} // namespace turbotrack

#endif // TURBOTRACK_HPP


#include <stan/math/prim.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>

stan::math::vector_d get_simplex_Phi(double lambda,
                                     const stan::math::vector_d& c) {
  using stan::math::Phi;
  int K = c.size() + 1;
  stan::math::vector_d theta(K);
  theta(0) = 1.0 - Phi(lambda - c(0));
  for (int k = 1; k < (K - 1); ++k)
    theta(k) = Phi(lambda - c(k - 1)) - Phi(lambda - c(k));
  // - 0.0
  theta(K - 1) = Phi(lambda - c(K - 2));
  return theta;
}

TEST(ProbDistributions, ordered_probit_stability) {
  using stan::math::is_inf;
  using stan::math::ordered_probit_log;

  Eigen::VectorXd c(3);
  c << -0.3, 0.1, 1.2;

  EXPECT_FALSE(is_inf(ordered_probit_log(1, 10, c)));
  EXPECT_FALSE(is_inf(ordered_probit_log(2, 10, c)));
  EXPECT_NE(ordered_probit_log(4, 10, c), 0);

  EXPECT_NE(ordered_probit_log(1, -38, c), 0);
  EXPECT_FALSE(is_inf(ordered_probit_log(2, -38, c)));
  EXPECT_FALSE(is_inf(ordered_probit_log(4, -38, c)));
}

TEST(ProbDistributions, ordered_probit_vals) {
  using Eigen::Dynamic;
  using Eigen::Matrix;

  using stan::math::ordered_probit_log;
  using stan::math::Phi;

  int K = 5;
  Matrix<double, Dynamic, 1> c(K - 1);
  c << -1.7, -0.3, 1.2, 2.6;
  double lambda = 1.1;

  stan::math::vector_d theta = get_simplex_Phi(lambda, c);

  double sum = 0.0;
  for (int k = 0; k < theta.size(); ++k)
    sum += theta(k);
  EXPECT_FLOAT_EQ(1.0, sum);

  for (int k = 0; k < K; ++k)
    EXPECT_FLOAT_EQ(log(theta(k)), ordered_probit_log(k + 1, lambda, c));

  EXPECT_THROW(ordered_probit_log(0, lambda, c), std::domain_error);
  EXPECT_THROW(ordered_probit_log(6, lambda, c), std::domain_error);
}

TEST(ProbDistributions, ordered_probit_vals_2) {
  using Eigen::Dynamic;
  using Eigen::Matrix;

  using stan::math::ordered_probit_log;
  using stan::math::Phi;

  int K = 3;
  Matrix<double, Dynamic, 1> c(K - 1);
  c << -0.2, 4;
  double lambda = -0.9;

  stan::math::vector_d theta = get_simplex_Phi(lambda, c);

  double sum = 0.0;
  for (int k = 0; k < theta.size(); ++k)
    sum += theta(k);
  EXPECT_FLOAT_EQ(1.0, sum);

  for (int k = 0; k < K; ++k)
    EXPECT_FLOAT_EQ(log(theta(k)), ordered_probit_log(k + 1, lambda, c));

  EXPECT_THROW(ordered_probit_log(0, lambda, c), std::domain_error);
  EXPECT_THROW(ordered_probit_log(4, lambda, c), std::domain_error);
}

TEST(ProbDistributions, ordered_probit) {
  using stan::math::ordered_probit_log;
  int K = 4;
  Eigen::Matrix<double, Eigen::Dynamic, 1> c(K - 1);
  c << -0.3, 0.1, 1.2;
  double lambda = 0.5;
  EXPECT_THROW(ordered_probit_log(-1, lambda, c), std::domain_error);
  EXPECT_THROW(ordered_probit_log(0, lambda, c), std::domain_error);
  EXPECT_THROW(ordered_probit_log(5, lambda, c), std::domain_error);
  for (int k = 1; k <= K; ++k)
    EXPECT_NO_THROW(ordered_probit_log(k, lambda, c));

  // init size zero
  Eigen::Matrix<double, Eigen::Dynamic, 1> c_zero;
  EXPECT_EQ(0, c_zero.size());
  EXPECT_THROW(ordered_probit_log(1, lambda, c_zero), std::invalid_argument);

  Eigen::Matrix<double, Eigen::Dynamic, 1> c_neg(1);
  c_neg << -13.7;
  EXPECT_NO_THROW(ordered_probit_log(1, lambda, c_neg));

  Eigen::Matrix<double, Eigen::Dynamic, 1> c_unord(3);
  c_unord << 1.0, 0.4, 2.0;
  EXPECT_THROW(ordered_probit_log(1, lambda, c_unord), std::domain_error);

  Eigen::Matrix<double, Eigen::Dynamic, 1> c_unord_2(3);
  c_unord_2 << 1.0, 2.0, 0.4;
  EXPECT_THROW(ordered_probit_log(1, lambda, c_unord_2), std::domain_error);

  double nan = std::numeric_limits<double>::quiet_NaN();
  double inf = std::numeric_limits<double>::infinity();

  EXPECT_THROW(ordered_probit_log(1, nan, c), std::domain_error);
  EXPECT_THROW(ordered_probit_log(1, inf, c), std::domain_error);

  Eigen::Matrix<double, Eigen::Dynamic, 1> cbad(2);
  cbad << 0.2, inf;
  EXPECT_THROW(ordered_probit_log(1, 1.0, cbad), std::domain_error);
  cbad[1] = nan;
  EXPECT_THROW(ordered_probit_log(1, 1.0, cbad), std::domain_error);

  Eigen::Matrix<double, Eigen::Dynamic, 1> cbad1(1);
  cbad1 << inf;
  EXPECT_THROW(ordered_probit_log(1, 1.0, cbad1), std::domain_error);
  cbad1[0] = nan;
  EXPECT_THROW(ordered_probit_log(1, 1.0, cbad1), std::domain_error);

  Eigen::Matrix<double, Eigen::Dynamic, 1> cbad3(3);
  cbad3 << 0.5, inf, 1.0;
  EXPECT_THROW(ordered_probit_log(1, 1.0, cbad3), std::domain_error);
  cbad3[1] = nan;
  EXPECT_THROW(ordered_probit_log(1, 1.0, cbad3), std::domain_error);
}

TEST(ProbDistributionOrderedProbit, error_check) {
  boost::random::mt19937 rng;
  double inf = std::numeric_limits<double>::infinity();
  Eigen::VectorXd c(4);
  c << -2, 2.0, 5, 10;
  EXPECT_NO_THROW(stan::math::ordered_probit_rng(4.0, c, rng));

  EXPECT_THROW(
      stan::math::ordered_probit_rng(stan::math::positive_infinity(), c, rng),
      std::domain_error);
  c << -inf, 2.0, -5, inf;
  EXPECT_THROW(stan::math::ordered_probit_rng(4.0, c, rng), std::domain_error);
}

TEST(ProbDistributionOrderedProbit, chiSquareGoodnessFitTest) {
  using stan::math::Phi;
  boost::random::mt19937 rng;
  int N = 10000;
  double eta = 1.0;
  Eigen::VectorXd theta(3);
  theta << -0.4, 4.0, 6.2;
  Eigen::VectorXd prob(4);
  prob(0) = 1 - Phi(eta - theta(0));
  prob(1) = Phi(eta - theta(0)) - Phi(eta - theta(1));
  prob(2) = Phi(eta - theta(1)) - Phi(eta - theta(2));
  prob(3) = Phi(eta - theta(2));
  int K = prob.rows();
  boost::math::chi_squared mydist(K - 1);

  Eigen::VectorXd loc(prob.rows());
  for (int i = 0; i < prob.rows(); i++)
    loc(i) = 0;

  for (int i = 0; i < prob.rows(); i++) {
    for (int j = i; j < prob.rows(); j++)
      loc(j) += prob(i);
  }

  int count = 0;
  int bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * prob(i);
  }

  while (count < N) {
    int a = stan::math::ordered_probit_rng(eta, theta, rng);
    bin[a - 1]++;
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);
  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


#ifndef CONCEPTS_DIMENSIONS_GROUP_HPP_INCLUDED
#define CONCEPTS_DIMENSIONS_GROUP_HPP_INCLUDED

#include <limits>

#include <boost/hana/assert.hpp>
#include <boost/hana/zero.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/less.hpp>

#include <gem/fwd/dimensions.hpp>

namespace boost::hana {

template<typename T>
static constexpr
auto safe_sub(const T & v1, const T & v2)
{
    return v1 >= v2 ? v1 - v2 : 0;
}

gem::concepts::detail::DimensionPair {T1, cv1, max1, min1, T2, cv2, max2, min2}
struct minus_impl<gem::Dimension<T1, cv1, max1, min1>,
                  gem::Dimension<T2, cv2, max2, min2>>
{
private:
    using ctype = typename boost::hana::common<T1, T2>::type;

public:
    static constexpr auto
    apply(const gem::Dimension<T1, cv1, max1, min1>& d1,
          const gem::Dimension<T2, cv2, max2, min2>& d2)
    {
        BOOST_HANA_RUNTIME_CHECK_MSG(d1.value() >= d2.value(),
                                     "Dimension underflow...");
        return gem::Dimension<ctype, safe_sub<ctype>(cv1, cv2),
                                     safe_sub<ctype>(max1, min2),
                                     safe_sub<ctype>(min1, max2)> {d1.value() -
                                                                   d2.value()};
    }
};

template<typename T1, T1 cv1, typename T2, T2 cv2>
struct minus_impl<gem::Dimension<T1, cv1, cv1, cv1>,
                  gem::Dimension<T2, cv2, cv2, cv2>>
{
private:
    using ctype = typename boost::hana::common<T1, T2>::type;

public:
    static constexpr auto
    apply(const gem::Dimension<T1, cv1, cv1, cv1> &,
          const gem::Dimension<T2, cv2, cv2, cv2> &)
    {
        constexpr auto c = integral_c<ctype, cv1>;
        constexpr auto m = integral_c<ctype, cv2>;
        BOOST_HANA_CONSTANT_CHECK_MSG(c >= m, "Dimension underflow...");
        constexpr ctype s = static_cast<ctype>(cv1) -
                            static_cast<ctype>(cv2);
        return gem::Dimension<ctype, s, s, s> {};
    }
};

}  // namespace boost::hana

namespace gem {

template <typename T1, T1 cv1, T1 cv_max1, T1 cv_min1,
          typename T2, T2 cv2, T2 cv_max2, T2 cv_min2>
constexpr inline auto
operator-(const gem::Dimension<T1, cv1, cv_max1, cv_min1> & d1,
          const gem::Dimension<T2, cv2, cv_max2, cv_min2> & d2)
{
    return boost::hana::minus(d1, d2);
}

}  // namespace gem

#endif  // !CONCEPTS_DIMENSIONS_GROUP_HPP_INCLUDED


/** @file spanning_trees.cc
 * @author David F. Gleich
 * @date 2008-09-29
 * @copyright Stanford University, 2006-2008
 * Implement the BGL spanning tree wrappers.
 */

/** History
 *  2006-04-20: Initial version
 *  2006-11-10: Fixed bug with incorrect number of edges returned,
 *    when the input graph has multiple components.
 *    The nedges output parameter is now set correctly for all algorithms.
 *    Although, it depends on a somewhat dubious "hack" to detect
 *    unused portions of the output iterator.
 *  2007-07-09: Switched to simple_csr_matrix graph type
 *    Switched to kruskal mst from boost mod to fix bug with output iterator
 *  2007-11-16: Added root vertex option to prim's MST
 *  2008-10-01: Changed copy_to_ijval to use mbglIndex instead of int.
 *    Removed old commented regions.
 */

#include "include/matlab_bgl.h"

#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <yasmic/boost_mod/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

#include <vector>

/*template <class Graph, class Edge>
class spanning_tree_insert_iterator
    : public boost::iterator_facade<
        spanning_tree_insert_iterator<Edge>
      , Edge
      , boost::forward_traversal_tag
    >
{
public:
    int* i; int* j; double* val;
    Graph& g;

    spanning_tree_insert_iterator(int* _i, int* _j, double* _val)
        : i(_i), j(_j), val(_val)
    {}

private:
    friend class boost::iterator_core_access;

    void increment()
    {
        i++;
        j++;
        val++;
    }

    bool equal(spanning_tree_insert_iterator const& other)
    {
        return (i == other.i && j == other.j && val == other.val);
    }

    Edge

};*/

template <class Graph, class EdgeWeightPropMap, class Iterator>
mbglIndex copy_to_ijval(Graph& g, EdgeWeightPropMap ewpm, Iterator oi,
                   Iterator oi_end, mbglIndex* i, mbglIndex* j, double* val)
{
    using namespace boost;

    mbglIndex ei;
    for (ei= 0; oi != oi_end; ++oi, ++ei) {
        typename graph_traits<Graph>::edge_descriptor e = *oi;
        i[ei] = source(e,g);
        j[ei] = target(e,g);
        val[ei] = ewpm[e];
    }

    return (ei);
}

int kruskal_mst(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex* i, mbglIndex* j, double* val /* tree output */,
    mbglIndex* nedges)
{
    using namespace yasmic;
    using namespace boost;

    // create the graph g
    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    //
    // warning, this code assumes that the default constructor for
    // an edges has source == target, otherwise we will detect
    // incorrect edges in the next step.
    //
    std::vector<graph_traits<crs_weighted_graph>::edge_descriptor>
        oi(nverts-1);

    std::vector<graph_traits<crs_weighted_graph>::edge_descriptor>::iterator
        oi_end = kruskal_minimum_spanning_tree(g,oi.begin());

    //*nedges = nverts-1;
    //*nedges = (int)(oi_end - oi.begin());

    // warning, this code assumes that the default constructor for
    // an edges has source == target, otherwise we will detect
    // incorrect edges in the next step.
    *nedges = copy_to_ijval(g,get(edge_weight,g),
        oi.begin(), oi_end, i, j, val);

    return (0);
}

int prim_mst_rooted(mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    double *weight, /* connectivity params */
    mbglIndex* i, mbglIndex* j, double* val, mbglIndex *nedges, /* tree output */
    mbglIndex root /* tree root */)
{
  using namespace yasmic;
  using namespace boost;

  typedef simple_csr_matrix<mbglIndex, double> crs_weighted_graph;
  crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

  std::vector<mbglIndex> pred(nverts);

  prim_minimum_spanning_tree(g, make_iterator_property_map(pred.begin(), get(
      vertex_index, g)), root_vertex(root));

  mbglIndex edge_num = 0;
  for (mbglIndex pi = 0; pi < nverts; pi++) {
    if (pred[pi] == pi) {
      // this edge isn't present
    } else {
      assert(edge_num<nverts-1);

      i[edge_num] = pi;
      j[edge_num] = pred[pi];
      val[edge_num] = 0.0;

      for (mbglIndex k = ia[pred[pi]]; k < ia[pred[pi] + 1]; k++) {
        if (ja[k] == pi) {
          val[edge_num] = weight[k];
          break;
        }
      }

      edge_num++;
    }
  }

  *nedges = edge_num;

  return (0);
}

/**
 * Compute a minimum spanning tree starting from vertex 0 using Prim's algorithm
 */
int prim_mst(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex* i, mbglIndex* j, double* val, mbglIndex *nedges /* tree output */)
{
    // for our graph type, calling for a rooted tree with root 0 is identical
    // to calling for the default root.
    return prim_mst_rooted(nverts, ja, ia, weight, i, j, val, nedges, 0);
}


/**
 * This file contains functions for solving stochastic optimization problems provided the user extends the
 * "Parameters" class to update the model underlying the parameters and provides both an objective function and a
 * Jacobian thereof that conforms to the expected format for the optimization algorithm.
 */

#ifndef PROMETHEUS_OPTIMIZATION_HPP
#define PROMETHEUS_OPTIMIZATION_HPP

#include <Eigen/Dense>
#include "stopwatch.hpp"

// zach's-attempt-at-math
namespace zaamath {

    /**
     * template class responsible for maintaining the parameters used during optimization and updating the parameters
     * (and optionally the underlying model of those parameters) once an updated set of parameters are passed in by the
     * optimization algorithm
     * @tparam RowIndexType the number of rows of the parameter vector (number of parameters)
     */
    template <int RowIndexType>
    class Parameters {
    protected:
        Eigen::Matrix<double, RowIndexType, 1> eigen_params_;

    public:
        Parameters(Eigen::Matrix<double, RowIndexType, 1>& eigen_params) : eigen_params_(eigen_params) {

        }

        virtual ~Parameters() = default;

        virtual void update(Eigen::Matrix<double, RowIndexType, 1>& eigen_params) {
            eigen_params_ = eigen_params;
        }

        Eigen::Matrix<double, RowIndexType, 1> eigen_params() {
            return eigen_params_;
        }
    };

    /**
     * This class implements the stochastic gradient descent optimization algorithm known as ADAM (short for Adaptive
     * Moment Estimation) as well as holds meta-data for the algorithm such as termination conditions and
     * hyper-parameters.
     *
     * The class performs inplace optimization, updating the Parameters object passed in by reference by the user every
     * iteration. Additional the class stores member variables about the status of the algorithm at each iteration,
     * which can be read by the client to discover things about the covergence of the algorthim. Therefore, if one seeks
     * to perform more than one optimization with the same Adam instance, one must first reset those member variables
     * with the
     */
    class Adam {
    private:
        bool show_trace_{true};
        int show_trace_every_{15};
        int f_x_tol_interval_{20};
        double f_x_tol_{1e-10};
        int x_tol_interval_{20};
        double x_tol_{1e-12};
        int max_iterations_{10000};
        double alpha_{0.01}; // learning rate / step size
        std::size_t batch_size_{1}; // not sure how to implement
        double beta1_{0.9};
        double beta2_{0.999};
        double epsilon_{1e-8};
        Eigen::Matrix<double, Eigen::Dynamic, 1> f_x_prev_; // don't initialize to any size yet
        Eigen::Matrix<double, Eigen::Dynamic, 1> times_;

        /**
         * implements the termination conditions for the loop in the optimization algorithm, returning true if the
         * process as converged or run out of iterations and false otherwise
         * @tparam RealRowIndexType the number of columns in the parameter vector
         * @param f_x_prev a running series of past error of all the previous iteration of the algorithm
         * @param x_prev the previous "x_tol_interval_" parameter values
         * @param iterations the iteration number that we are currently on
         * @return true if we have reached any termination criterion, false otherwise
         */
        template <int RealRowIndexType>
        bool reached_termination(Eigen::Matrix<double, Eigen::Dynamic, 1>& f_x_prev, Eigen::Matrix<double, Eigen::Dynamic, RealRowIndexType>& x_prev, int iterations) {
            if (iterations >= max_iterations_) {
                std::cout << "Quitting b/c reached max iterations" << std::endl;
                return true;
            }

            if (iterations >= f_x_tol_interval_) {
                Eigen::Matrix<double, Eigen::Dynamic, 1> last_n_items = f_x_prev(Eigen::lastN(f_x_tol_interval_));
                if ((zaamath::range(last_n_items).cwiseAbs().array() < f_x_tol_).all()) {
                    std::cout << "Quitting b/c Error Converged" << std::endl;
                    std::cout << " - Range: " << zaamath::range(last_n_items).cwiseAbs().array() << " < " << f_x_tol_ << std::endl;
                    return true;
                }
            }

            if (iterations >= x_tol_interval_) {
                if ((zaamath::range(x_prev).cwiseAbs().array() < x_tol_).all()) {
                    std::cout << "Quitting b/c Parameters Converged" << std::endl;
                    std::cout << " - Range: " << zaamath::range(x_prev).cwiseAbs().array() << " < " << x_tol_ << std::endl;
                    return true;
                }
            }

            return false;
        }

    public:

        /**
         * clears the member variables having to do with the performance of the optimization at each iteration so that
         * the object may be used again for another optimization.
         */
        void clear() {
//            f_x_prev_ = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero();
            f_x_prev_.resize(0);
//            times_ = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero();
            times_.resize(0);
        }

        /**
         * implements the vanilla Adam optimization algorithm
         * @tparam RowIndexType the number of parameters (the number of rows of the input parameter vector)
         * @param initial_params the initial parameter set
         * @param obj_func the objective function
         * @param jacobian the jacobian of the objective function w.r.t. the parameters
         */
        template <int RowIndexType>
        void optimize(Parameters<RowIndexType>& initial_params, std::function<double(Parameters<RowIndexType>&)>& obj_func, std::function<Eigen::Matrix<double, RowIndexType, 1>(Parameters<RowIndexType>&, int)>& jacobian) {

            // initialize to zero
            Eigen::Matrix<double, RowIndexType, 1> mt = Eigen::Matrix<double, RowIndexType, 1>::Zero();
            Eigen::Matrix<double, RowIndexType, 1> vt = Eigen::Matrix<double, RowIndexType, 1>::Zero();
            Eigen::Matrix<double, RowIndexType, 1> mthat = Eigen::Matrix<double, RowIndexType, 1>::Zero();
            Eigen::Matrix<double, RowIndexType, 1> vthat = Eigen::Matrix<double, RowIndexType, 1>::Zero();

            // Grab the last 'x_tol_interval' inputs
            Eigen::Matrix<double, Eigen::Dynamic, RowIndexType> x_prev = Eigen::Matrix<double, Eigen::Dynamic, RowIndexType>::Zero(x_tol_interval_, RowIndexType);
            x_prev.row(0) = initial_params.eigen_params();

            // track number of iterations
            int iterations{0};

            // trace table headers
            std::size_t header_widths[4] = {
                std::string(" Iteration ").size()-1,
                std::string("    Time    ").size()-std::string(" [us] ").size(),
                std::string("   Error   ").size()-1,
                std::string(" Min. Error ").size()-1
            };

            // Grab the stopwatch
            StopWatch stop_watch;

            while (!reached_termination(f_x_prev_, x_prev, iterations)) {
                stop_watch.start();
                Eigen::Matrix<double, RowIndexType, 1> grad = jacobian(initial_params, iterations);
                std::size_t batch{1};
                while (batch < batch_size_) {
                    grad += jacobian(initial_params, iterations + batch);
                    ++batch;
                }

                // update mt and vt
                mt = beta1_ * mt + (1 - beta1_) * grad;
                vt = beta2_ * vt + (1 - beta2_) * grad.array().square().matrix();

                // update mthat and vthat
                mthat = mt * 1 / (1 + std::pow(beta1_, iterations + 1));
                vthat = vt * 1 / (1 + std::pow(beta2_, iterations + 1));

                // update theta
                Eigen::Matrix<double, RowIndexType, 1> new_engine_params = initial_params.eigen_params() - (alpha_ * mthat.array() * (vthat.array().sqrt() + epsilon_).inverse()).matrix();
                initial_params.update(new_engine_params);

                stop_watch.stop();
                times_.conservativeResize(times_.rows()+1, Eigen::NoChange);
                times_(times_.rows()-1) = stop_watch.duration() * 1e-3;

                f_x_prev_.conservativeResize(f_x_prev_.rows()+1, Eigen::NoChange);
                f_x_prev_(f_x_prev_.rows()-1) = obj_func(initial_params);

                if (show_trace_) {
                    if (iterations == 0) {
                        std::cout << "| Iteration |    Time    |   Error   | Min. Error |" <<  std::endl;
                        std::cout << "+-----------+------------+-----------+------------+" << std::endl;
                        std::cout << "|";
                        std::cout << std::setw(header_widths[0]) << (iterations+1) << " ";
                        std::cout << "|";
                        double duration = times_(0);
                        std::cout << std::setw(header_widths[1]) << std::setprecision(4) << duration << " [us] ";
                        std::cout << "|";
                        double error = f_x_prev_(0);
                        std::cout << std::setw(header_widths[2]) << std::setprecision(4) << error << " ";
                        std::cout << "|";
                        std::cout << std::setw(header_widths[3]) << std::setprecision(4) << error << " ";
                        std::cout << "|" << std::endl;
                    } else if ((iterations + 1) % show_trace_every_ == 0) {
                        std::cout << "|";
                        std::cout << std::setw(header_widths[0]) << (iterations+1) << " ";
                        std::cout << "|";
                        double duration = times_.middleRows(iterations+1-show_trace_every_+1, show_trace_every_-1).sum();
                        std::cout << std::setw(header_widths[1]) << std::setprecision(4) << duration << " [us] ";
                        std::cout << "|";
                        double error = f_x_prev_(f_x_prev_.rows()-1);
                        std::cout << std::setw(header_widths[2]) << std::setprecision(4) << error << " ";
                        std::cout << "|";
                        double min_error = f_x_prev_.minCoeff();
                        std::cout << std::setw(header_widths[3]) << std::setprecision(4) << min_error << " ";
                        std::cout << "|" << std::endl;
                    }
                }
                ++iterations;
            }
        }

        Eigen::Matrix<double, Eigen::Dynamic, 1> f_x_prev() {
            return f_x_prev_;
        }

        Eigen::Matrix<double, Eigen::Dynamic, 1> times() {
            return times_;
        }
    };

};

#endif //PROMETHEUS_OPTIMIZATION_HPP


/**
 * @file gausslobattoparabolic.cc
 * @brief NPDE exam TEMPLATE CODE FILE
 * @author Oliver Rietmann
 * @date 22.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include "gausslobattoparabolic.h"

#include <lf/assemble/assemble.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseLU>
#include <functional>
#include <memory>
#include <utility>

namespace GaussLobattoParabolic {

/* SAM_LISTING_BEGIN_1 */
lf::assemble::COOMatrix<double> initMbig(
    std::shared_ptr<const lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space) {
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
#if SOLUTION
  // Diffusion coefficient =0, reaction coefficient = 1
  lf::mesh::utils::MeshFunctionConstant alpha(0.0), gamma(1.0);
  lf::uscalfe::ReactionDiffusionElementMatrixProvider entity_matrix_provider(
      fe_space, alpha, gamma);
  // Compute mass matrix for full finite element space
  lf::assemble::COOMatrix<double> M =
      lf::assemble::AssembleMatrixLocally<lf::assemble::COOMatrix<double>,
                                          decltype(entity_matrix_provider)>(
          0, dofh, entity_matrix_provider);
  // Find mesh nodes on the boundary
  const lf::mesh::utils::CodimMeshDataSet<bool> bd_flags =
      lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 2);
  // Predicate for selecting matrix rows induced by test functions associated
  // with nodes on the boundary
  auto pred = [&bd_flags, &dofh](int i, int j) {
    return bd_flags(dofh.Entity(i));
  };
  // Set the corresponding triplets to zero using LehrFEM++ helper function
  M.setZero(pred);
#else
  //====================
  // Your code goes here
  // Replace this dummy assignment for M:
  int N = dofh.NumDofs();
  lf::assemble::COOMatrix<double> M(N, N);
  //====================
#endif

  return M;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
lf::assemble::COOMatrix<double> initAbig(
    std::shared_ptr<const lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space) {
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
#if SOLUTION
  // Diffusion coefficient =1, reaction coefficient = 0
  lf::mesh::utils::MeshFunctionConstant alpha(1.0), gamma(0.0);
  lf::uscalfe::ReactionDiffusionElementMatrixProvider entity_matrix_provider(
      fe_space, alpha, gamma);
  // Compute Galerkin matrix for -Laplacian on full FE space
  lf::assemble::COOMatrix<double> A =
      lf::assemble::AssembleMatrixLocally<lf::assemble::COOMatrix<double>,
                                          decltype(entity_matrix_provider)>(
          0, dofh, entity_matrix_provider);
  // Find mesh nodes on the boundary
  const lf::mesh::utils::CodimMeshDataSet<bool> bd_flags =
      lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 2);
  // Predicate for selecting matrix rows induced by test functions associated
  // with nodes on the boundary
  auto pred = [&bd_flags, &dofh](int i, int j) {
    return bd_flags(dofh.Entity(i));
  };
  // Set the corresponding triplets to zero using LehrFEM++ helper function
  A.setZero(pred);
  // Set "boundary block" to the identity matrix
  for (int i = 0; i < dofh.NumDofs(); ++i) {
    if (bd_flags(dofh.Entity(i))) A.AddToEntry(i, i, 1.0);
  }
#else
  //====================
  // Your code goes here
  // Replace this dummy assignment for A:
  int N = dofh.NumDofs();
  lf::assemble::COOMatrix<double> A(N, N);
  //====================
#endif

  return A;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
RHSProvider::RHSProvider(const lf::assemble::DofHandler &dofh,
                         std::function<double(double)> g)
    : g_(std::move(g)) {
#if SOLUTION
  // Finde nodes on the boundary
  const lf::mesh::utils::CodimMeshDataSet<bool> bd_flags =
      lf::mesh::utils::flagEntitiesOnBoundary(dofh.Mesh(), 2);
  int N = dofh.NumDofs();
  // Initialize the fixed vector, components for degrees of freedom associated
  // with nodes on the boundary are set to 1, all other to 0
  zero_one_ = Eigen::VectorXd(N);
  for (int i = 0; i < N; ++i) {
    zero_one_(i) = bd_flags(dofh.Entity(i)) ? 1.0 : 0.0;
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
}

Eigen::VectorXd RHSProvider::operator()(double t) const {
#if SOLUTION
  // Just rescale the stored vector
  return g_(t) * zero_one_;
#else
  //====================
  // Your code goes here
  // Replace this dummy return value:
  return Eigen::VectorXd(0);
  //====================
#endif
}
/* SAM_LISTING_END_3 */

}  // namespace GaussLobattoParabolic


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
MatrixXf A(2,2), B(3,2);
B << 2, 0,  0, 3, 1, 1;
A << 2, 0, 0, -2;
A = (B * A).cwiseAbs();
cout << A;

}
  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 RVM regression object
    from the dlib C++ Library.

    This example will train on data from the sinc function.

*/

#include <iostream>
#include <vector>

#include <dlib/svm.h>

using namespace std;
using namespace dlib;

// Here is the sinc function we will be trying to learn with rvm regression
double sinc(double x)
{
    if (x == 0)
        return 1;
    return sin(x)/x;
}

int main()
{
    // Here we declare that our samples will be 1 dimensional column vectors.
    typedef matrix<double,1,1> sample_type;

    // Now sample some points from the sinc() function
    sample_type m;
    std::vector<sample_type> samples;
    std::vector<double> labels;
    for (double x = -10; x <= 4; x += 1)
    {
        m(0) = x;
        samples.push_back(m);
        labels.push_back(sinc(x));
    }

    // Now we are making a typedef for the kind of kernel we want to use.  I picked the
    // radial basis kernel because it only has one parameter and generally gives good
    // results without much fiddling.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Here we declare an instance of the rvm_regression_trainer object.  This is the
    // object that we will later use to do the training.
    rvm_regression_trainer<kernel_type> trainer;

    // Here we set the kernel we want to use for training.   The radial_basis_kernel
    // has a parameter called gamma that we need to determine.  As a rule of thumb, a good
    // gamma to try is 1.0/(mean squared distance between your sample points).  So
    // below we are using a similar value.   Note also that using an inappropriately large
    // gamma will cause the RVM training algorithm to run extremely slowly.  What
    // "large" means is relative to how spread out your data is.  So it is important
    // to use a rule like this as a starting point for determining the gamma value
    // if you want to use the RVM.  It is also probably a good idea to normalize your
    // samples as shown in the rvm_ex.cpp example program.
    const double gamma = 2.0/compute_mean_squared_distance(samples);
    cout << "using gamma of " << gamma << endl;
    trainer.set_kernel(kernel_type(gamma));

    // One thing you can do to reduce the RVM training time is to make its
    // stopping epsilon bigger.  However, this might make the outputs less
    // reliable.  But sometimes it works out well.  0.001 is the default.
    trainer.set_epsilon(0.001);

    // now train a function based on our sample points
    decision_function<kernel_type> test = trainer.train(samples, labels);

    // now we output the value of the sinc function for a few test points as well as the
    // value predicted by our regression.
    m(0) = 2.5; cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = 0.1; cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = -4;  cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = 5.0; cout << sinc(m(0)) << "   " << test(m) << endl;

    // The output is as follows:
    //using gamma of 0.05
    //0.239389   0.240989
    //0.998334   0.999538
    //-0.189201   -0.188453
    //-0.191785   -0.226516

    // The first column is the true value of the sinc function and the second
    // column is the output from the rvm estimate.

    // Another thing that is worth knowing is that just about everything in dlib is serializable.
    // So for example, you can save the test object to disk and recall it later like so:
    ofstream fout("saved_function.dat",ios::binary);
    serialize(test,fout);
    fout.close();

    // now lets open that file back up and load the function object it contains
    ifstream fin("saved_function.dat",ios::binary);
    deserialize(test, fin);

}


#include <cstdlib>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef ublas::vector<double> vector;
  typedef ublas::matrix<double, ublas::column_major> matrix;
  typedef typename vector::size_type size_type;

  rand_normal<double>::reset();
  size_type n=128;
  matrix A(n, n);
  for (size_type j=0; j<n; ++j) {
    for (size_type i=0; i<=j; ++i) {
      A(i, j)=rand_normal<double>::get();
      A(j, i)=A(i, j);
    }
  }
  {
    vector lambda(n);
    matrix A_bak(A);
    int info=lapack::syev('V', lapack::upper(A), lambda);
    if (info==0) {
      for (int i=0; i<n; ++i) {
	// res <- A*vr(i) - lambda(i)*vr(i)
	ublas::matrix_column<matrix> v(A, i);
	vector res(v);
	blas::gemv(1., A_bak, v, -lambda(i), res);
	std::cout << "norm of residual (right eigen vector " << i
		  << " ): " << blas::nrm2(res) << '\n';
      }
    } else
      if (info>0)
	std::cout << "unable to compute all eigen values\n";
      else
	std::cout << "illegal arguments\n";
  }
  return EXIT_SUCCESS;
}