#pragma once

#include <Eigen/Core>
#include <complex>
#include <cstddef>
#include <memory>
#include <utility>
#include <vector>
#include "ear/fft.hpp"

namespace ear {
  namespace dsp {

    namespace block_convolver_impl {
      /// Type for real data (float).
      using real_t = float;
      /// Type for complex data.
      using complex_t = std::complex<real_t>;

      using TDVector = Eigen::Matrix<real_t, Eigen::Dynamic, 1>;
      using FDVector = Eigen::Matrix<complex_t, Eigen::Dynamic, 1>;

      /** Static data required to perform convolution of a particular block
       * size; may be shared between any number of BlockConvolver and
       * BlockConvolver::Filter instances. */
      class Context {
       public:
        /** Create a Context with a given block size.
         * @param block_size Block size in samples.
         * @param fft_impl FFT implementation to use.
         */
        Context(size_t block_size, FFTImpl<real_t> &fft_impl);

       private:
        // number of samples in a block
        const size_t block_size;

        // fft of block_size * 2
        std::shared_ptr<FFTPlan<real_t>> fft;

        // time domain fft block size
        const size_t td_size;
        // frequency domain fft block size
        const size_t fd_size;

        friend class BlockConvolver;
        friend class Filter;
      };

      /** A filter response which may be shared between many BlockConvolver
       * instances.
       *
       * This stores the pre-transformed filter blocks. */
      class Filter {
       public:
        Filter(const std::shared_ptr<Context> &ctx, const TDVector &filter);
        Filter(const std::shared_ptr<Context> &ctx, size_t n,
               const real_t *filter);

        /** The number of blocks in the filter. */
        size_t num_blocks() const { return blocks.size(); }

       private:
        std::vector<FDVector> blocks;

        friend class BlockConvolver;
      };

      /** BlockConvolver implements partitioned overlap-add convolution with a
       * fixed block size, with efficient fading between filters.
       */
      class BlockConvolver {
       public:
        /** Create a BlockConvolver given the block size and number of blocks.
         * @param ctx Context required for transformations.
         * @param num_blocks Maximum number of blocks of any filter used.
         */
        BlockConvolver(const std::shared_ptr<Context> &ctx, size_t num_blocks);

        /** Create a BlockConvolver given the block size and number of blocks.
         *  If filter == nullptr, num_blocks must be specified.
         * @param ctx Context required for transformations.
         * @param filter Initial filter to be used, or nullptr for no filter.
         * @param num_blocks Maximum number of blocks of any filter used; using
         * 0 will take the number of blocks from the passed filter.
         */
        BlockConvolver(const std::shared_ptr<Context> &ctx,
                       const std::shared_ptr<const Filter> &filter,
                       size_t num_blocks = 0);

        /** Pass a block of audio through the filter.
         * @param in Input samples of length block_size
         * @param out Output samples of length block_size
         */
        void process(const Eigen::Ref<const TDVector> &in,
                     Eigen::Ref<TDVector> out);

        void process(const float *in, float *out);

        /** Crossfade to a new filter during the next block.
         *
         * This is equivalent to:
         * - Creating a new convolver.
         * - Passing the next block of samples through the old and new
         * convolvers, with the input to the old faded down across the block,
         * and the input to the new faded up across the block. All subsequent
         * blocks are passed through the new filter.
         * - Mixing the output of the old and new filters for the next
         * num_blocks blocks.
         *
         * @param filter Filter to crossfade to; should be alive for as long as
         * it is active. Pass nullptr for no filter.
         */
        void crossfade_filter(const std::shared_ptr<const Filter> &filter);

        /** Switch to a different filter at the start of the next block.
         * @param filter Filter to switch to; should be alive for as long as it
         * is active. Pass nullptr for no filter.
         */
        void set_filter(const std::shared_ptr<const Filter> &filter);

       private:
        // wrapper around an eigen type which knows if it contains only zeros
        template <typename T>
        struct ZeroTrack {
          template <typename... Args>
          ZeroTrack(Args &&...args) : data(std::forward<Args>(args)...) {
            clear();
          }
          T data;
          bool zero = false;
          /** Get access to the data for reading. */
          const T &read() { return data; }
          /** Get access to the data for writing; clears the zero flag. */
          T &write() {
            zero = false;
            return data;
          }
          /** Zero the buffer and set the zero flag. */
          void clear() {
            if (!zero) {
              zero = true;
              data.setZero();
            }
          }
        };

        std::shared_ptr<Context> ctx;
        std::unique_ptr<FFTWorkBuf> fft_work_buf;
        const size_t num_blocks;

        // check that a filter is valid for use with this convolver
        void check_filter(const std::shared_ptr<const Filter> &filter);

        // filters(i) accesses a circular buffer of filters, length num_blocks
        // + 1. on each frame, the input is crossfaded up and down, and passed
        // through
        // - filters[1:] (old)
        // - filters[:-1] (new)
        // After process, filters has been shifted one along, with filters(0)
        // left at filters(1); filters(0) is then set to filters(1), such that
        // the next filter is the same as the previous by default. set_filter
        // simply writes to filters(0).
        std::shared_ptr<const Filter> &filters(size_t i);
        // filter_queue and filter_ofs implement the above circular buffer.
        std::vector<std::shared_ptr<const Filter>> filter_queue;
        size_t filter_ofs;

        // a queue of spectra of the input, after zero padding on the right hand
        // side. If the filter is changed before the input block i frames ago,
        // spectra_old(i) contains the input faded down (to convolve with the
        // old filter) and spectra_new(i) contains the input faded up (to
        // convolve with the new filter). If the filter is not changed, only
        // spectra_new(i) contains data. num_blocks in length, each of size n+1.
        ZeroTrack<FDVector> &spectra_old(size_t i);
        ZeroTrack<FDVector> &spectra_new(size_t i);
        // implementation the above circular buffer
        std::vector<ZeroTrack<FDVector>> spectra_queue_old;
        std::vector<ZeroTrack<FDVector>> spectra_queue_new;
        size_t spectra_ofs;

        // Rotate the filter and spectra queues.
        void rotate_queues();

        // The second half of the ifft output for the last block, added to the
        // first half before output; size n.
        ZeroTrack<TDVector> last_tail;

        // Temporaries used by process to store the padded and faded inputs;
        // size 2n, the second half should always be zeros.
        ZeroTrack<TDVector> current_td_old;
        ZeroTrack<TDVector> current_td_new;
        // Temporary used by process to store the multiplied spectrum; size n+1.
        ZeroTrack<FDVector> multiply_out;
        // Temporary used to store the time domain output, size 2n.
        ZeroTrack<TDVector> out_td;
      };

    }  // namespace block_convolver_impl
  }  // namespace dsp
}  // namespace ear


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "Utils/Math/QuaternionFit.h"
#include "Utils/Geometry/ElementInfo.h"
#include "Utils/Typenames.h"
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <utility>

using namespace Eigen;

namespace Scine {
namespace Utils {

Eigen::VectorXd QuaternionFit::makeWeightsVector(const ElementTypeCollection& elements) {
  Eigen::VectorXd vec(elements.size());
  for (int i = 0; i < elements.size(); ++i) {
    vec(i) = ElementInfo::mass(elements[i]);
  }
  return vec;
}

void QuaternionFit::align() {
  /*
   * Translation into origin
   */
  // multiply positions with weights colwise: mat.array().colwise()*weights.array()
  // sum into vector and divide: (...).sum() / weights.sum()
  refCenter_ = (refMat_.array().colwise() * weights_.array()).colwise().sum() / weights_.sum();
  fitCenter_ = (fitMat_.array().colwise() * weights_.array()).colwise().sum() / weights_.sum();
  fittedMat_ = fitMat_.rowwise() - fitCenter_.transpose();

  /*
   * Rotation
   */
  Eigen::Matrix4d b = Eigen::Matrix4d::Zero();
  // generate decomposable matrix per atom and add them
  for (int i = 0; i < fitMat_.rows(); i++) {
    Eigen::Vector3d fitPos = fittedMat_.row(i);
    Eigen::Vector3d refPos = refMat_.row(i) - refCenter_.transpose();
    Eigen::Matrix4d a = Eigen::Matrix4d::Zero();
    a.block(0, 1, 1, 3) = (fitPos - refPos).transpose();
    a.block(1, 0, 3, 1) = refPos - fitPos;
    a.block(1, 1, 3, 3) = Eigen::Matrix3d::Identity().rowwise().cross(refPos + fitPos);
    b += a.transpose() * a * weights_[i];
  }

  // Decompose b
  SelfAdjointEigenSolver<Matrix4d> eigensolver(b);

  // Apply rotation
  //   If the eigenvalue of the last eigenvector is larger (absolute) than that of the first one
  //   it is beneficial to allow a rotation including inversion, as this will lead to the better
  //   fit.
  if (improperRotationIsAllowed_ && fabs(eigensolver.eigenvalues()[0]) < fabs(eigensolver.eigenvalues()[3])) {
    const Eigen::Vector4d& q = eigensolver.eigenvectors().col(3);
    rotMat_ = -Quaterniond(q[0], q[1], q[2], q[3]).toRotationMatrix();
    fittedMat_ = (rotMat_ * fittedMat_.transpose()).transpose();
    maxEigenvalue_ = eigensolver.eigenvalues()[3];
  }
  else {
    const Eigen::Vector4d& q = eigensolver.eigenvectors().col(0);
    rotMat_ = Quaterniond(q[0], q[1], q[2], q[3]).toRotationMatrix();
    fittedMat_ = (rotMat_ * fittedMat_.transpose()).transpose();
    maxEigenvalue_ = eigensolver.eigenvalues()[0];
  }

  /*
   * Translation onto reference center
   */
  fittedMat_ = fittedMat_.rowwise() + refCenter_.transpose();
}

double QuaternionFit::getRMSD() const {
  // collect squared norms: ((refMat_-fittedMat_).rowwise().squaredNorm()
  // sum, divide and sqrt: std::sqrt( (...).sum() / refMat_.rows() )
  return std::sqrt((refMat_ - fittedMat_).rowwise().squaredNorm().sum() / refMat_.rows());
}

double QuaternionFit::getWeightedRMSD(const Eigen::VectorXd& weights) const {
  // collect sqared norms: ((refMat_-fittedMat_).rowwise().squaredNorm()
  // multiply with corresponding weight: (...).array()*weights.array()
  // sum, divide and sqrt: std::sqrt( (...).sum() / refMat_.rows() )
  return std::sqrt(((refMat_ - fittedMat_).rowwise().squaredNorm().array() * weights.array()).sum() / refMat_.rows());
}

double QuaternionFit::getWeightedRMSD() const {
  return std::move(getWeightedRMSD(weights_));
}

Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> QuaternionFit::getFittedData() const {
  return fittedMat_;
}

Eigen::Matrix3d QuaternionFit::getRotationMatrix() const {
  return rotMat_.transpose();
}

Eigen::Vector3d QuaternionFit::getTransVector() const {
  return fitCenter_ - refCenter_;
}

double QuaternionFit::getRotRMSD() const {
  double rotRMSD = (fitMat_.rowwise() - fitCenter_.transpose()).rowwise().squaredNorm().sum();
  rotRMSD += (refMat_.rowwise() - refCenter_.transpose()).rowwise().squaredNorm().sum();
  rotRMSD -= 2.0 * abs(maxEigenvalue_);
  if (rotRMSD > 0)
    return sqrt(rotRMSD / refMat_.rows());
  return 0.0;
}

} /* namespace Utils */
} /* namespace Scine */


