#define BOOST_TEST_MODULE "test_matrix"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <mill/math/Matrix.hpp>

#include <random>
constexpr static unsigned int seed = 32479327;
constexpr static std::size_t N = 10000;

BOOST_AUTO_TEST_CASE(construct_matrix_3x3)
{
    {
        mill::Matrix<double, 3, 3> mat;
        BOOST_CHECK_EQUAL((mat(0, 0)), 0);
        BOOST_CHECK_EQUAL((mat(0, 1)), 0);
        BOOST_CHECK_EQUAL((mat(0, 2)), 0);
        BOOST_CHECK_EQUAL((mat(1, 0)), 0);
        BOOST_CHECK_EQUAL((mat(1, 1)), 0);
        BOOST_CHECK_EQUAL((mat(1, 2)), 0);
        BOOST_CHECK_EQUAL((mat(2, 0)), 0);
        BOOST_CHECK_EQUAL((mat(2, 1)), 0);
        BOOST_CHECK_EQUAL((mat(2, 2)), 0);
    }

    {
        mill::Matrix<double, 3, 3> mat(1, 2, 3, 4, 5, 6, 7, 8, 9);
        BOOST_CHECK_EQUAL((mat(0, 0)), 1);
        BOOST_CHECK_EQUAL((mat(0, 1)), 2);
        BOOST_CHECK_EQUAL((mat(0, 2)), 3);
        BOOST_CHECK_EQUAL((mat(1, 0)), 4);
        BOOST_CHECK_EQUAL((mat(1, 1)), 5);
        BOOST_CHECK_EQUAL((mat(1, 2)), 6);
        BOOST_CHECK_EQUAL((mat(2, 0)), 7);
        BOOST_CHECK_EQUAL((mat(2, 1)), 8);
        BOOST_CHECK_EQUAL((mat(2, 2)), 9);
    }
}

BOOST_AUTO_TEST_CASE(add_matrix_3x3)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        mill::Matrix<double, 3, 3> lhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                lhs(i, j) = uni(mt);

        mill::Matrix<double, 3, 3> rhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                rhs(i, j) = uni(mt);

        mill::Matrix<double, 3, 3> mat = lhs + rhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                BOOST_CHECK_EQUAL((mat(i, j)), (lhs(i, j) + rhs(i, j)));
    }
}

BOOST_AUTO_TEST_CASE(sub_matrix_3x3)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        mill::Matrix<double, 3, 3> lhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                lhs(i, j) = uni(mt);

        mill::Matrix<double, 3, 3> rhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                rhs(i, j) = uni(mt);

        mill::Matrix<double, 3, 3> mat = lhs - rhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                BOOST_CHECK_EQUAL((mat(i, j)), (lhs(i, j) - rhs(i, j)));
    }
}

BOOST_AUTO_TEST_CASE(scalar_mul_matrix_3x3)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        mill::Matrix<double, 3, 3> lhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                lhs(i, j) = uni(mt);
        const double scl = uni(mt);

        {
        mill::Matrix<double, 3, 3> mat = lhs * scl;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                BOOST_CHECK_EQUAL((mat(i, j)), (lhs(i, j) * scl));
        }

        {
        mill::Matrix<double, 3, 3> mat = scl * lhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                BOOST_CHECK_EQUAL((mat(i, j)), (scl * lhs(i, j)));
        }

    }
}

BOOST_AUTO_TEST_CASE(scalar_div_matrix_3x3)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        mill::Matrix<double, 3, 3> lhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                lhs(i, j) = uni(mt);
        const double scl = uni(mt);

        mill::Matrix<double, 3, 3> mat = lhs / scl;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<3; ++j)
                BOOST_CHECK_EQUAL((mat(i, j)), (lhs(i, j) / scl));
    }
}

BOOST_AUTO_TEST_CASE(multi_matrix_2x2_2x2)
{
    mill::Matrix<int, 2, 2> lhs;
    lhs(0,0) = 5;
    lhs(0,1) = 6;
    lhs(1,0) = 7;
    lhs(1,1) = 8;
    mill::Matrix<int, 2, 2> rhs;
    rhs(0,0) = 1;
    rhs(0,1) = 2;
    rhs(1,0) = 3;
    rhs(1,1) = 4;
    mill::Matrix<int, 2, 2> mat = lhs * rhs;
    BOOST_CHECK_EQUAL((mat(0,0)), 23);
    BOOST_CHECK_EQUAL((mat(0,1)), 34);
    BOOST_CHECK_EQUAL((mat(1,0)), 31);
    BOOST_CHECK_EQUAL((mat(1,1)), 46);
}

BOOST_AUTO_TEST_CASE(multi_matrix_3x4_4x2)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        mill::Matrix<double, 3, 4> lhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<4; ++j)
                lhs(i, j) = uni(mt);

        mill::Matrix<double, 4, 2> rhs;
        for(std::size_t i=0; i<4; ++i)
            for(std::size_t j=0; j<2; ++j)
                rhs(i, j) = uni(mt);

        mill::Matrix<double, 3, 2> mat = lhs * rhs;
        for(std::size_t i=0; i<3; ++i)
            for(std::size_t j=0; j<2; ++j)
            {
                double sum = 0.;
                for(std::size_t k=0; k<4; ++k)
                    sum += lhs(i, k) * rhs(k, j);
                BOOST_CHECK_EQUAL((mat(i, j)), sum);
            }
    }
}

BOOST_AUTO_TEST_CASE(multi_matrix_dynamic)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        mill::Matrix<double, mill::DYNAMIC, mill::DYNAMIC> lhs({10u, 10u});
        mill::Matrix<double, mill::DYNAMIC, mill::DYNAMIC> rhs({10u, 10u});

        for(std::size_t i=0; i<lhs.len(); ++i)
        {
            lhs[i] = uni(mt);
            rhs[i] = uni(mt);
        }

        mill::Matrix<double, mill::DYNAMIC, mill::DYNAMIC> sum = lhs + rhs;
        BOOST_TEST(sum.row() == 10u);
        BOOST_TEST(sum.col() == 10u);
        for(std::size_t i=0; i<lhs.len(); ++i)
        {
            BOOST_CHECK_EQUAL(sum[i], lhs[i] + rhs[i]);
        }

        mill::Matrix<double, mill::DYNAMIC, mill::DYNAMIC> mul = lhs * rhs;
        BOOST_TEST(mul.row() == 10u);
        BOOST_TEST(mul.col() == 10u);
        for(std::size_t i=0; i<lhs.row(); ++i)
        {
            for(std::size_t j=0; j<rhs.col(); ++j)
            {
                double s=0;
                for(std::size_t k=0; k<lhs.col(); ++k)
                {
                    s += lhs(i, k) * rhs(k, j);
                }
                BOOST_TEST(mul(i, j) == s, boost::test_tools::tolerance(1e-8));
            }
        }
    }
}


#pragma once
#include <Eigen/Dense>
using namespace Eigen;

class lrsgd
{
public:
	lrsgd();
	~lrsgd() {};

	VectorXf sigmoid(VectorXf& a);
	void lr_objective(float& cost, VectorXf& grad, VectorXf& theta);
	void fit(void);
	void generate_data(MatrixXf& X, VectorXi& y);

	int num_iter;       //max number of iterations
	VectorXf theta;     //logistic regression weights
	float lambda;       //regularization parameter
	float cost;         //LR objective
	VectorXf grad;      //gradient of LR objective

	int tau0;           //learning rate parameter
	int kappa;          //learning rate parameter
	VectorXf eta;       //learning rate schedule

	MatrixXf X;         //input data n x d
	VectorXi y;         //input labels n x 1
};


#include "obj.h"
#include "assignment.h"
#include "HungarianAlg.h"

#include <math.h>
#include <Eigen/Dense>

#include <cmath>
#include <ctime>

#include <iostream>
#include <vector>
#include <string>
#include <iostream>

template<typename LeftClass, typename RightClass>
double function_t(LeftClass& lc, RightClass& rc) {
	return std::sqrt((lc.x - rc.x)*(lc.x -	rc.x) + (lc.y - rc.y)*(lc.y -	rc.y));
}

#define RANGE 100.0

int main (int argc, char** argv) {
#ifdef (WIN32 || WIN64)
	srand((unsigned)time(NULL));
#else
	srandom((unsigned)time(NULL));
#endif
	int M = 10;
	int N = 15;
	double dist_thresh = 10.0;
	std::vector<ele::LeftObj> left_obj(M);
	std::vector<ele::RightObj> right_obj(N);

	Eigen::MatrixXd l_value = (Eigen::MatrixXd::Random(M, 2)).array().abs() * 100.0;
	for (int i=0; i<l_value.rows(); i++) {
		left_obj[i].x = l_value(i, 0);
		left_obj[i].x = l_value(i, 1);
	}
	Eigen::MatrixXd r_value = (Eigen::MatrixXd::Random(N, 2)).array().abs() * 100.0;
	for (int i=0; i<r_value.rows(); i++) {
		right_obj[i].x = r_value(i, 0);
		right_obj[i].x = r_value(i, 1);
	}

	std::cout << "create Assingment" << std::endl;
	ele::Assignment<ele::LeftObj, ele::RightObj> assign_engine(&left_obj,
			&right_obj,  function_t<ele::LeftObj, ele::RightObj>, dist_thresh);

	std::cout << "start Solve..." << std::endl;
	assign_engine.Solve();

	std::cout << "Solved and get result" << std::endl;
	std::vector<int> result = assign_engine.Assign();

	for (size_t i=0; i<result.size(); i++) {
		if (result[i] < 0)
			std::cout << "Left: " << i << " --> null" << std::endl;
		else
			std::cout << "Left: " << i << " --> Right: " << result[i] << std::endl;
	}

  return 0;
}


#define BOOST_TEST_MODULE functions
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "exprtest.hpp"

EXPRTEST(func_abs   , "abs   (-1.0)", std::abs   (-1.0))
EXPRTEST(func_acos  , "acos  ( 1.0)", std::acos  ( 1.0))
EXPRTEST(func_acosh , "acosh ( 1.0)", std::acosh ( 1.0))
EXPRTEST(func_asin  , "asin  ( 1.0)", std::asin  ( 1.0))
EXPRTEST(func_asinh , "asinh ( 1.0)", std::asinh ( 1.0))
EXPRTEST(func_atan  , "atan  ( 1.0)", std::atan  ( 1.0))
EXPRTEST(func_atanh , "atanh ( 0.0)", std::atanh ( 0.0))
EXPRTEST(func_cbrt  , "cbrt  ( 1.0)", std::cbrt  ( 1.0))
EXPRTEST(func_ceil  , "ceil  ( 0.5)", std::ceil  ( 0.5))
EXPRTEST(func_cos   , "cos   ( 1.0)", std::cos   ( 1.0))
EXPRTEST(func_cosh  , "cosh  ( 1.0)", std::cosh  ( 1.0))
EXPRTEST(func_erf   , "erf   ( 1.0)", std::erf   ( 1.0))
EXPRTEST(func_erfc  , "erfc  ( 1.0)", std::erfc  ( 1.0))
EXPRTEST(func_exp   , "exp   ( 1.0)", std::exp   ( 1.0))
EXPRTEST(func_exp2  , "exp2  ( 1.0)", std::exp2  ( 1.0))
EXPRTEST(func_floor , "floor ( 0.5)", std::floor ( 0.5))
EXPRTEST(func_log   , "log   ( 1.0)", std::log   ( 1.0))
EXPRTEST(func_log2  , "log2  ( 1.0)", std::log2  ( 1.0))
EXPRTEST(func_log10 , "log10 ( 1.0)", std::log10 ( 1.0))
EXPRTEST(func_round , "round ( 0.5)", std::round ( 0.5))
EXPRTEST(func_sgn   , "sgn   (-1.0)",             -1.0 )
EXPRTEST(func_sin   , "sin   ( 1.0)", std::sin   ( 1.0))
EXPRTEST(func_sinh  , "sinh  ( 1.0)", std::sinh  ( 1.0))
EXPRTEST(func_sqrt  , "sqrt  ( 1.0)", std::sqrt  ( 1.0))
EXPRTEST(func_tan   , "tan   ( 1.0)", std::tan   ( 1.0))
EXPRTEST(func_tanh  , "tanh  ( 1.0)", std::tanh  ( 1.0))
EXPRTEST(func_tgamma, "tgamma( 4.0)", std::tgamma( 4.0))

EXPRTEST(func_atan2, "atan2(2.0, 3.0)", std::atan2(2.0,3.0))
EXPRTEST(func_max,   "max  (2.0, 3.0)", std::fmax(2.0,3.0))
EXPRTEST(func_min,   "min  (2.0, 3.0)", std::fmin(2.0,3.0))
EXPRTEST(func_pow,   "pow  (2.0, 3.0)", std::pow(2.0,3.0))


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

