#pragma once

// std c++ headers
#include <cmath>

// mtl headers
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/reduction_functors.hpp>
#include <boost/numeric/mtl/operation/dot.hpp>

// AMDiS headers
#include "operations/assign.hpp"
#include "operations/functors.hpp"

namespace AMDiS
{
  namespace functors
  {
    // unary reduction functors: import from mtl
    using MTL_VEC::infinity_norm_functor;
    using MTL_VEC::sum_functor;
    using MTL_VEC::product_functor;
    using MTL_VEC::max_functor;
    using MTL_VEC::min_functor;

    /// Reduction functor to calculate the ||v||_1
    /** Defines \ref init, \ref update and \ref post_reduction
     *  so that ||v||_1 := |v_0| + |v_1| + ...:
     *  init: result = 0,
     *  update: result += |v_i|,
     *  post_reduction: result = result
     **/
    template <class A>
    struct one_norm_functor
    {
      using result_type = A;

      template <class Value>
      static void init(Value& value)
      {
        using ::math::zero;
        value= zero(value);
      }

      template <class Value, class Element>
      static void update(Value& value, const Element& x)
      {
        using std::abs;
        value+= abs(x);
      }

      template <class Value>
      static void finish(Value& value, const Value& value2)
      {
        value+= value2;
      }

      template <class Value>
      static constexpr Value post_reduction(Value const& value)
      {
        return value;
      }
    };

    /// Reduction functor to calculate the |v|_2
    /** Defines \ref init, \ref update and \ref post_reduction
     *  so that ||v||_2 := sqrt(|v_0|^2 + |v_1|^2 + ...):
     *  init: result = 0,
     *  update: result += |v_i|^2,
     *  post_reduction: result = sqrt(result)
     **/
    template <class A>
    struct two_norm_functor
    {
      using result_type = A;

      template <class Value>
      static void init(Value& value)
      {
        using ::math::zero;
        value= zero(value);
      }

      template <class Value, class Element>
      static void update(Value& value, Element const& x)
      {
        using mtl::squared_abs;
        value+= squared_abs(x);
      }

      template <class Value>
      static void finish(Value& value, Value const& value2)
      {
        value+= value2;
      }

      // After reduction compute square root
      template <class Value>
      static Value post_reduction(Value const& value)
      {
        using std::sqrt;
        return sqrt(value);
      }
    };

    /// Same as \ref two_norm_functor without the root at the end:
    /**
     *  post_reduction: result = result
     **/
    template <class A>
    struct unary_dot_functor
      : two_norm_functor<A>
    {
      template <typename Value>
      static Value post_reduction(Value const& value)
      {
        return value;
      }
    };

    /// \cond HIDDEN_SYMBOLS
    template <class A, class B, class ConjOp>
    struct dot_functor_aux
    {
      using result_type = decltype( std::declval<ConjOp>()(std::declval<A>()) * std::declval<B>() );

      template <class Value>
      static void init(Value& value)
      {
        using ::math::zero;
        value= zero(value);
      }

      template <class Value, class Element1, class Element2>
      static void update(Value& value, Element1 const& x, Element2 const& y)
      {
        value+= ConjOp()(x) * y;
      }

      template <class Value>
      static void finish(Value& value, Value const& value2, Value const& value3)
      {
        value+= ConjOp()(value2) * value3;
      }

      template <class Value>
      static constexpr Value post_reduction(Value const& value)
      {
        return value;
      }
    };
    /// \endcond

    /// Binary reduction functor (scalar product)
    /** Same as reduction functors, but \ref update has two arguments:
     *  init: result = 0,
     *  update: result += v_i^H * w_i,
     *  post_reduction: result =result
     **/
    template <class A, class B>
    using dot_functor
      = dot_functor_aux<A,B, MTL_VEC::detail::with_conj>;

    /// Binary reduction functor (scalar product)
    /** Same as reduction functors, but \ref update has two arguments:
     *  init: result = 0,
     *  update: result += v_i^T * w_i,
     *  post_reduction: result =result
     **/
    template <class A, class B>
    using dot_real_functor
      = dot_functor_aux<A,B, MTL_VEC::detail::without_conj>;

    template <class ResultType, class InitAssign, class UpdateAssign,
              class PostOp = identity<ResultType>, class FinishAssign = UpdateAssign>
    struct general_unary_reduction_functor
    {
      using result_type = ResultType;

      template <class Value>
      static void init(Value& value)
      {
        InitAssign()(value);
      }

      template <class Value, class Element>
      static void update(Value& value, Element const& x)
      {
        UpdateAssign()(value, x);
      }

      template <class Value>
      static void finish(Value& value, Value const& value2)
      {
        FinishAssign()(value, value2);
      }

      // After reduction compute square root
      template <class Value>
      static constexpr Value post_reduction(Value const& value)
      {
        return PostOp()(value);
      }
    };

    // max(v0, v1, v2, v3,...)
    template <class T>
    using max_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::min_value<T>, AMDiS::assign::max<T>>;

    // max(|v0|,|v1|,|v2|,...)
    template <class T>
    using abs_max_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::ct_value<T, int, 0>,
        AMDiS::assign::compose<AMDiS::assign::max<T>, 2, abs<T>>>;

    // min(v0, v1, v2, v3, ...)
    template <class T>
    using min_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::max_value<T>, AMDiS::assign::min<T>>;

    // min(|v0|,|v1|,|v2|,...)
    template <class T>
    using abs_min_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::max_value<T>,
        AMDiS::assign::compose<AMDiS::assign::min<T>, 2, abs<T>>>;

    // v0+v1+v2+v3+...
    template <class T>
    using sum_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::ct_value<T, int, 0>, AMDiS::assign::plus<T>>;

    // v0*v1*v2*v3*...
    template <class T>
    using prod_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::ct_value<T, int, 1>, AMDiS::assign::multiplies<T>>;

  } // end namespace functors
} // end namespace AMDiS


#ifndef ILP_TASK_HPP
#define ILP_TASK_HPP

#include <eigen3/Eigen/Core>
#include <boost/container_hash/hash.hpp>

#include <utility>

namespace ilp
{
    using index_t = Eigen::Index;

    template <typename T>
    using matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

    template <typename T>
    using rvector = Eigen::Matrix<T, 1, Eigen::Dynamic>;

    template <typename T>
    using cvector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

    namespace detail
    {
        template <typename T>
        struct VectorHash
        {
            std::size_t operator () (const rvector<T>& row) const
            {
                std::size_t seed = 0;
                for (index_t i = 0; i != row.cols(); ++i)
                {
                    boost::hash_combine(seed, row(i, 0));
                }

                return seed;
            }

            std::size_t operator () (const cvector<T>& column) const
            {
                std::size_t seed = 0;
                for (index_t i = 0; i != column.rows(); ++i)
                {
                    boost::hash_combine(seed, column(i, 0));
                }

                return seed;
            }
        };

    } // namespace detail

    class ilp_task
    {
    public:
        ilp_task(matrix<int> A, cvector<int> b, rvector<int> c);
        ~ilp_task() = default;

        [[nodiscard]] std::size_t size_m() const;
        [[nodiscard]] std::size_t size_n() const;

    public:
        matrix<int> A;
        cvector<int> b;
        rvector<int> c;
        cvector<int> x;

        std::size_t m;
        std::size_t n;
    };

    struct ilp_solution
    {
        bool is_feasible = false;
        bool is_bounded = false;
        cvector<int> x;
        int c_result = 0;
        double time = 0;
    };

} // namespace ilp

template <typename T>
std::ostream& operator << (std::ostream& os, const ilp::rvector<T>& vec)
{
    bool flag{false};

    os << "(";
    for (ilp::index_t col = 0; col < vec.cols(); ++col)
    {
        if (flag)
        {
            os << ", ";
        }
        flag = true;
        os << vec(col);
    }
    os << ")";
    return os;
}

template <typename T>
std::ostream& operator << (std::ostream& os, const ilp::cvector<T>& vec)
{
    bool flag{false};

    os << "(";
    for (ilp::index_t row = 0; row < vec.rows(); ++row)
    {
        if (flag)
        {
            os << ", ";
        }
        flag = true;
        os << vec(row);
    }
    os << ")^T";
    return os;
}

template <typename T>
std::ostream& operator << (std::ostream& os, const ilp::matrix<T>& A)
{
    for (int row = 0; row < A.rows(); ++row)
    {
        for (int col = 0; col < A.cols(); ++col)
        {
            os << "   " << A(row, col) << " ";
        }

        if (row != A.rows() - 1)
        {
            os <<  "\n";
        }
    }

    return os;
}

#endif // ILP_TASK_HPP


#ifndef visoptslider_hpp
#define visoptslider_hpp

#include <vector>
#include <Eigen/Core>
#include <QGroupBox>

class QLineEdit;
class QSlider;
namespace visopt { namespace internal { class VisualizationWidget; } }

namespace visopt
{
    class SlidersWidget : public QGroupBox
    {
    public:
        SlidersWidget(QWidget* parent = nullptr);

        void initialize(const int num_dimensions,
                        const std::function<double(const Eigen::VectorXd&)>& target_function,
                        const Eigen::VectorXd& upper_bound,
                        const Eigen::VectorXd& lower_bound,
                        const double maximum_value,
                        const double minimum_value,
                        const std::vector<std::string>& labels = {},
                        const bool show_values = false,
                        const int resolution = 200,
                        const int visualization_minimum_width = 200,
                        const int visualization_minimum_height = 32);

        int getNumDimensions() const { return num_dimensions_; }
        void setNumDimensions(const int num_dimensions)
        {
            num_dimensions_ = num_dimensions;
        }

        const Eigen::VectorXd& getArgument() const { return argument_; }

        void setTargetFunction(const std::function<double(const Eigen::VectorXd&)>& target_function)
        {
            target_function_ = target_function;
        }

        const Eigen::VectorXd& getUpperBound() const { return upper_bound_; }
        void setUpperBound(const Eigen::VectorXd& upper_bound)
        {
            upper_bound_ = upper_bound;
        }

        const Eigen::VectorXd& getLowerBound() const { return lower_bound_; }
        void setLowerBound(const Eigen::VectorXd& lower_bound)
        {
            lower_bound_ = lower_bound;
        }