/** @file
 *
 * This file is part of Boost, not part of Chaste per se.
 *
 * We use the <autogenerated> tag to ignore it from our Doxygen checker.
 *
 * This file is provided to users of Boost up to 1.63 inclusive and gives forward compatibility with
 * Boost 1.64-1.65 and hopefully beyond.
 * (An optimisation in exponential distributions was applied to boost in version 1.64,
 * which also affects normal distribution tails.)
 *
 * Minimal changes were made to the file to include it here, simply giving the class a unique name
 * and making it use the Chaste copy of exponential_distribution rather than the boost one.
 *
 */

/* boost random/gamma_distribution.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 */

#ifndef BOOST_165_RANDOM_GAMMA_DISTRIBUTION_HPP
#define BOOST_165_RANDOM_GAMMA_DISTRIBUTION_HPP

#include <boost/assert.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/config.hpp>
//#include <boost/random/exponential_distribution.hpp> // swapped for chaste copy of exponential v1.65
#include <boost/static_assert.hpp>
#include <iosfwd>
#include <istream>
#include "Boost165ExponentialDistribution.hpp"

namespace boost
{
namespace random
{

    // The algorithm is taken from Knuth

    /**
 * The gamma distribution is a continuous distribution with two
 * parameters alpha and beta.  It produces values > 0.
 *
 * It has
 * \f$\displaystyle p(x) = x^{\alpha-1}\frac{e^{-x/\beta}}{\beta^\alpha\Gamma(\alpha)}\f$.
 */
    template <class RealType = double>
    class gamma_distribution_v165
    {
    public:
        typedef RealType input_type;
        typedef RealType result_type;

        class param_type
        {
        public:
            typedef gamma_distribution_v165 distribution_type;

            /**
         * Constructs a @c param_type object from the "alpha" and "beta"
         * parameters.
         *
         * Requires: alpha > 0 && beta > 0
         */
            param_type(const RealType& alpha_arg = RealType(1.0),
                       const RealType& beta_arg = RealType(1.0))
                    : _alpha(alpha_arg), _beta(beta_arg)
            {
            }

            /** Returns the "alpha" parameter of the distribution. */
            RealType alpha() const { return _alpha; }
            /** Returns the "beta" parameter of the distribution. */
            RealType beta() const { return _beta; }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
            /** Writes the parameters to a @c std::ostream. */
            template <class CharT, class Traits>
            friend std::basic_ostream<CharT, Traits>&
            operator<<(std::basic_ostream<CharT, Traits>& os,
                       const param_type& parm)
            {
                os << parm._alpha << ' ' << parm._beta;
                return os;
            }

            /** Reads the parameters from a @c std::istream. */
            template <class CharT, class Traits>
            friend std::basic_istream<CharT, Traits>&
            operator>>(std::basic_istream<CharT, Traits>& is, param_type& parm)
            {
                is >> parm._alpha >> std::ws >> parm._beta;
                return is;
            }
#endif

            /** Returns true if the two sets of parameters are the same. */
            friend bool operator==(const param_type& lhs, const param_type& rhs)
            {
                return lhs._alpha == rhs._alpha && lhs._beta == rhs._beta;
            }
            /** Returns true if the two sets fo parameters are different. */
            friend bool operator!=(const param_type& lhs, const param_type& rhs)
            {
                return !(lhs == rhs);
            }

        private:
            RealType _alpha;
            RealType _beta;
        };

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
        BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

        /**
     * Creates a new gamma_distribution with parameters "alpha" and "beta".
     *
     * Requires: alpha > 0 && beta > 0
     */
        explicit gamma_distribution_v165(const result_type& alpha_arg = result_type(1.0),
                                         const result_type& beta_arg = result_type(1.0))
                : _exp(result_type(1)), _alpha(alpha_arg), _beta(beta_arg)
        {
            BOOST_ASSERT(_alpha > result_type(0));
            BOOST_ASSERT(_beta > result_type(0));
            init();
        }

        /** Constructs a @c gamma_distribution from its parameters. */
        explicit gamma_distribution_v165(const param_type& parm)
                : _exp(result_type(1)), _alpha(parm.alpha()), _beta(parm.beta())
        {
            init();
        }

        // compiler-generated copy ctor and assignment operator are fine

        /** Returns the "alpha" paramter of the distribution. */
        RealType alpha() const { return _alpha; }
        /** Returns the "beta" parameter of the distribution. */
        RealType beta() const { return _beta; }
        /** Returns the smallest value that the distribution can produce. */
        RealType min BOOST_PREVENT_MACRO_SUBSTITUTION() const { return 0; }
        /* Returns the largest value that the distribution can produce. */
        RealType max BOOST_PREVENT_MACRO_SUBSTITUTION() const
        {
            return (std::numeric_limits<RealType>::infinity)();
        }

        /** Returns the parameters of the distribution. */
        param_type param() const { return param_type(_alpha, _beta); }
        /** Sets the parameters of the distribution. */
        void param(const param_type& parm)
        {
            _alpha = parm.alpha();
            _beta = parm.beta();
            init();
        }

        /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
        void reset() { _exp.reset(); }

        /**
     * Returns a random variate distributed according to
     * the gamma distribution.
     */
        template <class Engine>
        result_type operator()(Engine& eng)
        {
#ifndef BOOST_NO_STDC_NAMESPACE
            // allow for Koenig lookup
            using std::tan;
            using std::sqrt;
            using std::exp;
            using std::log;
            using std::pow;
#endif
            if (_alpha == result_type(1))
            {
                return _exp(eng) * _beta;
            }
            else if (_alpha > result_type(1))
            {
                // Can we have a boost::mathconst please?
                const result_type pi = result_type(3.14159265358979323846);
                for (;;)
                {
                    result_type y = tan(pi * uniform_01<RealType>()(eng));
                    result_type x = sqrt(result_type(2) * _alpha - result_type(1)) * y
                        + _alpha - result_type(1);
                    if (x <= result_type(0))
                        continue;
                    if (uniform_01<RealType>()(eng) > (result_type(1) + y * y) * exp((_alpha - result_type(1))
                                                                                * log(x / (_alpha - result_type(1)))
                                                                                - sqrt(result_type(2) * _alpha
                                                                                - result_type(1))
                                                                                * y))
                        continue;
                    return x * _beta;
                }
            }
            else /* alpha < 1.0 */
            {
                for (;;)
                {
                    result_type u = uniform_01<RealType>()(eng);
                    result_type y = _exp(eng);
                    result_type x, q;
                    if (u < _p)
                    {
                        x = exp(-y / _alpha);
                        q = _p * exp(-x);
                    }
                    else
                    {
                        x = result_type(1) + y;
                        q = _p + (result_type(1) - _p) * pow(x, _alpha - result_type(1));
                    }
                    if (u >= q)
                        continue;
                    return x * _beta;
                }
            }
        }

        template <class URNG>
        RealType operator()(URNG& urng, const param_type& parm) const
        {
            return gamma_distribution_v165(parm)(urng);
        }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
        /** Writes a @c gamma_distribution_v165 to a @c std::ostream. */
        template <class CharT, class Traits>
        friend std::basic_ostream<CharT, Traits>&
        operator<<(std::basic_ostream<CharT, Traits>& os,
                   const gamma_distribution_v165& gd)
        {
            os << gd.param();
            return os;
        }

        /** Reads a @c gamma_distribution from a @c std::istream. */
        template <class CharT, class Traits>
        friend std::basic_istream<CharT, Traits>&
        operator>>(std::basic_istream<CharT, Traits>& is, gamma_distribution_v165& gd)
        {
            gd.read(is);
            return is;
        }
#endif

        /**
     * Returns true if the two distributions will produce identical
     * sequences of random variates given equal generators.
     */
        friend bool operator==(const gamma_distribution_v165& lhs,
                               const gamma_distribution_v165& rhs)
        {
            return lhs._alpha == rhs._alpha
                && lhs._beta == rhs._beta
                && lhs._exp == rhs._exp;
        }