#include <pch.hpp>

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/tools/test.hpp>
#include "functor.hpp"
#include <boost/array.hpp>

#include "handle_test_result.hpp"

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the function binomial_coefficient<T>.
// The accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these function.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double";
   }
   else
   {
      largest_type = "long double";
   }
#else
   largest_type = "(long\\s+)?double";
#endif
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      ".*large.*",                   // test data group
      ".*", 100, 20);                 // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      ".*large.*",                   // test data group
      ".*", 250, 100);               // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      ".*",                          // test data group
      ".*", 150, 30);                 // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                          // test type(s)
      ".*",                          // test data group
      ".*", 2, 1);                   // test function

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class T>
T binomial_wrapper(T n, T k)
{
   return boost::math::binomial_coefficient<T>(
      boost::math::itrunc(n),
      boost::math::itrunc(k));
}

template <class T>
void test_binomial(T, const char* type_name)
{
   using namespace std;

   typedef T (*func_t)(T, T);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   func_t f = &binomial_wrapper<T>;
#else
   func_t f = &binomial_wrapper;
#endif

#include "binomial_data.ipp"

   boost::math::tools::test_result<T> result = boost::math::tools::test(
      binomial_data,
      bind_func(f, 0, 1),
      extract_result(2));

   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n"
      "Test results for small arguments and type " << type_name << std::endl << std::endl;
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";
   handle_test_result(result, binomial_data[result.worst()], result.worst(), type_name, "binomial_coefficient", "Binomials: small arguments");
   std::cout << std::endl;

#include "binomial_large_data.ipp"

   result = boost::math::tools::test(
      binomial_large_data,
      bind_func(f, 0, 1),
      extract_result(2));

   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n"
      "Test results for large arguments and type " << type_name << std::endl << std::endl;
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";
   handle_test_result(result, binomial_large_data[result.worst()], result.worst(), type_name, "binomial_coefficient", "Binomials: large arguments");
   std::cout << std::endl;
}

template <class T>
void test_spots(T, const char* name)
{
   T tolerance = boost::math::tools::epsilon<T>() * 50 * 100;  // 50 eps as a percentage
   if(!std::numeric_limits<T>::is_specialized)
      tolerance *= 10;  // beta function not so accurate without lanczos support

   std::cout << "Testing spot checks for type " << name << " with tolerance " << tolerance << "%\n";

   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 0), static_cast<T>(1));
   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 1), static_cast<T>(20));
   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 2), static_cast<T>(190));
   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 3), static_cast<T>(1140));
   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 20), static_cast<T>(1));
   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 19), static_cast<T>(20));
   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 18), static_cast<T>(190));
   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 17), static_cast<T>(1140));
   BOOST_CHECK_EQUAL(boost::math::binomial_coefficient<T>(20, 10), static_cast<T>(184756L));

   BOOST_CHECK_CLOSE(boost::math::binomial_coefficient<T>(100, 5), static_cast<T>(7.528752e7L), tolerance);
   BOOST_CHECK_CLOSE(boost::math::binomial_coefficient<T>(100, 81), static_cast<T>(1.323415729392122674e20L), tolerance);

   BOOST_CHECK_CLOSE(boost::math::binomial_coefficient<T>(300, 3), static_cast<T>(4.45510e6L), tolerance);
   BOOST_CHECK_CLOSE(boost::math::binomial_coefficient<T>(300, 7), static_cast<T>(4.043855956140000e13L), tolerance);
   BOOST_CHECK_CLOSE(boost::math::binomial_coefficient<T>(300, 290), static_cast<T>(1.3983202332417017700000000e18L), tolerance);
   BOOST_CHECK_CLOSE(boost::math::binomial_coefficient<T>(300, 275), static_cast<T>(1.953265141442868389822364184842211512000000e36L), tolerance);
}

int test_main(int, char* [])
{
   expected_results();
   BOOST_MATH_CONTROL_FP;

   test_spots(1.0F, "float");
   test_spots(1.0, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(1.0L, "long double");
   test_spots(boost::math::concepts::real_concept(), "real_concept");
#endif

   test_binomial(1.0F, "float");
   test_binomial(1.0, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_binomial(1.0L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
   test_binomial(boost::math::concepts::real_concept(), "real_concept");
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif
   return 0;
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

int main(int argc, char **argv)
{
  // exploit namespaces to shorten code
  using namespace boost::numeric::ublas;
  using std::cout;
  using std::endl;

  // declare three 3x3 matrices of complex<long double> elements
  matrix<std::complex<long double> > m(11, 11);

  // iterate over 3x3 matrix entries
  // r : row index
  // c : column index
  for (unsigned r = 0; r < m.size1(); r++) {
    for (unsigned c = 0; c < m.size2(); c++) {
      // enumerated matrix entries
        int a = r -5;
        int b = c - 5;
      m(r,c) =  a + b * 1i;

    }
  }

  // print to screen as demonstration
  cout << "m:" << endl;
  cout << m << endl;

    /*
  cout << endl << "n:" << endl;
  cout << n << endl;
  cout << endl << "o:" << endl;
  cout << o << endl;
  cout << endl << "m + n:" << endl;
  cout << m + n << endl;
  cout << endl << "m * n:" << endl;
  cout << prod(m, n) << endl;*/
}


#include <Eigen/Eigen>
#include <gtest/gtest.h>
#include <torch/Torch.h>

using namespace torch;

const float epsilon = 1E-4;

Eigen::Matrix4f GetRotationMatrix(const Vector& rotation)
{
  const Eigen::Quaternionf quaternion =
      Eigen::AngleAxisf(rotation.x, Eigen::Vector3f::UnitX()) *
      Eigen::AngleAxisf(rotation.y, Eigen::Vector3f::UnitY()) *
      Eigen::AngleAxisf(rotation.z, Eigen::Vector3f::UnitZ());

  Eigen::Matrix4f result(Eigen::Matrix4f::Identity());
  result.block<3, 3>(0, 0) = Eigen::Matrix3f(quaternion);
  return result;
}

const Eigen::Matrix4f GetRotationMatrix(const optix::Matrix4x4& rotation)
{
  typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> RowMatrix;
  return Eigen::Map<const RowMatrix>(rotation.getData());
}

TEST(Transform, Translation)
{
  Transform transform;
  Vector expected;
  Vector found;

  expected = Vector(0, 0, 0);
  found = transform.GetTranslation();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);

  expected = Vector(1, 2, 3);
  transform.SetTranslation(expected);
  found = transform.GetTranslation();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);

  expected = Vector(0, -1, 2);
  transform.SetTranslation(expected);
  found = transform.GetTranslation();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);

  expected = Vector(0, 0, 0);
  transform.SetTranslation(expected);
  found = transform.GetTranslation();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);
}

TEST(Transform, Rotation)
{
  Transform transform;
  Eigen::Matrix4f expected;
  Eigen::Matrix4f found;
  Vector rotation;

  rotation = Vector(0, 0, 0);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }

  rotation = Vector(0.5, 0, 0);
  transform.SetRotation(rotation);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }

  rotation = Vector(0, 0.5, 0);
  transform.SetRotation(rotation);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }

  rotation = Vector(0, 0, 0.5);
  transform.SetRotation(rotation);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }

  rotation = Vector(0.1, 0.2, 0.3);
  transform.SetRotation(rotation);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }

  rotation = Vector(1, 2, 3);
  transform.SetRotation(rotation);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }

  rotation = Vector(-1, 2, -3);
  transform.SetRotation(rotation);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }

  rotation = Vector(0.1, 0.2, 0.3);
  transform.SetScale(1, 2, 3);
  transform.SetRotation(rotation);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }

  rotation = Vector(-1, 2, -3);
  transform.SetScale(0.1, 0.2, 0.3);
  transform.SetRotation(rotation);
  expected = GetRotationMatrix(rotation);
  found = GetRotationMatrix(transform.GetRotationMatrix());

  for (unsigned int i = 0; i < 16; ++i)
  {
    ASSERT_NEAR(expected.data()[i], found.data()[i], epsilon);
  }
}

TEST(Transform, Scale)
{
  Transform transform;
  Vector expected;
  Vector found;

  expected = Vector(1, 1, 1);
  found = transform.GetScale();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);

  expected = Vector(1, 2, 3);
  transform.SetScale(expected);
  found = transform.GetScale();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);

  expected = Vector(2, 2, 2);
  transform.SetScale(expected);
  found = transform.GetScale();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);

  expected = Vector(0.1, 0.2, 0.3);
  transform.SetScale(expected);
  found = transform.GetScale();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);

  transform.SetRotation(0.1, 0.2, 0.3);
  expected = Vector(0.1, 0.2, 0.3);
  transform.SetScale(expected);
  found = transform.GetScale();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);

  transform.SetRotation(0.3, 0.2, 0.1);
  expected = Vector(1, 2, 3);
  transform.SetScale(expected);
  found = transform.GetScale();
  ASSERT_NEAR(expected.x, found.x, epsilon);
  ASSERT_NEAR(expected.y, found.y, epsilon);
  ASSERT_NEAR(expected.z, found.z, epsilon);
}


#include "stdafx.h"
#include "Calibration.h"
#include "Configuration.h"
#include "IPCClient.h"

#include <string>
#include <vector>
#include <iostream>

#include <Eigen/Dense>

static IPCClient Driver;
CalibrationContext CalCtx;
CalibrationState LastState = CalibrationState::None;
Eigen::Vector3d ReferenceTranslation;
Eigen::Vector3d ReferenceRotation;

void InitCalibrator()
{
	Driver.Connect();
}

struct Pose
{
	Eigen::Matrix3d rot;
	Eigen::Vector3d trans;

	Pose() { }
	Pose(vr::HmdMatrix34_t hmdMatrix)
	{
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				rot(i,j) = hmdMatrix.m[i][j];
			}
		}
		trans = Eigen::Vector3d(hmdMatrix.m[0][3], hmdMatrix.m[1][3], hmdMatrix.m[2][3]);
	}
	Pose(double x, double y, double z) : trans(Eigen::Vector3d(x,y,z)) { }
};

Pose ReferencePose;

struct Sample
{
	Pose ref, target;
	bool valid;
	Sample() : valid(false) { }
	Sample(Pose ref, Pose target) : valid(true), ref(ref), target(target) { }
};

struct DSample
{
	bool valid;
	Eigen::Vector3d ref, target;
};

bool StartsWith(const std::string &str, const std::string &prefix)
{
	if (str.length() < prefix.length())
		return false;

	return str.compare(0, prefix.length(), prefix) == 0;
}

bool EndsWith(const std::string &str, const std::string &suffix)
{
	if (str.length() < suffix.length())
		return false;

	return str.compare(str.length() - suffix.length(), suffix.length(), suffix) == 0;
}

Eigen::Vector3d AxisFromRotationMatrix3(Eigen::Matrix3d rot)
{
	return Eigen::Vector3d(rot(2,1) - rot(1,2), rot(0,2) - rot(2,0), rot(1,0) - rot(0,1));
}

double AngleFromRotationMatrix3(Eigen::Matrix3d rot)
{
	return acos((rot(0,0) + rot(1,1) + rot(2,2) - 1.0) / 2.0);
}

DSample DeltaRotationSamples(Sample s1, Sample s2)
{
	// Difference in rotation between samples.
	auto dref = s1.ref.rot * s2.ref.rot.transpose();
	auto dtarget = s1.target.rot * s2.target.rot.transpose();

	// When stuck together, the two tracked objects rotate as a pair,
	// therefore their axes of rotation must be equal between any given pair of samples.
	DSample ds;
	ds.ref = AxisFromRotationMatrix3(dref);
	ds.target = AxisFromRotationMatrix3(dtarget);

	// Reject samples that were too close to each other.
	auto refA = AngleFromRotationMatrix3(dref);
	auto targetA = AngleFromRotationMatrix3(dtarget);
	ds.valid = refA > 0.4 && targetA > 0.4 && ds.ref.norm() > 0.01 && ds.target.norm() > 0.01;

	ds.ref.normalize();
	ds.target.normalize();
	return ds;
}

