///////////////////////////////////////////////////////////////////////////////
/// \file vector.hpp
///
//  Copyright 2005 Eric Niebler. 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_NUMERIC_FUNCTIONAL_VECTOR_HPP_EAN_12_12_2005
#define BOOST_NUMERIC_FUNCTIONAL_VECTOR_HPP_EAN_12_12_2005

#ifdef BOOST_NUMERIC_FUNCTIONAL_HPP_INCLUDED
# error Include this file before boost/accumulators/numeric/functional.hpp
#endif

#include <vector>
#include <functional>
#include <boost/assert.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_scalar.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/typeof/std/vector.hpp>
#include <boost/accumulators/numeric/functional_fwd.hpp>

namespace boost { namespace numeric
{
    namespace operators
    {
        namespace acc_detail
        {
            template<typename Fun>
            struct make_vector
            {
                typedef std::vector<typename Fun::result_type> type;
            };
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> / Right where Right is a scalar.
        template<typename Left, typename Right>
        typename lazy_enable_if<
            is_scalar<Right>
          , acc_detail::make_vector<functional::divides<Left, Right> >
        >::type
        operator /(std::vector<Left> const &left, Right const &right)
        {
            typedef typename functional::divides<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::divides(left[i], right);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> / vector<Right>.
        template<typename Left, typename Right>
        std::vector<typename functional::divides<Left, Right>::result_type>
        operator /(std::vector<Left> const &left, std::vector<Right> const &right)
        {
            typedef typename functional::divides<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::divides(left[i], right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> * Right where Right is a scalar.
        template<typename Left, typename Right>
        typename lazy_enable_if<
            is_scalar<Right>
          , acc_detail::make_vector<functional::multiplies<Left, Right> >
        >::type
        operator *(std::vector<Left> const &left, Right const &right)
        {
            typedef typename functional::multiplies<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::multiplies(left[i], right);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle Left * vector<Right> where Left is a scalar.
        template<typename Left, typename Right>
        typename lazy_enable_if<
            is_scalar<Left>
          , acc_detail::make_vector<functional::multiplies<Left, Right> >
        >::type
        operator *(Left const &left, std::vector<Right> const &right)
        {
            typedef typename functional::multiplies<Left, Right>::result_type value_type;
            std::vector<value_type> result(right.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::multiplies(left, right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> * vector<Right>
        template<typename Left, typename Right>
        std::vector<typename functional::multiplies<Left, Right>::result_type>
        operator *(std::vector<Left> const &left, std::vector<Right> const &right)
        {
            typedef typename functional::multiplies<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::multiplies(left[i], right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> + vector<Right>
        template<typename Left, typename Right>
        std::vector<typename functional::plus<Left, Right>::result_type>
        operator +(std::vector<Left> const &left, std::vector<Right> const &right)
        {
            typedef typename functional::plus<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::plus(left[i], right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> - vector<Right>
        template<typename Left, typename Right>
        std::vector<typename functional::minus<Left, Right>::result_type>
        operator -(std::vector<Left> const &left, std::vector<Right> const &right)
        {
            typedef typename functional::minus<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::minus(left[i], right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> += vector<Left>
        template<typename Left>
        std::vector<Left> &
        operator +=(std::vector<Left> &left, std::vector<Left> const &right)
        {
            BOOST_ASSERT(left.size() == right.size());
            for(std::size_t i = 0, size = left.size(); i != size; ++i)
            {
                numeric::plus_assign(left[i], right[i]);
            }
            return left;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle -vector<Arg>
        template<typename Arg>
        std::vector<typename functional::unary_minus<Arg>::result_type>
        operator -(std::vector<Arg> const &arg)
        {
            typedef typename functional::unary_minus<Arg>::result_type value_type;
            std::vector<value_type> result(arg.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::unary_minus(arg[i]);
            }
            return result;
        }
    }

    namespace functional
    {
        struct std_vector_tag;

        template<typename T, typename Al>
        struct tag<std::vector<T, Al> >
        {
            typedef std_vector_tag type;
        };

        ///////////////////////////////////////////////////////////////////////////////
        // element-wise min of std::vector
        template<typename Left, typename Right>
        struct min_assign<Left, Right, std_vector_tag, std_vector_tag>
          : std::binary_function<Left, Right, void>
        {
            void operator ()(Left &left, Right &right) const
            {
                BOOST_ASSERT(left.size() == right.size());
                for(std::size_t i = 0, size = left.size(); i != size; ++i)
                {
                    if(numeric::less(right[i], left[i]))
                    {
                        left[i] = right[i];
                    }
                }
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // element-wise max of std::vector
        template<typename Left, typename Right>
        struct max_assign<Left, Right, std_vector_tag, std_vector_tag>
          : std::binary_function<Left, Right, void>
        {
            void operator ()(Left &left, Right &right) const
            {
                BOOST_ASSERT(left.size() == right.size());
                for(std::size_t i = 0, size = left.size(); i != size; ++i)
                {
                    if(numeric::greater(right[i], left[i]))
                    {
                        left[i] = right[i];
                    }
                }
            }
        };

        // partial specialization for std::vector.
        template<typename Left, typename Right>
        struct fdiv<Left, Right, std_vector_tag, void>
          : mpl::if_<
                are_integral<typename Left::value_type, Right>
              , divides<Left, double const>
              , divides<Left, Right>
            >::type
        {};

        // promote
        template<typename To, typename From>
        struct promote<To, From, std_vector_tag, std_vector_tag>
          : std::unary_function<From, To>
        {
            To operator ()(From &arr) const
            {
                typename remove_const<To>::type res(arr.size());
                for(std::size_t i = 0, size = arr.size(); i != size; ++i)
                {
                    res[i] = numeric::promote<typename To::value_type>(arr[i]);
                }
                return res;
            }
        };

        template<typename ToFrom>
        struct promote<ToFrom, ToFrom, std_vector_tag, std_vector_tag>
          : std::unary_function<ToFrom, ToFrom>
        {
            ToFrom &operator ()(ToFrom &tofrom) const
            {
                return tofrom;
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // functional::as_min
        template<typename T>
        struct as_min<T, std_vector_tag>
          : std::unary_function<T, typename remove_const<T>::type>
        {
            typename remove_const<T>::type operator ()(T &arr) const
            {
                return 0 == arr.size()
                  ? T()
                  : T(arr.size(), numeric::as_min(arr[0]));
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // functional::as_max
        template<typename T>
        struct as_max<T, std_vector_tag>
          : std::unary_function<T, typename remove_const<T>::type>
        {
            typename remove_const<T>::type operator ()(T &arr) const
            {
                return 0 == arr.size()
                  ? T()
                  : T(arr.size(), numeric::as_max(arr[0]));
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // functional::as_zero
        template<typename T>
        struct as_zero<T, std_vector_tag>
          : std::unary_function<T, typename remove_const<T>::type>
        {
            typename remove_const<T>::type operator ()(T &arr) const
            {
                return 0 == arr.size()
                  ? T()
                  : T(arr.size(), numeric::as_zero(arr[0]));
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // functional::as_one
        template<typename T>
        struct as_one<T, std_vector_tag>
          : std::unary_function<T, typename remove_const<T>::type>
        {
            typename remove_const<T>::type operator ()(T &arr) const
            {
                return 0 == arr.size()
                  ? T()
                  : T(arr.size(), numeric::as_one(arr[0]));
            }
        };

    } // namespace functional

}} // namespace boost::numeric

#endif


//==============================================================================
//          Copyright 2015 - J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_TENPOWER_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_TENPOWER_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  tenpower generic tag

      Represents the tenpower function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct tenpower_ : ext::elementwise_<tenpower_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<tenpower_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_tenpower_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::tenpower_, Site> dispatching_tenpower_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::tenpower_, Site>();
    }
    template<class... Args>
    struct impl_tenpower_;
  }

  /*!
    @brief Returns \f$10^n\f$ in the floating type  corresponding to A0

    @par semantic:
    For any given value n  of integral type @c I, and T of type as_floating<I>::type

    @code
    T r = tenpower(n);
    @endcode

    code is similar to:

    @code
    T r = exp10(T(n));
    @endcode

    @par Note:

    This function is not defined for floating entries

    @param  n

    @return a value of the floating associated type.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::tenpower_, tenpower, 1)
} }
#endif


/* Copyright 2017-2018 PaGMO development team

This file is part of the PaGMO library.

The PaGMO library is free software; you can redistribute it and/or modify
it under the terms of either:

  * the GNU Lesser General Public License as published by the Free
    Software Foundation; either version 3 of the License, or (at your
    option) any later version.

or

  * the GNU General Public License as published by the Free Software
    Foundation; either version 3 of the License, or (at your option) any
    later version.

or both in parallel, as here.

The PaGMO library is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
for more details.

You should have received copies of the GNU General Public License and the
GNU Lesser General Public License along with the PaGMO library.  If not,
see https://www.gnu.org/licenses/. */

#ifndef PAGMO_MULTI_OBJECTIVE_HPP
#define PAGMO_MULTI_OBJECTIVE_HPP

/** \file multi_objective.hpp
 * \brief Multi objective optimization utilities.
 *
 * This header contains utilities used to compute non dominated fronts and other
 * quantities useful for multi objective optimization
 */

#include <algorithm>
#include <boost/numeric/conversion/cast.hpp>
#include <limits>
#include <numeric>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>

#include <pagmo/detail/custom_comparisons.hpp>
#include <pagmo/exceptions.hpp>
#include <pagmo/io.hpp>
#include <pagmo/population.hpp>
#include <pagmo/types.hpp>
#include <pagmo/utils/discrepancy.hpp> // halton

namespace pagmo
{

namespace detail
{
// Recursive function building all m-ple of elements of X summing to s
// In C/C++ implementations there exists a limit on the number of times you
// can call recursively a function. It depends on a variety of factors,
// but probably it a number around few thousands on modern machines.
// If the limit is surpassed, the program terminates.
// I was thinking that one could create a problem with a few thousands objectives,
// call this function thus causing a crash from Python. In principle I think we
// can prevent this by limiting the recursion (e.g., via a function parameter that
// gets increased each time the function is called from itself).
// But for now I'd just put a note about this.
inline void reksum(std::vector<std::vector<double>> &retval, const std::vector<population::size_type> &X,
                   population::size_type m, population::size_type s, std::vector<double> eggs = std::vector<double>())
{
    if (m == 1u) {
        if (std::find(X.begin(), X.end(), s) == X.end()) { // not found
            return;
        } else {
            eggs.push_back(static_cast<double>(s));
            retval.push_back(eggs);
        }
    } else {
        for (decltype(X.size()) i = 0u; i < X.size(); ++i) {
            eggs.push_back(static_cast<double>(X[i]));
            reksum(retval, X, m - 1u, s - X[i], eggs);
            eggs.pop_back();
        }
    }
}
} // namespace detail

/// Pareto-dominance
/**
 * Return true if \p obj1 Pareto dominates \p obj2, false otherwise. Minimization
 * is assumed.
 *
 * Each pair of corresponding elements in \p obj1 and \p obj2 is compared: if all
 * elements in \p obj1 are less or equal to the corresponding element in \p obj2,
 * but at least one is different, \p true will be returned. Otherwise, \p false will be returned.
 *
 * @param obj1 first vector of objectives.
 * @param obj2 second vector of objectives.
 *
 * @return \p true if \p obj1 is dominating \p obj2, \p false otherwise.
 *
 * @throws std::invalid_argument if the dimensions of the two objectives are different
 */
inline bool pareto_dominance(const vector_double &obj1, const vector_double &obj2)
{
    if (obj1.size() != obj2.size()) {
        pagmo_throw(std::invalid_argument,
                    "Different number of objectives found in input fitnesses: " + std::to_string(obj1.size()) + " and "
                        + std::to_string(obj2.size()) + ". I cannot define dominance");
    }
    vector_double::size_type count1 = 0u;
    vector_double::size_type count2 = 0u;
    for (decltype(obj1.size()) i = 0u; i < obj1.size(); ++i) {
        if (obj1[i] < obj2[i]) {
            ++count1;
        }
        if (obj1[i] == obj2[i]) {
            ++count2;
        }
    }
    return (((count1 + count2) == obj1.size()) && (count1 > 0u));
}

/// Non dominated front 2D (Kung's algorithm)
/**
 * Finds the non dominated front of a set of two dimensional objectives. Complexity is O(N logN) and is thus lower than
 * the
 * complexity of calling pagmo::fast_non_dominated_sorting
 *
 * See: Jensen, Mikkel T. "Reducing the run-time complexity of multiobjective EAs: The NSGA-II and other algorithms."
 * IEEE Transactions on Evolutionary Computation 7.5 (2003): 503-515.
 *
 * @param input_objs an <tt>std::vector</tt> containing the points (i.e. vector of objectives)
 *
 * @return A <tt>std::vector</tt> containing the indexes of the points in the non-dominated front
 *
 * @throws std::invalid_argument If the objective vectors are not all containing two-objectives
 */
inline std::vector<vector_double::size_type> non_dominated_front_2d(const std::vector<vector_double> &input_objs)
{
    // If the input is empty return an empty vector
    if (input_objs.size() == 0u) {
        return {};
    }
    // How many objectives? M, of course.
    auto M = input_objs[0].size();
    // We make sure all input_objs contain M objectives
    if (!std::all_of(input_objs.begin(), input_objs.end(),
                     [M](const vector_double &item) { return item.size() == M; })) {
        pagmo_throw(std::invalid_argument, "Input contains vector of objectives with heterogeneous dimensionalities");
    }
    // We make sure this function is only requested for two objectives.
    if (M != 2u) {
        pagmo_throw(std::invalid_argument, "The number of objectives detected is " + std::to_string(M)
                                               + ", while Kung's algorithm only works for two objectives.");
    }
    // Sanity checks are over. We may run Kung's algorithm.
    std::vector<vector_double::size_type> front;
    std::vector<vector_double::size_type> indexes(input_objs.size());
    std::iota(indexes.begin(), indexes.end(), vector_double::size_type(0u));
    // Sort in ascending order with respect to the first component
    std::sort(indexes.begin(), indexes.end(),
              [&input_objs](vector_double::size_type idx1, vector_double::size_type idx2) {
                  if (input_objs[idx1][0] == input_objs[idx2][0]) {
                      return detail::less_than_f(input_objs[idx1][1], input_objs[idx2][1]);
                  }
                  return detail::less_than_f(input_objs[idx1][0], input_objs[idx2][0]);
              });
    for (auto i : indexes) {
        bool flag = false;
        for (auto j : front) {
            if (pareto_dominance(input_objs[j], input_objs[i])) {
                flag = true;
                break;
            }
        }
        if (!flag) {
            front.push_back(i);
        }
    }
    return front;
}

/// Return type for the fast_non_dominated_sorting algorithm
using fnds_return_type
    = std::tuple<std::vector<std::vector<vector_double::size_type>>, std::vector<std::vector<vector_double::size_type>>,
                 std::vector<vector_double::size_type>, std::vector<vector_double::size_type>>;

/// Fast non dominated sorting
/**
 * An implementation of the fast non dominated sorting algorithm. Complexity is \f$ O(MN^2)\f$ where \f$M\f$ is the
 * number of objectives
 * and \f$N\f$ is the number of individuals.
 *
 * See: Deb, Kalyanmoy, et al. "A fast elitist non-dominated sorting genetic algorithm
 * for multi-objective optimization: NSGA-II." Parallel problem solving from nature PPSN VI. Springer Berlin Heidelberg,
 * 2000.
 *
 * @param points An std::vector containing the objectives of different individuals. Example
 * {{1,2,3},{-2,3,7},{-1,-2,-3},{0,0,0}}
 *
 * @return an std::tuple containing:
 *  - the non dominated fronts, an <tt>std::vector<std::vector<vector_double::size_type>></tt>
 * containing the non dominated fronts. Example {{1,2},{3},{0}}
 *  - the domination list, an <tt>std::vector<std::vector<vector_double::size_type>></tt>
 * containing the domination list, i.e. the indexes of all individuals
 * dominated by the individual at position \f$i\f$. Example {{},{},{0,3},{0}}
 *  - the domination count, an <tt>std::vector<vector_double::size_type></tt> containing the number of individuals
 * that dominate the individual at position \f$i\f$. Example {2, 0, 0, 1}
 *  - the non domination rank, an <tt>std::vector<vector_double::size_type></tt> containing the index of the non
 * dominated front to which the individual at position \f$i\f$ belongs. Example {2,0,0,1}
 *
 * @throws std::invalid_argument If the size of \p points is not at least 2
 */
inline fnds_return_type fast_non_dominated_sorting(const std::vector<vector_double> &points)
{
    auto N = points.size();
    // We make sure to have two points at least (one could also be allowed)
    if (N < 2u) {
        pagmo_throw(std::invalid_argument, "At least two points are needed for fast_non_dominated_sorting: "
                                               + std::to_string(N) + " detected.");
    }
    // Initialize the return values
    std::vector<std::vector<vector_double::size_type>> non_dom_fronts(1u);
    std::vector<std::vector<vector_double::size_type>> dom_list(N);
    std::vector<vector_double::size_type> dom_count(N);
    std::vector<vector_double::size_type> non_dom_rank(N);

    // Start the fast non dominated sort algorithm
    for (decltype(N) i = 0u; i < N; ++i) {
        dom_list[i].clear();
        dom_count[i] = 0u;
        for (decltype(N) j = 0u; j < N; ++j) {
            if (i == j) {
                continue;
            }
            if (pareto_dominance(points[i], points[j])) {
                dom_list[i].push_back(j);
            } else if (pareto_dominance(points[j], points[i])) {
                ++dom_count[i];
            }
        }
        if (dom_count[i] == 0u) {
            non_dom_rank[i] = 0u;
            non_dom_fronts[0].push_back(i);
        }
    }
    // we copy dom_count as we want to output its value at this point
    auto dom_count_copy(dom_count);
    auto current_front = non_dom_fronts[0];
    std::vector<std::vector<vector_double::size_type>>::size_type front_counter(0u);
    while (current_front.size() != 0u) {
        std::vector<vector_double::size_type> next_front;
        for (decltype(current_front.size()) p = 0u; p < current_front.size(); ++p) {
            for (decltype(dom_list[current_front[p]].size()) q = 0u; q < dom_list[current_front[p]].size(); ++q) {
                --dom_count_copy[dom_list[current_front[p]][q]];
                if (dom_count_copy[dom_list[current_front[p]][q]] == 0u) {
                    non_dom_rank[dom_list[current_front[p]][q]] = front_counter + 1u;
                    next_front.push_back(dom_list[current_front[p]][q]);
                }
            }
        }
        ++front_counter;
        current_front = next_front;
        if (current_front.size() != 0u) {
            non_dom_fronts.push_back(current_front);
        }
    }
    return std::make_tuple(std::move(non_dom_fronts), std::move(dom_list), std::move(dom_count),
                           std::move(non_dom_rank));
}

/// Crowding distance
/**
 * An implementation of the crowding distance. Complexity is \f$ O(MNlog(N))\f$ where \f$M\f$ is the number of
 * objectives
 * and \f$N\f$ is the number of individuals. The function assumes the input is a non-dominated front. Failiure to this
 * condition
 * will result in undefined behaviour.
 *
 * See: Deb, Kalyanmoy, et al. "A fast elitist non-dominated sorting genetic algorithm
 * for multi-objective optimization: NSGA-II." Parallel problem solving from nature PPSN VI. Springer Berlin Heidelberg,
 * 2000.
 *
 * @param non_dom_front An <tt>std::vector<vector_double></tt> containing a non dominated front. Example
 * {{0,0},{-1,1},{2,-2}}
 *
 * @returns a vector_double containing the crowding distances. Example: {2, inf, inf}
 *
 * @throws std::invalid_argument If \p non_dom_front does not contain at least two points
 * @throws std::invalid_argument If points in \p do not all have at least two objectives
 * @throws std::invalid_argument If points in \p non_dom_front do not all have the same dimensionality
 */
inline vector_double crowding_distance(const std::vector<vector_double> &non_dom_front)
{

    auto N = non_dom_front.size();

    if (N < 2u)
    {
        vector_double retval(N, std::numeric_limits<double>::infinity());
        return retval;
    }

    // We make sure to have two points at least
    if (N < 2u) {
        pagmo_throw(std::invalid_argument,
                    "A non dominated front must contain at least two points: " + std::to_string(N) + " detected.");
    }
    auto M = non_dom_front[0].size();
    // We make sure the first point of the input non dominated front contains at least two objectives
    if (M < 2u) {
        pagmo_throw(std::invalid_argument, "Points in the non dominated front must contain at least two objectives: "
                                               + std::to_string(M) + " detected.");
    }
    // We make sure all points contain the same number of objectives
    if (!std::all_of(non_dom_front.begin(), non_dom_front.end(),
                     [M](const vector_double &item) { return item.size() == M; })) {
        pagmo_throw(std::invalid_argument, "A non dominated front must contain points of uniform dimensionality. Some "
                                           "different sizes were instead detected.");
    }
    std::vector<vector_double::size_type> indexes(N);
    std::iota(indexes.begin(), indexes.end(), vector_double::size_type(0u));
    vector_double retval(N, 0.);
    for (decltype(M) i = 0u; i < M; ++i) {
        std::sort(indexes.begin(), indexes.end(),
                  [i, &non_dom_front](vector_double::size_type idx1, vector_double::size_type idx2) {
                      return detail::less_than_f(non_dom_front[idx1][i], non_dom_front[idx2][i]);
                  });
        retval[indexes[0]] = std::numeric_limits<double>::infinity();
        retval[indexes[N - 1u]] = std::numeric_limits<double>::infinity();
        double df = non_dom_front[indexes[N - 1u]][i] - non_dom_front[indexes[0]][i];
        for (decltype(N - 2u) j = 1u; j < N - 1u; ++j) {
            retval[indexes[j]] += (non_dom_front[indexes[j + 1u]][i] - non_dom_front[indexes[j - 1u]][i]) / df;
        }
    }
    return retval;
}

/// Sorts a population in multi-objective optimization
/**
 * Sorts a population (intended here as an <tt>std::vector<vector_double></tt> containing the  objective vectors)
 * with respect to the following strict ordering:
 * - \f$f_1 \prec f_2\f$ if the non domination ranks are such that \f$i_1 < i_2\f$. In case
 * \f$i_1 = i_2\f$, then \f$f_1 \prec f_2\f$ if the crowding distances are such that \f$d_1 > d_2\f$.
 *
 * Complexity is \f$ O(MN^2)\f$ where \f$M\f$ is the number of objectives and \f$N\f$ is the number of individuals.
 *
 * This function will also work for single objective optimization, i.e. with 1 objective
 * in which case, though, it is more efficient to sort using directly one of the following forms:
 *
 * @code{.unparsed}
 * std::sort(input_f.begin(), input_f.end(), [] (auto a, auto b) {return a[0] < b[0];});
 * @endcode
 * @code{.unparsed}
 * std::vector<vector_double::size_type> idx(input_f.size());
 * std::iota(idx.begin(), idx.end(), vector_double::size_type(0u));
 * std::sort(idx.begin(), idx.end(), [] (auto a, auto b) {return input_f[a][0] < input_f[b][0];});
 * @endcode
 *
 * @param input_f Input objectives vectors. Example {{0.25,0.25},{-1,1},{2,-2}};
 *
 * @returns an <tt>std::vector</tt> containing the indexes of the sorted objectives vectors. Example {1,2,0}
 *
 * @throws unspecified all exceptions thrown by pagmo::fast_non_dominated_sorting and pagmo::crowding_distance
 */
inline std::vector<vector_double::size_type> sort_population_mo(const std::vector<vector_double> &input_f)
{
    if (input_f.size() < 2u) { // corner cases
        if (input_f.size() == 0u) {
            return {};
        }
        if (input_f.size() == 1u) {
            return {0u};
        }
    }
    // Create the indexes 0....N-1
    std::vector<vector_double::size_type> retval(input_f.size());
    std::iota(retval.begin(), retval.end(), vector_double::size_type(0u));
    // Run fast-non-dominated sorting and compute the crowding distance for all input objectives vectors
    auto tuple = fast_non_dominated_sorting(input_f);
    vector_double crowding(input_f.size());
    for (const auto &front : std::get<0>(tuple)) {
        if (front.size() == 1u) {
            crowding[front[0]] = 0u; // corner case of a non dominated front containing one individual. Crowding
                                     // distance is not defined nor it will be used
        } else {
            std::vector<vector_double> non_dom_fits(front.size());
            for (decltype(front.size()) i = 0u; i < front.size(); ++i) {
                non_dom_fits[i] = input_f[front[i]];
            }
            vector_double tmp(crowding_distance(non_dom_fits));
            for (decltype(front.size()) i = 0u; i < front.size(); ++i) {
                crowding[front[i]] = tmp[i];
            }
        }
    }
    // Sort the indexes
    std::sort(retval.begin(), retval.end(),
              [&tuple, &crowding](vector_double::size_type idx1, vector_double::size_type idx2) {
                  if (std::get<3>(tuple)[idx1] == std::get<3>(tuple)[idx2]) {        // same non domination rank
                      return detail::greater_than_f(crowding[idx1], crowding[idx2]); // crowding distance decides
                  } else {                                                           // different non domination ranks
                      return std::get<3>(tuple)[idx1] < std::get<3>(tuple)[idx2];    // non domination rank decides
                  };
              });
    return retval;
}

/// Selects the best N individuals in multi-objective optimization
/**
 * Selects the best N individuals out of a population, (intended here as an
 * <tt>std::vector<vector_double></tt> containing the  objective vectors). The strict ordering used
 * is the same as that defined in pagmo::sort_population_mo.
 *
 * Complexity is \f$ O(MN^2)\f$ where \f$M\f$ is the number of objectives and \f$N\f$ is the number of individuals.
 *
 * While the complexity is the same as that of pagmo::sort_population_mo, this function returns a permutation
 * of:
 *
 * @code{.unparsed}
 * auto ret = pagmo::sort_population_mo(input_f).resize(N);
 * @endcode
 *
 * but it is faster than the above code: it avoids to compute the crowding distance for all individuals and only
 * computes
 * it for the last non-dominated front that contains individuals included in the best N.
 *
 * @param input_f Input objectives vectors. Example {{0.25,0.25},{-1,1},{2,-2}};
 * @param N Number of best individuals to return
 *
 * @returns an <tt>std::vector</tt> containing the indexes of the best N objective vectors. Example {2,1}
 *
 * @throws unspecified all exceptions thrown by pagmo::fast_non_dominated_sorting and pagmo::crowding_distance
 */
inline std::vector<vector_double::size_type> select_best_N_mo(const std::vector<vector_double> &input_f,
                                                              vector_double::size_type N)
{
    if (N < 1u) {
        pagmo_throw(std::invalid_argument,
                    "The best: " + std::to_string(N) + " individuals were requested, while 1 is the minimum");
    }
    if (input_f.size() == 0u) { // corner case
        return {};
    }
    if (input_f.size() == 1u) { // corner case
        return {0u};
    }
    if (N >= input_f.size()) { // corner case
        std::vector<vector_double::size_type> retval(input_f.size());
        std::iota(retval.begin(), retval.end(), vector_double::size_type(0u));
        return retval;
    }
    std::vector<vector_double::size_type> retval;
    std::vector<vector_double::size_type>::size_type front_id(0u);
    // Run fast-non-dominated sorting
    auto tuple = fast_non_dominated_sorting(input_f);
    // Insert all non dominated fronts if not more than N
    for (const auto &front : std::get<0>(tuple)) {
        if (retval.size() + front.size() <= N) {
            for (auto i : front) {
                retval.push_back(i);
            }
            if (retval.size() == N) {
                return retval;
            }
            ++front_id;
        } else {
            break;
        }
    }
    auto front = std::get<0>(tuple)[front_id];
    std::vector<vector_double> non_dom_fits(front.size());
    // Run crowding distance for the front
    for (decltype(front.size()) i = 0u; i < front.size(); ++i) {
        non_dom_fits[i] = input_f[front[i]];
    }
    vector_double cds(crowding_distance(non_dom_fits));
    // We now have front and crowding distance, we sort the front w.r.t. the crowding
    std::vector<vector_double::size_type> idxs(front.size());
    std::iota(idxs.begin(), idxs.end(), vector_double::size_type(0u));
    std::sort(idxs.begin(), idxs.end(), [&cds](vector_double::size_type idx1, vector_double::size_type idx2) {
        return detail::greater_than_f(cds[idx1], cds[idx2]);
    }); // Descending order1
    auto remaining = N - retval.size();
    for (decltype(remaining) i = 0u; i < remaining; ++i) {
        retval.push_back(front[idxs[i]]);
    }
    return retval;
}

/// Ideal point
/**
 * Computes the ideal point of an input population, (intended here as an
 * <tt>std::vector<vector_double></tt> containing the  objective vectors).
 *
 * Complexity is \f$ O(MN)\f$ where \f$M\f$ is the number of objectives and \f$N\f$ is the number of individuals.
 *
 * @param points Input objectives vectors. Example {{-1,3,597},{1,2,3645},{2,9,789},{0,0,231},{6,-2,4576}};
 *
 * @returns A vector_double containing the ideal point. Example: {-1,-2,231}
 *
 * @throws std::invalid_argument if the input objective vectors are not all of the same size
 */
inline vector_double ideal(const std::vector<vector_double> &points)
{
    // Corner case
    if (points.size() == 0u) {
        return {};
    }

    // Sanity checks
    auto M = points[0].size();
    for (const auto &f : points) {
        if (f.size() != M) {
            pagmo_throw(std::invalid_argument,
                        "Input vector of objectives must contain fitness vector of equal dimension "
                            + std::to_string(M));
        }
    }
    // Actual algorithm
    vector_double retval(M);
    for (decltype(M) i = 0u; i < M; ++i) {
        retval[i]
            = (*std::min_element(points.begin(), points.end(),
                                 [i](const vector_double &f1, const vector_double &f2) { return f1[i] < f2[i]; }))[i];
    }
    return retval;
}

/// Nadir point
/**
 * Computes the nadir point of an input population, (intended here as an
 * <tt>std::vector<vector_double></tt> containing the  objective vectors).
 *
 * Complexity is \f$ O(MN^2)\f$ where \f$M\f$ is the number of objectives and \f$N\f$ is the number of individuals.
 *
 * @param points Input objective vectors. Example {{0,7},{1,5},{2,3},{4,2},{7,1},{10,0},{6,6},{9,15}}
 *
 * @returns A vector_double containing the nadir point. Example: {10,7}
 *
 */
inline vector_double nadir(const std::vector<vector_double> &points)
{
    // Corner case
    if (points.size() == 0u) {
        return {};
    }
    // Sanity checks
    auto M = points[0].size();
    // We extract all objective vectors belonging to the first non dominated front (the Pareto front)
    auto pareto_idx = std::get<0>(fast_non_dominated_sorting(points))[0];
    std::vector<vector_double> nd_points;
    for (auto idx : pareto_idx) {
        nd_points.push_back(points[idx]);
    }
    // And compute the nadir over them
    vector_double retval(M);
    for (decltype(M) i = 0u; i < M; ++i) {
        retval[i]
            = (*std::max_element(nd_points.begin(), nd_points.end(),
                                 [i](const vector_double &f1, const vector_double &f2) { return f1[i] < f2[i]; }))[i];
    }
    return retval;
}

/// Decomposition weights generation
/**
 * Generates a requested number of weight vectors to be used to decompose a multi-objective problem. Three methods are
 *available:
 * - "grid" generates weights on an uniform grid. This method may only be used when the number of requested weights to
 *be genrated is such that a uniform grid is indeed possible. In
 * two dimensions this is always the case, but in larger dimensions uniform grids are possible only in special cases
 * - "random" generates weights randomly distributing them uniformly on the simplex (weights are such that \f$\sum_i
 * \lambda_i = 1\f$)
 * - "low discrepancy" generates weights using a low-discrepancy sequence to, eventually, obtain a
 * better coverage of the Pareto front. Halton sequence is used since low dimensionalities are expected in the number of
 * objectives (i.e. less than 20), hence Halton sequence is deemed as appropriate.
 *
 * \verbatim embed:rst:leading-asterisk
 * .. note::
 *
 *    All genration methods are guaranteed to generate weights on the simplex (:math:`\sum_i \lambda_i = 1`). All
 *    weight generation methods are guaranteed to generate the canonical weights [1,0,0,...], [0,1,0,..], ... first.
 *
 * \endverbatim
 *
 * Example: to generate 10 weights distributed somehow regularly to decompose a three dimensional problem:
 * @code{.unparsed}
 * detail::random_engine_type r_engine();
 * auto lambdas = decomposition_weights(3u, 10u, "low discrepancy", r_engine);
 * @endcode
 *
 * @param n_f dimension of each weight vector (i.e. fitness dimension)
 * @param n_w number of weights to be generated
 * @param method methods to generate the weights of the decomposed problems. One of "grid", "random",
 *"low discrepancy"
 * @param r_engine random engine
 *
 * @returns an <tt>std:vector</tt> containing the weight vectors
 *
 * @throws if \p nf and \p nw are not compatible with the selected weight generation method or if \p method
 * is not one of "grid", "random" or "low discrepancy"
 */
inline std::vector<vector_double> decomposition_weights(vector_double::size_type n_f, vector_double::size_type n_w,
                                                        const std::string &method, detail::random_engine_type &r_engine)
{
    // Sanity check
    if (n_f > n_w) {
        pagmo_throw(std::invalid_argument,
                    "A fitness size of " + std::to_string(n_f)
                        + " was requested to the weight generation routine, while " + std::to_string(n_w)
                        + " weights were requested to be generated. To allow weight be generated correctly the number "
                          "of weights must be strictly larger than the number of objectives");
    }

    if (n_f < 2u) {
        pagmo_throw(
            std::invalid_argument,
            "A fitness size of " + std::to_string(n_f)
                + " was requested to generate decomposed weights. A dimension of at least two must be requested.");
    }

    // Random distributions
    std::uniform_real_distribution<double> drng(0., 1.); // to generate a number in [0, 1)
    std::vector<vector_double> retval;
    if (method == "grid") {
        // find the largest H resulting in a population smaller or equal to NP
        decltype(n_w) H;
        if (n_f == 2u) {
            H = n_w - 1u;
        } else if (n_f == 3u) {
            H = static_cast<decltype(H)>(std::floor(0.5 * (std::sqrt(8. * static_cast<double>(n_w) + 1.) - 3.)));
        } else {
            H = 1u;
            while (binomial_coefficient(H + n_f - 1u, n_f - 1u) <= static_cast<double>(n_w)) {
                ++H;
            }
            H--;
        }
        // We check that NP equals the population size resulting from H
        if (std::abs(static_cast<double>(n_w) - binomial_coefficient(H + n_f - 1u, n_f - 1u)) > 1E-8) {
            std::ostringstream error_message;
            error_message << "Population size of " << std::to_string(n_w) << " is detected, but not supported by the '"
                          << method << "' weight generation method selected. A size of "
                          << binomial_coefficient(H + n_f - 1u, n_f - 1u) << " or "
                          << binomial_coefficient(H + n_f, n_f - 1u) << " is possible.";
            pagmo_throw(std::invalid_argument, error_message.str());
        }
        // We generate the weights
        std::vector<population::size_type> range(H + 1u);
        std::iota(range.begin(), range.end(), std::vector<population::size_type>::size_type(0u));
        detail::reksum(retval, range, n_f, H);
        for (decltype(retval.size()) i = 0u; i < retval.size(); ++i) {
            for (decltype(retval[i].size()) j = 0u; j < retval[i].size(); ++j) {
                retval[i][j] /= static_cast<double>(H);
            }
        }
    } else if (method == "low discrepancy") {
        // We first push back the "corners" [1,0,0,...], [0,1,0,...]
        for (decltype(n_f) i = 0u; i < n_f; ++i) {
            retval.push_back(vector_double(n_f, 0.));
            retval[i][i] = 1.;
        }
        // Then we add points on the simplex randomly genrated using Halton low discrepancy sequence
        halton ld_seq{boost::numeric_cast<unsigned int>(n_f - 1u), boost::numeric_cast<unsigned int>(n_f)};
        for (decltype(n_w) i = n_f; i < n_w; ++i) {
            retval.push_back(sample_from_simplex(ld_seq()));
        }
    } else if (method == "random") {
        // We first push back the "corners" [1,0,0,...], [0,1,0,...]
        for (decltype(n_f) i = 0u; i < n_f; ++i) {
            retval.push_back(vector_double(n_f, 0.));
            retval[i][i] = 1.;
        }
        for (decltype(n_w) i = n_f; i < n_w; ++i) {
            vector_double dummy(n_f - 1u, 0.);
            for (decltype(n_f) j = 0u; j < n_f - 1u; ++j) {
                dummy[j] = drng(r_engine);
            }
            retval.push_back(sample_from_simplex(dummy));
        }
    } else {
        pagmo_throw(std::invalid_argument,
                    "Weight generation method " + method
                        + " is unknown. One of 'grid', 'random' or 'low discrepancy' was expected");
    }
    return retval;
}

/// Decomposes a vector of objectives.
/**
 * A vector of objectives is reduced to one only objective using a decomposition
 * technique.
 *
 * Three different *decomposition methods* are here made available:
 *
 * - weighted decomposition,
 * - Tchebycheff decomposition,
 * - boundary interception method (with penalty constraint).
 *
 * In the case of \f$n\f$ objectives, we indicate with: \f$ \mathbf f(\mathbf x) = [f_1(\mathbf x), \ldots,
 * f_n(\mathbf x)] \f$ the vector containing the original multiple objectives, with: \f$ \boldsymbol \lambda =
 * (\lambda_1, \ldots, \lambda_n) \f$ an \f$n\f$-dimensional weight vector and with: \f$ \mathbf z^* = (z^*_1, \ldots,
 * z^*_n) \f$ an \f$n\f$-dimensional reference point. We also ussume \f$\lambda_i > 0, \forall i=1..n\f$ and \f$\sum_i
 * \lambda_i = 1\f$.
 *
 * The resulting single objective is thus defined as:
 *
 * - weighted decomposition: \f$ f_d(\mathbf x) = \boldsymbol \lambda \cdot \mathbf f \f$,
 * - Tchebycheff decomposition: \f$ f_d(\mathbf x) = \max_{1 \leq i \leq m} \lambda_i \vert f_i(\mathbf x) - z^*_i \vert
 * \f$,
 * - boundary interception method (with penalty constraint): \f$ f_d(\mathbf x) = d_1 + \theta d_2\f$,
 *
 * where \f$d_1 = (\mathbf f - \mathbf z^*) \cdot \hat {\mathbf i}_{\lambda}\f$,
 * \f$d_2 = \vert (\mathbf f - \mathbf z^*) - d_1 \hat {\mathbf i}_{\lambda})\vert\f$ and
 * \f$ \hat {\mathbf i}_{\lambda} = \frac{\boldsymbol \lambda}{\vert \boldsymbol \lambda \vert}\f$.
 *
 * @param f input vector of objectives.
 * @param weight the weight to be used in the decomposition.
 * @param ref_point the reference point to be used if either "tchebycheff" or "bi".
 * was indicated as a decomposition method. Its value is ignored if "weighted" was indicated.
 * @param method decomposition method: one of "weighted", "tchebycheff" or "bi"
 *
 * @return the decomposed objective.
 *
 * @throws std::invalid_argument if \p f, \p weight and \p ref_point have different sizes
 * @throws std::invalid_argument if \p method is not one of "weighted", "tchebycheff" or "bi"
 */
inline vector_double decompose_objectives(const vector_double &f, const vector_double &weight,
                                          const vector_double &ref_point, const std::string &method)
{
    if (weight.size() != f.size()) {
        pagmo_throw(std::invalid_argument,
                    "Weight vector size must be equal to the number of objectives. The size of the weight vector is "
                        + std::to_string(weight.size()) + " while " + std::to_string(f.size())
                        + " objectives were detected");
    }
    if (ref_point.size() != f.size()) {
        pagmo_throw(
            std::invalid_argument,
            "Reference point size must be equal to the number of objectives. The size of the reference point is "
                + std::to_string(ref_point.size()) + " while " + std::to_string(f.size())
                + " objectives were detected");
    }
    if (f.size() == 0u) {
        pagmo_throw(std::invalid_argument, "The number of objectives detected is: " + std::to_string(f.size())
                                               + ". Cannot decompose this into anything.");
    }
    double fd = 0.;
    if (method == "weighted") {
        for (decltype(f.size()) i = 0u; i < f.size(); ++i) {
            fd += weight[i] * f[i];
        }
    } else if (method == "tchebycheff") {
        double tmp, fixed_weight;
        for (decltype(f.size()) i = 0u; i < f.size(); ++i) {
            (weight[i] == 0.) ? (fixed_weight = 1e-4)
                              : (fixed_weight = weight[i]); // fixes the numerical problem of 0 weights
            tmp = fixed_weight * std::abs(f[i] - ref_point[i]);
            if (tmp > fd) {
                fd = tmp;
            }
        }
    } else if (method == "bi") { // BI method
        const double THETA = 5.;
        double d1 = 0.;
        double weight_norm = 0.;
        for (decltype(f.size()) i = 0u; i < f.size(); ++i) {
            d1 += (f[i] - ref_point[i]) * weight[i];
            weight_norm += std::pow(weight[i], 2);
        }
        weight_norm = std::sqrt(weight_norm);
        d1 = d1 / weight_norm;

        double d2 = 0.;
        for (decltype(f.size()) i = 0u; i < f.size(); ++i) {
            d2 += std::pow(f[i] - (ref_point[i] + d1 * weight[i] / weight_norm), 2);
        }
        d2 = std::sqrt(d2);
        fd = d1 + THETA * d2;
    } else {
        pagmo_throw(std::invalid_argument, "The decomposition method chosen was: " + method
                                               + R"(, but only "weighted", "tchebycheff" or "bi" are allowed)");
    }
    return {fd};
}

} // namespace pagmo
#endif


#include <boost/serialization/export.hpp>
#include "StochasticGradientDescent.hpp"
#include "../layer/neuron/RecurrentNeuron.hpp"
#include "../layer/neuron/SimpleNeuron.hpp"

using namespace std;
using namespace snn;
using namespace internal;

BOOST_CLASS_EXPORT(StochasticGradientDescent)

StochasticGradientDescent::StochasticGradientDescent(const float learningRate, const float momentum)
    : learningRate(learningRate), momentum(momentum)
{
}

shared_ptr<NeuralNetworkOptimizer> StochasticGradientDescent::clone() const
{
    return make_shared<StochasticGradientDescent>(*this);
}

void StochasticGradientDescent::updateWeights(SimpleNeuron& neuron, float error) const
{
    for (size_t w = 0; w < neuron.weights.size(); ++w)
    {
        auto deltaWeights = this->learningRate * error * neuron.lastInputs[w];
        deltaWeights += this->momentum * neuron.previousDeltaWeights[w];
        neuron.weights[w] += deltaWeights;
        neuron.previousDeltaWeights[w] = deltaWeights;
    }
}

void StochasticGradientDescent::updateWeights(RecurrentNeuron& neuron, float error) const
{
    size_t w;
    for (w = 0; w < neuron.lastInputs.size(); ++w)
    {
        auto deltaWeights = this->learningRate * error * neuron.lastInputs[w];
        deltaWeights += this->momentum * neuron.previousDeltaWeights[w];
        neuron.weights[w] += deltaWeights;
        neuron.previousDeltaWeights[w] = deltaWeights;
    }
    neuron.recurrentError = error + neuron.recurrentError * neuron.outputFunction->derivative(neuron.previousSum) *
        neuron.weights[w];

    auto deltaWeights = this->learningRate * neuron.recurrentError * neuron.previousOutput;
    deltaWeights += this->momentum * neuron.previousDeltaWeights[w];
    neuron.weights[w] += deltaWeights;
    neuron.previousDeltaWeights[w] = deltaWeights;
}

int StochasticGradientDescent::isValid()
{
    if (this->learningRate <= 0.0f || this->learningRate >= 1.0f)
        return 103;
    if (this->momentum < 0.0f || this->momentum > 1.0f)
        return 104;
    return 0;
}

bool StochasticGradientDescent::operator==(const NeuralNetworkOptimizer& optimizer) const
{
    try
    {
        const auto& o = dynamic_cast<const StochasticGradientDescent&>(optimizer);
        return this->NeuralNetworkOptimizer::operator==(optimizer)
            && this->learningRate == o.learningRate
            && this->momentum == o.momentum;
    }
    catch (bad_cast&)
    {
        return false;
    }
}

bool StochasticGradientDescent::operator!=(const NeuralNetworkOptimizer& optimizer) const
{
    return !(*this == optimizer);
}


// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

/// \copyright Copyright 2021 Apex.AI, Inc.
/// All rights reserved.

#ifndef STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_
#define STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_

#include <common/types.hpp>
#include <measurement_conversion/measurement_typedefs.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <state_estimation/kalman_filter/kalman_filter.hpp>
#include <state_estimation_nodes/filter_typedefs.hpp>
#include <state_estimation_nodes/history.hpp>
#include <state_estimation_nodes/steady_time_grid.hpp>
#include <state_estimation_nodes/visibility_control.hpp>
#include <state_vector/common_states.hpp>

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

#include <chrono>
#include <cstdint>
#include <limits>
#include <memory>
#include <string>
#include <vector>

namespace autoware
{
namespace common
{
namespace state_estimation
{

///
/// @brief      This class provides a high level interface to the Kalman Filter allowing to predict
///             the state of the filter with time and observe it by receiving ROS messages.
///
/// @tparam     FilterT           Type of filter used internally.
///
template<typename FilterT>
class STATE_ESTIMATION_NODES_PUBLIC KalmanFilterWrapper
{
  using HistoryT = History<
    FilterT,
    PredictionEvent,
    ResetEvent<FilterT>,
    PoseMeasurementXYZ32,
    PoseMeasurementXYZRPY32>;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using State = typename FilterT::State;

  ///
  /// @brief      Create an EKF wrapper.
  ///
  /// @param[in]  motion_model              The motion model that is to be used.
  /// @param[in]  noise_model               The noise model that is to be used.
  /// @param[in]  initial_state_covariance  The initial covariances for the state. This is usually a
  ///                                       diagonal matrix with sigmas squared for each state
  ///                                       dimension on the diagonal.
  /// @param[in]  expected_dt               Expected time difference between updates of the filter.
  /// @param[in]  frame_id                  The frame id in which tracking takes place.
  /// @param[in]  history_duration          Length of the history of events.
  /// @param[in]  mahalanobis_threshold     The threshold on the Mahalanobis distance for outlier
  ///                                       rejection.
  ///
  KalmanFilterWrapper(
    const typename FilterT::MotionModel motion_model,
    const typename FilterT::NoiseModel noise_model,
    const typename FilterT::State::Matrix initial_state_covariance,
    const std::chrono::nanoseconds & expected_dt,
    const std::string & frame_id,
    const std::chrono::nanoseconds & history_duration = std::chrono::milliseconds{5000},
    common::types::float32_t mahalanobis_threshold =
    std::numeric_limits<common::types::float32_t>::max())
  : m_initial_covariance{initial_state_covariance},
    m_frame_id{frame_id},
    m_mahalanobis_threshold{mahalanobis_threshold},
    m_expected_prediction_period{expected_dt},
    m_filter{
      motion_model,
      noise_model,
      State{},
      initial_state_covariance,
    },
    m_history{
      m_filter,
      static_cast<std::size_t>(history_duration / expected_dt),
      m_mahalanobis_threshold} {}

  ///
  /// Reset the filter state using the default covariance and state derived from the measurement.
  ///
  /// @param[in]  measurement   The measurement from which we initialize the state.
  ///
  /// @tparam     MeasurementT  Type of measurement.
  ///
  template<typename MeasurementT>
  inline void add_reset_event_to_history(const MeasurementT & measurement)
  {
    add_reset_event_to_history(
      measurement.measurement.map_into(State{}),
      m_initial_covariance,
      measurement.timestamp);
  }

  ///
  /// Reset the filter state. This must be called at least once to start / tracking.
  ///
  /// @param[in]  state               The full state to set the system to.
  /// @param[in]  initial_covariance  The initial covariance.
  /// @param[in]  event_timestamp     The event timestamp. Ideally this should be in the same clock
  ///                                 as the one that timestamps the messages.
  ///
  inline void add_reset_event_to_history(
    const State & state,
    const typename State::Matrix & initial_covariance,
    const std::chrono::system_clock::time_point & event_timestamp)
  {
    m_history.emplace_event(event_timestamp, ResetEvent<FilterT>{state, initial_covariance});
    m_time_grid = SteadyTimeGrid{event_timestamp, m_expected_prediction_period};
  }

  ///
  /// Predict state of filter at the next timestep defined by the period of this node.
  ///
  /// @return     true if the update was successful and false otherwise. In case false is returned,
  ///             this update had no effect on the state of the filter.
  ///
  inline common::types::bool8_t add_next_temporal_update_to_history()
  {
    if (!is_initialized()) {return false;}
    const auto next_prediction_timestamp =
      m_time_grid.get_next_timestamp_after(m_history.get_last_timestamp());
    m_history.emplace_event(next_prediction_timestamp, PredictionEvent{});
    return true;
  }

  ///
  /// Update the filter state with a measurement.
  ///
  /// @param[in]  measurement            The measurement. It is expected to be a concrete
  ///                                    instantiation of the Measurement class.
  ///
  /// @tparam     MeasurementT           Measurement type that is a concrete template specialization
  ///                                    of the Measurement class.
  ///
  /// @return     true if the observation was successful, false otherwise. In case of an
  ///             unsuccessful update, the state of the underlying filter has not been changed.
  ///
  template<typename MeasurementT>
  common::types::bool8_t add_observation_to_history(const MeasurementT & measurement)
  {
    if (!is_initialized()) {return false;}
    m_history.emplace_event(measurement.timestamp, measurement.measurement);
    return true;
  }

  /// Check if the filter is is_initialized with a state.
  inline common::types::bool8_t is_initialized() const noexcept
  {
    return (!m_history.empty()) && m_time_grid.is_initialized();
  }

  /// Get the current state of the system as an odometry message.
  nav_msgs::msg::Odometry get_state() const;

private:
  /// Initial covariance of the filter.
  typename State::Matrix m_initial_covariance{};
  /// Time represented in a frame based on the last measurement timestamp.
  SteadyTimeGrid m_time_grid{};
  /// Frame in which the estimation happens, e.g. "odom".
  std::string m_frame_id{};
  /// The threshold on the Mahalanobis distance used to reject outliers.
  common::types::float32_t m_mahalanobis_threshold{};
  /// What duration passes between prediction events.
  std::chrono::nanoseconds m_expected_prediction_period{};
  /// Wrapper owns the filter implementation.
  FilterT m_filter{};
  /// History of all events is stored here.
  HistoryT m_history{};
};

using ConstantAccelerationFilterWrapperXY =
  KalmanFilterWrapper<ConstAccelerationKalmanFilterXY>;

using ConstantAccelerationFilterWrapperXYZRPY =
  KalmanFilterWrapper<ConstAccelerationKalmanFilterXYZRPY>;

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_


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

The Medical Imaging Interaction Toolkit (MITK)

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

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

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

===================================================================*/
#ifndef __itkOdfMaximaExtractionFilter_cpp
#define __itkOdfMaximaExtractionFilter_cpp

#include "itkOdfMaximaExtractionFilter.h"
#include <itkImageRegionIterator.h>
#include <itkContinuousIndex.h>

#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>

#include <boost/progress.hpp>
#include <boost/math/special_functions.hpp>
#include <vnl/vnl_det.h>
#include <vnl/vnl_trace.h>

using namespace boost::math;
using namespace std;

namespace itk {

template< class TOdfPixelType >
OdfMaximaExtractionFilter< TOdfPixelType >::OdfMaximaExtractionFilter():
    m_NormalizationMethod(MAX_VEC_NORM),
    m_PeakThreshold(0.2),
    m_MaxNumPeaks(10),
    m_ShCoeffImage(NULL),
    m_OutputFiberBundle(NULL),
    m_NumDirectionsImage(NULL),
    m_DirectionImageContainer(NULL)
{

}

// solve ax? + bx? + cx + d = 0 using cardanos method
template< class TOdfPixelType >
bool OdfMaximaExtractionFilter<TOdfPixelType>::ReconstructQballImage()
{
    if (m_ShCoeffImage.IsNotNull())
    {
        cout << "Using preset coefficient image\n";
        return true;
    }

    cout << "Starting qball reconstruction\n";
    try {
        QballReconstructionFilterType::Pointer filter = QballReconstructionFilterType::New();
        filter->SetGradientImage( m_DiffusionGradients, m_DiffusionImage );
        filter->SetBValue(m_Bvalue);
        filter->SetLambda(0.006);
        filter->SetNormalizationMethod(QballReconstructionFilterType::QBAR_SOLID_ANGLE);
        filter->Update();
        m_ShCoeffImage = filter->GetCoefficientImage();
        if (m_ShCoeffImage.IsNull())
            return false;
        return true;
    }
    catch (...)
    {
        return false;
    }
}

// solve ax³ + bx² + cx + d = 0 using cardanos method
template< class TOdfPixelType >
std::vector<double> OdfMaximaExtractionFilter< TOdfPixelType >
::SolveCubic(const double& a, const double& b, const double& c, const double& d)
{
    double A, B, p, q, r, D, offset, ee, tmp, root;
    vector<double> roots;
    double inv3 = 1.0/3.0;

    if (a!=0) // solve ax³ + bx² + cx + d = 0
    {
        p = b/a; q = c/a; r = d/a; // x³ + px² + qx + r = 0
        A = q-p*p*inv3;
        B = (2.0*p*p*p-9.0*p*q+27.0*r)/27.0;
        A = A*inv3;
        B = B*0.5;
        D = B*B+A*A*A;
        offset = p*inv3;

        if (D>0.0) // one real root
        {
            ee = sqrt(D);
            tmp = -B+ee;  root  = cbrt(tmp);
            tmp = -B-ee;  root += cbrt(tmp);
            root -= offset; roots.push_back(root);
        }
        else if (D<0.0) // three real roots
        {
            ee = sqrt(-D);
            double tmp2 = -B;
            double angle =  2.0*inv3*atan(ee/(sqrt(tmp2*tmp2+ee*ee)+tmp2));
            double sqrt3 = sqrt(3.0);
            tmp = cos(angle);
            tmp2 = sin(angle);
            ee = sqrt(-A);
            root = 2*ee*tmp-offset;             roots.push_back(root);
            root = -ee*(tmp+sqrt3*tmp2)-offset; roots.push_back(root);
            root = -ee*(tmp-sqrt3*tmp2)-offset; roots.push_back(root);
        }
        else // one or two real roots
        {
            tmp=-B;
            tmp=cbrt(tmp);
            root=2*tmp-offset;      roots.push_back(root);
            if (A!=0 || B!=0)
                root=-tmp-offset;   roots.push_back(root);
        }
    }
    else if (b!=0) // solve bx² + cx + d = 0
    {
        D = c*c-4*b*d;
        if (D>0)
        {
            tmp = sqrt(D);
            root = (-c+tmp)/(2.0*b); roots.push_back(root);
            root = (-c-tmp)/(2.0*b); roots.push_back(root);
        }
        else if (D==0)
            root = -c/(2.0*b); roots.push_back(root);
    }
    else if (c!=0) // solve cx + d = 0
        root = -d/c; roots.push_back(root);

    return roots;
}

template< class TOdfPixelType >
double OdfMaximaExtractionFilter< TOdfPixelType >
::ODF_dtheta(const double& sn, const double& cs, const double& A, const double& B, const double& C, const double& D, const double& E, const double& F, const double& G, const double& H)
{
    double dtheta=(G-7*E)*sn*sn + (7*F-35*D-H)*sn*cs + (H+C-F-3*A-5*D)*sn + (0.5*E+B+0.5*G)*cs -0.5*G+3.5*E;
    return dtheta;
}

template< class TOdfPixelType >
double OdfMaximaExtractionFilter< TOdfPixelType >
::ODF_dtheta2(const double& sn, const double& cs, const double& A, const double& B, const double& C, const double& D, const double& E, const double& F, const double& G, const double& H)
{
    double dtheta2=4*(G-7*E)*sn*cs + 2*(7*F-35*D-H)*(2*cs*cs-1) + 2*(H+C-F-3*A-5*D)*cs -(E+2*B+G)*sn;
    return dtheta2;
}

template< class TOdfPixelType >
double OdfMaximaExtractionFilter< TOdfPixelType >
::ODF_dphi2(const double& sn, const double& cs, const double& A, const double& B, const double& C, const double& D, const double& E, const double& F, const double& G, const double& H)
{
    double dphi2=35*D*((1+cs)*(1+cs)/4)+(3*A-30*D)*(1+cs)/2.0+3*D-A + 0.5*(7*E*(1+cs)/2.0-3*E+B)*sn + (7*F*(1+cs)/2+C-F)*(1-cs)/2.0 + G*sn*(1-cs)/4.0 + H*((1-cs)*(1-cs)/4);
    return dphi2;
}

template< class TOdfPixelType >
void OdfMaximaExtractionFilter< TOdfPixelType >
::FindCandidatePeaks(const CoefficientPixelType& SHcoeff)
{
    const double thr = 0.03;             // threshold on the derivative of the ODF with respect to theta
    const double phi_step = 0.005;       // step size for 1D exhaustive search on phi
    bool highRes;                        // when close to maxima increase resolution
    double mag, Y, Yp, sn, cs;
    double phi, dPhi;
    double A, B, C, D, E, F, G, H, Bp, Cp, Ep, Fp, Gp, Hp, Bs, Cs, Es, Fs, Gs, Hs;
    CoefficientPixelType a, ap;
    a = SHcoeff; ap = SHcoeff;

    m_CandidatePeaks.clear();   // clear peaks of last voxel

    for (int adaptiveStepwidth=0; adaptiveStepwidth<=1; adaptiveStepwidth++)
    {
    phi=0;
    while (phi<(2*M_PI)) // phi exhaustive search 0..pi
    {
        // calculate 4th order SH representtaion of ODF and according derivative
        for (int l=0; l<=4; l=l+2)
        {
            for (int m=-l; m<=l; m++)
            {
                int j=l*(l+1)/2+m;
                if (m<0)
                {
                    mag = sqrt(((2*l+1)/(2*M_PI))*factorial<double>(l+m)/factorial<double>(l-m));
                    Y = mag*cos(m*phi);
                    Yp = -m*mag*sin(m*phi);
                }
                else if (m==0)
                {
                    Y = sqrt((2*l+1)/(4*M_PI));
                    Yp = 0;
                }
                else
                {
                    mag = pow(-1.0,m)*sqrt(((2*l+1)/(2*M_PI))*factorial<double>(l-m)/factorial<double>(l+m));
                    Y = mag*sin(m*phi);
                    Yp = m*mag*cos(m*phi);
                }
                a[j] = SHcoeff[j]*Y;
                ap[j] = SHcoeff[j]*Yp;
            }
        }

        // ODF
        A=0.5*a[3]; B=-3*(a[2]+a[4]); C=3*(a[1]+a[5]); D=0.125*a[10]; E=-2.5*(a[9]+a[11]);
        F=7.5*(a[8]+a[12]); G=-105*(a[7]+a[13]); H=105*(a[6]+a[14]);

        // phi derivative
        Bp=-3*(ap[2]+ap[4]); Cp=3*(ap[1]+ap[5]); Ep=-2.5*(ap[9]+ap[11]);
        Fp=7.5*(ap[8]+ap[12]); Gp=-105*(ap[7]+ap[13]); Hp=105*(ap[6]+ap[14]);

        // 2phi derivative
        Bs=-B;    Cs=-4*C;  Es=-E;
        Fs=-4*F;  Gs=-9*G;  Hs=-16*H;

        // solve cubic for tan(theta)
        std::vector<double> tanTheta = SolveCubic(Hp+Cp-Fp, Gp+Bp-3*Ep, 6*Fp+Cp, Bp+4*Ep);

        highRes = false;
        dPhi = phi_step;

        //for each real cubic solution for tan(theta)
        for (int n=0; n<tanTheta.size(); n++)
        {
            double tmp = atan(tanTheta[n]); // arcus tangens of root (theta -pi/2..pi/2)
            double theta = floor(tmp/M_PI); // project theta to 0..pi ...
            theta = tmp - theta*M_PI;       // ... as the modulo of the division atan(tth[n])/M_PI

            sn = sin(2*theta); cs = cos(2*theta);
            tmp = ODF_dtheta(sn, cs, A, B, C, D, E, F, G, H);

            if (fabs(tmp) < thr) // second condition for maximum is true (theta derivative < eps)
            {
                //Compute the Hessian
                vnl_matrix_fixed< double, 2, 2 > hessian;
                hessian(0,0) = ODF_dtheta2(sn, cs, A, B, C, D, E, F, G, H);
                hessian(0,1) = ODF_dtheta(sn, cs, 0, Bp, Cp, 0, Ep, Fp, Gp, Hp);
                hessian(1,0) = hessian(0,1);
                hessian(1,1) = ODF_dphi2(sn, cs, 0, Bs, Cs, 0, Es, Fs, Gs, Hs);

                double det = vnl_det(hessian);  // determinant
                double tr = vnl_trace(hessian); // trace

                highRes = true; // we are close to a maximum, so turn on high resolution 1D exhaustive search
                if (det>=0 && tr<=0) // check if we really have a local maximum
                {
                    vnl_vector_fixed< double, 2 > peak;
                    peak[0] = theta;
                    peak[1] = phi;
                    m_CandidatePeaks.push_back(peak);
                }
            }

            if (adaptiveStepwidth) // calculate adaptive step width
            {
                double t2=tanTheta[n]*tanTheta[n];  double t3=t2*tanTheta[n]; double t4=t3*tanTheta[n];
                double const_step=phi_step*(1+t2)/sqrt(t2+t4+pow((((Hs+Cs-Fs)*t3+(Gs+Bs-3*Es)*t2+(6*Fs+Cs)*tanTheta[n]+(Bs+4*Es))/(3*(Hp+Cp-Fp)*t2+2*(Gp+Bp-3*Ep)*tanTheta[n]+(6*Fp+Cp))),2.0));
                if (const_step<dPhi)
                    dPhi=const_step;
            }
        }

        // update phi
        if (highRes)
            phi=phi+dPhi*0.5;
        else
            phi=phi+dPhi;
    }
    }
}

template< class TOdfPixelType >
std::vector< vnl_vector_fixed< double, 3 > > OdfMaximaExtractionFilter< TOdfPixelType >
::ClusterPeaks(const CoefficientPixelType& shCoeff)
{
    const double distThres = 0.4;
    int npeaks = 0, nMin = 0;
    double dMin, dPos, dNeg, d;
    Vector3D u;
    vector< Vector3D > v;

    // initialize container for vector clusters
    std::vector < std::vector< Vector3D > > clusters;
    clusters.resize(m_CandidatePeaks.size());

    for (int i=0; i<m_CandidatePeaks.size(); i++)
    {
        // calculate cartesian representation of peak
        u[0] = sin(m_CandidatePeaks[i](0))*cos(m_CandidatePeaks[i](1));
        u[1] = sin(m_CandidatePeaks[i](0))*sin(m_CandidatePeaks[i](1));
        u[2] = cos(m_CandidatePeaks[i](0));

        dMin = itk::NumericTraits<double>::max();
        for (int n=0; n<npeaks; n++) //for each other maximum v already visited
        {
            // euclidean distance from u/-u to other clusters
            dPos = vnl_vector_ssd(v[n],u);
            dNeg = vnl_vector_ssd(v[n],-u);
            d = std::min(dPos,dNeg);

            if ( d<dMin )
            {
                dMin = d; // adjust minimum
                nMin = n; // store its index
                if ( dNeg<dPos ) // flip u if neccesary
                    u=-u;
            }
        }
        if ( dMin<distThres ) // if u is very close to any other maximum v
        {
            clusters[nMin].push_back(u);  //store it with all other vectors that are close to v vector (with index nMin)
        }
        else // otherwise store u as output peak
        {
            v.push_back(u);
            npeaks++;
        }
    }

    // calculate mean vector of each cluster
    for (int i=0; i<m_CandidatePeaks.size(); i++)
        if ( !clusters[i].empty() )
        {
            v[i].fill(0.0);
            for (int vc=0; vc<clusters[i].size(); vc++)
                v[i]=v[i]+clusters[i][vc];
            v[i].normalize();
        }

    if (npeaks!=0)
    {
        // check the ODF amplitudes at each candidate peak
        vnl_matrix< double > shBasis, sphCoords;

        Cart2Sph(v, sphCoords);                 // convert candidate peaks to spherical angles
        shBasis = CalcShBasis(sphCoords, 4);  // evaluate spherical harmonics at each peak
        vnl_vector<double> odfVals(npeaks);
        odfVals.fill(0.0);
        double maxVal = itk::NumericTraits<double>::NonpositiveMin();
        int maxPos;
        for (int i=0; i<npeaks; i++) //compute the ODF value at each peak
        {
            for (int j=0; j<15; j++)
                odfVals(i) += shCoeff[j]*shBasis(i,j);

            if ( odfVals(i)>maxVal )
            {
                maxVal = odfVals(i);
                maxPos = i;
            }
        }
        v.clear();
        vector< double > restVals;
        for (int i=0; i<npeaks; i++) // keep only peaks with high enough amplitude and convert back to cartesian coordinates
            if ( odfVals(i)>=m_PeakThreshold*maxVal )
            {
                u[0] = odfVals(i)*cos(sphCoords(i,1))*sin(sphCoords(i,0));
                u[1] = odfVals(i)*sin(sphCoords(i,1))*sin(sphCoords(i,0));
                u[2] = odfVals(i)*cos(sphCoords(i,0));
                restVals.push_back(odfVals(i));
                v.push_back(u);
            }
        npeaks = v.size();

        if (npeaks>m_MaxNumPeaks) // if still too many peaks, keep only the m_MaxNumPeaks with maximum value
        {
            vector< Vector3D > v2;
            for (int i=0; i<m_MaxNumPeaks; i++)
            {
                maxVal = itk::NumericTraits<double>::NonpositiveMin();  //Get the maximum ODF peak value and the corresponding peak index
                for (int i=0; i<npeaks; i++)
                    if ( restVals[i]>maxVal )
                    {
                        maxVal = restVals[i];
                        maxPos = i;
                    }

                v2.push_back(v[maxPos]);
                restVals[maxPos] = 0;               //zero that entry in order to find the next maximum
            }
            return v2;
        }
    }
    return v;
}

// convert cartesian to spherical coordinates
template< class TOdfPixelType >
void OdfMaximaExtractionFilter< TOdfPixelType >
::Cart2Sph(const std::vector< Vector3D >& dir, vnl_matrix<double>& sphCoords)
{
    sphCoords.set_size(dir.size(), 2);

    for (int i=0; i<dir.size(); i++)
    {
        double mag = dir[i].magnitude();

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

// generate spherical harmonic values of the desired order for each input direction
template< class TOdfPixelType >
vnl_matrix<double> OdfMaximaExtractionFilter< TOdfPixelType >
::CalcShBasis(vnl_matrix<double>& sphCoords, const int& shOrder)
{
    int R = (shOrder+1)*(shOrder+2)/2;
    int M = sphCoords.rows();
    int j, m; double mag, plm;
    vnl_matrix<double> shBasis;
    shBasis.set_size(M,R);

    for (int p=0; p<M; p++)
    {
        j=0;
        for (int l=0; l<=shOrder; l=l+2)
            for (m=-l; m<=l; m++)
            {
                plm = legendre_p<double>(l,abs(m),cos(sphCoords(p,0)));
                mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;

                if (m<0)
                    shBasis(p,j) = sqrt(2.0)*mag*cos(fabs((double)m)*sphCoords(p,1));
                else if (m==0)
                    shBasis(p,j) = mag;
                else
                    shBasis(p,j) = pow(-1.0, m)*sqrt(2.0)*mag*sin(m*sphCoords(p,1));
                j++;
            }
    }
    return shBasis;
}

template< class TOdfPixelType >
void OdfMaximaExtractionFilter< TOdfPixelType >
::GenerateData()
{
    if (!ReconstructQballImage())
        return;

    std::cout << "Starting maxima extraction\n";

    switch (m_NormalizationMethod)
    {
    case NO_NORM:
        std::cout << "NO_NORM\n";
        break;
    case SINGLE_VEC_NORM:
        std::cout << "SINGLE_VEC_NORM\n";
        break;
    case MAX_VEC_NORM:
        std::cout << "MAX_VEC_NORM\n";
        break;
    }

    typedef ImageRegionConstIterator< CoefficientImageType > InputIteratorType;

    InputIteratorType git(m_ShCoeffImage, m_ShCoeffImage->GetLargestPossibleRegion() );

    itk::Vector<double,3> spacing = m_ShCoeffImage->GetSpacing();
    double minSpacing = spacing[0];
    if (spacing[1]<minSpacing)
        minSpacing = spacing[1];
    if (spacing[2]<minSpacing)
        minSpacing = spacing[2];

    mitk::Point3D origin = m_ShCoeffImage->GetOrigin();
    itk::Matrix<double, 3, 3> direction = m_ShCoeffImage->GetDirection();
    ImageRegion<3> imageRegion = m_ShCoeffImage->GetLargestPossibleRegion();

    // initialize num directions image
    m_NumDirectionsImage = ItkUcharImgType::New();
    m_NumDirectionsImage->SetSpacing( spacing );
    m_NumDirectionsImage->SetOrigin( origin );
    m_NumDirectionsImage->SetDirection( direction );
    m_NumDirectionsImage->SetRegions( imageRegion );
    m_NumDirectionsImage->Allocate();
    m_NumDirectionsImage->FillBuffer(0);

    vtkSmartPointer<vtkCellArray> m_VtkCellArray = vtkSmartPointer<vtkCellArray>::New();
    vtkSmartPointer<vtkPoints>    m_VtkPoints = vtkSmartPointer<vtkPoints>::New();

    m_DirectionImageContainer = ItkDirectionImageContainer::New();
    for (int i=0; i<m_MaxNumPeaks; i++)
    {
        itk::Vector< float, 3 > nullVec; nullVec.Fill(0.0);
        ItkDirectionImage::Pointer img = ItkDirectionImage::New();
        img->SetSpacing( spacing );
        img->SetOrigin( origin );
        img->SetDirection( direction );
        img->SetRegions( imageRegion );
        img->Allocate();
        img->FillBuffer(nullVec);
        m_DirectionImageContainer->InsertElement(m_DirectionImageContainer->Size(), img);
    }

    if (m_MaskImage.IsNull())
    {
        m_MaskImage = ItkUcharImgType::New();
        m_MaskImage->SetSpacing( spacing );
        m_MaskImage->SetOrigin( origin );
        m_MaskImage->SetDirection( direction );
        m_MaskImage->SetRegions( imageRegion );
        m_MaskImage->Allocate();
        m_MaskImage->FillBuffer(1);
    }

    itk::ImageRegionIterator<ItkUcharImgType> dirIt(m_NumDirectionsImage, m_NumDirectionsImage->GetLargestPossibleRegion());
    itk::ImageRegionIterator<ItkUcharImgType> maskIt(m_MaskImage, m_MaskImage->GetLargestPossibleRegion());

    int maxProgress = m_MaskImage->GetLargestPossibleRegion().GetSize()[0]*m_MaskImage->GetLargestPossibleRegion().GetSize()[1]*m_MaskImage->GetLargestPossibleRegion().GetSize()[2];

    boost::progress_display disp(maxProgress);

    git.GoToBegin();
    while( !git.IsAtEnd() )
    {
        ++disp;
        if (maskIt.Value()<=0)
        {
            ++git;
            ++dirIt;
            ++maskIt;
            continue;
        }

        CoefficientPixelType c = git.Get();
        FindCandidatePeaks(c);
        std::vector< Vector3D > directions = ClusterPeaks(c);

        typename CoefficientImageType::IndexType index = git.GetIndex();

        float max = 0.0;
        for (int i=0; i<directions.size(); i++)
            if (directions.at(i).magnitude()>max)
                max = directions.at(i).magnitude();
        if (max<0.0001)
            max = 1.0;

        for (int i=0; i<directions.size(); i++)
        {
            ItkDirectionImage::Pointer img = m_DirectionImageContainer->GetElement(i);
            itk::Vector< float, 3 > pixel;
            vnl_vector<double> dir = directions.at(i);

            vtkSmartPointer<vtkPolyLine> container = vtkSmartPointer<vtkPolyLine>::New();
            itk::ContinuousIndex<double, 3> center;
            center[0] = index[0];
            center[1] = index[1];
            center[2] = index[2];
            itk::Point<double> worldCenter;
            m_ShCoeffImage->TransformContinuousIndexToPhysicalPoint( center, worldCenter );

            switch (m_NormalizationMethod)
            {
            case NO_NORM:
                break;
            case SINGLE_VEC_NORM:
                dir.normalize();
                break;
            case MAX_VEC_NORM:
                dir /= max;
                break;
            }

            dir = m_MaskImage->GetDirection()*dir;
            pixel.SetElement(0, dir[0]);
            pixel.SetElement(1, dir[1]);
            pixel.SetElement(2, dir[2]);
            img->SetPixel(index, pixel);

            itk::Point<double> worldStart;
            worldStart[0] = worldCenter[0]-dir[0]/2 * minSpacing;
            worldStart[1] = worldCenter[1]-dir[1]/2 * minSpacing;
            worldStart[2] = worldCenter[2]-dir[2]/2 * minSpacing;
            vtkIdType id = m_VtkPoints->InsertNextPoint(worldStart.GetDataPointer());
            container->GetPointIds()->InsertNextId(id);
            itk::Point<double> worldEnd;
            worldEnd[0] = worldCenter[0]+dir[0]/2 * minSpacing;
            worldEnd[1] = worldCenter[1]+dir[1]/2 * minSpacing;
            worldEnd[2] = worldCenter[2]+dir[2]/2 * minSpacing;
            id = m_VtkPoints->InsertNextPoint(worldEnd.GetDataPointer());
            container->GetPointIds()->InsertNextId(id);
            m_VtkCellArray->InsertNextCell(container);
        }

        dirIt.Set(directions.size());

        ++git;
        ++dirIt;
        ++maskIt;
    }

    vtkSmartPointer<vtkPolyData> directionsPolyData = vtkSmartPointer<vtkPolyData>::New();
    directionsPolyData->SetPoints(m_VtkPoints);
    directionsPolyData->SetLines(m_VtkCellArray);
    m_OutputFiberBundle = mitk::FiberBundle::New(directionsPolyData);
    std::cout << "Maxima extraction finished\n";
}
}

#endif // __itkOdfMaximaExtractionFilter_cpp