        /**
     * Returns true if the two distributions can produce different
     * sequences of random variates, given equal generators.
     */
        friend bool operator!=(const gamma_distribution_v165& lhs,
                               const gamma_distribution_v165& rhs)
        {
            return !(lhs == rhs);
        }

    private:
        /// \cond hide_private_members

        template <class CharT, class Traits>
        void read(std::basic_istream<CharT, Traits>& is)
        {
            param_type parm;
            if (is >> parm)
            {
                param(parm);
            }
        }

        void init()
        {
#ifndef BOOST_NO_STDC_NAMESPACE
            // allow for Koenig lookup
            using std::exp;
#endif
            _p = exp(result_type(1)) / (_alpha + exp(result_type(1)));
        }
        /// \endcond

        exponential_distribution_v165<RealType> _exp;
        result_type _alpha;
        result_type _beta;
        // some data precomputed from the parameters
        result_type _p;
    };

} // namespace random

} // namespace boost

#endif // BOOST_165_RANDOM_GAMMA_DISTRIBUTION_HPP


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

#ifndef BOOST_MATH_DISTRIBUTIONS_DETAIL_HG_CDF_HPP
#define BOOST_MATH_DISTRIBUTIONS_DETAIL_HG_CDF_HPP

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/distributions/detail/hypergeometric_pdf.hpp>

namespace boost{ namespace math{ namespace detail{

   template <class T, class Policy>
   T hypergeometric_cdf_imp(unsigned x, unsigned r, unsigned n, unsigned N, bool invert, const Policy& pol)
   {
#ifdef BOOST_MSVC
#  pragma warning(push)
#  pragma warning(disable:4267)
#endif
      BOOST_MATH_STD_USING
      T result = 0;
      T mode = floor(T(r + 1) * T(n + 1) / (N + 2));
      if(x < mode)
      {
         result = hypergeometric_pdf<T>(x, r, n, N, pol);
         T diff = result;
         unsigned lower_limit = static_cast<unsigned>((std::max)(0, (int)(n + r) - (int)(N)));
         while(diff > (invert ? T(1) : result) * tools::epsilon<T>())
         {
            diff = T(x) * T((N + x) - n - r) * diff / (T(1 + n - x) * T(1 + r - x));
            result += diff;
            BOOST_MATH_INSTRUMENT_VARIABLE(x);
            BOOST_MATH_INSTRUMENT_VARIABLE(diff);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
            if(x == lower_limit)
               break;
            --x;
         }
      }
      else
      {
         invert = !invert;
         unsigned upper_limit = (std::min)(r, n);
         if(x != upper_limit)
         {
            ++x;
            result = hypergeometric_pdf<T>(x, r, n, N, pol);
            T diff = result;
            while((x <= upper_limit) && (diff > (invert ? T(1) : result) * tools::epsilon<T>()))
            {
               diff = T(n - x) * T(r - x) * diff / (T(x + 1) * T((N + x + 1) - n - r));
               result += diff;
               ++x;
               BOOST_MATH_INSTRUMENT_VARIABLE(x);
               BOOST_MATH_INSTRUMENT_VARIABLE(diff);
               BOOST_MATH_INSTRUMENT_VARIABLE(result);
            }
         }
      }
      if(invert)
         result = 1 - result;
      return result;
#ifdef BOOST_MSVC
#  pragma warning(pop)
#endif
   }

   template <class T, class Policy>
   inline T hypergeometric_cdf(unsigned x, unsigned r, unsigned n, unsigned N, bool invert, const Policy&)
   {
      BOOST_FPU_EXCEPTION_GUARD
      typedef typename tools::promote_args<T>::type result_type;
      typedef typename policies::evaluation<result_type, Policy>::type value_type;
      typedef typename policies::normalise<
         Policy,
         policies::promote_float<false>,
         policies::promote_double<false>,
         policies::discrete_quantile<>,
         policies::assert_undefined<> >::type forwarding_policy;

      value_type result;
      result = detail::hypergeometric_cdf_imp<value_type>(x, r, n, N, invert, forwarding_policy());
      if(result > 1)
      {
         result  = 1;
      }
      if(result < 0)
      {
         result = 0;
      }
      return policies::checked_narrowing_cast<result_type, forwarding_policy>(result, "boost::math::hypergeometric_cdf<%1%>(%1%,%1%,%1%,%1%)");
   }

}}} // namespaces

#endif


////////////////////////////////////////////////////////////////////////////////
// Exact Diagonalization: Kitaev Model, Honeycomb Lattice, 2x2 unit cells     //
// cylindrical boundary conditions
// Copyright (C) 2016 by Tim Eschmann
////////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <fstream>
#include <math.h>
#include <complex>
//#define ARMA_64BIT_WORD
#include <armadillo>

using namespace arma;
using namespace std;

// Flip k1-th and k2-th bit of integer number
int bitflip(int *v, int k1, int k2)
{
    int erg;
    erg = *v;
    // Flip bits:
    erg ^= 1 << k1;
    erg ^= 1 << k2;
    return erg;
}

// Calculate diagonal element (periodic boundary conditions)
int diagel(int *w)
{
    int b, b0, b1, b2, b3, b4, b5, b6, b7;
    signed int s1, s2, s3, s4;

    b = *w;
    // Check bits of states
    b0 = (b >> 0) & 1; // 0th bit and so on ...
    b1 = (b >> 1) & 1;
    b2 = (b >> 2) & 1;
    b3 = (b >> 3) & 1;
    b4 = (b >> 4) & 1;
    b5 = (b >> 5) & 1;
    b6 = (b >> 6) & 1;
    b7 = (b >> 7) & 1;

    if (b0 == b4)
        s1 = 1;
    else
        s1 = -1;

    if (b5 == b1)
        s2 = 1;
    else
        s2 = -1;

    if (b2 == b6)
        s3 = 1;
    else
        s3 = -1;

    if (b7 == b3)
        s4 = 1;
    else
        s4 = -1;

    return s1 + s2 + s3 + s4;
}

// Sign check for yy interactions
int signcheck(int *x, int var1, int var2)
{
    int c, c1, c2;
    signed int s;
    c = *x;
    c1 = (c >> var1) & 1;
    c2 = (c >> var2) & 1;

    if (c1 == c2)
        s = 1;
    else
        s = -1;

    return s;
}

// Main function
int main(void)
{
    int i,j,k,l,m,p; // running indices
    int steps = 100; // How many measurement steps?
    int Jx, Jy, Jz; // coupling constants
    int N = 8; //  number of sites
    int size = 256; // matrix size
    double Z, E; // partition sum, energy value

    int xflips[3], yflips[3]; // Arrays for spin flips, size depending on # of bonds
    signed int si0, si1, si2;

    vec ev;
    double temp[steps];
    double energy[steps];

    mat ham = mat(size,size, fill::zeros); // Initialize matrix

    Jx = 1;
    Jy = 1;
    Jz = 1;

    cout << "Calculating matrix ..." << endl;

    // Fill Hamiltonian matrix with elements
    for (j = 0; j < size; j++)
    {
        // Diagonal element
        ham(j,j) = -Jz * diagel(&j);
        // Calculate interaction states via spin flips
        // xx:
        xflips[0] = bitflip(&j, 1, 2);
        xflips[1] = bitflip(&j, 4, 5);
        xflips[2] = bitflip(&j, 6, 7);

        //yy:
        yflips[0] = bitflip(&j, 0, 1);
        yflips[1] = bitflip(&j, 2, 3);
        yflips[2] = bitflip(&j, 5, 6);

        // signs for yy-interaction matrix elements
        si0 = signcheck(&j, 0, 1);
        si1 = signcheck(&j, 2, 3);
        si2 = signcheck(&j, 5, 6);

        ham(j,xflips[0]) = Jx;
        ham(j,xflips[1]) = Jx;
        ham(j,xflips[2]) = Jx;

        ham(xflips[0], j) = -Jx;
        ham(xflips[1], j) = -Jx;
        ham(xflips[2], j) = -Jx;

        ham(j,yflips[0]) = Jy*si0;
        ham(j,yflips[1]) = Jy*si1;
        ham(j,yflips[2]) = Jy*si2;

        ham(yflips[0], j) = -Jy*si0;
        ham(yflips[1], j) = -Jy*si1;
        ham(yflips[2], j) = -Jy*si2;

    }

    cout << "Diagonalizing ... " << endl;

    // Compute eigenvalues of ham:
    ev = eig_sym(ham);
    ev.save("eigenvalues.mat", csv_ascii);

    cout << "Calculating energy per temperature ... " << endl;

    // Calculating energy per temperature (with Boltzmann weights):
    for (l = 0; l < steps; l++)
    {
        Z = 0;
        E = 0;
        //temp[l] = 0.000001 + l*0.01;
        temp[l] = pow(10,-2+(3*l/float(steps))); //logspace

        for (m = 0; m < size; m++)
        {
            Z = Z + exp(-ev[m]/temp[l]);
            E = E + ev[m]*exp(-ev[m]/temp[l]);
        }
        energy[l] = E/(N*Z);
    }

    // Write measured data to text file:
    std::fstream f("dataexd.txt", std::ios::out);
    for (int i = 0; i < steps; ++i)
    {
        if (i == 0 || i == steps - 1)
            f << temp[i] << " " << energy[i] << " " << 0 << "\n";
        else
            f << temp[i] << " " << energy[i] << " " << (energy[i+1] - energy[i-1])/(2*(temp[i+1] - temp[i-1])) << "\n";
    }
    f.close();

    return 0;

}


#ifndef __CRUNCHY__
#define __CRUNCHY__

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/algorithm/string.hpp>

#include <vector>

#include "result.hpp"