Eigen::Vector3d CalibrateRotation(const std::vector<Sample> &samples)
{
	std::vector<DSample> deltas;

	for (size_t i = 0; i < samples.size(); i++)
	{
		for (size_t j = 0; j < i; j++)
		{
			auto delta = DeltaRotationSamples(samples[i], samples[j]);
			if (delta.valid)
				deltas.push_back(delta);
		}
	}
	char buf[256];
	snprintf(buf, sizeof buf, "Got %zd samples with %zd delta samples\n", samples.size(), deltas.size());
	CalCtx.Log(buf);

	// Kabsch algorithm

	Eigen::MatrixXd refPoints(deltas.size(), 3), targetPoints(deltas.size(), 3);
	Eigen::Vector3d refCentroid(0,0,0), targetCentroid(0,0,0);

	for (size_t i = 0; i < deltas.size(); i++)
	{
		refPoints.row(i) = deltas[i].ref;
		refCentroid += deltas[i].ref;

		targetPoints.row(i) = deltas[i].target;
		targetCentroid += deltas[i].target;
	}

	refCentroid /= (double) deltas.size();
	targetCentroid /= (double) deltas.size();

	for (size_t i = 0; i < deltas.size(); i++)
	{
		refPoints.row(i) -= refCentroid;
		targetPoints.row(i) -= targetCentroid;
	}

	auto crossCV = refPoints.transpose() * targetPoints;

	Eigen::BDCSVD<Eigen::MatrixXd> bdcsvd;
	auto svd = bdcsvd.compute(crossCV, Eigen::ComputeThinU | Eigen::ComputeThinV);

	Eigen::Matrix3d i = Eigen::Matrix3d::Identity();
	if ((svd.matrixU() * svd.matrixV().transpose()).determinant() < 0)
	{
		i(2,2) = -1;
	}

	Eigen::Matrix3d rot = svd.matrixV() * i * svd.matrixU().transpose();
	rot.transposeInPlace();

	Eigen::Vector3d euler = rot.eulerAngles(2, 1, 0) * 180.0 / EIGEN_PI;

	snprintf(buf, sizeof buf, "Calibrated rotation: yaw=%.2f pitch=%.2f roll=%.2f\n", euler[1], euler[2], euler[0]);
	CalCtx.Log(buf);
	return euler;
}

Eigen::Vector3d CalibrateTranslation(const std::vector<Sample> &samples)
{
	std::vector<std::pair<Eigen::Vector3d, Eigen::Matrix3d>> deltas;

	for (size_t i = 0; i < samples.size(); i++)
	{
		for (size_t j = 0; j < i; j++)
		{
			auto QAi = samples[i].ref.rot.transpose();
			auto QAj = samples[j].ref.rot.transpose();
			auto dQA = QAj - QAi;
			auto CA = QAj * (samples[j].ref.trans - samples[j].target.trans) - QAi * (samples[i].ref.trans - samples[i].target.trans);
			deltas.push_back(std::make_pair(CA, dQA));

			auto QBi = samples[i].target.rot.transpose();
			auto QBj = samples[j].target.rot.transpose();
			auto dQB = QBj - QBi;
			auto CB = QBj * (samples[j].ref.trans - samples[j].target.trans) - QBi * (samples[i].ref.trans - samples[i].target.trans);
			deltas.push_back(std::make_pair(CB, dQB));
		}
	}

	Eigen::VectorXd constants(deltas.size() * 3);
	Eigen::MatrixXd coefficients(deltas.size() * 3, 3);

	for (size_t i = 0; i < deltas.size(); i++)
	{
		for (int axis = 0; axis < 3; axis++)
		{
			constants(i * 3 + axis) = deltas[i].first(axis);
			coefficients.row(i * 3 + axis) = deltas[i].second.row(axis);
		}
	}

	Eigen::Vector3d trans = coefficients.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(constants);
	auto transcm = trans * 100.0;

	char buf[256];
	snprintf(buf, sizeof buf, "Calibrated translation x=%.2f y=%.2f z=%.2f\n", transcm[0], transcm[1], transcm[2]);
	CalCtx.Log(buf);
	return transcm;
}

Sample CollectSample(const CalibrationContext &ctx)
{
	vr::TrackedDevicePose_t reference, target;
	reference.bPoseIsValid = false;
	target.bPoseIsValid = false;

	reference = ctx.devicePoses[ctx.referenceID];
	target = ctx.devicePoses[ctx.targetID];

	bool ok = true;
	if (!reference.bPoseIsValid)
	{
		CalCtx.Log("Reference device is not tracking\n"); ok = false;
	}
	if (!target.bPoseIsValid)
	{
		CalCtx.Log("Target device is not tracking\n"); ok = false;
	}
	if (!ok)
	{
		CalCtx.Log("Aborting calibration!\n");
		CalCtx.state = CalibrationState::None;
		return Sample();
	}

	return Sample(
		Pose(reference.mDeviceToAbsoluteTracking),
		Pose(target.mDeviceToAbsoluteTracking)
	);
}

vr::HmdQuaternion_t VRRotationQuat(Eigen::Vector3d eulerdeg)
{
	auto euler = eulerdeg * EIGEN_PI / 180.0;

	Eigen::Quaterniond rotQuat =
		Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitZ()) *
		Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY()) *
		Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitX());

	vr::HmdQuaternion_t vrRotQuat;
	vrRotQuat.x = rotQuat.coeffs()[0];
	vrRotQuat.y = rotQuat.coeffs()[1];
	vrRotQuat.z = rotQuat.coeffs()[2];
	vrRotQuat.w = rotQuat.coeffs()[3];
	return vrRotQuat;
}

vr::HmdVector3d_t VRTranslationVec(Eigen::Vector3d transcm)
{
	auto trans = transcm * 0.01;
	vr::HmdVector3d_t vrTrans;
	vrTrans.v[0] = trans[0];
	vrTrans.v[1] = trans[1];
	vrTrans.v[2] = trans[2];
	return vrTrans;
}

void ResetAndDisableOffsets(uint32_t id)
{
	vr::HmdVector3d_t zeroV;
	zeroV.v[0] = zeroV.v[1] = zeroV.v[2] = 0;

	vr::HmdQuaternion_t zeroQ;
	zeroQ.x = 0; zeroQ.y = 0; zeroQ.z = 0; zeroQ.w = 1;

	protocol::Request req(protocol::RequestSetDeviceTransform);
	req.setDeviceTransform = { id, false, zeroV, zeroQ };
	Driver.SendBlocking(req);
}

static_assert(vr::k_unTrackedDeviceIndex_Hmd == 0, "HMD index expected to be 0");

void ScanAndApplyProfile(CalibrationContext &ctx)
{
	char buffer[vr::k_unMaxPropertyStringSize];
	ctx.enabled = ctx.validProfile;

	for (uint32_t id = 0; id < vr::k_unMaxTrackedDeviceCount; ++id)
	{
		auto deviceClass = vr::VRSystem()->GetTrackedDeviceClass(id);
		if (deviceClass == vr::TrackedDeviceClass_Invalid)
			continue;

		/*if (deviceClass == vr::TrackedDeviceClass_HMD) // for debugging unexpected universe switches
		{
			vr::ETrackedPropertyError err = vr::TrackedProp_Success;
			auto universeId = vr::VRSystem()->GetUint64TrackedDeviceProperty(id, vr::Prop_CurrentUniverseId_Uint64, &err);
			printf("uid %d err %d\n", universeId, err);
			ResetAndDisableOffsets(id);
			continue;
		}*/

		if (!ctx.enabled)
		{
			ResetAndDisableOffsets(id);
			continue;
		}

		vr::ETrackedPropertyError err = vr::TrackedProp_Success;
		vr::VRSystem()->GetStringTrackedDeviceProperty(id, vr::Prop_TrackingSystemName_String, buffer, vr::k_unMaxPropertyStringSize, &err);

		if (err != vr::TrackedProp_Success)
		{
			ResetAndDisableOffsets(id);
			continue;
		}

		std::string trackingSystem(buffer);

		if (id == vr::k_unTrackedDeviceIndex_Hmd)
		{
			//auto p = ctx.devicePoses[id].mDeviceToAbsoluteTracking.m;
			//printf("HMD %d: %f %f %f\n", id, p[0][3], p[1][3], p[2][3]);

			if (trackingSystem != ctx.referenceTrackingSystem)
			{
				// Currently using an HMD with a different tracking system than the calibration.
				ctx.enabled = false;
			}

			ResetAndDisableOffsets(id);
			continue;
		}

		if (trackingSystem != ctx.targetTrackingSystem)
		{
			ResetAndDisableOffsets(id);
			continue;
		}

		protocol::Request req(protocol::RequestSetDeviceTransform);
		req.setDeviceTransform = {
			id,
			true,
			VRTranslationVec(ctx.calibratedTranslation),
			VRRotationQuat(ctx.calibratedRotation)
		};
		Driver.SendBlocking(req);
	}

	if (ctx.enabled && ctx.chaperone.valid && ctx.chaperone.autoApply)
	{
		uint32_t quadCount = 0;
		vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(nullptr, &quadCount);

		// Heuristic: when SteamVR resets to a blank-ish chaperone, it uses empty geometry,
		// but manual adjustments (e.g. via a play space mover) will not touch geometry.
		if (quadCount != ctx.chaperone.geometry.size())
		{
			ApplyChaperoneBounds();
		}
	}
}

void StartCalibration()
{
	CalCtx.state = CalibrationState::Begin;
	CalCtx.wantedUpdateInterval = 0.0;
	CalCtx.messages.clear();
}

void SetReferenceOffset() {
	auto &ctx = CalCtx;
	Pose pose(ctx.devicePoses[ctx.referenceID].mDeviceToAbsoluteTracking);
	ReferencePose = pose;
	ReferenceTranslation = ctx.calibratedTranslation;
	ReferenceRotation = ctx.calibratedRotation;
}