        double getMaximumValue() const { return maximum_value_; }
        void setMaximumValue(const double maximum_value)
        {
            maximum_value_ = maximum_value;
        }

        double getMinimumValue() const { return minimum_value_; }
        void setMinimumValue(const double minimum_value)
        {
            minimum_value_ = minimum_value;
        }

        int getResolution() const { return resolution_; }
        void setResolution(const int resolution)
        {
            resolution_ = resolution;
        }

        double calculateValue(const Eigen::VectorXd& argument) const
        {
            return target_function_(argument);
        }

        void setArgumentAndUpdateSliders(const Eigen::VectorXd& argument);

        void setCallback(const std::function<void(void)>& callback)
        {
            callback_ = callback;
        }

        void setVisualizationMinimumSize(const int minimum_width, const int minimum_height);

    private:
        void slidersManipulatedViaGui();
        Eigen::VectorXd calculateArgumentFromCurrentSliders() const;
        void setSliderValuesUsingCurrentArgument();
        void setLabelsUsingCurrentArgument();

        int resolution_;

        int num_dimensions_;
        Eigen::VectorXd argument_;
        std::function<double(const Eigen::VectorXd&)> target_function_;

        Eigen::VectorXd upper_bound_;
        Eigen::VectorXd lower_bound_;
        double maximum_value_;
        double minimum_value_;

        std::vector<QSlider*> sliders_;
        std::vector<internal::VisualizationWidget*> visualizations_widgets_;
        std::vector<QLineEdit*> value_labels_;

        std::function<void(void)> callback_;
    };

    namespace internal
    {
        class VisualizationWidget : public QWidget
        {
        public:
            VisualizationWidget(const int target_dimension,
                                SlidersWidget* parent,
                                const int minimum_width,
                                const int minimum_height);

        protected:
            void paintEvent(QPaintEvent* event);

        private:
            const int target_dimension_;
            const SlidersWidget* parent_widget_ = nullptr;
        };
    }
}

#endif


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2010: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example static_interval.cpp \file static_interval.cpp
    \brief Intervals with static interval bounds.

    Intervals types with static or fixed interval bounds. Statically
    bounded intervals use up to 33% less memory than dynamically
    bounded ones. Of the four possible statically bounded intervals types
    right_open_intervals are the most important ones. We can switch the
    library default to statically bounded intervals by defining
    BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS.

    \include static_interval_/static_interval.cpp
*/
//[example_static_interval
#include <iostream>
#include <string>
#include <math.h>
#include <boost/type_traits/is_same.hpp>

// We can change the library default for the interval types by defining
#define BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS
// prior to other inluces from the icl.
// The interval type that is automatically used with interval
// containers then is the statically bounded right_open_interval.

#include <boost/icl/interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>
// The statically bounded interval type 'right_open_interval'
// is indirectly included via interval containers.

#include "../toytime.hpp"
#include <boost/icl/rational.hpp>

using namespace std;
using namespace boost;
using namespace boost::icl;

int main()
{
    cout << ">> Interval Container Library: Sample static_interval.cpp <<\n";
    cout << "------------------------------------------------------------\n";

    // Statically bounded intervals are the user defined library default for
    // interval parameters in interval containers now.
    BOOST_STATIC_ASSERT((
        boost::is_same< interval_set<int>::interval_type
                      , right_open_interval<int> >::value
                      ));

    BOOST_STATIC_ASSERT((
        boost::is_same< interval_set<float>::interval_type
                      , right_open_interval<float> >::value
                      ));

    // As we can see the library default both for discrete and continuous
    // domain_types T is 'right_open_interval<T>'.
    // The user defined library default for intervals is also available via
    // the template 'interval':
    BOOST_STATIC_ASSERT((
        boost::is_same< interval<int>::type
                      , right_open_interval<int> >::value
                      ));

    // Again we are declaring and initializing the four test intervals that have been used
    // in the example 'interval' and 'dynamic_interval'
    interval<int>::type    int_interval  = interval<int>::right_open(3, 8); // shifted the upper bound
    interval<double>::type sqrt_interval = interval<double>::right_open(1/sqrt(2.0), sqrt(2.0));

    // Interval ("Barcelona", "Boston"] can not be represented because there is no 'steppable next' on
    // lower bound "Barcelona". Ok. this is a different interval:
    interval<string>::type city_interval = interval<string>::right_open("Barcelona", "Boston");

    // Toy Time is discrete again so we can transfrom open(Time(monday,8,30), Time(monday,17,20))
    //                                       to right_open(Time(monday,8,31), Time(monday,17,20))
    interval<Time>::type   time_interval = interval<Time>::right_open(Time(monday,8,31), Time(monday,17,20));

    cout << "----- Statically bounded intervals ----------------------------------------\n";
    cout << "right_open_interval<int>   : " << int_interval  << endl;
    cout << "right_open_interval<double>: " << sqrt_interval << " does "
                                            << string(contains(sqrt_interval, sqrt(2.0))?"":"NOT")
                                            << " contain sqrt(2)" << endl;
    cout << "right_open_interval<string>: " << city_interval << " does "
                                            << string(contains(city_interval,"Barcelona")?"":"NOT")
                                            << " contain 'Barcelona'" << endl;
    cout << "right_open_interval<string>: " << city_interval << " does "
                                            << string(contains(city_interval, "Boston")?"":"NOT")
                                            << " contain 'Boston'" << endl;
    cout << "right_open_interval<Time>  : " << time_interval << "\n\n";

    // Using statically bounded intervals does not allows to apply operations
    // with elements on all interval containers, if their domain_type is continuous.
    // The code that follows is identical to example 'dynamic_interval'. Only 'internally'
    // the library default for the interval template now is 'right_open_interval'
    interval<rational<int> >::type unit_interval
        = interval<rational<int> >::right_open(rational<int>(0), rational<int>(1));
    interval_set<rational<int> > unit_set(unit_interval);
    interval_set<rational<int> > ratio_set(unit_set);
    // ratio_set -= rational<int>(1,3); // This line will not compile, because we can not
                                        // represent a singleton interval as right_open_interval.
    return 0;
}

// Program output:
//>> Interval Container Library: Sample static_interval.cpp <<
//------------------------------------------------------------
//----- Statically bounded intervals ----------------------------------------
//right_open_interval<int>   : [3,8)
//right_open_interval<double>: [0.707107,1.41421) does NOT contain sqrt(2)
//right_open_interval<string>: [Barcelona,Boston) does  contain 'Barcelona'
//right_open_interval<string>: [Barcelona,Boston) does NOT contain 'Boston'
//right_open_interval<Time>  : [mon:08:31,mon:17:20)
//]


#include <vector>

#include <boost/mpi.hpp>
#include <boost/test/minimal.hpp>

#include <elemental.hpp>
#include <CombBLAS.h>
#include <SpParMat.h>

#include <skylark.hpp>

#include "../../base/Gemm.hpp"
#include "../../base/Gemm_detail.hpp"

/** Typedef DistMatrix and Matrix */
typedef elem::Matrix<double> MatrixType;
typedef elem::DistMatrix<double, elem::VC, elem::STAR> DistMatrixVCSType;
typedef elem::DistMatrix<double> DistMatrixType;

typedef SpDCCols< size_t, double> col_t;
typedef SpParMat< size_t, double, col_t > cbDistMatrixType;

static const size_t matrix_size = 50;

static MatrixType nn_expected;
static MatrixType tn_expected;
static MatrixType nt_expected;
static MatrixType tt_expected;

template <typename dist_matrix_t>
void check_matrix(const dist_matrix_t &result, const MatrixType &expected,
                  const std::string error) {

    elem::DistMatrix<double, elem::STAR, elem::STAR> full_result = result;
    for(size_t j = 0; j < full_result.Height(); j++ )
        for(size_t i = 0; i < full_result.Width(); i++ ) {
            if(full_result.GetLocal(j, i) != expected.Get(j, i)) {
                std::cout << result.GetLocal(j, i) << " != "
                          << expected.Get(j, i)
                          << " at index (" << j << ", " << i << ")"
                          << std::endl;
                BOOST_FAIL(error.c_str());
            }
        }
}

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

    namespace mpi = boost::mpi;