namespace ipaddress {
/**
 * based
 * Crunch - Arbitrary-precision integer arithmetic library
 * Copyright (C) 2014 Nenad Vukicevic crunch.secureroom.net/license
 *
 */
/**
 * @module Crunch
 * Radix: 28 bits
 * Endianness: Big
 *
 * @param {boolean} rawIn   - expect 28-bit arrays
 * @param {boolean} rawOut  - return 28-bit arrays
 */
// import Crunch from './crunch';

class NotImplementedException : public std::exception
{
public:
    NotImplementedException() {}
    ~NotImplementedException() {}
    virtual char const * what() const throw() { return "Function not yet implemented."; }
};

class Crunchy {
  // we need for ipv6 129 bits
  typedef boost::multiprecision::uint256_t CrunchyType;
  CrunchyType num;
public:
  Crunchy clone() const {
    Crunchy ret;
    ret.num = this->num;
    return ret;
  }

  static Crunchy from_8bit(const std::vector<unsigned char> &number) {
    Crunchy ret;
    for (auto i : number) {
        ret.num = (ret.num << 8) + CrunchyType(i);
    }
    return ret;
  }

  static Result<Crunchy> parse(const std::string &val) {
    return Crunchy::from_string(val, 10);
  }

  static Crunchy from_number(size_t num)  {
    Crunchy ret;
    ret.num = CrunchyType(num);
    return ret;
  }
  static Result<Crunchy> from_string(const std::string &val, size_t radix = 10) {
  Crunchy ret;
  auto f(val.begin());
  auto l(val.end());
  if (radix == 10) {
    boost::spirit::qi::int_parser<CrunchyType, 10, 0, 39> uint256_dec;
    if (!boost::spirit::qi::parse(f, l, uint256_dec, ret.num)) {
        return Err<Crunchy>("inpossible to parse");
    }
  } else if (radix == 16) {
    boost::spirit::qi::int_parser<CrunchyType, 16, 0, 39> uint256_hex;
    if (!boost::spirit::qi::parse(f, l, uint256_hex, ret.num)) {
        return Err<Crunchy>("inpossible to parse");
    }
  } else {
    throw NotImplementedException();
  }
  return Ok(ret);
  }

  std::vector<unsigned char> to_8bit() const {
    std::vector<unsigned char> ret;
    auto my = this->num;
    // std::cout << "to_8bit:" << std::hex << my << std::endl;
    do {
        ret.push_back(static_cast<size_t>(my&0xff));
        my = my >> 8;
    } while (my != 0);
    std::reverse(ret.begin(),ret.end());
    // std::stringstream s2;
    // for (auto i : ret) {
    //     s2 << " " << std::hex << static_cast<size_t>(i);
    // }
    // std::cout << "to_8bit:" << s2.str() << std::endl;
    return ret;
  }

  int compare(const Crunchy &y) const {
    if (this->num < y.num) {
      // std::cout << "-1=" << this->num << "---" << y.num << std::endl;
      return -1;
    } else if (this->num > y.num) {
      // std::cout << "1=" << this->num << "---" << y.num << std::endl;
      return 1;
    }
    // std::cout << "0=" << this->num << "---" << y.num << std::endl;
    return 0;
  }

  bool eq(const Crunchy &oth) const {
    return this->compare(oth) == 0;
  }

  bool lte(const Crunchy &oth) const {
    return this->compare(oth) <= 0;
  }
  bool lt(const Crunchy &oth) const {
    return this->compare(oth) < 0;
  }

  bool gt(const Crunchy &oth) const {
    return this->compare(oth) > 0;
  }
  bool gte(const Crunchy &oth) const {
    return this->compare(oth) >= 0;
  }

  Crunchy add(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num + oth.num;
    return ret;
  }

  Crunchy sub(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num - oth.num;
    return ret;
  }
  Crunchy mul(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num * oth.num;
    return ret;
  }

  Crunchy shr(size_t s) const {
    Crunchy ret;
    ret.num = this->num >> s;
    return ret;
  }

  Crunchy shl(size_t s) const {
    Crunchy ret;
    ret.num = this->num << s;
    // std::cout << "shl:" << std::hex << this->num << ":" << s << ":" << ret.num << std::endl;
    return ret;
  }

  Crunchy div(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num / oth.num;
    return ret;
  }
  // public sub(cry: Crunchy): Crunchy {
  //   return Crunchy.from_8bit(Crunch.sub(this->num, cry.num));
  // }

  Crunchy mod(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num % oth.num;
    return ret;
  }

  size_t mds(size_t n) const {
    return static_cast<size_t>(this->num % n);
  }

  std::string toString(size_t radix = 10) const {
    std::stringstream s2;
    if (radix == 10) {
      s2 << std::dec;
      s2 << this->num;
      return s2.str();
    } else if (radix == 16) {
      s2 << std::hex;
      s2 << this->num;
      auto ret = s2.str();
      boost::algorithm::to_lower(ret);
      return ret;
    } else if (radix == 2) {
      std::vector<size_t> bits;
      auto my = this->num;
      do {
        size_t tmp = static_cast<size_t>(my&0x1);
        bits.push_back(tmp);
        my = my >> 1;
      } while(my != 0);
      for (int i = bits.size()-1; i >= 0; --i) {
        s2 << bits[i];
      }
      return s2.str();
    } else {
      throw NotImplementedException();
    }
  }

  static const Crunchy &zero();
  static const Crunchy &one();
  static const Crunchy &two();

};

std::ostream& operator<<(std::ostream &o, const Crunchy &crunchy);

}

#endif


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