void CalibrationTick(double time)
{
	if (!vr::VRSystem())
		return;

	auto &ctx = CalCtx;
	if ((time - ctx.timeLastTick) < 0.05)
		return;

	ctx.timeLastTick = time;
	vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0.0f, ctx.devicePoses, vr::k_unMaxTrackedDeviceCount);

	if (ctx.state == CalibrationState::None)
	{
		ctx.wantedUpdateInterval = 1.0;

		if ((time - ctx.timeLastScan) >= 1.0)
		{
			ScanAndApplyProfile(ctx);
			ctx.timeLastScan = time;
		}
		return;
	}

	if (ctx.state == CalibrationState::Editing)
	{
		ctx.wantedUpdateInterval = 0.1;

		if ((time - ctx.timeLastScan) >= 0.1)
		{
			ScanAndApplyProfile(ctx);
			ctx.timeLastScan = time;
		}
		return;
	}

	if (ctx.state == CalibrationState::Referencing)
	{
		Pose pose(ctx.devicePoses[ctx.referenceID].mDeviceToAbsoluteTracking);
		Eigen::Vector3d deltaTrans = pose.trans - ReferencePose.trans;
		ctx.calibratedTranslation = (ReferenceTranslation + (deltaTrans * 100));

		// Attempt # 1, getting teh euler delta and adding it to the original reference rotation - does not work.
		//auto rotation = pose.rot.eulerAngles(2, 1, 0) * 180.0 / EIGEN_PI;
		/*ctx.calibratedRotation[0] = ReferenceRotation(0) + rotation(0);
		ctx.calibratedRotation[1] = ReferenceRotation(1) + rotation(1);
		ctx.calibratedRotation[2] = ReferenceRotation(2) + rotation(2);*/
		//ctx.calibratedRotation[0] = rotation(0);
		//ctx.calibratedRotation[1] = rotation(1);
		//ctx.calibratedRotation[2] = rotation(2);

		// Attempt #2, convert it all to quaternions ?? didnt get far with this one.
		/*Eigen::Quaterniond currentQuat =
			Eigen::AngleAxisd(ctx.calibratedRotation(0), Eigen::Vector3d::UnitZ()) *
			Eigen::AngleAxisd(ctx.calibratedRotation(1), Eigen::Vector3d::UnitY()) *
			Eigen::AngleAxisd(ctx.calibratedRotation(2), Eigen::Vector3d::UnitX());
		Eigen::Matrix3d deltaRot = pose.rot - ReferencePose.rot;
		Eigen::Quaternionf delta(deltaRot);
		vr::HmdQuaternion_t deltaQuat;
		deltaQuat.x = delta.coeffs()[0];
		deltaQuat.y = delta.coeffs()[1];
		deltaQuat.z = delta.coeffs()[2];
		deltaQuat.w = delta.coeffs()[3];*/
		//currentQuat.normalize();
		// Eigen::Matrix3d updatedRot = currentQuat.toRotationMatrix() + deltaRot;

		ctx.wantedUpdateInterval = 0.025;

		if ((time - ctx.timeLastScan) >= 0.025)
		{
			ScanAndApplyProfile(ctx);
			ctx.timeLastScan = time;
		}
		return;
	}
	LastState = ctx.state;

	if (ctx.state == CalibrationState::Begin)
	{
		bool ok = true;

		char referenceSerial[256], targetSerial[256];
		vr::VRSystem()->GetStringTrackedDeviceProperty(ctx.referenceID, vr::Prop_SerialNumber_String, referenceSerial, 256);
		vr::VRSystem()->GetStringTrackedDeviceProperty(ctx.targetID, vr::Prop_SerialNumber_String, targetSerial, 256);

		char buf[256];
		snprintf(buf, sizeof buf, "Reference device ID: %d, serial: %s\n", ctx.referenceID, referenceSerial);
		CalCtx.Log(buf);
		snprintf(buf, sizeof buf, "Target device ID: %d, serial %s\n", ctx.targetID, targetSerial);
		CalCtx.Log(buf);

		if (ctx.referenceID == -1)
		{
			CalCtx.Log("Missing reference device\n"); ok = false;
		}
		else if (!ctx.devicePoses[ctx.referenceID].bPoseIsValid)
		{
			CalCtx.Log("Reference device is not tracking\n"); ok = false;
		}

		if (ctx.targetID == -1)
		{
			CalCtx.Log("Missing target device\n"); ok = false;
		}
		else if (!ctx.devicePoses[ctx.targetID].bPoseIsValid)
		{
			CalCtx.Log("Target device is not tracking\n"); ok = false;
		}

		if (!ok)
		{
			ctx.state = CalibrationState::None;
			CalCtx.Log("Aborting calibration!\n");
			return;
		}

		ResetAndDisableOffsets(ctx.targetID);
		ctx.state = CalibrationState::Rotation;
		ctx.wantedUpdateInterval = 0.0;

		CalCtx.Log("Starting calibration...\n");
		return;
	}

	auto sample = CollectSample(ctx);
	if (!sample.valid)
	{
		return;
	}

	static std::vector<Sample> samples;
	samples.push_back(sample);

	CalCtx.Progress(samples.size(), CalCtx.SampleCount());

	if (samples.size() == CalCtx.SampleCount())
	{
		CalCtx.Log("\n");
		if (ctx.state == CalibrationState::Rotation)
		{
			ctx.calibratedRotation = CalibrateRotation(samples);

			auto vrRotQuat = VRRotationQuat(ctx.calibratedRotation);

			protocol::Request req(protocol::RequestSetDeviceTransform);
			req.setDeviceTransform = { ctx.targetID, true, vrRotQuat };
			Driver.SendBlocking(req);

			ctx.state = CalibrationState::Translation;
		}
		else if (ctx.state == CalibrationState::Translation)
		{
			ctx.calibratedTranslation = CalibrateTranslation(samples);

			auto vrTrans = VRTranslationVec(ctx.calibratedTranslation);

			protocol::Request req(protocol::RequestSetDeviceTransform);
			req.setDeviceTransform = { ctx.targetID, true, vrTrans };
			Driver.SendBlocking(req);

			ctx.validProfile = true;
			SaveProfile(ctx);
			CalCtx.Log("Finished calibration, profile saved\n");

			ctx.state = CalibrationState::None;
		}

		samples.clear();
	}
}

void LoadChaperoneBounds()
{
	vr::VRChaperoneSetup()->RevertWorkingCopy();

	uint32_t quadCount = 0;
	vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(nullptr, &quadCount);

	CalCtx.chaperone.geometry.resize(quadCount);
	vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(&CalCtx.chaperone.geometry[0], &quadCount);
	vr::VRChaperoneSetup()->GetWorkingStandingZeroPoseToRawTrackingPose(&CalCtx.chaperone.standingCenter);
	vr::VRChaperoneSetup()->GetWorkingPlayAreaSize(&CalCtx.chaperone.playSpaceSize.v[0], &CalCtx.chaperone.playSpaceSize.v[1]);
	CalCtx.chaperone.valid = true;
}

void ApplyChaperoneBounds()
{
	vr::VRChaperoneSetup()->RevertWorkingCopy();
	vr::VRChaperoneSetup()->SetWorkingCollisionBoundsInfo(&CalCtx.chaperone.geometry[0], CalCtx.chaperone.geometry.size());
	vr::VRChaperoneSetup()->SetWorkingStandingZeroPoseToRawTrackingPose(&CalCtx.chaperone.standingCenter);
	vr::VRChaperoneSetup()->SetWorkingPlayAreaSize(CalCtx.chaperone.playSpaceSize.v[0], CalCtx.chaperone.playSpaceSize.v[1]);
	vr::VRChaperoneSetup()->CommitWorkingCopy(vr::EChaperoneConfigFile_Live);
}


#pragma once
#include <functional>
#include <random>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

namespace filter_bay
{
/*!
Simplifies drawing from a multivariant normal distribution.
*/
class NormalSampler
{
  // Drawing from multivariate normal distribution:
  // https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Drawing_values_from_the_distribution
  // Sample by projecting standard normal distributed values with L, where
  // L L^T = covarianc
public:
  // typedef Eigen::Matrix<double, dim, dim> MatrixDim;
  // typedef Eigen::Matrix<double, dim, 1> VectorDim;

  /*!
  Create the sampler using a random_device to create the seed.
  */
  NormalSampler()
  {
    std::random_device random_device;
    uniform_generator = std::mt19937(random_device());
  }

  /*!
  Create the sampler via a seed. For testing it should always be the same seed.
  In real world use the random_device class to generate the seed.
  */
  NormalSampler(unsigned int seed) : uniform_generator(seed) {}

  /*!
  Samples a random vector from a normal distribution using a robust
  decomposition algorithm.
  \param mean the mean of the distribution
  \param covariance the covariance of the distribution
  */
  template <typename scalar, int dim>
  auto sample_robust(const Eigen::Matrix<scalar, dim, 1> &mean,
                     const Eigen::Matrix<scalar, dim, dim> &covariance)
      -> Eigen::Matrix<scalar, dim, 1>
  {
    // Use LDLT decomposition which works with semi definite covariances:
    auto ldlt = covariance.ldlt();
    // L = P^T L sqrt(D), where D is a diagonal matrix.
    // https://stats.stackexchange.com/questions/48749/how-to-sample-from-a-multivariate-normal-given-the-pt-ldlt-p-decomposition-o
    auto P_T = ldlt.transpositionsP().transpose();
    auto L = ldlt.matrixL().toDenseMatrix();
    // Root for diagonal matrix can be calculated element wise
    auto sqrt_D = ldlt.vectorD().cwiseSqrt().asDiagonal();
    auto decomposed = P_T * L * sqrt_D;
    return sample<scalar, dim>(mean, decomposed);
  }

  /*!
  Samples a random vector from a normal distribution using the Cholesky
  decomposition. It might fail, if the covariance is not positive definite.
  \param mean the mean of the distribution
  \param covariance the covariance of the distribution
  */
  template <typename scalar, int dim>
  auto sample_cholesky(const Eigen::Matrix<scalar, dim, 1> &mean,
                       const Eigen::Matrix<scalar, dim, dim> &covariance)
      -> Eigen::Matrix<scalar, dim, 1>
  {
    auto decomposed = covariance.llt().matrixL();
    return sample<scalar, dim>(mean, decomposed);
  }

  /*!
  Draws random values from a normal distribution parametrized by the mean and
  standard deviation (NOT variance as in the covariance matrix version).
  */
  template <typename scalar>
  scalar draw_normal(scalar mean, scalar standard_deviation)
  {
    std::normal_distribution<scalar> normal_dist;
    return normal_dist(uniform_generator) * standard_deviation + mean;
  }

private:
  // Algorithm that provides uniform distributed pseudo-random numbers.
  // Real randomness is imposed via a random seed.
  std::mt19937 uniform_generator;

  /*!
  Samples with the given mean and decomposed covariance matrix.
  */
  template <typename scalar, int dim>
  auto sample(const Eigen::Matrix<scalar, dim, 1> &mean,
              const Eigen::Matrix<scalar, dim, dim> &decomposed)
      -> Eigen::Matrix<scalar, dim, 1>
  {
    auto norm_dist = create_normal_dist_vector<scalar, dim>();
    return mean + decomposed * norm_dist;
  }

  /*!
  Creates a vector with all elements drawn randlomly from a standard normal
  distribution.
  */
  template <typename scalar, int dim>
  Eigen::Matrix<scalar, dim, 1> create_normal_dist_vector()
  {
    std::normal_distribution<scalar> normal_dist;
    Eigen::Matrix<scalar, dim, 1> result;
    for (int i = 0; i < dim; i++)
    {
      result(i) = normal_dist(uniform_generator);
    }
    return result;
  }
};
} // namespace filter_bay


#include <kv/Heine.hpp>
#include <kv/qAiry.hpp>
#include <kv/qBessel.hpp>
#include <cmath>
#include <iostream>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>

// define rounding operations for double
// if "rdouble.hpp" is not included, result of computation is not "verified"
#include <kv/rdouble.hpp>
typedef kv::interval<double> itv;
typedef kv::complex< kv::interval<double> > cp;
using namespace std;
namespace ub = boost::numeric::ublas;

int main()
{
  cout.precision(17);
  ub::vector< itv > x(30);
  int n=20;
  itv y,nu,q,xx;
  q="0.7";
  nu=1.5;
  y=20.;

  x(0)=y;

  for(int i=1;i<=n;i++){
    xx=mid(x(i-1));
    x(i)=xx-kv::HEratio(itv(xx),itv(nu),itv(q))*(1-q)*x(i-1)
      /(kv::HEratio(itv(x(i-1)),itv(nu),itv(q))-kv::HEratio(itv(q*x(i-1)),itv(nu),itv(q)));
    cout<<x(i)<<endl;
    //cout<<i<<endl;
    cout<<"value of HE inf"<<kv::Hahn_Exton(itv(x(i).lower()),itv(nu),itv(q))<<endl;
    cout<<"value of HE sup"<<kv::Hahn_Exton(itv(x(i).upper()),itv(nu),itv(q))<<endl;
    cout<<"value of HE mid"<<kv::Hahn_Exton(itv(mid(x(i))),itv(nu),itv(q))<<endl;
  }
 }
  // to be updated


#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iostream>

#include <Eigen/Dense>
#include <algorithm>

#include "aux/eigen2hdf.hpp"
#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/h2n_1d.hpp"
#include "spectral/hermite_to_nodal.hpp"
#include "spectral/polar_to_hermite.hpp"

//#include <deal.II/base/exceptions.h>

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

int main(int argc, char* argv[])
{
  typedef Eigen::VectorXd vec_t;

  int K = atoi(argv[1]);

  // create basis files
  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, K, 2);
  cout << "Hermite basis size: " << hermite_basis.n_dofs() << endl;

  // hermite to nodal
  typedef Eigen::MatrixXd mat_t;
  Hermite2Nodal<hermite_basis_t> h2n(
      hermite_basis, K, [K](mat_t& m1, mat_t& m2) { H2N_1d<>::create(m1, m2, K); });
  Hermite2Nodal<hermite_basis_t> h2ng(
      hermite_basis, K, [K](mat_t& m1, mat_t& m2) { H2NG_1d::create(m1, m2, K, 1.0); });

  auto& M = h2n.get_h2n();

  auto& M2 = h2ng.get_h2n();

  auto diff = M - M2;
  diff = diff.select(abs(diff) < 1e-15, Eigen::MatrixXd::Zero(diff.rows(), diff.cols()), diff);

  cout << "difference matrix\n";
  cout << diff << endl;
  // cout << "M := H2N-matrix\n";
  // cout << "M.T * M\n";
  // cout << M.transpose()* M  << endl;

  // fname = "coefficients.h5";
  // if(!boost::filesystem::exists(fname)) {
  //   cout << endl << fname << " does not exist. Abort!\n";
  //   return 1;
  // }
  // vec_t cp(N); // polar coefficients
  // hid_t h5_init = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
  // eigen2hdf::load(h5_init, "coeffs", cp);
  // H5Fclose(h5_init);

  // AssertDimension(polar_basis.n_dofs(), cp.size());

  hid_t h5f = H5Fcreate("test.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  eigen2hdf::save(h5f, "M", M);
  eigen2hdf::save(h5f, "Mg", M2);
  // eigen2hdf::save(h5f, "coeffs_nodal", cn);
  // eigen2hdf::save(h5f, "coeffs_hermite", ch);
  // eigen2hdf::save(h5f, "coeffs", cp2);
  H5Fclose(h5f);

  return 0;
}


///
/// \file hankel_gemv.hpp
///
#ifndef MXPFIT_HANKEL_MATRIX_HPP
#define MXPFIT_HANKEL_MATRIX_HPP