#ifdef SKYLARK_HAVE_OPENMP
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
#endif

    mpi::environment env (argc, argv);
    mpi::communicator world;

    elem::Initialize (argc, argv);
    MPI_Comm mpi_world(world);
    elem::Grid grid (mpi_world);

    // compute local expected value
    MatrixType localA(matrix_size, matrix_size);
    for( size_t j = 0; j < localA.Height(); j++ ) {
        for( size_t i = 0; i < localA.Width(); i++ ) {
            double value = j * matrix_size + i + 1;
            localA.Set(j, i, value);
        }
    }

    elem::Ones(nn_expected, matrix_size, matrix_size);
    elem::Gemm(elem::NORMAL, elem::NORMAL, -1.0, localA, localA,
                1.5, nn_expected);
    elem::Ones(nt_expected, matrix_size, matrix_size);
    elem::Gemm(elem::NORMAL, elem::TRANSPOSE, -1.0, localA, localA,
                1.5, nt_expected);
    elem::Ones(tn_expected, matrix_size, matrix_size);
    elem::Gemm(elem::TRANSPOSE, elem::NORMAL, -1.0, localA, localA,
                1.5, tn_expected);
    elem::Ones(tt_expected, matrix_size, matrix_size);
    elem::Gemm(elem::TRANSPOSE, elem::TRANSPOSE, -1.0, localA, localA,
                1.5, tt_expected);

    // prepare an Elemental matrix with the test data
    double val = 0.0;
    elem::DistMatrix<double, elem::STAR, elem::STAR>
        A_stst(matrix_size, matrix_size, grid);
    for( size_t j = 0; j < A_stst.LocalHeight(); j++ ) {
        for( size_t i = 0; i < A_stst.LocalWidth(); i++ ) {
            val = (j * A_stst.ColStride() + A_stst.ColShift()) * matrix_size +
                   i * A_stst.RowStride() + A_stst.RowShift() + 1;
            A_stst.SetLocal(j, i, val);
        }
    }

    // and fill a CombBLAS sparse matrix (with the same data)
    FullyDistVec<size_t, double> cols(matrix_size * matrix_size, 0.0);
    FullyDistVec<size_t, double> rows(matrix_size * matrix_size, 0.0);
    FullyDistVec<size_t, double> vals(matrix_size * matrix_size, 0.0);

    for(size_t i = 0; i < matrix_size * matrix_size; ++i) {
        rows.SetElement(i, floor(i / matrix_size));
        cols.SetElement(i, i % matrix_size);
        vals.SetElement(i, static_cast<double>(i+1));
    }

    cbDistMatrixType B(matrix_size, matrix_size, rows, cols, vals);

    //std::vector<double> local_matrix;
    //skylark::base::detail::mixed_gemm_local_part_tt(-1.0, B, A_stst, 0.0,
            //local_matrix);
    //for(size_t idx = 0; idx < local_matrix.size(); idx++)
        //std::cout << local_matrix[idx] << std::endl;

    if(world.rank() == 0)
        std::cout << "Testing CombBLAS^T x Elemental (VX/*) = Elemental (*/*) :";

    elem::DistMatrix<double, elem::STAR, elem::STAR>
        result_stst(matrix_size, matrix_size, grid);
    for( size_t j = 0; j < result_stst.LocalHeight(); j++ )
        for( size_t i = 0; i < result_stst.LocalWidth(); i++ )
            result_stst.SetLocal(j, i, 1.0);

    DistMatrixVCSType A_vcs = A_stst;
    skylark::base::detail::outer_panel_mixed_gemm_impl_tn(
            -1.0, B, A_vcs, 1.5, result_stst);
    check_matrix(result_stst, tn_expected,
                 "Result of outer panel TN gemm not as expected");

    if(world.rank() == 0)
        std::cout << "outer panel: OK" << std::endl;

    if(world.rank() == 0)
        std::cout << "Testing CombBLAS x Elemental (*/*) = Elemental (VX/*) :";

    DistMatrixVCSType result_vcs(matrix_size, matrix_size, grid);
    for( size_t j = 0; j < result_vcs.LocalHeight(); j++ )
        for( size_t i = 0; i < result_vcs.LocalWidth(); i++ )
            result_vcs.SetLocal(j, i, 1.0);

    skylark::base::detail::outer_panel_mixed_gemm_impl_nn(
            -1.0, B, A_stst, 1.5, result_vcs);
    check_matrix(result_vcs, nn_expected,
                 "Result of outer panel NN gemm not as expected");

    if(world.rank() == 0)
        std::cout << "outer panel: OK" << std::endl;

    for( size_t j = 0; j < result_vcs.LocalHeight(); j++ )
        for( size_t i = 0; i < result_vcs.LocalWidth(); i++ )
            result_vcs.SetLocal(j, i, 1.0);

    skylark::base::detail::inner_panel_mixed_gemm_impl_nn(
            -1.0, B, A_stst, 1.5, result_vcs);
    check_matrix(result_vcs, nn_expected,
                 "Result of inner panel NN gemm not as expected");

    if(world.rank() == 0)
        std::cout << "inner panel: OK" << std::endl;

    elem::Finalize();

    return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "NoveltyFeature.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

class NoveltySegmentation
{

public:
  using ArrayXd = Eigen::ArrayXd;

  NoveltySegmentation(index maxKernelSize, index maxFilterSize)
      : mNovelty(maxKernelSize, maxFilterSize)
  {}

  void init(index kernelSize, index filterSize, index nDims)
  {
    mNovelty.init(kernelSize, filterSize, nDims);
    mDebounceCount = 1;
    mPeakBuffer.setZero();
  }

  double processFrame(const RealVectorView input, double threshold,
                      index minSliceLength)
  {
    double detected = 0.;

    mPeakBuffer.segment(0, 2) = mPeakBuffer.segment(1, 2);
    mPeakBuffer(2) = mNovelty.processFrame(input);

    if (mPeakBuffer(1) > mPeakBuffer(0) && mPeakBuffer(1) > mPeakBuffer(2) &&
        mPeakBuffer(1) > threshold && mDebounceCount == 0)
    {
      detected = 1.0;
      mDebounceCount = minSliceLength;
    }
    else
    {
      if (mDebounceCount > 0) mDebounceCount--;
    }
    return detected;
  }

private:
  NoveltyFeature mNovelty;
  ArrayXd      mPeakBuffer{3};
  index        mDebounceCount{1};
};
} // namespace algorithm
} // namespace fluid


#include "tkdCmdParser.h"

#include "nrFourier.h"

#include "vnl/vnl_matrix.h"
#include "vnl/vnl_vector.h"
#include "vnl/algo/vnl_determinant.h"
#include "vnl/algo/vnl_svd.h"
#include "vnl/algo/vnl_fft_1d.h"
#include "vnl/algo/vnl_cholesky.h"

#include "itkNumericTraits.h"

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

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

#include "Array.h"
#include "fftw++.h"

using namespace Array;
using namespace fftwpp;

/**
 * Option list.
 */
struct parameters
{
	std::string inputFileName;
	std::string separation;
	bool verbose;
};

/**
 * General Linear Model (GLM) with autocorrelation prewhitening.
 *
 * Refs:
 * 1. Book: 'Functional MRI an introduction to methods',
 * 		P. Jezzard, P.M. Matthews, S.M. Smith, Oxford (2005).
 *
 * 2. Article: 'Temporal autocorrelation in Univariate Modeling of FMRI Data',
 * 		M.W. Woolrich, B.D. Ripley, M. Brady and S.M. Smith, Neuroimage 14, 1370-1386 (2001).
 */
namespace general_linear_model
{
	typedef double PixelType;
	typedef vnl_matrix< PixelType > MatrixType;
	typedef vnl_vector< PixelType > VectorType;

	class GLM
	{
	public:

		/**
		 * Run glm.
		 */
		void Run( parameters& list )
		{
			// data Y
			VectorType Y( 8, 0 );
			Y( 0 ) = 0.1;
			Y( 1 ) = 3.5;
			Y( 2 ) = 6.7;
			Y( 3 ) = 6.4;
			Y( 4 ) = 3.5;
			Y( 5 ) = 3.7;
			Y( 6 ) = 3.1;
			Y( 7 ) = 2.1;

			// design matrix X
			MatrixType X( 8, 3 );
			X( 0, 0 ) = 1;
			X( 1, 0 ) = 1;
			X( 2, 0 ) = 1;
			X( 3, 0 ) = 1;
			X( 4, 0 ) = 1;
			X( 5, 0 ) = 1;
			X( 6, 0 ) = 1;
			X( 7, 0 ) = 1;

			X( 0, 1 ) = 0;
			X( 1, 1 ) = 0;
			X( 2, 1 ) = 1;
			X( 3, 1 ) = 1;
			X( 4, 1 ) = 0;
			X( 5, 1 ) = 0;
			X( 6, 1 ) = 0;
			X( 7, 1 ) = 0;

			X( 0, 2 ) = 0;
			X( 1, 2 ) = 0;
			X( 2, 2 ) = 0;
			X( 3, 2 ) = 0;
			X( 4, 2 ) = 1;
			X( 5, 2 ) = 1;
			X( 6, 2 ) = 0;
			X( 7, 2 ) = 0;

			// Toeplitz matrix S
			MatrixType S( Y.size(), Y.size() );
			S.set_identity();

			// Adjust for correlation using Cochrane–Orcutt estimation
			VectorType B = OLS( Y, X, S );

			std::cout << "Betas: " << B << std::endl;

			// Residuals
			VectorType r = Residuals( Y, X, B );

			std::cout << "Residuals: " << r << std::endl;

			// Raw autocorrelation (all lags)
			VectorType ac = AutoCorrelation( r );

			std::cout << "Raw autocorrelation: " << ac << std::endl;

			// non-parametric autocorrelation smoothing (M = 2sqrt(N))
			unsigned int M = 2. * std::sqrt( Y.size() );

			VectorType sac = TaperedCosineWindow( ac, M );

			std::cout << "Smoothed autocorrelation (" << M << ")" << ": " << sac << std::endl;

			// construct V
			MatrixType V = GetV( sac, true );

			vnl_svd< PixelType > svd( V );
			//svd.solve();
			MatrixType W = svd.W();
			MatrixType Vprime = W * W.transpose();

			for( unsigned int r = 0; r < V.rows(); r++ )
			{
				for( unsigned int c = 0; c < V.cols(); c++ )
				{
					std::cout << V( r, c ) << " ";
				}
				std::cout << std::endl;
			}

			std::cout << "UU'" << std::endl;

			for( unsigned int r = 0; r < Vprime.rows(); r++ )
			{
				for( unsigned int c = 0; c < Vprime.cols(); c++ )
				{
					std::cout << Vprime( r, c ) << " ";
				}
				std::cout << std::endl;
			}

			exit( 0 );

			// refit
			//B = OLS( Y, X, S );
		}

	protected:

		/**
		 * Return inverted Cholesky V=KK'
		 */
		MatrixType GetInvertedK( const MatrixType& V )
		{
			vnl_svd< PixelType > svd( V );
			MatrixType pinvV = svd.inverse();

		}

		/**
		 * Return V.
		 */
		MatrixType GetV( const VectorType& v, bool circular )
		{
			MatrixType result( v.size(), v.size(), 0.0 );

			for( unsigned int r = 0; r < result.rows(); r++ )
				for( unsigned int c = r; c < result.cols(); c++ )
					result( r, c ) = v( r );

			result += result.transpose();

			result.fill_diagonal( 1 );

			return result;
		}

		/**
		 * Tukey windowing.
		 *
		 * http://en.wikipedia.org/wiki/Window_function#Tukey_window
		 */
		VectorType TaperedCosineWindow( const VectorType& ar, unsigned int M )
		{
			VectorType rho( ar.size(), 0 );

			for ( unsigned int i = 0; i < ar.size(); i++ )
			{
				if ( i < M )
				{
					PixelType PI = boost::math::constants::pi< PixelType >();
					rho( i ) = 0.5 * ( 1 + std::cos( ( PI * i ) / M ) ) * ar( i );
				} else
				{
					rho( i ) = 0;
				}
			}

			return rho;
		}