typedef double real;
typedef std::complex<real> complex;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<real> vector;
    typedef ublas::matrix<real> matrix;
    typedef vector::size_type size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<real>::get();
    std::cout << "ublas using vectors : nrm2(v) = " << ublas::norm_2(v) << '\n'
	      << "blas using vectors  : nrm2(v) = " << blas::nrm2(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    mc=v;
    std::cout << "blas using cols     : nrm2(v) = " << blas::nrm2(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : nrm2(v) = " << blas::nrm2(mr) << '\n';
  }
  {
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<complex>::get();
    std::cout << "ublas using vectors : nrm2(v) = " << ublas::norm_2(v) << '\n'
	      << "blas using vectors  : nrm2(v) = " << blas::nrm2(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    mc=v;
    std::cout << "blas using cols     : nrm2(v) = " << blas::nrm2(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : nrm2(v) = " << blas::nrm2(mr) << '\n';
  }
  {
    typedef Eigen::Matrix<real, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<real>::get();
    std::cout << "eigen using vectors : nrm2(v) = " << v.norm() << '\n'
	      << "blas using vectors  : nrm2(v) = " << blas::nrm2(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    mc=v;
    std::cout << "blas using cols     : nrm2(v) = " << blas::nrm2(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : nrm2(v) = " << blas::nrm2(mr) << '\n';
  }
  {
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<complex>::get();
    std::cout << "eigen using vectors : nrm2(v) = " << v.norm() << '\n'
	      << "blas using vectors  : nrm2(v) = " << blas::nrm2(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    mc=v;
    std::cout << "blas using cols     : nrm2(v) = " << blas::nrm2(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : nrm2(v) = " << blas::nrm2(mr) << '\n';
  }
  return EXIT_SUCCESS;
}


#pragma once

#include <polyfem/Common.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>

#include <polyfem/AutodiffTypes.hpp>

#include <Eigen/Dense>
#include <functional>

namespace polyfem
{
	class IncompressibleLinearElasticityDispacement
	{
	public:
		// res is R^{dim²}
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 9, 1>
		assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void set_size(const int size);
		inline int size() const { return size_; }

		inline double &mu() { return mu_; }
		inline double mu() const { return mu_; }

		inline double &lambda() { return lambda_; }
		inline double lambda() const { return lambda_; }

		void set_parameters(const json &params);

		void compute_von_mises_stresses(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &stresses) const;
		void compute_stress_tensor(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &tensor) const;
	private:
		int size_ = -1;
		double mu_ = 1;
		double lambda_ = 1;

		void assign_stress_tensor(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, const int all_size, Eigen::MatrixXd &all, const std::function<Eigen::MatrixXd(const Eigen::MatrixXd &)> &fun) const;
	};

	class IncompressibleLinearElasticityMixed
	{
	public:
		// res is R^{dim}
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		assemble(const ElementAssemblyValues &psi_vals, const ElementAssemblyValues &phi_vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void set_size(const int size);

		inline double &mu() { return mu_; }
		inline double mu() const { return mu_; }

		inline double &lambda() { return lambda_; }
		inline double lambda() const { return lambda_; }

		inline int rows() const { return size_; }
		inline int cols() const { return 1; }

		void set_parameters(const json &params);
	private:
		int size_ = -1;
		double mu_ = 1;
		double lambda_ = 1;
	};

	class IncompressibleLinearElasticityPressure
	{
	public:
		// res is R^{1}
		Eigen::Matrix<double, 1, 1>
		assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, 1, 1>
		compute_rhs(const AutodiffHessianPt &pt) const
		{
			assert(false);
			return Eigen::Matrix<double, 1, 1>::Zero(1,1);
		}

		inline int size() const { return 1; }

		void set_parameters(const json &params);

				inline double &mu() { return mu_; }
		inline double mu() const { return mu_; }

		inline double &lambda() { return lambda_; }
		inline double lambda() const { return lambda_; }

	private:
		double mu_ = 1;
		double lambda_ = 1;
	};
}


#ifndef MOCHIMOCHI_ADAM_HPP_
#define MOCHIMOCHI_ADAM_HPP_

#include <Eigen/Dense>
#include <cassert>
#include "../../functions/enumerate.hpp"

class ADAM {
private :
  const std::size_t kDim;

private :
  std::size_t _timestep;
  Eigen::VectorXd _w;
  Eigen::VectorXd _m;
  Eigen::VectorXd _v;

public :
  ADAM(const std::size_t dim)
    : kDim(dim),
      _timestep(0),
      _w(Eigen::VectorXd::Zero(kDim)),
      _m(Eigen::VectorXd::Zero(kDim)),
      _v(Eigen::VectorXd::Zero(kDim)) {

    assert(dim > 0);
  }

  virtual ~ADAM() { }

private :

  double suffer_loss(const Eigen::VectorXd& x, const int y) const {
    return std::max(0.0, 1.0 - y * _w.dot(x));
  }

  double calculate_margin(const Eigen::VectorXd& x) const {
    return _w.dot(x);
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    constexpr auto kAlpha = 0.001;
    constexpr auto kBeta1 = 0.9;
    constexpr auto kBeta2 = 0.999;
    constexpr auto  kEpsilon = 0.00000001;
    constexpr auto kLambda = 0.99999999;

    if (suffer_loss(feature, label) <= 0.0) { return false; }

    const Eigen::VectorXd gradiant = -label * feature;
    const auto beta1_t = std::pow(kLambda, _timestep) * kBeta1;

    _timestep++;
    functions::enumerate(gradiant.data(), gradiant.data() + gradiant.size(), 0,
                       [&](const std::size_t index, const double value) {
                         _m[index] = beta1_t * _m[index] + (1.0 - beta1_t) * value;
                         _v[index] = kBeta2 * _v[index] + (1.0 - kBeta2) * value * value;
                         const auto m_t = _m[index] / (1.0 - std::pow(kBeta1, _timestep));
                         const auto v_t = _v[index] / (1.0 - std::pow(kBeta2, _timestep));
                         _w[index] -= kAlpha * m_t / (std::sqrt(v_t) + kEpsilon);
                       });

    return true;
  }

  int predict(const Eigen::VectorXd& feature) const {
    return calculate_margin(feature) > 0.0 ? 1 : -1;
  }

};

#endif //MOCHIMOCHI_ADAM_HPP_


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_SCALAR_MNORMINF_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_SCALAR_MNORMINF_HPP_INCLUDED
#include <nt2/linalg/functions/mnorminf.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/asum1.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/globalasum1.hpp>
#include <nt2/include/functions/iscolumn.hpp>
#include <nt2/include/functions/isrow.hpp>
#include <nt2/include/functions/ismatrix.hpp>
#include <nt2/core/container/dsl/forward.hpp>
#include <nt2/core/functions/table/details/is_definitely_vector.hpp>
#include <boost/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/static_assert.hpp>

//  infinity norm  of a matrix  (maximum row sum)
// TODO optimize mnorminf(trans(a)) as mnorm1(a)

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( mnorminf_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef typename meta::as_real<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return nt2::abs(a0);
    }

  };

  BOOST_DISPATCH_IMPLEMENT  ( mnorminf_, tag::cpu_
                            , (A0)
                            , ((ast_<A0, nt2::container::domain>))
                            )
  {
    typedef typename A0::value_type                   type_t;
    typedef typename meta::as_real<type_t>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      BOOST_ASSERT_MSG(nt2::ismatrix(a0), "a0 is not a matrix");
      typedef typename details::is_col_vector<typename A0::extent_type>::type choice_t;
      return eval(a0, choice_t());
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, boost::mpl::true_ const &) const //static col vector
    {
      return nt2::globalmax(nt2::abs(a0));
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, boost::mpl::false_ const &) const  // not static col vector
    {
      typedef typename details::is_row_vector<typename A0::extent_type>::type choice_t;
      return eval2(a0, choice_t());
    }

    BOOST_FORCEINLINE result_type
    eval2(A0 const& a0, boost::mpl::true_ const &) const //static row vector
    {
      return nt2::globalasum1(a0);
    }

    BOOST_FORCEINLINE result_type
    eval2(A0 const& a0, boost::mpl::false_ const &) const //not static vector
    {
      return nt2::globalmax(nt2::asum1(a0, 2));
    }

    BOOST_FORCEINLINE result_type
    eval2(A0 const& a0, nt2::meta::indeterminate_ const &) const
    {
      return  eval(a0, nt2::meta::indeterminate_());
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, nt2::meta::indeterminate_ const &) const
    {
      if (iscolumn(a0)) // col vector
      {
        return eval(a0, boost::mpl::true_());
      }
      else if (isrow(a0)) //row vector
      {
        return eval2(a0, boost::mpl::true_());
      }
      else  // matrix but not vector
      {
        return eval2(a0, boost::mpl::false_());
      }
    }
  };
} }

#endif


/** \file   mri_core_kspace_filter.cpp
    \brief  Implementation kspace filter functionalities for 2D and 3D MRI parallel imaging
    \author Hui Xue
*/

#include "mri_core_kspace_filter.h"
#include "hoNDArray_elemwise.h"
#include <boost/algorithm/string.hpp>

#ifdef M_PI
    #undef M_PI
#endif // M_PI
#define M_PI 3.14159265358979323846

namespace Gadgetron
{

ISMRMRDKSPACEFILTER get_kspace_filter_type(const std::string& name)
{
    std::string name_lower(name);
    boost::algorithm::to_lower(name_lower);

    if (name_lower == "gaussian")
    {
        return ISMRMRD_FILTER_GAUSSIAN;
    }
    else if (name_lower == "hanning")
    {
        return ISMRMRD_FILTER_HANNING;
    }
    else if (name_lower == "taperedhanning")
    {
        return ISMRMRD_FILTER_TAPERED_HANNING;
    }
    else if (name_lower == "none")
    {
        return ISMRMRD_FILTER_NONE;
    }

    GERROR_STREAM("Unrecognized kspace filter name : " << name);

    return ISMRMRD_FILTER_NONE;
}

std::string get_kspace_filter_name(ISMRMRDKSPACEFILTER v)
{
    std::string name;

    switch (v)
    {
        case ISMRMRD_FILTER_GAUSSIAN:
        {
            name = "Gaussian";
            break;
        }

        case ISMRMRD_FILTER_HANNING:
        {
            name = "Hanning";
            break;
        }

        case ISMRMRD_FILTER_TAPERED_HANNING:
        {
            name = "TaperedHanning";
            break;
        }

        case ISMRMRD_FILTER_NONE:
        {
            name = "none";
            break;
        }

        default:
        {
            GERROR_STREAM("Unrecognized kspace filter type : " << v);
            name = "none";
        }
    }

    return name;
}

template<typename T>
void generate_symmetric_filter(size_t len, hoNDArray<T>& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width)
{
    try
    {
        if (len == 0) return;

        filter.create(len);

        if (width == 0 || width >= len) width = 1;

        size_t ii;
        if (filterType == ISMRMRD_FILTER_GAUSSIAN)
        {
            double r = -1.0*sigma*sigma / 2;

            if (len % 2 == 0)
            {
                // to make sure the zero points match and boundary of filters are symmetric
                double stepSize = 2.0 / (len - 2);
                std::vector<double> x(len - 1);

                for (ii = 0; ii<len - 1; ii++)
                {
                    x[ii] = -1 + ii*stepSize;
                }

                for (ii = 0; ii<len - 1; ii++)
                {
                    filter(ii + 1) = T(std::exp(r*(x[ii] * x[ii])));
                }

                filter(0) = T(0);
            }
            else
            {
                double stepSize = 2.0 / (len - 1);
                std::vector<double> x(len);

                for (ii = 0; ii<len; ii++)
                {
                    x[ii] = -1 + ii*stepSize;
                }

                for (ii = 0; ii<len; ii++)
                {
                    filter(ii) = T(std::exp(r*(x[ii] * x[ii])));
                }
            }
        }
        else if (filterType == ISMRMRD_FILTER_TAPERED_HANNING)
        {
            hoNDArray<T> w(width);

            for (ii = 1; ii <= width; ii++)
            {
                w(ii - 1) = T((0.5 * (1 - std::cos(2.0*M_PI*ii / (2 * width + 1)))));
            }

            // make sure the center of the filter will end up being 1:
            Gadgetron::fill(filter, T(1.0));

            if (len % 2 == 0)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii) = w(ii - 1);
                    filter(len - ii) = filter(ii);
                }

                filter(0) = T(0);
            }
            else
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii - 1) = w(ii - 1);
                    filter(len - ii) = filter(ii - 1);
                }
            }
        }
        else if (filterType == ISMRMRD_FILTER_HANNING)
        {
            if (len % 2 == 0)
            {
                size_t N = len - 1;
                double halfLen = (double)((N + 1) / 2);
                for (ii = 1; ii <= halfLen; ii++)
                {
                    filter(ii) = T((0.5 * (1 - std::cos(2.0*M_PI*ii / (N + 1)))));
                }

                for (ii = (size_t)halfLen; ii<N; ii++)
                {
                    filter(ii + 1) = filter(N - ii);
                }

                filter(0) = T(0);
            }
            else
            {
                double halfLen = (double)((len + 1) / 2);
                for (ii = 1; ii <= (size_t)halfLen; ii++)
                {
                    filter(ii - 1) = T((0.5 * (1 - std::cos(2.0*M_PI*ii / (len + 1)))));
                }

                for (ii = (size_t)halfLen; ii<len; ii++)
                {
                    filter(ii) = filter(len - 1 - ii);
                }
            }
        }
        else if (filterType == ISMRMRD_FILTER_NONE)
        {
            Gadgetron::fill(filter, T(1.0));
        }
        else
        {
            GADGET_THROW("generate_symmetric_filter, unrecognized fiter type ... ");
        }

        T sos = 0.0f;
        for (ii = 0; ii<len; ii++)
        {
            sos += filter(ii)*filter(ii);
        }

        T r = T(1.0 / std::sqrt(std::abs(sos) / (len)));
        for (ii = 0; ii<len; ii++)
        {
            filter(ii) *= r;
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in generate_symmetric_filter(...) ... ");
    }
}

