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

#ifdef _MSC_VER
#  pragma warning (disable : 4224)
#endif

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#define DISTRIBUTIONS_TEST

#include <boost/math/distributions.hpp>
#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>
#include "../../test/table_type.hpp"
#include "table_helper.hpp"
#include "performance.hpp"
#include <iostream>

#ifdef TEST_GSL
#include <gsl/gsl_cdf.h>
#endif

class distribution_tester
{
   std::string distro_name;
   static const double quantiles[19];
   double sum;

   struct param_info
   {
      std::vector<double> params;
      std::vector<double> x_values;
   };
   std::vector<param_info> tests;
   double sanitize_x(double x)
   {
      if(x > boost::math::tools::max_value<float>() / 2)
         return boost::math::tools::max_value<float>() / 2;
      if(x < -boost::math::tools::max_value<float>() / 2)
         return -boost::math::tools::max_value<float>() / 2;
      return x;
   }
public:
   distribution_tester(const char* name) : distro_name(name), sum(0) {}

   template <class F>
   void add_test_case(F f)
   {
      tests.push_back(param_info());
      for(unsigned i = 0; i < sizeof(quantiles) / sizeof(quantiles[0]); ++i)
      {
         tests.back().x_values.push_back(sanitize_x(f(quantiles[i])));
      }
   }
   template <class F>
   void add_test_case(double p1, F f)
   {
      tests.push_back(param_info());
      tests.back().params.push_back(p1);
      for(unsigned i = 0; i < sizeof(quantiles) / sizeof(quantiles[0]); ++i)
      {
         tests.back().x_values.push_back(sanitize_x(f(p1, quantiles[i])));
      }
   }
   template <class F>
   void add_test_case(double p1, double p2, F f)
   {
      tests.push_back(param_info());
      tests.back().params.push_back(p1);
      tests.back().params.push_back(p2);
      for(unsigned i = 0; i < sizeof(quantiles) / sizeof(quantiles[0]); ++i)
      {
         tests.back().x_values.push_back(sanitize_x(f(p1, p2, quantiles[i])));
      }
   }
   template <class F>
   void add_test_case(double p1, double p2, double p3, F f)
   {
      tests.push_back(param_info());
      tests.back().params.push_back(p1);
      tests.back().params.push_back(p2);
      tests.back().params.push_back(p3);
      for(unsigned i = 0; i < sizeof(quantiles) / sizeof(quantiles[0]); ++i)
      {
         tests.back().x_values.push_back(sanitize_x(f(p1, p2, p3, quantiles[i])));
      }
   }

   enum
   {
      main_table = 1,
      boost_only_table = 2,
      both_tables = 3
   };

   template <class F>
   void run_timed_tests(F f, std::string sub_name, std::string column, bool p_value = false, int where = main_table)
   {
      std::cout << "Testing " << distro_name + " (" + std::string(sub_name) + ")" << " with library " << column << std::endl;
      try{
         double t = 0;
         unsigned repeats = 1;
         unsigned data_size;
         do{
            data_size = 0;
            stopwatch<boost::chrono::high_resolution_clock> w;

            for(unsigned count = 0; count < repeats; ++count)
            {
               for(unsigned i = 0; i < tests.size(); ++i)
               {
                  for(unsigned j = 0; j < tests[i].x_values.size(); ++j)
                  {
                     if((boost::math::isfinite)(tests[i].x_values[j]))
                        sum += f(tests[i].params, p_value ? quantiles[j] : tests[i].x_values[j]);
                     ++data_size;
                  }
               }
            }

            t = boost::chrono::duration_cast<boost::chrono::duration<double>>(w.elapsed()).count();
            if(t < 0.5)
               repeats *= 2;
         } while(t < 0.5);

         static const std::string main_table_name = std::string("Distribution performance comparison with ") + compiler_name() + std::string(" on ") + platform_name();
         static const std::string boost_table_name = std::string("Distribution performance comparison for different performance options with ") + compiler_name() + std::string(" on ") + platform_name();

         if (where & 1)
         {
            report_execution_time(
               t / data_size,
               main_table_name,
               distro_name + " (" + std::string(sub_name) + ")",
               column);
         }
         if (where & 2)
         {
            report_execution_time(
               t / data_size,
               boost_table_name,
               distro_name + " (" + std::string(sub_name) + ")",
               column);
         }
      }
      catch(const std::exception& e)
      {
         std::cerr << "Aborting due to exception: " << e.what() << std::endl;
         std::cerr << "In " << distro_name + " (" + std::string(sub_name) + ")" << std::endl;
         report_execution_time(
            (std::numeric_limits<std::uintmax_t>::max)(),
            std::string("Distribution performance comparison with ") + compiler_name() + std::string(" on ") + platform_name(),
            distro_name + " (" + std::string(sub_name) + ")",
            column);
      }
   }
};

const double distribution_tester::quantiles[19] =
{
   0.000001,
   0.00001,
   0.0001,
   0.001,
   0.01,
   0.1,
   0.2,
   0.3,
   0.4,
   0.5,
   0.6,
   0.7,
   0.8,
   0.9,
   0.99,
   0.999,
   0.9999,
   0.99999,
   0.999999
};

template <class D>
struct three_param_quantile
{
   template <class T, class U, class V, class X>
   double operator()(T x, U y, V z, X q)const
   {
      return quantile(D(x, y, z), q);
   }
};

template <class D>
struct two_param_quantile
{
   template <class T, class U, class V>
   double operator()(T x, U y, V q)const
   {
      return quantile(D(x, y), q);
   }
};

template <class D>
struct one_param_quantile
{
   template <class T, class V>
   double operator()(T x, V q)const
   {
      return quantile(D(x), q);
   }
};

template <template <class T, class U> class D>
void test_boost_1_param(distribution_tester& tester)
{
   //
   // Define some custom policies to test:
   //
   typedef boost::math::policies::policy<> default_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_double<false> > no_promote_double_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_double<false>, boost::math::policies::digits10<10> > no_promote_double_10_digits_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_float<false> > no_promote_float_policy;

   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, default_policy>(v[0]), x); }, "PDF", boost_name(), false, distribution_tester::both_tables);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, default_policy>(v[0]), x); }, "CDF", boost_name(), false, distribution_tester::both_tables);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, default_policy>(v[0]), x); }, "quantile", boost_name(), true, distribution_tester::both_tables);
   if(sizeof(double) != sizeof(long double))
   {
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, no_promote_double_policy>(v[0]), x); }, "PDF", "Boost[br]promote_double<false>", false, distribution_tester::both_tables);
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, no_promote_double_policy>(v[0]), x); }, "CDF", "Boost[br]promote_double<false>", false, distribution_tester::both_tables);
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, no_promote_double_policy>(v[0]), x); }, "quantile", "Boost[br]promote_double<false>", true, distribution_tester::both_tables);
   }
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, no_promote_double_10_digits_policy>(v[0]), x); }, "PDF", "Boost[br]promote_double<false>[br]digits10<10>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, no_promote_double_10_digits_policy>(v[0]), x); }, "CDF", "Boost[br]promote_double<false>[br]digits10<10>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, no_promote_double_10_digits_policy>(v[0]), x); }, "quantile", "Boost[br]promote_double<false>[br]digits10<10>", true, distribution_tester::boost_only_table);

   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<float, no_promote_float_policy>(static_cast<float>(v[0])), static_cast<float>(x)); }, "PDF", "Boost[br]float[br]promote_float<false>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<float, no_promote_float_policy>(static_cast<float>(v[0])), static_cast<float>(x)); }, "CDF", "Boost[br]float[br]promote_float<false>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<float, no_promote_float_policy>(static_cast<float>(v[0])), static_cast<float>(x)); }, "quantile", "Boost[br]float[br]promote_float<false>", true, distribution_tester::boost_only_table);
}

template <template <class T, class U> class D>
void test_boost_2_param(distribution_tester& tester)
{
   //
   // Define some custom policies to test:
   //
   typedef boost::math::policies::policy<> default_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_double<false> > no_promote_double_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_double<false>, boost::math::policies::digits10<10> > no_promote_double_10_digits_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_float<false> > no_promote_float_policy;

   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, default_policy>(v[0], v[1]), x); }, "PDF", boost_name(), false, distribution_tester::both_tables);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, default_policy>(v[0], v[1]), x); }, "CDF", boost_name(), false, distribution_tester::both_tables);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, default_policy>(v[0], v[1]), x); }, "quantile", boost_name(), true, distribution_tester::both_tables);
   if(sizeof(double) != sizeof(long double))
   {
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, no_promote_double_policy>(v[0], v[1]), x); }, "PDF", "Boost[br]promote_double<false>", false, distribution_tester::both_tables);
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, no_promote_double_policy>(v[0], v[1]), x); }, "CDF", "Boost[br]promote_double<false>", false, distribution_tester::both_tables);
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, no_promote_double_policy>(v[0], v[1]), x); }, "quantile", "Boost[br]promote_double<false>", true, distribution_tester::both_tables);
   }
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, no_promote_double_10_digits_policy>(v[0], v[1]), x); }, "PDF", "Boost[br]promote_double<false>[br]digits10<10>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, no_promote_double_10_digits_policy>(v[0], v[1]), x); }, "CDF", "Boost[br]promote_double<false>[br]digits10<10>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, no_promote_double_10_digits_policy>(v[0], v[1]), x); }, "quantile", "Boost[br]promote_double<false>[br]digits10<10>", true, distribution_tester::boost_only_table);

   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<float, no_promote_float_policy>(static_cast<float>(v[0]), static_cast<float>(v[1])), static_cast<float>(x)); }, "PDF", "Boost[br]float[br]promote_float<false>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<float, no_promote_float_policy>(static_cast<float>(v[0]), static_cast<float>(v[1])), static_cast<float>(x)); }, "CDF", "Boost[br]float[br]promote_float<false>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<float, no_promote_float_policy>(static_cast<float>(v[0]), static_cast<float>(v[1])), static_cast<float>(x)); }, "quantile", "Boost[br]float[br]promote_float<false>", true, distribution_tester::boost_only_table);
}

template <template <class T, class U> class D>
void test_boost_3_param(distribution_tester& tester)
{
   //
   // Define some custom policies to test:
   //
   typedef boost::math::policies::policy<> default_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_double<false> > no_promote_double_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_double<false>, boost::math::policies::digits10<10> > no_promote_double_10_digits_policy;
   typedef boost::math::policies::policy<boost::math::policies::promote_float<false> > no_promote_float_policy;

   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, default_policy>(v[0], v[1], v[2]), x); }, "PDF", boost_name(), false, distribution_tester::both_tables);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, default_policy>(v[0], v[1], v[2]), x); }, "CDF", boost_name(), false, distribution_tester::both_tables);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, default_policy>(v[0], v[1], v[2]), x); }, "quantile", boost_name(), true, distribution_tester::both_tables);
   if(sizeof(double) != sizeof(long double))
   {
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, no_promote_double_policy>(v[0], v[1], v[2]), x); }, "PDF", "Boost[br]promote_double<false>", false, distribution_tester::both_tables);
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, no_promote_double_policy>(v[0], v[1], v[2]), x); }, "CDF", "Boost[br]promote_double<false>", false, distribution_tester::both_tables);
      tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, no_promote_double_policy>(v[0], v[1], v[2]), x); }, "quantile", "Boost[br]promote_double<false>", true, distribution_tester::both_tables);
   }
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<double, no_promote_double_10_digits_policy>(v[0], v[1], v[2]), x); }, "PDF", "Boost[br]promote_double<false>[br]digits10<10>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<double, no_promote_double_10_digits_policy>(v[0], v[1], v[2]), x); }, "CDF", "Boost[br]promote_double<false>[br]digits10<10>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<double, no_promote_double_10_digits_policy>(v[0], v[1], v[2]), x); }, "quantile", "Boost[br]promote_double<false>[br]digits10<10>", true, distribution_tester::boost_only_table);

   tester.run_timed_tests([](const std::vector<double>& v, double x){  return pdf(D<float, no_promote_float_policy>(static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2])), static_cast<float>(x)); }, "PDF", "Boost[br]float[br]promote_float<false>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return cdf(D<float, no_promote_float_policy>(static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2])), static_cast<float>(x)); }, "CDF", "Boost[br]float[br]promote_float<false>", false, distribution_tester::boost_only_table);
   tester.run_timed_tests([](const std::vector<double>& v, double x){  return quantile(D<float, no_promote_float_policy>(static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2])), static_cast<float>(x)); }, "quantile", "Boost[br]float[br]promote_float<false>", true, distribution_tester::boost_only_table);
}