#include <Eigen/Core>

#include <fftw3/shared_plan.hpp>

namespace mxpfit
{

///
/// ### HankelMatrix
///
/// \brief Expression of a generalized Hankel matrix.
///
/// \tparam T  Scalar type of matrix elements
///
/// For the given vector \f$ \boldsymbol{h} = [h_0,h_1,...,h_{n+m-1}]^{T},\f$ a
/// \f$ m \times n \f$ Hankel matrix, \f$ A, \f$ is defined as
///
/// \f[
///   A = \left[ \begin{array}{cccccc}
///     h_0    & h_1    & h_2    & h_3   & \cdots & h_{n-1} \\
///     h_1    & h_2    & h_3    & h_4   & \cdots & h_{n}   \\
///     h_2    & h_3    & h_4    & h_5   & \cdots & h_{n+1} \\
///     h_3    & h_4    & h_5    & h_6   & \cdots & h_{n+2} \\
///     \vdots & \vdots & \vdots &\vdots & \ddots & \vdots \\
///     h_{m-1} & h_{m} & h_{m+1} & h_{m+2} & \cdots & h_{m+n-1}
///   \end{array} \right]
/// \f]
///
/// This class represents a Hankel matrix expression from the given number of
/// rows, \f$ m, \f$ the number of columns \f$n,\f$ and a vector expression for
/// the coefficients, \f$ \boldsymbol{h}. \f$ If the given vector expression is
/// l-value, this class wraps the existing vector expression, otherwise storage
/// for coefficients are allocated and stored.
///
/// This class also provides the interface for matrix-vector multiplication
/// compatible to `MatrixFreeGEMV`. This operation can be performed efficiently
/// using the fast Fourier transform (FFT) in \f$O((m+n)\log(m+n).\f$
///
/// For this purpose, the class also allocate internal vectors as working space.
/// The FFT plans are automatically generated whenever matrix size is updated.
/// This operation is done in thread-safe manner, using mutex-lock.
///
template <typename T>
class HankelMatrix
{
public:
    using Scalar              = T;
    using RealScalar          = typename Eigen::NumTraits<T>::Real;
    using ComplexScalar       = std::complex<RealScalar>;
    using StorageIndex        = Eigen::Index;
    using Index               = Eigen::Index;
    using CoeffsVector        = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using CoeffsVectorRef     = Eigen::Ref<const CoeffsVector>;
    using ComplexCoeffsVector = Eigen::Matrix<ComplexScalar, Eigen::Dynamic, 1>;
    using PlainObject = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

private:
    using FFT  = fftw3::FFT<RealScalar>;
    using IFFT = fftw3::IFFT<RealScalar>;

    typename FFT::PlanPointer m_fft_plan;
    typename IFFT::PlanPointer m_ifft_plan;

    Index m_rows;
    Index m_cols;

    mutable CoeffsVector m_work;
    CoeffsVectorRef m_coeffs;
    ComplexCoeffsVector m_caux;
    mutable ComplexCoeffsVector m_xaux;

public:
    enum
    {
        /// `Scalar` is complex number?
        IsComplex = Eigen::NumTraits<Scalar>::IsComplex
    };

    /// Default constructor: create an empty matrix
    HankelMatrix()
        : m_fft_plan(),
          m_ifft_plan(),
          m_rows(),
          m_cols(),
          m_work(),
          m_coeffs(m_work), // need to initialize Eigen::Ref object
          m_caux(),
          m_xaux()
    {
    }

    /// Copy constructor
    HankelMatrix(const HankelMatrix &other)
        : m_fft_plan(other.m_fft_plan),
          m_ifft_plan(other.m_ifft_plan),
          m_rows(other.m_rows),
          m_cols(other.m_cols),
          m_work(other.m_work),
          m_coeffs(other.m_coeffs),
          m_caux(other.m_caux),
          m_xaux(other.m_xaux)
    {
    }

    ///
    /// Construct a Hankel matrix with memory allocation
    ///
    /// \param[in] nrows the number of rows
    /// \param[in] ncols the number of columns
    /// \param[in] prescribed_fft_size the FFT length (optional)
    ///
    /// \pre  `nrows >= 0 && ncols >= 0` is required
    ///
    HankelMatrix(Index nrows, Index ncols, Index prescribed_fft_size = 0)
        : m_fft_plan(),
          m_ifft_plan(),
          m_rows(nrows),
          m_cols(ncols),
          m_work(get_fft_length(nrows, ncols, prescribed_fft_size)),
          m_coeffs(m_work), // need to initialize Eigen::Ref
          m_caux(IsComplex ? m_work.size() : m_work.size() / 2 + 1),
          m_xaux(m_caux.size())
    {
        assert(nrows >= Index() && "nrows must be a non-negative integer");
        assert(ncols >= Index() && "ncols must be a non-negative integer");
        set_fft_plans();
    }

    /// Default destructor
    ~HankelMatrix()
    {
    }

    /// Assignment operator is deleted as a consequence that Eigen::Ref is
    /// non-assignable
    HankelMatrix &operator=(const HankelMatrix &other) = delete;

    /// \return the number of rows
    Index rows() const
    {
        return m_rows;
    }

    /// \return the number of columns
    Index cols() const
    {
        return m_cols;
    }

    /// \return size of vector that defines current Hankel matrix
    Index size() const
    {
        // Return 0 if the matrix is empty
        return std::max(Index(), rows() + cols() - 1);
    }

    /// \return  A const reference of the coefficient vector
    const CoeffsVectorRef &coeffs() const
    {
        return m_coeffs;
    }

    ///
    /// Resize internal Hankel matrix and update FFT plan for matrix-vector
    /// multiplication if necessary.
    ///
    void resize(Index nrows, Index ncols, Index prescribed_fft_size = 0)
    {
        assert(nrows >= Index() && "nrows must be a non-negative integer");
        assert(ncols >= Index() && "ncols must be a non-negative integer");

        m_rows = nrows;
        m_cols = ncols;

        const Index n1 = m_work.size();
        const Index n2 = get_fft_length(nrows, ncols, prescribed_fft_size);

        if (n1 != n2)
        {
            m_work.resize(n2);
            m_caux.resize(IsComplex ? m_work.size() : m_work.size() / 2 + 1);
            m_xaux.resize(m_caux.size());
            set_fft_plans();

            if (coeffs().size() != size())
            {
                // Eigen::Ref to dummy object
                m_coeffs.~CoeffsVectorRef();
                ::new (&m_coeffs) CoeffsVectorRef(m_work);
            }
        }
    }

    ///
    /// Set a Hankel matrix from the number of rows, columns and vector
    /// expression of matrix coefficients.
    ///
    /// \param[in] coeffs   the vector expression of matrix coefficients
    ///
    /// \pre `coeffs.size() ==  size()` is required
    ///
    template <typename Derived>
    void setCoeffs(const Eigen::EigenBase<Derived> &coeffs)
    {
        assert(coeffs.size() == size() &&
               "Invalid size for the vector of coefficients");
        if (rows() == Index() || cols() == Index())
        {
            // Matrix is empty. Nothing to do.
            return;
        }

        m_coeffs.~CoeffsVectorRef();
        ::new (&m_coeffs) CoeffsVectorRef(coeffs.derived());

        compute_aux_vector();
    }

    void setCoeffs(const CoeffsVectorRef &coeffs)
    {
        assert(coeffs.size() == size() &&
               "Invalid size for the vector of coefficients");
        if (rows() == Index() || cols() == Index())
        {
            // Matrix is empty. Nothing to do.
            return;
        }

        if (&(coeffs.derived()) != &m_coeffs)
        {
            m_coeffs.~CoeffsVectorRef();
            ::new (&m_coeffs) CoeffsVectorRef(coeffs);
            compute_aux_vector();
        }
    }

    /// \return The same Hankel matrix in dense form.
    PlainObject toDenseMatrix() const
    {
        PlainObject ret(rows(), cols());
        for (Index j = 0; j < cols(); ++j)
        {
            for (Index i = 0; i < rows(); ++i)
            {
                ret(i, j) = coeffs()(i + j);
            }
        }
        return ret;
    }

    /// Compute `dst += alpha * A * rhs`
    template <typename Dest, typename RHS>
    void apply(Dest &dst, const Eigen::MatrixBase<RHS> &rhs, Scalar alpha) const
    {
        apply_impl<false>(dst, rhs, alpha);
    }

    /// Compute `dst += alpha * A.conjugate() * rhs`
    template <typename Dest, typename RHS>
    void applyConjugate(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                        Scalar alpha) const
    {
        apply_impl<(IsComplex != 0)>(dst, rhs, alpha);
    }

    /// Compute `dst += alpha * A.transpose() * rhs`
    template <typename Dest, typename RHS>
    void applyTranspose(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                        Scalar alpha) const
    {
        apply_transpose_impl<false>(dst, rhs, alpha);
    }

    /// Compute `dst += alpha * A.transpose() * rhs`
    template <typename Dest, typename RHS>
    void applyAdjoint(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                      Scalar alpha) const
    {
        apply_transpose_impl<(IsComplex != 0)>(dst, rhs, alpha);
    }

private:
    static Index get_fft_length(Index nrows, Index ncols,
                                Index prescribed_fft_size)
    {
        // Enforce fft_size becomes even
        const Index n = (nrows + ncols) / 2;
        return std::max({prescribed_fft_size, 2 * n});
    }

    void set_fft_plans()
    {
        const int n       = static_cast<int>(m_work.size());
        const int howmany = 1;
        m_fft_plan  = FFT::make_plan(n, howmany, m_work.data(), m_xaux.data());
        m_ifft_plan = IFFT::make_plan(n, howmany, m_xaux.data(), m_work.data());
    }

    // Pre-compute auxiliary vector for matrix-vector multiplication operation
    void compute_aux_vector()
    {
        // Set first column of circulant matrix C. Then compute the discrete
        // Fourier transform this vector and store the result into \c caux.
        const auto nhead    = rows();
        const auto ntail    = cols() - 1;
        const auto npadding = m_work.size() - nhead - ntail;

        m_work.head(nhead) = coeffs().tail(nhead);
        if (npadding > Index())
        {
            m_work.segment(nhead, npadding).setZero();
        }
        m_work.tail(ntail) = coeffs().head(ntail);

        // m_caux <-- FFT[m_work]
        FFT::run(m_fft_plan, m_work.data(), m_caux.data());
        m_caux *= RealScalar(1) / m_work.size();
    }

    template <bool ComplexConjugate, typename Dest, typename RHS>
    void apply_impl(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                    Scalar alpha) const
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Dest);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(RHS);

        assert(dst.size() == rows());
        assert(rhs.size() == cols());

        if (alpha == Scalar(/*zero*/))
        {
            return;
        }
        //
        // Form new vector x' = [x(n-1),x(n-2),...,x(0),0....0] of
        // length n + m - 1, and compute FFT.
        //
        if (ComplexConjugate)
        {
            m_work.head(cols()) = rhs.reverse().conjugate();
        }
        else
        {
            m_work.head(cols()) = rhs.reverse();
        }

        m_work.tail(m_work.size() - cols()).setZero();

        // m_xaux <-- FFT[m_work]
        FFT::run(m_fft_plan, m_work.data(), m_xaux.data());
        //
        // y[0:nrows] = IFFT(FFT(c') * FFT(x'))[0:nrows]
        //
        m_xaux = m_xaux.cwiseProduct(m_caux);
        IFFT::run(m_ifft_plan, m_xaux.data(), m_work.data());

        if (ComplexConjugate)
        {
            dst += alpha * m_work.head(rows()).conjugate();
        }
        else
        {
            dst += alpha * m_work.head(rows());
        }
    }

    // Compute `dst += alpha * A^T * rhs`
    template <bool ComplexConjugate, typename Dest, typename RHS>
    void apply_transpose_impl(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                              Scalar alpha) const
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Dest);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(RHS);

        assert(dst.size() == cols());
        assert(rhs.size() == rows());

        if (alpha == Scalar(/*zero*/))
        {
            return;
        }
        //
        // Form new vector x' = [0,0,...,0,x(m-1),x(m-2),...,x(0)] of
        // length n + m - 1, and compute FFT.
        //
        m_work.head(m_work.size() - rows()).setZero();
        if (ComplexConjugate)
        {
            m_work.tail(rows()) = rhs.reverse().conjugate();
        }
        else
        {
            m_work.tail(rows()) = rhs.reverse();
        }

        // m_xaux <-- FFT[m_work]
        FFT::run(m_fft_plan, m_work.data(), m_xaux.data());
        //
        // y[0:ncols-1] = IFFT(FFT(c') * FFT(x'))[nrows:size - 1]
        //
        m_xaux = m_xaux.cwiseProduct(m_caux);
        IFFT::run(m_ifft_plan, m_xaux.data(), m_work.data());

        if (ComplexConjugate)
        {
            dst += alpha * m_work.tail(cols()).conjugate();
        }
        else
        {
            dst += alpha * m_work.tail(cols());
        }
    }
};

} // namespace mxpfit