template EXPORTMRICORE void generate_symmetric_filter(size_t len, hoNDArray<float>& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width);
template EXPORTMRICORE void generate_symmetric_filter(size_t len, hoNDArray<double>& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width);
template EXPORTMRICORE void generate_symmetric_filter(size_t len, hoNDArray< std::complex<float> >& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width);
template EXPORTMRICORE void generate_symmetric_filter(size_t len, hoNDArray< std::complex<double> >& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width);

// ------------------------------------------------------------------------

template<typename T>
void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray<T>& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp)
{
    try
    {
        if (len == 0) return;

        if (start > len - 1) start = 0;
        if (end > len - 1) end = len - 1;

        if (start > end)
        {
            start = 0;
            end = len - 1;
        }

        filter.create(len);
        Gadgetron::clear(filter);

        size_t ii;
        for (ii = start; ii <= end; ii++)
        {
            filter(ii) = T(1.0);
        }

        if (width == 0 || width >= len) width = 1;

        hoNDArray<T> w(width);

        if (filterType == ISMRMRD_FILTER_TAPERED_HANNING)
        {
            for (ii = 1; ii <= width; ii++)
            {
                w(ii - 1) = T((0.5 * (1 - std::cos(2.0*M_PI*ii / (2 * width + 1)))));
            }
        }
        else if (filterType == ISMRMRD_FILTER_NONE)
        {
            Gadgetron::fill(w, T(1.0));
        }
        else
        {
            GADGET_THROW("generate_symmetric_filter, unrecognized fiter type ... ");
        }

        if (densityComp)
        {
            size_t startSym(0), endSym(len - 1);
            find_symmetric_sampled_region(start, end, len / 2, startSym, endSym);

            if (start == 0 && end == len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii - 1) = w(ii - 1);
                    filter(len - ii) = filter(ii - 1);
                }
            }

            if (start == 0 && end<len - 1)
            {
                for (ii = 0; ii<startSym; ii++)
                {
                    filter(ii) = 2.0;
                }

                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii - 1 + startSym) = T(1.0) + w(width - ii);
                    filter(end - ii + 1) = w(ii - 1);
                }
            }

            if (start>0 && end == len - 1)
            {
                for (ii = endSym + 1; ii<len; ii++)
                {
                    filter(ii) = 2.0;
                }

                for (ii = 1; ii <= width; ii++)
                {
                    filter(endSym - ii + 1) = T(1.0) + w(width - ii);
                    filter(start + ii - 1) = w(ii - 1);
                }
            }

            if (start>0 && end<len - 1)
            {
                if (start == startSym && end == endSym)
                {
                    for (ii = 1; ii <= width; ii++)
                    {
                        filter(start + ii - 1) = w(ii - 1);
                        filter(end - ii + 1) = w(ii - 1);
                    }
                }
                else if (start == startSym && end>endSym)
                {
                    for (ii = endSym + 1; ii <= end; ii++)
                    {
                        filter(ii) = 2.0;
                    }

                    for (ii = 1; ii <= width; ii++)
                    {
                        filter(end - ii + 1) = T(1.0) + w(ii - 1);
                        filter(endSym - ii + 1) = w(width - ii);
                        filter(start + ii - 1) = w(ii - 1);
                    }
                }
                else if (start<startSym && end == endSym)
                {
                    for (ii = start; ii<startSym; ii++)
                    {
                        filter(ii) = 2.0;
                    }

                    for (ii = 1; ii <= width; ii++)
                    {
                        filter(ii - 1 + start) = T(1.0) + w(ii - 1);
                        filter(ii - 1 + startSym) = w(width - ii);
                        filter(end - ii + 1) = w(ii - 1);
                    }
                }
                else
                {
                    for (ii = 1; ii <= width; ii++)
                    {
                        filter(start + ii - 1) = w(ii - 1);
                        filter(end - ii + 1) = w(ii - 1);
                    }
                }
            }
        }
        else
        {
            if (start == 0 && end == len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii - 1) = w(ii - 1);
                    filter(len - ii) = filter(ii - 1);
                }
            }

            if (start == 0 && end<len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(end - ii + 1) = w(ii - 1);
                }
            }

            if (start>0 && end == len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(start + ii - 1) = w(ii - 1);
                }
            }

            if (start>0 && end<len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(start + ii - 1) = w(ii - 1);
                    filter(end - ii + 1) = w(ii - 1);
                }
            }
        }

        T sos = 0.0f;
        for (ii = 0; ii<len; ii++)
        {
            sos += filter(ii)*filter(ii);
        }

        T r = (T)(1.0 / std::sqrt(std::abs(sos) / (end - start + 1))); // SNR unit filter
        for (ii = 0; ii<len; ii++)
        {
            filter(ii) *= r;
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in generate_asymmetric_filter(...) ... ");
    }
}

template EXPORTMRICORE void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray<float>& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp);
template EXPORTMRICORE void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray<double>& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp);
template EXPORTMRICORE void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray< std::complex<float> >& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp);
template EXPORTMRICORE void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray< std::complex<double> >& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp);

// ------------------------------------------------------------------------

template<typename T>
void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray<T>& filter)
{
    try
    {
        GADGET_CHECK_THROW(len >= 2);
        GADGET_CHECK_THROW(start >= 0 && end <= len - 1 && start <= end);

        if (start == 0 && end == len - 1)
        {
            generate_symmetric_filter(len, filter, ISMRMRD_FILTER_HANNING);
            return;
        }

        size_t centerInd = len / 2;

        size_t lenFilter(0); // make a symmetric filter with zero at the center
        size_t lenFilterEnd = 2 * (end - centerInd) + 1;
        size_t lenFilterStart = 2 * (centerInd - start) + 1;

        if (start == 0 && end<len - 1)
        {
            lenFilter = lenFilterEnd;
        }
        else if (start>0 && end == len - 1)
        {
            lenFilter = lenFilterStart;
        }
        else if (start>0 && end<len - 1)
        {
            lenFilter = ((lenFilterStart<lenFilterEnd) ? lenFilterStart : lenFilterEnd);
        }
        else
        {
            GERROR_STREAM("generate_symmetric_filter_ref, invalid inputs : start - end - len ... " << start << " " << end << " " << len);
            GADGET_THROW("generate_symmetric_filter_ref, invalid inputs ... ");
        }

        GADGET_CHECK_THROW(lenFilter>0);

        hoNDArray<T> filterSym(lenFilter);
        generate_symmetric_filter(lenFilter, filterSym, ISMRMRD_FILTER_HANNING);

        filter.create(len);
        Gadgetron::clear(&filter);

        if (start == 0 && end<len - 1)
        {
            memcpy(filter.begin() + end - lenFilter + 1, filterSym.begin(), filterSym.get_number_of_bytes());
            return;
        }
        else if (start>0 && end == len - 1)
        {
            memcpy(filter.begin() + start, filterSym.begin(), filterSym.get_number_of_bytes());
            return;
        }
        else if (start>0 && end<len - 1)
        {
            if (lenFilter == lenFilterStart)
            {
                memcpy(filter.begin() + start, filterSym.begin(), filterSym.get_number_of_bytes());
            }
            else
            {
                memcpy(filter.begin() + end - lenFilter + 1, filterSym.begin(), filterSym.get_number_of_bytes());
            }

            return;
        }
        else
        {
            GERROR_STREAM("Invalid inputs : start - end - len : " << start << " " << end << " " << len);
            GADGET_THROW("generate_symmetric_filter_ref, invalid inputs : start - end - len");
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in generate_symmetric_filter_ref(...) ... ");
    }
}

template EXPORTMRICORE void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray<float>& filter);
template EXPORTMRICORE void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray<double>& filter);
template EXPORTMRICORE void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray< std::complex<float> >& filter);
template EXPORTMRICORE void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray< std::complex<double> >& filter);

// ------------------------------------------------------------------------

template <typename T>
void compute_2d_filter(const hoNDArray<T>& fx, const hoNDArray<T>& fy, hoNDArray<T>& fxy)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);

        fxy.create(RO, E1);
        T* pFxy = fxy.begin();

        size_t x, y;

        for (y = 0; y<E1; y++)
        {
            for (x = 0; x<RO; x++)
            {
                pFxy[y*RO + x] = fx(x) * fy(y);
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_2d_filter(...) ... ");
    }
}

template EXPORTMRICORE void compute_2d_filter(const hoNDArray<float>& fx, const hoNDArray<float>& fy, hoNDArray<float>& fxy);
template EXPORTMRICORE void compute_2d_filter(const hoNDArray<double>& fx, const hoNDArray<double>& fy, hoNDArray<double>& fxy);
template EXPORTMRICORE void compute_2d_filter(const hoNDArray< std::complex<float> >& fx, const hoNDArray< std::complex<float> >& fy, hoNDArray< std::complex<float> >& fxy);
template EXPORTMRICORE void compute_2d_filter(const hoNDArray< std::complex<double> >& fx, const hoNDArray< std::complex<double> >& fy, hoNDArray< std::complex<double> >& fxy);

// ------------------------------------------------------------------------

void compute_2d_filter(const hoNDArray<float>& fx, const hoNDArray<float>& fy, hoNDArray< std::complex<float> >& fxy)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);

        fxy.create(RO, E1);
        std::complex<float> * pFxy = fxy.begin();

        size_t x, y;

        for (y = 0; y<E1; y++)
        {
            for (x = 0; x<RO; x++)
            {
                pFxy[y*RO + x] = std::complex<float>(fx(x) * fy(y));
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_2d_filter(float) ... ");
    }
}

// ------------------------------------------------------------------------