int main()
{
   try {
      //
      // Normal:
      //
      distribution_tester n("Normal");
      n.add_test_case(0, 1, two_param_quantile<boost::math::normal_distribution<> >());
      n.add_test_case(20, 20, two_param_quantile<boost::math::normal_distribution<> >());
      n.add_test_case(-20, 0.0125, two_param_quantile<boost::math::normal_distribution<> >());

      test_boost_2_param<boost::math::normal_distribution>(n);

      distribution_tester arcsine("ArcSine");
      arcsine.add_test_case(0, 1, two_param_quantile<boost::math::arcsine_distribution<> >());
      arcsine.add_test_case(20, 500, two_param_quantile<boost::math::arcsine_distribution<> >());
      arcsine.add_test_case(-20, 100000, two_param_quantile<boost::math::arcsine_distribution<> >());

      test_boost_2_param<boost::math::arcsine_distribution>(arcsine);

      distribution_tester beta("Beta");
      beta.add_test_case(1, 4, two_param_quantile<boost::math::beta_distribution<> >());
      beta.add_test_case(20, 500, two_param_quantile<boost::math::beta_distribution<> >());
      beta.add_test_case(0.1, 0.01, two_param_quantile<boost::math::beta_distribution<> >());

      test_boost_2_param<boost::math::beta_distribution>(beta);

      distribution_tester binomial("Binomial");
      binomial.add_test_case(5, 0.125, two_param_quantile<boost::math::binomial_distribution<> >());
      binomial.add_test_case(200, 0.75, two_param_quantile<boost::math::binomial_distribution<> >());
      binomial.add_test_case(2000, 0.5, two_param_quantile<boost::math::binomial_distribution<> >());
      binomial.add_test_case(20000, 0.001, two_param_quantile<boost::math::binomial_distribution<> >());
      binomial.add_test_case(200000, 0.99, two_param_quantile<boost::math::binomial_distribution<> >());

      test_boost_2_param<boost::math::binomial_distribution>(binomial);

      distribution_tester cauchy("Cauchy");
      cauchy.add_test_case(0, 1, two_param_quantile<boost::math::cauchy_distribution<> >());
      cauchy.add_test_case(20, 20, two_param_quantile<boost::math::cauchy_distribution<> >());
      cauchy.add_test_case(-20, 0.0125, two_param_quantile<boost::math::cauchy_distribution<> >());

      test_boost_2_param<boost::math::cauchy_distribution>(cauchy);

      distribution_tester chi_squared("ChiSquared");
      chi_squared.add_test_case(3, one_param_quantile<boost::math::chi_squared_distribution<> >());
      chi_squared.add_test_case(20, one_param_quantile<boost::math::chi_squared_distribution<> >());
      chi_squared.add_test_case(200, one_param_quantile<boost::math::chi_squared_distribution<> >());
      chi_squared.add_test_case(2000, one_param_quantile<boost::math::chi_squared_distribution<> >());
      chi_squared.add_test_case(20000, one_param_quantile<boost::math::chi_squared_distribution<> >());
      chi_squared.add_test_case(200000, one_param_quantile<boost::math::chi_squared_distribution<> >());

      test_boost_1_param<boost::math::chi_squared_distribution>(chi_squared);

      distribution_tester exponential("Exponential");
      exponential.add_test_case(0.001, one_param_quantile<boost::math::exponential_distribution<> >());
      exponential.add_test_case(0.01, one_param_quantile<boost::math::exponential_distribution<> >());
      exponential.add_test_case(0.1, one_param_quantile<boost::math::exponential_distribution<> >());
      exponential.add_test_case(1, one_param_quantile<boost::math::exponential_distribution<> >());
      exponential.add_test_case(10, one_param_quantile<boost::math::exponential_distribution<> >());
      exponential.add_test_case(100, one_param_quantile<boost::math::exponential_distribution<> >());
      exponential.add_test_case(1000, one_param_quantile<boost::math::exponential_distribution<> >());

      test_boost_1_param<boost::math::exponential_distribution>(exponential);

      distribution_tester extreme_value("ExtremeValue");
      extreme_value.add_test_case(0, 1, two_param_quantile<boost::math::extreme_value_distribution<> >());
      extreme_value.add_test_case(20, 20, two_param_quantile<boost::math::extreme_value_distribution<> >());
      extreme_value.add_test_case(-20, 0.0125, two_param_quantile<boost::math::extreme_value_distribution<> >());

      test_boost_2_param<boost::math::extreme_value_distribution>(extreme_value);

      distribution_tester fisher("F");
      for (unsigned i = 2; i <= 200000; i *= 10)
      {
         for (unsigned j = 2; j <= 200000; j *= 10)
         {
            fisher.add_test_case(i, j, two_param_quantile<boost::math::fisher_f_distribution<> >());
         }
      }
      test_boost_2_param<boost::math::fisher_f_distribution>(fisher);

      distribution_tester gamma("Gamma");
      gamma.add_test_case(0.1, 1, two_param_quantile<boost::math::gamma_distribution<> >());
      gamma.add_test_case(20, 20, two_param_quantile<boost::math::gamma_distribution<> >());
      gamma.add_test_case(200, 0.0125, two_param_quantile<boost::math::gamma_distribution<> >());
      gamma.add_test_case(2000, 500, two_param_quantile<boost::math::gamma_distribution<> >());

      test_boost_2_param<boost::math::gamma_distribution>(gamma);

      distribution_tester geometric("Geometric");
      geometric.add_test_case(0.001, one_param_quantile<boost::math::geometric_distribution<> >());
      geometric.add_test_case(0.01, one_param_quantile<boost::math::geometric_distribution<> >());
      geometric.add_test_case(0.1, one_param_quantile<boost::math::geometric_distribution<> >());
      geometric.add_test_case(0.5, one_param_quantile<boost::math::geometric_distribution<> >());
      geometric.add_test_case(0.9, one_param_quantile<boost::math::geometric_distribution<> >());
      geometric.add_test_case(0.99, one_param_quantile<boost::math::geometric_distribution<> >());
      geometric.add_test_case(0.999, one_param_quantile<boost::math::geometric_distribution<> >());

      test_boost_1_param<boost::math::geometric_distribution>(geometric);

      distribution_tester hypergeometric("Hypergeometric");
      hypergeometric.add_test_case(10, 5, 100, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(50, 75, 100, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(30, 20, 100, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(100, 50, 1000000, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(500000, 3000, 1000000, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(20000, 800000, 1000000, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(100, 5, 1000, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(500, 50, 1000, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(2, 25, 1000, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(1, 5, 1000, three_param_quantile<boost::math::hypergeometric_distribution<> >());
      hypergeometric.add_test_case(100, 500, 1000, three_param_quantile<boost::math::hypergeometric_distribution<> >());

      test_boost_3_param<boost::math::hypergeometric_distribution>(hypergeometric);

      distribution_tester inverse_chi_squared("InverseChiSquared");
      inverse_chi_squared.add_test_case(5, 0.125, two_param_quantile<boost::math::inverse_chi_squared_distribution<> >());
      inverse_chi_squared.add_test_case(200, 0.75, two_param_quantile<boost::math::inverse_chi_squared_distribution<> >());
      inverse_chi_squared.add_test_case(2000, 1, two_param_quantile<boost::math::inverse_chi_squared_distribution<> >());
      inverse_chi_squared.add_test_case(20000, 10, two_param_quantile<boost::math::inverse_chi_squared_distribution<> >());
      inverse_chi_squared.add_test_case(200000, 100, two_param_quantile<boost::math::inverse_chi_squared_distribution<> >());

      test_boost_2_param<boost::math::inverse_chi_squared_distribution>(inverse_chi_squared);

      distribution_tester inverse_gamma("InverseGamma");
      inverse_gamma.add_test_case(0.1, 1, two_param_quantile<boost::math::inverse_gamma_distribution<> >());
      inverse_gamma.add_test_case(20, 20, two_param_quantile<boost::math::inverse_gamma_distribution<> >());
      inverse_gamma.add_test_case(200, 0.0125, two_param_quantile<boost::math::inverse_gamma_distribution<> >());
      inverse_gamma.add_test_case(2000, 500, two_param_quantile<boost::math::inverse_gamma_distribution<> >());

      test_boost_2_param<boost::math::inverse_gamma_distribution>(inverse_gamma);

      distribution_tester inverse_gaussian("InverseGaussian");
      inverse_gaussian.add_test_case(0.001, 1, two_param_quantile<boost::math::inverse_gaussian_distribution<> >());
      inverse_gaussian.add_test_case(20, 20, two_param_quantile<boost::math::inverse_gaussian_distribution<> >());

      test_boost_2_param<boost::math::inverse_gaussian_distribution>(inverse_gaussian);

      distribution_tester kolmogorov("KolmogorovSmirnov");
      kolmogorov.add_test_case(3, one_param_quantile<boost::math::kolmogorov_smirnov_distribution<> >());
      kolmogorov.add_test_case(20, one_param_quantile<boost::math::kolmogorov_smirnov_distribution<> >());
      kolmogorov.add_test_case(200, one_param_quantile<boost::math::kolmogorov_smirnov_distribution<> >());
      kolmogorov.add_test_case(2000, one_param_quantile<boost::math::kolmogorov_smirnov_distribution<> >());
      kolmogorov.add_test_case(20000, one_param_quantile<boost::math::kolmogorov_smirnov_distribution<> >());
      kolmogorov.add_test_case(200000, one_param_quantile<boost::math::kolmogorov_smirnov_distribution<> >());

      test_boost_1_param<boost::math::kolmogorov_smirnov_distribution>(kolmogorov);

      distribution_tester laplace("Laplace");
      laplace.add_test_case(0, 1, two_param_quantile<boost::math::laplace_distribution<> >());
      laplace.add_test_case(20, 20, two_param_quantile<boost::math::laplace_distribution<> >());
      laplace.add_test_case(-20, 0.0125, two_param_quantile<boost::math::laplace_distribution<> >());

      test_boost_2_param<boost::math::laplace_distribution>(laplace);

      distribution_tester logistic("Logistic");
      logistic.add_test_case(0, 1, two_param_quantile<boost::math::logistic_distribution<> >());
      logistic.add_test_case(20, 20, two_param_quantile<boost::math::logistic_distribution<> >());
      logistic.add_test_case(-20, 0.0125, two_param_quantile<boost::math::logistic_distribution<> >());

      test_boost_2_param<boost::math::logistic_distribution>(logistic);

      distribution_tester lognormal("LogNormal");
      lognormal.add_test_case(0, 1, two_param_quantile<boost::math::lognormal_distribution<> >());
      lognormal.add_test_case(20, 20, two_param_quantile<boost::math::lognormal_distribution<> >());
      lognormal.add_test_case(-20, 0.0125, two_param_quantile<boost::math::lognormal_distribution<> >());

      test_boost_2_param<boost::math::lognormal_distribution>(lognormal);

      distribution_tester negative_binomial("NegativeBinomial");
      negative_binomial.add_test_case(5, 0.125, two_param_quantile<boost::math::negative_binomial_distribution<> >());
      negative_binomial.add_test_case(200, 0.75, two_param_quantile<boost::math::negative_binomial_distribution<> >());
      negative_binomial.add_test_case(2000, 0.001, two_param_quantile<boost::math::negative_binomial_distribution<> >());
      negative_binomial.add_test_case(20000, 0.5, two_param_quantile<boost::math::negative_binomial_distribution<> >());
      negative_binomial.add_test_case(200000, 0.99, two_param_quantile<boost::math::negative_binomial_distribution<> >());

      test_boost_2_param<boost::math::negative_binomial_distribution>(negative_binomial);

      distribution_tester non_central_beta("NonCentralBeta");
      non_central_beta.add_test_case(2, 5, 2.1, three_param_quantile<boost::math::non_central_beta_distribution<> >());
      non_central_beta.add_test_case(0.25, 0.01, 20, three_param_quantile<boost::math::non_central_beta_distribution<> >());
      non_central_beta.add_test_case(20, 3, 30, three_param_quantile<boost::math::non_central_beta_distribution<> >());
      non_central_beta.add_test_case(100, 200, 400, three_param_quantile<boost::math::non_central_beta_distribution<> >());
      non_central_beta.add_test_case(100, 0.25, 20, three_param_quantile<boost::math::non_central_beta_distribution<> >());

      test_boost_3_param<boost::math::non_central_beta_distribution>(non_central_beta);

      distribution_tester non_central_chi_squared("NonCentralChiSquared");
      non_central_chi_squared.add_test_case(5, 0.5, two_param_quantile<boost::math::non_central_chi_squared_distribution<> >());
      non_central_chi_squared.add_test_case(200, 2, two_param_quantile<boost::math::non_central_chi_squared_distribution<> >());
      non_central_chi_squared.add_test_case(2000, 20, two_param_quantile<boost::math::non_central_chi_squared_distribution<> >());
      non_central_chi_squared.add_test_case(20000, 10, two_param_quantile<boost::math::non_central_chi_squared_distribution<> >());
      non_central_chi_squared.add_test_case(200000, 50, two_param_quantile<boost::math::non_central_chi_squared_distribution<> >());

      test_boost_2_param<boost::math::non_central_chi_squared_distribution>(non_central_chi_squared);

      distribution_tester non_central_f("NonCentralF");
      non_central_f.add_test_case(20, 20, 3, three_param_quantile<boost::math::non_central_f_distribution<> >());
      non_central_f.add_test_case(20, 50, 20, three_param_quantile<boost::math::non_central_f_distribution<> >());
      non_central_f.add_test_case(100, 20, 30, three_param_quantile<boost::math::non_central_f_distribution<> >());
      non_central_f.add_test_case(100, 200, 100, three_param_quantile<boost::math::non_central_f_distribution<> >());
      non_central_f.add_test_case(1000, 100000, 20, three_param_quantile<boost::math::non_central_f_distribution<> >());

      test_boost_3_param<boost::math::non_central_f_distribution>(non_central_f);

      distribution_tester non_central_t("NonCentralT");
      non_central_t.add_test_case(5, 0.5, two_param_quantile<boost::math::non_central_t_distribution<> >());
      non_central_t.add_test_case(200, 2, two_param_quantile<boost::math::non_central_t_distribution<> >());
      non_central_t.add_test_case(2000, 20, two_param_quantile<boost::math::non_central_t_distribution<> >());
      non_central_t.add_test_case(20000, 10, two_param_quantile<boost::math::non_central_t_distribution<> >());
      non_central_t.add_test_case(200000, 50, two_param_quantile<boost::math::non_central_t_distribution<> >());

      test_boost_2_param<boost::math::non_central_t_distribution>(non_central_t);

      distribution_tester pareto("Pareto");
      pareto.add_test_case(0.1, 1, two_param_quantile<boost::math::pareto_distribution<> >());
      pareto.add_test_case(20, 20, two_param_quantile<boost::math::pareto_distribution<> >());
      pareto.add_test_case(200, 0.0125, two_param_quantile<boost::math::pareto_distribution<> >());
      pareto.add_test_case(2000, 500, two_param_quantile<boost::math::pareto_distribution<> >());

      test_boost_2_param<boost::math::pareto_distribution>(pareto);

      distribution_tester poisson("Poisson");
      poisson.add_test_case(0.001, one_param_quantile<boost::math::poisson_distribution<> >());
      poisson.add_test_case(0.01, one_param_quantile<boost::math::poisson_distribution<> >());
      poisson.add_test_case(0.1, one_param_quantile<boost::math::poisson_distribution<> >());
      poisson.add_test_case(1, one_param_quantile<boost::math::poisson_distribution<> >());
      poisson.add_test_case(10, one_param_quantile<boost::math::poisson_distribution<> >());
      poisson.add_test_case(100, one_param_quantile<boost::math::poisson_distribution<> >());
      poisson.add_test_case(1000, one_param_quantile<boost::math::poisson_distribution<> >());

      test_boost_1_param<boost::math::poisson_distribution>(poisson);

      distribution_tester rayleigh("Rayleigh");
      rayleigh.add_test_case(0.001, one_param_quantile<boost::math::rayleigh_distribution<> >());
      rayleigh.add_test_case(0.01, one_param_quantile<boost::math::rayleigh_distribution<> >());
      rayleigh.add_test_case(0.1, one_param_quantile<boost::math::rayleigh_distribution<> >());
      rayleigh.add_test_case(1, one_param_quantile<boost::math::rayleigh_distribution<> >());
      rayleigh.add_test_case(10, one_param_quantile<boost::math::rayleigh_distribution<> >());
      rayleigh.add_test_case(100, one_param_quantile<boost::math::rayleigh_distribution<> >());
      rayleigh.add_test_case(1000, one_param_quantile<boost::math::rayleigh_distribution<> >());

      test_boost_1_param<boost::math::rayleigh_distribution>(rayleigh);

      distribution_tester skew_norm("SkewNormal");
      skew_norm.add_test_case(0, 1, 0.1, three_param_quantile<boost::math::skew_normal_distribution<> >());
      skew_norm.add_test_case(20, 20, 30, three_param_quantile<boost::math::skew_normal_distribution<> >());
      skew_norm.add_test_case(-20, 0.0125, 10, three_param_quantile<boost::math::skew_normal_distribution<> >());

      test_boost_3_param<boost::math::skew_normal_distribution>(skew_norm);

      distribution_tester students_t("StudentsT");
      students_t.add_test_case(3, one_param_quantile<boost::math::students_t_distribution<> >());
      students_t.add_test_case(20, one_param_quantile<boost::math::students_t_distribution<> >());
      students_t.add_test_case(200, one_param_quantile<boost::math::students_t_distribution<> >());
      students_t.add_test_case(2000, one_param_quantile<boost::math::students_t_distribution<> >());
      students_t.add_test_case(20000, one_param_quantile<boost::math::students_t_distribution<> >());
      students_t.add_test_case(200000, one_param_quantile<boost::math::students_t_distribution<> >());

      test_boost_1_param<boost::math::students_t_distribution>(students_t);

      distribution_tester weibull("Weibull");
      weibull.add_test_case(0.1, 1, two_param_quantile<boost::math::weibull_distribution<> >());
      weibull.add_test_case(20, 20, two_param_quantile<boost::math::weibull_distribution<> >());
      weibull.add_test_case(200, 0.0125, two_param_quantile<boost::math::weibull_distribution<> >());
      weibull.add_test_case(2000, 500, two_param_quantile<boost::math::weibull_distribution<> >());

      test_boost_2_param<boost::math::weibull_distribution>(weibull);

#ifdef TEST_GSL
      // normal, note no location param
      n.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_gaussian_P(x, v[1]); }, "CDF", "GSL");
      n.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_gaussian_Pinv(x, v[1]); }, "quantile", "GSL", true);
      // exponential:
      exponential.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_exponential_P(x, 1 / v[0]); }, "CDF", "GSL");
      exponential.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_exponential_Pinv(x, 1 / v[0]); }, "quantile", "GSL", true);
      // laplace, note no location param:
      laplace.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_laplace_P(x, v[1]); }, "CDF", "GSL");
      laplace.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_laplace_Pinv(x, v[1]); }, "quantile", "GSL", true);
      // cauchy, note no location param:
      cauchy.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_cauchy_P(x, v[1]); }, "CDF", "GSL");
      cauchy.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_cauchy_Pinv(x, v[1]); }, "quantile", "GSL", true);
      // rayleigh:
      rayleigh.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_rayleigh_P(x, v[0]); }, "CDF", "GSL");
      rayleigh.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_rayleigh_Pinv(x, v[0]); }, "quantile", "GSL", true);
      // gamma:
      gamma.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_gamma_P(x, v[0], v[1]); }, "CDF", "GSL");
      gamma.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_gamma_Pinv(x, v[0], v[1]); }, "quantile", "GSL", true);
      // lognormal:
      lognormal.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_lognormal_P(x, v[0], v[1]); }, "CDF", "GSL");
      lognormal.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_lognormal_Pinv(x, v[0], v[1]); }, "quantile", "GSL", true);
      // chi squared:
      chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_chisq_P(x, v[0]); }, "CDF", "GSL");
      chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_chisq_Pinv(x, v[0]); }, "quantile", "GSL", true);
      // F:
      fisher.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_fdist_P(x, v[0], v[1]); }, "CDF", "GSL");
      fisher.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_fdist_Pinv(x, v[0], v[1]); }, "quantile", "GSL", true);
      // T:
      students_t.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_tdist_P(x, v[0]); }, "CDF", "GSL");
      students_t.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_tdist_Pinv(x, v[0]); }, "quantile", "GSL", true);
      // beta:
      beta.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_beta_P(x, v[0], v[1]); }, "CDF", "GSL");
      beta.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_beta_Pinv(x, v[0], v[1]); }, "quantile", "GSL", true);
      // logistic, note no location param
      logistic.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_logistic_P(x, v[1]); }, "CDF", "GSL");
      logistic.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_logistic_Pinv(x, v[1]); }, "quantile", "GSL", true);
      // pareto:
      pareto.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_pareto_P(x, v[1], v[0]); }, "CDF", "GSL");
      pareto.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_pareto_Pinv(x, v[1], v[0]); }, "quantile", "GSL", true);
      // weibull:
      weibull.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_weibull_P(x, v[1], v[0]); }, "CDF", "GSL");
      weibull.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_weibull_Pinv(x, v[1], v[0]); }, "quantile", "GSL", true);
      // poisson:
      poisson.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_poisson_P(x, v[0]); }, "CDF", "GSL");
      //poisson.run_timed_tests([](const std::vector<double>& v, double x){  return gsl_cdf_poisson_Pinv(x, v[0]); }, "quantile", "GSL", true);
      // binomial:
      binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_binomial_P(x, v[1], v[0]); }, "CDF", "GSL");
      //binomial.run_timed_tests([](const std::vector<double>& v, double x){  return gsl_cdf_binomial_Pinv(x, v[1], v[0]); }, "quantile", "GSL", true);
      // negative_binomial:
      negative_binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_negative_binomial_P(x, v[1], v[0]); }, "CDF", "GSL");
      //negative_binomial.run_timed_tests([](const std::vector<double>& v, double x){  return gsl_cdf_negative_binomial_Pinv(x, v[1], v[0]); }, "quantile", "GSL", true);
      // geometric:
      geometric.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_geometric_P(x + 1, v[0]); }, "CDF", "GSL");
      //geometric.run_timed_tests([](const std::vector<double>& v, double x){  return gsl_cdf_geometric_Pinv(x, v[0]) - 1; }, "quantile", "GSL", true);
      // hypergeometric:
      hypergeometric.run_timed_tests([](const std::vector<double>& v, double x) {  return gsl_cdf_hypergeometric_P(x, v[0], v[2] - v[0], v[1]); }, "CDF", "GSL");
      //hypergeometric.run_timed_tests([](const std::vector<double>& v, double x){  return gsl_cdf_hypergeometric_Pinv(x, v[0], v[2] - v[0], v[1]); }, "quantile", "GSL", true);