#endif /* MXPFIT_HANKEL_MATRIX_HPP */


/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

============================================================================*/

#include <boost/math/constants/constants.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/version.hpp>

#include "mitkFormulaParser.h"
#include "mitkFresnel.h"

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;
namespace phx = boost::phoenix;

typedef std::string::const_iterator Iter;
typedef ascii::space_type Skipper;

namespace qi = boost::spirit::qi;

namespace mitk
{
  /*!
   *	@brief			Transforms the given number from degrees to radians and returns it.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] deg	A scalar value in degrees.
   *	@return			The given value in radians.
   */
  template<typename T>
  inline T deg2rad(const T deg)
  {
    return deg * boost::math::constants::pi<T>() / static_cast<T>(180);
  }

  /*!
   *	@brief			Returns the cosine of the given degree scalar.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	A scalar value in degrees whose cosine should be returned.
   *	@return			The cosine of the given degree scalar.
   */
  template<typename T>
  inline T cosd(const T t)
  {
    return std::cos(deg2rad(t));
  }

  /*!
   *	@brief			Returns the sine of the given degree scalar.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	A scalar value in degrees whose sine should be returned.
   *	@return			The sine of the given degree scalar.
   */
  template<typename T>
  inline T sind(const T t)
  {
    return std::sin(deg2rad(t));
  }

  /*!
   *	@brief			Returns the tangent of the given degree scalar.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	A scalar value in degrees whose tangent should be returned.
   *	@return			The tangent of the given degree scalar.
   */
  template<typename T>
  inline T tand(const T t)
  {
    return std::tan(deg2rad(t));
  }

  /*!
   *	@brief			Returns the fresnel integral sine at the given x-coordinate.
   *	@details		Code for "fresnel_s()" (fresnel.cpp and fresnel.h) taken as-is from the GNU
   *					Scientific Library (http://www.gnu.org/software/gsl/), specifically from
   *					http://www.network-theory.co.uk/download/gslextras/Fresnel/.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	The x-coordinate at which the fresnel integral sine should be returned.
   *	@return			The fresnel integral sine at the given x-coordinate.
   */
  template<typename T>
  T fresnelS(const T t)
  {
    T x = t / boost::math::constants::root_half_pi<T>();
    return static_cast<T>(fresnel_s(x) / boost::math::constants::root_two_div_pi<T>());
  }

  /*!
   *	@brief			Returns the fresnel integral cosine at the given x-coordinate.
   *	@details		Code for "fresnel_c()" (fresnel.cpp and fresnel.h) taken as-is from the GNU
   *					Scientific Library (http://www.gnu.org/software/gsl/), specifically from
   *					http://www.network-theory.co.uk/download/gslextras/Fresnel/.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	The x-coordinate at which the fresnel integral cosine should be returned.
   *	@return			The fresnel integral cosine at the given x-coordinate.
   */
  template<typename T>
  T fresnelC(const T t)
  {
    T x = t / boost::math::constants::root_half_pi<T>();
    return static_cast<T>(fresnel_c(x) / boost::math::constants::root_two_div_pi<T>());
  }

  /*!
   *	@brief		The grammar that defines the language (i.e. what is allowed) for the parser.
   */
  class Grammar : public qi::grammar<Iter, FormulaParser::ValueType(), Skipper>
  {
    /*!
     *	@brief	Helper structure that makes it easier to dynamically call any
     *			one-parameter-function by overloading the @c () operator.
     */
    struct func1_
    {
      // Required for Phoenix 3+
      template<typename Sig>
      struct result;

      /*!
       *	@brief				Helper structure that is needed for compatibility with
       *						@c boost::phoenix.
       *	@tparam Functor		Type of the functor (this struct).
       *	@tparam Function	Type of the function that should be called.
       *	@tparam Arg1		Type of the argument the function should be called with.
       *
       */
      template<typename Functor, typename Function, typename Arg1>
      struct result<Functor(Function, Arg1&)>
      {
        /*! @brief The result structure always needs this typedef */
        typedef Arg1 type;
      };

      /*!
       *	@brief				Calls the function @b f with the argument @b a1 and returns the
       *						result.
       *						The result always has the same type as the argument.
       *	@tparam Function	Type of the function that should be called.
       *	@tparam Arg1			Type of the argument the function should be called with.
       *	@param[in] f		The function that should be called.
       *	@param[in] a1		The argument the function should be called with.
       *	@return				The result of the called function.
       */
      template<typename Function, typename Arg1>
      Arg1 operator()(const Function f, const Arg1 a1) const
      {
        return f(a1);
      }
    };

    /*!
     *	@brief	Helper structure that maps strings to function calls so that parsing e.g.
     *			@c "cos(0)" actually calls the @c std::cos function with parameter @c 1 so it
     *			returns @c 0.
     */
    class unaryFunction_ :
      public qi::symbols<typename std::iterator_traits<Iter>::value_type, FormulaParser::ValueType(*)(FormulaParser::ValueType)>
    {
    public:
      /*!
       *	@brief Constructs the structure, this is where the mapping takes place.
       */
      unaryFunction_()
      {
        this->add
        ("abs", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::abs))
          ("exp", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::exp)) // @TODO: exp ignores division by zero
          ("sin", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::sin))
          ("cos", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::cos))
          ("tan", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::tan))
          ("sind", &sind)
          ("cosd", &cosd)
          ("tand", &tand)
          ("fresnelS", &fresnelS)
          ("fresnelC", &fresnelC);
      }
    } unaryFunction;

  public:
    /*!
     *	@brief							Constructs the grammar with the given formula parser.
     *	@param[in, out] formulaParser	The formula parser this grammar is for - so it can
     *									access its variable map.
     */
    Grammar(FormulaParser& formulaParser) : Grammar::base_type(start)
    {
      using qi::_val;
      using qi::_1;
      using qi::_2;
      using qi::char_;
      using qi::alpha;
      using qi::alnum;
      using qi::double_;
      using qi::as_string;

      phx::function<func1_> func1;

      start = expression > qi::eoi;

      expression = term[_val = _1]
        >> *(('+' >> term[_val += _1])
          | ('-' >> term[_val -= _1]));

      term = factor[_val = _1]
        >> *(('*' >> factor[_val *= _1])
          | ('/' >> factor[_val /= _1]));

      factor = primary[_val = _1];
      /*!	@TODO:	Repair exponentiation */
      //>> *('^' >> factor[phx::bind<FormulaParser::ValueType, FormulaParser::ValueType, FormulaParser::ValueType>(std::pow, _val, _1)]);

      variable = as_string[alpha >> *(alnum | char_('_'))]
        [_val = phx::bind(&FormulaParser::lookupVariable, &formulaParser, _1)];

      primary = double_[_val = _1]
        | '(' >> expression[_val = _1] >> ')'
        | ('-' >> primary[_val = -_1])
        | ('+' >> primary[_val = _1])
        | (unaryFunction >> '(' >> expression >> ')')[_val = func1(_1, _2)]
        | variable[_val = _1];
    }

    /*! the rules of the grammar. */
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> start;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> expression;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> term;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> factor;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> variable;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> primary;
  };

  FormulaParser::FormulaParser(const VariableMapType* variables) : m_Variables(variables)
  {}

  FormulaParser::ValueType FormulaParser::parse(const std::string& input)
  {
    std::string::const_iterator iter = input.begin();
    std::string::const_iterator end = input.end();
    FormulaParser::ValueType result = static_cast<FormulaParser::ValueType>(0);

    try
    {
      if (!qi::phrase_parse(iter, end, Grammar(*this), ascii::space, result))
      {
        mitkThrowException(FormulaParserException) << "Could not parse '" << input <<
          "': Grammar could not be applied to the input " << "at all.";
      }
    }
    catch (qi::expectation_failure<Iter>& e)
    {
      std::string parsed = "";

      for (Iter i = input.begin(); i != e.first; i++)
      {
        parsed += *i;
      }
      mitkThrowException(FormulaParserException) << "Error while parsing '" << input <<
        "': Unexpected character '" << *e.first << "' after '" << parsed << "'";
    }

    return result;
  };

  FormulaParser::ValueType FormulaParser::lookupVariable(const std::string var)
  {
    if (m_Variables == nullptr)
    {
      mitkThrowException(FormulaParserException) << "Map of variables is empty";
    }

    try
    {
      return m_Variables->at(var);
    }
    catch (std::out_of_range&)
    {
      mitkThrowException(FormulaParserException) << "No variable '" << var << "' defined in lookup";
    }
  };

}


// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"

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

int main()
{
    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian(2, 2);      //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd gradient(2);                    //Q: n*1向量
    Eigen::SparseMatrix<double> linearMatrix(2, 2); //A: m*n矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd lowerBound(2);                  //L: m*1下限向量
    Eigen::VectorXd upperBound(2);                  //U: m*1上限向量

    hessian.insert(0, 0) = 2.0; //注意稀疏矩阵的初始化方式,无法使用<<初始化
    hessian.insert(1, 1) = 2.0;
    // std::cout << "hessian:" << std::endl
    //           << hessian << std::endl;
    gradient << -2, -2;
    linearMatrix.insert(0, 0) = 1.0; //注意稀疏矩阵的初始化方式,无法使用<<初始化
    linearMatrix.insert(1, 1) = 1.0;
    // std::cout << "linearMatrix:" << std::endl
    //           << linearMatrix << std::endl;
    lowerBound << 1, 1;
    upperBound << 1.5, 1.5;

    // instantiate the solver
    OsqpEigen::Solver solver;

    // settings
    solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    solver.data()->setNumberOfVariables(2);   //变量数n
    solver.data()->setNumberOfConstraints(2); //约束数m
    if (!solver.data()->setHessianMatrix(hessian))
        return 1;
    if (!solver.data()->setGradient(gradient))
        return 1;
    if (!solver.data()->setLinearConstraintsMatrix(linearMatrix))
        return 1;
    if (!solver.data()->setLowerBound(lowerBound))
        return 1;
    if (!solver.data()->setUpperBound(upperBound))
        return 1;

    // instantiate the solver
    if (!solver.initSolver())
        return 1;

    Eigen::VectorXd QPSolution;

    // solve the QP problem
    if (!solver.solve())
    {
        return 1;
    }

    QPSolution = solver.getSolution();
    std::cout << "QPSolution" << std::endl
              << QPSolution << std::endl; //输出为m*1的向量
    return 0;
}


/* ROS-CHOMP.
 *
 * Copyright (C) 2015 Jafar Qutteineh. All rights reserved.
 * License (3-Cluase BSD): https://github.com/j3sq/ROS-CHOMP/blob/master/LICENSE
 *
 * This code uses and is based on code from:
 *   Project: trychomp https://github.com/poftwaresatent/trychomp
 *   Copyright (C) 2014 Roland Philippsen. All rights reserved.
 *   License (3-Clause BSD) : https://github.com/poftwaresatent/trychomp
 * **
 * \file chomp.cpp
 *
 * CHOMP for point vehicles (x,y) moving holonomously in the plane. It will
 * plan a trajectory (xi) connecting start point (qs) to end point (qe) while
 * avoiding obstacles (obs)
 */
#include <iostream>
#include <Eigen/Dense>
#include <stdlib.h>
#include <err.h>
#include "chomp.hpp"

typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Isometry3d Transform;

using namespace std;