void compute_2d_filter(const hoNDArray<double>& fx, const hoNDArray<double>& fy, hoNDArray< std::complex<double> >& fxy)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);

        fxy.create(RO, E1);
        std::complex<double> * pFxy = fxy.begin();

        size_t x, y;

        for (y = 0; y<E1; y++)
        {
            for (x = 0; x<RO; x++)
            {
                pFxy[y*RO + x] = std::complex<double>(fx(x) * fy(y));
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_2d_filter(double) ... ");
    }
}

// ------------------------------------------------------------------------

template <typename T>
void compute_3d_filter(const hoNDArray<T>& fx, const hoNDArray<T>& fy, const hoNDArray<T>& fz, hoNDArray<T>& fxyz)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);
        size_t E2 = fz.get_size(0);

        fxyz.create(RO, E1, E2);
        T* pFxyz = fxyz.begin();

        const T* px = fx.begin();
        const T* py = fy.begin();
        const T* pz = fz.begin();

        size_t x, y, z;

        T vz, vy, vx;

        for (z = 0; z<E2; z++)
        {
            vz = pz[z];
            for (y = 0; y<E1; y++)
            {
                vy = py[y];
                for (x = 0; x<RO; x++)
                {
                    vx = px[x];
                    pFxyz[x+y*RO+z*RO*E1] = (vx*vz*vy);
                }
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_3d_filter(...) ... ");
    }
}

template EXPORTMRICORE void compute_3d_filter(const hoNDArray<float>& fx, const hoNDArray<float>& fy, const hoNDArray<float>& fz, hoNDArray<float>& fxyz);
template EXPORTMRICORE void compute_3d_filter(const hoNDArray<double>& fx, const hoNDArray<double>& fy, const hoNDArray<double>& fz, hoNDArray<double>& fxyz);
template EXPORTMRICORE void compute_3d_filter(const hoNDArray< std::complex<float> >& fx, const hoNDArray< std::complex<float> >& fy, const hoNDArray< std::complex<float> >& fz, hoNDArray< std::complex<float> >& fxyz);
template EXPORTMRICORE void compute_3d_filter(const hoNDArray< std::complex<double> >& fx, const hoNDArray< std::complex<double> >& fy, const hoNDArray< std::complex<double> >& fz, hoNDArray< std::complex<double> >& fxyz);

// ------------------------------------------------------------------------

void compute_3d_filter(const hoNDArray<float>& fx, const hoNDArray<float>& fy, const hoNDArray<float>& fz, hoNDArray< std::complex<float> >& fxyz)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);
        size_t E2 = fz.get_size(0);

        fxyz.create(RO, E1, E2);
        std::complex<float> * pFxyz = fxyz.begin();

        size_t x, y, z;

        for (z = 0; z<E2; z++)
        {
            for (y = 0; y<E1; y++)
            {
                for (x = 0; x<RO; x++)
                {
                    pFxyz[z*RO*E1 + y*RO + x] = std::complex<float>(fx(x)*fy(y)*fz(z));
                }
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_3d_filter(float) ... ");
    }
}

// ------------------------------------------------------------------------

