/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2018 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

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

#include <cmath>  // float comparison
#include <limits>
#include <string>
#include <vector>
#include <algorithm>  // std::max

#include <gudhi/Cech_complex.h>
// to construct Cech_complex from a OFF file of points
#include <gudhi/Points_off_io.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Unitary_tests_utils.h>
#include <gudhi/Miniball.hpp>

// Type definitions
using Simplex_tree = Gudhi::Simplex_tree<>;
using Filtration_value = Simplex_tree::Filtration_value;
using Point = std::vector<Filtration_value>;
using Point_cloud = std::vector<Point>;
using Points_off_reader = Gudhi::Points_off_reader<Point>;
using Cech_complex = Gudhi::cech_complex::Cech_complex<Simplex_tree, Point_cloud>;

using Point_iterator = Point_cloud::const_iterator;
using Coordinate_iterator = Point::const_iterator;
using Min_sphere = Gudhi::Miniball::Miniball<Gudhi::Miniball::CoordAccessor<Point_iterator, Coordinate_iterator>>;

BOOST_AUTO_TEST_CASE(Cech_complex_for_documentation) {
  // ----------------------------------------------------------------------------
  //
  // Init of a Cech complex from a point cloud
  //
  // ----------------------------------------------------------------------------
  Point_cloud points;
  points.push_back({1., 0.});                  // 0
  points.push_back({0., 1.});                  // 1
  points.push_back({2., 1.});                  // 2
  points.push_back({3., 2.});                  // 3
  points.push_back({0., 3.});                  // 4
  points.push_back({3. + std::sqrt(3.), 3.});  // 5
  points.push_back({1., 4.});                  // 6
  points.push_back({3., 4.});                  // 7
  points.push_back({2., 4. + std::sqrt(3.)});  // 8
  points.push_back({0., 4.});                  // 9
  points.push_back({-0.5, 2.});                // 10

  Filtration_value max_radius = 1.0;
  std::cout << "========== NUMBER OF POINTS = " << points.size() << " - Cech max_radius = " << max_radius
            << "==========" << std::endl;

  Cech_complex cech_complex_for_doc(points, max_radius);

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(cech_complex_for_doc.max_radius(), max_radius);
  std::size_t i = 0;
  for (; i < points.size(); i++) {
    BOOST_CHECK(points[i] == cech_complex_for_doc.get_point(i));
  }

  const int DIMENSION_1 = 1;
  Simplex_tree st;
  cech_complex_for_doc.create_complex(st, DIMENSION_1);
  std::cout << "st.dimension()=" << st.dimension() << std::endl;
  BOOST_CHECK(st.dimension() == DIMENSION_1);

  const int NUMBER_OF_VERTICES = 11;
  std::cout << "st.num_vertices()=" << st.num_vertices() << std::endl;
  BOOST_CHECK(st.num_vertices() == NUMBER_OF_VERTICES);

  std::cout << "st.num_simplices()=" << st.num_simplices() << std::endl;
  BOOST_CHECK(st.num_simplices() == 27);

  // Check filtration values of vertices is 0.0
  for (auto f_simplex : st.skeleton_simplex_range(0)) {
    BOOST_CHECK(st.filtration(f_simplex) == 0.0);
  }

  // Check filtration values of edges
  for (auto f_simplex : st.skeleton_simplex_range(DIMENSION_1)) {
    if (DIMENSION_1 == st.dimension(f_simplex)) {
      std::vector<Point> vp;
      std::cout << "vertex = (";
      for (auto vertex : st.simplex_vertex_range(f_simplex)) {
        std::cout << vertex << ",";
        vp.push_back(points.at(vertex));
      }
      std::cout << ") - distance =" << Gudhi::Minimal_enclosing_ball_radius()(vp.at(0), vp.at(1))
                << " - filtration =" << st.filtration(f_simplex) << std::endl;
      BOOST_CHECK(vp.size() == 2);
      GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex),
                                      Gudhi::Minimal_enclosing_ball_radius()(vp.at(0), vp.at(1)));
    }
  }

  const int DIMENSION_2 = 2;

#ifdef GUDHI_DEBUG
  BOOST_CHECK_THROW(cech_complex_for_doc.create_complex(st, DIMENSION_2), std::invalid_argument);
#endif

  Simplex_tree st2;
  cech_complex_for_doc.create_complex(st2, DIMENSION_2);
  std::cout << "st2.dimension()=" << st2.dimension() << std::endl;
  BOOST_CHECK(st2.dimension() == DIMENSION_2);

  std::cout << "st2.num_vertices()=" << st2.num_vertices() << std::endl;
  BOOST_CHECK(st2.num_vertices() == NUMBER_OF_VERTICES);

  std::cout << "st2.num_simplices()=" << st2.num_simplices() << std::endl;
  BOOST_CHECK(st2.num_simplices() == 30);

  Point_cloud points012;
  for (std::size_t vertex = 0; vertex <= 2; vertex++) {
    points012.push_back(cech_complex_for_doc.get_point(vertex));
  }
  std::size_t dimension = points[0].end() - points[0].begin();
  Min_sphere ms012(dimension, points012.begin(), points012.end());

  Simplex_tree::Filtration_value f012 = st2.filtration(st2.find({0, 1, 2}));
  std::cout << "f012= " << f012 << " | ms012_radius= " << std::sqrt(ms012.squared_radius()) << std::endl;

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f012, std::sqrt(ms012.squared_radius()));

  Point_cloud points1410;
  points1410.push_back(cech_complex_for_doc.get_point(1));
  points1410.push_back(cech_complex_for_doc.get_point(4));
  points1410.push_back(cech_complex_for_doc.get_point(10));
  Min_sphere ms1410(dimension, points1410.begin(), points1410.end());

  Simplex_tree::Filtration_value f1410 = st2.filtration(st2.find({1, 4, 10}));
  std::cout << "f1410= " << f1410 << " | ms1410_radius= " << std::sqrt(ms1410.squared_radius()) << std::endl;

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f1410, std::sqrt(ms1410.squared_radius()));

  Point_cloud points469;
  points469.push_back(cech_complex_for_doc.get_point(4));
  points469.push_back(cech_complex_for_doc.get_point(6));
  points469.push_back(cech_complex_for_doc.get_point(9));
  Min_sphere ms469(dimension, points469.begin(), points469.end());

  Simplex_tree::Filtration_value f469 = st2.filtration(st2.find({4, 6, 9}));
  std::cout << "f469= " << f469 << " | ms469_radius= " << std::sqrt(ms469.squared_radius()) << std::endl;

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f469, std::sqrt(ms469.squared_radius()));

  BOOST_CHECK((st2.find({6, 7, 8}) == st2.null_simplex()));
  BOOST_CHECK((st2.find({3, 5, 7}) == st2.null_simplex()));
}

BOOST_AUTO_TEST_CASE(Cech_complex_from_points) {
  // ----------------------------------------------------------------------------
  // Init of a list of points
  // ----------------------------------------------------------------------------
  Point_cloud points;
  std::vector<double> coords = {0.0, 0.0, 0.0, 1.0};
  points.push_back(Point(coords.begin(), coords.end()));
  coords = {0.0, 0.0, 1.0, 0.0};
  points.push_back(Point(coords.begin(), coords.end()));
  coords = {0.0, 1.0, 0.0, 0.0};
  points.push_back(Point(coords.begin(), coords.end()));
  coords = {1.0, 0.0, 0.0, 0.0};
  points.push_back(Point(coords.begin(), coords.end()));

  // ----------------------------------------------------------------------------
  // Init of a Cech complex from the list of points
  // ----------------------------------------------------------------------------
  Cech_complex cech_complex_from_points(points, 2.0);

  std::cout << "========== cech_complex_from_points ==========" << std::endl;
  Simplex_tree st;
  const int DIMENSION = 3;
  cech_complex_from_points.create_complex(st, DIMENSION);

  // Another way to check num_simplices
  std::cout << "Iterator on Cech complex simplices in the filtration order, with [filtration value]:" << std::endl;
  int num_simplices = 0;
  for (auto f_simplex : st.filtration_simplex_range()) {
    num_simplices++;
    std::cout << "   ( ";
    for (auto vertex : st.simplex_vertex_range(f_simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << ") -> "
              << "[" << st.filtration(f_simplex) << "] ";
    std::cout << std::endl;
  }
  BOOST_CHECK(num_simplices == 15);
  std::cout << "st.num_simplices()=" << st.num_simplices() << std::endl;
  BOOST_CHECK(st.num_simplices() == 15);

  std::cout << "st.dimension()=" << st.dimension() << std::endl;
  BOOST_CHECK(st.dimension() == DIMENSION);
  std::cout << "st.num_vertices()=" << st.num_vertices() << std::endl;
  BOOST_CHECK(st.num_vertices() == 4);

  for (auto f_simplex : st.filtration_simplex_range()) {
    std::cout << "dimension(" << st.dimension(f_simplex) << ") - f = " << st.filtration(f_simplex) << std::endl;
    switch (st.dimension(f_simplex)) {
      case 0:
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), 0.0);
        break;
      case 1:
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), 0.707107, .00001);
        break;
      case 2:
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), 0.816497, .00001);
        break;
      case 3:
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), 0.866025, .00001);
        break;
      default:
        BOOST_CHECK(false);  // Shall not happen
        break;
    }
  }
}

#ifdef GUDHI_DEBUG
BOOST_AUTO_TEST_CASE(Cech_create_complex_throw) {
  // ----------------------------------------------------------------------------
  //
  // Init of a Cech complex from a OFF file
  //
  // ----------------------------------------------------------------------------
  std::string off_file_name("alphacomplexdoc.off");
  double max_radius = 12.0;
  std::cout << "========== OFF FILE NAME = " << off_file_name << " - Cech max_radius=" << max_radius
            << "==========" << std::endl;

  Gudhi::Points_off_reader<Point> off_reader(off_file_name);
  Cech_complex cech_complex_from_file(off_reader.get_point_cloud(), max_radius);

  Simplex_tree stree;
  std::vector<int> simplex = {0, 1, 2};
  stree.insert_simplex_and_subfaces(simplex);
  std::cout << "Check exception throw in debug mode" << std::endl;
  // throw excpt because stree is not empty
  BOOST_CHECK_THROW(cech_complex_from_file.create_complex(stree, 1), std::invalid_argument);
}
#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the relevance vector machine
    utilities from the dlib C++ Library.

    This example creates a simple set of data to train on and then shows
    you how to use the cross validation and rvm training functions
    to find a good decision function that can classify examples in our
    data set.

    The data used in this example will be 2 dimensional data and will
    come from a distribution where points with a distance less than 10
    from the origin are labeled +1 and all other points are labeled
    as -1.

*/

#include <iostream>
#include <dlib/svm.h>

using namespace std;
using namespace dlib;

