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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

#include <mitkFiberBundle.h>
#include <mitkCommandLineParser.h>
#include <mitkDiffusionDataIOHelper.h>
#include <mitkLexicalCast.h>
#include <itkTractDistanceFilter.h>
#include <mitkClusteringMetricEuclideanMean.h>
#include <mitkClusteringMetricEuclideanMax.h>
#include <mitkClusteringMetricEuclideanStd.h>
#include <boost/progress.hpp>

typedef itk::Image< unsigned char, 3 >  ItkUcharImageType;
typedef itk::Image< float, 4 >  ItkPeakImgType;

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

  parser.setTitle("Reference Similarity");
  parser.setCategory("Fiber Tracking Evaluation");
  parser.setContributor("MIC");

  parser.setArgumentPrefix("--", "-");
  parser.addArgument("", "i", mitkCommandLineParser::StringList, "Input Tracts:", "input tracts folder", us::Any(), false);
  parser.addArgument("reference_tracts", "", mitkCommandLineParser::StringList, "", "", us::Any(), false);
  parser.addArgument("reference_masks", "", mitkCommandLineParser::StringList, "", "", us::Any(), false);
  parser.addArgument("reference_peaks", "", mitkCommandLineParser::StringList, "", "", us::Any(), false);

  parser.addArgument("", "o", mitkCommandLineParser::String, "", "", us::Any(), false);

  parser.addArgument("fiber_points", "", mitkCommandLineParser::Int, "Fiber points:", "", 20);

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  std::string out_folder = us::any_cast<std::string>(parsedArgs["o"]);
  mitkCommandLineParser::StringContainerType input_tract_files = us::any_cast<mitkCommandLineParser::StringContainerType>(parsedArgs["i"]);
  mitkCommandLineParser::StringContainerType reference_tract_files = us::any_cast<mitkCommandLineParser::StringContainerType>(parsedArgs["reference_tracts"]);
  mitkCommandLineParser::StringContainerType reference_mask_files = us::any_cast<mitkCommandLineParser::StringContainerType>(parsedArgs["reference_masks"]);
  mitkCommandLineParser::StringContainerType reference_peak_files = us::any_cast<mitkCommandLineParser::StringContainerType>(parsedArgs["reference_peaks"]);

  int fiber_points = 20;
  if (parsedArgs.count("fiber_points"))
    fiber_points = us::any_cast<int>(parsedArgs["fiber_points"]);

  try
  {
    std::vector<std::string>  input_tract_names;
    std::vector<std::string>  ref_tract_names;
    std::vector< mitk::FiberBundle::Pointer > input_tracts = mitk::DiffusionDataIOHelper::load_fibs(input_tract_files, &input_tract_names);
    std::vector< mitk::FiberBundle::Pointer > reference_tracts = mitk::DiffusionDataIOHelper::load_fibs(reference_tract_files, &ref_tract_names);
    std::vector< ItkUcharImageType::Pointer > reference_masks = mitk::DiffusionDataIOHelper::load_itk_images<ItkUcharImageType>(reference_mask_files);
    std::vector< ItkPeakImgType::Pointer > reference_peaks = mitk::DiffusionDataIOHelper::load_itk_images<ItkPeakImgType>(reference_peak_files);

    MITK_INFO << "Calculating distances";
    itk::TractDistanceFilter::Pointer distance_calculator = itk::TractDistanceFilter::New();
    distance_calculator->SetNumPoints(fiber_points);
    distance_calculator->SetTracts1(input_tracts);
    distance_calculator->SetTracts2(reference_tracts);
    distance_calculator->SetMetrics({new mitk::ClusteringMetricEuclideanMean()});
    distance_calculator->Update();
    auto distances = distance_calculator->GetAllDistances();

    vnl_matrix<float> voxel_overlap; voxel_overlap.set_size(input_tracts.size(), reference_tracts.size());
    vnl_matrix<float> dir_overlap; dir_overlap.set_size(input_tracts.size(), reference_tracts.size());

    MITK_INFO << "Calculating overlap";
    boost::progress_display disp(input_tracts.size()*reference_tracts.size());
    int r=0;
    for (auto fib : input_tracts)
    {
      int c=0;
      for (auto ref_mask : reference_masks)
      {
        ++disp;
        std::streambuf *old = cout.rdbuf(); // <-- save
        std::stringstream ss;
        std::cout.rdbuf (ss.rdbuf());       // <-- redirect
        float overlap = 0;
        float directional_overlap = 0;
        std::tie(directional_overlap, overlap) = fib->GetDirectionalOverlap(ref_mask, reference_peaks.at(c));
        voxel_overlap[r][c] = overlap;
        dir_overlap[r][c] = directional_overlap;
        std::cout.rdbuf (old);              // <-- restore
        ++c;
      }
      ++r;
    }

    ofstream logfile;
    logfile.open(out_folder + "ref_tract_names.txt");
    for (unsigned int i=0; i<ref_tract_names.size(); ++i)
      logfile << ist::GetFilenameWithoutExtension(ref_tract_names.at(i)) << std::endl;
    logfile << "----------" << std::endl;
    for (unsigned int i=0; i<input_tract_names.size(); ++i)
      logfile << ist::GetFilenameWithoutExtension(input_tract_names.at(i)) << std::endl;
    logfile.close();

    MITK_INFO << "Saving matrices";
    {
      std::ofstream OutputStream(out_folder + "ref_distances.txt");
      OutputStream << distances;
      OutputStream.close();
    }
    {
      std::ofstream OutputStream(out_folder + "ref_voxel_overlap.txt");
      OutputStream << voxel_overlap;
      OutputStream.close();
    }
    {
      std::ofstream OutputStream(out_folder + "ref_dir_overlap.txt");
      OutputStream << dir_overlap;
      OutputStream.close();
    }

  }
  catch (itk::ExceptionObject e)
  {
    std::cout << e;
    return EXIT_FAILURE;
  }
  catch (std::exception e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (...)
  {
    std::cout << "ERROR!?!";
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


#include <iostream>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/accumulators.hpp>

int main()
{
	using namespace boost::accumulators;
	accumulator_set<int, features<tag::count> > acc;
	acc(0);
	acc(0);
	acc(0);
	acc(0);
	assert( 4 == count(acc));
	std::cout <<" count:" << count(acc) << '\n';
	return 0;
}


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

#include "algorithm/TupleSpace.hpp"

using namespace mist;
using namespace algorithm;

class Counter : public TupleSpaceTraverser {
public:
  void process_tuple(TupleSpace::count_t tuple_no, TupleSpace::tuple_t const& tuple){ this->count++; };
  void process_tuple_entropy(TupleSpace::count_t tuple_no, TupleSpace::tuple_t const& tuple, it::Entropy const& e) { this->count++; };
  TupleSpace::index_t count = 0;
};

BOOST_AUTO_TEST_CASE(simple_names)
{
  TupleSpace ts;
  ts.set_names({ "v1", "v2", "v3" });
  auto names = ts.names();
  BOOST_TEST(names[0] == "v1");
  BOOST_TEST(names[1] == "v2");
  BOOST_TEST(names[2] == "v3");
}

BOOST_AUTO_TEST_CASE(traverse_equal_count)
{
  TupleSpace ts;
  ts.addVariableGroup("A", {0,1,2,3,4});
  ts.addVariableGroup("B", {5,6});
  ts.addVariableGroupTuple({0,1});
  Counter cntr;
  ts.traverse(cntr);
  BOOST_TEST(ts.count_tuples() == cntr.count);
  BOOST_TEST(ts.count_tuples() == 10);
}

BOOST_AUTO_TEST_CASE(traverse_equal_count2)
{
  TupleSpace ts(10, 2);
  Counter cntr;
  ts.traverse(cntr);
  BOOST_TEST(ts.count_tuples() == cntr.count);
  BOOST_TEST(ts.count_tuples() == 45);
}

// tests that we are in the regime of uint64, otherwise there would be overflow
BOOST_AUTO_TEST_CASE(count_large)
{
  TupleSpace ts(1e5, 2);
  BOOST_TEST(ts.count_tuples() == 4999950000);
}

//BOOST_AUTO_TEST_CASE(count_large1)
//{
//  TupleSpace ts(1e6, 3);
//  BOOST_TEST(ts.count_tuples() == 166666166667000000);
//}

//BOOST_AUTO_TEST_CASE(count_large2)
//{
//  TupleSpace ts(1e7, 2);
//  BOOST_TEST(ts.count_tuples() == 49999995000000);
//}

#if 0
BOOST_AUTO_TEST_CASE(simple_index_d2_n4) {
    TupleSpace ts(2);
    ts.addGroup({0,1})
    ts.addGroup({4,5})
    // default min = 0, max = 1
    auto tuples = TupleSpace::generateTuples(ts);
    BOOST_TEST(tuples[0] = { 0, 4 });
    BOOST_TEST(tuples[1] = { 0, 5 });
    BOOST_TEST(tuples[2] = { 1, 4 });
    BOOST_TEST(tuples[3] = { 1, 5 });
}
#endif


/*
 * Copyright 2011-2013 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 * Example for the lorenz system with a 3D point type
*/

#include <iostream>
#include <cmath>

#include <boost/operators.hpp>

#include <boost/numeric/odeint.hpp>

//[point3D
class point3D :
    boost::additive1< point3D ,
    boost::additive2< point3D , double ,
    boost::multiplicative2< point3D , double > > >
{
public:

    double x , y , z;

    point3D()
        : x( 0.0 ) , y( 0.0 ) , z( 0.0 )
    { }

    point3D( const double val )
        : x( val ) , y( val ) , z( val )
    { }

    point3D( const double _x , const double _y , const double _z  )
        : x( _x ) , y( _y ) , z( _z )
    { }

    point3D& operator+=( const point3D &p )
    {
        x += p.x; y += p.y; z += p.z;
        return *this;
    }

    point3D& operator*=( const double a )
    {
        x *= a; y *= a; z *= a;
        return *this;
    }

};
//]

//[point3D_abs_div
// only required for steppers with error control
point3D operator/( const point3D &p1 , const point3D &p2 )
{
    return point3D( p1.x/p2.x , p1.y/p2.y , p1.z/p1.z );
}

point3D abs( const point3D &p )
{
    return point3D( std::abs(p.x) , std::abs(p.y) , std::abs(p.z) );
}
//]

//[point3D_norm
// also only for steppers with error control
namespace boost { namespace numeric { namespace odeint {
template<>
struct vector_space_norm_inf< point3D >
{
    typedef double result_type;
    double operator()( const point3D &p ) const
    {
        using std::max;
        using std::abs;
        return max( max( abs( p.x ) , abs( p.y ) ) , abs( p.z ) );
    }
};
} } }
//]

std::ostream& operator<<( std::ostream &out , const point3D &p )
{
    out << p.x << " " << p.y << " " << p.z;
    return out;
}

//[point3D_main
const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;

void lorenz( const point3D &x , point3D &dxdt , const double t )
{
    dxdt.x = sigma * ( x.y - x.x );
    dxdt.y = R * x.x - x.y - x.x * x.z;
    dxdt.z = -b * x.z + x.x * x.y;
}

using namespace boost::numeric::odeint;

int main()
{

    point3D x( 10.0 , 5.0 , 5.0 );
    // point type defines it's own operators -> use vector_space_algebra !
    typedef runge_kutta_dopri5< point3D , double , point3D ,
                                double , vector_space_algebra > stepper;
    int steps = integrate_adaptive( make_controlled<stepper>( 1E-10 , 1E-10 ) , lorenz , x ,
                                    0.0 , 10.0 , 0.1 );
    std::cout << x << std::endl;
    std::cout << "steps: " << steps << std::endl;
}
//]


//  Copyright (c) 2018 Robert Ramey
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <stdexcept>
#include <iostream>
#include <sstream>

#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/safe_integer_range.hpp>
#include <boost/safe_numerics/native.hpp>
#include <boost/safe_numerics/exception.hpp>

#include "safe_format.hpp" // prints out range and value of any type

using namespace boost::safe_numerics;

using safe_t = safe_signed_range<
    -24,
    82,
    native,
    loose_exception_policy
>;

// define variables used for input
using input_safe_t = safe_signed_range<
    -24,
    82,
    native, // we don't need automatic in this case
    loose_exception_policy // assignment of out of range value should throw
>;

// function arguments can never be outside of limits
auto f(const safe_t & x, const safe_t & y){
    auto z = x + y;  // we know that this cannot fail
    std::cout << "z = " << safe_format(z) << std::endl;
    std::cout << "(x + y) = " << safe_format(x + y) << std::endl;
    std::cout << "(x - y) = " << safe_format(x - y) << std::endl;
    return z;
}

bool test(const char * test_string){
    std::istringstream sin(test_string);
    input_safe_t x, y;
    try{
        std::cout << "type in values in format x y:" << test_string << std::endl;
        sin >> x >> y; // read varibles, maybe throw exception
    }
    catch(const std::exception & e){
        // none of the above should trap. Mark failure if they do
        std::cout << e.what() << '\n' << "input failure" << std::endl;
        return false;
    }
    std::cout << "x" << safe_format(x) << std::endl;
    std::cout << "y" << safe_format(y) << std::endl;
    std::cout << safe_format(f(x, y)) << std::endl;
    std::cout << "input success" << std::endl;
    return true;
}

int main(){
    std::cout << "example 84:\n";
    bool result =
        !  test("123 125")
        && test("0 0")
        && test("-24 82")
    ;
    std::cout << (result ? "Success!" : "Failure") << std::endl;
    return result ? EXIT_SUCCESS : EXIT_FAILURE;
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout;  using std::endl;
#include <cerrno> // for ::errno

//[policy_eg_7

#include <boost/math/distributions.hpp> // All distributions.
// using boost::math::normal; // Would create an ambiguity between
// boost::math::normal_distribution<RealType> boost::math::normal and
// 'anonymous-namespace'::normal'.

namespace
{ // anonymous or unnamed (rather than named as in policy_eg_6.cpp).

  using namespace boost::math::policies;
   // using boost::math::policies::errno_on_error; // etc.
  typedef policy<
     // return infinity and set errno rather than throw:
     overflow_error<errno_on_error>,
     // Don't promote double -> long double internally:
     promote_double<false>,
     // Return the closest integer result for discrete quantiles:
     discrete_quantile<integer_round_nearest>
  > my_policy;

  BOOST_MATH_DECLARE_DISTRIBUTIONS(double, my_policy)

} // close namespace my_namespace

int main()
{
   // Construct distribution with something we know will overflow.
   normal norm(10, 2); // using 'anonymous-namespace'::normal
   errno = 0;
   cout << "Result of quantile(norm, 0) is: "
      << quantile(norm, 0) << endl;
   cout << "errno = " << errno << endl;
   errno = 0;
   cout << "Result of quantile(norm, 1) is: "
      << quantile(norm, 1) << endl;
   cout << "errno = " << errno << endl;
   //
   // Now try a discrete distribution:
   binomial binom(20, 0.25);
   cout << "Result of quantile(binom, 0.05) is: "
      << quantile(binom, 0.05) << endl;
   cout << "Result of quantile(complement(binom, 0.05)) is: "
      << quantile(complement(binom, 0.05)) << endl;
}

//] //[/policy_eg_7]

/*

Output:

  Result of quantile(norm, 0) is: -1.#INF
  errno = 34
  Result of quantile(norm, 1) is: 1.#INF
  errno = 34
  Result of quantile(binom, 0.05) is: 1
  Result of quantile(complement(binom, 0.05)) is: 8

*/


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_STANDARD_FUNCTION_SCALAR_FREXP_HPP_INCLUDED
#define NT2_TOOLBOX_STANDARD_FUNCTION_SCALAR_FREXP_HPP_INCLUDED
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/at.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(standard::tag::frexp_, tag::cpu_,
                       (A0),
                       (real_<A0>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<standard::tag::frexp_(tag::real_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0> struct result<This(A0)>
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type mantissa;
      typedef typename meta::as_integer<A0,signed>::type          exponent;
      typedef boost::fusion::vector<mantissa,exponent>             type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      int r1t;
      boost::fusion::at_c<0>(res) = std::frexp(a0, &r1t);
      boost::fusion::at_c<1>(res) =  r1t;
      return res;
    }

  };
} }

#endif
// modified by jt the 29/12/2010
// modified manually by jt the 29/12/2010


// Copyright (C) 2009  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "../tester.h"
#include <dlib/geometry.h>
#include <dlib/matrix.h>

#ifndef DLIB_USE_BLAS
#error "BLAS bindings must be used for this test to make any sense"
#endif

namespace dlib
{
    namespace blas_bindings
    {

#ifdef DLIB_TEST_BLAS_BINDINGS
        extern int& counter_gemm();
        extern int& counter_gemv();
        extern int& counter_ger();
        extern int& counter_dot();
#endif

    }
}

namespace
{
    using namespace test;
    using namespace std;
    // Declare the logger we will use in this test.  The name of the logger
    // should start with "test."
    dlib::logger dlog("test.vector");

    class vector_tester : public tester
    {
    public:
        vector_tester (
        ) :
            tester (
                "test_vector", // the command line argument name for this test
                "Run tests on dlib::vector.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {}

        template <typename type>
        void test_vector(
        ) const
        {
            using namespace dlib;
            using namespace dlib::blas_bindings;

            dlib::vector<type,2> a2, b2, c2;
            dlib::vector<type,3> a3, b3, c3;

            matrix<type> mat2(2,2), mat3(3,3);
            mat2 = 0;
            mat3 = 0;

            type var = 0;

            // We want to make sure that the BLAS bindings are being called for the 2D and 3D vectors.  That would
            // be very slow.
            counter_gemm() = 0;
            counter_gemv() = 0;
            counter_ger() = 0;
            counter_dot() = 0;

            var = trans(a2)*(a2);
            var = dot(a2,a2);

            a2 = mat2*b2;
            var = trans(b2)*mat2*b2;

            var = trans(a3)*(a3);
            var = dot(a3,a3);

            a3 = mat3*b3;
            var = trans(b3)*mat3*b3;

            mat3 = c3*trans(a3);
            mat2 = c2*trans(a2);

            DLIB_TEST(counter_gemm() == 0 && counter_gemv() == 0 && counter_ger() == 0 && counter_dot() == 0);

        }

        void perform_test (
        )
        {
            using namespace dlib;

            dlog << dlib::LINFO << "test double";
            test_vector<double>();

            dlog << dlib::LINFO << "test float";
            test_vector<float>();

            dlog << dlib::LINFO << "test int";
            test_vector<int>();

            dlog << dlib::LINFO << "test short";
            test_vector<short>();

            print_spinner();
        }
    };

    vector_tester a;

}


// Copyright 2019 Erik Teichmann <kontakt.teichmann@gmail.com>

#ifndef INCLUDE_SAM_SYSTEM_EULER_SYSTEM_HPP_
#define INCLUDE_SAM_SYSTEM_EULER_SYSTEM_HPP_

#include <vector>

#include <boost/numeric/odeint/integrate/null_observer.hpp>

#include "./generic_system.hpp"

namespace sam {

/*! \brief A system that is integrated with an Euler method of order O(dt).
 *
 * A system that is integrated with an Euler method of order O(dt). The Euler
 * method is defined as
 * \f[ x_{n+1} = x_{n} + f_n dt, \f]
 * where \f$ f_n \f$ is the derivative at timestep \f$ n \f$.
 */
template<typename ODE, typename state_type = std::vector<double>>
class EulerSystem: public GenericSystem<ODE, state_type> {
 public:
  template<typename... Ts>
  explicit EulerSystem(unsigned int system_size, unsigned int dimension,
                       Ts... parameters);

  template<typename observer_type = boost::numeric::odeint::null_observer>
  void Integrate(double dt, unsigned int number_steps,
                 observer_type observer
                     = boost::numeric::odeint::null_observer());

 private:
  template<typename system_type, typename observer_type>
  double EulerMethod(system_type system, state_type& x, double t, double dt,
                     unsigned int number_steps, observer_type observer);
};

// Implementation

template<typename ODE, typename state_type>
template<typename... Ts>
EulerSystem<ODE, state_type>::EulerSystem(unsigned int system_size,
                                          unsigned int dimension,
                                          Ts... parameters)
      : GenericSystem<ODE, state_type>(system_size, dimension, parameters...) {}

template<typename ODE, typename state_type>
template<typename observer_type>
void EulerSystem<ODE, state_type>::Integrate(double dt,
                                             unsigned int number_steps,
                                             observer_type observer) {
    this->t_ = EulerMethod(*(this->ode_), this->x_, this->t_, dt, number_steps,
                           observer);
}

template<typename ODE, typename state_type>
template<typename system_type, typename observer_type>
double EulerSystem<ODE, state_type>::EulerMethod(system_type system,
                                                 state_type& x, double t,
                                                 double dt,
                                                 unsigned int number_steps,
                                                 observer_type observer) {
  observer(x, t);
  for (unsigned int i = 0; i < number_steps; ++i) {
    // TODO(boundter): copy the value? how to best initialize?
    state_type dx = x;
    system(x, dx, t);
    // TODO(boundter): Rather use iterators
    for (size_t j = 0; j < x.size(); ++j) x[j] += dx[j]*dt;
    t += dt;
    observer(x, t);
  }
  return t;
}

}  // namespace sam

#endif  // INCLUDE_SAM_SYSTEM_EULER_SYSTEM_HPP_


#include "Mbr.h"
#include <iostream>
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <vector>
#include <iterator>
#include <string>
#include <limits>
#include <algorithm>
#include <math.h>
using namespace std;
Mbr::Mbr()
{
}

Mbr::Mbr(float x1, float y1, float x2, float y2)
{
    this->x1 = x1;
    this->y1 = y1;
    this->x2 = x2;
    this->y2 = y2;
}

void Mbr::update(Point point)
{
    update(point.x, point.y);
}

void Mbr::update(float x, float y)
{
    if (x < x1)
    {
        x1 = x;
    }
    if (y < y1)
    {
        y1 = y;
    }
    if (x > x2)
    {
        x2 = x;
    }
    if (y > y2)
    {
        y2 = y;
    }
}

void Mbr::update(Mbr mbr)
{
    if (mbr.x1 < x1)
    {
        x1 = mbr.x1;
    }
    if (mbr.y1 < y1)
    {
        y1 = mbr.y1;
    }
    if (mbr.x2 > x2)
    {
        x2 = mbr.x2;
    }
    if (mbr.y2 > y2)
    {
        y2 = mbr.y2;
    }
}

bool Mbr::contains(Point point)
{
    if (x1 > point.x || point.x > x2 || y1 > point.y || point.y > y2)
    {
        return false;
    }
    else
    {
        return true;
    }
}

bool Mbr::strict_contains(Point point)
{
    if (x1 < point.x && point.x < x2 && y1 < point.y && point.y < y2)
    {
        return true;
    }
    else
    {
        return false;
    }
}

bool Mbr::interact(Mbr mbr)
{
    if ((x1 <= mbr.x1 && mbr.x1 <= x2 && y1 <= mbr.y1 && mbr.y1 <= y2) || (x1 <= mbr.x1 && mbr.x1 <= x2 && y1 <= mbr.y2 && mbr.y2 <= y2) || (x1 <= mbr.x2 && mbr.x2 <= x2 && y1 <= mbr.y1 && mbr.y1 <= y2) || (x1 <= mbr.x2 && mbr.x2 <= x2 && y1 <= mbr.y2 && mbr.y2 <= y2))
    {
        return true;
    }
    if ((mbr.x1 <= x1 && x1 <= mbr.x2 && mbr.y1 <= y1 && y1 <= mbr.y2) || (mbr.x1 <= x1 && x1 <= mbr.x2 && mbr.y1 <= y2 && y2 <= mbr.y2) || (mbr.x1 <= x2 && x2 <= mbr.x2 && mbr.y1 <= y1 && y1 <= mbr.y2) || (mbr.x1 <= x2 && x2 <= mbr.x2 && mbr.y1 <= y2 && y2 <= mbr.y2))
    {
        return true;
    }
    return false;
}

vector<Mbr> Mbr::get_mbrs(vector<Point> dataset, float area, int num, float ratio)
{

    vector<Mbr> mbrs;
    srand(time(0));
    int maxInt = numeric_limits<int>::max();
    float x = sqrt(area * ratio);
    float y = sqrt(area / ratio);
    int i = 0;
    int length = dataset.size();
    while (i < num)
    {
        int index = rand() % length;
        Point point = dataset[index];
        if (point.x + x <= 1 && point.y + y <= 1)
        {
            Mbr mbr(point.x, point.y, point.x + x, point.y + y);
            mbrs.push_back(mbr);
            i++;
        }
    }

    return mbrs;
}

float Mbr::cal_dist(Point point)
{
    if (this->contains(point))
    {
        return 0;
    }
    else
    {
        float dist;
        if (point.x < x1)
        {
            if (point.y < y1)
            {
                dist = sqrt(pow((point.x - x1), 2) + pow((point.y - y1), 2));
            }
            else if (point.y <= y2)
            {
                dist = x1 - point.x;
            }
            else
            {
                dist = sqrt(pow((point.x - x1), 2) + pow((point.y - y2), 2));
            }
        }
        else if (point.x <= x2)
        {
            if (point.y < y1)
            {
                dist = y1 - point.y;
            }
            else
            {
                dist = point.y - y2;
            }
        }
        else
        {
            if (point.y < y1)
            {
                dist = sqrt(pow((point.x - x2), 2) + pow((point.y - y1), 2));
            }
            else if (point.y <= y2)
            {
                dist = point.x - x2;
            }
            else
            {
                dist = sqrt(pow((point.x - x2), 2) + pow((point.y - y2), 2));
            }
        }
        return dist;
    }
}

void Mbr::print()
{
    cout << "(x1=" << x1 << " y1=" << y1 << " x2=" << x2 << " y2=" << y2 << ")" << endl;
}

vector<Point> Mbr::get_corner_points()
{
    vector<Point> result;
    Point point1(0, x1, y1);
    Point point2(0, x2, y1);
    Point point3(0, x1, y2);
    Point point4(0, x2, y2);
    result.push_back(point1);
    result.push_back(point2);
    result.push_back(point3);
    result.push_back(point4);
    return result;
}

Mbr Mbr::get_mbr(Point point, float knnquerySide)
{
    float x1 = point.x - knnquerySide;
    float x2 = point.x + knnquerySide;
    float y1 = point.y - knnquerySide;
    float y2 = point.y + knnquerySide;

    x1 = x1 < 0 ? 0 : x1;
    y1 = y1 < 0 ? 0 : y1;

    x2 = x2 > 1 ? 1 : x2;
    y2 = y2 > 1 ? 1 : y2;

    Mbr mbr(x1, y1, x2, y2);
    return mbr;
}

void Mbr::clean()
{
    x1 = 0;
    x2 = 0;
    y1 = 0;
    y2 = 0;
}

string Mbr::get_self()
{
    return to_string(x1) + "," + to_string(y1) + "," + to_string(x2) + "," + to_string(y2) + "\n";
}


#pragma once

#include <iostream>
#include <functional>
#include <iomanip>

#include <boost/spirit/home/x3.hpp>

#include "ast.hpp"
#include "ast_adapted.hpp"
#include "parser.hpp"

namespace client {
namespace parser {
    namespace x3 = boost::spirit::x3;

    expression_type const expression{"expression"};
    conditional_type const conditional{"conditional"};
    primary_type const primary{"primary"};
    logical_type const logical{"logical"};
    equality_type const equality{"equality"};
    relational_type const relational{"relational"};
    multiplicative_type const multiplicative{"multiplicative"};
    variable_type const variable{"variable"};

#define add_operation(NAME, OP) this->add(NAME, {NAME, OP});

    struct multiplicative_op_ : x3::symbols<ast::binary_operator> {
        multiplicative_op_() {
            add_operation("%", [](uint a, uint b) { return std::fmod(a, b); });
        }
    } multiplicative_op;

    struct logical_op_ : x3::symbols<ast::binary_operator> {
        logical_op_() {
            add_operation("&&", std::logical_and<uint>{});
            add_operation("||", std::logical_or<uint>{});
        }
    } logical_op;

    struct relational_op_ : x3::symbols<ast::binary_operator> {
        relational_op_() {
            add_operation("<", std::less<uint>{});
            add_operation("<=", std::less_equal<uint>{});
            add_operation(">", std::greater<uint>{});
            add_operation(">=", std::greater_equal<uint>{});
        }
    } relational_op;

    struct equality_op_ : x3::symbols<ast::binary_operator> {
        equality_op_() {
            add_operation("==", std::equal_to<uint>{});
            add_operation("!=", std::not_equal_to<uint>{});
        }
    } equality_op;
#undef add_operation

    auto make_conditional_op = [](auto& ctx) {
        using boost::fusion::at_c;
        x3::_val(ctx) = ast::conditional_op{
            x3::_val(ctx), at_c<0>(x3::_attr(ctx)), at_c<1>(x3::_attr(ctx))};
    };

    // Rule defintions
    auto const expression_def = conditional;

    auto const conditional_def =
        logical[([](auto& ctx) { _val(ctx) = _attr(ctx); })] >>
        -('?' > expression > ':' > expression)[make_conditional_op];

    auto const logical_def = equality >> *((logical_op > logical) |
                                           (logical_op > equality));

    auto const equality_def = relational >> *(equality_op > relational);

    auto const relational_def = multiplicative >>
                                *(relational_op > multiplicative);

    auto const multiplicative_def = primary >> *(multiplicative_op > primary);

    auto const primary_def = x3::uint_ | ('(' > expression > ')') | variable;

    auto const variable_def = x3::lexeme[x3::alpha >> *x3::alnum];
    // clang-format on

    BOOST_SPIRIT_DEFINE(
        expression,
        logical,
        equality,
        relational,
        multiplicative,
        primary,
        conditional,
        variable);

} // namespace parser

parser::expression_type
expression() {
    return parser::expression;
}

} // namespace client


/*
MIT License

Copyright (c) 2019 Kalu U. Ogbureke

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.

Authors: Kalu U. Ogbureke
Change Log: 01.04.2019 - Version 1.0.0
*/
#include <iostream>
#include <cmath>
#include <boost/program_options.hpp>
#include "libutil.h"
#include "optimizer.h"

using namespace mlearn;
namespace po = boost::program_options;
/**
    The trainModel functions is responsible for training.

    @param mode Program usage mode: train
    @param optimizer optimization algorithm used: sgd, adagrad, rmsprop
    @param cost_function Cost function used: mse, mae, crossentropy
    @param learning_rate Learning rate used for training
    @param batch_size Batch size used for training
    @param num_epochs Number of training epochs
    @param lambda Regularization parameter used for training
    @param reg Type of regularization (L1, L2 or None)
    @param beta Momentum term/parameter
    @param dataset Path to the train or test file
    @param header Determines whether dataset file contains header
    @param feature_dim Number of input points/features
    @param label_dim Number of output points/labels
    @param test_size % of train dataset used for validation
    @param num_hidden_layers Number of hidden layers
    @param hidden_dim Number of hidden units
    @param hidden_activation Activation function of hidden layers
    @param output_activation Activation function of the output layer
    @param model_file Path to model file (for saving trained model)
*/
int trainModel(
          std::string mode,
          std::string optimizer,
          std::string cost_function,
          double learning_rate,
          uint32_t batch_size,
          uint32_t num_epochs,
          double lambda,
          std::string reg,
          double beta,
          std::string dataset,
          bool header,
          uint64_t feature_dim,
          uint64_t label_dim,
          double test_size,
          uint16_t num_hidden_layers,
          uint64_t hidden_dim,
          std::string hidden_activation,
          std::string output_activation,
          std::string model_file
          );
/**
    The testModel functions is responsible for testing
    on a held-out set with a trained model.

    @param mode Program usage mode: test
    @param cost_function Cost function used: mse, mae, crossentropy
    @param dataset Path to the train or test file
    @param header Determines whether dataset file contains header
    @param feature_dim Number of input points/features
    @param label_dim Number of output points/labels
    @param model_file Path to model file (for saving trained model)
*/
int testModel(
            std::string mode,
            std::string cost_function,
            std::string dataset,
            bool header,
            uint64_t feature_dim,
            uint64_t label_dim,
            std::string model_file
            );
/**
    The is the entry point of the program. It processes user
    command line options and implements the train or test functions.
*/
int main(int argc, char** argv)
{
    std::string mode = "train";
    std::string optimizer = "sgd";
    std::string cost_function = "MSE";
    double learning_rate = 0.1;
    uint32_t batch_size = 10;
    uint32_t num_epochs = 20;
    double lambda = 0.0;
    std::string reg = "None";
    double beta = 0.0;
    std::string dataset = "mnist";
    bool header = false;
    uint64_t feature_dim = 0;
    uint64_t label_dim = 0;
    double test_size = 0;
    uint16_t num_hidden_layers = 1;
    uint64_t hidden_dim = 0;
    std::string hidden_activation = "sigmoid";
    std::string output_activation = "sigmoid";
    std::string model_file = "temp.bin";
    po::variables_map vm;
    try
    {

        po::options_description desc("Training Options");
        desc.add_options()
            ("help,h", "gives information about usage")
            ("mode,m", po::value<std::string>(&mode)->default_value("train"), "program usage mode: either train or test")
            ("optimizer,o", po::value<std::string>(&optimizer)->default_value("sgd"), "optimization algorithm used: sgd, adagrad, rmsprop")
            ("cost_function,c", po::value<std::string>(&cost_function)->default_value("crossentropy"), "cost function used: mse, mae, crossentropy")
            ("learning_rate,r", po::value<double>(&learning_rate)->default_value(0.05), "learning rate used for training")
            ("batch_size,s", po::value<uint32_t>(&batch_size)->default_value(10), "batch size used for training")
            ("num_epochs,n", po::value<uint32_t>(&num_epochs)->default_value(20), "number of training epochs")
            ("lambda,l", po::value<double>(&lambda)->default_value(0.0), "regularization parameter used for training")
            ("reg,R", po::value<std::string>(&reg)->default_value("None"), "type of regularization (L1, L2 or None)")
            ("beta,b", po::value<double>(&beta)->default_value(0.0), "momentum term/parameter")
            ("dataset,d", po::value<std::string>(&dataset)->default_value("mnist"), "the path to the train or test file")
            ("header,H", po::value<bool>(&header)->default_value(false), "determines whether dataset file contains header")
            ("feature_dim,F", po::value<uint64_t>(&feature_dim)->default_value(784), "number of input points/features")
            ("label_dim,L", po::value<uint64_t>(&label_dim)->default_value(10), "number of output points/labels")
            ("test_size,t", po::value<double>(&test_size)->default_value(0.0), "% of train dataset used for validation")
            ("num_hidden_layers,N", po::value<uint16_t>(&num_hidden_layers)->default_value(1), "number of hidden layers")
            ("hidden_dim,D", po::value<uint64_t>(&hidden_dim)->default_value(100), "number of hidden units")
            ("hidden_activation,a", po::value<std::string>(&hidden_activation)->default_value("sigmoid"), "activation function of hidden layers")
            ("output_activation,A", po::value<std::string>(&output_activation)->default_value("softmax"), "activation function of output layer")
            ("model_file,f", po::value<std::string>(&model_file)->default_value("temp.bin"), "path to model file")
            ;

        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);

        if (vm.count("help"))
        {
            std::cout << desc << std::endl;
            return 0;
        }
        if (vm.count("mode"))mode = vm["mode"].as<std::string>();
        if (vm.count("optimizer"))optimizer = vm["optimizer"].as<std::string>();
        if (vm.count("cost_function"))cost_function = vm["cost_function"].as<std::string>();
        if (vm.count("learning_rate"))learning_rate = vm["learning_rate"].as<double>();
        if (vm.count("batch_size"))batch_size = vm["batch_size"].as<uint32_t>();
        if (vm.count("num_epochs"))num_epochs = vm["num_epochs"].as<uint32_t>();
        if (vm.count("lambda"))lambda = vm["lambda"].as<double>();
        if (vm.count("reg"))reg = vm["reg"].as<std::string>();
        if (vm.count("beta"))beta = vm["beta"].as<double>();
        if (vm.count("dataset"))dataset = vm["dataset"].as<std::string>();
        if (vm.count("header"))header = vm["header"].as<bool>();
        if (vm.count("feature_dim"))feature_dim = vm["feature_dim"].as<uint64_t>();
        if (vm.count("label_dim"))label_dim = vm["label_dim"].as<uint64_t>();
        if (vm.count("test_size"))test_size = vm["test_size"].as<double>();
        if (vm.count("num_hidden_layers"))num_hidden_layers = vm["num_hidden_layers"].as<uint16_t>();
        if (vm.count("hidden_dim"))hidden_dim = vm["hidden_dim"].as<uint64_t>();
        if (vm.count("hidden_activation"))hidden_activation = vm["hidden_activation"].as<std::string>();
        if (vm.count("output_activation"))output_activation = vm["output_activation"].as<std::string>();
        if (vm.count("model_file"))model_file = vm["model_file"].as<std::string>();

    }
    catch(std::exception& e)
    {
        std::cerr << "error: " << e.what() << std::endl;
        exit(EXIT_FAILURE);
    }

    if(vm["mode"].as<std::string>() == "train")
    {
        std::set<std::string> optimizers{"sgd", "adagrad", "rmsprop", "psdsquare"};
        std::set<std::string> cost_functions{"mse", "mae", "crossentropy"};
        if (optimizers.count(optimizer) == 0)
        {
            std::cerr << "optimizer error: sgd, adagrad, rmsprop, psdsquare" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (cost_functions.count(cost_function) == 0)
        {
            std::cerr << "cost function error: mse, mse, crossentropy" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (cost_function == "crossentropy" && output_activation != "softmax")
        {

            std::cerr << "cost function/output activation error: crossentropy goes with softmax!" << std::endl;
            exit(EXIT_FAILURE);
        }
        std::cout << "-----T r a i n   C o n f i g u r a t i o n-----" << std::endl << std::endl;
        std::cout << "program usage mode: " << vm["mode"].as<std::string>() << std::endl;
        std::cout << "optimization algorithm used: " << vm["optimizer"].as<std::string>() << std::endl;
        std::cout << "cost function used: " << vm["cost_function"].as<std::string>() << std::endl;
        std::cout << "learning rate: " << vm["learning_rate"].as<double>() << std::endl;
        std::cout << "batch size: " << vm["batch_size"].as<uint32_t>() << std::endl;
        std::cout << "number of training epochs: " << vm["num_epochs"].as<uint32_t>() << std::endl;
        std::cout << "regularization parameter lambda: " << vm["lambda"].as<double>() << std::endl;
        std::cout << "type of regularization: " << vm["reg"].as<std::string>() << std::endl;
        std::cout << "momentum term/parameter beta: " << vm["beta"].as<double>() << std::endl;
        std::cout << "dataset: " << vm["dataset"].as<std::string>() << std::endl;
        std::cout << "dataset header info: " << vm["header"].as<bool>() << std::endl;
        std::cout << "input feature dimension: " << vm["feature_dim"].as<uint64_t>() << std::endl;
        std::cout << "output label dimension: " << vm["label_dim"].as<uint64_t>() << std::endl;
        std::cout << "% validation: " << vm["test_size"].as<double>() << std::endl;
        std::cout << "number of hidden layers: " << vm["num_hidden_layers"].as<uint16_t>() << std::endl;
        std::cout << "number of hidden units: " << vm["hidden_dim"].as<uint64_t>() << std::endl;
        std::cout << "activation function of hidden layers: " << vm["hidden_activation"].as<std::string>() << std::endl;
        std::cout << "activation function of output layer: " << vm["output_activation"].as<std::string>() << std::endl;
        std::cout << "path to model file: " << vm["model_file"].as<std::string>() << std::endl;

        trainModel(
                mode,
                optimizer,
                cost_function,
                learning_rate,
                batch_size,
                num_epochs,
                lambda,
                reg,
                beta,
                dataset,
                header,
                feature_dim,
                label_dim,
                test_size,
                num_hidden_layers,
                hidden_dim,
                hidden_activation,
                output_activation,
                model_file
                );
    }
    else if(vm["mode"].as<std::string>() == "test")
    {
        std::cout << "-----T e s t   C o n f i g u r a t i o n-----" << std::endl << std::endl;
        std::cout << "program usage mode: " << vm["mode"].as<std::string>() << std::endl;
        std::cout << "dataset: " << vm["dataset"].as<std::string>() << std::endl;
        std::cout << "dataset header info: " << vm["header"].as<bool>() << std::endl;
        std::cout << "input feature dimension: " << vm["feature_dim"].as<uint64_t>() << std::endl;
        std::cout << "output label dimension: " << vm["label_dim"].as<uint64_t>() << std::endl;
        std::cout << "path to model file: " << vm["model_file"].as<std::string>() << std::endl;

        testModel(mode, cost_function, dataset, header, feature_dim, label_dim, model_file);
    }
    else
    {
        std::cerr << "error: program mode not set/wrong mode" << std::endl;
        exit(EXIT_FAILURE);
    }

    return 0;
}

int trainModel(
          std::string mode,
          std::string optimizer,
          std::string cost_function,
          double learning_rate,
          uint32_t batch_size,
          uint32_t num_epochs,
          double lambda,
          std::string reg,
          double beta,
          std::string dataset,
          bool header,
          uint64_t feature_dim,
          uint64_t label_dim,
          double test_size,
          uint16_t num_hidden_layers,
          uint64_t hidden_dim,
          std::string hidden_activation,
          std::string output_activation,
          std::string model_file
          )
{

    Activation<double> a_hidden(hidden_activation), a_output(output_activation);
    Network<double>* model(nullptr);
    Optimizer<double>* opt(nullptr);
    Layer<double>* layer(nullptr);
    DataReader<double>* train(nullptr);
    DataReader<double>* validation(nullptr);

    if(cost_function == "mse") model = new Network<double>(new MSE<double>);
    else if(cost_function == "mae") model = new Network<double>(new MAE<double>);
    else if(cost_function == "crossentropy") model =  new Network<double>(new CrossEntropy<double>);
    if(num_hidden_layers > 0)
    {
        layer = new Layer<double>(feature_dim, hidden_dim, "hidden", a_hidden);
        model->addLayer(*layer);
        if(layer)delete layer;
    }

    for(uint16_t i = 1; i < num_hidden_layers; ++i)
    {
        layer = new Layer<double>(hidden_dim, hidden_dim, "hidden", a_hidden);
        model->addLayer(*layer);
        if(layer)delete layer;
    }
    if(num_hidden_layers > 0)layer = new Layer<double>(hidden_dim, label_dim, "output", a_output);
    else layer = new Layer<double>(feature_dim, label_dim, "output", a_output);
    model->addLayer(*layer);
    if(layer)delete layer;
    model->connectLayers();
    if(optimizer == "sgd") opt = new SGD<double>(learning_rate, batch_size, num_epochs, lambda, reg, beta);
    else if(optimizer == "adagrad") opt = new Adagrad<double>(learning_rate, batch_size, num_epochs, lambda, reg, beta);
    else if(optimizer == "rmsprop") opt = new RMSProp<double>(learning_rate, batch_size, num_epochs, lambda, reg, beta);
    else if(optimizer == "psdsquare") opt = new PSDSquare<double>(learning_rate, batch_size, num_epochs, lambda, reg, beta);
    else
    {
        std::cerr << "optimizer is not implemented" << std::endl;
        exit(EXIT_FAILURE);
    }

    if (dataset == "mnist")
    {
        train = new MNISTReader<double>("data/mnist_train.csv", ',',  false);
        train->read();
        if(test_size > 0.0)
        {
            validation = new MNISTReader<double>;
            train->trainTestSplit(*validation, test_size);
            opt->train(*model, model_file, train, validation);
        }
        else opt->train(*model, model_file, train);
    }
    else
    {
        train = new GenericReader<double>(dataset, feature_dim, label_dim, ',',  header);
        train->read();
        if(test_size > 0.0)
        {
            validation = new GenericReader<double>;
            train->trainTestSplit(*validation, test_size);
            opt->train(*model, model_file, train, validation);
        }
        else opt->train(*model, model_file, train);

    }
    if(opt)delete opt;
    if(model)delete model;
    if(train)delete train;
    if(validation)delete validation;
    return 0;

}
int testModel(
            std::string mode,
            std::string cost_function,
            std::string dataset,
            bool header,
            uint64_t feature_dim,
            uint64_t label_dim,
            std::string model_file
            )
{
    std::set<std::string> cost_functions{"mse", "mae", "crossentropy"};
    Optimizer<double>* opt = new SGD<double>;
    DataReader<double>* test(nullptr);
    Network<double>* model(nullptr);
    double accuracy;
    std::string test_info;

    if (cost_functions.count(cost_function) == 0)
    {
        std::cerr << "cost function error: mse, mse, crossentropy" << std::endl;
        exit(EXIT_FAILURE);
    }
    if(cost_function == "mse") model = new Network<double>(new MSE<double>);
    else if(cost_function == "mae") model = new Network<double>(new MAE<double>);
    else if(cost_function == "crossentropy") model =  new Network<double>(new CrossEntropy<double>);

    if (dataset == "mnist")
    {
        try
        {
            test = new MNISTReader<double>("data/mnist_test.csv", ',',  false);
            test->read();
        }catch (std::exception &e){
            std::cerr << e.what() << std::endl;
            exit(EXIT_FAILURE);
        }
    }
    else
    {
        try
        {
            test = new GenericReader<double>(dataset, feature_dim, label_dim, ',',  header);
            test->read();
        }catch (std::exception &e){
            std::cerr << e.what() << std::endl;
            exit(EXIT_FAILURE);
        }
    }
    accuracy = opt->predict(*model, test, model_file);
    if (cost_function == "mse")test_info = "MSE: " + std::to_string(accuracy);
    else if (cost_function == "mae")test_info = "MAE: " + std::to_string(accuracy);
    else if (cost_function == "crossentropy")test_info = "Classification Accuracy: " + std::to_string(accuracy * 100) + " %";
    if(opt)delete opt;
    if(test)delete test;
    if(model)delete model;
    std::cout << test_info << std::endl;

    return 0;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/abss.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>

STF_CASE_TPL( "Check abss behavior with floating", STF_IEEE_TYPES )
{
  namespace bs = boost::simd;
  using bs::abss;
  using r_t = decltype(abss(T()));
  STF_TYPE_IS(r_t, T);

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(abss(bs::Inf<T>()),  bs::Inf<r_t>());
  STF_EQUAL(abss(bs::Minf<T>()), bs::Inf<r_t>());
  STF_IEEE_EQUAL(abss(bs::Nan<T>()),  bs::Nan<r_t>());
#endif
  STF_EQUAL(abss(bs::Mone<T>()), bs::One<T>());
  STF_EQUAL(abss(bs::One<T>()), bs::One<T>());
  STF_EQUAL(abss(bs::Valmax<T>()), bs::Valmax<T>());
  STF_EQUAL(abss(bs::Valmin<T>()), bs::Valmax<T>());
  STF_EQUAL(abss(bs::Zero<T>()), bs::Zero<T>());
}


/**
 * Some part of the source code is from the Rodinia benchmark suite.
 * Link quoted in the README of the original implementation: http://weather.unisys.com/hurricane/
 */

#include <boost/math/special_functions/next.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <tuple>

#include "../device_hopper/core.h"

#include <omp.h>
#include <sys/mman.h>

#define RODINIA_NN_EPSILON 10e-05
#define REPOSITORY_PATH std::string(std::getenv("PLASTICITY_ROOT"))
#define PREFERRED_DEVICE CPU

using namespace device_hopper;

typedef struct latLong
{
    float lat;
    float lng;
} LatLong;

void gen_data(struct latLong *locations, int numRecods) {
    for (size_t record_i = 0; record_i < numRecods; ++record_i) {
        locations[record_i].lat = ((float) (7 + rand() % 63)) + ((float) rand() / (float) 0x7fffffff);
        locations[record_i].lng = ((float) (rand() % 358)) + ((float) rand() / (float) 0x7fffffff);
    }
}

bool verifyResults(
        float* ocl_recordDistances,
        int numRecords,
        struct latLong *locations,
        float latitude,
        float longitude) {

    float *ref_distances = (float *)malloc(sizeof(float) * numRecords);

    // calculate distances on CPU
    omp_set_num_threads(4);
#pragma omp parallel for
    for (int i = 0; i < numRecords; i++) {
        const struct latLong location = locations[i];
        ref_distances[i] = (float) sqrt((latitude  - location.lat) * (latitude  - location.lat) +
                                        (longitude - location.lng) * (longitude - location.lng));
    }

    // compare to ocl result
    bool results_are_correct = true;
    float largest_difference = 0;
    float current_difference = 0;
    for (int i = 0; i < numRecords; i++) {
        current_difference = abs(ref_distances[i] - ocl_recordDistances[i]);
        if (current_difference > largest_difference) largest_difference = current_difference;
        if (current_difference > RODINIA_NN_EPSILON) {
            if (results_are_correct) {
                    std::cout << "Distance mismatch at index " << i << " by: " <<
                              abs(ref_distances[i] - ocl_recordDistances[i]) << "\n";
                    std::cout << "OCL: " << std::setprecision(10) << ocl_recordDistances[i] << "\n";
                    std::cout << "Ref: " << std::setprecision(10) << ref_distances[i] << "\n";
                    //std::cout << "Record: " << records[i].recString << std::endl;
                    results_are_correct = false;
            }
        }
    }
    std::cout << "Largest epsilon: " << largest_difference << std::endl;

    free(ref_distances);
    return results_are_correct;
}

DEVICE_HOPPER_MAIN(int argc, char* argv[]) {
    DEVICE_HOPPER_SETUP

    // Parse CLI parameters
    boost::program_options::options_description desc("Options");
    desc.add_options() ("problem-size", boost::program_options::value<long>(), "Sample count");
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    size_t problem_size = 0;
    if (vm.count("problem-size") == 0) {
        std::cerr << "Error: Problem size is missing." << std::endl;
        std::exit(EXIT_FAILURE);
    } else {
        // This parameter is passed to the application by the Python scripts that automate the experiments.
        problem_size = vm["problem-size"].as<long>();
    }

    // TODO The data paths are currently all hard coded
    // Choose the problem size
    std::string data_dir;
    std::string data_file;
    int numRecords = 0;
    if (problem_size == 1) {
        data_dir = "small";
        data_file = "list81920k_4.txt";
        numRecords = 81920 * 1000;
    } else if (problem_size == 2) {
        data_dir = "medium";
        data_file = "list148480k_8.txt";
        numRecords = 148480 * 1000;
    } else if (problem_size == 3) {
        data_dir = "large";
        data_file = "list368640k_8.txt";
        numRecords = 368640 * 1000;
    } else {
        std::cerr << "Error: Unknown problem size" << std::endl;
        std::exit(EXIT_FAILURE);
    }
    // TODO this is currently not used
    std::string benchmark_data_root = REPOSITORY_PATH + "/benchmarks/input_data/rodinia/nn/" + data_dir;

    // Allocate input buffer
    struct latLong *locations = (struct latLong *) device_hopper::malloc(numRecords, sizeof(struct latLong));

    // Load input data
    gen_data(locations, numRecords);

    // Allocate output buffer
    float *recordDistances = (float *) device_hopper::malloc(numRecords, sizeof(float));

    // Default kernel parameters
    float latitude=30.0;
    float longitude=90.0;

    // Create parallel for
    parallel_for pf(0, numRecords, [=]DEVICE_HOPPER_LAMBDA() {
        int i = GET_ITERATION();
        struct latLong *latLong = locations+i;
        if (i < numRecords) { // Not necessary with our programming model
            float *dist = recordDistances + i;
            *dist = (float) sqrt((latitude-latLong->lat)  * (latitude-latLong->lat) +
                                 (longitude-latLong->lng) * (longitude-latLong->lng));
        }
    });
    // Register buffers and describe accesses
    pf.add_buffer_access_patterns(
        device_hopper::buf(locations,       direction::IN,  pattern::SUCCESSIVE_SUBSECTIONS(pf.batch_size)),
        device_hopper::buf(recordDistances, direction::OUT, pattern::SUCCESSIVE_SUBSECTIONS(pf.batch_size)));
    // Add scalar kernel parameters
    pf.add_scalar_parameters(latitude, longitude, numRecords);
    // Set optional tuning parameters and call run()
    pf.opt_set_batch_size(256).opt_set_simple_indices(true).opt_set_is_idempotent(true).run();

    // Check if the results are correct
    if (!verifyResults(
            recordDistances,
            numRecords,
            locations,
            latitude,
            longitude)) {
        // Do not change these messages and the return codes.
        // The Python scripts that automate the experiments expect them.
        std::cout << "Error: The results are incorrect" << std::endl;
        return EXIT_FAILURE;
    } else {
        std::cout << "Info: The results are correct" << std::endl;
    }

    free(locations);
    free(recordDistances);

    return EXIT_SUCCESS;
}


#include "worldengine/images/elevation_image.h"

#include <boost/iterator/zip_iterator.hpp>
#include <boost/log/trivial.hpp>

namespace WorldEngine
{
ElevationImage::ElevationImage(const World& world, bool shadow) :
    Image(world, false), shadow_(shadow)
{
}
ElevationImage::~ElevationImage() {}

void ElevationImage::DrawImage(boost::gil::rgb8_image_t::view_t& target)
{
   const ElevationArrayType& e     = world_.GetElevationData();
   const OceanArrayType&     ocean = world_.GetOceanData();

   float seaLevel = world_.oceanLevel();
   bool  hasOcean = (seaLevel != NAN) && (!ocean.empty());
   float minElev  = std::numeric_limits<float>::infinity();
   float maxElev  = -std::numeric_limits<float>::infinity();

   for (uint32_t y = 0; y < world_.height(); y++)
   {
      for (uint32_t x = 0; x < world_.width(); x++)
      {
         if (!hasOcean || !ocean[y][x])
         {
            if (minElev > e[y][x])
            {
               minElev = e[y][x];
            }
            if (maxElev < e[y][x])
            {
               maxElev = e[y][x];
            }
         }
      }
   }

   BOOST_LOG_TRIVIAL(debug) << "minElev = " << minElev;
   BOOST_LOG_TRIVIAL(debug) << "maxElev = " << maxElev;

   float elevDelta = maxElev - minElev;
   float elevation;

   for (uint32_t y = 0; y < world_.height(); y++)
   {
      for (uint32_t x = 0; x < world_.width(); x++)
      {
         if (hasOcean && ocean[y][x])
         {
            target(x, y) = boost::gil::rgb8_pixel_t(0, 0, 255);
         }
         else
         {
            elevation = ((e[y][x] - minElev) / elevDelta);
            int32_t c = static_cast<int32_t>(255 - elevation * 255);

            if (shadow_ && y > 2 && x > 2)
            {
               if (e[y - 1][x - 1] > e[y][x])
               {
                  c -= 15;
               }
               if (e[y - 2][x - 2] > e[y][x] &&
                   e[y - 2][x - 2] > e[y - 1][x - 1])
               {
                  c -= 10;
               }
               if (e[y - 3][x - 3] > e[y][x] &&
                   e[y - 3][x - 3] > e[y - 1][x - 1] &&
                   e[y - 3][x - 3] > e[y - 2][x - 2])
               {
                  c -= 5;
               }
               if (c < 0)
               {
                  c = 0;
               }
            }

            target(x, y) = boost::gil::rgb8_pixel_t(static_cast<uint8_t>(c),
                                                    static_cast<uint8_t>(c),
                                                    static_cast<uint8_t>(c));
         }
      }
   }
}
} // namespace WorldEngine


#include <algorithm>
#include <array>
#include <charconv>
#include <iostream>
#include <unordered_map>

#include <boost/range/irange.hpp>

#include "input.hpp"

using uintmax = std::uintmax_t;

// compile-time parsing so I can use arrays later on without magic numbers
struct ParseStats {
  uintmax num_words,
          num_lines;
};

constexpr auto SPACE = ' ';
constexpr auto NEWLINE = '\n';

constexpr auto parse_stats(std::string_view input) {
  auto stats = ParseStats{};

  auto& num_words = stats.num_words;
  auto& num_lines = stats.num_lines;

  for(auto c : input) {
    num_words += (c == SPACE);
    num_lines += (c == NEWLINE);
  }

  // there's no newline character on the last line, so we need to add it manually
  ++num_lines;
  // splitting by whitespace misses the last word, so we need to adjust manually, too
  num_words /= num_lines;
  ++num_words;

  return stats;
}

constexpr auto PARSE_STATS = parse_stats(puzzle_input);

constexpr auto NUM_WORDS = PARSE_STATS.num_words - 2;
constexpr auto NUM_LINES = PARSE_STATS.num_lines;

constexpr auto NUM_AUNTS = NUM_LINES;
constexpr auto NUM_CLUES = 3;

auto split_input(std::string_view input) {
  auto lines = std::array<decltype(input), NUM_LINES>{};
  decltype(input.size()) pos = 0;
  auto index = 0;
  while(pos != input.npos) {
    pos = input.find(NEWLINE);
    lines[index++] = input.substr(0, pos);
    input.remove_prefix(pos + 1);
  }
  return lines;
}

auto split_line(std::string_view line, std::string_view delims = " ,:") {
  auto words = std::array<decltype(line), NUM_WORDS>{};
  decltype(line.size()) pos = 0;
  auto index = 0;
  while(pos != line.npos) {
    const auto end = line.find_first_of(delims);
    words[index++] = line.substr(0, end);
    pos = line.find_first_not_of(delims, end);
    line.remove_prefix(pos);
  }

  return words;
}

// no sorting necessary, so unordered_map it is
using Aunt = std::unordered_map<std::string_view, uintmax>;
using Aunts = std::array<Aunt, NUM_AUNTS>;

auto parse(std::string_view input) {

  auto aunts = Aunts{};
  auto index = 0;

  const auto to_int = [] (auto input) {
    uintmax value = 0;
    std::from_chars(input.begin(), input.end(), value);
    return value;
  };

  for(auto line : split_input(input)) {

    // ignore the aunt's id, we will get it from the array's index, if needed
    const auto pos = line.find(':');
    line.remove_prefix(pos + 2);

    const auto& words = split_line(line);

    auto& aunt = aunts[index++];

    for(const auto i : boost::irange(decltype(NUM_WORDS){}, NUM_WORDS, 2)) {
      auto name = words[i];
      aunt[name] = to_int(words[i+1]);
    }
  }

  return aunts;
}

auto solution(std::string_view input) {

  const auto aunts = parse(input);

  const auto sue = Aunt{
    {"children", 3},
    {"cats", 7},
    {"samoyeds", 2},
    {"pomeranians", 3},
    {"akitas", 0},
    {"vizslas", 0},
    {"goldfish", 5},
    {"trees", 3},
    {"cars", 2},
    {"perfumes", 1}
  };

  const auto equal_or = [] (auto name, auto lhs, auto rhs) {
    return (name == lhs) || (name == rhs);
  };

  const auto result = std::find_if(aunts.begin(), aunts.end(), [&] (const auto& aunt) {

    return std::all_of(aunt.begin(), aunt.end(), [&] (auto& clue) {

      auto result = false;

      const auto [name, num] = clue;
      const auto sue_num = sue.at(name);

      if(equal_or(name, "cats", "trees")) {
        result = (num > sue_num);
      } else if(equal_or(name, "pomeranians", "goldfish")) {
        result = (num < sue_num);
      } else {
        result = (num == sue_num);
      }

      return result;
    });
  });

  const auto id = (result - aunts.begin() + 1);

  return id;
}

int main() {
  std::cout << solution(puzzle_input) << std::endl;
}


#include <iostream>
#include <cassert>
#include <chrono>
#include <fstream>
#include <algorithm>
#include <signal.h>

#include <Eigen/Dense>

#include "csv.h"
#include "server_http.hpp"

#include "server_util.h"
#include "startree.h"

using namespace startree;
using namespace Eigen;
using namespace std;
using namespace std::chrono;

const uint32_t kMaxLeafSize = 8;
const string kDataFileName = "data/hygdata_min3.csv";
// const string kDataFileName = "data/thc.csv";

// Signal handler sets this to false to stop the server
volatile bool running = true;
void signal_handler(int signum) {
    if (signum == SIGINT ||
        signum == SIGTERM ||
        signum == SIGHUP) {
        running = false;
    }
}

int main(int argc, char* argv[]) {
    // Setup signal handler
    signal(SIGINT, signal_handler);
    signal(SIGHUP, signal_handler);
    signal(SIGTERM, signal_handler);

    // Read CSV file and create Star objects
    cout << "Reading CSV file..." << endl;
    io::CSVReader<8> star_reader(kDataFileName);

    uint64_t id = 0;
    string idstr;
    double x, y, z;
    double absmag;
    double lum;
    double r, g, b;

    double minx,miny,minz,maxx,maxy,maxz;

    vector<Star> stars;
    //while(star_reader.read_row(idstr, x, y, z, absmag, r, g, b)) {
    while(star_reader.read_row(id, x, y, z, lum, r, g, b)) {
        minx = min(x, minx);
        miny = min(y, miny);
        minz = min(z, minz);
        maxx = max(x, maxx);
        maxy = max(y, maxy);
        maxz = max(z, maxz);
        //lum = exp(-0.4 * (absmag - 4.85));
        //stars.push_back(Star(id++, x, y , z, lum, r, g, b));
        stars.push_back(Star(id, x, y , z, lum, r, g, b));
    }
    cout << "Min x: " << minx << " Min y: " << miny << " Min z: " << minz
         << endl
         << "Max x: " << maxx << " Max y: " << maxy << " Max z: " << maxz
         << endl;
    cout << "Done reading CSV file." << endl;

    cout << "Loading stars into tree..." << endl;
    high_resolution_clock::time_point t1 = high_resolution_clock::now();

    // Calculate bounding box
    double maxmax = fmax(maxx, maxy);
    maxmax = fmax(maxmax, maxz);
    Vector3d maxVec(maxmax + 1.0, maxmax + 1.0, maxmax + 1.0);

    double minmin = fmin(minx, miny);
    minmin = fmin(minmin, minz);
    Vector3d minVec(minmin - 1.0, minmin - 1.0, minmin - 1.0);

    if (maxVec.norm() > minVec.norm()) {
        minVec = -1 * maxVec;
    } else {
        maxVec = -1 * minVec;
    }

    // Put all the stars into the octree
    StarTree tree(kMaxLeafSize, Vector3d::Zero(),
                  minVec,
                  maxVec);
    map<uint64_t,const StarTree*> treeMap;
    treeMap[0] = &tree;

    for (unsigned int i = 0; i < stars.size(); i++) {
        tree.addStar(&(stars[i]), treeMap);
    }
    high_resolution_clock::time_point t2 = high_resolution_clock::now();
    duration<double> time_span =
        duration_cast<duration<double>>(t2 - t1);
    cout << "Done loading stars into tree (" << time_span.count()
         <<"s)." << endl;
    cout << "Number of nodes: " << treeMap.size() << endl;

    // Create web server
    cout << "Staring web server..." << endl;
    HttpServer server(8080, 1);

    // starsInRadius API
    server.resource["^/starsInRadius[?]"
                    "((radius=[^=&]*)|"
                    "(pointX=[^=&]*)|"
                    "(pointY=[^=&]*)|"
                    "(pointZ=[^=&]*)|"
                    "&)*"]["GET"] =
        [&server,&tree](shared_ptr<HttpServer::Response> response,
                        shared_ptr<HttpServer::Request> request) {
        starsInRadiusHandler(server, response, request, tree);
    };

    // visibleStars API
    server.resource["^/visibleStars[?]"
                    "((minLum=[^=&]*)|"
                    "(pointX=[^=&]*)|"
                    "(pointY=[^=&]*)|"
                    "(pointZ=[^=&]*)|"
                    "&)*"]["GET"] =
        [&server,&tree](shared_ptr<HttpServer::Response> response,
                        shared_ptr<HttpServer::Request> request) {
        visibleStarsHandler(server, response, request, tree);
    };

    // visibleStarsMagic API
    server.resource["^/visibleStarsMagic[?]"
                    "((minLum=[^=&]*)|"
                    "(blurRad=[^=&]*)|"
                    "(pointX=[^=&]*)|"
                    "(pointY=[^=&]*)|"
                    "(pointZ=[^=&]*)|"
                    "&)*"]["GET"] =
        [&server,&tree](shared_ptr<HttpServer::Response> response,
                        shared_ptr<HttpServer::Request> request) {
        visibleStarsMagicHandler(server, response, request, tree);
    };

    // visibleOctants API
    server.resource["^/visibleOctants[?]"
                    "((minLum=[^=&]*)|"
                    "(pointX=[^=&]*)|"
                    "(pointY=[^=&]*)|"
                    "(pointZ=[^=&]*)|"
                    "&)*"]["GET"] =
        [&server,&tree](shared_ptr<HttpServer::Response> response,
                        shared_ptr<HttpServer::Request> request) {
        visibleOctantsHandler(server, response, request, tree);
    };

    // visibleOctantsMagic API
    server.resource["^/visibleOctantsMagic[?]"
                    "((minLum=[^=&]*)|"
                    "(blurRad=[^=&]*)|"
                    "(pointX=[^=&]*)|"
                    "(pointY=[^=&]*)|"
                    "(pointZ=[^=&]*)|"
                    "&)*"]["GET"] =
        [&server,&tree](shared_ptr<HttpServer::Response> response,
                        shared_ptr<HttpServer::Request> request) {
        visibleOctantsMagicHandler(server, response, request, tree);
    };

    // getNodeStars API
    server.resource["/getNodeStars"]["POST"] =
        [&server,&treeMap](shared_ptr<HttpServer::Response> response,
                           shared_ptr<HttpServer::Request> request) {
        getNodeStarsHandler(server, response, request, treeMap);
    };

    // Serve the static pages
    server.default_resource["GET"] =
        [&server](shared_ptr<HttpServer::Response> response,
                  shared_ptr<HttpServer::Request> request) {
        dflt_res_request_handler(server, response, request);
    };

    // Start the webserver on another thread
    thread server_thread([&server]() {
            server.start();
        });
    cout << "Web server started." << endl;

    // This thread should just sleep while we're running.
    while (running) {
        this_thread::sleep_for(seconds(1));
    }

    // Stop the webserver
    cout << "Web server shutting down." << endl;
    server.stop();
    // Join the webserver thread
    server_thread.join();

    // Run a search for visible stars
    /*
    cout << "Searching for visible stars..." << endl;
    t1 = high_resolution_clock::now();
    vector<const StarTree*> searchList{&tree};
    vector<const Star*> foundStars;
    visibleStars(cameraPosition, 0.1, searchList, foundStars);
    t2 = high_resolution_clock::now();
    time_span = duration_cast<duration<double>>(t2 - t1);
    cout << "Found " << foundStars.size() << " visible stars ("
         << time_span.count() << "s)." << endl;
    */

    return 0;
}


/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * 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 Willow Garage, Inc. 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 OWNER 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.
 */

/*
 * Author: Stuart Glaser
 */

#ifndef NAV_CONTROL_EIGEN_KDL_CONVERSIONS_HPP
#define NAV_CONTROL_EIGEN_KDL_CONVERSIONS_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <kdl/frames.hpp>
#include <lcmtypes/drc_lcmtypes.hpp>

namespace nav_control {

/// Converts a KDL twist into an Eigen matrix
void transformKDLtwistToEigen(const KDL::Twist &k, Eigen::Matrix<double, 6, 1> &e)
{
  e[0] = k.vel.x();
  e[1] = k.vel.y();
  e[2] = k.vel.z();
  e[3] = k.rot.x();
  e[4] = k.rot.y();
  e[5] = k.rot.z();
};

/// Converts a LCM twist into an Eigen matrix
void transformLCMtwistToEigen(const drc::twist_t &t, Eigen::Matrix<double, 6, 1> &e)
{

KDL::Vector lin_vel(t.linear_velocity.x,t.linear_velocity.y,t.linear_velocity.z);
KDL::Vector ang_vel(t.angular_velocity.x,t.angular_velocity.y,t.angular_velocity.z);
KDL::Twist k(lin_vel,ang_vel);
transformKDLtwistToEigen(k,e);
};

/// Converts a KDL frame into an Eigen transform
void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
{
  e(0,3) = k.p[0];
  e(1,3) = k.p[1];
  e(2,3) = k.p[2];

  e(0,0) = k.M(0,0);
  e(0,1) = k.M(0,1);
  e(0,2) = k.M(0,2);
  e(1,0) = k.M(1,0);
  e(1,1) = k.M(1,1);
  e(1,2) = k.M(1,2);
  e(2,0) = k.M(2,0);
  e(2,1) = k.M(2,1);
  e(2,2) = k.M(2,2);

  e(3,0) = 0.0;
  e(3,1) = 0.0;
  e(3,2) = 0.0;
  e(3,3) = 1.0;
};

/// Converts a LCM position3D_t into an Eigen matrix
void transformLCMToEigen(const drc::position_3d_t &t, Eigen::Affine3d &e)
{
   //TransformLCMToKDLFrame
    KDL::Frame k;
    k.p[0] =t.translation.x;
    k.p[1] =t.translation.y;
    k.p[2] =t.translation.z;
    //  Eigen::Quaterniond q(t.rotation.w,t.rotation.x,t.rotation.y,t.rotation.z);
    // q.normalize();
    KDL::Rotation M;
    //M =  KDL::Rotation::Quaternion(q.x(),q.y(),q.z(),q.w());
    M =  KDL::Rotation::Quaternion(t.rotation.x,t.rotation.y,t.rotation.z,t.rotation.w);
    k.M = M;
    transformKDLToEigen(k, e);
};

/// Converts a KDL frame into an Eigen transform
void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
{
  k.p[0]=  e(0,3);
  k.p[1]=  e(1,3);
  k.p[2]=  e(2,3);

  k.M(0,0) = e(0,0) ;
  k.M(0,1) = e(0,1) ;
  k.M(0,2) = e(0,2) ;
  k.M(1,0) = e(1,0) ;
  k.M(1,1) = e(1,1);
  k.M(1,2) = e(1,2);
  k.M(2,0) = e(2,0);
  k.M(2,1) = e(2,1);
  k.M(2,2) = e(2,2);

};

void transformEigenToLCM(const Eigen::Affine3d &e, drc::position_3d_t &m)
{
  m.translation.x = e.translation()[0];
  m.translation.y = e.translation()[1];
  m.translation.z = e.translation()[2];
  Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
  q.normalize();
  m.rotation.x = q.x();
  m.rotation.y = q.y();
  m.rotation.z = q.z();
  m.rotation.w = q.w();
  if (m.rotation.w < 0) {
    m.rotation.x *= -1;
    m.rotation.y *= -1;
    m.rotation.z *= -1;
    m.rotation.w *= -1;
  }
};

} // namespace

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <boost/numeric/mtl/mtl.hpp>

template <typename ResMatrix, typename ArgMatrix>
void test(const ResMatrix&, const ArgMatrix& B)
{
    ResMatrix C(B * B);

    C+= trans(B) * B;
    C+= trans(B) * B * B;
#if 0
	std::cout << typeid(typename mtl::traits::category<mtl::mat::mat_mat_times_expr<ArgMatrix, ArgMatrix> >::type).name() << '\n';
	std::cout << typeid(typename mtl::traits::category<mtl::mat::rscaled_view<ArgMatrix, double> >::type).name() << '\n';
	char c; std::cin >> c;
#endif
	C+= B * 3.5 * B * B;
    C+= trans(B) * 3.5 * B * B;

    C+= 3.5 * ArgMatrix(B * B);
    C= 3.5 * ArgMatrix(B * B);

    //C+= 3.5 * (B * B);
    //C= 3.5 * (B * B);
}

int main(int, char**)
{
    using namespace mtl;
    typedef mat::parameters<tag::row_major, mtl::index::c_index, mtl::fixed::dimensions<2, 2>, true> fmat_para;

    float ma[2][2]= {{2., 3.}, {4., 5.}};

    dense2D<float>                   A_dyn(ma);
    dense2D<float, fmat_para>        A_stat(ma);

    test(A_dyn, A_dyn);
    //test(A_dyn, A_stat);

    return 0;
}


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

// This file simply creates a solverNlopt object, sets its params and some random obstacles, and then checks all the
// gradients numerically

#include <iostream>
#include <vector>
#include <iomanip>
#include <nlopt.hpp>

#include <Eigen/Dense>
#include <random>
#include "timer.hpp"

#include "solver_nlopt.hpp"
#include "nlopt_utils.hpp"

bool nlopt_utils::checkGradientsNlopt(std::string basis)
{
  double x_min = -10.0;
  double x_max = 10.0;

  double y_min = -10.0;
  double y_max = 10.0;

  double z_min = -4.0;
  double z_max = 4.0;
  double dc = 0.01;
  double Ra = 4.0;
  int deg_pol = 3;
  int samples_per_interval = 1;
  double weight = 10000.0;  // Note that internally, this weight will be changed to other value for the check (to get
                            // rid of numerical issues)
  double epsilon_tol_constraints = 0.001;
  double xtol_rel = 1e-07;
  double ftol_rel = 1e-07;
  std::string solver = "LD_MMA";
  double kappa = 0.0;
  double mu = 1.0;
  int a_star_samp_x = 7;
  int a_star_samp_y = 7;
  int a_star_samp_z = 7;
  double increment = 0.3;  // grid used to prune nodes that are on the same cell
  double runtime = 1.0;    //(not use this criterion)  //[seconds]
  double v_max = 10;
  double a_max = 60;
  mt::state initial;
  initial.pos = Eigen::Vector3d(-4.0, 1.0, -2.0);
  initial.vel = Eigen::Vector3d(1.2, 3.5, 1.2);
  initial.vel = Eigen::Vector3d(-1.3, 0.6, 1.5);
  mt::state final;
  final.pos = Eigen::Vector3d(4.0, 2.0, 3.0);
  double dist_to_use_straight_guess = 1.0;
  double a_star_fraction_voxel_size = 0.0;
  int num_pol = 8;

  ms::par_solver param;

  param.x_min = x_min;
  param.x_max = x_max;

  param.y_min = y_min;
  param.y_max = y_max;

  param.z_min = z_min;
  param.z_max = z_max;
  param.v_max = Eigen::Vector3d(v_max, v_max, v_max);
  param.a_max = Eigen::Vector3d(a_max, a_max, a_max);
  param.dc = dc;
  param.dist_to_use_straight_guess = dist_to_use_straight_guess;
  param.a_star_samp_x = a_star_samp_x;
  param.a_star_samp_y = a_star_samp_y;
  param.a_star_samp_z = a_star_samp_z;
  param.a_star_fraction_voxel_size = a_star_fraction_voxel_size;
  param.num_pol = num_pol;
  param.deg_pol = deg_pol;
  param.weight = weight;
  param.epsilon_tol_constraints = epsilon_tol_constraints;
  param.xtol_rel = xtol_rel;
  param.ftol_rel = ftol_rel;
  param.solver = solver;
  param.basis = basis;

  double t_min = 0.0;
  double t_max = t_min + (final.pos - initial.pos).norm() / (0.3 * v_max);

  mt::Polyhedron_Std hull(3, 8);

  hull.col(0) = Eigen::Vector3d(-0.5, -0.5, -70.0);
  hull.col(1) = Eigen::Vector3d(-0.5, 0.5, 70.0);
  hull.col(2) = Eigen::Vector3d(0.5, -0.5, 70.0);
  hull.col(3) = Eigen::Vector3d(0.5, 0.5, -70.0);
  hull.col(4) = Eigen::Vector3d(-0.5, -0.5, 70.0);
  hull.col(5) = Eigen::Vector3d(0.5, -0.5, -70.0);
  hull.col(6) = Eigen::Vector3d(-0.5, 0.5, -70.0);
  hull.col(7) = Eigen::Vector3d(0.5, 0.5, 70.0);

  mt::ConvexHullsOfCurves_Std hulls_curves;
  mt::ConvexHullsOfCurve_Std hulls_curve;
  // Assummes static obstacle
  for (int i = 0; i < num_pol; i++)
  {
    hulls_curve.push_back(hull);
  }

  hulls_curves.push_back(hulls_curve);

  SolverNlopt snlopt(param);  // snlopt(a,g) a polynomials of degree 3
  snlopt.setHulls(hulls_curves);
  snlopt.setMaxRuntimeKappaAndMu(runtime, kappa, mu);

  snlopt.setInitStateFinalStateInitTFinalT(initial, final, t_min, t_max);

  return snlopt.checkGradientsUsingFiniteDiff();
}


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#ifndef __itkShCoefficientImageImporter_cpp
#define __itkShCoefficientImageImporter_cpp

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

#include "itkShCoefficientImageImporter.h"
#include <itkImageRegionIterator.h>
#include <boost/math/special_functions.hpp>

using namespace boost::math;

namespace itk {

template< class PixelType, int ShOrder >
ShCoefficientImageImporter< PixelType, ShOrder >::ShCoefficientImageImporter()
    : m_Toolkit(FSL)
{
    m_ShBasis.set_size(QBALL_ODFSIZE, (ShOrder+1)*(ShOrder+2)/2);
}

template< class PixelType, int ShOrder >
void ShCoefficientImageImporter< PixelType, ShOrder >
::GenerateData()
{
    CalcShBasis();
    if (m_InputImage.IsNull())
        return;

    Vector<float, 4> spacing4 = m_InputImage->GetSpacing();
    Point<float, 4> origin4 = m_InputImage->GetOrigin();
    Matrix<double, 4, 4> direction4 = m_InputImage->GetDirection();
    ImageRegion<4> imageRegion4 = m_InputImage->GetLargestPossibleRegion();

    Vector<double, 3> spacing3;
    Point<float, 3> origin3;
    Matrix<double, 3, 3> direction3;
    ImageRegion<3> imageRegion3;

    spacing3[0] = spacing4[0]; spacing3[1] = spacing4[1]; spacing3[2] = spacing4[2];
    origin3[0] = origin4[0]; origin3[1] = origin4[1]; origin3[2] = origin4[2];
    for (int r=0; r<3; r++)
        for (int c=0; c<3; c++)
            direction3[r][c] = direction4[r][c];
    imageRegion3.SetSize(0, imageRegion4.GetSize()[0]);
    imageRegion3.SetSize(1, imageRegion4.GetSize()[1]);
    imageRegion3.SetSize(2, imageRegion4.GetSize()[2]);

    m_QballImage = QballImageType::New();
    m_QballImage->SetSpacing( spacing3 );
    m_QballImage->SetOrigin( origin3 );
    m_QballImage->SetDirection( direction3 );
    m_QballImage->SetRegions( imageRegion3 );
    m_QballImage->Allocate();
    Vector< PixelType, QBALL_ODFSIZE > nullVec1; nullVec1.Fill(0.0);
    m_QballImage->FillBuffer(nullVec1);

    m_CoefficientImage = CoefficientImageType::New();
    m_CoefficientImage->SetSpacing( spacing3 );
    m_CoefficientImage->SetOrigin( origin3 );
    m_CoefficientImage->SetDirection( direction3 );
    m_CoefficientImage->SetRegions( imageRegion3 );
    m_CoefficientImage->Allocate();
    Vector< PixelType, (ShOrder*ShOrder + ShOrder + 2)/2 + ShOrder > nullVec2; nullVec2.Fill(0.0);
    m_CoefficientImage->FillBuffer(nullVec2);

    typedef ImageRegionConstIterator< InputImageType > InputIteratorType;
    int x = imageRegion4.GetSize(0);
    int y = imageRegion4.GetSize(1);
    int z = imageRegion4.GetSize(2);
    int numCoeffs = imageRegion4.GetSize(3);

    for (int a=0; a<x; a++)
        for (int b=0; b<y; b++)
            for (int c=0; c<z; c++)
            {
                vnl_matrix<double> coeffs((ShOrder*ShOrder + ShOrder + 2)/2 + ShOrder,1);

                typename InputImageType::IndexType index;
                index.SetElement(0,a);
                index.SetElement(1,b);
                index.SetElement(2,c);
                typename CoefficientImageType::PixelType pix;
                for (int d=0; d<numCoeffs; d++)
                {
                    index.SetElement(3,d);
                    pix[d] = m_InputImage->GetPixel(index);
                    coeffs[d][0] = pix[d];
                }
                typename CoefficientImageType::IndexType index2;
                index2.SetElement(0,a);
                index2.SetElement(1,b);
                index2.SetElement(2,c);
                m_CoefficientImage->SetPixel(index2, pix);

                typename QballImageType::PixelType pix2;
                vnl_matrix<double> odf = m_ShBasis*coeffs;
                for (int d=0; d<QBALL_ODFSIZE; d++)
                    pix2[d] = odf(d,0)*M_PI*4/QBALL_ODFSIZE;

                m_QballImage->SetPixel(index2,pix2);
            }
}

// generate spherical harmonic values of the desired order for each input direction
template< class PixelType, int ShOrder >
void ShCoefficientImageImporter< PixelType, ShOrder >
::CalcShBasis()
{
    vnl_matrix_fixed<double, 2, QBALL_ODFSIZE> sphCoords = GetSphericalOdfDirections();
    int j, m; double mag, plm;

    for (int p=0; p<QBALL_ODFSIZE; p++)
    {
        j=0;
        for (int l=0; l<=ShOrder; l=l+2)
            for (m=-l; m<=l; m++)
            {
                switch (m_Toolkit)
                {
                case FSL:
                    plm = legendre_p<double>(l,abs(m),cos(sphCoords(0,p)));
                    mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;
                    if (m<0)
                        m_ShBasis(p,j) = sqrt(2.0)*mag*cos(-m*sphCoords(1,p));
                    else if (m==0)
                        m_ShBasis(p,j) = mag;
                    else
                        m_ShBasis(p,j) = pow(-1.0, m)*sqrt(2.0)*mag*sin(m*sphCoords(1,p));
                    break;
                case MRTRIX:
                    plm = legendre_p<double>(l,abs(m),-cos(sphCoords(0,p)));
                    mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;
                    if (m>0)
                        m_ShBasis(p,j) = mag*cos(m*sphCoords(1,p));
                    else if (m==0)
                        m_ShBasis(p,j) = mag;
                    else
                        m_ShBasis(p,j) = mag*sin(-m*sphCoords(1,p));
                    break;
                }

                j++;
            }
    }
}

// convert cartesian to spherical coordinates
template< class PixelType, int ShOrder >
vnl_matrix_fixed<double, 2, QBALL_ODFSIZE> ShCoefficientImageImporter< PixelType, ShOrder >
::GetSphericalOdfDirections()
{
    itk::OrientationDistributionFunction< PixelType, QBALL_ODFSIZE > odf;
    vnl_matrix_fixed<double, 3, QBALL_ODFSIZE>* dir = odf.GetDirections();
    vnl_matrix_fixed<double, 2, QBALL_ODFSIZE> sphCoords;

    for (int i=0; i<QBALL_ODFSIZE; i++)
    {
        double mag = dir->get_column(i).magnitude();

        if( mag<mitk::eps )
        {
            sphCoords(0,i) = M_PI/2; // theta
            sphCoords(1,i) = M_PI/2; // phi
        }
        else
        {
            sphCoords(0,i) = acos(dir->get(2,i)/mag); // theta
            sphCoords(1,i) = atan2(dir->get(1,i), dir->get(0,i)); // phi
        }
    }
    return sphCoords;
}

}

#endif // __itkShCoefficientImageImporter_cpp


// Copyright (c) Jeremy Siek 2001
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appears in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  Silicon Graphics makes no
// representations about the suitability of this software for any
// purpose.  It is provided "as is" without express or implied warranty.

// NOTE: this final is generated by libs/graph/doc/transitive_closure.w

#ifdef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
#error The transitive closure algorithm uses partial specialization.
#endif

#include <boost/graph/transitive_closure.hpp>
#include <boost/graph/graphviz.hpp>

int
main(int, char *[])
{
  using namespace boost;
  typedef property < vertex_name_t, char >Name;
  typedef property < vertex_index_t, std::size_t, Name > Index;
  typedef adjacency_list < listS, listS, directedS, Index > graph_t;
  typedef graph_traits < graph_t >::vertex_descriptor vertex_t;
  graph_t G;
  std::vector < vertex_t > verts(4);
  for (int i = 0; i < 4; ++i)
    verts[i] = add_vertex(Index(i, Name('a' + i)), G);
  add_edge(verts[1], verts[2], G);
  add_edge(verts[1], verts[3], G);
  add_edge(verts[2], verts[1], G);
  add_edge(verts[3], verts[2], G);
  add_edge(verts[3], verts[0], G);

  std::cout << "Graph G:" << std::endl;
  print_graph(G, get(vertex_name, G));

  adjacency_list <> TC;
  transitive_closure(G, TC);

  std::cout << std::endl << "Graph G+:" << std::endl;
  char name[] = "abcd";
  print_graph(TC, name);
  std::cout << std::endl;

  std::ofstream out("tc-out.dot");
  write_graphviz(out, TC, make_label_writer(name));

  return 0;
}


//   -----------------------------------------------------------------------------------------------
//    Copyright 2015 André Bergner. Distributed under the Boost Software License, Version 1.0.
//     (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//      --------------------------------------------------------------------------------------------

#include <iostream>
#include <boost/proto/proto.hpp>

using namespace boost;

template< typename I >  struct placeholder        { using arity = I; };
template< typename T >  struct placeholder_arity  { using type = typename T::arity; };

namespace transforms {

    using namespace proto;

    template< typename T >
    struct placeholder_arity_f : callable
    {
        using result_type = int;

        int operator()() const { return T::arity::value; }
    };

    struct eval_it : or_
    <   when
        <   terminal< placeholder<_> >
        ,   placeholder_arity<_value>()
        >
    ,   when
        <   _
        ,   _default<eval_it>
        >
    >
    {};

    struct count_leaves : or_
    <   when
        <   terminal<placeholder<_>>
        ,   mpl::int_<1>()
        >
    ,   when
        <   terminal<_>
        ,   mpl::int_<1>()
        >
    ,   when
        <   unary_expr< _ , count_leaves>
        ,   count_leaves( _child )
        >
    ,   when
        <   binary_expr< _ , count_leaves, count_leaves >
        ,   mpl::plus< count_leaves(_left), count_leaves(_right) >()
        >
    >
    {};

    struct iplus : std::plus<int>, callable {};

    struct CountLeaves : or_
    <   when
        <   terminal<_>
        ,   mpl::int_<1>() >
    ,   otherwise
        <   fold<_, mpl::int_<0>(), iplus(CountLeaves, _state) > >
    >
    {};

    struct eval_it_inc : or_
    <   when
        <   terminal< placeholder<_> >
        ,   mpl::int_<0>()
        >
    ,   when
        <   terminal<_>
        ,   _value
        >
//    ,   when
//        <   plus< eval_it_inc, eval_it_inc >
//        ,   eval_it_inc( _left, mpl::plus<eval_it_inc(_right,_state),_state>() )
//        >
    ,   when
        <   plus< eval_it_inc, eval_it_inc >
        ,   iplus( iplus( eval_it_inc(_left) , eval_it_inc(_right) )
                 , mpl::plus< count_leaves(_left) , count_leaves(_right) >()
                 )
        >
        // TODO plus x y -> (x+y) * state++
        // plus( eval_it_inc(_left,state+1) , eval_it_inc(_right,eval_it_inc(_left,state+1).state+1) >
    ,   when
        <   _
        ,   _default<eval_it_inc>
        >
    >
    {};

}

using transforms::eval_it;
using transforms::count_leaves;
using transforms::CountLeaves;
using transforms::eval_it_inc;

const proto::terminal< placeholder< mpl::int_<0> >>::type   _   = {{}};
const proto::terminal< placeholder< mpl::int_<3> >>::type   _3  = {{}};
const proto::terminal< placeholder< mpl::int_<1337> >>::type   _1337  = {{}};

int main()
{
    std::cout << eval_it{}(_3 + 3) << std::endl;
    auto expr  =  (proto::lit(1) + 2) * _3;
    std::cout << eval_it{}(expr) << std::endl;

    std::cout << count_leaves{}(_+_+_) << std::endl;
    std::cout << CountLeaves{}( _+1+_+_+5, 1, 1 ) << std::endl;

    //std::cout << decltype(eval_it_inc{}( _+_+(_+_) ,mpl::int_<1>{} ))::value << std::endl;
    std::cout << eval_it_inc{}( _+_+(_+_)+(_+_+_) , mpl::int_<1>{} ) << std::endl;
    std::cout << eval_it_inc{}( (1+_)+_ ) << std::endl;
}


/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file gaussian_filter_linear_vs_unscented_kalman_filter_test.cpp
 * \date July 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>
#include "../typecast.hpp"

#include <Eigen/Dense>

#include "gaussian_filter_linear_vs_x_test_suite.hpp"
#include <fl/util/meta.hpp>
#include <fl/filter/gaussian/gaussian_filter.hpp>

using namespace fl;

template <
    int StateDimension,
    int InputDimension,
    int ObsrvDimension,
    int ModelKey
>
struct UkfTestConfig
{
    enum : signed int
    {
        StateDim = StateDimension,
        InputDim = InputDimension,
        ObsrvDim = ObsrvDimension
    };

    enum : signed int { SelectedModel = ModelKey };

    template <typename StateTransitionModel, typename ObservationModel>
    struct FilterDefinition
    {
        typedef GaussianFilter<
                    StateTransitionModel,
                    ObservationModel,
                    SigmaPointQuadrature<UnscentedTransform>/*,
                    SigmaPointPredictPolicy<
                        SigmaPointQuadrature<UnscentedTransform>,
                        typename UseAs<NonAdditive<StateTransitionModel>>::Type>,
                    SigmaPointUpdatePolicy<
                        SigmaPointQuadrature<UnscentedTransform>,
                        typename UseAs<NonAdditive<ObservationModel>>::Type>*/
                > Type;
    };

    template <typename F, typename H>
    static typename FilterDefinition<F, H>::Type create_filter(F&& f, H&& h)
    {
        return typename FilterDefinition<F, H>::Type(
            f, h, UnscentedQuadrature());
    }
};

typedef ::testing::Types<
            StaticTest<UkfTestConfig<4, 4, 4, GaussianModel>>,
            StaticTest<UkfTestConfig<3, 3, 10, GaussianModel>>,
            StaticTest<UkfTestConfig<10, 4, 10, GaussianModel>>,
            StaticTest<UkfTestConfig<20, 4, 2, GaussianModel>>,

            DynamicTest<UkfTestConfig<3, 1, 2, GaussianModel>>,
            DynamicTest<UkfTestConfig<3, 3, 3, GaussianModel>>,
            DynamicTest<UkfTestConfig<10, 4, 4, GaussianModel>>,
            DynamicTest<UkfTestConfig<20, 4, 2, GaussianModel>>,

            StaticTest<UkfTestConfig<3, 1, 2, DecorrelatedGaussianModel>>,
            StaticTest<UkfTestConfig<3, 3, 10, DecorrelatedGaussianModel>>,
            StaticTest<UkfTestConfig<20, 4, 2, DecorrelatedGaussianModel>>,
            StaticTest<UkfTestConfig<10, 3, 50, DecorrelatedGaussianModel>>,
            StaticTest<UkfTestConfig<10, 10, 500, DecorrelatedGaussianModel>>,

            DynamicTest<UkfTestConfig<3, 1, 2, DecorrelatedGaussianModel>>,
            DynamicTest<UkfTestConfig<3, 3, 10, DecorrelatedGaussianModel>>,
            DynamicTest<UkfTestConfig<10, 3, 50, DecorrelatedGaussianModel>>,
            DynamicTest<UkfTestConfig<20, 4, 2, DecorrelatedGaussianModel>>,
            DynamicTest<UkfTestConfig<10, 10, 500, DecorrelatedGaussianModel>>
        > TestTypes;

INSTANTIATE_TYPED_TEST_CASE_P(GaussianFilterLinearVUkfTest,
                              GaussianFilterLinearVsXTest,
                              TestTypes);


#ifndef MTGP64_CALC_POLY_HPP
#define MTGP64_CALC_POLY_HPP
/**
 * @file mtgp64-calc-poly.hpp
 *
 * @brief header file for functions of calculating jump polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <string>
#include <NTL/GF2X.h>
#include "mtgp64-fast.h"

void mtgp64_calc_characteristic(NTL::GF2X& poly, mtgp64_fast_t * mtgp64);
void mtgp64_calc_characteristic(std::string& str, mtgp64_fast_t * mtgp64);

#endif