namespace chomp {
//////////////////////////////////////////////////
// trajectory etc

static size_t const nq(20);             // number of q stacked into xi
static size_t const cdim(2);            // dimension of config space
static size_t const xidim(nq *cdim);    // dimension of trajectory, xidim = nq * cdim
static size_t const iteration_limit(10000);
static double const dt(1.0);            // time step
static double const eta(100.0);         // >= 1, regularization factor for gradient descent
static double const lambda(1.0);        // weight of smoothness objective

//////////////////////////////////////////////////
// gradient descent etc

Matrix AA;                      // metric
Vector bb;                      // acceleration bias for start and end config
Matrix Ainv;                    // inverse of AA

static void init_chomp(Vector const &qs, Vector const &qe, Vector  &xi)
{
	if (xi.rows() == xidim) {
		//do nothing. Use existing trajectory
	} else {
		//initalize a new trajectory based on a direct line connecting qs to qe
		xi = Vector::Zero(xidim);
		Vector dxi(cdim);
		dxi << (qe(0) - qs(0)) / (nq - 1), (qe(0) - qs(0)) / (nq - 1);
		for (size_t ii(0); ii < nq; ++ii)
			xi.block(cdim * ii, 0, cdim, 1) = qs + ii * dxi;

		/*
		 * //use this instead if you want to inalize all points to qs
		 * for (size_t ii (0); ii < nq; ++ii) {
		 * xi.block (cdim * ii, 0, cdim, 1) = qs;
		 * }
		 */
	}
	AA = Matrix::Zero(xidim, xidim);
	for (size_t ii(0); ii < nq; ++ii) {
		AA.block(cdim * ii, cdim * ii, cdim, cdim) = 2.0 * Matrix::Identity(cdim, cdim);
		if (ii > 0) {
			AA.block(cdim * (ii - 1), cdim * ii, cdim, cdim) = -1.0 * Matrix::Identity(cdim, cdim);
			AA.block(cdim * ii, cdim * (ii - 1), cdim, cdim) = -1.0 * Matrix::Identity(cdim, cdim);
		}
	}
	AA /= dt * dt * (nq + 1);

	bb = Vector::Zero(xidim);
	bb.block(0, 0, cdim, 1) = qs;
	bb.block(xidim - cdim, 0, cdim, 1) = qe;
	bb /= -dt * dt * (nq + 1);

	// not needed anyhow
	// double cc (double (qs.transpose() * qs) + double (qe.transpose() * qe));
	// cc /= dt * dt * (nq + 1);

	Ainv = AA.inverse();
}

static double chomp_iteration(Vector const &qs, Vector const &qe, Vector  &xi, Matrix const &obs)
{
	//////////////////////////////////////////////////
	// beginning of "the" CHOMP iteration
	Vector nabla_smooth(AA * xi + bb);
	Vector const & xidd(nabla_smooth); // indeed, it is the same in this formulation...

	Vector nabla_obs(Vector::Zero(xidim));

	for (size_t iq(0); iq < nq; ++iq) {
		Vector const qq(xi.block(iq * cdim, 0, cdim, 1));
		Vector qd;
		if (0 == iq) {
			qd = 0.5 * (xi.block((iq + 1) * cdim, 0, cdim, 1) - qs);
		} else if (iq == nq - 1) {
			qd = 0.5 * (qe - xi.block((iq - 1) * cdim, 0, cdim, 1));
		} else {
			qd = 0.5 * (xi.block((iq + 1) * cdim, 0, cdim, 1) - xi.block((iq - 1) * cdim, 0, cdim, 1));;
		}

		// In this case, C and W are the same, Jacobian is identity.  We
		// still write more or less the full-fledged CHOMP expressions
		// (but  we only use one body point) to make subsequent extension
		// easier.
		//
		Vector const & xx(qq);
		Vector const & xd(qd);
		Matrix const JJ(Matrix::Identity(2, 2));        // a little silly here, as noted above.
		double const vel(xd.norm());
		if (vel < 1.0e-3)                               // avoid div by zero further down
			continue;
		Vector const xdn(xd / vel);
		Vector const xdd(JJ * xidd.block(iq * cdim, 0, cdim, 1));
		Matrix const prj(Matrix::Identity(2, 2) - xdn * xdn.transpose()); // hardcoded planar case
		Vector const kappa(prj * xdd / pow(vel, 2.0));

		for (int ii = 0; ii < obs.cols(); ii++) {
			Vector delta(xx - obs.block(0, ii, cdim, 1));
			double const dist(delta.norm());
			if ((dist >= obs(2, ii)) || (dist < 1e-9))
				continue;
			static double const gain(10.0);                                                 // hardcoded param
			double const cost(gain * obs(2, ii) * pow(1.0 - dist / obs(2, ii), 3.0) / 3.0); // hardcoded param
			delta *= -gain *pow(1.0 - dist / obs(2, ii), 2.0) / dist;                       // hardcoded param
			nabla_obs.block(iq * cdim, 0, cdim, 1) += JJ.transpose() * vel * (prj * delta - cost * kappa);
		}
	}

	Vector dxi(Ainv * (nabla_obs + lambda * nabla_smooth));
	xi -= dxi / eta;
	//return the error (in Euclidean sense ). Remeber that the difference is -dxi/eta
	return dxi.norm() / eta;

	// end of "the" CHOMP iteration
	//////////////////////////////////////////////////
}

void generatePath(Vector const &qs, Vector const &qe, Vector &xi, Matrix const &obs)
{
	init_chomp(qs, qe, xi);
	double err;
	for (size_t ii = 0; ii < iteration_limit; ii++) {
		err = chomp_iteration(qs, qe, xi, obs);
		if (err < 0.01)
			break;
	}
}
} //namespace


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/Gaussian.h>
#include <BayesFilters/GaussianFilter.h>
#include <BayesFilters/KFCorrection.h>
#include <BayesFilters/KFPrediction.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>
#include <BayesFilters/utils.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class KFSimulation : public GaussianFilter
{
public:
    KFSimulation
    (
        Gaussian& initial_state,
        std::unique_ptr<GaussianPrediction> prediction,
        std::unique_ptr<GaussianCorrection> correction,
        std::size_t simulation_steps
    ) noexcept :
        GaussianFilter(initial_state, std::move(prediction), std::move(correction)),
        simulation_steps_(simulation_steps)
    { }

protected:
    bool runCondition() override
    {
        if (getFilteringStep() < simulation_steps_)
            return true;
        else
            return false;
    }

    std::vector<std::string> log_filenames(const std::string& prefix_path, const std::string& prefix_name) override
    {
        return  {prefix_path + "/" + prefix_name + "_pred_mean",
                 prefix_path + "/" + prefix_name + "_cor_mean"};
    }

    void log() override
    {
        logger(predicted_state_.mean().transpose(), corrected_state_.mean().transpose());
    }

private:
    std::size_t simulation_steps_;
};

int main()
{
    std::cout << "Running a KF filter on a simulated target." << std::endl;
    std::cout << "Data is logged in the test folder with prefix testKF." << std::endl;

    /* A set of parameters needed to run a Kalman filter in a simulated environment. */
    Vector4d initial_simulated_state(10.0f, 0.0f, 10.0f, 0.0f);
    std::size_t simulation_time = 100;

    /* Step 1 - Initialization */

    Gaussian initial_state(4);
    Vector4d initial_mean(4.0f, 0.04f, 15.0f, 0.4f);
    Matrix4d initial_covariance;
    initial_covariance << pow(0.05, 2), 0,            0,            0,
                          0,            pow(0.05, 2), 0,            0,
                          0,            0,            pow(0.01, 2), 0,
                          0,            0,            0,            pow(0.01, 2);
    initial_state.mean() = initial_mean;
    initial_state.covariance() = initial_covariance;

    /* Step 2 - Prediction */

    /* Step 2.1 - Define the state model. */

    /* Initialize a white noise acceleration state model. */
    double T = 1.0f;
    double tilde_q = 10.0f;

    std::unique_ptr<LinearStateModel> wna = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);

    /* Step 2.2 - Define the prediction step. */

    /* Initialize the Kalman filter prediction step and pass the ownership of the state model. */
    std::unique_ptr<KFPrediction> kf_prediction = utils::make_unique<KFPrediction>(std::move(wna));

    /* Step 3 - Correction */

    /* Step 3.1 - Define where the measurement are originated from (simulated in this case). */

    /* Initialize simulated target model with a white noise acceleration. */
    std::unique_ptr<StateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_simulated_state, simulation_time);
    simulated_state_model->enable_log(".", "testKF");

    /* Step 3.2 - Initialize a measurement model (a linear sensor reading x and y coordinates). */
    std::unique_ptr<LinearMeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model));
    simulated_linear_sensor->enable_log(".", "testKF");

    /* Step 3.3 - Initialize the Kalman filter correction step and pass the ownership of the measurement model. */
    std::unique_ptr<KFCorrection> kf_correction = utils::make_unique<KFCorrection>(std::move(simulated_linear_sensor));

    /* Step 4 - Assemble the Kalman filter. */
    std::cout << "Constructing Kalman filter..." << std::flush;
    KFSimulation kf(initial_state, std::move(kf_prediction), std::move(kf_correction), simulation_time);
    kf.enable_log(".", "testKF");
    std::cout << "done!" << std::endl;

    /* Step 5 - Boot the filter. */
    std::cout << "Booting Kalman filter..." << std::flush;
    kf.boot();
    std::cout << "completed!" << std::endl;

    /* Step 6 - Run the filter and wait until it is closed. */
    /* Note that since this is a simulation, the filter will end upon simulation termination. */
    std::cout << "Running Kalman filter..." << std::flush;
    kf.run();
    std::cout << "waiting..." << std::flush;

    if (!kf.wait())
        return EXIT_FAILURE;

    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}


#include "ode45.hpp"

#include <iostream>

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

// Comment to disable test
#define MAKE_TEST1
#define MAKE_TEST2
#define MAKE_TEST3
#define MAKE_TEST4
#define EIGEN_DONT_VECTORIZE
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT // I just added that to fix an error
void test1() {
#ifdef MAKE_TEST1
    std::cout << "Prey/Predator model test:" << std::endl;

    // Prey/Predator model
    const double alpha1 = 3;
    const double alpha2 = 2;
    const double beta1 = 0.1;
    const double beta2 = 0.1;
    auto f = [&alpha1, &alpha2, &beta1, &beta2] (const Eigen::VectorXd & y) {
        Eigen::VectorXd temp = y;
        temp(0) *= (alpha1 - beta1*y(1));
        temp(1) *= (beta2*y(0) - alpha2);
        return temp;
    };

//     Eigen::VectorXd y0(2);
    // or
    Eigen::Vector2d y0;
    y0 << 100, 5;

    const double T = 10;

    // Basic usage:
    ode45<Eigen::VectorXd> O(f);
//     O.options.do_statistics = true;
    auto sol = O.solve(y0, T);

    // Print info
//     O.print();

    // Print some info
    std::cout << "T = " << sol.back().second << std::endl;
    std::cout << "y(T) = " << std::endl << sol.back().first << std::endl;
#endif
}

void test2() {
#ifdef MAKE_TEST2
    std::cout << "Fundamental types test and validation:" << std::endl;

    // Test class with fundamental types
    auto f = [] (double y) { return 1 / y; };

    double y0 = 0.2;
    // Large step size
    const double T = 10000;

    // Quick syntax
    ode45<double> O(f);
//     O.options.do_statistics = true;
    auto sol = O.solve(y0, T);

    // Print info
//     O.print();

    // Print some info
    std::cout << "T = " << sol.back().second << std::endl;
    std::cout << "y(T) = " << std::endl << sol.back().first << std::endl;
    auto y_ex = [] (double t) { return std::sqrt(2*t+0.04); };
    std::cout << "y_ex(T) = " << std::endl << y_ex(T) << std::endl;
#endif
}

void test3() {
#ifdef MAKE_TEST3
    std::cout << "Multidimensional test:" << std::endl;

    // Construct data for the IVP
    double T = 1;
    // Many dimensions
    int n = 5;

    // Multidimensional rhs
    Eigen::VectorXd y0(2*n);
    for(int i = 0; i < n; ++i) {
        y0(i)=(i+1.)/n;
        y0(i+n)=-1;
    }

    // Multidimensional rhs
    auto f = [n] (Eigen::VectorXd y) {
        Eigen::VectorXd fy(2*n);

        Eigen::VectorXd g(n);
        g(0) = y(0)*(y(1)+y(0));
        g(n-1) = y(n-1)*(y(n-1)+y(n-2));
        for(int i = 1; i < n-1; ++i) {
            g(i) = y(i)*(y(i-1)+y(i+1));
        }

        Eigen::SparseMatrix<double> C(n,n);
        C.reserve(3);
        for(int i = 0; i < n; ++i) {
            C.insert(i,i) = 2;
            if(i < n-1) C.insert(i,i+1) = -1;
            if(i >= 1)  C.insert(i,i-1) = -1;
        }
        C.makeCompressed();
        fy.head(n) = y.head(n);

        Eigen::SparseLU< Eigen::SparseMatrix<double> >  solver;
        solver.analyzePattern(C);
        solver.compute(C);
        fy.tail(n) = solver.solve(g);
        return fy;
    };

    // Constructor:
    ode45<Eigen::VectorXd> O(f);

    // Setup options
    O.options.do_statistics = true;

    // Solve
    auto sol = O.solve(y0, T);

    // Print info
    O.print();

    std::cout << "T = " << sol.back().second << std::endl;
    std::cout << "y(T) = " << std::endl << sol.back().first << std::endl;
#endif
}