int main()
{
    // The rvm functions use column vectors to contain a lot of the data on which they
    // operate. So the first thing we do here is declare a convenient typedef.

    // This typedef declares a matrix with 2 rows and 1 column.  It will be the
    // object that contains each of our 2 dimensional samples.   (Note that if you wanted
    // more than 2 features in this vector you can simply change the 2 to something else.
    // Or if you don't know how many features you want until runtime then you can put a 0
    // here and use the matrix.set_size() member function)
    typedef matrix<double, 2, 1> sample_type;

    // This is a typedef for the type of kernel we are going to use in this example.
    // In this case I have selected the radial basis kernel that can operate on our
    // 2D sample_type objects
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Now we make objects to contain our samples and their respective labels.
    std::vector<sample_type> samples;
    std::vector<double> labels;

    // Now let's put some data into our samples and labels objects.  We do this
    // by looping over a bunch of points and labeling them according to their
    // distance from the origin.
    for (int r = -20; r <= 20; ++r)
    {
        for (int c = -20; c <= 20; ++c)
        {
            sample_type samp;
            samp(0) = r;
            samp(1) = c;
            samples.push_back(samp);

            // if this point is less than 10 from the origin
            if (sqrt((double)r*r + c*c) <= 10)
                labels.push_back(+1);
            else
                labels.push_back(-1);

        }
    }

    // Here we normalize all the samples by subtracting their mean and dividing by their standard deviation.
    // This is generally a good idea since it often heads off numerical stability problems and also
    // prevents one large feature from smothering others.  Doing this doesn't matter much in this example
    // so I'm just doing this here so you can see an easy way to accomplish this with
    // the library.
    vector_normalizer<sample_type> normalizer;
    // let the normalizer learn the mean and standard deviation of the samples
    normalizer.train(samples);
    // now normalize each sample
    for (unsigned long i = 0; i < samples.size(); ++i)
        samples[i] = normalizer(samples[i]);

    // Now that we have some data we want to train on it.  However, there is a parameter to the
    // training.  This is the gamma parameter of the RBF kernel.  Our choice for this parameter will
    // influence how good the resulting decision function is.  To test how good a particular choice of
    // kernel parameters is we can use the cross_validate_trainer() function to perform n-fold cross
    // validation on our training data.  However, there is a problem with the way we have sampled
    // our distribution.  The problem is that there is a definite ordering to the samples.
    // That is, the first half of the samples look like they are from a different distribution
    // than the second half.  This would screw up the cross validation process but we can
    // fix it by randomizing the order of the samples with the following function call.
    randomize_samples(samples, labels);

    // here we make an instance of the rvm_trainer object that uses our kernel type.
    rvm_trainer<kernel_type> trainer;

    // One thing you can do to reduce the RVM training time is to make its
    // stopping epsilon bigger.  However, this might make the outputs less
    // reliable.  But sometimes it works out well.  0.001 is the default.
    trainer.set_epsilon(0.001);
    // You can also set an explicit limit on the number of iterations used by the numeric
    // solver.  The default is 2000.
    trainer.set_max_iterations(2000);

    // Now we loop over some different gamma values to see how good they are.  Note
    // that this is a very simple way to try out a few possible parameter choices.  You
    // should look at the model_selection_ex.cpp program for examples of more sophisticated
    // strategies for determining good parameter choices.
    cout << "doing cross validation" << endl;
    for (double gamma = 0.000001; gamma <= 1; gamma *= 5)
    {
        // tell the trainer the parameters we want to use
        trainer.set_kernel(kernel_type(gamma));

        cout << "gamma: " << gamma;
        // Print out the cross validation accuracy for 3-fold cross validation using the current gamma.
        // cross_validate_trainer() returns a row vector.  The first element of the vector is the fraction
        // of +1 training examples correctly classified and the second number is the fraction of -1 training
        // examples correctly classified.
        cout << "     cross validation accuracy: " << cross_validate_trainer(trainer, samples, labels, 3);
    }

    // From looking at the output of the above loop it turns out that a good value for
    // gamma for this problem is 0.08.  So that is what we will use.

    // Now we train on the full set of data and obtain the resulting decision function.  We use the
    // value of 0.08 for gamma.  The decision function will return values >= 0 for samples it predicts
    // are in the +1 class and numbers < 0 for samples it predicts to be in the -1 class.
    trainer.set_kernel(kernel_type(0.08));
    typedef decision_function<kernel_type> dec_funct_type;
    typedef normalized_function<dec_funct_type> funct_type;

    // Here we are making an instance of the normalized_function object.  This object provides a convenient
    // way to store the vector normalization information along with the decision function we are
    // going to learn.
    funct_type learned_function;
    learned_function.normalizer = normalizer;  // save normalization information
    learned_function.function = trainer.train(samples, labels); // perform the actual RVM training and save the results

    // Print out the number of relevance vectors in the resulting decision function.
    cout << "\nnumber of relevance vectors in our learned_function is "
         << learned_function.function.basis_vectors.size() << endl;

    // Now let's try this decision_function on some samples we haven't seen before
    sample_type sample;

    sample(0) = 3.123;
    sample(1) = 2;
    cout << "This is a +1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 3.123;
    sample(1) = 9.3545;
    cout << "This is a +1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This is a -1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This is a -1 class example, the classifier output is " << learned_function(sample) << endl;

    // We can also train a decision function that reports a well conditioned probability
    // instead of just a number > 0 for the +1 class and < 0 for the -1 class.  An example
    // of doing that follows:
    typedef probabilistic_decision_function<kernel_type> probabilistic_funct_type;
    typedef normalized_function<probabilistic_funct_type> pfunct_type;

    pfunct_type learned_pfunct;
    learned_pfunct.normalizer = normalizer;
    learned_pfunct.function = train_probabilistic_decision_function(trainer, samples, labels, 3);
    // Now we have a function that returns the probability that a given sample is of the +1 class.

    // print out the number of relevance vectors in the resulting decision function.
    // (it should be the same as in the one above)
    cout << "\nnumber of relevance vectors in our learned_pfunct is "
         << learned_pfunct.function.decision_funct.basis_vectors.size() << endl;

    sample(0) = 3.123;
    sample(1) = 2;
    cout << "This +1 class example should have high probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 3.123;
    sample(1) = 9.3545;
    cout << "This +1 class example should have high probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This -1 class example should have low probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This -1 class example should have low probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

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

    // Now let's open that file back up and load the function object it contains.
    deserialize("saved_function.dat") >> learned_pfunct;

}


#include <gtest/gtest.h>
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include "pid_controller/pid_controller.h"

namespace controls
{

    class PID_Test : public ::testing::Test
    {
        protected:
        /// Pointer for Kalman Filter object
        std::shared_ptr<PID_Controller> pid_ptr;
        double kp, ki, kd;
        double max, min;
        double imax, imin;
        double init_val;
        double fltr_coef;

        virtual void SetUp()
        {
            kp = 0.0F;
            ki = 0.0F;
            kd = 0.0F;
            max = 10.0F;
            min = -max;
            imax = 3.0F;
            imin = -imax;
            init_val = 0.0F;
            fltr_coef = 1.0F;
            pid_ptr = std::make_shared<PID_Controller>(kp, ki, kd, max, min, imax, imin, init_val, fltr_coef);
        }

        virtual void TearDown()
        {
        }
    };

    TEST_F(PID_Test, PTEST)
    {
        double kp = 3.0F;
        double ki = 0.0F;
        double kd = 0.0F;
        double dT = 0.1F;
        bool result = false;

        Eigen::VectorXd error(63);          /// Control Error
        Eigen::VectorXd pid_expected(63);   /// Expected
        Eigen::VectorXd pid_out(63);        /// Controller Output
        double cmp_error = 0.0F;            /// Compare Error
        double mae = 0.0F;                  /// Mean Abs Error

        pid_ptr->Init();
        pid_ptr->Update_Gains(kp, ki, kd);
        pid_ptr->Update_dT(dT);

        /// Run Controler with 5.17 deg/100ms error signal
        for(int i=0; i<63; i++)
        {
            error(i) = sin(i*dT);
            pid_expected(i) = kp*error(i);
            pid_out(i) = pid_ptr->Step(error(i));
            cmp_error = pid_expected(i) - pid_out(i);
            mae += cmp_error;
        }

        /// Mean Absolute Error
        mae /= 63;
        if(fabs(mae) < 1e-6)
        {
            result = true;
        }
        else
        {
            result = false;
            std::cerr << "PTest Mean Absolute Arror: " << mae << std::endl;
        }
        EXPECT_TRUE(result);
    }

    TEST_F(PID_Test, ITEST)
    {
        double kp = 0.0F;
        double ki = 1.0F;
        double kd = 0.0F;
        double dT = 0.01F;
        double imax = 5.0F;
        double imin = -5.0F;
        double initial_val = 0.0F;
        bool result = false;

        Eigen::VectorXd error(630);          /// Control Error
        Eigen::VectorXd pid_expected(630);   /// Expected
        Eigen::VectorXd pid_out(630);        /// Controller Output
        double cmp_error = 0.0F;            /// Compare Error
        double mae = 0.0F;                  /// Mean Abs Error

        /// Initialize Controller
        pid_ptr->Init();
        pid_ptr->Update_I_Sat_Limit(imax, imin);
        pid_ptr->Update_InitVal(initial_val);
        pid_ptr->Update_Gains(kp, ki, kd);
        pid_ptr->Update_dT(dT);

        /// Run Controler with 0.57 deg/10ms error signal
        for(int i=0; i<630; i++)
        {
            error(i) = sin(i*dT);

            pid_expected(i) = ki*1.0F-cos(i*dT);
            pid_out(i) = pid_ptr->Step(error(i));
            cmp_error = pid_expected(i) - pid_out(i);
            mae += cmp_error;
        }

        /// Mean Absolute Error
        mae /= 630;
        if(fabs(mae) < 0.002)
        {
            result = true;
        }
        else
        {
            result = false;
            std::cerr << "ITest Mean Absolute Error: " << mae << std::endl;
        }
        EXPECT_TRUE(result);
    }

    TEST_F(PID_Test, ISAT_TEST)
    {
        double kp = 0.0F;
        double ki = 1.0F;
        double kd = 0.0F;
        double dT = 0.01F;
        double max = 1.0F;
        double min = -1.0F;
        double imax = 0.5F;
        double imin = -0.5F;
        double initial_val = 0.0F;
        bool result = false;

        double pid_out;        /// Controller Output
        double error;          /// Controller Error

        /// Initialize Controller
        pid_ptr->Init();
        pid_ptr->Update_Sat_Limit(max, min);
        pid_ptr->Update_I_Sat_Limit(imax, imin);
        pid_ptr->Update_InitVal(initial_val);
        pid_ptr->Update_Gains(kp, ki, kd);
        pid_ptr->Update_dT(dT);

        /// Run Controler with 0.57 deg/10ms error signal
        for(int i=0; i<1260; i++)
        {
            error = sin(i*dT);
            pid_out = pid_ptr->Step(error);
            if(pid_out > imax || pid_out < imin)
            {
                result = false;
                std::cerr << "PID_OUT: " << pid_out << "   I LIM +/-: "
                          << imax << std::endl;
            }
        }
        EXPECT_TRUE(result);
    }