		/**
		 * Cross correlation for all lags
		 * @param a Vector 1 (of length n)
		 * @param b Vector 2 (of length n)
		 * @param output Output vector of length n
		 *
		 * Normalize the sequence so the autocorrelations at zero lag are identically 1.0.
		 */
		VectorType AutoCorrelation( const VectorType& a )
		{
			const int n = a.size();

			// added zeropadding!
			int zeropad = (int) pow( 2, ceil( log( a.size() ) / log( 2 ) ) );
			VectorType output( zeropad, 0 );

			for ( int i = 0; i < n; ++i )
				output[i] = a[i];

			array1< PixelType > finput( a.size(), sizeof( Complex ) );
			array1< Complex > foutput( a.size(), sizeof( Complex ) );
			array1< Complex > ftemp( a.size(), sizeof( Complex ) );

			for( unsigned int i = 0; i < a.size(); i++ )
				finput[ i ] = a( i );

			rcfft1d Forward( a.size(), finput, foutput );

			Forward.fft( finput, foutput ); // fill foutput ...
			Forward.fft( finput, ftemp ); // fill ftemp ...

			const PixelType no2 = static_cast< PixelType > ( n >> 1 );

			for ( int i = 2; i < n; i += 2 )
			{
				Complex ftmp = foutput[i];
				foutput[i] = ( foutput[i] * ftemp[i] + foutput[i + 1] * ftemp[i + 1] ) / no2;
				foutput[i + 1] = ( foutput[i + 1] * ftemp[i] - ftmp * ftemp[i + 1] ) / no2;
			}

			foutput[0] = foutput[0] * ftemp[0] / no2;
			foutput[1] = foutput[1] * ftemp[1] / no2;

			crfft1d Backward( a.size(), foutput, finput );
			Backward.fftNormalized( foutput, finput );

			VectorType finalOutput( n );

			// recrop
			for ( int i = 0; i < n; ++i )
				finalOutput( i ) = std::abs( foutput( i ) / foutput( 0 ) ); // normalize

			return finalOutput;
		}

		/**
		 * Return standard deviation.
		 */
		template< class T >
		T GetSD( const vnl_vector< T >& distances )
		{
			int size = distances.size();

			if ( size < 2 )
			{
				return 0;
			}

			T mean = distances.mean();

			T sd = 0;
			for ( int i = 0; i < size; i++ )
			{
				sd += pow( distances[i] - mean, 2 );
			}

			return vcl_sqrt( sd / size );
		}

		/**
		 * Return residuals of fit.
		 */
		VectorType Residuals( const VectorType& Y, const MatrixType& X, const VectorType& B )
		{
			return Y - X * B;

			// TODO
			//VectorType r( B.size(), 0 );
			//return r;
		}

		/**
		 * Ordinary Least Squares ( OLS ) estimation, by finding the
		 * maximum likelihood estimates of a linear regression model.
		 *
		 * Ref: http://en.wikipedia.org/wiki/Ordinary_least_squares
		 */
		VectorType OLS( const VectorType& Y, const MatrixType& X, const MatrixType& S )
		{
			if ( Y.size() != X.rows() )
			{
				std::cerr << "*** ERROR ***: The data vector Y and rows of the "
					"design matrix X must have the same length." << std::endl;
				exit( EXIT_FAILURE );
			}
			if ( X.rows() != S.rows() )
			{
				std::cerr << "*** ERROR ***: The Toeplitz matrix S and "
					"design matrix X must have the same number of rows." << std::endl;
				exit( EXIT_FAILURE );
			}

			// SY = SXB + mu, where mu ~ N( 0, sigma^2 SVS') => Bhat = pinv(SX)SY,
			// where pinv(SX) is the Moore-Penrose pseudoinverse: ((SX)'SX)^-1(SX)'
			vnl_svd< PixelType > svd( ( S * X ).transpose() * S * X );
			MatrixType pinvSX = svd.inverse() * ( S * X ).transpose();

			return pinvSX * S * Y;
		}
	};
} // end namespace general_linear_model

/**
 * Main.
 */
int main( int argc, char ** argv )
{
	tkd::CmdParser p( argv[0], "Generalized linear model" );

	parameters list;
	list.verbose = false;

	p.AddArgument( list.inputFileName, "input" ) ->AddAlias( "i" ) ->SetInput( "filename" ) ->SetDescription(
			"Input file: list of covariate(s) and response function (column format)" ) ->SetRequired( true );

	p.AddArgument( list.separation, "separation" ) ->AddAlias( "s" ) ->SetInput( "string" ) ->SetDescription(
			"Data separator string (default: \\t" );

	p.AddArgument( list.verbose, "verbose" ) ->AddAlias( "v" ) ->SetInput( "bool" ) ->SetDescription( "Verbose (default: false" );

	if ( !p.Parse( argc, argv ) )
	{
		p.PrintUsage( std::cout );
		return EXIT_FAILURE;
	}

	general_linear_model::GLM glm;

	glm.Run( list );

	return EXIT_SUCCESS;
}


#include <iostream>
#include <cmath>
#include <ctime>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <iomanip>
#include <vector>

#include "mystuff.h"

using namespace mosya;

#ifdef ZAPPY_ZAPPY
#include "RandomNormalPair.cpp"
#include "AtoG.cpp"
//#include "GtoA.cpp"
//#include "ProjectPointsOntoEllipse.cpp"
//#include "ProjectPointsOntoHyperbola.cpp"
//#include "ProjectPointsOntoParabola.cpp"
//#include "ProjectPointsOntoConicByEberlyOrig.cpp"
#include "eigen2x2.cpp"
#include "ProjectPointsOntoConicByEberlyModified.cpp"
#include "AdjustProjectedPointsOnConic.cpp"
#include "DistanceToConicApprx.cpp"
#include "ProjectionQuality.cpp"
#include "IsCrossingWindow.cpp"
#include "RootOfCubicEquation.cpp"
//#include "ProjectPointsOntoConicByWEPqz.cpp"
//#include "ProjectPointsOntoConicByWEPeq3Schur.cpp"
#include "ProjectPointsOntoConicByWEPeq3NR.cpp"
//#include "ProjectPointsOntoConicByWEPinv.cpp"
//#include "ProjectPointsOntoConicByQuarticEquation.cpp"
#include "ProjectPointsOntoConicByNewtonAK.cpp"
#endif

//  Main program for testing projection methods
//  Specify the methods by selecting which functions the program calls
//  Choose double or long double precision in file "mystuff.h"