void compute_3d_filter(const hoNDArray<double>& fx, const hoNDArray<double>& fy, const hoNDArray<double>& fz, hoNDArray< std::complex<double> >& fxyz)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);
        size_t E2 = fz.get_size(0);

        fxyz.create(RO, E1, E2);
        std::complex<double> * pFxyz = fxyz.begin();

        size_t x, y, z;

        for (z = 0; z<E2; z++)
        {
            for (y = 0; y<E1; y++)
            {
                for (x = 0; x<RO; x++)
                {
                    pFxyz[z*RO*E1 + y*RO + x] = std::complex<double>(fx(x)*fy(y)*fz(z));
                }
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_3d_filter(double) ... ");
    }
}

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_RO(hoNDArray<T>& data, const hoNDArray<T>& fRO)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_number_of_elements());
        Gadgetron::multiply(data, fRO, data);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_RO(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_RO(hoNDArray<float>& data, const hoNDArray<float>& fRO);
template EXPORTMRICORE void apply_kspace_filter_RO(hoNDArray<double>& data, const hoNDArray<double>& fRO);
template EXPORTMRICORE void apply_kspace_filter_RO(hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO);
template EXPORTMRICORE void apply_kspace_filter_RO(hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO);

template <typename T>
void apply_kspace_filter_RO(const hoNDArray<T>& data, const hoNDArray<T>& fRO, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_number_of_elements());
        Gadgetron::multiply(data, fRO, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_RO(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_RO(const hoNDArray<float>& data, const hoNDArray<float>& fRO, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_RO(const hoNDArray<double>& data, const hoNDArray<double>& fRO, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_RO(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_RO(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_E1(const hoNDArray<T>& data, const hoNDArray<T>& fE1, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(1) == fE1.get_number_of_elements());

        hoNDArray<T> fRO(data.get_size(0));
        fRO.fill(T(1.0));

        hoNDArray<T> fxy;
        compute_2d_filter(fRO, fE1, fxy);

        Gadgetron::multiply(data, fxy, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_E1(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_E1(const hoNDArray<float>& data, const hoNDArray<float>& fE1, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1(const hoNDArray<double>& data, const hoNDArray<double>& fE1, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fE1, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fE1, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE1(const hoNDArray<T>& data, const hoNDArray<T>& fROE1, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fROE1.get_size(0));
        GADGET_CHECK_THROW(data.get_size(1) == fROE1.get_size(1));

        Gadgetron::multiply(data, fROE1, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE1(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray<float>& data, const hoNDArray<float>& fROE1, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray<double>& data, const hoNDArray<double>& fROE1, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fROE1, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fROE1, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE1(const hoNDArray<T>& data, const hoNDArray<T>& fRO, const hoNDArray<T>& fE1, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_size(0));
        GADGET_CHECK_THROW(data.get_size(1) == fE1.get_size(0));

        hoNDArray<T> fROE1;
        compute_2d_filter(fRO, fE1, fROE1);

        Gadgetron::multiply(data, fROE1, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE1(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray<float>& data, const hoNDArray<float>& fRO, const hoNDArray<float>& fE1, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray<double>& data, const hoNDArray<double>& fRO, const hoNDArray<double>& fE1, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO, const hoNDArray< std::complex<float> >& fE1, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO, const hoNDArray< std::complex<double> >& fE1, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_E2(const hoNDArray<T>& data, const hoNDArray<T>& fE2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(2) == fE2.get_number_of_elements());

        hoNDArray<T> fRO(data.get_size(0));
        fRO.fill(T(1.0));

        hoNDArray<T> fE1(data.get_size(1));
        fE1.fill(T(1.0));

        hoNDArray<T> fxyz;
        compute_3d_filter(fRO, fE1, fE2, fxyz);

        Gadgetron::multiply(data, fxyz, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_E2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_E2(const hoNDArray<float>& data, const hoNDArray<float>& fE2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E2(const hoNDArray<double>& data, const hoNDArray<double>& fE2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fE2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fE2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE2(const hoNDArray<T>& data, const hoNDArray<T>& fRO, const hoNDArray<T>& fE2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_number_of_elements());
        GADGET_CHECK_THROW(data.get_size(2) == fE2.get_number_of_elements());

        hoNDArray<T> fE1(data.get_size(1));
        fE1.fill(T(1.0));

        hoNDArray<T> fxyz;
        compute_3d_filter(fRO, fE1, fE2, fxyz);

        Gadgetron::multiply(data, fxyz, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE2(const hoNDArray<float>& data, const hoNDArray<float>& fRO, const hoNDArray<float>& fE2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE2(const hoNDArray<double>& data, const hoNDArray<double>& fRO, const hoNDArray<double>& fE2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO, const hoNDArray< std::complex<float> >& fE2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO, const hoNDArray< std::complex<double> >& fE2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_E1E2(const hoNDArray<T>& data, const hoNDArray<T>& fE1, const hoNDArray<T>& fE2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(1) == fE1.get_number_of_elements());
        GADGET_CHECK_THROW(data.get_size(2) == fE2.get_number_of_elements());

        hoNDArray<T> fRO(data.get_size(0));
        fRO.fill(T(1.0));

        hoNDArray<T> fxyz;
        compute_3d_filter(fRO, fE1, fE2, fxyz);

        Gadgetron::multiply(data, fxyz, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_E1E2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_E1E2(const hoNDArray<float>& data, const hoNDArray<float>& fE1, const hoNDArray<float>& fE2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1E2(const hoNDArray<double>& data, const hoNDArray<double>& fE1, const hoNDArray<double>& fE2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1E2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fE1, const hoNDArray< std::complex<float> >& fE2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1E2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fE1, const hoNDArray< std::complex<double> >& fE2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE1E2(const hoNDArray<T>& data, const hoNDArray<T>& fROE1E2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fROE1E2.get_size(0));
        GADGET_CHECK_THROW(data.get_size(1) == fROE1E2.get_size(1));
        GADGET_CHECK_THROW(data.get_size(2) == fROE1E2.get_size(2));

        Gadgetron::multiply(data, fROE1E2, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE1E2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray<float>& data, const hoNDArray<float>& fROE1E2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray<double>& data, const hoNDArray<double>& fROE1E2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fROE1E2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fROE1E2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE1E2(const hoNDArray<T>& data, const hoNDArray<T>& fRO, const hoNDArray<T>& fE1, const hoNDArray<T>& fE2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_number_of_elements());
        GADGET_CHECK_THROW(data.get_size(1) == fE1.get_number_of_elements());
        GADGET_CHECK_THROW(data.get_size(2) == fE2.get_number_of_elements());

        hoNDArray<T> fxyz;
        compute_3d_filter(fRO, fE1, fE2, fxyz);

        Gadgetron::multiply(data, fxyz, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE1E2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray<float>& data, const hoNDArray<float>& fRO, const hoNDArray<float>& fE1, const hoNDArray<float>& fE2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray<double>& data, const hoNDArray<double>& fRO, const hoNDArray<double>& fE1, const hoNDArray<double>& fE2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO, const hoNDArray< std::complex<float> >& fE1, const hoNDArray< std::complex<float> >& fE2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO, const hoNDArray< std::complex<double> >& fE1, const hoNDArray< std::complex<double> >& fE2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

void find_symmetric_sampled_region(size_t start, size_t end, size_t center, size_t& startSym, size_t& endSym)
{
    GADGET_CHECK_THROW(end >= start);
    GADGET_CHECK_THROW(center >= start);
    GADGET_CHECK_THROW(end >= center);

    size_t halfSizeStart = center - start;
    size_t halfSizeEnd = end - center;

    if (halfSizeStart > halfSizeEnd)
    {
        startSym = center - halfSizeEnd;
        endSym = center + halfSizeEnd;
    }
    else
    {
        startSym = center - halfSizeStart;
        endSym = center + halfSizeStart;
    }
}

// ------------------------------------------------------------------------

template<typename T>
void compute_filter_SNR_unit_scale_factor(const hoNDArray<T>& filter, T& scalFactor)
{
    size_t ii, len;

    len = filter.get_number_of_elements();
    if (len == 0)
    {
        scalFactor = T(1.0);
        return;
    }

    T sos(0.0);
    for (ii = 0; ii<len; ii++)
    {
        sos += filter(ii)*filter(ii);
    }

    scalFactor = (T)(1.0 / std::sqrt(std::abs(sos) / len));
}

template EXPORTMRICORE void compute_filter_SNR_unit_scale_factor(const hoNDArray<float>& filter, float& scalFactor);
template EXPORTMRICORE void compute_filter_SNR_unit_scale_factor(const hoNDArray<double>& filter, double& scalFactor);

template<> EXPORTMRICORE
void compute_filter_SNR_unit_scale_factor(const hoNDArray< std::complex<float> >& filter, std::complex<float> & scalFactor)
{
    size_t ii, len;

    len = filter.get_number_of_elements();
    if (len == 0)
    {
        scalFactor = std::complex<float>(1.0);
        return;
    }

    std::complex<float> sos(0.0);
    for (ii = 0; ii<len; ii++)
    {
        sos += filter(ii)*std::conj(filter(ii));
    }

    scalFactor = (std::complex<float>)(1.0 / std::sqrt(std::abs(sos) / len));
}

template<> EXPORTMRICORE
void compute_filter_SNR_unit_scale_factor(const hoNDArray< std::complex<double> >& filter, std::complex<double> & scalFactor)
{
    size_t ii, len;

    len = filter.get_number_of_elements();
    if (len == 0)
    {
        scalFactor = std::complex<double>(1.0);
        return;
    }

    std::complex<double> sos(0.0);
    for (ii = 0; ii<len; ii++)
    {
        sos += filter(ii)*std::conj(filter(ii));
    }

    scalFactor = (std::complex<double>)(1.0 / std::sqrt(std::abs(sos) / len));
}

// ------------------------------------------------------------------------

}


/**
    @file data.cpp
    Helper functions for saving and loading data
*/

#include <utils/data.hpp>

#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <Eigen/Dense>

/** Save matrix to .csv file. */
void Data::save_matrix(const std::string& file_name, Eigen::MatrixXd matrix) {
    const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                        Eigen::DontAlignCols, ", ", "\n");
    std::ofstream file(file_name); // open a file to write
    if (file.is_open())
    {
        file << matrix.format(CSVFormat);
        file.close();
    }
}

/** Load matrix from .csv file */
Eigen::MatrixXd Data::load_matrix(const std::string& file_name) {
    std::ifstream file(file_name); // open a file to read
    if (!file.good()) {
        std::cerr << "File: " << file_name << " does not exist" << std::endl;
        exit(1);
    }

    // Read through file and extract all data elements
    std::string row;
    int i = 0; // row counter
    std::string entry;
    std::vector<double> entries;
    while (std::getline(file, row)) {
        std::stringstream row_stream(row);
        while (std::getline(row_stream, entry, ',')) {
            entries.push_back(std::stod(entry));
        }
        i++; // increment row counter
    }

    // Convert vector into matrix of proper shape
    return Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(entries.data(), i, entries.size() / i);
}

/** Load vector from .csv file */
Eigen::VectorXd Data::load_vector(const std::string& file_name) {
    Eigen::MatrixXd M = Data::load_matrix(file_name);
    return Eigen::Map<Eigen::VectorXd>(M.data(), M.rows());
}


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

#include <iostream>
#include <algorithm>
#include <boost/ref.hpp>
#include <boost/bind.hpp>
#include <boost/array.hpp>
#include <boost/foreach.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

using namespace boost;
using namespace boost::accumulators;

// Helper that uses BOOST_FOREACH to display a range of doubles
template<typename Range>
void output_range(Range const &rng)
{
    bool first = true;
    BOOST_FOREACH(double d, rng)
    {
        if(!first) std::cout << ", ";
        std::cout << d;
        first = false;
    }
    std::cout << '\n';
}

///////////////////////////////////////////////////////////////////////////////
// example1
//
//  Calculate some useful stats using accumulator_set<> and std::for_each()
//
void example1()
{
    accumulator_set<
        double
      , stats<tag::min, tag::mean(immediate), tag::sum, tag::moment<2> >
    > acc;

    boost::array<double, 4> data = {0., 1., -1., 3.14159};

    // std::for_each pushes each sample into the accumulator one at a
    // time, and returns a copy of the accumulator.
    acc = std::for_each(data.begin(), data.end(), acc);

    // The following would be equivalent, and could be more efficient
    // because it doesn't pass and return the entire accumulator set
    // by value.
    //std::for_each(data.begin(), data.end(), bind<void>(ref(acc), _1));

    std::cout << "  min""(acc)        = " << (min)(acc) << std::endl; // Extra quotes are to prevent complaints from Boost inspect tool
    std::cout << "  mean(acc)       = " << mean(acc) << std::endl;

    // since mean depends on count and sum, we can get their results, too.
    std::cout << "  count(acc)      = " << count(acc) << std::endl;
    std::cout << "  sum(acc)        = " << sum(acc) << std::endl;
    std::cout << "  moment<2>(acc)  = " << accumulators::moment<2>(acc) << std::endl;
}

///////////////////////////////////////////////////////////////////////////////
// example2
//
//  Calculate some tail statistics. This demonstrates how to specify
//  constructor and accumulator parameters. Note that the tail statistics
//  return multiple values, which are returned in an iterator_range.
//
//  It pushes data in and displays the intermediate results to demonstrate
//  how the tail statistics are updated.
void example2()
{
    // An accumulator which tracks the right tail (largest N items) and
    // some data that are covariate with them. N == 4.
    accumulator_set<
        double
      , stats<tag::tail_variate<double, tag::covariate1, right> >
    > acc(tag::tail<right>::cache_size = 4);

    acc(2.1, covariate1 = .21);
    acc(1.1, covariate1 = .11);
    acc(2.1, covariate1 = .21);
    acc(1.1, covariate1 = .11);

    std::cout << "  tail            = "; output_range(tail(acc));
    std::cout << "  tail_variate    = "; output_range(tail_variate(acc));
    std::cout << std::endl;

    acc(21.1, covariate1 = 2.11);
    acc(11.1, covariate1 = 1.11);
    acc(21.1, covariate1 = 2.11);
    acc(11.1, covariate1 = 1.11);

    std::cout << "  tail            = "; output_range(tail(acc));
    std::cout << "  tail_variate    = "; output_range(tail_variate(acc));
    std::cout << std::endl;

    acc(42.1, covariate1 = 4.21);
    acc(41.1, covariate1 = 4.11);
    acc(42.1, covariate1 = 4.21);
    acc(41.1, covariate1 = 4.11);

    std::cout << "  tail            = "; output_range(tail(acc));
    std::cout << "  tail_variate    = "; output_range(tail_variate(acc));
    std::cout << std::endl;

    acc(32.1, covariate1 = 3.21);
    acc(31.1, covariate1 = 3.11);
    acc(32.1, covariate1 = 3.21);
    acc(31.1, covariate1 = 3.11);

    std::cout << "  tail            = "; output_range(tail(acc));
    std::cout << "  tail_variate    = "; output_range(tail_variate(acc));
}

///////////////////////////////////////////////////////////////////////////////
// example3
//
//  Demonstrate how to calculate weighted statistics. This example demonstrates
//  both a simple weighted statistical calculation, and a more complicated
//  calculation where the weight statistics are calculated and stored in an
//  external weight accumulataor.
void example3()
{
    // weight == double
    double w = 1.;

    // Simple weighted calculation
    {
        // stats that depend on the weight are made external
        accumulator_set<double, stats<tag::mean>, double> acc;

        acc(0., weight = w);
        acc(1., weight = w);
        acc(-1., weight = w);
        acc(3.14159, weight = w);

        std::cout << "  mean(acc)       = " << mean(acc) << std::endl;
    }

    // Weighted calculation with an external weight accumulator
    {
        // stats that depend on the weight are made external
        accumulator_set<double, stats<tag::mean>, external<double> > acc;

        // Here's an external weight accumulator
        accumulator_set<void, stats<tag::sum_of_weights>, double> weight_acc;

        weight_acc(weight = w); acc(0., weight = w);
        weight_acc(weight = w); acc(1., weight = w);
        weight_acc(weight = w); acc(-1., weight = w);
        weight_acc(weight = w); acc(3.14159, weight = w);

        std::cout << "  mean(acc)       = " << mean(acc, weights = weight_acc) << std::endl;
    }
}

///////////////////////////////////////////////////////////////////////////////
// main
int main()
{
    std::cout << "Example 1:\n";
    example1();

    std::cout << "\nExample 2:\n";
    example2();

    std::cout << "\nExample 3:\n";
    example3();

    return 0;
}