    TEST_F(PID_Test, PIDSAT_DYN_UPDATE_TEST)
    {
        double kp = 1.0F;
        double ki = 2.0F;
        double kd = 0.0F;
        double dT = 0.01F;
        double max = 1.6F;
        double min = 1.6F;
        double imax = 0.5F;
        double imin = 0.5F;
        double initial_val = 0.0F;
        bool result = false;

        double pid_out;        /// Controller Output
        double error;          /// Controller Error

        /// Initialize Controller
        pid_ptr->Init();
        pid_ptr->Update_Sat_Limit(max, min);
        pid_ptr->Update_I_Sat_Limit(imax, imin);
        pid_ptr->Update_InitVal(initial_val);
        pid_ptr->Update_Gains(kp, ki, kd);
        pid_ptr->Update_dT(dT);

        /// Run Controler with 0.57 deg/10ms error signal
        for(int i=0; i<1260; i++)
        {

            if(i < 315)
            {
                error = 1.0F;
            }
            else if (i == 315)
            {
                /// Dynamically Update P-Gain so that PID Limit is Exercised
                /// At this point, the I term should be saturated at 0.5F
                /// The PID Out should be Saturated at 1.5F (I:0.5F P:1.0F)
                kp = 5.0F;
                pid_ptr->Update_Gains(kp, ki, kd);
            }
            else if(i == 630)
            {
                error = -1.0F;
                kp = 1.0F;
                pid_ptr->Update_Gains(kp, ki, kd);
            }
            else if(i == 945)
            {
                /// Same Strategy as above but Exercising the negative limit
                kp = 5.0F;
                pid_ptr->Update_Gains(kp, ki, kd);
            }

            /// Step PID Controller
            pid_out = pid_ptr->Step(error);

            if(pid_out > max || pid_out < min)
            {
                result = false;
                std::cerr << "PID_OUT: " << pid_out << "   PID LIM +/-: "
                          << max << std::endl;
            }
        }
        EXPECT_TRUE(result);
    }

    TEST_F(PID_Test, DTERM_LPF_TEST)
    {
        double kp = 0.0F;
        double ki = 0.0F;
        double kd = 1.0F;
        double dT = 0.01F;
        double fltr_coef = 0.75F;
        double initial_val = 0.0F;
        bool result = false;

        Eigen::VectorXd error(315);        /// Control Error
        Eigen::VectorXd pid_expected(315); /// Expected
        Eigen::VectorXd pid_out(315);      /// Controller Output
        double cmp_error = 0.0F;            /// Compare Error
        double mae = 0.0F;                  /// Mean Abs Error
        double rmse = 0.0F;                 //RMSE

        /// Initialize Controller
        pid_ptr->Init();
        pid_ptr->Update_InitVal(initial_val);
        pid_ptr->Update_Gains(kp, ki, kd);
        pid_ptr->Update_D_Filter(fltr_coef);
        pid_ptr->Update_dT(dT);

        /// Run Controler with 0.57 deg/10ms error signal
        for(int i=0; i<315; i++)
        {
            error(i) = sin(i*dT);
            pid_expected(i) = kd*cos(i*dT);

            /// Inject Transient
            /// RMSE NO Transient No LPF:           0.0564541
            /// RMSE With Transient No LPF:         0.069098
            /// RMSE No Tranisent With COEF 0.75:   0.0584871
            /// RMSE Transient With COEF 0.75       0.0643062
            if(i == 310)
            {
                error(i) = 0.005F+error(i);
            }

            pid_out(i) = pid_ptr->Step(error(i));
            cmp_error = pid_expected(i) - pid_out(i);
            mae += cmp_error;
            rmse += cmp_error *cmp_error;
        }

        mae /= 315;
        rmse /= 315;
        rmse = sqrt(rmse);

        if(rmse > 0.065F)
        {
            result = false;
            std::cerr << "RMSE: " << rmse << std::endl;
            std::cerr << "MAE: " << mae << std::endl;
        }

        EXPECT_TRUE(result);
    }

    TEST_F(PID_Test, DTEST)
    {
        double kp = 0.0F;
        double ki = 0.0F;
        double kd = 1.3F;
        double dT = 0.01F;
        bool result = false;

        Eigen::VectorXd error(630);          /// Control Error
        Eigen::VectorXd pid_expected(630);   /// Expected
        Eigen::VectorXd pid_out(630);        /// Controller Output
        double cmp_error = 0.0F;            /// Compare Error
        double mae = 0.0F;
        double rmse = 0.0F;                  /// Mean Abs Error

        /// Initialize Controller
        pid_ptr->Init();
        pid_ptr->Update_Gains(kp, ki, kd);
        pid_ptr->Update_dT(dT);

        /// Run Controler with 0.57 deg/10ms error signal
        for(int i=0; i<630; i++)
        {
            error(i) = sin(i*dT);

            pid_expected(i) = kd*cos(i*dT);
            pid_out(i) = pid_ptr->Step(error(i));
            cmp_error = pid_expected(i) - pid_out(i);
            mae += cmp_error;
            cmp_error = cmp_error * cmp_error;
            rmse += cmp_error;
        }

        /// Mean Absolute Error
        mae /= 630;
        rmse /= 630;
        rmse = sqrt(rmse);
        //std::cerr << "DTest RMSE " << rmse << std::endl;
        //std::cerr << "DTest MAE " << mae << std::endl;
        if(fabs(mae) < 0.005)
        {
            result = true;
        }
        else
        {
            result = false;
            std::cerr << "DTest Mean Absolute Error: " << mae << std::endl;
        }
        EXPECT_TRUE(result);
    }
};

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


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#include "GaussSeq.h"
#include "ARSeq.h"

using utils::my_float;

int main() {
    using boost::multiprecision::cpp_bin_float_50;

    // set precision of output
//    std::streamsize precision = std::numeric_limits<cpp_bin_float_50>::digits10;
//    std::cout << std::setprecision(10);

    // testing
    std::map<int, my_float> coeff {
        {3, 0.5},
        {20, 20},
    };
    ARSeq seq(coeff, 1);
    seq.print_lags();
    std::vector<my_float> v;
    for (auto i = 0; i < 20; i++) {
        v.push_back(0);
    }
    v[2] = 2;
    v[19] = 3;

    seq.seed_prev_vals(v);

    seq.print_past_vals();
    for (auto i = 0; i < 100; i++) {
        seq.next();
    }
    seq.print_past_vals();

    return 0;
}


#pragma once

// C++ standard library
#include <utility>

// Armadillo
#include <armadillo>

namespace mant {
  namespace itd {
    const double heliocentricGravitationalConstant = 1.32712440018e20;
    const double solarMass = 1.98892e30;

    double stumpffFunction(
        const double parameter,
        const arma::uword type);

    double timeOfFlight(
        const double universalVariable,
        const arma::vec::fixed<3>& departurePosition,
        const arma::vec::fixed<3>& arrivalPosition,
        const bool useProgradeTrajectory);

    double sphereOfInfluenceRadius(
        const double semimajorAxis,
        const double mass);

    std::pair<arma::vec::fixed<3>, arma::vec::fixed<3>> positionAndVelocityOnOrbit(
        const double modifiedJulianDate,
        const arma::vec::fixed<7>& keplerianElements);

    arma::vec::fixed<3> gravityAssist(
        const arma::vec::fixed<3>& satelliteVelocity,
        const arma::vec::fixed<3>& planetPosition,
        const arma::vec::fixed<3>& planetVelocity,
        const double standardGravitationalParameter,
        const double periapsis);

    std::pair<arma::vec::fixed<3>, arma::vec::fixed<3>> lambert(
        const arma::vec::fixed<3>& departurePosition,
        const arma::vec::fixed<3>& arrivalPosition,
        const bool useProgradeTrajectory,
        const arma::uword numberOfRevolutions,
        const double transferTime);
  }
}


#define CATCH_CONFIG_RUNNER
#include "catch.hpp"

#ifdef _OPENMP
#include <omp.h>
#endif
#include <general/nmspc_tensor_omp.h>

#ifdef OpenBLAS_AVAILABLE
#include <cblas.h>
    #include <openblas_config.h>
#endif

#ifdef MKL_AVAILABLE
#include <mkl.h>
#include <mkl_service.h>
#endif
#include <thread>

#include <Eigen/Core>
#include <h5pp/h5pp.h>
#include <math/svd.h>

TEST_CASE("Singular value decomposition in Eigen and Lapacke", "[svd]") {
    SECTION("Test all svd decompositions") {
        using reciter = h5pp::fs::recursive_directory_iterator;
        svd::settings svd_settings;
        svd_settings.threshold = 1e-8;
        svd_settings.loglevel  = 0;
        svd_settings.use_lapacke = false;
        svd::solver svd(svd_settings);

        [[maybe_unused]] Eigen::MatrixXcd U1,V1,U2,V2;
        Eigen::VectorXcd S1,S2;
        for(auto &item : reciter(std::string(TEST_MATRIX_DIR))) {
            if(item.path().filename().string().find("svdmatrix") == std::string::npos) continue;
            if(item.path().extension() != ".h5") continue;
            size_t     logLevel = 2;
            h5pp::File file(item.path().string(), h5pp::FilePermission::READONLY, logLevel);
            auto       matrix = file.readDataset<Eigen::MatrixXcd>("svdmatrix");
            svd.use_lapacke   = false;
            std::tie(U1, S1, V1) = svd.decompose(matrix);
            svd.use_lapacke   = true;
            std::tie(U2, S2, V2) = svd.decompose(matrix);
            double differenceS = std::log10((S2.array() - S1.array()).cwiseAbs().sum());
            fmt::print("S {:<32} diff {:.24f}\n", item.path().filename().string(), differenceS);
            REQUIRE(differenceS < -12);
        }
    }
}

int main(int argc, char **argv){

// Set the number of threads to be used
    [[maybe_unused]] int num_threads = 1;
#ifdef _OPENMP
    omp_set_num_threads(num_threads);
    Eigen::setNbThreads(num_threads);
    Textra::omp::setNumThreads(num_threads);
#ifdef OpenBLAS_AVAILABLE
    openblas_set_num_threads(num_threads);
    std::cout << OPENBLAS_VERSION
              << " compiled with parallel mode " << openblas_get_parallel()
              << " for target " << openblas_get_corename()
              << " with config " << openblas_get_config()
              << " with multithread threshold " << OPENBLAS_GEMM_MULTITHREAD_THRESHOLD
              << ". Running with " << openblas_get_num_threads() << " thread(s)" << std::endl;
#endif

#ifdef MKL_AVAILABLE
    mkl_set_num_threads(num_threads);
    std::cout <<  "Using Intel MKL with " <<  mkl_get_max_threads() << " threads" << std::endl;
#endif

#endif

    return Catch::Session().run(argc, argv);
}