int main()
{
    const int n=2,histCols=18,histRows=6,timing=1;

    integers i,j,k,N=0,N0=0,N1=0,N2=0,N3=0,Nmiss=0;
    int code,hist1[histRows][histCols]={0}, hist2[histRows][histCols]={0};

    const reals Window=One,Emin=pow(10.,-histCols-1);

    reals r1,r2;
    reals E1[histRows][n], E2[histRows][n];
    double Times[histRows]={0.};

    M6x1 A;
    M5x1 ParG;
    Mnx1 X(n,1),Y(n,1),D(n,1),Dmin(n,1),Q(n,1);
    Mnx1 Xproj(n,1),Yproj(n,1),XprojA(n,1),YprojA(n,1),Xbest(n,1),Ybest(n,1);
    Mnxm XprojAll(n,histRows),YprojAll(n,histRows);

    vector<reals> Xv(n),Yv(n);

    clock_t iniTime, finTime;        // auxiliary variables for timing
    timespec ts1, ts2;

    srand ( (unsigned)time(NULL) );  // seed the random generator
    cout.precision(15);

    while(1)   //  main loop over random conics and points
	{
            //  generate a conic:

            RandomNormalPair(r1,r2);  A(0) = r1;  A(1) = r2;
            RandomNormalPair(r1,r2);  A(2) = r1;  A(3) = r2;
            RandomNormalPair(r1,r2);  A(4) = r1;  A(5) = r2;

            reals eps=1.e-13; RandomNormalPair(r1,r2);  A(2) = A(1)*A(1)/A(0) + eps*r1;  // make it a parabola (if desired)

            A.normalize();      //  normalization of the parameter vector (unnecessary)

            //  discard the conic if it is of the wrong type:

            if (IsCrossingWindow(A,Window)==0)  {  Nmiss++; continue;  }  //  discard conics that do not cross the data window

            AtoG(A,ParG,code);                       //   determine the conic type
            if (code>3) { N0++;  continue; }         //   discard degenerate conics

            if (code==1) N1++;   //  count ellipses
            if (code==2) N2++;   //  count hyperbolas
            if (code==3) N3++;   //  count parabolas
            N++;                 //  count all conics together

            //   generate n data points:

            for (i=0; i<n; i++)
                {
                    Xv[i] = Window*(Two*rand()/RAND_MAX - One);
                    Yv[i] = Window*(Two*rand()/RAND_MAX - One);
                    X(i) = Xv[i];
                    Y(i) = Yv[i];
                    Dmin(i) = REAL_MAX;
                }

            //   First projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            ProjectPointsOntoConicByWEPeq3NR(X,Y,A,Xproj,Yproj);  //  choose a projection method here
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[0] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[0][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(0) = Xproj;
            YprojAll.col(0) = Yproj;

            //   Second projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            AdjustProjectedPointsOnConic(X,Y,A,Xproj,Yproj,XprojA,YprojA,2);  //  choose a projection method here
            Xproj = XprojA;  Yproj = YprojA;
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[1] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[1][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(1) = Xproj;
            YprojAll.col(1) = Yproj;

            //   Third projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            ProjectPointsOntoConicByNewtonAK(X,Y,A,Xproj,Yproj);  //  choose a projection method here
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[2] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[2][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(2) = Xproj;
            YprojAll.col(2) = Yproj;

            //   Fourth projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            AdjustProjectedPointsOnConic(X,Y,A,Xproj,Yproj,XprojA,YprojA,2);  //  choose a projection method here
            Xproj = XprojA;  Yproj = YprojA;
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[3] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[3][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(3) = Xproj;
            YprojAll.col(3) = Yproj;

            //   Fifth projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            ProjectPointsOntoConicByEberlyModified(X,Y,A,Xproj,Yproj);  //  choose a projection method here
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[4] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[4][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(4) = Xproj;
            YprojAll.col(4) = Yproj;

            //   Sixth projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            AdjustProjectedPointsOnConic(X,Y,A,Xproj,Yproj,XprojA,YprojA,2);  //  choose a projection method here
            Xproj = XprojA;  Yproj = YprojA;
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[5] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            //if (timing) {cout << endl; cin.ignore();}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[5][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(5) = Xproj;
            YprojAll.col(5) = Yproj;

            //   Record the results of all projection methods

            for (i=0; i<n; i++)
                {
                    for (j=0; j<histRows; j++)   {  E1[j][i] = sqrt(SQR(XprojAll(i,j)-Xbest(i)) + SQR(YprojAll(i,j)-Ybest(i)));  }

                    for (j=0; j<histRows; j++)   {  if (E1[j][i] < Emin) E1[j][i] = Emin;  }
                    for (j=0; j<histRows; j++)   {  if (E2[j][i] < Emin) E2[j][i] = Emin;  }
                }

            for (i=0; i<n; i++)
                {
                    for (j=0; j<histRows; j++)
                        {
                            k = floor(-log10(E1[j][i]));
                            if (k<1) k=1;
                            if (k<=histCols) hist1[j][k-1]++;

                            k = floor(-log10(E2[j][i]));
                            if (k<1) k=1;
                            if (k<=histCols) hist2[j][k-1]++;
                        }
                }

            if (((N*n)%2000000)==0)
                {
                    for (j=0; j<histRows; j++)
                        {
                            for (i=0;i<histCols;i++)  cout << setw(6) << hist1[j][i];
                            cout << endl;
                        }
                    cout << endl;

                    for (j=0; j<histRows; j++)
                        {
                            for (i=0;i<histCols;i++)  cout << setw(6) << hist2[j][i];
                            cout << endl;
                        }
                    cout << endl;

                    if (timing)
                        {
                            cout << " Runnig times per conic (in milisec): ";
                            for (j=0; j<histRows; j++) cout << "  " << setprecision(5) << Times[j]/N*1000000.;
                            cout << endl << endl;;
                        }

                    cout << " conics used:  N = " << N << "  (Ell: " << N1 << "  Hyp: " << N2 << "  Par: " << N3 << ") " << endl;
                    cout << " conics removed:  " << N0 << " degenetare and " << Nmiss << " missing the window" << endl << endl;
                }
            //if ((N*n)>=1000000000) return 0;
        }
}


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

// Written by Simon Praetorius

#ifndef ITL_FGMRES_INCLUDE
#define ITL_FGMRES_INCLUDE

#include <algorithm>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/multi_vector.hpp>
#include <boost/numeric/mtl/operation/givens.hpp>
#include <boost/numeric/mtl/operation/two_norm.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>

#include "details.hpp"

namespace itl
{

  /// Flexible Generalized Minimal Residual method (without restart)
  /// Cite: Youcef Saad, A Flexible Inner-Outer Preconditioned GMRES Algorithm, 1993
  /** It computes at most kmax_in iterations (or size(x) depending on what is smaller)
      regardless on whether the termination criterion is reached or not.   **/
  template <typename Matrix, typename Vector, typename Preconditioner, typename Iteration>
  int fgmres_full(const Matrix& A, Vector& x, const Vector& b,
                  Preconditioner& P, Iteration& iter)
  {
    using mtl::irange;
    using mtl::iall;
    using std::abs;
    using std::sqrt;
    using math::reciprocal;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    typedef typename mtl::Collection<Vector>::size_type  Size;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");

    const Scalar                zero= math::zero(Scalar()), breakdown_tol= 1.e-16, kappa = 10.0;
    Scalar                      rho, bnrm2, temp, hr;
    Size                        k, kmax(std::min(size(x), Size(iter.max_iterations() - iter.iterations())));
    Vector                      w(resource(x)), r(b - A*x);
    mtl::matrix::multi_vector<Vector>   V(Vector(resource(x), zero), kmax+1);
    mtl::matrix::multi_vector<Vector>   Z(Vector(resource(x), zero), kmax+1);
    mtl::matrix::dense2D<Scalar>        H(kmax+1, kmax);

    mtl::vector::dense_vector<Scalar>   sn(kmax, zero), cs(kmax, zero), s(kmax+1, zero), y(kmax, zero);  // replicated in distributed solvers

    bnrm2 = two_norm(b);
    if (bnrm2 == zero)
    {
      set_to_zero(x);
      // b == 0 => solution = 0
      return iter.terminate(bnrm2);
    }

    rho = two_norm(r);				// norm of preconditioned residual
    if (iter.finished(rho))			// initial guess is good enough solution
      return iter;

    V.vector(0) = r * reciprocal(rho);
    H = zero;
    s[0] = rho;

    // FGMRES iteration
    for (k= 0; k < kmax && !iter.finished(rho) ; ++k, ++iter)
    {
      Z.vector(k) = solve(P, V.vector(k));
      w = A * Z.vector(k);
      temp = two_norm(w);

      for (Size j= 0; j < k+1; j++)
      {
        H[j][k] = dot(V.vector(j), w);
        w -= H[j][k] * V.vector(j);
      }
      H[k+1][k]= two_norm(w);

      // reorthogonalization, only if "heuristic" condition is fulfilled
      if (H[k+1][k] < temp * reciprocal(kappa))
      {
        for (Size i= 0; i < k+1; i++)
        {
          hr = dot(w, V.vector(i));
          H[i][k] += hr;
          w -= hr * V.vector(i);
        }
        temp = two_norm(w);

        if (temp < H[k+1][k] * reciprocal(kappa))
        {
          set_to_zero(w);
          H[k+1][k] = 0.0;
        }
        else
        {
          H[k+1][k] = temp;
        }
      }

      if (H[k+1][k] < breakdown_tol)
        return iter.fail(2, "FGMRES: Singular matrix - nearly hard breakdown");

      V.vector(k+1) = w * reciprocal(H[k+1][k]);

      // k Given's rotations
      for(Size i= 0; i < k; i++)
      {
        temp =        cs[i]*H[i][k] + sn[i]*H[i+1][k];
        H[i+1][k] = - sn[i]*H[i][k] + cs[i]*H[i+1][k];
        H[i][k] = temp;
      }

      details::rotmat(H[k][k], H[k+1][k], cs[k], sn[k]);

      s[k+1] = -sn[k]*s[k];
      s[k]   =  cs[k]*s[k];
      H[k][k] = cs[k]*H[k][k] + sn[k]*H[k+1][k];
      H[k+1][k] = 0.0;

      rho = std::abs(s[k+1]);
    }

    // reduce k, to get regular matrix
    //     while (k > 0 && abs(s[k-1]) <= iter.atol()) k--;

    // iteration is finished -> compute x: solve H*y=g as far as rank of H allows
    irange range(k);
    for (; !range.empty(); --range)
    {
      try
      {
        y[range] = upper_trisolve(H[range][range], s[range]);
      }
      catch (mtl::matrix_singular)
      {
        continue;    // if singular then try with sub-matrix
      }
      break;
    }

    if (range.finish() < k)
      std::cerr << "GMRES orhogonalized with " << k << " vectors but matrix singular, can only use "
                << range.finish() << " vectors!\n";
    if (range.empty())
      return iter.fail(3, "GMRES did not find any direction to correct x");

    x += Z.vector(range) * y[range];

    //     r = b - A*x;
    return iter.terminate(rho);
  }

  /// Flexible Generalized Minimal Residual method with restart
  template <typename Matrix, typename Vector, typename LeftPreconditioner,
            typename RightPreconditioner, typename Iteration>
  int fgmres(const Matrix& A, Vector& x, const Vector& b,
             LeftPreconditioner& /*L*/, RightPreconditioner& R,
             Iteration& iter, typename mtl::Collection<Vector>::size_type restart)
  {
    do
    {
      Iteration inner(iter);
      inner.set_max_iterations(std::min(int(iter.iterations()+restart), iter.max_iterations()));
      inner.suppress_resume(true);
      fgmres_full(A, x, b, R, inner);
      iter.update_progress(inner);
    }
    while (!iter.finished());

    return iter;
  }

} // namespace itl

#endif // ITL_FGMRES_INCLUDE


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


// Copyright (C) 2017 Vicente J. Botet Escriba
//
//  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)

// <experimental/chrono.hpp>

#include <experimental/chrono.hpp>

namespace stdex = std::experimental;

// Basic frame durations
using frames       = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<10>,  std::chrono::milliseconds::period>::type>;
using subframes    = std::chrono::duration<int32_t,  std::ratio_divide<frames::period,    std::ratio<10>>::type>;
using slots        = std::chrono::duration<int32_t,  std::ratio_divide<subframes::period, std::ratio<2>>::type>;
using symbols      = std::chrono::duration<int32_t,  std::ratio_divide<slots::period,     std::ratio<7>>::type>;

using bi_frames     = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<2>,    frames::period>::type>;
using x_frames     = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<1024>, frames::period>::type>;
using h_frames     = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<1024>, x_frames::period>::type>;
using bi_slots      = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<2>,    slots::period>::type>;

// relative numbers
using h_frame_number    = stdex::chrono::modulo<x_frames,  h_frames,  uint16_t>; //1024
using frame_number      = stdex::chrono::modulo<frames,    x_frames,  uint16_t>; //1024
using x_frame_number    = frame_number;

using hx_frame_number    = stdex::chrono::modulo<frames,  h_frames,  uint16_t>; //1024*1024

using subframe_number   = stdex::chrono::modulo<subframes, frames,     uint8_t>; //10
using slot_number       = stdex::chrono::modulo<slots,     subframes,  uint8_t>; //2
using symbol_number     = stdex::chrono::modulo<symbols,   slots,      uint8_t>; //7
using bi_frame_number    = stdex::chrono::modulo<frames,    bi_frames,   uint8_t>; //2

using h_subframe_number = stdex::chrono::modulo<subframes, h_frames, uint32_t>; // 10*1240*1024
using x_subframe_number = stdex::chrono::modulo<subframes, x_frames, uint16_t>; // 10*1240
using bi_subframe_number = stdex::chrono::modulo<subframes, bi_frames, uint8_t>; // 10*2
using bi_symbol_number = stdex::chrono::modulo<symbols, bi_slots, uint8_t>;      // 7*2

inline constexpr h_subframe_number make_h_subframe_number(x_frame_number xfn, frame_number fn, subframe_number sfn) {
       return h_subframe_number(xfn.to_duration() + fn.to_duration() + sfn.to_duration()) ;
}

inline constexpr x_subframe_number make_x_subframe_number(frame_number fn, subframe_number sfn) {
       return x_subframe_number(fn.to_duration() + sfn.to_duration()) ;
}

inline constexpr bi_subframe_number make_bi_subframe_number(bi_frame_number fn, subframe_number sfn) {
       return bi_subframe_number(fn.to_duration() + sfn.to_duration()) ;
}

inline constexpr bi_symbol_number make_bi_subframe_number(slot_number sn, symbol_number syn) {
       return bi_symbol_number(sn.to_duration() + syn.to_duration()) ;
}

template <class SubDuration, class Duration, class Rep>
constexpr x_frame_number to_x_frame_number(stdex::chrono::modulo<SubDuration, Duration, Rep> m)
{
  return stdex::chrono::modulo_cast<x_frame_number, h_frames>(m);

}

template <class SubDuration, class Duration, class Rep>
constexpr frame_number to_frame_number(stdex::chrono::modulo<SubDuration, Duration, Rep> m)
{
    return stdex::chrono::modulo_cast<frame_number, h_frames>(m);
}

template <class SubDuration, class Duration, class Rep>
constexpr subframe_number to_subframe_number(stdex::chrono::modulo<SubDuration, Duration, Rep> m)
{
  return stdex::chrono::modulo_cast<subframe_number, h_frames>(m);
}

template <class SubDuration, class Duration, class Rep>
constexpr slot_number to_slot_number(stdex::chrono::modulo<SubDuration, Duration, Rep> m)
{
  return stdex::chrono::modulo_cast<slot_number, h_frames>(m);
}

template <class SubDuration, class Duration, class Rep>
constexpr symbol_number to_symbol_number(stdex::chrono::modulo<SubDuration, Duration, Rep> m)
{
  return stdex::chrono::modulo_cast<symbol_number, h_frames>(m);
}

// what about defining x_subframe_tuple using tuples as underlying type.
// This will allow to define it as a strong type inheriting from all the relational operators

// x_subframe_tuple =  tuple<frame_number, subframe_number>
// x_subframe_tuple =  tuple<modulo<x_frames, frames>, modulo<frames, subframes>>
// todo: define a tuple_modulo where
// tuple_modulo<T1, T2, T3> = tuple<modulo<T1, T2>, modulo<T2, T3>>
//template <class T1, T2, T3>
//using tuple_modulo = std::tuple<modulo<T1, T2>, modulo<T2, T3>>;
//  convertible to T3 and modulo<T1, T3>
//  access to modulo<T1, T2> and modulo<T2, T3>
// tuple_modulo<T1, T2, T3, T4> = tuple<modulo<T1, T2>, modulo<T2, T3>, modulo<T3, T4>>
// x_subframe_tuple must define the same operations than x_subframe_number
struct x_subframe_tuple {
private:
    frame_number fn;
    subframe_number sfn;
public:

#if defined __clang__
    // gcc doesn't accepts this
    constexpr
#endif
    x_subframe_tuple() = default;

    /// pre-condition: fn and sfn are valid
    constexpr x_subframe_tuple(frame_number fn, subframe_number sfn) : fn(fn), sfn(sfn) {}
    /// pre-condition: xsfn is valid
    constexpr x_subframe_tuple(x_subframe_number xsfn) : fn(to_frame_number(xsfn)), sfn(to_subframe_number(xsfn)) {}

    /// pre-condition: fn  isvalid
    constexpr frame_number frame() const {return fn;}
    /// pre-condition: sfn is valid
    constexpr subframe_number subframe() const {return sfn;}
    /// pre-condition: fn and sfn are valid

    constexpr subframes get_duration() const { return frames(fn) + subframes(sfn); }
    constexpr x_subframe_number get_modulo() const { return x_subframe_number(get_duration()); }

    constexpr operator subframes() const { return get_duration(); }
    constexpr x_subframe_number x_subframe() const { return get_modulo(); }
    constexpr operator x_subframe_number() const { return get_modulo(); }

    // pre-condition 0 <= xsfn.frame() + df < 1024
    friend constexpr x_subframe_tuple operator+(x_subframe_tuple const& xsfn, const frames& df) noexcept
    {
        return { xsfn.fn + df, xsfn.sfn };
    }

    // pre-condition 0 <= xsfn.frame() - df < 1024
    friend constexpr x_subframe_tuple operator-(x_subframe_tuple const& xsfn, const frames& df) noexcept
    {
        return { xsfn.fn - df, xsfn.sfn };
    }
    // pre-condition 0 <= xsfn.frame() - df < 1024
    friend constexpr x_subframe_tuple operator+(const frames& df, x_subframe_tuple const& xsfn) noexcept
    {
        return xsfn + df;
    }

    friend constexpr subframes operator-(x_subframe_tuple const& x, const x_subframe_tuple& y) noexcept
    {
        return subframes(x) - subframes(y);
    }

    // pre-condition 0 <= xsfn.subframes() + dsf < 1024*10
    friend JASEL_CXX14_CONSTEXPR x_subframe_tuple operator+(x_subframe_tuple const& xsfn, const subframes& dsf) noexcept
    {
        const int cardinal = subframe_number::cardinal;
        auto dsfni = xsfn.sfn.count() + dsf.count();
        auto dfni = ((dsfni >= 0)  ? dsfni : dsfni-cardinal+1 ) / cardinal;
        dsfni = dsfni - dfni * cardinal;
        return x_subframe_tuple( xsfn.fn + frames(dfni), subframe_number(subframes(dsfni)) );
    }
    // pre-condition 0 <= xsfn.subframes() + dsf < 1024*10
    friend JASEL_CXX14_CONSTEXPR x_subframe_tuple operator+(const subframes& dsf, x_subframe_tuple const& xsfn) noexcept
    {
        return xsfn + dsf;
    }
    // pre-condition 0 <= xsfn.subframes() - dsf < 1024*10
    friend JASEL_CXX14_CONSTEXPR x_subframe_tuple operator-(x_subframe_tuple const& xsfn, const subframes& dsf) noexcept
    {
        return xsfn + -dsf;
    }

    // pre-condition 0 <= this->subframes() + df < 1024*10
    JASEL_CXX14_CONSTEXPR x_subframe_tuple& operator+=(const frames& df) noexcept
    {
        *this = *this + df;
        return *this;
    }
    // pre-condition 0 <= this->subframes() - df < 1024*10
    JASEL_CXX14_CONSTEXPR x_subframe_tuple& operator-=(const frames& df) noexcept
    {
        *this = *this - df;
        return *this;
    }

    // pre-condition 0 <= this->subframes() +dsf < 1024*10
    JASEL_CXX14_CONSTEXPR x_subframe_tuple& operator+=(const subframes& dsf) noexcept
    {
        *this = *this + dsf;
        return *this;
    }
    // pre-condition 0 <= this->subframes() + subframe(1) < 1024*10
    JASEL_CXX14_CONSTEXPR x_subframe_tuple& operator++() noexcept
    {
        *this = *this + subframes(1);
        return *this;
    }
    // pre-condition 0 <= this->subframes() + subframe(1) < 1024*10
    JASEL_CXX14_CONSTEXPR x_subframe_tuple operator++(int) noexcept
    {
        x_subframe_tuple tmp (*this);
        ++(*this);
        return tmp;
    }
    // pre-condition 0 <= this->subframes() - dsf < 1024*10
    JASEL_CXX14_CONSTEXPR x_subframe_tuple& operator-=(const subframes& dsf) noexcept
    {
        *this = *this - dsf;
        return *this;
    }
    // pre-condition 0 <= this->subframes() - subframe(1) < 1024*10
    JASEL_CXX14_CONSTEXPR x_subframe_tuple& operator--() noexcept
    {
        *this = *this - subframes(1);
        return *this;
    }
    // pre-condition 0 <= this->subframes() - subframe(1) < 1024*10
    JASEL_CXX14_CONSTEXPR x_subframe_tuple operator--(int) noexcept
    {
        x_subframe_tuple tmp (*this);
        --(*this);
        return tmp;
    }
};

// operator/ play the role of a factory as it does on Date library.
// Note that there is no sense to divide a frame_number and subframe_number, we need to first convert to duration
x_subframe_tuple operator/(frame_number fn, subframe_number sfn)
{
    return {fn, sfn};
}

// Should the following be members?
// No because for modulo types it is a non-member function
inline constexpr h_frame_number to_h_frame_number(x_subframe_tuple )
{
    return h_frame_number(0);
}
inline constexpr x_subframe_number to_x_subframe_number(x_subframe_tuple xsfn)
{
    //return xsfn.x_subframe();
    return make_x_subframe_number(xsfn.frame(), xsfn.subframe());
}
inline constexpr frame_number to_frame_number(x_subframe_tuple xsfn)
{
    return xsfn.frame();
}
inline constexpr subframe_number to_subframe_number(x_subframe_tuple xsfn)
{
    return xsfn.subframe();
}
inline constexpr slot_number to_slot_number(x_subframe_tuple )
{
    return slot_number(0);
}
inline constexpr symbol_number to_symbol_number(x_subframe_tuple )
{
    return symbol_number(0);
}

struct h_subframe_tuple {
    x_frame_number xfn;
    frame_number fn;
    subframe_number sfn;
    h_subframe_tuple() = default;
    constexpr h_subframe_tuple(x_frame_number xfn, frame_number fn, subframe_number sfn) : xfn(xfn), fn(fn) , sfn(sfn) {}
    constexpr operator h_subframe_number() const { return make_h_subframe_number(xfn, fn, sfn); }
};

inline constexpr h_subframe_number to_h_frame_number(h_subframe_tuple hsfn)
{
    return h_subframe_number(hsfn);
}
inline constexpr x_frame_number to_x_frame_number(h_subframe_tuple hsfn)
{
    return hsfn.xfn;
}
inline constexpr frame_number to_frame_number(h_subframe_tuple hsfn)
{
    return hsfn.fn;
}
inline constexpr subframe_number to_subframe_number(h_subframe_tuple hsfn)
{
    return hsfn.sfn;
}
inline constexpr slot_number to_slot_number(h_subframe_tuple )
{
    return slot_number(0);
}
inline constexpr symbol_number to_symbol_number(h_subframe_tuple )
{
    return symbol_number(0);
}

static_assert(frame_number::cardinal  == 1024, "1024 frames are not a x_frame");
static_assert(subframe_number::cardinal  == 10, "10 sub frames are not a frame");
static_assert(bi_subframe_number::cardinal  == 20, "20 sub frames are not a bi_frame");

static_assert(slots::period::num  == 1, "");
static_assert(slots::period::den  == 2000, "");
static_assert(symbols::period::num  == 1, "");
static_assert(symbols::period::den  == 7*2000, "");

//! Auxiliary functions
inline constexpr bool parity(bi_subframe_number x) {
  return to_frame_number(x) == frame_number{0};
}

// todo Add literals
// h_frames -> _hfs
// frames -> _xfs
// subframes -> _sfs

// frame_number -> _fn
// subframe_number -> _sfn
// slot_number -> _sn
// symbol_number -> _syn

#include <boost/detail/lightweight_test.hpp>
#include <iostream>

int main()
{
  {
    constexpr   x_subframe_number xsfn =  make_x_subframe_number(frame_number{5}, subframe_number{3});
    static_assert(xsfn.count()  == 53, "");

  }
  {
  subframes num_of_sfs{4};
  num_of_sfs++;
  frames num_of_fs{2};
  num_of_fs++;
  auto x = num_of_fs + num_of_sfs;
  std::cout << "sfn = " << x.count() << "\n";
  {
      x_subframe_number xsfn {x};
      std::cout << "xsfn = " << xsfn << "\n";
      slot_number slotn =  to_slot_number(xsfn);
      std::cout << "slotn = " << slotn << "\n";
      subframe_number sfn =  to_subframe_number(xsfn);
      std::cout << "sfn = " << sfn << "\n";
      frame_number fn =  to_frame_number(xsfn);
      std::cout << "fn = " << fn << "\n";
      x_frame_number xfn =  to_x_frame_number(xsfn);
      std::cout << "xfn = " << xfn << "\n";

      {
          x_subframe_number xsfn2 { frames(fn) + subframes(sfn) };
          std::cout << "xsfn2 = " << xsfn2 << "\n";
      }
      {
          auto xsfn2 = make_x_subframe_number( fn, sfn );
          std::cout << "xsfn2 = " << xsfn2 << "\n";
      }
      {
          x_subframe_number xsfn2 = make_x_subframe_number( fn, sfn );
          std::cout << "xsfn2 = " << xsfn2 << "\n";
      }
      {
          auto xsfn2 = fn.to_duration() + sfn.to_duration();
          std::cout << "xsfn2 = " << xsfn2.count() << "\n";
      }

  }

  subframe_number sfn (num_of_sfs) ;
  frame_number fn (num_of_fs) ;
  bi_frame_number bfn (num_of_fs) ;
  bi_subframe_number bsfn = make_bi_subframe_number(bfn,sfn) ;
  std::cout << "bsfn = " << bsfn << "\n";

  std::cout << "==================\n";

  x_subframe_number  x2 = make_x_subframe_number( fn, sfn );
  std::cout << "xsfn2= " << x2 << "\n";
  bi_subframe_number bsfn2 = stdex::chrono::modulo_cast<bi_subframe_number, h_frames>(x2) ;
      std::cout << "bsfn2= " << bsfn2 << "\n";

  bi_frame_number bfn2 = stdex::chrono::modulo_cast<bi_frame_number, h_frames>(x2) ;
  std::cout << "bfn2= " << bfn2 << "\n";

  x_subframe_number x3 = stdex::chrono::modulo_cast<x_subframe_number, h_frames>(bfn2) ;
  std::cout << "x3= " << x3 << "\n";

  x_subframe_number x4 = stdex::chrono::modulo_cast<x_subframe_number, h_frames>(bsfn2) ;
  std::cout << "x4= " << x4 << "\n";
  }
  {
      frame_number fn {1};
      subframe_number sfn {2};
      x_subframe_tuple xsfn = fn/sfn;
      BOOST_TEST(xsfn.frame().count()==1);
      BOOST_TEST(xsfn.subframe().count()==2);
      BOOST_TEST(xsfn.x_subframe().count()==12);

      BOOST_TEST(to_frame_number(xsfn).count()==1);
      BOOST_TEST(to_subframe_number(xsfn).count()==2);
      BOOST_TEST(to_x_subframe_number(xsfn).count()==12);

      xsfn += frames(3);
      BOOST_TEST(xsfn.x_subframe().count()==42);
      BOOST_TEST(xsfn.frame().count()==4);

      xsfn += subframes(23);
      std::cout << "xsfn.x_subframe()= " << xsfn.x_subframe() << "\n";
      BOOST_TEST(xsfn.x_subframe().count()==65);

      ++xsfn;
      BOOST_TEST(xsfn.x_subframe().count()==66);

      --xsfn;
      BOOST_TEST(xsfn.x_subframe().count()==65);

      auto x1 = xsfn++;
      BOOST_TEST(x1.x_subframe().count()==65);
      BOOST_TEST(xsfn.x_subframe().count()==66);

      auto x2 = xsfn--;
      BOOST_TEST(x2.x_subframe().count()==66);
      BOOST_TEST(xsfn.x_subframe().count()==65);

  }
  {
      frame_number fn {1};
      subframe_number sfn {2};
      x_subframe_tuple xsfn = fn/sfn;
      auto x = xsfn - xsfn;
      BOOST_TEST(x.count()==0);

  }

  return ::boost::report_errors();
}


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
/**
 * @file basis.hpp
 * @author Boston Cleek
 * @date 20 Nov 2020
 * @brief Fourier cosine basis
 */
#ifndef BASIS_HPP
#define BASIS_HPP

#include <armadillo>

namespace ergodic_exploration
{
using arma::imat;
using arma::mat;
using arma::vec;

/**
 * @brief Fourier cosine basis */
class Basis
{
private:
  template <class ModelT>
  friend class ErgodicControl;

public:
  /**
   * @brief Constructor
   * @param lx - x-axis lenght of domain
   * @param ly - y-axis lenght of domain
   * @param num_basis - number of each basis functions per dimension
   */
  Basis(double lx, double ly, unsigned int num_basis);

  /**
   * @brief Compose cosine basis functions given positon
   * @param x - robot position [x y]
   * @return cosine basis functions (num_basis^2 x 1)
   * @details x must be on domain [0 lx] x [0 ly]
   */
  vec fourierBasis(const vec& x) const;

  /**
   * @brief Compose gradient cosine basis functions
   * @param x - robot position [x y]
   * @return gradient of each basis function (2 x num_basis^2)
   * @details x must be on domain [0 lx] x [0 ly]
   */
  mat gradFourierBasis(const vec& x) const;

  /**
   * @brief Compose trajectory fourier coefficients
   * @param xt - trajectory
   * @return trajectory fourier coefficients (num_basis^2 x 1)
   * @details xt must be on domain [0 lx] x [0 ly] and robot heading is not required
   */
  vec trajCoeff(const mat& xt) const;

  /**
   * @brief Compose spatial fourier coefficients
   * @param phi_vals - target evaluated at each grid cell in phi_grid
   * @param phi_grid - discretization of fourier domain
   * @return spatial fourier coefficients (num_basis^2 x 1)
   * @details phi_grid is not the occupancy grid, the first row contains the
   * x-cordinates and the second row contains the corresponding y-coordinates.
   * The index of the elements in phi_vals must correspond to the columns in phi_grid.
   */
  vec spatialCoeff(const vec& phi_vals, const mat& phi_grid) const;

private:
  double lx_, ly_;            // length of domain
  unsigned int total_basis_;  // number of basis functions
  vec lamdak_;                // frequency coefficients weights
  imat k_;                    // basis number
};
}  // namespace ergodic_exploration
#endif


#ifndef __ALGORITHM_BASE__
#define __ALGORITHM_BASE__
#include <Eigen/Dense>
#include <ros/ros.h>
#include <math.h>
#include <stdexcept>

namespace manipulation_algorithms{
  /**
    Class that defines an algorithm base.
  **/
  class AlgorithmBase
  {
  public:
    AlgorithmBase();
    virtual ~AlgorithmBase();

    /**
      Obtain the parameters relevant to the controller from the parameter server.

      @param n The nodehandle that will be used to query the parameter server
      @return False in case of error
    **/
    virtual bool getParams(const ros::NodeHandle &n) = 0;

  protected:
    /**
      Initialize a nxb matrix from values obtained from the ros parameter
      server.

      @param M The matrix to be initialized
      @param configName The parameter server location
      @param n The ros nodehandle used to query the parameter server

      @return True for success, False otherwise
    **/
    virtual bool parseMatrixData(Eigen::MatrixXd &M, const std::string configName, const ros::NodeHandle &n);
    virtual bool parseMatrixData(Eigen::Matrix3d &M, const std::string configName, const ros::NodeHandle &n);

    /**
      Fill in a nxn matrix with the given values.

      @param M The matrix to be filled in. Will be set to the size nxn.
      @param vals A vector with the values to fill in
    **/
    virtual void initializeEigenMatrix(Eigen::MatrixXd &M, const std::vector<double> vals);
    virtual void initializeEigenMatrix(Eigen::Matrix3d &M, const std::vector<double> vals);

    /**
      Saturates a control output

      @param original The computed output
      @param max The maximum allowed absolute value for the computed output
      @return The original, if abs(original) <= max, sign(original)*max otherwise
    **/
    virtual double saturateOutput(const double original, const double max);

    /**
      Computed the skew-symmetric matrix of a 3-dimensional vector.

      @param v The 3-dimensional vector
      @return The skew-symmetric matrix
    **/
    Eigen::Matrix3d computeSkewSymmetric(Eigen::Vector3d v);
  };
}
#endif


/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
Solutions for testdata were generated with Scilab line:

M=fscanfMat('nsm1.example');e=spec(M);e=gsort(e);rr=real(e);ii=imag(e);e=cat(1, rr, ii); s=strcat(string(e), ' ');write('tmp', s);
*/

#ifndef NDEBUG
  #define NDEBUG
#endif

//#define VIENNACL_DEBUG_ALL
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <vector>

#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/qr-method.hpp"

#include <examples/benchmarks/benchmark-utils.hpp>

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

namespace ublas = boost::numeric::ublas;

typedef float ScalarType;

const ScalarType EPS = 0.00001f;

void read_matrix_size(std::fstream& f, std::size_t& sz)
{
    if(!f.is_open())
    {
        throw std::invalid_argument("File is not opened");
    }

    f >> sz;
}

void read_matrix_body(std::fstream& f, viennacl::matrix<ScalarType>& A)
{
    if(!f.is_open())
    {
        throw std::invalid_argument("File is not opened");
    }

    boost::numeric::ublas::matrix<ScalarType> h_A(A.size1(), A.size2());

    for(std::size_t i = 0; i < h_A.size1(); i++) {
        for(std::size_t j = 0; j < h_A.size2(); j++) {
            ScalarType val = 0.0;
            f >> val;
            h_A(i, j) = val;
        }
    }

    viennacl::copy(h_A, A);
}

void read_vector_body(std::fstream& f, ublas::vector<ScalarType>& v) {
    if(!f.is_open())
        throw std::invalid_argument("File is not opened");

    for(std::size_t i = 0; i < v.size(); i++)
    {
            ScalarType val = 0.0;
            f >> val;
            v[i] = val;
    }
}

bool check_tridiag(viennacl::matrix<ScalarType>& A_orig)
{
    ublas::matrix<ScalarType> A(A_orig.size1(), A_orig.size2());
    viennacl::copy(A_orig, A);

    for (unsigned int i = 0; i < A.size1(); i++) {
        for (unsigned int j = 0; j < A.size2(); j++) {
            if ((std::abs(A(i, j)) > EPS) && ((i - 1) != j) && (i != j) && ((i + 1) != j))
            {
                // std::cout << "Failed at " << i << " " << j << " " << A(i, j) << "\n";
                return false;
            }
        }
    }
    return true;
}

bool check_hessenberg(viennacl::matrix<ScalarType>& A_orig)
{
    ublas::matrix<ScalarType> A(A_orig.size1(), A_orig.size2());
    viennacl::copy(A_orig, A);

    for (std::size_t i = 0; i < A.size1(); i++) {
        for (std::size_t j = 0; j < A.size2(); j++) {
            if ((std::abs(A(i, j)) > EPS) && (i > (j + 1)))
            {
                // std::cout << "Failed at " << i << " " << j << " " << A(i, j) << "\n";
                return false;
            }
        }
    }
    return true;
}

ScalarType matrix_compare(ublas::matrix<ScalarType>& res,
                            ublas::matrix<ScalarType>& ref)
{
    ScalarType diff = 0.0;
    ScalarType mx = 0.0;

    for(std::size_t i = 0; i < res.size1(); i++)
    {
        for(std::size_t j = 0; j < res.size2(); j++)
        {
            diff = std::max(diff, std::abs(res(i, j) - ref(i, j)));
            mx = std::max(mx, res(i, j));
        }
    }

    return diff / mx;
}

ScalarType vector_compare(ublas::vector<ScalarType>& res,
                          ublas::vector<ScalarType>& ref)
{
    std::sort(ref.begin(), ref.end());
    std::sort(res.begin(), res.end());

    ScalarType diff = 0.0;
    ScalarType mx = 0.0;
    for(size_t i = 0; i < ref.size(); i++)
    {
        diff = std::max(diff, std::abs(res[i] - ref[i]));
        mx = std::max(mx, res[i]);
    }

    return diff / mx;
}

void test_eigen(const std::string& fn, bool is_symm)
{
    std::cout << "Reading..." << "\n";
    std::size_t sz;
    // read file
    std::fstream f(fn.c_str(), std::fstream::in);
    //read size of input matrix
    read_matrix_size(f, sz);

    viennacl::matrix<ScalarType> A_input(sz, sz), A_ref(sz, sz), Q(sz, sz);
    ublas::vector<ScalarType> eigen_ref_re(sz, 0), eigen_ref_im(sz, 0), eigen_re(sz, 0), eigen_im(sz, 0);

    read_matrix_body(f, A_input);

    read_vector_body(f, eigen_ref_re);

    if(!is_symm)
        read_vector_body(f, eigen_ref_im);

    f.close();

    A_ref = A_input;

    std::cout << "Calculation..." << "\n";

    Timer timer;
    timer.start();

    if(is_symm)
        viennacl::linalg::qr_method_sym(A_input, Q, eigen_re);
    else
        viennacl::linalg::qr_method_nsm(A_input, Q, eigen_re, eigen_im);

    // std::cout << A_input << "\n";
    viennacl::ocl::get_queue().finish();

    double time_spend = timer.get();

    std::cout << "Verification..." << "\n";

    bool is_hessenberg = check_hessenberg(A_input);
    bool is_tridiag = check_tridiag(A_input);

    ublas::matrix<ScalarType> result1(sz, sz), result2(sz, sz), tmp(sz, sz);
    viennacl::copy(A_ref, tmp);
    viennacl::copy(A_input, result1);
    viennacl::copy(Q, result2);

    result1 = ublas::prod(result2, result1);
    result2 = ublas::prod(tmp, result2);

    ScalarType prods_diff = matrix_compare(result1, result2);
    ScalarType eigen_diff = vector_compare(eigen_ref_re, eigen_re);

    bool is_ok = is_hessenberg;

    if(is_symm)
        is_ok = is_ok && is_tridiag;

    is_ok = is_ok && (eigen_diff < EPS);
    is_ok = is_ok && (prods_diff < EPS);

    // std::cout << A_ref << "\n";
    // std::cout << A_input << "\n";
    // std::cout << Q << "\n";
    // std::cout << eigen_re << "\n";
    // std::cout << eigen_im << "\n";
    // std::cout << eigen_ref_re << "\n";
    // std::cout << eigen_ref_im << "\n";

    // std::cout << result1 << "\n";
    // std::cout << result2 << "\n";
    // std::cout << eigen_ref << "\n";
    // std::cout << eigen << "\n";

    printf("%6s [%dx%d] %40s time = %.4f\n", is_ok?"[[OK]]":"[FAIL]", (int)A_ref.size1(), (int)A_ref.size2(), fn.c_str(), time_spend);
    printf("tridiagonal = %d, hessenberg = %d prod-diff = %f eigen-diff = %f\n", is_tridiag, is_hessenberg, prods_diff, eigen_diff);
}

int main()
{
  // test_eigen("../../examples/testdata/eigen/symm1.example", true);
  // test_eigen("../../examples/testdata/eigen/symm2.example", true);
  // test_eigen("../../examples/testdata/eigen/symm3.example", true);

  test_eigen("../../examples/testdata/eigen/nsm1.example", false);
  test_eigen("../../examples/testdata/eigen/nsm2.example", false);
  test_eigen("../../examples/testdata/eigen/nsm3.example", false);
  test_eigen("../../examples/testdata/eigen/nsm4.example", false);

  std::cout << std::endl;
  std::cout << "------- Test completed --------" << std::endl;
  std::cout << std::endl;

  return EXIT_SUCCESS;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_HYPOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object computes the hypothenuse length: \f$(x^2 + y^2)^{1/2}\f$

    @par Header <boost/simd/function/hypot.hpp>

    @par Decorators

     - pedantic_ with this decorator provisions are made to avoid overflow
       and to compute  @c hypot as accurately as possible in any cases.

     - std_ call std::hypot

    @see sqr, sqrt

    @par Example:

      @snippet hypot.cpp hypot

    @par Possible output:

      @snippet hypot.txt hypot

  **/
  IEEEValue hypot(IEEEValue const& x, IEEEValue const& y);
} }
#endif

#include <boost/simd/function/scalar/hypot.hpp>
#include <boost/simd/function/scalar/hypot.hpp>
#include <boost/simd/function/simd/hypot.hpp>

#endif


#include <boost/test/unit_test.hpp>
#include "algorithms/math/partition_equally.hpp"

BOOST_AUTO_TEST_SUITE(TestPartitionEquality)

BOOST_AUTO_TEST_CASE(test)
{
    {
        const std::vector<uint32_t> elements = { 2, 1, 4, 5, 3, 3 };
        const uint32_t numOfPartitions = 3;
        BOOST_CHECK(
            Algo::Math::PartitionEqually::CanPartition(elements, numOfPartitions));
    }

    {
        const std::vector<uint32_t> elements = { 3, 3, 3, 3 };
        const uint32_t numOfPartitions = 3;
        BOOST_CHECK(
            false == Algo::Math::PartitionEqually::CanPartition(elements, numOfPartitions));
    }

    {
        const std::vector<uint32_t> elements = { 40 };
        const uint32_t numOfPartitions = 3;
        BOOST_CHECK(
            false == Algo::Math::PartitionEqually::CanPartition(elements, numOfPartitions));
    }

    {
        const std::vector<uint32_t> elements =
            { 17, 59, 34, 57, 17, 23, 67, 1, 18, 2, 59 };
        const uint32_t numOfPartitions = 3;
        BOOST_CHECK(
            Algo::Math::PartitionEqually::CanPartition(elements, numOfPartitions));
    }

    {
        const std::vector<uint32_t> elements =
            { 1, 2, 3, 4, 5, 5, 7, 7, 8, 10, 12, 19, 25 };
        const uint32_t numOfPartitions = 3;
        BOOST_CHECK(
            Algo::Math::PartitionEqually::CanPartition(elements, numOfPartitions));
    }
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * \ file AttackReleaseFilter.cpp
 */

#include <ATK/Dynamic/AttackReleaseFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/scoped_array.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( AttackReleaseFilter_triangle_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[i] = i / 48000;
  }
  for(int64_t i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[PROCESSSIZE/2 + i] = (PROCESSSIZE/2 - i) / 48000;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::AttackReleaseFilter<float> filter(1);
  filter.set_attack(std::exp(-1./(48000 * 1e-3)));
  filter.set_release(std::exp(-1./(48000 * 100e-3)));
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE/2; ++i)
  {
    BOOST_REQUIRE_GE(data[i], outdata[i]);
  }
  for(int64_t i = 0; i < PROCESSSIZE/2; ++i)
  {
    BOOST_REQUIRE_GE(outdata[PROCESSSIZE/2+i], outdata[PROCESSSIZE/2+i-1]);
  }
}