#endif

#ifdef TEST_RMATH
   // beta
      beta.run_timed_tests([](const std::vector<double>& v, double x) {  return dbeta(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      beta.run_timed_tests([](const std::vector<double>& v, double x) {  return pbeta(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      beta.run_timed_tests([](const std::vector<double>& v, double x) {  return qbeta(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // non-central beta
      non_central_beta.run_timed_tests([](const std::vector<double>& v, double x) {  return dnbeta(x, v[0], v[1], v[2], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      non_central_beta.run_timed_tests([](const std::vector<double>& v, double x) {  return pnbeta(x, v[0], v[1], v[2], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      non_central_beta.run_timed_tests([](const std::vector<double>& v, double x) {  return qnbeta(x, v[0], v[1], v[2], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // binomial
      binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return dbinom(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return pbinom(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return qbinom(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // cauchy
      cauchy.run_timed_tests([](const std::vector<double>& v, double x) {  return dcauchy(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      cauchy.run_timed_tests([](const std::vector<double>& v, double x) {  return pcauchy(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      cauchy.run_timed_tests([](const std::vector<double>& v, double x) {  return qcauchy(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // chi squared
      chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return dchisq(x, v[0], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return pchisq(x, v[0], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return qchisq(x, v[0], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // non central chi squared
      non_central_chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return dnchisq(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      non_central_chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return pnchisq(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      non_central_chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return qnchisq(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // exponential
      exponential.run_timed_tests([](const std::vector<double>& v, double x) {  return dexp(x, 1 / v[0], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      exponential.run_timed_tests([](const std::vector<double>& v, double x) {  return pexp(x, 1 / v[0], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      exponential.run_timed_tests([](const std::vector<double>& v, double x) {  return qexp(x, 1 / v[0], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // F
      fisher.run_timed_tests([](const std::vector<double>& v, double x) {  return df(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      fisher.run_timed_tests([](const std::vector<double>& v, double x) {  return pf(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      fisher.run_timed_tests([](const std::vector<double>& v, double x) {  return qf(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // non central F
      non_central_f.run_timed_tests([](const std::vector<double>& v, double x) {  return dnf(x, v[0], v[1], v[2], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      non_central_f.run_timed_tests([](const std::vector<double>& v, double x) {  return pnf(x, v[0], v[1], v[2], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      non_central_f.run_timed_tests([](const std::vector<double>& v, double x) {  return qnf(x, v[0], v[1], v[2], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // gamma
      gamma.run_timed_tests([](const std::vector<double>& v, double x) {  return dgamma(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      gamma.run_timed_tests([](const std::vector<double>& v, double x) {  return pgamma(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      gamma.run_timed_tests([](const std::vector<double>& v, double x) {  return qgamma(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // geometric
      geometric.run_timed_tests([](const std::vector<double>& v, double x) {  return dgeom(x, v[0], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      geometric.run_timed_tests([](const std::vector<double>& v, double x) {  return pgeom(x, v[0], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      geometric.run_timed_tests([](const std::vector<double>& v, double x) {  return qgeom(x, v[0], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // hypergeometric
      hypergeometric.run_timed_tests([](const std::vector<double>& v, double x) {  return dhyper(x, v[0], v[2] - v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      hypergeometric.run_timed_tests([](const std::vector<double>& v, double x) {  return phyper(x, v[0], v[2] - v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      hypergeometric.run_timed_tests([](const std::vector<double>& v, double x) {  return qhyper(x, v[0], v[2] - v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // logistic
      logistic.run_timed_tests([](const std::vector<double>& v, double x) {  return dlogis(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      logistic.run_timed_tests([](const std::vector<double>& v, double x) {  return plogis(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      logistic.run_timed_tests([](const std::vector<double>& v, double x) {  return qlogis(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // lognormal
      lognormal.run_timed_tests([](const std::vector<double>& v, double x) {  return dlnorm(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      lognormal.run_timed_tests([](const std::vector<double>& v, double x) {  return plnorm(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      lognormal.run_timed_tests([](const std::vector<double>& v, double x) {  return qlnorm(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // negative_binomial
      negative_binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return dnbinom(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      negative_binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return pnbinom(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      negative_binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return qnbinom(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // normal
      n.run_timed_tests([](const std::vector<double>& v, double x) {  return dnorm(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      n.run_timed_tests([](const std::vector<double>& v, double x) {  return pnorm(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      n.run_timed_tests([](const std::vector<double>& v, double x) {  return qnorm(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // poisson
      poisson.run_timed_tests([](const std::vector<double>& v, double x) {  return dpois(x, v[0], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      poisson.run_timed_tests([](const std::vector<double>& v, double x) {  return ppois(x, v[0], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      poisson.run_timed_tests([](const std::vector<double>& v, double x) {  return qpois(x, v[0], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // T
      students_t.run_timed_tests([](const std::vector<double>& v, double x) {  return dt(x, v[0], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      students_t.run_timed_tests([](const std::vector<double>& v, double x) {  return pt(x, v[0], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      students_t.run_timed_tests([](const std::vector<double>& v, double x) {  return qt(x, v[0], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // non central T
      non_central_t.run_timed_tests([](const std::vector<double>& v, double x) {  return dnt(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      non_central_t.run_timed_tests([](const std::vector<double>& v, double x) {  return pnt(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      non_central_t.run_timed_tests([](const std::vector<double>& v, double x) {  return qnt(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);
      // weibull
      weibull.run_timed_tests([](const std::vector<double>& v, double x) {  return dweibull(x, v[0], v[1], 0); }, "PDF", "Rmath "  R_VERSION_STRING);
      weibull.run_timed_tests([](const std::vector<double>& v, double x) {  return pweibull(x, v[0], v[1], 1, 0); }, "CDF", "Rmath "  R_VERSION_STRING);
      weibull.run_timed_tests([](const std::vector<double>& v, double x) {  return qweibull(x, v[0], v[1], 1, 0); }, "quantile", "Rmath "  R_VERSION_STRING, true);

#endif

#ifdef TEST_DCDFLIB
      n.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_norm_cdf(x, v[0], v[1]); }, "CDF", "DCDFLIB");
      n.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_norm_quantile(x, v[0], v[1]); }, "quantile", "DCDFLIB", true);

      beta.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_beta_cdf(x, v[0], v[1]); }, "CDF", "DCDFLIB");
      beta.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_beta_quantile(x, v[0], v[1]); }, "quantile", "DCDFLIB", true);

      binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_binomial_cdf(x, v[0], v[1]); }, "CDF", "DCDFLIB");
      binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_binomial_quantile(x, v[0], v[1]); }, "quantile", "DCDFLIB", true);

      chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_chi_cdf(x, v[0]); }, "CDF", "DCDFLIB");
      chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_chi_quantile(x, v[0]); }, "quantile", "DCDFLIB", true);

      non_central_chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_chi_n_cdf(x, v[0], v[1]); }, "CDF", "DCDFLIB");
      non_central_chi_squared.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_chi_n_quantile(x, v[0], v[1]); }, "quantile", "DCDFLIB", true);

      fisher.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_f_cdf(x, v[0], v[1]); }, "CDF", "DCDFLIB");
      fisher.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_f_quantile(x, v[0], v[1]); }, "quantile", "DCDFLIB", true);

      non_central_f.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_f_n_cdf(x, v[0], v[1], v[2]); }, "CDF", "DCDFLIB");
      non_central_f.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_f_n_quantile(x, v[0], v[1], v[2]); }, "quantile", "DCDFLIB", true);

      gamma.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_gamma_cdf(x, v[0], v[1]); }, "CDF", "DCDFLIB");
      gamma.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_gamma_quantile(x, v[0], v[1]); }, "quantile", "DCDFLIB", true);

      negative_binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_nbin_cdf(x, v[0], v[1]); }, "CDF", "DCDFLIB");
      negative_binomial.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_nbin_quantile(x, v[0], v[1]); }, "quantile", "DCDFLIB", true);

      poisson.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_poisson_cdf(x, v[0]); }, "CDF", "DCDFLIB");
      poisson.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_poisson_quantile(x, v[0]); }, "quantile", "DCDFLIB", true);

      students_t.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_t_cdf(x, v[0]); }, "CDF", "DCDFLIB");
      students_t.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_t_quantile(x, v[0]); }, "quantile", "DCDFLIB", true);

      //non_central_t.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_t_n_cdf(x, v[0], v[1]); }, "CDF", "DCDFLIB");
      //non_central_t.run_timed_tests([](const std::vector<double>& v, double x) {  return dcdflib_t_n_quantile(x, v[0], v[1]); }, "quantile", "DCDFLIB", true);
#endif

   }
   catch(const std::exception& e)
   {
      std::cout << "Test run aborted due to thrown exception: " << e.what() << std::endl;
      return 1;
   }
   return 0;
}


/*
 * Copyright (c) 2010-2020 OTClient <https://github.com/edubart/otclient>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "crypt.h"
#include <framework/stdext/math.h>
#include <framework/core/logger.h>
#include <framework/core/resourcemanager.h>
#include <framework/platform/platform.h>
#include <framework/core/application.h>

#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>

#include <boost/functional/hash.hpp>

#ifndef USE_GMP
#include <openssl/rsa.h>
#include <openssl/bn.h>
#include <openssl/err.h>
#endif

static const std::string base64_chars = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
static inline bool is_base64(unsigned char c) { return (isalnum(c) || (c == '+') || (c == '/')); }

Crypt g_crypt;

Crypt::Crypt()
{
#ifdef USE_GMP
    mpz_init(m_p);
    mpz_init(m_q);
    mpz_init(m_d);
    mpz_init(m_e);
    mpz_init(m_n);
#else
    m_rsa = RSA_new();
#endif
}

Crypt::~Crypt()
{
#ifdef USE_GMP
    mpz_clear(m_p);
    mpz_clear(m_q);
    mpz_clear(m_n);
    mpz_clear(m_d);
    mpz_clear(m_e);
#else
    RSA_free(m_rsa);
#endif
}

std::string Crypt::base64Encode(const std::string& decoded_string)
{
    std::string ret;
    int i = 0;
    int j = 0;
    uint8 char_array_3[3];
    uint8 char_array_4[4];
    int pos = 0;
    int len = decoded_string.size();

    while(len--) {
        char_array_3[i++] = decoded_string[pos++];
        if(i == 3) {
            char_array_4[0] = (char_array_3[0] & 0xfc) >> 2;
            char_array_4[1] = ((char_array_3[0] & 0x03) << 4) + ((char_array_3[1] & 0xf0) >> 4);
            char_array_4[2] = ((char_array_3[1] & 0x0f) << 2) + ((char_array_3[2] & 0xc0) >> 6);
            char_array_4[3] = char_array_3[2] & 0x3f;

            for(i = 0; (i < 4); i++)
                ret += base64_chars[char_array_4[i]];
            i = 0;
        }
    }

    if(i) {
        for(j = i; j < 3; j++)
            char_array_3[j] = '\0';

        char_array_4[0] = (char_array_3[0] & 0xfc) >> 2;
        char_array_4[1] = ((char_array_3[0] & 0x03) << 4) + ((char_array_3[1] & 0xf0) >> 4);
        char_array_4[2] = ((char_array_3[1] & 0x0f) << 2) + ((char_array_3[2] & 0xc0) >> 6);
        char_array_4[3] = char_array_3[2] & 0x3f;

        for(j = 0; (j < i + 1); j++)
            ret += base64_chars[char_array_4[j]];

        while((i++ < 3))
            ret += '=';
    }

    return ret;
}

std::string Crypt::base64Decode(const std::string& encoded_string)
{
    int len = encoded_string.size();
    int i = 0;
    int j = 0;
    int in_ = 0;
    uint8 char_array_4[4], char_array_3[3];
    std::string ret;

    while(len-- && (encoded_string[in_] != '=') && is_base64(encoded_string[in_])) {
        char_array_4[i++] = encoded_string[in_]; in_++;
        if(i == 4) {
            for(i = 0; i < 4; i++)
                char_array_4[i] = base64_chars.find(char_array_4[i]);

            char_array_3[0] = (char_array_4[0] << 2) + ((char_array_4[1] & 0x30) >> 4);
            char_array_3[1] = ((char_array_4[1] & 0xf) << 4) + ((char_array_4[2] & 0x3c) >> 2);
            char_array_3[2] = ((char_array_4[2] & 0x3) << 6) + char_array_4[3];

            for(i = 0; (i < 3); i++)
                ret += char_array_3[i];
            i = 0;
        }
    }

    if(i) {
        for(j = i; j < 4; j++)
            char_array_4[j] = 0;

        for(j = 0; j < 4; j++)
            char_array_4[j] = base64_chars.find(char_array_4[j]);

        char_array_3[0] = (char_array_4[0] << 2) + ((char_array_4[1] & 0x30) >> 4);
        char_array_3[1] = ((char_array_4[1] & 0xf) << 4) + ((char_array_4[2] & 0x3c) >> 2);
        char_array_3[2] = ((char_array_4[2] & 0x3) << 6) + char_array_4[3];

        for(j = 0; (j < i - 1); j++)
            ret += char_array_3[j];
    }

    return ret;
}

std::string Crypt::xorCrypt(const std::string& buffer, const std::string& key)
{
    std::string out;
    out.resize(buffer.size());
    size_t j = 0;
    for(size_t i = 0; i < buffer.size(); ++i) {
        out[i] = buffer[i] ^ key[j++];
        if(j >= key.size())
            j = 0;
    }
    return out;
}

std::string Crypt::genUUID()
{
    boost::uuids::random_generator gen;
    const boost::uuids::uuid u = gen();
    return to_string(u);
}

bool Crypt::setMachineUUID(std::string uuidstr)
{
    if(uuidstr.empty())
        return false;
    uuidstr = _decrypt(uuidstr, false);
    if(uuidstr.length() != 16)
        return false;
    std::copy(uuidstr.begin(), uuidstr.end(), m_machineUUID.begin());
    return true;
}

std::string Crypt::getMachineUUID()
{
    if(m_machineUUID.is_nil()) {
        boost::uuids::random_generator gen;
        m_machineUUID = gen();
    }
    return _encrypt(std::string(m_machineUUID.begin(), m_machineUUID.end()), false);
}

std::string Crypt::getCryptKey(bool useMachineUUID)
{
    const boost::hash<boost::uuids::uuid> uuid_hasher;
    boost::uuids::uuid uuid;
    if(useMachineUUID) {
        uuid = m_machineUUID;
    } else {
        const boost::uuids::nil_generator nilgen;
        uuid = nilgen();
    }
    const boost::uuids::name_generator namegen(uuid);
    const boost::uuids::uuid u = namegen(g_app.getCompactName() + g_platform.getCPUName() + g_platform.getOSName() + g_resources.getUserDir());
    std::size_t hash = uuid_hasher(u);
    std::string key;
    key.assign((const char*)&hash, sizeof(hash));
    return key;
}

std::string Crypt::_encrypt(const std::string& decrypted_string, bool useMachineUUID)
{
    std::string tmp = "0000" + decrypted_string;
    const uint32 sum = stdext::adler32((const uint8*)decrypted_string.c_str(), decrypted_string.size());
    stdext::writeULE32((uint8*)&tmp[0], sum);
    std::string encrypted = base64Encode(xorCrypt(tmp, getCryptKey(useMachineUUID)));
    return encrypted;
}

std::string Crypt::_decrypt(const std::string& encrypted_string, bool useMachineUUID)
{
    const std::string decoded = base64Decode(encrypted_string);
    const std::string tmp = xorCrypt(decoded, getCryptKey(useMachineUUID));
    if(tmp.length() >= 4) {
        const uint32 readsum = stdext::readULE32((const uint8*)tmp.c_str());
        std::string decrypted_string = tmp.substr(4);
        const uint32 sum = stdext::adler32((const uint8*)decrypted_string.c_str(), decrypted_string.size());
        if(readsum == sum)
            return decrypted_string;
    }
    return std::string();
}

void Crypt::rsaSetPublicKey(const std::string& n, const std::string& e)
{
#ifdef USE_GMP
    mpz_set_str(m_n, n.c_str(), 10);
    mpz_set_str(m_e, e.c_str(), 10);
#else
#if OPENSSL_VERSION_NUMBER < 0x10100005L
    BN_dec2bn(&m_rsa->n, n.c_str());
    BN_dec2bn(&m_rsa->e, e.c_str());
    // clear rsa cache
    if(m_rsa->_method_mod_n) {
        BN_MONT_CTX_free(m_rsa->_method_mod_n);
        m_rsa->_method_mod_n = nullptr;
    }
#else
    BIGNUM* bn = nullptr, * be = nullptr;
    BN_dec2bn(&bn, n.c_str());
    BN_dec2bn(&be, e.c_str());
    RSA_set0_key(m_rsa, bn, be, nullptr);
#endif
#endif
}

void Crypt::rsaSetPrivateKey(const std::string& p, const std::string& q, const std::string& d)
{
#ifdef USE_GMP
    mpz_set_str(m_p, p.c_str(), 10);
    mpz_set_str(m_q, q.c_str(), 10);
    mpz_set_str(m_d, d.c_str(), 10);

    // n = p * q
    mpz_mul(n, p, q);
#else
#if OPENSSL_VERSION_NUMBER < 0x10100005L
    BN_dec2bn(&m_rsa->p, p.c_str());
    BN_dec2bn(&m_rsa->q, q.c_str());
    BN_dec2bn(&m_rsa->d, d.c_str());
    // clear rsa cache
    if(m_rsa->_method_mod_p) {
        BN_MONT_CTX_free(m_rsa->_method_mod_p);
        m_rsa->_method_mod_p = nullptr;
    }
    if(m_rsa->_method_mod_q) {
        BN_MONT_CTX_free(m_rsa->_method_mod_q);
        m_rsa->_method_mod_q = nullptr;
    }
#else
    BIGNUM* bp = nullptr, * bq = nullptr, * bd = nullptr;
    BN_dec2bn(&bp, p.c_str());
    BN_dec2bn(&bq, q.c_str());
    BN_dec2bn(&bd, d.c_str());
    RSA_set0_key(m_rsa, nullptr, nullptr, bd);
    RSA_set0_factors(m_rsa, bp, bq);
#endif
#endif
}

bool Crypt::rsaEncrypt(unsigned char* msg, int size)
{
    if(size != rsaGetSize())
        return false;

#ifdef USE_GMP
    mpz_t c, m;
    mpz_init(c);
    mpz_init(m);
    mpz_import(m, size, 1, 1, 0, 0, msg);

    // c = m^e mod n
    mpz_powm(c, m, m_e, m_n);

    size_t count = (mpz_sizeinbase(m, 2) + 7) / 8;
    memset((char*)msg, 0, size - count);
    mpz_export((char*)msg + (size - count), nullptr, 1, 1, 0, 0, c);

    mpz_clear(c);
    mpz_clear(m);

    return true;
#else
    return RSA_public_encrypt(size, msg, msg, m_rsa, RSA_NO_PADDING) != -1;
#endif
}

bool Crypt::rsaDecrypt(unsigned char* msg, int size)
{
    if(size != rsaGetSize())
        return false;

#ifdef USE_GMP
    mpz_t c, m;
    mpz_init(c);
    mpz_init(m);
    mpz_import(c, size, 1, 1, 0, 0, msg);

    // m = c^d mod n
    mpz_powm(m, c, m_d, m_n);

    size_t count = (mpz_sizeinbase(m, 2) + 7) / 8;
    memset((char*)msg, 0, size - count);
    mpz_export((char*)msg + (size - count), nullptr, 1, 1, 0, 0, m);

    mpz_clear(c);
    mpz_clear(m);

    return true;
#else
    return RSA_private_decrypt(size, msg, msg, m_rsa, RSA_NO_PADDING) != -1;
#endif
}

int Crypt::rsaGetSize()
{
#ifdef USE_GMP
    size_t count = (mpz_sizeinbase(m_n, 2) + 7) / 8;
    return ((int)count / 128) * 128;
#else
    return RSA_size(m_rsa);
#endif
}


#include <pthread.h>
//#include <stdio.h>
//#include <stdlib.h>
//#include <unistd.h>
//#include <iostream>
//#include <fstream>
//#include <cmath>
//#include <math.h>

#include "pevents/pevents.h"	//Includes event handling (https://github.com/NeoSmart/PEvents)
#include "I2C/i2c.h"
#include "control/MatricesAndVectors.h"
#include "control/QuatRotEuler.h"
#include "control/AttitudeControl.h"
#include "control/PID_3DOF.h"

#include "rosserial/ros.h"
#include "rosserial/geometry_msgs/Wrench.h"
#include "rosserial/qcontrol_defs/PVA.h"
#include <Eigen/Dense>
#include "threads/keyboard_thread.h"
#include "threads/control_thread.h"
#include "threads/pca_thread.h"
#include "threads/print_thread.h"
#include "threads/mpu_thread.h"
#include "threads/stateMachine.h"
#include "threads/Ros_threads.h"
#include "threads/logger_thread.h"
// #include "overo/overo.h"

#include "kalman.h"

//TODO add feed-forward from position to attitude. Check yawref when changing from att to pos mode

using namespace neosmart;
using namespace std;
using Eigen::Matrix;

#define PI 3.1415

int currentState = INITIALIZING;
int YawSource = _IMU;

//Ros handle and IP variable
ros::NodeHandle  _nh;
char *rosSrvrIp;
qcontrol_defs::PVA PVA_quadVicon;
qcontrol_defs::PVA PVA_quadKalman;
qcontrol_defs::PVA PVA_RefJoy;
qcontrol_defs::PVA PVA_RefClient;

//Events and mutexes
neosmart_event_t e_Key1, e_Key2, e_Key3, e_Key4, e_Key5, e_Key6, e_Key7, e_Key8, e_Key9, e_KeyESC;
// neosmart_event_t e_ButtonLB, e_ButtonRB;
neosmart_event_t e_buttonX, e_buttonY, e_buttonA, e_buttonB;
neosmart_event_t e_Motor_Up, e_Motor_Down, e_Motor_Kill;
neosmart_event_t e_Timeout; //Always false event for forcing timeout of WaitForEvent
neosmart_event_t e_SwitchYawSource;
neosmart_event_t e_IMU_trigger;
neosmart_event_t e_PCA_trigger;
neosmart_event_t e_Log_trigger;
neosmart_event_t e_AttControl_trigger, e_PosControl_trigger;
neosmart_event_t e_Kalman_Trigger;
neosmart_event_t e_endInit; //Event that indicates that initialization is done
pthread_mutex_t IMU_Mutex;	//protect IMU data
pthread_mutex_t PCA_Mutex;
pthread_mutex_t ThrustJoy_Mutex;
pthread_mutex_t ThrustPosControl_Mutex;
pthread_mutex_t attRefJoy_Mutex, posRefJoy_Mutex;
pthread_mutex_t posRefClient_Mutex;
pthread_mutex_t attRefPosControl_Mutex;
pthread_mutex_t Contr_Input_Mutex;
pthread_mutex_t PID_Mutex;
pthread_mutex_t stateMachine_Mutex;
pthread_mutex_t PVA_Vicon_Mutex;
pthread_mutex_t PVA_Kalman_Mutex;
pthread_mutex_t ROS_Mutex;
pthread_mutex_t YawSource_Mutex;

//Global variables
float ThrustJoy = 0;
float ThrustPosControl = 0;
Matrix<float, 3, 1> IMU_Data_RPY, IMU_Data_RPY_ViconYaw;
Matrix<float, 3, 1> IMU_Data_Accel, IMU_Data_AngVel;
Matrix<float, 4, 1> IMU_Data_Quat, IMU_Data_QuatNoYaw, IMU_Data_Quat_ViconYaw;
Matrix<float, 3, 1> attRefJoy, angVelRefJoy;
Matrix<float, 3, 3> Rdes_PosControl;
Matrix<float, 4, 1> Contr_Input;
Matrix<float, 4, 1> PCA_Data;
PID_3DOF PID_angVel, PID_att, PID_pos; 	//Control PIDs
int threadCount = 0;		//Counts active threads

// ofstream kalman_v;
// ofstream vicon_p;

I2C i2c('3');

void *rosSpinTask(void *threadID){
	int SamplingTime = 10;	//Sampling time in milliseconds
	int localCurrentState;
	printf("rosSpinTask has started!\n");

	//ros joy subscriber
	ros::Subscriber<sensor_msgs::Joy> sub_mp_joy("/joy", handle_mp_joy_msg);
	_nh.subscribe(sub_mp_joy);

	ros::Subscriber<qcontrol_defs::PVA> sub_ros_pva("/pva", handle_client_pva_msg);
	_nh.subscribe(sub_ros_pva);

  	ros::Subscriber<geometry_msgs::TransformStamped> sub_tform("/vicon/Quad7/Quad7", handle_Vicon);
  	_nh.subscribe(sub_tform);

  	while(1){
		WaitForEvent(e_Timeout,SamplingTime);

		//Check system state
		pthread_mutex_lock(&stateMachine_Mutex);
			localCurrentState = currentState;
		pthread_mutex_unlock(&stateMachine_Mutex);

		//check if system should be terminated
		if(localCurrentState == TERMINATE){
			break;
		}

		//Trigger subscribers
	  	pthread_mutex_lock(&ROS_Mutex);
		    _nh.spinOnce();
	    pthread_mutex_unlock(&ROS_Mutex);

  	}

  	printf("rosSpinTask stopping...\n");
	threadCount -= 1;
	pthread_exit(NULL);
}

void *rosPublisherTask(void *threadID){
	printf("rosPublisherTask has started!\n");
	int SamplingTime = 50;	//Sampling time in milliseconds
	int localCurrentState, localYawSource;
	Matrix<float, 3, 1> RPY;

	//ros publisher
  	geometry_msgs::Point RPY_IMU;
  	geometry_msgs::Point RPY_Ref;
  	geometry_msgs::Wrench controlInputs;
  	qcontrol_defs::PVA PVA_EstimatedPVA;
  	qcontrol_defs::PVA PVA_PVARef;
  	qcontrol_defs::PVA PVA_Vicon;

  	ros::Publisher imurpy_pub("Quad_RPY", &RPY_IMU);
  	ros::Publisher Refrpy_pub("Quad_RPYRef", &RPY_Ref);
  	ros::Publisher imuWrench_pub("Quad_Inputs", &controlInputs);
  	ros::Publisher PvaEst_pub("Quad_EstimatedPVA", &PVA_EstimatedPVA);
  	ros::Publisher PvaRef_pub("Quad_PVARef", &PVA_PVARef);
  	ros::Publisher PvaVicon_pub("Quad_ViconPVA", &PVA_Vicon);
  	_nh.advertise(imurpy_pub);
  	_nh.advertise(Refrpy_pub);
  	_nh.advertise(imuWrench_pub);
	_nh.advertise(PvaEst_pub);
	_nh.advertise(PvaRef_pub);
	_nh.advertise(PvaVicon_pub);

	  while(1){
		WaitForEvent(e_Timeout,SamplingTime);

		//Check system state
		pthread_mutex_lock(&stateMachine_Mutex);
			localCurrentState = currentState;
		pthread_mutex_unlock(&stateMachine_Mutex);

		//check if system should be terminated
		if(localCurrentState == TERMINATE){
			break;
		}

		//Check if yaw source should come from IMU of Vicon
		pthread_mutex_lock(&YawSource_Mutex);
			localYawSource = YawSource;
		pthread_mutex_unlock(&YawSource_Mutex);

		//Get IMU roll pitch yaw data
		if (localYawSource == _IMU){
		  	pthread_mutex_lock(&IMU_Mutex);
			  	RPY_IMU.x = IMU_Data_RPY(0);
			  	RPY_IMU.y = IMU_Data_RPY(1);
			  	RPY_IMU.z = IMU_Data_RPY(2);
		  	pthread_mutex_unlock(&IMU_Mutex);
		}
		else{ //Get IMU roll and pitch; yaw comes from vicon
			pthread_mutex_lock(&PVA_Vicon_Mutex);
				RPY_IMU.x = IMU_Data_RPY_ViconYaw(0);
			  	RPY_IMU.y = IMU_Data_RPY_ViconYaw(1);
			  	RPY_IMU.z = IMU_Data_RPY_ViconYaw(2);
			pthread_mutex_unlock(&PVA_Vicon_Mutex);
		}

		//Get position/velocity estimations
		pthread_mutex_lock(&PVA_Kalman_Mutex);
			PVA_EstimatedPVA = PVA_quadKalman;
		pthread_mutex_unlock(&PVA_Kalman_Mutex);

		//Get position and attitude from vicon
	 	pthread_mutex_lock(&PVA_Vicon_Mutex);
	 		PVA_Vicon = PVA_quadVicon;
	  	pthread_mutex_unlock(&PVA_Vicon_Mutex);

		//Get attitude references
		if(localCurrentState == ATTITUDE_MODE){
			pthread_mutex_lock(&attRefJoy_Mutex);
				RPY_Ref.x = attRefJoy(0);
				RPY_Ref.y = attRefJoy(1);
				RPY_Ref.z = attRefJoy(2);
			pthread_mutex_unlock(&attRefJoy_Mutex);
		}
		else if(localCurrentState == POSITION_JOY_MODE){
			pthread_mutex_lock(&attRefPosControl_Mutex);
				RPY = Quat2RPY(Rot2quat(Rdes_PosControl));
			pthread_mutex_unlock(&attRefPosControl_Mutex);
			RPY_Ref.x = RPY(0);
			RPY_Ref.y = RPY(1);
			RPY_Ref.z = RPY(2);
		}
		else{
			RPY_Ref.x = 0;
			RPY_Ref.y = 0;
			RPY_Ref.z = 0;
		}

		pthread_mutex_lock(&posRefJoy_Mutex);
			PVA_PVARef.pos.position = PVA_RefJoy.pos.position;
	  	pthread_mutex_unlock(&posRefJoy_Mutex);

	  	//Get inputs for quadcopter
		pthread_mutex_lock(&Contr_Input_Mutex);
			controlInputs.force.x = 0;
			controlInputs.force.y = 0;
			controlInputs.force.z =  Contr_Input(0);
			controlInputs.torque.x = Contr_Input(1);
			controlInputs.torque.y = Contr_Input(2);
			controlInputs.torque.z = Contr_Input(3);
		pthread_mutex_unlock(&Contr_Input_Mutex);

	  	//Publish everything
	  	pthread_mutex_lock(&ROS_Mutex);
		    imurpy_pub.publish( &RPY_IMU);
		    Refrpy_pub.publish( &RPY_Ref);
		    imuWrench_pub.publish( &controlInputs);
		    PvaEst_pub.publish( &PVA_EstimatedPVA);
		    PvaRef_pub.publish ( &PVA_PVARef);
		    PvaVicon_pub.publish(&PVA_Vicon);
	    pthread_mutex_unlock(&ROS_Mutex);

  	}

  	printf("rosPublisherTask stopping...\n");
	threadCount -= 1;
	pthread_exit(NULL);
}

// //change red light to green after restart
// void set_pwm_mux_to_ap() {
//   // This function is intended to toggle the PWM mux on the MikiPilot AP Board (R2) to autopilot
//   // mode (green LED). Once done, the channel is turned back into an input to avoid contention
//   // if something is driving this GPIO. It is assumed that JP2 is populated (see page 5 of
//   // MikiPilot AP Board (R2) Schematics).
//   mc_overo.gpio_set_direction(overo::GPIO_147,overo::OUT);
//   mc_overo.gpio_set_value(overo::GPIO_147,overo::HIGH);
//   mc_overo.gpio_set_value(overo::GPIO_147,overo::LOW);
//   mc_overo.gpio_set_direction(overo::GPIO_147,overo::IN);
// }

void *Kalman_Timer(void *threadID){
	printf("Kalman_Timer has started!\n");
	int SamplingTime = 10;	//Sampling time in milliseconds
	int localCurrentState;

	//setup();

	while(1){
		WaitForEvent(e_Timeout,SamplingTime);

		//Check system state
		pthread_mutex_lock(&stateMachine_Mutex);
		localCurrentState = currentState;
		pthread_mutex_unlock(&stateMachine_Mutex);

		//check if system should be terminated
		if(localCurrentState == TERMINATE){
			break;
		}

		SetEvent(e_Kalman_Trigger);

	}

	printf("Kalman_Timer stopping...\n");
	threadCount -= 1;
	pthread_exit(NULL);
}

void *Kalman_Task(void *threadID){
	// int SamplingTime = 5;	//Sampling time in milliseconds
	int localCurrentState;

	Matrix<float, 3, 1> Zeros = Matrix<float, 3, 1>::Zero();

	ros::Time current_time = ros::Time(0,0);
	ros::Time prev_time = ros::Time(0,0);

	Eigen::Matrix<float, 12, 1> kalman_state = Eigen::Matrix<float, 12, 1>::Zero();
    Eigen::Matrix<float, 3, 1> z;   // measurement vector
    qcontrol_defs::PVA localPVA_quadVicon;
    Matrix<float, 3, 1> IMU_localData_Accel = Zeros;
    Matrix<float, 3, 1> prev_IMU_localData_Accel = Zeros;
    Matrix<float, 4, 1> IMU_localData_Quat;
    Matrix<float, 3, 3> Rbw;

    kalman_init(); //Initialize kalman filter

  	while(1){

		WaitForEvent(e_Kalman_Trigger,500);

		pthread_mutex_lock(&stateMachine_Mutex);
			localCurrentState = currentState;
		pthread_mutex_unlock(&stateMachine_Mutex);

		if(localCurrentState == TERMINATE){
			break;
		}

		kalman_state = kalman_propagate();

		//Check system state
		pthread_mutex_lock(&PVA_Vicon_Mutex);
			localPVA_quadVicon = PVA_quadVicon;
		pthread_mutex_unlock(&PVA_Vicon_Mutex);

		pthread_mutex_lock(&IMU_Mutex);
			IMU_localData_Accel = IMU_Data_Accel;
		pthread_mutex_unlock(&IMU_Mutex);

		//Check if there is a new measurement. If so, update estimation
		current_time = localPVA_quadVicon.t;

		if (prev_time.toSec() != current_time.toSec())
		{
			//Measurement vector z
			z <<  localPVA_quadVicon.pos.position.x,
			      localPVA_quadVicon.pos.position.y,
			      localPVA_quadVicon.pos.position.z;

			kalman_state = kalman_estimate_pos(z);
		}

		prev_time = current_time;

		if (prev_IMU_localData_Accel != IMU_localData_Accel)
		{
			//Get vehicle orientation
			IMU_localData_Quat(0) = localPVA_quadVicon.pos.orientation.w;
			IMU_localData_Quat(1) = localPVA_quadVicon.pos.orientation.x;
			IMU_localData_Quat(2) = localPVA_quadVicon.pos.orientation.y;
			IMU_localData_Quat(3) = localPVA_quadVicon.pos.orientation.z;

			Rbw = Quat2rot(IMU_localData_Quat);

			//Rotate acceleration into inertial frame
			IMU_localData_Accel = Rbw*IMU_localData_Accel;

			//Measurement vector z
			z <<  IMU_localData_Accel(0),
			      IMU_localData_Accel(1),
			      IMU_localData_Accel(2);

			kalman_state = kalman_estimate_acc(z);
		}

		prev_IMU_localData_Accel = IMU_localData_Accel;

		pthread_mutex_lock(&PVA_Kalman_Mutex);
			PVA_quadKalman.pos.position.x = kalman_state(0,0);
			PVA_quadKalman.pos.position.y = kalman_state(1,0);
			PVA_quadKalman.pos.position.z = kalman_state(2,0);
			PVA_quadKalman.vel.linear.x = kalman_state(3,0);
			PVA_quadKalman.vel.linear.y = kalman_state(4,0);
			PVA_quadKalman.vel.linear.z = kalman_state(5,0);
			PVA_quadKalman.acc.linear.x = kalman_state(6,0);
			PVA_quadKalman.acc.linear.y = kalman_state(7,0);
			PVA_quadKalman.acc.linear.z = kalman_state(8,0);
			PVA_quadKalman.pos.orientation = localPVA_quadVicon.pos.orientation;
		pthread_mutex_unlock(&PVA_Kalman_Mutex);
	  	// kalman_v << kalman_state(3,0) << "," << kalman_state(4,0) << "," << kalman_state(5,0) << "\n";
	  	// vicon_p << z(0,0) << "," << z(1,0) << "," << z(2,0) << "\n";
		//check if system should be terminated

  	}

	printf("Kalman_Task stopping...\n");
	threadCount -= 1;
	pthread_exit(NULL);
}

int main(int argc, char *argv[])
{
	//handles for threads
	pthread_t keyboardThread;		//Handle for keyboard thread
	pthread_t Motor_ControlThread;
	pthread_t StateMachine_Thread;
	pthread_t AttControl_Thread;
	pthread_t AttControl_TimerThread;
	pthread_t PosControl_Thread;
	pthread_t PosControl_TimerThread;
	pthread_t IMU_Thread;			//handle for IMU thread
	pthread_t IMU_TimerThread;		//handle for IMU Timer thread
	pthread_t PCA_Thread;			//handle for PCA thread
	pthread_t PCA_TimerThread;		//handle for PCA Timer thread
	pthread_t MAG_Thread;			//handle for MAG thread
	pthread_t PrintThread;			//handle for Printing thread
	pthread_t rosSpinThread;
	pthread_t rosPublisherThread;
	pthread_t Kalman_Thread;
	pthread_t Kalman_TimerThread;
	pthread_t LoggerTrigger;
	pthread_t LoggerThread;
	//long IDthreadKeyboard, IDthreadIMU, IDthreadMAG; //Stores ID for threads
	int ReturnCode;

	// kalman_v.open ("kalman_velocity.txt");
	// vicon_p.open ("vicon_position.txt");

	//Check if number of arguments are correct
	printf("Number of arguments: %d\n",argc);
	if(argc != 2) {
	  _nh.logfatal("Incorrect number of arguments\n");
	  return -1;
	}
	else {
	  rosSrvrIp = argv[1];
	}

	//Start ROS handle
	_nh.initNode(rosSrvrIp);

	//Set initial reference
	attRefJoy(0) = 0; attRefJoy(1) = 0; attRefJoy(2) = 0;
	PVA_RefJoy.pos.position.x = 0;
	PVA_RefJoy.pos.position.y = 0;
	PVA_RefJoy.pos.position.z = 1;
	PVA_RefJoy.vel.linear.x = 0;
	PVA_RefJoy.vel.linear.y = 0;
	PVA_RefJoy.vel.linear.z = 0;

	//event that signalizes end of initialization
	e_endInit = CreateEvent(false,false);

	//switch source of yaw (vicon x imu)
	e_SwitchYawSource = CreateEvent(true,false);

	//Keyboard Events for printing information
	e_Key1 = CreateEvent(true,false); 			//manual-reset event
	e_Key2 = CreateEvent(true,false); 			//manual-reset event
	e_Key3 = CreateEvent(true,false);           //manual-reset event
	e_Key4 = CreateEvent(true,false);           //manual-reset event
	e_Key5 = CreateEvent(true,false);           //manual-reset event
	e_Key6 = CreateEvent(true,false);           //manual-reset event
	e_Key7 = CreateEvent(true,false);           //manual-reset event
	e_Key8 = CreateEvent(true,false);           //manual-reset event
	e_Key9 = CreateEvent(true,false);           //manual-reset event

	//Keyboard event for execution termination
	e_KeyESC = CreateEvent(true,false); 		//abort manual-reset event

	//Joystick events for changing flight modes
	e_buttonX = CreateEvent(true,false);
	e_buttonY = CreateEvent(true,false);
	e_buttonA = CreateEvent(true,false);
	e_buttonB = CreateEvent(true,false);
	// e_ButtonLB = CreateEvent(true,false);
	// e_ButtonRB = CreateEvent(true,false);

	//Events for changing thrust values
	e_Motor_Up = CreateEvent(false,false);      //auto-reset event
	e_Motor_Down = CreateEvent(false,false);    //auto-reset event
	e_Motor_Kill = CreateEvent(false,false);    //auto-reset event

	//Events that trigget threads
	e_Timeout = CreateEvent(false,false);		//timeout event (always false)
	e_IMU_trigger = CreateEvent(false,false); 	//auto-reset event
	e_PCA_trigger = CreateEvent(false,false); 	//auto-reset event
	e_AttControl_trigger = CreateEvent(false, false);  //auto-reset event
	e_PosControl_trigger = CreateEvent(false, false);  //auto-reset event
	e_Log_trigger = CreateEvent(false,false);
	e_Kalman_Trigger = CreateEvent(false,false);

	//Create mutexes
	pthread_mutex_init(&IMU_Mutex, NULL);
	pthread_mutex_init(&PCA_Mutex, NULL);
	pthread_mutex_init(&ThrustJoy_Mutex, NULL);
	pthread_mutex_init(&Contr_Input_Mutex, NULL);
	pthread_mutex_init(&PID_Mutex, NULL);
	pthread_mutex_init(&attRefJoy_Mutex, NULL);
	pthread_mutex_init(&stateMachine_Mutex, NULL);
	pthread_mutex_init(&PVA_Vicon_Mutex, NULL);
	pthread_mutex_init(&ROS_Mutex, NULL);
	pthread_mutex_init(&posRefJoy_Mutex, NULL);
	pthread_mutex_init(&ThrustPosControl_Mutex, NULL);
	pthread_mutex_init(&YawSource_Mutex, NULL);

	//Load PID parameters from file
	pthread_mutex_lock(&PID_Mutex);
		updatePar(&PID_att, &PID_angVel, &PID_pos,"configAtt.txt","configPos.txt");
		PrintVec3(PID_att.K_p, "PID_att Kp");
	    PrintVec3(PID_att.K_i, "PID_att Ki");
	    PrintVec3(PID_att.K_d, "PID_att Kd");
	    PrintVec3(PID_angVel.K_p, "PID_w Kp");
	    PrintVec3(PID_angVel.K_i, "PID_w Ki");
	    PrintVec3(PID_angVel.K_d, "PID_w Kd");
	    PrintVec3(PID_pos.K_p, "PID_pos Kp");
	    PrintVec3(PID_pos.K_i, "PID_pos Ki");
	    PrintVec3(PID_pos.K_d, "PID_pos Kd");
	pthread_mutex_unlock(&PID_Mutex);

	//Start keyboard task
	if (ReturnCode = pthread_create(&keyboardThread, NULL, KeyboardTask, NULL)){
		printf("Start KeyboardTask failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start rosSpin task
	if (ReturnCode = pthread_create(&rosSpinThread, NULL, rosSpinTask, NULL)){
		printf("Start rosSpin failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start ros publisher task
	if (ReturnCode = pthread_create(&rosPublisherThread, NULL, rosPublisherTask, NULL)){
		printf("Start rosPublisher failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start state StateMachineTask
	if (ReturnCode = pthread_create(&StateMachine_Thread, NULL, StateMachineTask, NULL)){
		printf("Start StateMachineTask failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start Attitude Control timer task
	if (ReturnCode = pthread_create(&AttControl_TimerThread, NULL, AttControl_Timer, NULL)){
		printf("Start Control_TimerTask failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start Attitude Control task
	if (ReturnCode = pthread_create(&AttControl_Thread, NULL, AttControl_Task, NULL)){
		printf("Start Control_Task failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
			threadCount += 1;

	//Start Position Control timer task
	if (ReturnCode = pthread_create(&PosControl_TimerThread, NULL, PosControl_Timer, NULL)){
		printf("Start PosControl_Timer failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start position Control task
	if (ReturnCode = pthread_create(&PosControl_Thread, NULL, PosControl_Task, NULL)){
		printf("Start PosControl_Task failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
			threadCount += 1;

	//Start keyboard control task
	if (ReturnCode = pthread_create(&Motor_ControlThread, NULL, Motor_KeyboardControl, NULL)){
		printf("Start Motor_ContorlTask failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start IMU task
	if (ReturnCode = pthread_create(&IMU_Thread, NULL, IMU_Task, NULL)){
		printf("Start IMU_Task failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start IMU timer task
	if (ReturnCode = pthread_create(&IMU_TimerThread, NULL, IMU_Timer, NULL)){
		printf("Start IMU_TimerTask failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start PCA task
	if (ReturnCode = pthread_create(&PCA_Thread, NULL, PCA_Task, NULL)){
		printf("Start PCA_Task failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start PCA timer task
	if (ReturnCode = pthread_create(&PCA_TimerThread, NULL, PCA_Timer, NULL)){
		printf("Start PCA_TimerTask failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start printing task
	if (ReturnCode = pthread_create(&PrintThread, NULL, PrintTask, NULL)){
		printf("Start PrintThread failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start kalman filter timer task
	if (ReturnCode = pthread_create(&Kalman_TimerThread, NULL, Kalman_Timer, NULL)){
		printf("Start Kalman_TimerThread failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	//Start kalman filter task
	if (ReturnCode = pthread_create(&Kalman_Thread, NULL, Kalman_Task, NULL)){
		printf("Start PrintThread failed; return code from pthread_create() is %d\n", ReturnCode);
		exit(-1);
	}
	else
		threadCount += 1;

	// //Start logger timer task
	// if (ReturnCode = pthread_create(&LoggerTrigger, NULL, Logger_Timer, NULL)){
	// 	printf("Start LoggerTrigger failed; return code from pthread_create() is %d\n", ReturnCode);
	// 	exit(-1);
	// }
	// else
	// 	threadCount += 1;

	// //Start logger task
	// if (ReturnCode = pthread_create(&LoggerThread, NULL, Log_Task, NULL)){
	// 	printf("Start LoggerThread failed; return code from pthread_create() is %d\n", ReturnCode);
	// 	exit(-1);
	// }
	// else
	// 	threadCount += 1;

	//This has to be removed from here and put somewhere that indicates configurating the vechicle has been done
	SetEvent(e_endInit);

	//Check every 500ms if all threads have returned
	while(1){
		WaitForEvent(e_Timeout,500);
		if(threadCount == 0)
			break;
	}

	printf("Closing program... \n");

	//Destroy events
	DestroyEvent(e_Key1);
	DestroyEvent(e_Key2);
	DestroyEvent(e_Key3);
	DestroyEvent(e_Key4);
	DestroyEvent(e_Key5);
	DestroyEvent(e_Key6);
	DestroyEvent(e_Key7);
	DestroyEvent(e_Key8);
	DestroyEvent(e_Key9);
	DestroyEvent(e_KeyESC);
	DestroyEvent(e_buttonX);
	DestroyEvent(e_buttonY);
	DestroyEvent(e_buttonA);
	DestroyEvent(e_buttonB);
	DestroyEvent(e_Motor_Up);
	DestroyEvent(e_Motor_Down);
	DestroyEvent(e_Motor_Kill);
	DestroyEvent(e_Timeout);
	DestroyEvent(e_IMU_trigger);
	DestroyEvent(e_PCA_trigger);
	DestroyEvent(e_AttControl_trigger);
	DestroyEvent(e_PosControl_trigger);
	DestroyEvent(e_endInit);
	DestroyEvent(e_SwitchYawSource);
	DestroyEvent(e_Log_trigger);
	DestroyEvent(e_Kalman_Trigger);

	//Destroy mutexes
	pthread_mutex_destroy(&IMU_Mutex);
	pthread_mutex_destroy(&PCA_Mutex);
	pthread_mutex_destroy(&ThrustJoy_Mutex);
	pthread_mutex_destroy(&Contr_Input_Mutex);
	pthread_mutex_destroy(&PID_Mutex);
	pthread_mutex_destroy(&attRefJoy_Mutex);
	pthread_mutex_destroy(&stateMachine_Mutex);
	pthread_mutex_destroy(&PVA_Vicon_Mutex);
	pthread_mutex_destroy(&ROS_Mutex);
	pthread_mutex_destroy(&posRefJoy_Mutex);
	pthread_mutex_destroy(&ThrustPosControl_Mutex);
	pthread_mutex_destroy(&YawSource_Mutex);

   /* Last thing that main() should do */
	// vicon_p.close();
	// kalman_v.close();
    pthread_exit(NULL);
}


#pragma once
#include <istream>
#include <vector>
#include <tuple>
#include <utility>
#include <include/half.hpp>
#include <Eigen/SVD>

class SVD {
public:
    struct metadata
    {
        long U_height;
        long V_width;
        long rank;
        unsigned char max_value;
    };

    struct decomp
    {
        metadata meta;
        std::vector<half_float::half> U;
        std::vector<half_float::half> S;
        std::vector<half_float::half> V;
    };

    static decomp pgmSvdToHalfStream(std::istream &header, std::istream &pgm, int rank);

    static void writePgmAsSvd(const std::string &output_path, decomp decomposition);

    static std::tuple<std::string, long> svdToPGMString(const std::string &input_filename);
    static SVD::decomp rankDecomp(const Eigen::MatrixXd &U, const Eigen::MatrixXd &S, const Eigen::MatrixXd &V, int rank, unsigned char max_value);
    static void writeAllDecomps(std::istream &header, std::istream &pgm, const std::string &filename);
};


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

extern "C" {
#include <vlfeat/vl/fisher.h>
#include <vlfeat/vl/gmm.h>
}

#include <Eigen/Core>
#include <glog/logging.h>
#include <memory>
#include <vector>

#include "theia/matching/fisher_vector_extractor.h"

namespace theia {
namespace {
Eigen::MatrixXf ConvertVectorOfFeaturesToMatrix(
    const std::vector<Eigen::VectorXf>& features) {
  Eigen::MatrixXf feature_table(features[0].size(), features.size());
  for (int i = 0; i < feature_table.cols(); i++) {
    feature_table.col(i) = features[i];
  }
  return feature_table;
}
}  // namespace

// Wrapper for the VlFeat gmm.
class FisherVectorExtractor::GaussianMixtureModel {
 public:
  // NOTE: We need the unique_ptr to use vlfeat's delete function for the GMM.
  GaussianMixtureModel(const int num_clusters)
      : num_clusters_(num_clusters), gmm_(nullptr, vl_gmm_delete) {}

  int num_clusters() const { return num_clusters_; }

  // Computes the Gaussian mixture model based on the input training features.
  bool Compute(const Eigen::MatrixXf& data_points) {
    CHECK(!data_points.hasNaN());

    // Create the GMM instance.
    gmm_.reset(vl_gmm_new(VL_TYPE_FLOAT, data_points.rows(), num_clusters_));
    if (!gmm_) {
      return false;
    }
    // Compute the model.
    const double distortion =
        vl_gmm_cluster(gmm_.get(), data_points.data(), data_points.cols());
    CHECK(!std::isnan(distortion));
    return true;
  }

  // Helper accessor methods for retreiving VLFeat types.
  void const* GetMeans() const { return vl_gmm_get_means(gmm_.get()); }
  void const* GetCovariances() const {
    return vl_gmm_get_covariances(gmm_.get());
  }
  void const* GetPriors() const { return vl_gmm_get_priors(gmm_.get()); }

 private:
  // Number of clusters.
  const int num_clusters_;
  // The VlFeat GMM model.
  std::unique_ptr<VlGMM, void (*)(VlGMM*)> gmm_;
};

FisherVectorExtractor::FisherVectorExtractor(const Options& options)
    : gmm_(new GaussianMixtureModel(options.num_gmm_clusters)),
      training_feature_sampler_(options.max_num_features_for_training) {}

FisherVectorExtractor::~FisherVectorExtractor() {}

void FisherVectorExtractor::AddFeaturesForTraining(
    const std::vector<Eigen::VectorXf>& features) {
  for (const Eigen::VectorXf& feature : features) {
    CHECK(!feature.hasNaN()) << "Feature: " << feature.transpose();
    training_feature_sampler_.AddElementToSampler(feature);
  }
}

bool FisherVectorExtractor::Train() {
  // Get the features randomly sampled for training.
  const auto& sampled_features = training_feature_sampler_.GetAllSamples();
  CHECK_GT(sampled_features.size(), 0);
  LOG(INFO) << "Training GMM for Fisher Vector extractin with "
            << sampled_features.size() << " features sampled from "
            << training_feature_sampler_.NumElementsAdded()
            << " total features.";

  // Train the GMM using the training feaures.
  const Eigen::MatrixXf feature_table =
      ConvertVectorOfFeaturesToMatrix(sampled_features);
  return gmm_->Compute(feature_table);
}

Eigen::VectorXf FisherVectorExtractor::ExtractGlobalDescriptor(
    const std::vector<Eigen::VectorXf>& features) {
  // Ensure there are input features and they are not zero dimensions.
  CHECK_GT(features.size(), 0);
  CHECK_GT(features[0].size(), 0);

  // Convert the features into a continuous memory block. The matrix is of size
  // D x N where D is the number of descrip
  const Eigen::MatrixXf feature_table =
      ConvertVectorOfFeaturesToMatrix(features);
  // Compute the fisher vector encoding.
  Eigen::VectorXf fisher_vector(2 * feature_table.rows() *
                                gmm_->num_clusters());
  vl_fisher_encode(fisher_vector.data(),
                   VL_TYPE_FLOAT,
                   gmm_->GetMeans(),
                   feature_table.rows(),
                   gmm_->num_clusters(),
                   gmm_->GetCovariances(),
                   gmm_->GetPriors(),
                   feature_table.data(),
                   feature_table.cols(),
                   VL_FISHER_FLAG_IMPROVED);
  DCHECK(std::isfinite(fisher_vector.sum()));
  return fisher_vector;
}

}  // namespace theia


#include <boost/multiprecision/cpp_bin_float.hpp>


#include <boost/test/unit_test.hpp>

#include <am/succinct/dbitv/dbitv.hpp>

using namespace std;

#define BITV_SZ 262144LL

BOOST_AUTO_TEST_SUITE( t_dense_succinct_bitv_suite )

BOOST_AUTO_TEST_CASE(rank)
{
    izenelib::am::succinct::dense::DBitV bv(false);
    vector<uint64_t> bits(BITV_SZ / 64);
    for (size_t i = 0; i < BITV_SZ / 64; ++i)
    {
        if (i % 2 == 0)
            bits[i] = 0x5555555555555555;
        else
            bits[i] = 0xAAAAAAAAAAAAAAAA;
    }
    bv.build(bits, BITV_SZ);

    size_t nrank0 = 0;
    size_t nrank1 = 0;
    size_t rank;

    for (int i = 0; i < BITV_SZ; i++)
    {
        if (bv.access(i, rank))
        {
            BOOST_CHECK_EQUAL(nrank1, rank);
            ++nrank1;
        }
        else
        {
            BOOST_CHECK_EQUAL(nrank0, rank);
            ++nrank0;
        }
    }
}

BOOST_AUTO_TEST_CASE(select)
{
    izenelib::am::succinct::dense::DBitV bv(true);
    vector<uint64_t> bits(BITV_SZ / 64);
    for (size_t i = 0; i < BITV_SZ / 64; ++i)
    {
        if (i % 2 == 0)
            bits[i] = 0x5555555555555555;
        else
            bits[i] = 0xAAAAAAAAAAAAAAAA;
    }
    bv.build(bits, BITV_SZ);

    size_t nrank0 = 0;
    size_t nrank1 = 0;

    for (int i = 0; i < BITV_SZ; i++)
    {
        if (bv.access(i))
        {
            BOOST_CHECK_EQUAL(i, bv.select1(nrank1));
            ++nrank1;
        }
        else
        {
            BOOST_CHECK_EQUAL(i, bv.select0(nrank0));
            ++nrank0;
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()