#ifndef __PROBABILITY_DISTRIBUTIONS__ASYMMETRIC_NORMAL_HPP__
#define __PROBABILITY_DISTRIBUTIONS__ASYMMETRIC_NORMAL_HPP__

#include "asymmetric_distribution.hpp"
#include "normal.hpp"

#include <boost/random/normal_distribution.hpp>

namespace ProbabilityDistributions {
  template <class D, class W = D, class T = W>
  class AsymmetricNormal:
    public AsymmetricDistribution<AsymmetricNormal<D,W,T>,D,W,T> {
    public:
      AsymmetricNormal(T p, T mu, T sigma);

      static constexpr unsigned int sample_size = 1;

      void fix_sigma(bool fixed = true) { fixed_sigma_ = fixed; }
      bool is_sigma_fixed() const { return fixed_sigma_; }
      void set_sigma(T sigma) {
        assert(sigma > 0); sigma_ = sigma; sigma2_ = 2*sigma*sigma; }
      T get_sigma() const { return sigma_; }

      AsymmetricNormal<D,W,T> const&
        operator=(AsymmetricNormal<D,W,T> const& other) {
          base_class::set_p(other.get_p());
          base_class::set_mu(other.get_mu());
          set_sigma(other.get_sigma());
          base_class::fixed_p_ = other.is_p_fixed();
          base_class::fixed_mu_ = other.is_mu_fixed();
          fixed_sigma_ = other.is_sigma_fixed();
          return *this;
        }
      AsymmetricNormal<D,W,T> const&
        operator=(Normal<D,W,T> const& other) {
          base_class::set_p(0.5);
          base_class::set_mu(other.get_mu());
          set_sigma(other.get_sigma());
          base_class::fixed_p_ = true;
          base_class::fixed_mu_ = other.is_mu_fixed();
          fixed_sigma_ = other.is_sigma_fixed();
          return *this;
        }

    private:
      typedef AsymmetricDistribution<AsymmetricNormal<D,W,T>,D,W,T> base_class;
      friend class AsymmetricDistribution<AsymmetricNormal<D,W,T>,D,W,T>;

      struct TruncatedNormal {
        TruncatedNormal(T sigma): dist(0, sigma) { }

        template <class RNG>
        T operator()(RNG& rng) { return std::abs(dist(rng)); }

        boost::random::normal_distribution<T> dist;
      };

      TruncatedNormal create_gamma_plus() const;
      TruncatedNormal create_gamma_minus() const;

      T constant_likelihood() const;
      T negative_ll(T s, T mu) const;
      T positive_ll(T s, T mu) const;
      void set_parameter_vector(std::vector<T> const& p) {
        assert(p.size() == 1); sigma_ = p[0]; }
      std::vector<T> get_parameter_vector() const { return {sigma_}; }

      void init_MLE(MA::ConstArray<D> const& data,
          MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes);
      void end_MLE();
      void updated_p();

      void MLE_fixed_p(MA::ConstArray<D> const& data,
          MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes);

      bool fixed_sigma_;
      T sigma_, sigma2_, alpha_, alpha_inv_, alpha2_, alpha2_inv_;
      std::vector<T> pos_sum_all_0_, pos_sum_all_1_, pos_sum_all_2_;
      std::vector<T> neg_sum_all_0_, neg_sum_all_1_, neg_sum_all_2_;
  };
};

#include "asymmetric_normal_impl.hpp"

#endif


#include <iostream>
#include <fstream>
#include <vector>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/iterator.h>
#include <CGAL/Timer.h>

#include <boost/version.hpp>
#if BOOST_VERSION < 107200
#include <boost/progress.hpp>
using boost::progress_display;
#else
#include <boost/timer/progress_display.hpp>
using boost::timer::progress_display;
#endif

#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/K_neighbor_search.h>
#include <CGAL/Search_traits_3.h>

#include <ANN/ANN.h>
#include <ANN/ANNperf.h>

#include<sfcnn.hpp>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef Kernel::Point_3 Point_3;
typedef CGAL::Random_points_in_cube_3<Point_3> Random_points_iterator;
typedef CGAL::Counting_iterator<Random_points_iterator> N_Random_points_iterator;

typedef CGAL::Search_traits_3<Kernel> TreeTraits;
typedef CGAL::Sliding_midpoint<TreeTraits> Splitter;
typedef CGAL::Orthogonal_k_neighbor_search<TreeTraits> OK_search;

template <class Output_iterator>
int read_points(std::string filename,Output_iterator out)
{
  std::ifstream file(filename.c_str());
  int nb_pts;
  Point_3 p;
  file >> nb_pts;
  for (int i=0;i<nb_pts;++i){
    file >> p;
    *out++=p;
  }
  return nb_pts;
}

int main(int argc,char** argv)
{

  if (argc!=3 && argc!=4){
    std::cerr << "Usage: main nb_input_point nb_neighbors_needed\n";
    std::cerr << "or     main Input.xyz Queries.xyz nb_neighbors_needed\n";
    exit(EXIT_FAILURE);
  }

  int nb_input_point,k,nb_queries;
  std::vector<Point_3> points;
  std::vector<Point_3> queries;

  if (argc==3){
    nb_input_point=atoi(argv[1]);
    k=atoi(argv[2]);
    nb_queries=1;
    points.reserve(nb_input_point);
    Random_points_iterator rpit(1.0);
    points.insert(points.begin(),N_Random_points_iterator(rpit,0), N_Random_points_iterator(nb_input_point));
    queries.push_back(Point_3(0,0,0));
  }
  else{
    k=atoi(argv[3]);
    nb_input_point=read_points(argv[1],std::back_inserter(points));
    nb_queries=read_points(argv[2],std::back_inserter(queries));
  }

  std::cout << "Looking for " << k << " nearest neighbors amongst " << nb_input_point << " points, with "<< nb_queries <<" query points.\n";

  std::random_shuffle(points.begin(),points.end());

  double STANN_time=0,ANN_time=0,OK_time=0;

  CGAL::Timer time;
//Building trees
//--STANN
  time.start();
  sfcnn<Point_3, 3, double> NN(&points[0], nb_input_point);
  time.stop();
  double STANN_tree=time.time();
  time.reset();
//--ANN
  ANNpointArray    dataPts;     // data points
  ANNpoint         queryPt;     // query point
  ANNidxArray      nnIdx;       // near neighbor indices
  ANNdistArray     dists;       // near neighbor distances
  ANNkd_tree*      kdTree;      // search structure
  dataPts = annAllocPts(nb_input_point, 3);      // allocate data points

  queryPt = annAllocPt(3);          // allocate query point
  nnIdx = new ANNidx[k];            // allocate near neigh indices
  dists = new ANNdist[k];            // allocate near neighbor dists
  //set points
  for (int i=0;i<nb_input_point;++i){
     dataPts[i][0]=points[i][0];
     dataPts[i][1]=points[i][1];
     dataPts[i][2]=points[i][2];
  }
  time.start();
  kdTree = new ANNkd_tree(dataPts,nb_input_point,3);
  time.stop();
  double ANN_tree=time.time();
  time.reset();
//--OK_search
  time.start();
  Splitter splitter(10); //bucket size can be changed here
  OK_search::Tree ok_tree(points.begin(),points.end(),splitter);
  ok_tree.build();
  time.stop();
  double OK_tree=time.time();
  time.reset();

  progress_display show_progress( nb_queries );

//running NN algorithms
  for (std::vector<Point_3>::const_iterator it=queries.begin();it!=queries.end();++it)
  {
    const Point_3& query=*it;

  //STANN
    CGAL::Timer time;
    std::vector<long unsigned int> answer;
    time.start();
    NN.ksearch(query, k, answer);
    time.stop();
    STANN_time+=time.time();
    time.reset();
  //ANN
    queryPt[0]=query[0];
    queryPt[1]=query[1];
    queryPt[2]=query[2];

    time.start();
    kdTree->annkSearch(            // search
                    queryPt,            // query point
                    k,                // number of near neighbors
                    nnIdx,              // nearest neighbors (returned)
                    dists,              // distance (returned)
                    0);              // error bound

    time.stop();
    ANN_time+=time.time();
    time.reset();

    //~ ANNkdStats stats;
    //~ kdTree->getStats(stats);
    //~ std::cout << "====ANN stats ====\n";
    //~ std::cout << "dimension of space " << stats.dim << "\n";
    //~ std::cout << "no. of points " << stats.n_pts << "\n";
    //~ std::cout << "bucket size " << stats.bkt_size << "\n";
    //~ std::cout << "no. of leaves (including trivial) " << stats.n_lf << "\n";
    //~ std::cout << "no. of trivial leaves (no points) " << stats.n_tl << "\n";
    //~ std::cout << "no. of splitting nodes " << stats.n_spl << "\n";
    //~ std::cout << "no. of shrinking nodes (for bd-trees) " << stats.n_shr << "\n";
    //~ std::cout << "depth of tree " << stats.depth << "\n";
    //~ std::cout << "sum of leaf aspect ratios " << stats.sum_ar << "\n";
    //~ std::cout << "average leaf aspect ratio " << stats.avg_ar << "\n";
    //~ std::cout << "==================\n";

    //~ std::cout << "====ANN tree  ====\n";
    //~ kdTree->Print(ANNtrue,std::cout);
    //~ std::cout << "==================\n";

  //Ortho-k-NN
    std::vector<std::pair<Point_3,double> > ok_result;
    ok_result.reserve(k);
    time.start();
    OK_search ok_search(ok_tree, query,k);
    time.stop();
    std::copy(ok_search.begin(),ok_search.end(),std::back_inserter(ok_result));
    OK_time+=time.time();

    //~ std::cout << std::endl; ok_tree.statistics(std::cout); std::cout << std::endl;

    //~ std::cout << "====CGAL tree ====\n";
    //~ ok_tree.print();
    //~ std::cout << "==================\n";

    for (int i = 0; i < k; i++) {      //check results are the same
      if ( nnIdx[i]!=answer[i] ){
        std::cerr << "STANN and ANN produced different results\n";
        exit(EXIT_FAILURE);
      }
      if ( ok_result[i].first!=points[answer[i]] ){
        std::cerr << "CGAL::OK_search and STANN (ANN) produced different results\n";
        exit(EXIT_FAILURE);
      }
    }
    ++show_progress;
  }

  std::cout << "Time to build trees\n";
  std::cout << "STANN ANN OK_search\n";
  std::cout << STANN_tree << " " << ANN_tree << " " << OK_tree << "\n";
  std::cout << "Time spent for all queries\n";
  std::cout << "STANN ANN OK_search\n";
  std::cout << STANN_time << " " << ANN_time << " " << OK_time << "\n";

  delete [] nnIdx;              // clean things up
  delete [] dists;
  delete kdTree;
  annClose();
}