void test4() {
#ifdef MAKE_TEST4
    std::cout << "Stiff ode test:" << std::endl;

    // Test class with fundamental types
    auto f = [] (double y) { return y*(y - y*y); };

    // cf. http://ch.mathworks.com/company/newsletters/articles/stiff-differential-equations.html
    double y0 = 0.0001; // try 0.01
    // Large step size
    const double T = 2 / y0;

    // Quick syntax
    ode45<double> O(f);
    O.options.do_statistics = true;
    auto sol = O.solve(y0, T);

    // Print info
    O.print();

    // Print some info
    std::cout << "T = " << sol.back().second << std::endl;
    std::cout << "y(T) = " << std::endl << sol.back().first << std::endl;

//     for(auto v: sol) {
//         std::cout << v.first << std::endl;
//     }
#endif
}

int main() {

    // Basic prey/predator test
    test1();

    // Test double interface
    test2();

    // A bit more involved example, also testing options and statistics
    test3();

    // A bit more involved example, also testing options and statistics
    test4();

    return 0;
}

Nummerical Methods for CSE/PS13/templates_ps13/ode45_test.cpp


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE( test )
{
    double v1 = 1.23456e-10;
    double v2 = 1.23457e-10;

    BOOST_CHECK_CLOSE( v1, v2, 0.0001 );
    // Absolute value of difference between these two values is 1e-15. They seems
    // to be very close. But we want to checks that these values differ no more then 0.0001%
    // of their value. And this test will fail at tolerance supplied.
}

//____________________________________________________________________________//


/**
 * @file taylorode.cc
 * @brief NPDE homework TaylorODE
 * @author ?, Philippe Peter
 * @date 24.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "taylorode.h"

#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

namespace TaylorODE {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector2d PredPreyModel::f(const Eigen::Vector2d& y) const {
  //====================
  // Your code goes here
  //====================
  return y;
}

Eigen::Vector2d PredPreyModel::df(const Eigen::Vector2d& y,
                                  const Eigen::Vector2d& z) const {
  //====================
  // Your code goes here
  //====================
  return y;
}

Eigen::Vector2d PredPreyModel::d2f(const Eigen::Vector2d& y,
                                   const Eigen::Vector2d& z) const {
  //====================
  // Your code goes here
  //====================
  return y;
}

std::vector<Eigen::Vector2d> SolvePredPreyTaylor(const PredPreyModel& model,
                                                 double T,
                                                 const Eigen::Vector2d& y0,
                                                 unsigned int M) {
  std::vector<Eigen::Vector2d> res;
  res.reserve(M + 1);

  //====================
  // Your code goes here
  //====================
  return res;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double TestCvgTaylorMethod() {
  // initialize parameters for the model:
  double T = 10;               // final time
  Eigen::Vector2d y0(100, 5);  // initial condition
  Eigen::Vector2d yex(0.319465882659820,
                      9.730809352326228);  // reference solution
  double alpha1 = 3.0;
  double alpha2 = 2.0;
  double beta1 = 0.1;
  double beta2 = 0.1;
  PredPreyModel model(alpha1, alpha2, beta1, beta2);

  // Initialize parameters for the convergence study
  unsigned int M0 = 128;    // Minimum number of timesteps
  unsigned int numRef = 8;  // Number of refinements

  // Convergence study
  Eigen::ArrayXd error(numRef);
  Eigen::ArrayXd M(numRef);
  // Run convergence study
  //====================
  // Your code goes here
  //====================

  PrintErrorTable(M, error);

  // Estimate convergence rate based on linear regression
  //====================
  // Your code goes here
  //====================
  return 0.0;
}
/* SAM_LISTING_END_2 */

void PrintErrorTable(const Eigen::ArrayXd& M, const Eigen::ArrayXd& error) {
  std::cout << std::setw(15) << "M" << std::setw(15) << "error" << std::setw(15)
            << "rate" << std::endl;

  for (unsigned int i = 0; i < M.size(); ++i) {
    std::cout << std::setw(15) << M(i) << std::setw(15) << error(i);
    if (i > 0) {
      std::cout << std::setw(15) << std::log2(error(i - 1) / error(i));
    }
    std::cout << std::endl;
  }
}

}  // namespace TaylorODE


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

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
//#include <opencv2/legacy/compat.hpp>

#include "dlib/opencv.h"
#include "dlib/image_processing/frontal_face_detector.h"
#include "dlib/image_processing/render_face_detections.h"
#include "dlib/gui_widgets.h"
#include <dlib/image_processing.h>

#include "util.h"
#include "constants.h"
#include "faceDetection.h"
#include "pupilDetection.h"
#include "faceModel.h"
using namespace dlib;
void preprocessROI(cv::Mat& roi_eye) {
	GaussianBlur(roi_eye, roi_eye, cv::Size(3,3), 0, 0);
	equalizeHist( roi_eye, roi_eye );
}

double find_sigma(int ln, int lf, double Rn, double theta) {
	double dz=0;
	double sigma;
	double m1 = ((double)ln*ln)/((double)lf*lf);
	double m2 = (cos(theta))*(cos(theta));

	if (m2 == 1)
	{
		dz = sqrt(	(Rn*Rn)/(m1 + (Rn*Rn))	);
	}
	if (m2>=0 && m2<1)
	{
		dz = sqrt(	((Rn*Rn) - m1 - 2*m2*(Rn*Rn) + sqrt(	((m1-(Rn*Rn))*(m1-(Rn*Rn))) + 4*m1*m2*(Rn*Rn)	))/ (2*(1-m2)*(Rn*Rn))	);
	}
	sigma = acos(dz);
	return sigma;
}

void faceModel::assign(full_object_detection shape , cv::Mat image, int mode = MODE_GAZE_VA) {
	assert(mode == MODE_GAZE_VA || mode == MODE_GAZE_QE);
	faceShape = shape;
	image.copyTo(inputImage);

	descriptors.clear();

	computePupil();
	computeNormal();
	computeGaze(mode);
}

void faceModel::computePupil() {
	// Computing left pupil
	std::vector<cv::Point> leftEyePoints = getFeatureDescriptors(INDEX_LEFT_EYE);
	rectLeftEye = cv::boundingRect(leftEyePoints)
	roiLeftEye = inputImage(rectLeftEye)
	preprocessROI(roiLeftEye);
	descriptors.push_back(get_pupil_coordinates(roiLeftEye,rectLeftEye));

	// Computing right pupil
	std::vector<cv::Point> rightEyePoints = getFeatureDescriptors(INDEX_RIGHT_EYE);
	rectRightEye = cv::boundingRect(rightEyePoints)
	roiRightEye = inputImage(rectRightEye)
	preprocessROI(roiRightEye);
	descriptors.push_back(get_pupil_coordinates(roiRightEye,rectRightEye));
}

void faceModel::computeNormal() {
	cv::Point midEye = get_mid_point(cv::Point(shape.part(39).x(), shape.part(39).y()),
		cv::Point(shape.part(40).x(), shape.part(40).y()));

	cv::Point mouth = get_mid_point(cv::Point(shape.part(48).x(), shape.part(48).y()),
		cv::Point(shape.part(54).x(), shape.part(54).y()));

	cv::Point noseTip = cv::Point(shape.part(30).x(), shape.part(30).y());
	cv::Point noseBase = cv::Point(shape.part(33).x(), shape.part(33).y());

	// symm angle - angle between the symmetry axis and the 'x' axis
	symm_x = get_angle_between(noseBase, midEye);
	// tilt angle - angle between normal in image and 'x' axis
	tau = get_angle_between(noseBase, noseTip);
	// theta angle - angle between the symmetry axis and the image normal
	theta = (abs(tau - symm_x)) * (PI/180.0);

	// sigma - slant angle
	sigma = find_sigma(get_distance(noseTip, noseBase), get_distance(midEye, mouth), Rn, theta);

	normal[0] = (sin(sigma))*(cos((360 - tau)*(PI/180.0)));
	normal[1] = (sin(sigma))*(sin((360 - tau)*(PI/180.0)));
	normal[2] = -cos(sigma);

	pitch = acos(sqrt((normal[0]*normal[0] + normal[2]*normal[2])/(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2])));
	if((noseTip.y - noseBase.y) < 0) {
		pitch = -pitch;
	}

	yaw = acos((abs(normal[2]))/(sqrt(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2])));
	if((noseTip.x - noseBase.x) < 0) {
		yaw = -yaw;
	}
}

void computeGaze(int mode) {
	compute_eye_gaze (FacePose* face_pose, dlib::full_object_detection shape, cv::Rect rect, cv::Point pupil, double mag_CP, double mag_LR, double mag_CR, double mag_CM, double theta, int mode, std::vector<double>& vec_CP) {
}

void faceModel::setOrigin(cv::Point origin) {
	this.origin = origin;
}

void faceModel::setOrigin(int mode) {
	assert(mode == ORIGIN_IMAGE || mode == ORIGIN_FACE_CENTRE);

	if (mode == ORIGIN_IMAGE) {
		origin.x = 0;
		origin.y = 0;
	}
	else if (mode == ORIGIN_FACE_CENTRE) {
		origin.x = shape.part(30).x();
		origin.y = shape.part(30).y();
	}
}

std::vector<double> getNormal() {
	return normal;
}

cv::Point faceModel::getPupil(int mode) {
	assert(mode == INDEX_LEFT_EYE_PUPIL || mode == INDEX_RIGHT_EYE_PUPIL);
	return descriptors[mode - INDEX_LEFT_EYE_PUPIL];
}

std::vector<double> getGaze() {
	return gaze;
}

std::vector<cv::Point> faceModel::getDescriptors(int index) {
	assert(index == INDEX_LEFT_EYE || index == INDEX_RIGHT_EYE || index == INDEX_LEFT_EYE_BROW || index == INDEX_RIGHT_EYE_BROW
		|| index == INDEX_NOSE_UPPER || index == INDEX_NOSE_LOWER || index == INDEX_MOUTH_OUTER || index == INDEX_MOUTH_INNER);

	if (index == INDEX_LEFT_EYE) {
		std::vector<cv::Point> leftEyePoints;
		for (int i=36; i<=41; i++){
			leftEyePoints.push_back(cv::Point(faceShape.part(i).x(), faceShape.part(i).y()));
		}
		return leftEyePoints;
	}

	else if (index == INDEX_RIGHT_EYE) {
		std::vector<cv::Point> rightEyePoints;
		for (int i=42; i<=47; i++){
			rightEyePoints.push_back(cv::Point(faceShape.part(i).x(), faceShape.part(i).y()));
		}
		return rightEyePoints;
	}

	else if (index == INDEX_LEFT_EYE_BROW) {
		std::vector<cv::Point> leftEyeBrowPoints;
		for (int i=17; i<=21; i++){
			leftEyeBrowPoints.push_back(cv::Point(faceShape.part(i).x(), faceShape.part(i).y()));
		}
		return leftEyeBrowPoints;
	}

	else if (index == INDEX_RIGHT_EYE_BROW) {
		std::vector<cv::Point> rightEyeBrowPoints;
		for (int i=22; i<=26; i++){
			rightEyeBrowPoints.push_back(cv::Point(faceShape.part(i).x(), faceShape.part(i).y()));
		}
		return rightEyeBrowPoints;
	}

	else if (index == INDEX_NOSE_UPPER)  {
		std::vector<cv::Point> noseUpperPoints;
		for (int i=27; i<=30; i++){
			noseUpperPoints.push_back(cv::Point(faceShape.part(i).x(), faceShape.part(i).y()));
		}
		return noseUpperPoints;
	}

	else if (index == INDEX_NOSE_LOWER) {
		std::vector<cv::Point> noseLowerPoints;
		for (int i=31; i<=35; i++){
			noseLowerPoints.push_back(cv::Point(faceShape.part(i).x(), faceShape.part(i).y()));
		}
		return noseLowerPoints;
	}

	else if (index == INDEX_MOUTH_OUTER) {
		std::vector<cv::Point> mouthOuterPoints;
		for (int i=48; i<59; i++){
			mouthOuterPoints.push_back(cv::Point(faceShape.part(i).x(), faceShape.part(i).y()));
		}
		return mouthOuterPoints;
	}

	else if (index == INDEX_MOUTH_INNER) {
		std::vector<cv::Point> mouthInnerPoints;
		for (int i=60; i<=67; i++){
			mouthInnerPoints.push_back(cv::Point(faceShape.part(i).x(), faceShape.part(i).y()));
		}
		return mouthInnerPoints;
	}
}