#include <boost/random/uniform_on_sphere.hpp>


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

// 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.

#ifndef BOOST_GEOMETRY_PROJECTIONS_PHI2_HPP
#define BOOST_GEOMETRY_PROJECTIONS_PHI2_HPP

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry { namespace projections {
namespace detail {

template <typename T>
inline T pj_phi2(T const& ts, T const& e)
{
    static const T TOL = 1.0e-10;
    static const int N_ITER = 15;

    T eccnth, Phi, con, dphi;
    int i;

    eccnth = .5 * e;
    Phi = geometry::math::half_pi<T>() - 2. * atan (ts);
    i = N_ITER;
    do {
        con = e * sin (Phi);
        dphi = geometry::math::half_pi<T>() - 2. * atan (ts * pow((1. - con) /
           (1. + con), eccnth)) - Phi;
        Phi += dphi;
    } while ( geometry::math::abs(dphi) > TOL && --i);
    if (i <= 0)
        BOOST_THROW_EXCEPTION( projection_exception(-18) );
    return Phi;
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)

#ifndef BOOST_MATH_FFT_GSLBACKEND_HPP
#define BOOST_MATH_FFT_GSLBACKEND_HPP

#include <complex>

#if defined(__GNUC__)
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>
#endif

#include <boost/math/fft/dft_api.hpp>

namespace boost { namespace math {
namespace fft { namespace detail {

    #if defined(__GNUC__)
    template<class T, class A>
    class gsl_backend;

    template<class T, class A>
    class gsl_rfft_backend;

    template<class Allocator_t>
    class gsl_backend< std::complex<double>, Allocator_t >
    {
    public:
      using value_type     = std::complex<double>;
      using allocator_type = Allocator_t;

    private:
      using real_value_type    = double;
      using complex_value_type = std::complex<real_value_type>;
      enum plan_type { forward_plan, backward_plan };

      std::size_t my_size;
      allocator_type my_alloc;

      // complex fft
      gsl_fft_complex_wavetable *wtable;
      gsl_fft_complex_workspace *wspace;

      void execute(plan_type p, const complex_value_type* in, complex_value_type* out) const
      {
        if(in!=out)
        {
          // we avoid this extra step for in-place transforms
          // notice that if in==out, the following code has
          // undefined-behavior
          std::copy(in,in+size(),out);
        }

        if(p==forward_plan)
        gsl_fft_complex_forward(
          reinterpret_cast<real_value_type*>(std::addressof(*out)),
          1, my_size, wtable, wspace);
        else
        gsl_fft_complex_backward(
          reinterpret_cast<real_value_type*>(std::addressof(*out)),
          1, my_size, wtable, wspace);
      }
      void free()
      {
        gsl_fft_complex_wavetable_free(wtable);
        gsl_fft_complex_workspace_free(wspace);
      }
      void alloc()
      {
        wtable = gsl_fft_complex_wavetable_alloc(size());
        wspace = gsl_fft_complex_workspace_alloc(size());
      }
   public:

      gsl_backend(std::size_t n, const allocator_type& = allocator_type{}):
          my_size{n}
      {
        alloc();
      }

      ~gsl_backend()
      {
        free();
      }
      std::size_t size() const {return my_size;}

      void resize(std::size_t new_size)
      {
        if(size()!=new_size)
        {
          free();
          my_size = new_size;
          alloc();
        }
      }

      void forward(const complex_value_type* in, complex_value_type* out) const
      {
        execute(forward_plan,in,out);
      }
      void backward(const complex_value_type* in, complex_value_type* out) const
      {
        execute(backward_plan,in,out);
      }
    };

    template<class Allocator_t>
    class gsl_rfft_backend< double, Allocator_t >
    {
    public:
      // using value_type     = double;
      using allocator_type = Allocator_t;

    private:
      enum plan_type { forward_plan, backward_plan };
      using real_value_type    = double;

      template<class U>
      using vector_t = std::vector<U, typename std::allocator_traits<allocator_type>::template rebind_alloc<U> >;

      std::size_t my_size;
      allocator_type my_alloc;

      gsl_fft_real_wavetable        *real_wtable;
      gsl_fft_halfcomplex_wavetable *halfcomplex_wtable;
      gsl_fft_real_workspace        *real_wspace;

      void pack_halfcomplex(real_value_type* out) const
      // precondition:
      // -> size(out) >= N
      {
        const std::size_t N = size();
        vector_t<real_value_type> tmp(out,out+N);
        out[0]=tmp[0];
        for(unsigned int i=1,j=1;j<N;++i,j+=2)
        {
          out[j] = tmp[i];
          if(j+1<N)
            out[j+1] = -tmp[N-i];
        }
      }
      void unpack_halfcomplex(real_value_type* out) const
      // precondition:
      // -> size(out) >= N
      {
        const std::size_t N = size();
        vector_t<real_value_type> tmp(out,out+N);
        out[0]=tmp[0];
        for(unsigned int i=1,j=1;j<N;++i,j+=2)
        {
          out[i] = tmp[j];
          if(j+1<N)
            out[N-i] = -tmp[j+1];
        }
      }
      template<plan_type p>
      void execute(const real_value_type* in,
                   real_value_type* out,
                   const typename std::enable_if<p==forward_plan>::type* = nullptr) const
      {
        const std::size_t N = size();
        if(in!=out)
        {
          // we avoid this extra step for in-place transforms
          // notice that if in==out, the following code has
          // undefined-behavior
          std::copy(in,in+N,out);
        }
        gsl_fft_real_transform(
          out,1, N, real_wtable, real_wspace);
        unpack_halfcomplex(out);
      }
      template<plan_type p>
      void execute(const real_value_type* in,
                   real_value_type* out,
                   const typename std::enable_if<p==backward_plan>::type* = nullptr) const
      {
        const std::size_t N = size();
        if(in!=out)
        {
          // we avoid this extra step for in-place transforms
          // notice that if in==out, the following code has
          // undefined-behavior
          std::copy(in,in+N,out);
        }
        pack_halfcomplex(out);
        gsl_fft_halfcomplex_transform(
          out,1, N, halfcomplex_wtable, real_wspace);
      }

      void free()
      {
        gsl_fft_real_wavetable_free(real_wtable);
        gsl_fft_halfcomplex_wavetable_free(halfcomplex_wtable);
        gsl_fft_real_workspace_free(real_wspace);
      }
      void alloc()
      {
        const std::size_t N = size();
        real_wtable        = gsl_fft_real_wavetable_alloc(N);
        halfcomplex_wtable = gsl_fft_halfcomplex_wavetable_alloc(N);
        real_wspace        = gsl_fft_real_workspace_alloc(N);
      }
   public:

      gsl_rfft_backend(std::size_t n, const allocator_type& A= allocator_type{}):
          my_size{n},
          my_alloc{A}
      {
        alloc();
      }

      ~gsl_rfft_backend()
      {
        free();
      }
      constexpr std::size_t size() const {return my_size;}
      constexpr std::size_t unique_complex_size() const {return my_size/2 + 1;}

      void resize(std::size_t new_size)
      {
        if(size()!=new_size)
        {
          free();
          my_size = new_size;
          alloc();
        }
      }

      void real_to_halfcomplex(const real_value_type* in, real_value_type* out) const
      {
        execute<forward_plan>(in,out);
      }
      void halfcomplex_to_real(const real_value_type* in, real_value_type* out) const
      {
        execute<backward_plan>(in,out);
      }
    };
    #endif
  } // namespace detail

  #if defined(__GNUC__)
  template<class RingType = std::complex<double>, class Allocator_t = std::allocator<RingType> >
  using gsl_dft = detail::complex_dft<detail::gsl_backend,RingType,Allocator_t>;

  template<class T = double, class Allocator_t = std::allocator<T> >
  using gsl_rdft = detail::real_dft<detail::gsl_rfft_backend,T,Allocator_t>;

  using gsl_transform = transform< gsl_dft<> >;
  using gsl_real_transform = transform< gsl_rdft<> >;
  #endif

} // namespace fft
} // namespace math
} // namespace boost

#endif // BOOST_MATH_FFT_GSLBACKEND_HPP


#ifndef EVALUATE_HPP
#define EVALUATE_HPP

#include <cstddef>
#include <tuple>
#include <type_traits>
#include <utility>
#include <boost/proto/proto.hpp>

#include "domain.hpp"
#include "terminal.hpp"
#include "tuple_expand.hpp"
#include "var.hpp"
#include "varstore.hpp"

namespace sme { namespace detail {

        namespace proto = boost::proto;

        struct eval_result {
            // create eval result
            template<typename Value, typename VarStore>
            static auto create(Value&& value, VarStore&& varstore) {
                return std::make_tuple(std::forward<Value>(value), std::forward<VarStore>(varstore));
            }

            // get value from eval result
            template<typename EvalResult>
            static decltype(auto) get_value(EvalResult const& result) {
                return std::get<0>(result);
            }

            // get varstore from eval result
            template<typename EvalResult>
            static decltype(auto) get_varstore(EvalResult const& result) {
                return std::get<1>(result);
            }
        };

        //
        template<
            typename Expr,
            typename Tag = typename proto::tag_of<Expr>::type
        >
        struct eval;

        // evaluate an expr with given varstore
        template<typename Expr, typename VarStore>
        auto evaluate(Expr const& expr, VarStore const& varstore) {
            eval<Expr const> eval_function;
            return eval_function(expr, varstore);
        }

        // +x, -x, !x ...
        #define UNARY_OPERATOR(OP, TAG)                                                                                \
            template<typename Expr>                                                                                \
            struct eval<Expr, proto::tag::TAG> {                                                                            \
                template<typename VarStore>                                                                                \
                auto operator()(Expr const& expr, VarStore const& varstore) const {                                         \
                    auto x = evaluate(proto::child(expr), varstore);                                                        \
                    return eval_result::create( OP eval_result::get_value(x), eval_result::get_varstore(x));                \
                }                                                                                \
            };

        // x + y, x - y ...
        #define BINARY_OPERATOR(OP, TAG)                                                                                \
            template<typename Expr>                                                                                \
            struct eval<Expr, proto::tag::TAG> {                                                                            \
                template<typename VarStore>                                                                                \
                auto operator()(Expr const& expr, VarStore const& varstore) const {                                         \
                    auto computed_lhs = evaluate(proto::left(expr), varstore);                                              \
                    auto computed_rhs = evaluate(                                                                           \
                        proto::right(expr),                                                                                \
                        varstore::create_view(eval_result::get_varstore(computed_lhs), varstore)                            \
                    );                                                                                \
                                                                                \
                    auto new_varstore = varstore::create_view(                                                              \
                        eval_result::get_varstore(computed_rhs),                                                            \
                        eval_result::get_varstore(computed_lhs)                                                             \
                    );                                                                                \
                                                                                \
                    return eval_result::create(                                                                             \
                        eval_result::get_value(computed_lhs) OP eval_result::get_value(computed_rhs),                       \
                        varstore::varstore_from_view(new_varstore)                                                          \
                    );                                                                                \
                }                                                                                \
            };

        // x += y ...
        #define COMPOUND_ASSIGNMENT_OPERATOR(OP, TAG)                                                                       \
            template<typename Expr>                                                                                \
            struct eval<Expr, proto::tag::TAG> {                                                                            \
                template<typename VarStore>                                                                                \
                auto operator()(Expr const& expr, VarStore const& varstore) const {                                         \
                    using VarId =                                                                                \
                        typename proto::result_of::value<                                                                   \
                            typename proto::result_of::left<Expr const>::type                                               \
                        >::type;                                                                                \
                    static_assert(                                                                                \
                        terminal::is_var<VarId>::value,                                                                     \
                        "\n\n\n\n*****     left hand side of an assignment must be a var     *****\n\n\n"                   \
                    );                                                                                \
                                                                                \
                    auto computed_rhs = evaluate(proto::right(expr), varstore);                                             \
                    auto lhs = var::get_value(                                                                              \
                        varstore::get_var<VarId>(                                                                           \
                            varstore::create_view(eval_result::get_varstore(computed_rhs), varstore)                        \
                        )                                                                                \
                    );                                                                                \
                                                                                \
                    lhs OP eval_result::get_value(computed_rhs);                                                            \
                                                                                \
                    auto new_var = varstore::varstore_from_var(var::create_var<VarId>(lhs));                                \
                    auto new_varstore = varstore::create_view(                                                              \
                        new_var,                                                                                \
                        eval_result::get_varstore(computed_rhs)                                                             \
                    );                                                                                \
                                                                                \
                    return eval_result::create(lhs, varstore::varstore_from_view(new_varstore));                            \
                }                                                                                \
            };

        // !sizeof(Expr) is always false
        // it's there just to make sure the compiler doesn't assert until this struct actually instantiated
        #define NOT_SUPPORTED_OPERATOR(OP, TAG)                                                                             \
            template<typename Expr>                                                                                \
            struct eval<Expr, proto::tag::TAG> {                                                                            \
                static_assert(                                                                                \
                    !sizeof(Expr),                                                                                \
                    "\n\n\n\n*****     " #OP " operator is not supported      *****\n\n\n"                                  \
                );                                                                                \
            };

        UNARY_OPERATOR(+, unary_plus)
        UNARY_OPERATOR(-, negate)
        UNARY_OPERATOR(~, complement)
        UNARY_OPERATOR(!, logical_not)
        UNARY_OPERATOR(++, pre_inc)
        UNARY_OPERATOR(--, pre_dec)
        NOT_SUPPORTED_OPERATOR(*, dereference)
        NOT_SUPPORTED_OPERATOR(&, address_of)
        NOT_SUPPORTED_OPERATOR(++, post_inc)
        NOT_SUPPORTED_OPERATOR(--, post_dec)

        BINARY_OPERATOR(<<, shift_left)
        BINARY_OPERATOR(>>, shift_right)
        BINARY_OPERATOR(*, multiplies)
        BINARY_OPERATOR(/, divides)
        BINARY_OPERATOR(%, modulus)
        BINARY_OPERATOR(+, plus)
        BINARY_OPERATOR(-, minus)

        BINARY_OPERATOR(<, less)
        BINARY_OPERATOR(>, greater)
        BINARY_OPERATOR(<=, less_equal)
        BINARY_OPERATOR(>=, greater_equal)
        BINARY_OPERATOR(==, equal_to)
        BINARY_OPERATOR(!=, not_equal_to)

        // cannot implement short circuit evaluation
        BINARY_OPERATOR(||, logical_or)
        BINARY_OPERATOR(&&, logical_and)

        BINARY_OPERATOR(&, bitwise_and)
        BINARY_OPERATOR(|, bitwise_or)
        BINARY_OPERATOR(^, bitwise_xor)

        COMPOUND_ASSIGNMENT_OPERATOR(<<=, shift_left_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(>>=, shift_right_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(*=, multiplies_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(/=, divides_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(%=, modulus_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(+=, plus_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(-=, minus_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(&=, bitwise_and_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(|=, bitwise_or_assign)
        COMPOUND_ASSIGNMENT_OPERATOR(^=, bitwise_xor_assign)

        #undef UNARY_OPERATOR
        #undef BINARY_OPERATOR
        #undef COMPOUND_ASSIGNMENT_OPERATOR
        #undef NOT_SUPPORTED_OPERATOR

        //
        // terminal
        //
        template<typename Expr>
        struct eval<Expr, proto::tag::terminal> {
            // constant terminal
            template<typename Terminal>
            struct helper {
                template<typename VarStore>
                auto operator()(Expr const& expr, VarStore const& varstore) const {
                    return eval_result::create(proto::value(expr), varstore::empty());
                }
            };

            // var terminal
            template<typename I>
            struct helper<terminal::var<I>> {
                template<typename VarStore>
                auto operator()(Expr const& expr, VarStore const& varstore) const {
                    return eval_result::create(
                        var::get_value(varstore::get_var<terminal::var<I>>(varstore)),
                        varstore::empty()
                    );
                }
            };

            template<typename VarStore>
            auto operator()(Expr const& expr, VarStore const& varstore) const {
                using Terminal = typename proto::result_of::value<Expr const>::type;
                return helper<Terminal>{}(expr, varstore);
            }
        };

        //
        // x = y
        //
        template<typename Expr>
        struct eval<Expr, proto::tag::assign> {
            template<typename VarStore>
            auto operator()(Expr const& expr, VarStore const& varstore) const {
                // left hand side must be a var
                using VarId =
                    typename proto::result_of::value<
                        typename proto::result_of::left<Expr const>::type
                    >::type;
                static_assert(
                    terminal::is_var<VarId>::value,
                    "\n\n\n\n*****     left hand side of an assignment must be a var     *****\n\n\n"
                );

                auto computed_rhs = evaluate(proto::right(expr), varstore);
                auto new_var = varstore::varstore_from_var(var::create_var<VarId>(eval_result::get_value(computed_rhs)));

                auto new_varstore = varstore::create_view(
                    new_var,
                    eval_result::get_varstore(computed_rhs)
                );

                return eval_result::create(
                    eval_result::get_value(computed_rhs),
                    varstore::varstore_from_view(new_varstore)
                );
            }
        };

        //
        // x(y, z, t)
        //
        template<typename Expr>
        struct eval<Expr, proto::tag::function> {

            // sme terminals
            template<typename Ignore0, std::size_t IsSmeTerminal>
            struct helper {
                template<typename, std::size_t SmeTerminalId>
                struct inner_helper;

                // original function
                template<typename Ignore1>
                struct inner_helper<Ignore1, terminal::id::original_function> {
                    template<std::size_t Begin, std::size_t End>
                    struct collect_and_call {
                        template<typename VarStore, typename Tuple>
                        auto operator()(Expr const& expr, VarStore const& varstore, Tuple const& t) const {
                            auto arg = evaluate(proto::child_c<Begin>(expr), varstore);
                            decltype(auto) value = eval_result::get_value(arg);
                            auto new_varstore = varstore::create_view(eval_result::get_varstore(arg), varstore);

                            return collect_and_call<Begin+1, End>{}(expr, new_varstore, std::tuple_cat(t, std::tie(value)));
                        }
                    };

                    template<std::size_t End>
                    struct collect_and_call<End, End> {
                        template<typename VarStore, typename Tuple>
                        auto operator()(Expr const& expr, VarStore const& varstore, Tuple const& t) const {
                            // expand the collected values from tuple to function
                            return tuple_utility::expand(t, proto::value(proto::child_c<0>(expr)));
                        }
                    };

                    template<typename VarStore>
                    auto operator()(Expr const& expr, VarStore const& varstore) const {
                        auto value = collect_and_call<1, proto::arity_of<Expr const>::value>{}(expr, varstore, std::tuple<>{});

                        // return computed value
                        return eval_result::create(value, varstore::empty());
                    }
                };

                // scoped
                template<typename Ignore1>
                struct inner_helper<Ignore1, terminal::id::scoped> {
                    template<typename VarStore>
                    auto operator()(Expr const& expr, VarStore const& varstore) const {
                        auto result = evaluate(proto::child_c<1>(expr), varstore);
                        return eval_result::create(eval_result::get_value(result), varstore::empty());
                    }
                };

                template<typename VarStore>
                auto operator()(Expr const& expr, VarStore const& varstore) const {
                    return inner_helper<
                        void,
                        terminal::sme_terminal_expr_id<
                            typename proto::result_of::child_c<Expr const, 0>::type
                        >::value
                    >{}(expr, varstore);
                }
            };

            // expr function
            template<typename Ignore0>
            struct helper<Ignore0, 0> {
                template<std::size_t Begin, std::size_t End>
                struct collect_and_call {
                    template<typename VarStore>
                    auto operator()(Expr const& expr, VarStore const& varstore) const {
                        auto arg = evaluate(proto::child_c<Begin>(expr), varstore);
                        auto new_varstore = varstore::create_view(eval_result::get_varstore(arg), varstore);

                        return collect_and_call<Begin+1, End>{}(expr, new_varstore);
                    }
                };

                template<std::size_t End>
                struct collect_and_call<End, End> {
                    template<typename VarStore>
                    auto operator()(Expr const& expr, VarStore const& varstore) const {
                        // looped through all arguments
                        // now evaluate function
                        auto result = evaluate(proto::child_c<0>(expr), varstore);

                        // only return the value, ignore the varstore
                        return eval_result::create(eval_result::get_value(result), varstore::empty());
                    }
                };

                template<typename VarStore>
                auto operator()(Expr const& expr, VarStore const& varstore) const {
                    return collect_and_call<1, proto::arity_of<Expr const>::value>{}(expr, varstore);
                }
            };

            template<typename VarStore>
            auto operator()(Expr const& expr, VarStore const& varstore) const {
                return helper<
                    void,
                    terminal::is_sme_terminal_expr<
                        typename proto::result_of::child_c<Expr const, 0>::type
                    >::value
                >{}(expr, varstore);
            }

        };

        //
        // (x + y) ->* z
        // compute (x + y), return z
        //
        template<typename Expr>
        struct eval<Expr, proto::tag::mem_ptr> {
            template<typename VarStore>
            auto operator()(Expr const& expr, VarStore const& varstore) const {
                auto computed_lhs = evaluate(proto::left(expr), varstore);
                auto computed_rhs = evaluate(
                    proto::right(expr),
                    varstore::create_view(eval_result::get_varstore(computed_lhs), varstore)
                );

                auto new_varstore = varstore::create_view(
                    eval_result::get_varstore(computed_rhs),
                    eval_result::get_varstore(computed_lhs)
                );

                return eval_result::create(
                    eval_result::get_value(computed_rhs),
                    varstore::varstore_from_view(new_varstore)
                );
            }
        };

        //
        // x[y]
        //
        template<typename Expr>
        struct eval<Expr, proto::tag::subscript> {
            template<typename VarStore>
            auto operator()(Expr const& expr, VarStore const& varstore) const {
                auto computed_lhs = evaluate(proto::left(expr), varstore);
                auto computed_rhs = evaluate(
                    proto::right(expr),
                    varstore::create_view(eval_result::get_varstore(computed_lhs), varstore)
                );

                auto new_varstore = varstore::create_view(
                    eval_result::get_varstore(computed_rhs),
                    eval_result::get_varstore(computed_lhs)
                );

                return eval_result::create(
                    eval_result::get_value(computed_lhs)[eval_result::get_value(computed_rhs)],
                    varstore::varstore_from_view(new_varstore)
                );
            }
        };

        //
        // x, y, z
        //
        template<typename Expr>
        struct eval<Expr, proto::tag::comma> {
            template<typename VarStore>
            auto operator()(Expr const& expr, VarStore const& varstore) const {
                auto computed_lhs = evaluate(proto::left(expr), varstore);
                auto computed_rhs = evaluate(
                    proto::right(expr),
                    varstore::create_view(eval_result::get_varstore(computed_lhs), varstore)
                );

                auto new_varstore = varstore::create_view(
                    eval_result::get_varstore(computed_rhs),
                    eval_result::get_varstore(computed_lhs)
                );

                return eval_result::create(
                    eval_result::get_value(computed_rhs),
                    varstore::varstore_from_view(new_varstore)
                );
            }
        };

}} // namespace sme::detail

#endif // EVALUATE_HPP


#include <iostream>
#include <omp.h>
#include <Eigen/Sparse>
#include <Eigen/Dense>

#include <unsupported/Eigen/SparseExtra> // For reading MatrixMarket files
#include <unsupported/Eigen/IterativeSolvers>

#include <amgcl/adapter/eigen.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/solver/bicgstab.hpp>
#include <amgcl/solver/bicgstabl.hpp>
#include <amgcl/solver/idrs.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/ilu0.hpp>
#include <amgcl/relaxation/as_preconditioner.hpp>
#include <amgcl/profiler.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/eigen.hpp>
#include <amgcl/backend/eigen.hpp>

#include "../include/config.hpp"

#ifdef USE_CUDA
#include <amgcl/backend/cuda.hpp>
#include <amgcl/relaxation/cusparse_ilu0.hpp>
#endif
#include "cxxopts.hpp"

struct LinearSystem
{
    Eigen::VectorXd b;
    Eigen::VectorXd x0;
    Eigen::SparseMatrix<double, Eigen::RowMajor> A;
};

struct Convoptions
{
    int iterations;
    double tolerance;
};

struct returnvalue
{
    int iterations;
    double error;
    double error_exact;
};

template <class T>
returnvalue RunEigen_Solver(amgcl::profiler<> &prof, Convoptions opt, const LinearSystem &Axb, const std::string &name)
{

    prof.tic("setup_" + name);
    T Solver;
    Solver.setTolerance(opt.tolerance);
    Solver.setMaxIterations(opt.iterations);
    Solver.compute(Axb.A);
    prof.toc("setup_" + name);
    prof.tic("solve_" + name);
    Eigen::VectorXd result_eigen = Solver.solveWithGuess(Axb.b, Axb.x0);
    prof.toc("solve_" + name);
    returnvalue result;
    result.iterations = Solver.iterations();
    result.error = Solver.error();
    result.error_exact=(Axb.A*result_eigen-Axb.b).norm()/Axb.b.norm();
    return result;
}

template <class T>
returnvalue RunAMGCLEigen_backend(amgcl::profiler<> &prof, Convoptions opt, const LinearSystem &Axb, const std::string &name)
{

    Eigen::VectorXd x = Axb.x0;
    typename T::params prm;
    prm.solver.tol = opt.tolerance;
    prm.solver.maxiter = opt.iterations;
    prof.tic("setup_" + name);
    T solve(Axb.A, prm);
    prof.toc("setup_" + name);
    returnvalue result;
    prof.tic("solve_" + name);
    std::tie(result.iterations, result.error) = solve(Axb.A, Axb.b, x);
    prof.toc("solve_" + name);
    std::cout << solve << std::endl;
    result.error_exact=(Axb.A*x-Axb.b).norm()/Axb.b.norm();
    return result;
}

template <class T>
returnvalue RunAMGCL_backend(amgcl::profiler<> &prof, Convoptions opt, const LinearSystem &Axb, const std::string &name)
{

    std::vector<double> x0 = std::vector<double>(Axb.x0.data(), Axb.x0.data() + Axb.x0.size());
    std::vector<double> b = std::vector<double>(Axb.b.data(), Axb.b.data() + Axb.b.size());
    size_t n = Axb.A.rows();
    const int *ptr = Axb.A.outerIndexPtr();
    const int *col = Axb.A.innerIndexPtr();
    const double *val = Axb.A.valuePtr();
    amgcl::backend::crs<double> A_amgcl(std::make_tuple(n,
                                                        amgcl::make_iterator_range(ptr, ptr + n + 1),
                                                        amgcl::make_iterator_range(col, col + ptr[n]),
                                                        amgcl::make_iterator_range(val, val + ptr[n])));

    typename T::params prm;
    prm.solver.tol = opt.tolerance;
    prm.solver.maxiter = opt.iterations;
    prof.tic("setup_" + name);
    T solve(A_amgcl, prm);
    prof.toc("setup_" + name);
    returnvalue result;
    prof.tic("solve_" + name);
    std::tie(result.iterations, result.error) = solve(A_amgcl, b, x0);
    prof.toc("solve_" + name);
    std::cout << solve << std::endl;
    Eigen::Map<Eigen::VectorXd> x=Eigen::Map<Eigen::VectorXd>(x0.data(),x0.size());

    result.error_exact=(Axb.A*x-Axb.b).norm()/Axb.b.norm();
    return result;
}

#ifdef USE_CUDA

template <class T>
returnvalue RunAMGCLCUDA_backend(amgcl::profiler<> &prof, Convoptions opt, const LinearSystem &Axb, const std::string &name)
{

	thrust::device_vector<double> X = std::vector<double>(Axb.x0.data(), Axb.x0.data() + Axb.x0.size());
	thrust::device_vector<double> F = std::vector<double>(Axb.b.data(), Axb.b.data() + Axb.b.size());
size_t n = Axb.A.rows();

    amgcl::backend::cuda<double>::params bprm;
    cusparseCreate(&bprm.cusparse_handle);
    typename T::params prm;
    prm.solver.tol = opt.tolerance;
    prm.solver.maxiter = opt.iterations;
    prof.tic("setup_" + name);
    T solve(Axb.A, prm,bprm);
    prof.toc("setup_" + name);
    returnvalue result;
    prof.tic("solve_" + name);
    std::tie(result.iterations, result.error) = solve(F, X);
    prof.toc("solve_" + name);
    std::cout << solve << std::endl;
    thrust::host_vector<double> X_cpu=X;
    Eigen::Map<Eigen::VectorXd> x=Eigen::Map<Eigen::VectorXd>(X_cpu.data(),X_cpu.size());
    result.error_exact=(Axb.A*x-Axb.b).norm()/Axb.b.norm();
    return result;
}
#endif

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

    cxxopts::Options options("SparseSolverBench", "Benchmarking various sparse linear solvers");

    options.add_options()("t,threads", "How many threads to use", cxxopts::value<int>()->default_value("1"))("s,tolerance", "Tolerance to which solvers should converge", cxxopts::value<double>()->default_value("1e-12"))("i,iterations", "max number of iterations", cxxopts::value<int>()->default_value("1000"))("A,SparseMatrix", "MM Format File for Sparse Matrix", cxxopts::value<std::string>())("b,Rightside", "MM Format File for Vector", cxxopts::value<std::string>())("x,initialguess", "MM Format File for InitialGuess", cxxopts::value<std::string>())("h,help", "Print usage");

    auto result = options.parse(argc, argv);

    if (result.count("help"))
    {
        std::cout << options.help() << std::endl;
        exit(0);
    }

    std::string intial_guess_filename = "";
    if (result.count("initialguess"))
    {
        intial_guess_filename = result["initialguess"].as<std::string>();
    }

    Convoptions opt;

    int threads = result["threads"].as<int>();
    opt.tolerance = result["tolerance"].as<double>();
    opt.iterations = result["iterations"].as<int>();
    std::string A_filename = result["SparseMatrix"].as<std::string>();
    std::string b_filename = result["Rightside"].as<std::string>();

    std::cout << "Running profiler with " << threads << " threads\n"
              << "Required tolerance " << opt.tolerance << " num iterations " << opt.iterations << "\nA:" << A_filename << "\nb" << b_filename << std::endl;
    if (intial_guess_filename.size())
    {
        std::cout << "x0:" << intial_guess_filename << std::endl;
    }
    omp_set_num_threads(threads);

    amgcl::profiler<> prof;
    LinearSystem Axb;

    prof.tic("read_input");
    Eigen::loadMarket(Axb.A, A_filename);

    Eigen::loadMarketVector(Axb.b, b_filename);
    Axb.x0 = Eigen::VectorXd::Zero(Axb.A.rows());
    if (intial_guess_filename.size())
    {
        Eigen::loadMarketVector(Axb.x0, intial_guess_filename);
    }
    prof.toc("read_input");

    std::vector<std::string> names;
    std::vector<returnvalue> results;

    // Setup the solver:
    typedef amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::builtin<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::bicgstab<amgcl::backend::builtin<double>>>
        Solver_bicgstab;
names.push_back("amgcl_bicgstab");
  results.push_back(RunAMGCL_backend<Solver_bicgstab>(prof, opt, Axb, names.back()));

    typedef amgcl::make_solver<
        amgcl::relaxation::as_preconditioner<
        amgcl::backend::builtin<double>,
        amgcl::relaxation::ilu0>,
        amgcl::solver::bicgstab<amgcl::backend::builtin<double>>>
        Solver_bicgstab_ilut;
  names.push_back("amgcl_bicgstab_ilut");
  results.push_back(RunAMGCL_backend<Solver_bicgstab_ilut>(prof, opt, Axb, names.back()));

    // Setup the solver:
    typedef amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::builtin<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::bicgstabl<amgcl::backend::builtin<double>>>
        Solver_bicgstabl;
   names.push_back("amgcl_bicgstabl");
   results.push_back(RunAMGCL_backend<Solver_bicgstabl>(prof, opt, Axb, names.back()));

    // Setup the solver:
    typedef amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::builtin<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::idrs<amgcl::backend::builtin<double>>>
        Solver_idrs;

   names.push_back("amgcl_idrs");
   results.push_back(RunAMGCL_backend<Solver_idrs>(prof, opt, Axb, names.back()));

    // Setup the solver:
    typedef amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::eigen<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::bicgstab<amgcl::backend::eigen<double>>>
        Solver2;

   names.push_back("amgcl_bicgstab_eigen");
   results.push_back(RunAMGCLEigen_backend<Solver2>(prof, opt, Axb, names.back()));

  names.push_back("eigen_bicgstab");
results.push_back(RunEigen_Solver<Eigen::BiCGSTAB<Eigen::SparseMatrix<double, Eigen::RowMajor>>>(prof, opt, Axb, names.back()));

    names.push_back("eigen_bicgstabl");
    results.push_back(RunEigen_Solver<Eigen::BiCGSTABL<Eigen::SparseMatrix<double, Eigen::RowMajor>>>(prof, opt, Axb, names.back()));

    names.push_back("eigen_idrstab");
    results.push_back(RunEigen_Solver<Eigen::IDRStab<Eigen::SparseMatrix<double, Eigen::RowMajor>>>(prof, opt, Axb, names.back()));

#ifdef USE_CUDA

    typedef amgcl::make_solver<
        amgcl::amg<
        amgcl::backend::cuda<double>,
       amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::bicgstab<amgcl::backend::cuda<double>>>
        Solver_cuda_bicgstab;
    names.push_back("amgcl_cuda_bicgstab");
    results.push_back(RunAMGCLCUDA_backend<Solver_cuda_bicgstab>(prof, opt, Axb, names.back()));

    typedef amgcl::make_solver<
            amgcl::relaxation::as_preconditioner<
            amgcl::backend::cuda<double>,
            amgcl::relaxation::ilu0>,
            amgcl::solver::bicgstab<amgcl::backend::cuda<double>>>
            Solver_cuda_bicgstab_ilut;
        names.push_back("amgcl_cuda_bicgstab_ilut");
        results.push_back(RunAMGCLCUDA_backend<Solver_cuda_bicgstab_ilut>(prof, opt, Axb, names.back()));
    #endif

    std::cout << "Eigen uses " << Eigen::nbThreads() << " threads" << std::endl;
    for(int i=0;i<names.size();i++){
        std::cout << names[i]<<"\t iter:" << results[i].iterations << "\t error " << results[i].error << "\t error_exact " << results[i].error_exact <<std::endl;
    }

    std::cout << prof << std::endl;
}


#pragma once

#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

#include "hermite_to_nodal.hpp"
#include "polar_to_hermite.hpp"

#include <omp.h>
#include <Eigen/Dense>
#include <cassert>
#include <memory>
#include <string>

namespace boltzmann {

template <typename PolarBasis = SpectralBasisFactoryKS::basis_type>
class Polar2Nodal
{
 public:
  typedef PolarBasis polar_basis_t;
  typedef SpectralBasisFactoryHN::basis_type hermite_basis_t;
  typedef Polar2Hermite<polar_basis_t, hermite_basis_t> p2h_t;
  typedef Hermite2Nodal<hermite_basis_t> h2n_t;

 public:
  Polar2Nodal() { /* empty */}

  /**
   *  @brief Transform coefficients btw Polar-Laguerre and nodal basis. The
   *         nodes of the nodal basis are located at the underlying
   *         Gauss-Hermite quadrature points.
   *
   *  @param polar_basis Polar-Laguerre basis object
   *  @param a exp weight factor in quad rule, e.g. for basis functions which
   *           decay like \f$exp(-r^/2)\f$, @param a is 1.0
   */
  Polar2Nodal(const polar_basis_t &polar_basis, double a = 1.0);

 public:
  void init(const polar_basis_t &polar_basis, double a);
  std::shared_ptr<p2h_t> get_p2h() const { return p2h_; }
  std::shared_ptr<h2n_t> get_h2n() const { return h2n_; }
  /// size Polar-Laguerre basis
  int N() const { return N_; }
  /// max. polynomial degree
  int K() const { return K_; }

  template <typename DERIVED1, typename DERIVED2>
  void to_nodal(Eigen::DenseBase<DERIVED1> &dst,
                const Eigen::DenseBase<DERIVED2> &src,
                bool transpose = false) const;

  template <typename DERIVED1, typename DERIVED2>
  void to_polar(Eigen::DenseBase<DERIVED1> &dst,
                const Eigen::DenseBase<DERIVED2> &src,
                bool transpose = false) const;

  bool is_initialized() const { return is_initialized_; }

 private:
  std::shared_ptr<p2h_t> p2h_;
  std::shared_ptr<h2n_t> h2n_;
  int N_;
  int K_;
  bool is_initialized_ = false;

  thread_local static ArrayBuffer<> buf_;
};

template <typename PolarBasis>
thread_local ArrayBuffer<> Polar2Nodal<PolarBasis>::buf_;

template <typename PolarBasis>
Polar2Nodal<PolarBasis>::Polar2Nodal(const polar_basis_t &polar_basis, double a)
{
  this->init(polar_basis, a);
}

//  -------------------------------------------------------------------------------------
template <typename PolarBasis>
void
Polar2Nodal<PolarBasis>::init(const polar_basis_t &polar_basis, double a)
{
  K_ = spectral::get_max_k(polar_basis) + 1;
  N_ = polar_basis.n_dofs();

  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, K_, 2);

  assert(hermite_basis.n_dofs() == polar_basis.n_dofs());

  // make_unique not available in c++11
  // p2h_ = std::make_unique<p2h_t>(polar_basis, hermite_basis);
  // h2n_ = std::make_unique<h2n_t>(hermite_basis, K);
  typedef Eigen::MatrixXd mat_t;
  p2h_ = std::make_shared<p2h_t>(polar_basis, hermite_basis);
  const int K = K_;
  if (a == 1.0) {
    h2n_ = std::make_shared<h2n_t>(
        hermite_basis, K_, [K](mat_t &m1, mat_t &m2) { H2N_1d<>::create(m1, m2, K); });
  } else {
    h2n_ = std::make_shared<h2n_t>(
        hermite_basis, K_, [K, a](mat_t &m1, mat_t &m2) { H2NG_1d::create(m1, m2, K, a); });
  }

  is_initialized_ = true;
}

// //
// --------------------------------------------------------------------------------------
// template<typename PolarBasis>
// void Polar2Nodal<PolarBasis>::
// to_nodal(numeric_t* dst, const numeric_t* src, bool transpose) const
// {
//   typedef Eigen::Map<Eigen::VectorXd> mvec_t;
//   auto buffer = buf_.get<mvec_t>(N_);

//  if(!transpose) {
//     p2h_->to_hermite(buffer, src);
//     h2n_->to_nodal(dst, buffer);
//   } else {
//     throw std::runtime_error(std::string(__FILE__)
//                              + ":" + std::to_string(__LINE__) + " not
//                              implemented");
//   }
// }

// template<typename PolarBasis>
// void Polar2Nodal<PolarBasis>::
// to_polar(numeric_t* dst, const numeric_t* src, bool transpose) const
// {
//   auto buffer = buf_.get<Eigen::VectorXd>(N_);

//   if(!transpose) {
//     h2n_->to_hermite(buffer, src);
//     p2h_->to_polar(dst, buffer);
//   } else {
//     throw std::runtime_error(std::string(__FILE__)
//                              + ":" + std::to_string(__LINE__) + " not
//                              implemented");
//   }
// }

template <typename PolarBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Nodal<PolarBasis>::to_nodal(Eigen::DenseBase<DERIVED1> &dst,
                                  const Eigen::DenseBase<DERIVED2> &src,
                                  bool transpose) const
{
  auto buffer = buf_.get<Eigen::VectorXd>(N_);
  BOOST_ASSERT(is_initialized_);

  if (!transpose) {
    p2h_->to_hermite(buffer, src);
    h2n_->to_nodal(dst, buffer);
  } else {
    throw std::runtime_error(std::string(__FILE__) + ":" + std::to_string(__LINE__) +
                             " not implemented");
  }
}

template <typename PolarBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Nodal<PolarBasis>::to_polar(Eigen::DenseBase<DERIVED1> &dst,
                                  const Eigen::DenseBase<DERIVED2> &src,
                                  bool transpose) const
{
  BOOST_ASSERT(is_initialized_);
  auto buffer = buf_.get<Eigen::VectorXd>(N_);

  if (!transpose) {
    h2n_->to_hermite(buffer, src);
    p2h_->to_polar(dst, buffer);
  } else {
    throw std::runtime_error(std::string(__FILE__) + ":" + std::to_string(__LINE__) +
                             " not implemented");
  }
}
}  // end namespace boltzmann


#include <igl/copyleft/marching_cubes.h>
#include <igl/sparse_voxel_grid.h>
#include <igl/opengl/glfw/Viewer.h>

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

#include "tutorial_shared_path.h"

int main(int argc, char * argv[])
{
  // An implicit function which is zero on the surface of a sphere centered at the origin with radius 1
  // This function is negative inside the surface and positive outside the surface
  std::function<double(const Eigen::RowVector3d&)> scalar_func = [](const Eigen::RowVector3d& pt) -> double {
    return pt.norm() - 1.0;
  };

  // We know that the point (0, 0, 1) lies on the implicit surface
  Eigen::RowVector3d p0(0., 0., 1.);

  // Construct a sparse voxel grid whose cubes have edge length eps = 0.1.
  // The cubes will form a thin shell around the implicit surface
  const double eps = 0.1;

  // CS will hold one scalar value at each cube vertex corresponding
  // the value of the implicit at that vertex
  Eigen::VectorXd CS;

  // CV will hold the positions of the corners of the sparse voxel grid
  Eigen::MatrixXd CV;

  // CI is a #cubes x 8 matrix of indices where each row contains the
  // indices into CV of the 8 corners of a cube
  Eigen::MatrixXi CI;

  // Construct the voxel grid, populating CS, CV, and CI
  igl::sparse_voxel_grid(p0, scalar_func, eps, 1024 /*expected_number_of_cubes*/, CS, CV, CI);

  // Given the sparse voxel grid, use Marching Cubes to construct a triangle mesh of the surface
  Eigen::MatrixXi F;
  Eigen::MatrixXd V;
  igl::copyleft::marching_cubes(CS, CV, CI, V, F);

  // Draw the meshed implicit surface
  igl::opengl::glfw::Viewer viewer;
  viewer.data().clear();
  viewer.data().set_mesh(V,F);
  viewer.data().set_face_based(true);
  viewer.launch();
}