/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * 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)
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_3_HPP
#define BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_3_HPP

#include <cassert>

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/atlas/cblas3_overloads.hpp>
#include <boost/numeric/bindings/atlas/cblas_enum.hpp>
#include <boost/type_traits/same_traits.hpp>
#include <boost/mpl/if.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace atlas {

    // C <- alpha * op (A) * op (B) + beta * C
    // op (A) == A || A^T || A^H
    template <typename T, typename MatrA, typename MatrB, typename MatrC>
    inline
    void gemm (CBLAS_TRANSPOSE const TransA, CBLAS_TRANSPOSE const TransB,
               T const& alpha, MatrA const& a, MatrB const& b,
               T const& beta, MatrC& c
               )
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrC>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrC>::ordering_type
      >::value));
#endif

      assert (TransA == CblasNoTrans
              || TransA == CblasTrans
              || TransA == CblasConjTrans);
      assert (TransB == CblasNoTrans
              || TransB == CblasTrans
              || TransB == CblasConjTrans);

      int const m = TransA == CblasNoTrans
        ? traits::matrix_size1 (a)
        : traits::matrix_size2 (a);
      int const n = TransB == CblasNoTrans
        ? traits::matrix_size2 (b)
        : traits::matrix_size1 (b);
      int const k = TransA == CblasNoTrans
        ? traits::matrix_size2 (a)
        : traits::matrix_size1 (a);
      assert (m == traits::matrix_size1 (c));
      assert (n == traits::matrix_size2 (c));
#ifndef NDEBUG
      int const k1 = TransB == CblasNoTrans
        ? traits::matrix_size1 (b)
        : traits::matrix_size2 (b);
      assert (k == k1);
#endif
      // .. what about AtlasConj?

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      detail::gemm (stor_ord, TransA, TransB, m, n, k, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (a),
#else
                    traits::matrix_storage_const (a),
#endif
                    traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (b),
#else
                    traits::matrix_storage_const (b),
#endif
                    traits::leading_dimension (b),
                    beta,
                    traits::matrix_storage (c),
                    traits::leading_dimension (c));
    }

    // C <- alpha * A * B + beta * C
    template <typename T, typename MatrA, typename MatrB, typename MatrC>
    inline
    void gemm (T const& alpha, MatrA const& a, MatrB const& b,
               T const& beta, MatrC& c)
    {
      gemm (CblasNoTrans, CblasNoTrans, alpha, a, b, beta, c) ;
    }

    // C <- A * B
    template <typename MatrA, typename MatrB, typename MatrC>
    inline
    void gemm (MatrA const& a, MatrB const& b, MatrC& c) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrC>::value_type val_t;
#else
      typedef typename MatrC::value_type val_t;
#endif
      gemm (CblasNoTrans, CblasNoTrans, (val_t) 1, a, b, (val_t) 0, c);
    }

    // C <- alpha * A * B + beta * C
    // C <- alpha * B * A + beta * C
    // A == A^T

    namespace detail {

      template <typename T, typename SymmA, typename MatrB, typename MatrC>
      inline
      void symm (CBLAS_SIDE const side, CBLAS_UPLO const uplo,
                 T const& alpha, SymmA const& a, MatrB const& b,
                 T const& beta, MatrC& c)
      {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrC>::matrix_structure,
          traits::general_t
        >::value));

        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<SymmA>::ordering_type,
          typename traits::matrix_traits<MatrB>::ordering_type
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<SymmA>::ordering_type,
          typename traits::matrix_traits<MatrC>::ordering_type
        >::value));
#endif

        assert (side == CblasLeft || side == CblasRight);
        assert (uplo == CblasUpper || uplo == CblasLower);

        int const m = traits::matrix_size1 (c);
        int const n = traits::matrix_size2 (c);

        assert (side == CblasLeft
                ? m == traits::matrix_size1 (a)
                  && m == traits::matrix_size2 (a)
                : n == traits::matrix_size1 (a)
                  && n == traits::matrix_size2 (a));
        assert (m == traits::matrix_size1 (b)
                && n == traits::matrix_size2 (b));

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        symm (stor_ord, side, uplo,
              m, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
              traits::matrix_storage (a),
#else
              traits::matrix_storage_const (a),
#endif
              traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
              traits::matrix_storage (b),
#else
              traits::matrix_storage_const (b),
#endif
              traits::leading_dimension (b),
              beta,
              traits::matrix_storage (c),
              traits::leading_dimension (c));
      }

    } // detail

    // C <- alpha * A * B + beta * C
    // C <- alpha * B * A + beta * C
    // A == A^T
    template <typename T, typename SymmA, typename MatrB, typename MatrC>
    inline
    void symm (CBLAS_SIDE const side, CBLAS_UPLO const uplo,
               T const& alpha, SymmA const& a, MatrB const& b,
               T const& beta, MatrC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      detail::symm (side, uplo, alpha, a, b, beta, c);
    }

    template <typename T, typename SymmA, typename MatrB, typename MatrC>
    inline
    void symm (CBLAS_SIDE const side,
               T const& alpha, SymmA const& a, MatrB const& b,
               T const& beta, MatrC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      detail::symm (side, uplo, alpha, a, b, beta, c);
    }

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

    namespace detail {

      // C <- alpha * A * B + beta * C ;  A == A^T
      struct symm_left {
        template <typename T, typename SymmA, typename MatrB, typename MatrC>
        static void f (T const& alpha, SymmA const& a, MatrB const& b,
                       T const& beta, MatrC& c)
        {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
          BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<SymmA>::matrix_structure,
            traits::symmetric_t
          >::value));
          BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<MatrB>::matrix_structure,
            traits::general_t
          >::value));
#endif

          int const m = traits::matrix_size1 (c);
          int const n = traits::matrix_size2 (c);

          assert (m == traits::matrix_size1 (a)
                  && m == traits::matrix_size2 (a));
          assert (m == traits::matrix_size1 (b)
                  && n == traits::matrix_size2 (b));

          CBLAS_ORDER const stor_ord
            = enum_cast<CBLAS_ORDER const>
            (storage_order<
              typename traits::matrix_traits<SymmA>::ordering_type
             >::value);

          CBLAS_UPLO const uplo
            = enum_cast<CBLAS_UPLO const>
            (uplo_triang<
              typename traits::matrix_traits<SymmA>::uplo_type
             >::value);

          symm (stor_ord, CblasLeft, uplo,
                m, n, alpha,
                traits::matrix_storage (a), traits::leading_dimension (a),
                traits::matrix_storage (b), traits::leading_dimension (b),
                beta,
                traits::matrix_storage (c), traits::leading_dimension (c));
        }
      };

      // C <- alpha * A * B + beta * C ;  B == B^T
      struct symm_right {
        template <typename T, typename MatrA, typename SymmB, typename MatrC>
        static void f (T const& alpha, MatrA const& a, SymmB const& b,
                       T const& beta, MatrC& c)
        {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
          BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<MatrA>::matrix_structure,
            traits::general_t
          >::value));
          BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<SymmB>::matrix_structure,
            traits::symmetric_t
          >::value));
#endif

          int const m = traits::matrix_size1 (c);
          int const n = traits::matrix_size2 (c);

          assert (n == traits::matrix_size1 (b)
                  && n == traits::matrix_size2 (b));
          assert (m == traits::matrix_size1 (a)
                  && n == traits::matrix_size2 (a));

          CBLAS_ORDER const stor_ord
            = enum_cast<CBLAS_ORDER const>
            (storage_order<
              typename traits::matrix_traits<SymmB>::ordering_type
             >::value);

          CBLAS_UPLO const uplo
            = enum_cast<CBLAS_UPLO const>
            (uplo_triang<
              typename traits::matrix_traits<SymmB>::uplo_type
             >::value);

          symm (stor_ord, CblasRight, uplo,
                m, n, alpha,
                traits::matrix_storage (b), traits::leading_dimension (b),
                traits::matrix_storage (a), traits::leading_dimension (a),
                beta,
                traits::matrix_storage (c), traits::leading_dimension (c));
        }
      };

    } // detail

    // C <- alpha * A * B + beta * C
    // C <- alpha * B * A + beta * C
    // A == A^T
    template <typename T, typename MatrA, typename MatrB, typename MatrC>
    inline
    void symm (T const& alpha, MatrA const& a, MatrB const& b,
               T const& beta, MatrC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrC>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrC>::ordering_type
      >::value));
#endif

      typedef typename
        boost::mpl::if_c<
          boost::is_same<
            typename traits::matrix_traits<MatrA>::matrix_structure,
            traits::symmetric_t
          >::value,
          detail::symm_left,
          detail::symm_right
        >::type functor;

      functor::f (alpha, a, b, beta, c);
    }

    // C <- A * B
    // C <- B * A
    template <typename MatrA, typename MatrB, typename MatrC>
    inline
    void symm (MatrA const& a, MatrB const& b, MatrC& c) {
      typedef typename traits::matrix_traits<MatrC>::value_type val_t;
      symm ((val_t) 1, a, b, (val_t) 0, c);
    }

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

    // C <- alpha * A * B + beta * C
    // C <- alpha * B * A + beta * C
    // A == A^H

    namespace detail {

      template <typename T, typename HermA, typename MatrB, typename MatrC>
      inline
      void hemm (CBLAS_SIDE const side, CBLAS_UPLO const uplo,
                 T const& alpha, HermA const& a, MatrB const& b,
                 T const& beta, MatrC& c)
      {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrC>::matrix_structure,
          traits::general_t
        >::value));

        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<HermA>::ordering_type,
          typename traits::matrix_traits<MatrB>::ordering_type
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<HermA>::ordering_type,
          typename traits::matrix_traits<MatrC>::ordering_type
        >::value));
#endif

        assert (side == CblasLeft || side == CblasRight);
        assert (uplo == CblasUpper || uplo == CblasLower);

        int const m = traits::matrix_size1 (c);
        int const n = traits::matrix_size2 (c);

        assert (side == CblasLeft
                ? m == traits::matrix_size1 (a)
                  && m == traits::matrix_size2 (a)
                : n == traits::matrix_size1 (a)
                  && n == traits::matrix_size2 (a));
        assert (m == traits::matrix_size1 (b)
                && n == traits::matrix_size2 (b));

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<HermA>::ordering_type
#else
            typename HermA::orientation_category
#endif
           >::value);

        hemm (stor_ord, side, uplo,
              m, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
              traits::matrix_storage (a),
#else
              traits::matrix_storage_const (a),
#endif
              traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
              traits::matrix_storage (b),
#else
              traits::matrix_storage_const (b),
#endif
              traits::leading_dimension (b),
              beta,
              traits::matrix_storage (c),
              traits::leading_dimension (c));
      }

    } // detail

    // C <- alpha * A * B + beta * C
    // C <- alpha * B * A + beta * C
    // A == A^H
    template <typename T, typename HermA, typename MatrB, typename MatrC>
    inline
    void hemm (CBLAS_SIDE const side, CBLAS_UPLO const uplo,
               T const& alpha, HermA const& a, MatrB const& b,
               T const& beta, MatrC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      detail::hemm (side, uplo, alpha, a, b, beta, c);
    }

    template <typename T, typename HermA, typename MatrB, typename MatrC>
    inline
    void hemm (CBLAS_SIDE const side,
               T const& alpha, HermA const& a, MatrB const& b,
               T const& beta, MatrC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<HermA>::uplo_type
#else
          typename HermA::packed_category
#endif
         >::value);

      detail::hemm (side, uplo, alpha, a, b, beta, c);
    }

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

    namespace detail {

      // C <- alpha * A * B + beta * C ;  A == A^H
      struct hemm_left {
        template <typename T, typename HermA, typename MatrB, typename MatrC>
        static void f (T const& alpha, HermA const& a, MatrB const& b,
                       T const& beta, MatrC& c)
        {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
          BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<HermA>::matrix_structure,
            traits::hermitian_t
          >::value));
          BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<MatrB>::matrix_structure,
            traits::general_t
          >::value));
#endif

          int const m = traits::matrix_size1 (c);
          int const n = traits::matrix_size2 (c);

          assert (m == traits::matrix_size1 (a)
                  && m == traits::matrix_size2 (a));
          assert (m == traits::matrix_size1 (b)
                  && n == traits::matrix_size2 (b));

          CBLAS_ORDER const stor_ord
            = enum_cast<CBLAS_ORDER const>
            (storage_order<
              typename traits::matrix_traits<HermA>::ordering_type
             >::value);

          CBLAS_UPLO const uplo
            = enum_cast<CBLAS_UPLO const>
            (uplo_triang<
              typename traits::matrix_traits<HermA>::uplo_type
             >::value);

          hemm (stor_ord, CblasLeft, uplo,
                m, n, alpha,
                traits::matrix_storage (a), traits::leading_dimension (a),
                traits::matrix_storage (b), traits::leading_dimension (b),
                beta,
                traits::matrix_storage (c), traits::leading_dimension (c));
        }
      };

      // C <- alpha * A * B + beta * C ;  B == B^H
      struct hemm_right {
        template <typename T, typename MatrA, typename HermB, typename MatrC>
        static void f (T const& alpha, MatrA const& a, HermB const& b,
                       T const& beta, MatrC& c)
        {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
          BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<MatrA>::matrix_structure,
            traits::general_t
          >::value));
          BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<HermB>::matrix_structure,
            traits::hermitian_t
          >::value));
#endif

          int const m = traits::matrix_size1 (c);
          int const n = traits::matrix_size2 (c);

          assert (n == traits::matrix_size1 (b)
                  && n == traits::matrix_size2 (b));
          assert (m == traits::matrix_size1 (a)
                  && n == traits::matrix_size2 (a));

          CBLAS_ORDER const stor_ord
            = enum_cast<CBLAS_ORDER const>
            (storage_order<
              typename traits::matrix_traits<HermB>::ordering_type
             >::value);

          CBLAS_UPLO const uplo
            = enum_cast<CBLAS_UPLO const>
            (uplo_triang<
              typename traits::matrix_traits<HermB>::uplo_type
             >::value);

          hemm (stor_ord, CblasRight, uplo,
                m, n, alpha,
                traits::matrix_storage (b), traits::leading_dimension (b),
                traits::matrix_storage (a), traits::leading_dimension (a),
                beta,
                traits::matrix_storage (c), traits::leading_dimension (c));
        }
      };

    }

    template <typename T, typename MatrA, typename MatrB, typename MatrC>
    inline
    void hemm (T const& alpha, MatrA const& a, MatrB const& b,
               T const& beta, MatrC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrC>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrC>::ordering_type
      >::value));
#endif

      typedef typename
        boost::mpl::if_c<
          boost::is_same<
            typename traits::matrix_traits<MatrA>::matrix_structure,
            traits::hermitian_t
          >::value,
          detail::hemm_left,
          detail::hemm_right
        >::type functor;

      functor::f (alpha, a, b, beta, c);
    }

    // C <- A * B
    // C <- B * A
    template <typename MatrA, typename MatrB, typename MatrC>
    inline
    void hemm (MatrA const& a, MatrB const& b, MatrC& c) {
      typedef typename traits::matrix_traits<MatrC>::value_type val_t;
      hemm ((val_t) 1, a, b, (val_t) 0, c);
    }

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

    // C <- alpha * A * A^T + beta * C
    // C <- alpha * A^T * A + beta * C
    // C == C^T

    namespace detail {

      template <typename T, typename MatrA, typename SymmC>
      inline
      void syrk (CBLAS_UPLO const uplo, CBLAS_TRANSPOSE trans,
                 T const& alpha, MatrA const& a,
                 T const& beta, SymmC& c)
      {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrA>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrA>::ordering_type,
          typename traits::matrix_traits<SymmC>::ordering_type
        >::value));
#endif

        assert (uplo == CblasUpper || uplo == CblasLower);
        assert (trans == CblasNoTrans
                || trans == CblasTrans
                || trans == CblasConjTrans);

        int const n = traits::matrix_size1 (c);
        assert (n == traits::matrix_size2 (c));

        int const k = trans == CblasNoTrans
          ? traits::matrix_size2 (a)
          : traits::matrix_size1 (a);
        assert (n == (trans == CblasNoTrans
                      ? traits::matrix_size1 (a)
                      : traits::matrix_size2 (a)));

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmC>::ordering_type
#else
            typename SymmC::orientation_category
#endif
           >::value);

        syrk (stor_ord, uplo, trans,
              n, k, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
              traits::matrix_storage (a),
#else
              traits::matrix_storage_const (a),
#endif
              traits::leading_dimension (a),
              beta,
              traits::matrix_storage (c),
              traits::leading_dimension (c));
      }

    } // detail

    // C <- alpha * A * A^T + beta * C
    // C <- alpha * A^T * A + beta * C
    // C == C^T
    template <typename T, typename MatrA, typename SymmC>
    inline
    void syrk (CBLAS_UPLO const uplo, CBLAS_TRANSPOSE trans,
               T const& alpha, MatrA const& a,
               T const& beta, SymmC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmC>::matrix_structure,
        traits::general_t
      >::value));
#endif

      detail::syrk (uplo, trans, alpha, a, beta, c);
    }

    template <typename T, typename MatrA, typename SymmC>
    inline
    void syrk (CBLAS_TRANSPOSE trans,
               T const& alpha, MatrA const& a,
               T const& beta, SymmC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmC>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmC>::uplo_type
#else
          typename SymmC::packed_category
#endif
         >::value);

      detail::syrk (uplo, trans, alpha, a, beta, c);
    }

    // C <- A * A^T + C
    // C <- A^T * A + C
    template <typename MatrA, typename SymmC>
    inline
    void syrk (CBLAS_TRANSPOSE trans, MatrA const& a, SymmC& c) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmC>::value_type val_t;
#else
      typedef typename SymmC::value_type val_t;
#endif
      syrk (trans, (val_t) 1, a, (val_t) 0, c);
    }

    // C <- alpha * A * B^T + conj(alpha) * B * A^T + beta * C
    // C <- alpha * A^T * B + conj(alpha) * B^T * A + beta * C
    // C == C^T

    namespace detail {

      template <typename T, typename MatrA, typename MatrB, typename SymmC>
      inline
      void syr2k (CBLAS_UPLO const uplo, CBLAS_TRANSPOSE trans,
                  T const& alpha, MatrA const& a, MatrB const& b,
                  T const& beta, SymmC& c)
      {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrA>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrA>::ordering_type,
          typename traits::matrix_traits<SymmC>::ordering_type
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::ordering_type,
          typename traits::matrix_traits<SymmC>::ordering_type
        >::value));
#endif

        assert (uplo == CblasUpper || uplo == CblasLower);
        assert (trans == CblasNoTrans
                || trans == CblasTrans
                || trans == CblasConjTrans);

        int const n = traits::matrix_size1 (c);
        assert (n == traits::matrix_size2 (c));

        int const k = trans == CblasNoTrans
          ? traits::matrix_size2 (a)
          : traits::matrix_size1 (a);
        assert (k == (trans == CblasNoTrans
                      ? traits::matrix_size2 (b)
                      : traits::matrix_size1 (b)));
        assert (n == (trans == CblasNoTrans
                      ? traits::matrix_size1 (a)
                      : traits::matrix_size2 (a)));
        assert (n == (trans == CblasNoTrans
                      ? traits::matrix_size1 (b)
                      : traits::matrix_size2 (b)));

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmC>::ordering_type
#else
            typename SymmC::orientation_category
#endif
           >::value);

        syr2k (stor_ord, uplo, trans,
               n, k, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (a),
#else
               traits::matrix_storage_const (a),
#endif
               traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (b),
#else
               traits::matrix_storage_const (b),
#endif
               traits::leading_dimension (b),
               beta,
               traits::matrix_storage (c),
               traits::leading_dimension (c));
      }

    } // detail

    // C <- alpha * A * B^T + conj(alpha) * B * A^T + beta * C
    // C <- alpha * A^T * B + conj(alpha) * B^T * A + beta * C
    // C == C^T
    template <typename T, typename MatrA, typename MatrB, typename SymmC>
    inline
    void syr2k (CBLAS_UPLO const uplo, CBLAS_TRANSPOSE trans,
               T const& alpha, MatrA const& a, MatrB const& b,
               T const& beta, SymmC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmC>::matrix_structure,
        traits::general_t
      >::value));
#endif

      detail::syr2k (uplo, trans, alpha, a, b, beta, c);
    }

    template <typename T, typename MatrA, typename MatrB, typename SymmC>
    inline
    void syr2k (CBLAS_TRANSPOSE trans,
               T const& alpha, MatrA const& a, MatrB const& b,
               T const& beta, SymmC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmC>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmC>::uplo_type
#else
          typename SymmC::packed_category
#endif
         >::value);

      detail::syr2k (uplo, trans, alpha, a, b, beta, c);
    }

    // C <- A * B^T + B * A^T + C
    // C <- A^T * B + B^T * A + C
    template <typename MatrA, typename MatrB, typename SymmC>
    inline
    void syr2k (CBLAS_TRANSPOSE trans,
                MatrA const& a, MatrB const& b, SymmC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmC>::value_type val_t;
#else
      typedef typename SymmC::value_type val_t;
#endif
      syr2k (trans, (val_t) 1, a, b, (val_t) 0, c);
    }

    // C <- alpha * A * A^H + beta * C
    // C <- alpha * A^H * A + beta * C
    // C == C^H

    namespace detail {

      template <typename T, typename MatrA, typename HermC>
      inline
      void herk (CBLAS_UPLO const uplo, CBLAS_TRANSPOSE trans,
                 T const& alpha, MatrA const& a,
                 T const& beta, HermC& c)
      {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrA>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrA>::ordering_type,
          typename traits::matrix_traits<HermC>::ordering_type
        >::value));
#endif

        assert (uplo == CblasUpper || uplo == CblasLower);
        assert (trans == CblasNoTrans || trans == CblasConjTrans);

        int const n = traits::matrix_size1 (c);
        assert (n == traits::matrix_size2 (c));

        int const k = trans == CblasNoTrans
          ? traits::matrix_size2 (a)
          : traits::matrix_size1 (a);
        assert (n == (trans == CblasNoTrans
                      ? traits::matrix_size1 (a)
                      : traits::matrix_size2 (a)));

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<HermC>::ordering_type
#else
            typename HermC::orientation_category
#endif
           >::value);

        herk (stor_ord, uplo, trans,
              n, k, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
              traits::matrix_storage (a),
#else
              traits::matrix_storage_const (a),
#endif
              traits::leading_dimension (a),
              beta,
              traits::matrix_storage (c),
              traits::leading_dimension (c));
      }

    } // detail

    // C <- alpha * A * A^H + beta * C
    // C <- alpha * A^H * A + beta * C
    // C == C^H
    template <typename T, typename MatrA, typename HermC>
    inline
    void herk (CBLAS_UPLO const uplo, CBLAS_TRANSPOSE trans,
               T const& alpha, MatrA const& a,
               T const& beta, HermC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermC>::matrix_structure,
        traits::general_t
      >::value));
#endif

      detail::herk (uplo, trans, alpha, a, beta, c);
    }

    template <typename T, typename MatrA, typename HermC>
    inline
    void herk (CBLAS_TRANSPOSE trans,
               T const& alpha, MatrA const& a,
               T const& beta, HermC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermC>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<HermC>::uplo_type
#else
          typename HermC::packed_category
#endif
         >::value);

      detail::herk (uplo, trans, alpha, a, beta, c);
    }

    // C <- A * A^H + C
    // C <- A^H * A + C
    template <typename MatrA, typename HermC>
    inline
    void herk (CBLAS_TRANSPOSE trans, MatrA const& a, HermC& c) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermC>::value_type val_t;
#else
      typedef typename HermC::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;
      herk (trans, (real_t) 1, a, (real_t) 0, c);
    }

    // C <- alpha * A * B^H + conj(alpha) * B * A^H + beta * C
    // C <- alpha * A^H * B + conj(alpha) * B^H * A + beta * C
    // C == C^H

    namespace detail {

      template <typename T1, typename T2,
                typename MatrA, typename MatrB, typename HermC>
      inline
      void her2k (CBLAS_UPLO const uplo, CBLAS_TRANSPOSE trans,
                  T1 const& alpha, MatrA const& a, MatrB const& b,
                  T2 const& beta, HermC& c)
      {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrA>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrA>::ordering_type,
          typename traits::matrix_traits<HermC>::ordering_type
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::ordering_type,
          typename traits::matrix_traits<HermC>::ordering_type
        >::value));
#endif

        assert (uplo == CblasUpper || uplo == CblasLower);
        assert (trans == CblasNoTrans || trans == CblasConjTrans);

        int const n = traits::matrix_size1 (c);
        assert (n == traits::matrix_size2 (c));

        int const k = trans == CblasNoTrans
          ? traits::matrix_size2 (a)
          : traits::matrix_size1 (a);
        assert (k == (trans == CblasNoTrans
                      ? traits::matrix_size2 (b)
                      : traits::matrix_size1 (b)));
        assert (n == (trans == CblasNoTrans
                      ? traits::matrix_size1 (a)
                      : traits::matrix_size2 (a)));
        assert (n == (trans == CblasNoTrans
                      ? traits::matrix_size1 (b)
                      : traits::matrix_size2 (b)));

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<HermC>::ordering_type
#else
            typename HermC::orientation_category
#endif
           >::value);

        her2k (stor_ord, uplo, trans,
               n, k, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (a),
#else
               traits::matrix_storage_const (a),
#endif
               traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (b),
#else
               traits::matrix_storage_const (b),
#endif
               traits::leading_dimension (b),
               beta,
               traits::matrix_storage (c),
               traits::leading_dimension (c));
      }

    } // detail

    // C <- alpha * A * B^H + conj(alpha) * B * A^H + beta * C
    // C <- alpha * A^H * B + conj(alpha) * B^H * A + beta * C
    // C == C^H
    template <typename T1, typename T2,
              typename MatrA, typename MatrB, typename HermC>
    inline
    void her2k (CBLAS_UPLO const uplo, CBLAS_TRANSPOSE trans,
               T1 const& alpha, MatrA const& a, MatrB const& b,
               T2 const& beta, HermC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermC>::matrix_structure,
        traits::general_t
      >::value));
#endif

      detail::her2k (uplo, trans, alpha, a, b, beta, c);
    }

    template <typename T1, typename T2,
              typename MatrA, typename MatrB, typename HermC>
    inline
    void her2k (CBLAS_TRANSPOSE trans,
               T1 const& alpha, MatrA const& a, MatrB const& b,
               T2 const& beta, HermC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermC>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<HermC>::uplo_type
#else
          typename HermC::packed_category
#endif
         >::value);

      detail::her2k (uplo, trans, alpha, a, b, beta, c);
    }

    // C <- A * B^H + B * A^H + C
    // C <- A^H * B + B^H * A + C
    template <typename MatrA, typename MatrB, typename HermC>
    inline
    void her2k (CBLAS_TRANSPOSE trans,
                MatrA const& a, MatrB const& b, HermC& c)
    {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermC>::value_type val_t;
#else
      typedef typename HermC::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;
      her2k (trans, (val_t) 1, a, b, (real_t) 0, c);
    }

  } // namespace atlas

}}}

#endif // BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_3_HPP


#pragma once

// Switching to Blitz Based Arrays
#include <blitz/array.h>
#include <fftw3.h>
#include <omp.h>
#include <string.h>
#include <algorithm>
#include <cmath>
#include <complex>
#include <fstream>
#include <iostream>
#include <vector>

// It should be ok at populate namespace with complex
using std::complex;

namespace NDarray {

using namespace blitz;

// FFT Libraries complex Float
void fftshift(Array<complex<float>, 3> &temp);
void fftshift(Array<complex<float>, 4> &temp);
void fft(Array<complex<float>, 3> &temp);
void fft(Array<complex<float>, 4> &temp);
void ifft(Array<complex<float>, 3> &temp);
void ifft(Array<complex<float>, 4> &temp);

void fft(Array<complex<float>, 3> &temp, int);
void ifft(Array<complex<float>, 3> &temp, int);
void fft3(Array<complex<float>, 3> &temp, int, int, bool);

void gaussian_filter(Array<float, 2> &, int);
void gaussian_filter(Array<complex<float>, 2> &, int);
complex<float> conj_sum(Array<complex<float>, 3> P, Array<complex<float>, 3> R);
void gaussian_blur(Array<complex<float>, 3> &In, float sigX, float sigY, float sigZ);
void gaussian_blur(Array<float, 3> &In, float sigX, float sigY, float sigZ);

inline void endian_swap(int &x) {
  x = (x << 24 & 0xFF000000) | (x << 8 & 0x00FF0000) | (x >> 8 & 0x0000FF00) |
      (x >> 24 & 0x000000FF);
}

template <typename T, int N>
void ArrayRead(blitz::Array<T, N> &temp, const char *name) {
  FILE *fid;
  if ((fid = fopen(name, "r")) == NULL) {
    cout << "Array:Can't Open " << name << endl;
    cout << "Exiting" << endl;
    exit(1);
  } else {
    int j;
    if ((j = fread(temp.data(), sizeof(T), temp.numElements(), fid)) !=
        (int)(temp.numElements())) {
      cout << "Array3:Not enough data: only read " << j << "points of"
           << temp.numElements() << endl;
      exit(1);
    }
    fclose(fid);
  }
}

template <typename T, const int N_rank>
void ArrayWriteMagAppend(Array<complex<T>, N_rank> &temp, const char *name) {
  ofstream ofs(name, ios_base::binary | ios_base::app);

  for (typename Array<complex<T>, N_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    T val = abs(*miter);
    ofs.write((char *)&val, sizeof(T));
  }
}

template <typename T, const int N_rank>
void ArrayWriteAppend(Array<T, N_rank> &temp, const char *name) {
  ofstream ofs(name, ios_base::binary | ios_base::app);

  for (typename Array<T, N_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    T val = *miter;
    ofs.write((char *)&val, sizeof(T));
  }
}

template <typename T, const int N_rank>
void ArrayWriteAppendAsComplexFloat(Array<T, N_rank> &temp, const char *name) {
  ofstream ofs(name, ios_base::binary | ios_base::app);

  for (typename Array<T, N_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    complex<float> val = *miter;
    ofs.write((char *)&val, sizeof(complex<float>));
  }
}

template <typename T>
void ArrayWriteAppendZeros(int number, const char *name) {
  ofstream ofs(name, ios_base::binary | ios_base::app);

  for (int i = 0; i < number; i++) {
    T val = (T)0.0;  // Zero is zero for all types!
    ofs.write((char *)&val, sizeof(T));
  }
}

template <typename T, const int N_rank>
void ArrayWrite(Array<T, N_rank> &temp, const char *name) {
  remove(name);  // This speeds things up considerably
  ofstream ofs(name, ios_base::binary);

  for (typename Array<T, N_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    T val = *miter;
    ofs.write((char *)&val, sizeof(T));
  }
}

template <typename T, const int N_rank>
void ArrayWriteMag(Array<complex<T>, N_rank> &temp, const char *name) {
  remove(name);  // This speeds things up considerably
  ofstream ofs(name, ios_base::binary);
  for (typename Array<complex<T>, N_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    T val = abs(*miter);
    ofs.write((char *)&val, sizeof(T));
  }
}

template <typename T, const int N_rank>
double ArrayEnergy(Array<complex<T>, N_rank> &temp) {
  double EE = 0;
  for (typename Array<complex<T>, N_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    EE += (double)(norm(*miter));
  }
  return (EE);
}

template <typename T, const int N_rank, const int M_rank>
double ArrayEnergy(Array<Array<complex<T>, N_rank>, M_rank> &temp) {
  double EE = 0;
  for (typename Array<Array<complex<T>, N_rank>, M_rank>::iterator miter =
           temp.begin();
       miter != temp.end(); miter++) {
    EE += ArrayEnergy(*miter);
  }
  return (EE);
}

template <typename T, const int N_rank>
void ArrayWritePhase(Array<complex<T>, N_rank> &temp, const char *name) {
  ofstream ofs(name, ios_base::binary);

  for (typename Array<complex<T>, N_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    T val = arg(*miter);
    ofs.write((char *)&val, sizeof(T));
  }
}

template <typename T, const int N_rank>
void ArrayWritePhaseAppend(Array<complex<T>, N_rank> &temp, const char *name) {
  ofstream ofs(name, ios_base::binary | ios_base::app);

  for (typename Array<complex<T>, N_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    T val = arg(*miter);
    ofs.write((char *)&val, sizeof(T));
  }
}

template <typename T, const int N_rank, const int M_rank>
void WriteCFL(Array<Array<T, N_rank>, M_rank> &temp, const char *name,
              int pad_dim1 = 0) {
  // Create names for header and binary
  char name_hdr[2048];
  char name_bin[2048];

  sprintf(name_hdr, "%s.hdr", name);
  sprintf(name_bin, "%s.cfl", name);

  // Now determine the size of the ND array
  TinyVector<int, M_rank> Dim1 = temp.shape();

  // Loop over all sub-arrays to get the max
  typename Array<Array<T, N_rank>, M_rank>::iterator miter = temp.begin();

  TinyVector<int, N_rank> Dim2 = (*miter).shape();
  for (; miter != temp.end(); miter++) {
    TinyVector<int, N_rank> Dim2_temp = (*miter).shape();
    for (int i = 0; i < N_rank; i++) {
      Dim2(i) = max(Dim2(i), Dim2_temp(i));
    }
  }

  // Combine the Size to get the total size
  Array<int, 1> Dim(N_rank + M_rank + pad_dim1);
  int count = 0;
  for (int i = 0; i < pad_dim1; i++) {
    Dim(count) = 1;
    count++;
  }
  for (int i = 0; i < N_rank; i++) {
    Dim(count) = Dim2(i);
    count++;
  }
  for (int i = 0; i < M_rank; i++) {
    Dim(count) = Dim1(i);
    count++;
  }

  // Debug info (temp)
  cout << "File Name = " << name_bin << endl;
  cout << "Header Name = " << name_hdr << endl;
  cout << "Output Size = " << Dim << endl;

  // Write the header
  FILE *fid;
  fid = fopen(name_hdr, "w");
  fprintf(fid, "# Dimensions\n");
  for (int i = 0; i < M_rank + N_rank; i++) {
    fprintf(fid, "%d ", Dim(i));
  }
  if ((N_rank + M_rank) < 5) {
    for (int i = 0; i < (5 - (M_rank + N_rank)); i++) {
      fprintf(fid, "%d ", 1);
    }
  }
  fclose(fid);

  // Write the binary data
  remove(name_bin);
  int inner_size = product(Dim2);
  for (typename Array<Array<T, N_rank>, M_rank>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    ArrayWriteAppendAsComplexFloat((*miter), name_bin);

    // Bart doesn't support container sizes. Just pad with zeros
    if ((*miter).numElements() < inner_size) {
      ArrayWriteAppendZeros<complex<float> >(
          inner_size - (*miter).numElements(), name_bin);
    }
  }
}

template <typename T, const int N_rank>
void WriteCFL(Array<T, N_rank> &temp, const char *name) {
  // Create names for header and binary
  char name_hdr[2048];
  char name_bin[2048];

  sprintf(name_hdr, "%s.hdr", name);
  sprintf(name_bin, "%s.cfl", name);

  // Now determine the size of the ND array
  TinyVector<int, N_rank> Dim = temp.shape();

  // Debug info (temp)
  cout << "File Name = " << name_bin << endl;
  cout << "Header Name = " << name_hdr << endl;
  cout << "Output Size = " << Dim << endl;

  // Write the header
  FILE *fid;
  fid = fopen(name_hdr, "w");
  fprintf(fid, "# Dimensions\n");
  for (int i = 0; i < N_rank; i++) {
    fprintf(fid, "%d ", Dim(i));
  }
  if ((N_rank) < 5) {
    for (int i = 0; i < (5 - (N_rank)); i++) {
      fprintf(fid, "%d ", 1);
    }
  }
  fclose(fid);

  // Write the binary data
  remove(name_bin);
  ArrayWriteAppendAsComplexFloat(temp, name_bin);
}

template <typename T, const int N_rank, const int M_rank>
void WriteCFL_triplet(Array<Array<T, N_rank>, M_rank> &temp,
                      Array<Array<T, N_rank>, M_rank> &temp2,
                      Array<Array<T, N_rank>, M_rank> &temp3,
                      const char *name) {
  // Create names for header and binary
  char name_hdr[2048];
  char name_bin[2048];

  sprintf(name_hdr, "%s.hdr", name);
  sprintf(name_bin, "%s.cfl", name);

  // Now determine the size of the ND array
  TinyVector<int, M_rank> Dim1 = temp.shape();

  // Loop over all sub-arrays to get the max
  typename Array<Array<T, N_rank>, M_rank>::iterator miter = temp.begin();

  TinyVector<int, N_rank> Dim2 = (*miter).shape();
  for (; miter != temp.end(); miter++) {
    TinyVector<int, N_rank> Dim2_temp = (*miter).shape();
    for (int i = 0; i < N_rank; i++) {
      Dim2(i) = max(Dim2(i), Dim2_temp(i));
    }
  }

  // Combine the Size to get the total size
  TinyVector<int, N_rank + M_rank> Dim;
  for (int i = 0; i < N_rank; i++) {
    Dim(i) = Dim2(i);
  }
  for (int i = 0; i < M_rank; i++) {
    Dim(i + N_rank) = Dim1(i);
  }

  TinyVector<int, N_rank + M_rank + 1> DimTriple;
  DimTriple(0) = 3;
  for (int i = 0; i < (N_rank + M_rank); i++) {
    DimTriple(i + 1) = Dim(i);
  }

  // Debug info (temp)
  cout << "File Name = " << name_bin << endl;
  cout << "Header Name = " << name_hdr << endl;
  cout << "Output Size = " << Dim << endl;

  // Write the header
  FILE *fid;
  fid = fopen(name_hdr, "w");
  fprintf(fid, "# Dimensions\n");
  for (int i = 0; i < M_rank + N_rank + 1; i++) {
    fprintf(fid, "%d ", DimTriple(i));
  }
  if ((N_rank + M_rank + 1) < 5) {
    for (int i = 0; i < (5 - (M_rank + N_rank + 1)); i++) {
      fprintf(fid, "%d ", 1);
    }
  }
  fclose(fid);

  // Write the binary data
  remove(name_bin);
  int inner_size = product(Dim2);
  ofstream ofs(name_bin, ios_base::binary | ios_base::app);

  // Three input arrays
  typename Array<Array<T, N_rank>, M_rank>::iterator miter1 = temp.begin();
  typename Array<Array<T, N_rank>, M_rank>::iterator miter2 = temp2.begin();
  typename Array<Array<T, N_rank>, M_rank>::iterator miter3 = temp3.begin();
  for (; (miter1 != temp.end()); miter1++, miter2++, miter3++) {
    // Pointers to the the inner array
    typename Array<T, N_rank>::iterator niter1 = (*miter1).begin();
    typename Array<T, N_rank>::iterator niter2 = (*miter2).begin();
    typename Array<T, N_rank>::iterator niter3 = (*miter3).begin();

    for (; (niter1 != (*miter1).end()); niter1++, niter2++, niter3++) {
      {
        complex<float> val = *niter1;
        ofs.write((char *)&val, sizeof(complex<float>));
      }
      {
        complex<float> val = *niter2;
        ofs.write((char *)&val, sizeof(complex<float>));
      }
      {
        complex<float> val = *niter3;
        ofs.write((char *)&val, sizeof(complex<float>));
      }
    }

    // Bart doesn't support container sizes. Just pad with zeros
    if ((*miter1).numElements() < inner_size) {
      ArrayWriteAppendZeros<complex<float> >(
          3 * (inner_size - (*miter1).numElements()), name_bin);
    }
  }
}

template <typename T>
Array<Array<T, 3>, 1> Alloc4DContainer(int x, int y, int z, int t) {
  Array<Array<T, 3>, 1> temp;
  temp.setStorage(ColumnMajorArray<1>());
  temp.resize(t);

  for (typename Array<Array<T, 3>, 1>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    (*miter).setStorage(ColumnMajorArray<3>());
    (*miter).resize(x, y, z);
    (*miter) = (T)0;
  }
  return (temp);
}

template <typename T>
Array<Array<T, 3>, 3> Alloc6DContainer(int x, int y, int z, int d1, int d2,
                                       int d3) {
  Array<Array<T, 3>, 3> temp;
  temp.setStorage(ColumnMajorArray<3>());
  temp.resize(d1, d2, d3);

  for (typename Array<Array<T, 3>, 3>::iterator miter = temp.begin();
       miter != temp.end(); miter++) {
    (*miter).setStorage(ColumnMajorArray<3>());
    (*miter).resize(x, y, z);
    (*miter) = (T)0;
  }
  return (temp);
}

template <typename T>
Array<Array<T, 3>, 2> Alloc5DContainer(int x, int y, int z, int d1, int d2) {
  Array<Array<T, 3>, 2> temp;
  temp.setStorage(ColumnMajorArray<2>());
  temp.resize(d1, d2);

  for (int i = 0; i < d1; i++) {
    for (int j = 0; j < d2; j++) {
      temp(i, j).setStorage(ColumnMajorArray<3>());
      temp(i, j).resize(x, y, z);
      temp(i, j) = (T)0;
    }
  }

  /* This leads to errors for no real reason
  for( typename Array< Array<T,3>,2>::iterator miter=temp.begin();   miter
  !=temp.end(); miter++){
          (*miter).setStorage(ColumnMajorArray<3>());
          (*miter).resize(x,y,z);
          (*miter)= (T )0;
  }
  */
  return (temp);
}

double Dmax(const Array<Array<double, 2>, 1> &A);
double Dmin(const Array<Array<double, 2>, 1> &A);

void nested_workaround(long index, int *N, int *idx, int total);

}  // namespace NDarray


// Vector production via a one-loop box diagram
//
// Author:       Daniel Winney (2021)
// Affiliation:  Joint Physics Analysis Center (JPAC)
// Email:        dwinney@iu.edu
// ---------------------------------------------------------------------------

#ifndef _BOX_AMP_
#define _BOX_AMP_

#include "constants.hpp"
#include "amplitudes/amplitude.hpp"
#include "amplitudes/reaction_kinematics.hpp"
#include "box/box_discontinuity.hpp"

#include <boost/math/quadrature/gauss_kronrod.hpp>

#include "Math/GSLIntegrator.h"
#include "Math/GaussLegendreIntegrator.h"
#include "Math/IntegrationTypes.h"
#include "Math/Functor.h"

namespace jpacPhoto
{
    class box_amplitude : public amplitude
    {
        public:
        // Constructor.
        // Need the parent reaction kinematics and pre-set sub-amplitudes
        box_amplitude(reaction_kinematics * xkinem, box_discontinuity * disc, std::string id = "Box Amplitude")
        : amplitude(xkinem, id), _disc(disc)
        {};

        box_amplitude(reaction_kinematics * xkinem, amplitude * left, amplitude * right, std::string id = "Box Amplitude")
        : amplitude(xkinem, id)
        {
            _disc = new box_discontinuity(left, right);
            _needDelete = true;
        };

        // Destructor
        ~box_amplitude()
        {
            if (_needDelete) delete _disc;
        };

        // Setter for max cutoff in dispersion relation
        inline void set_cutoff(double s_cut)
        {
            _s_cut = s_cut;
        };

        // only vector available
        inline std::vector<std::array<int,2>> allowedJP()
        {
            return { {1, -1} };
        };

        // Evaluate the helicity amplitude by dispersing
        std::complex<double> helicity_amplitude(std::array<int, 4> helicities, double s, double t);

        // Override the jpacPhoto::amplitude::integrated_xsection
        double integrated_xsection(double s);

        private:

        // Discontinutity given in terms of the two tree amplitudes
        box_discontinuity * _disc;
        bool _needDelete = false;

        // Integration momentum cutoff. Defaults to 2 GeV (an arbitrary but sensible value)
        double _s_cut = 2.;
    };
};

#endif


#include "util/coordinate.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/trigonometry_table.hpp"
#include "util/web_mercator.hpp"

#include <boost/assert.hpp>

#include <cmath>

#include <limits>
#include <utility>

namespace osrm
{
namespace util
{

namespace coordinate_calculation
{

// Does not project the coordinates!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs)
{
    const std::uint64_t dx = static_cast<std::int32_t>(lhs.lon - rhs.lon);
    const std::uint64_t dy = static_cast<std::int32_t>(lhs.lat - rhs.lat);

    return dx * dx + dy * dy;
}

double haversineDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    auto lon1 = static_cast<int>(coordinate_1.lon);
    auto lat1 = static_cast<int>(coordinate_1.lat);
    auto lon2 = static_cast<int>(coordinate_2.lon);
    auto lat2 = static_cast<int>(coordinate_2.lat);
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    const double lt1 = lat1 / COORDINATE_PRECISION;
    const double ln1 = lon1 / COORDINATE_PRECISION;
    const double lt2 = lat2 / COORDINATE_PRECISION;
    const double ln2 = lon2 / COORDINATE_PRECISION;

    const double dlat1 = lt1 * detail::DEGREE_TO_RAD;
    const double dlong1 = ln1 * detail::DEGREE_TO_RAD;
    const double dlat2 = lt2 * detail::DEGREE_TO_RAD;
    const double dlong2 = ln2 * detail::DEGREE_TO_RAD;

    const double dlong = dlong1 - dlong2;
    const double dlat = dlat1 - dlat2;

    const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) +
                         std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2);
    const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
    return detail::EARTH_RADIUS * charv;
}

double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    auto lon1 = static_cast<int>(coordinate_1.lon);
    auto lat1 = static_cast<int>(coordinate_1.lat);
    auto lon2 = static_cast<int>(coordinate_2.lon);
    auto lat2 = static_cast<int>(coordinate_2.lat);
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const double float_lat1 = (lat1 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lon1 = (lon1 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lat2 = (lat2 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lon2 = (lon2 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;

    const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0);
    const double y_value = float_lat2 - float_lat1;
    return std::hypot(x_value, y_value) * detail::EARTH_RADIUS;
}

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio)
{
    using namespace coordinate_calculation;

    BOOST_ASSERT(query_location.IsValid());

    FloatCoordinate projected_nearest;
    std::tie(ratio, projected_nearest) =
        projectPointOnSegment(web_mercator::fromWGS84(segment_source),
                              web_mercator::fromWGS84(segment_target),
                              web_mercator::fromWGS84(query_location));
    nearest_location = web_mercator::toWGS84(projected_nearest);

    const double approximate_distance = greatCircleDistance(query_location, nearest_location);
    BOOST_ASSERT(0.0 <= approximate_distance);
    return approximate_distance;
}

double perpendicularDistance(const Coordinate source_coordinate,
                             const Coordinate target_coordinate,
                             const Coordinate query_location)
{
    double ratio;
    Coordinate nearest_location;

    return perpendicularDistance(
        source_coordinate, target_coordinate, query_location, nearest_location, ratio);
}

Coordinate centroid(const Coordinate lhs, const Coordinate rhs)
{
    Coordinate centroid;
    // The coordinates of the midpoints are given by:
    // x = (x1 + x2) /2 and y = (y1 + y2) /2.
    centroid.lon = (lhs.lon + rhs.lon) / FixedLongitude{2};
    centroid.lat = (lhs.lat + rhs.lat) / FixedLatitude{2};
    return centroid;
}

double degToRad(const double degree)
{
    using namespace boost::math::constants;
    return degree * (pi<double>() / 180.0);
}

double radToDeg(const double radian)
{
    using namespace boost::math::constants;
    return radian * (180.0 * (1. / pi<double>()));
}

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate)
{
    const double lon_diff =
        static_cast<double>(toFloating(second_coordinate.lon - first_coordinate.lon));
    const double lon_delta = degToRad(lon_diff);
    const double lat1 = degToRad(static_cast<double>(toFloating(first_coordinate.lat)));
    const double lat2 = degToRad(static_cast<double>(toFloating(second_coordinate.lat)));
    const double y = std::sin(lon_delta) * std::cos(lat2);
    const double x =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    double result = radToDeg(std::atan2(y, x));
    while (result < 0.0)
    {
        result += 360.0;
    }

    while (result >= 360.0)
    {
        result -= 360.0;
    }
    return result;
}

double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third)
{
    using namespace boost::math::constants;
    using namespace coordinate_calculation;

    if (first == second || second == third)
        return 180;

    BOOST_ASSERT(first.IsValid());
    BOOST_ASSERT(second.IsValid());
    BOOST_ASSERT(third.IsValid());

    const double v1x = static_cast<double>(toFloating(first.lon - second.lon));
    const double v1y =
        web_mercator::latToY(toFloating(first.lat)) - web_mercator::latToY(toFloating(second.lat));
    const double v2x = static_cast<double>(toFloating(third.lon - second.lon));
    const double v2y =
        web_mercator::latToY(toFloating(third.lat)) - web_mercator::latToY(toFloating(second.lat));

    double angle = (atan2_lookup(v2y, v2x) - atan2_lookup(v1y, v1x)) * 180. / pi<double>();

    while (angle < 0.)
    {
        angle += 360.;
    }

    BOOST_ASSERT(angle >= 0);
    return angle;
}

boost::optional<Coordinate>
circleCenter(const Coordinate C1, const Coordinate C2, const Coordinate C3)
{
    // free after http://paulbourke.net/geometry/circlesphere/
    // require three distinct points
    if (C1 == C2 || C2 == C3 || C1 == C3)
    {
        return boost::none;
    }

    // define line through c1, c2 and c2,c3
    const double C2C1_lat = static_cast<double>(toFloating(C2.lat - C1.lat)); // yDelta_a
    const double C2C1_lon = static_cast<double>(toFloating(C2.lon - C1.lon)); // xDelta_a
    const double C3C2_lat = static_cast<double>(toFloating(C3.lat - C2.lat)); // yDelta_b
    const double C3C2_lon = static_cast<double>(toFloating(C3.lon - C2.lon)); // xDelta_b

    // check for collinear points in X-Direction / Y-Direction
    if ((std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon() &&
         std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon()) ||
        (std::abs(C2C1_lat) < std::numeric_limits<double>::epsilon() &&
         std::abs(C3C2_lat) < std::numeric_limits<double>::epsilon()))
    {
        return boost::none;
    }
    else if (std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon())
    {
        // vertical line C2C1
        // due to c1.lon == c2.lon && c1.lon != c3.lon we can rearrange this way
        BOOST_ASSERT(std::abs(static_cast<double>(toFloating(C3.lon - C1.lon))) >=
                         std::numeric_limits<double>::epsilon() &&
                     std::abs(static_cast<double>(toFloating(C2.lon - C3.lon))) >=
                         std::numeric_limits<double>::epsilon());
        return circleCenter(C1, C3, C2);
    }
    else if (std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon())
    {
        // vertical line C3C2
        // due to c2.lon == c3.lon && c1.lon != c3.lon we can rearrange this way
        // after rearrangement both deltas will be zero
        BOOST_ASSERT(std::abs(static_cast<double>(toFloating(C1.lon - C2.lon))) >=
                         std::numeric_limits<double>::epsilon() &&
                     std::abs(static_cast<double>(toFloating(C3.lon - C1.lon))) >=
                         std::numeric_limits<double>::epsilon());
        return circleCenter(C2, C1, C3);
    }
    else
    {
        const double C2C1_slope = C2C1_lat / C2C1_lon;
        const double C3C2_slope = C3C2_lat / C3C2_lon;

        if (std::abs(C2C1_slope) < std::numeric_limits<double>::epsilon())
        {
            // Three non-collinear points with C2,C1 on same latitude.
            // Due to the x-values correct, we can swap C3 and C1 to obtain the correct slope value
            return circleCenter(C3, C2, C1);
        }
        // valid slope values for both lines, calculate the center as intersection of the lines

        // can this ever happen?
        if (std::abs(C2C1_slope - C3C2_slope) < std::numeric_limits<double>::epsilon())
            return boost::none;

        const double C1_y = static_cast<double>(toFloating(C1.lat));
        const double C1_x = static_cast<double>(toFloating(C1.lon));
        const double C2_y = static_cast<double>(toFloating(C2.lat));
        const double C2_x = static_cast<double>(toFloating(C2.lon));
        const double C3_y = static_cast<double>(toFloating(C3.lat));
        const double C3_x = static_cast<double>(toFloating(C3.lon));

        const double lon = (C2C1_slope * C3C2_slope * (C1_y - C3_y) + C3C2_slope * (C1_x + C2_x) -
                            C2C1_slope * (C2_x + C3_x)) /
                           (2 * (C3C2_slope - C2C1_slope));
        const double lat = (0.5 * (C1_x + C2_x) - lon) / C2C1_slope + 0.5 * (C1_y + C2_y);
        if (lon < -180.0 || lon > 180.0 || lat < -90.0 || lat > 90.0)
            return boost::none;
        else
            return Coordinate(FloatLongitude{lon}, FloatLatitude{lat});
    }
}

double circleRadius(const Coordinate C1, const Coordinate C2, const Coordinate C3)
{
    // a circle by three points requires thee distinct points
    auto center = circleCenter(C1, C2, C3);
    if (center)
        return haversineDistance(C1, *center);
    else
        return std::numeric_limits<double>::infinity();
}

Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to)
{
    BOOST_ASSERT(0 <= factor && factor <= 1.0);

    const auto from_lon = static_cast<std::int32_t>(from.lon);
    const auto from_lat = static_cast<std::int32_t>(from.lat);
    const auto to_lon = static_cast<std::int32_t>(to.lon);
    const auto to_lat = static_cast<std::int32_t>(to.lat);

    FixedLongitude interpolated_lon{
        static_cast<std::int32_t>(from_lon + factor * (to_lon - from_lon))};
    FixedLatitude interpolated_lat{
        static_cast<std::int32_t>(from_lat + factor * (to_lat - from_lat))};

    return {std::move(interpolated_lon), std::move(interpolated_lat)};
}

// compute the signed area of a triangle
double signedArea(const Coordinate first_coordinate,
                  const Coordinate second_coordinate,
                  const Coordinate third_coordinate)
{
    const auto lat_1 = static_cast<double>(toFloating(first_coordinate.lat));
    const auto lon_1 = static_cast<double>(toFloating(first_coordinate.lon));
    const auto lat_2 = static_cast<double>(toFloating(second_coordinate.lat));
    const auto lon_2 = static_cast<double>(toFloating(second_coordinate.lon));
    const auto lat_3 = static_cast<double>(toFloating(third_coordinate.lat));
    const auto lon_3 = static_cast<double>(toFloating(third_coordinate.lon));
    return 0.5 * (-lon_2 * lat_1 + lon_3 * lat_1 + lon_1 * lat_2 - lon_3 * lat_2 - lon_1 * lat_3 +
                  lon_2 * lat_3);
}

// check if a set of three coordinates is given in CCW order
bool isCCW(const Coordinate first_coordinate,
           const Coordinate second_coordinate,
           const Coordinate third_coordinate)
{
    return signedArea(first_coordinate, second_coordinate, third_coordinate) > 0;
}

std::pair<util::Coordinate, util::Coordinate>
leastSquareRegression(const std::vector<util::Coordinate> &coordinates)
{
    BOOST_ASSERT(coordinates.size() >= 2);
    double sum_lon = 0, sum_lat = 0, sum_lon_lat = 0, sum_lon_lon = 0;
    double min_lon = static_cast<double>(toFloating(coordinates.front().lon));
    double max_lon = static_cast<double>(toFloating(coordinates.front().lon));
    for (const auto coord : coordinates)
    {
        min_lon = std::min(min_lon, static_cast<double>(toFloating(coord.lon)));
        max_lon = std::max(max_lon, static_cast<double>(toFloating(coord.lon)));
        sum_lon += static_cast<double>(toFloating(coord.lon));
        sum_lon_lon +=
            static_cast<double>(toFloating(coord.lon)) * static_cast<double>(toFloating(coord.lon));
        sum_lat += static_cast<double>(toFloating(coord.lat));
        sum_lon_lat +=
            static_cast<double>(toFloating(coord.lon)) * static_cast<double>(toFloating(coord.lat));
    }

    const auto dividend = coordinates.size() * sum_lon_lat - sum_lon * sum_lat;
    const auto divisor = coordinates.size() * sum_lon_lon - sum_lon * sum_lon;
    if (std::abs(divisor) < std::numeric_limits<double>::epsilon())
        return std::make_pair(coordinates.front(), coordinates.back());

    // slope of the regression line
    const auto slope = dividend / divisor;
    const auto intercept = (sum_lat - slope * sum_lon) / coordinates.size();

    const auto GetLatAtLon = [intercept,
                              slope](const util::FloatLongitude longitude) -> util::FloatLatitude {
        return {intercept + slope * static_cast<double>((longitude))};
    };

    const util::Coordinate regression_first = {
        toFixed(util::FloatLongitude{min_lon - 1}),
        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{min_lon - 1})))};
    const util::Coordinate regression_end = {
        toFixed(util::FloatLongitude{max_lon + 1}),
        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{max_lon + 1})))};

    return {regression_first, regression_end};
}

} // ns coordinate_calculation
} // ns util
} // ns osrm


#include <stdlib.h>
#include <assert.h>
#include <time.h>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <vector>
#include <set>
#include <boost/numeric/ublas/matrix.hpp>

#include "../Point.h"
#include "../ANN.h"
#include "../RMSUtils.h"
#include "../IOUtil.h"

using namespace std;

vector<Point> validDirs;
vector<double> validResults;

bool validate_coreset(const vector<Point> &fatP, vector<size_t> &idxs, double epsilon) {
    assert(idxs.size() <= fatP.size());

    if (fatP.empty())
        return true;

    if (idxs.empty())
        return false;

    assert(idxs[0] >= 0 && idxs[idxs.size() - 1] < fatP.size());

    for (size_t i = 0; i < validDirs.size(); i++) {
        double cor_max = fatP[idxs[0]].dotP(validDirs[i]);
        for (size_t j = 1; j < idxs.size(); j++) {
            double corval = fatP[idxs[j]].dotP(validDirs[i]);
            if (corval > cor_max)
                cor_max = corval;
        }

        double ptval = validResults[i];
        if (cor_max < (1 - epsilon) * ptval)
            return false;
    }
    return true;
}

void loss_distribution(const vector<Point> &coreset, const vector<Point> &queries,
                       const vector<double> &results, vector<double> &regret_dist) {
    int n = min(queries.size(), results.size());
    regret_dist.reserve(1000);

    vector<double> regret_ratios(n);
    for (int i = 0; i < n; i++) {
        assert(results[i] > 0);
        double dot_max = 0;
        for (int j = 0; j < coreset.size(); ++j) {
            double dotp = coreset[j].dotP(queries[i]);
            if (dotp > dot_max)
                dot_max = dotp;
        }

        double regret_ratio_i = (results[i] - dot_max) / results[i];
        regret_ratios[i] = regret_ratio_i;
    }

    sort (regret_ratios.begin(), regret_ratios.end());

    for (int i = 1; i < 1000; ++i) {
        regret_dist[i - 1] = regret_ratios[n * i / 1000];
    }
    regret_dist[999] = regret_ratios[n - 1];
}

bool validate_eps_kernel(const vector<Point> &fatP, vector<size_t> &idxs, double epsilon) {
    assert(idxs.size() <= fatP.size());

    if (fatP.empty())
        return true;

    if (idxs.empty())
        return false;

    assert(idxs[0] >= 0 && idxs[idxs.size() - 1] < fatP.size());

    size_t m = validDirs.size() / 2;
    for (size_t i = 0; i < m; i++) {
        double cor_max = fatP[idxs[0]].dotP(validDirs[2 * i]);
        double cor_min = fatP[idxs[0]].dotP(validDirs[2 * i + 1]);
        for (size_t j = 1; j < idxs.size(); j++) {
            double cor_val1 = fatP[idxs[j]].dotP(validDirs[2 * i]);
            double cor_val2 = fatP[idxs[j]].dotP(validDirs[2 * i + 1]);
            if (cor_val1 > cor_max)
                cor_max = cor_val1;
            if (cor_val2 > cor_min)
                cor_min = cor_val2;
        }

        if ((cor_max + cor_min) < (1 - epsilon) * (validResults[2 * i] + validResults[2 * i + 1]))
            return false;
    }
    return true;
}

void coreset_by_sample(ANN *ann_ds, size_t dim, double outer_rad, double delta, size_t deltam, vector<size_t> &idxs) {
    vector<Point> randomP;
    RMSUtils::get_random_sphere_points(outer_rad, dim, deltam, randomP, false);

    vector<size_t> new_idxs;
    ann_ds->getANNs(randomP, delta, new_idxs);

    //figure out all the distinct indices
    set<size_t> unique_idxs;
    for (size_t i = 0; i < idxs.size(); i++)
        unique_idxs.insert(idxs[i]);
    for (size_t i = 0; i < new_idxs.size(); i++)
        unique_idxs.insert(new_idxs[i]);

    idxs.clear();

    set<size_t>::iterator it;
    for (it = unique_idxs.begin(); it != unique_idxs.end(); ++it)
        idxs.push_back((*it));
}

vector<size_t> get_coreset(const vector<Point> &fatP, const int r, const double epsilon, bool &isValid) {
    size_t dim = fatP[0].get_dimension();

    double outer_rad;
    outer_rad = 1 + sqrt(dim);
    double delta = epsilon / (2 * outer_rad);

    vector<size_t> idxs;
    ANN *ann_ds = new ANN();
    ann_ds->insertPts(fatP);

    size_t m = r;
    bool b = false;
    while (!b) {
        coreset_by_sample(ann_ds, dim, outer_rad, delta, m, idxs);
        b = validate_coreset(fatP, idxs, epsilon);
        m += 5;

        if (b && idxs.size() <= r) {
            isValid = true;
            break;
        }

        if (idxs.size() > r) {
            isValid = false;
            break;
        }
    }

    delete ann_ds;
    return idxs;
}

int main(int argc, char **argv) {
    if (argc < 7) {
        cerr << "coreset: Usage " << argv[0] << "<r> <dim> <data_path> <query_path> <valid_path> <output_path>\n";
        exit(1);
    }

    int r = atoi(argv[1]);
    size_t dim = atoi(argv[2]);

    vector<Point> fatP;

    IOUtil::read_input_points(argv[3], dim, fatP);
    IOUtil::read_validate_dirs(argv[4], dim, validDirs);
    IOUtil::read_validate_results(argv[5], validResults);

    ofstream result_file;
    result_file.open(argv[6], ofstream::out | ofstream::app);

    cout << "ann " << argv[3] << " " << fatP.size() << " " << dim << endl;

    result_file << "dataset=" << argv[3] << " r=" << r << "\n" << flush;

    double eps = 0.2;

    vector<size_t> idxs;
    bool isValid;
    while (true) {
        cout << eps << endl;
        idxs = get_coreset(fatP, r, eps, isValid);
        if (isValid) {
            break;
        } else {
            eps += 0.01;
        }
    }

    vector<Point> coreset;
    vector<double> regretDist;
    coreset.reserve(idxs.size());
    for (int i = 0; i < idxs.size(); ++i)
        coreset.push_back(fatP[idxs[i]]);
    loss_distribution(coreset, validDirs, validResults, regretDist);

    cout << regretDist[999] << endl;

    for (int i = 0; i < 1000; ++i)
        result_file << regretDist[i] << "\n";
    result_file.flush();
    result_file.close();
}


#ifndef ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP
#define ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP

#include <aslam/backend/JacobianContainer.hpp>
#include "RotationExpressionNode.hpp"
#include "ScalarExpressionNode.hpp"
#include "TransformationExpressionNode.hpp"
#include "MatrixExpressionNode.hpp"
#include <boost/shared_ptr.hpp>
#include <Eigen/Core>
#include <sm/kinematics/RotationalKinematics.hpp>
#include <aslam/backend/VectorExpressionNode.hpp>

namespace aslam {
  namespace backend {
    template <int D> class VectorExpression;
    class HomogeneousExpressionNode;
    /**
     * \class EuclideanExpressionNode
     * \brief The superclass of all classes representing euclidean points.
     */
    typedef VectorExpressionNode<3>  EuclideanExpressionNode;

    /**
     * \class EuclideanExpressionNodeMultiply
     *
     * \brief A class representing the multiplication of two euclidean matrices.
     *
     */
    class EuclideanExpressionNodeMultiply : public EuclideanExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      EuclideanExpressionNodeMultiply(boost::shared_ptr<RotationExpressionNode> lhs,
                                      boost::shared_ptr<EuclideanExpressionNode> rhs);
      ~EuclideanExpressionNodeMultiply() override;

      void accept(ExpressionNodeVisitor& visitor) override;
    private:
      Eigen::Vector3d evaluateImplementation() const override;
      void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
      void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

      boost::shared_ptr<RotationExpressionNode> _lhs;
      mutable Eigen::Matrix3d _C_lhs;
      boost::shared_ptr<EuclideanExpressionNode> _rhs;
      mutable Eigen::Vector3d _p_rhs;
    };

    // ## New Class for Multiplication with a MatrixExpression
    /**
      * \class EuclideanExpressionNodeMatrixMultiply
      *
      * \brief A class representing the multiplication of a Matrix with a euclidean Vector.
      *
      */
     class EuclideanExpressionNodeMatrixMultiply : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeMatrixMultiply(boost::shared_ptr<MatrixExpressionNode> lhs, boost::shared_ptr<EuclideanExpressionNode> rhs);
       ~EuclideanExpressionNodeMatrixMultiply() override;

     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<MatrixExpressionNode> _lhs;
       mutable Eigen::Matrix3d _A_lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
       mutable Eigen::Vector3d _p_rhs;

     };

    /**
      * \class EuclideanExpressionNodeCrossEuclidean
      *
      * \brief A class representing the cross product of two euclidean expressions.
      *
      */
     class EuclideanExpressionNodeCrossEuclidean : public EuclideanExpressionNode
     {
     public:
       EuclideanExpressionNodeCrossEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
           boost::shared_ptr<EuclideanExpressionNode> rhs);
       ~EuclideanExpressionNodeCrossEuclidean() override;

       void accept(ExpressionNodeVisitor& visitor) override;
     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
     };

     /**
       * \class EuclideanExpressionNodeAddEuclidean
       *
       * \brief A class representing the addition of two euclidean expressions.
       *
       */
      class EuclideanExpressionNodeAddEuclidean : public EuclideanExpressionNode
      {
      public:
        EuclideanExpressionNodeAddEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
            boost::shared_ptr<EuclideanExpressionNode> rhs);
        ~EuclideanExpressionNodeAddEuclidean() override;

        void accept(ExpressionNodeVisitor& visitor) override;
      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<EuclideanExpressionNode> _lhs;
        boost::shared_ptr<EuclideanExpressionNode> _rhs;
      };

      /**
      * \class EuclideanExpressionNodeSubtractEuclidean
      *
      * \brief A class representing the subtraction of two Euclidean expressions.
      *
      */
     class EuclideanExpressionNodeSubtractEuclidean : public EuclideanExpressionNode
     {
     public:
       EuclideanExpressionNodeSubtractEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
           boost::shared_ptr<EuclideanExpressionNode> rhs);
       ~EuclideanExpressionNodeSubtractEuclidean() override;

     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
     };

    /**
      * \class EuclideanExpressionNodeSubtractVector
      *
      * \brief A class representing the subtraction of a vector from an Euclidean expression.
      *
      */
     class EuclideanExpressionNodeSubtractVector : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeSubtractVector(boost::shared_ptr<EuclideanExpressionNode> lhs,
              const Eigen::Vector3d & rhs);
       ~EuclideanExpressionNodeSubtractVector() override;

     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       Eigen::Vector3d _rhs;
     };

     /**
       * \class EuclideanExpressionNodeSubtractVector
       *
       * \brief A class representing the negated Euclidean expression.
       *
       */
      class EuclideanExpressionNodeNegated : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeNegated(boost::shared_ptr<EuclideanExpressionNode> operand);
        ~EuclideanExpressionNodeNegated() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<EuclideanExpressionNode> _operand;
      };

     /**
       * \class EuclideanExpressionNodeScalarMultiply
       *
       * \brief A class representing the multiplication of a ScalarExpression with an Euclidean expression.
       *
       */
      class EuclideanExpressionNodeScalarMultiply : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeScalarMultiply(boost::shared_ptr<EuclideanExpressionNode> p, boost::shared_ptr<ScalarExpressionNode> s);
        ~EuclideanExpressionNodeScalarMultiply() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<EuclideanExpressionNode> _p;
        boost::shared_ptr<ScalarExpressionNode> _s;
      };

      class EuclideanExpressionNodeTranslation : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeTranslation(boost::shared_ptr<TransformationExpressionNode> operand);
        ~EuclideanExpressionNodeTranslation() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<TransformationExpressionNode> _operand;
      };

  class EuclideanExpressionNodeRotationParameters : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeRotationParameters(boost::shared_ptr<RotationExpressionNode> operand, sm::kinematics::RotationalKinematics::Ptr rk);
        ~EuclideanExpressionNodeRotationParameters() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<RotationExpressionNode> _operand;
        sm::kinematics::RotationalKinematics::Ptr _rk;
      };

  class EuclideanExpressionNodeFromHomogeneous : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeFromHomogeneous(boost::shared_ptr<HomogeneousExpressionNode> root);
        ~EuclideanExpressionNodeFromHomogeneous() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<HomogeneousExpressionNode> _root;
      };

    /**
      * \class EuclideanExpressionNodeElementwiseMultiplyEuclidean
      *
      * \brief A class representing the elementwise product of two euclidean expressions.
      *
      */
     class EuclideanExpressionNodeElementwiseMultiplyEuclidean : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeElementwiseMultiplyEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
           boost::shared_ptr<EuclideanExpressionNode> rhs);
       ~EuclideanExpressionNodeElementwiseMultiplyEuclidean() override;

     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
     };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP */


#include "../../include/waveO1/observer.hpp"
#include "../../include/waveO1/utilities.hpp"
#include "../../include/mymfem/utilities.hpp"

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

double WaveO1Observer
:: eval_dgpFtimeError (std::shared_ptr<GridFunction>& p,
                      std::shared_ptr<GridFunction>& v,
                      double t) const
{
    double errp=0;
    double errv=0;

    // assumes that all elements have the same geometry
    auto faceElGeomType = p->FESpace()->GetMesh()
                ->GetFaceGeometryType(0);

    // L2 error in pressure
    // set integration rules
    IntegrationRules rule1{};
    const IntegrationRule *ir1;
    int order1 = 2*p->FESpace()->GetFE(0)->GetOrder()+2;
    ir1 = &rule1.Get(faceElGeomType, order1);
    m_pE_coeff->SetTime(t);
    m_assembleDgpSmoothError->SetIntegrationRule(ir1);
    errp = m_assembleDgpSmoothError->Ftime(p.get(), m_pE_coeff.get());

    // L2 error in velocity
    // set integration rules
    IntegrationRules rule2{};
    const IntegrationRule *ir2;
    int order2 = 2*v->FESpace()->GetFE(0)->GetOrder()+2;
    ir2 = &rule2.Get(faceElGeomType, order2);
    m_vE_coeff->SetTime(t);
    m_assembleDgpSmoothError->SetIntegrationRule(ir2);
    errv = m_assembleDgpSmoothError->Ftime(v.get(), m_vE_coeff.get());

    return (errp + errv);
}

//! Computes DG error for a space-like face
//! at a specified time
double WaveO1Observer
:: eval_dgpFspaceError
(std::shared_ptr<GridFunction>& p,
 std::shared_ptr<GridFunction>& v,
 double t,
 std::shared_ptr<WaveO1InvSqMediumCoeff>&) const
{
    double errp=0;
    double errv=0;

    // assumes that all elements have the same geometry
    auto elGeomType = p->FESpace()
                ->GetFE(0)->GetGeomType();

    IntegrationRules rule1{}, rule2{};
    const IntegrationRule *ir1, *ir2;

    m_pE_coeff->SetTime(t);
    m_vE_coeff->SetTime(t);

    // pressure
    int order1 = 2*p->FESpace()->GetFE(0)->GetOrder()+2;
    ir1 = &rule1.Get(elGeomType, order1);
    m_assembleDgpSmoothError->SetIntegrationRule(ir1);
    errp = m_assembleDgpSmoothError->Fspace
            (p.get(), m_pE_coeff.get());
            //(p.get(), m_pE_coeff.get(), invSqMed.get());

    // velocity
    int order2 = 2*v->FESpace()->GetFE(0)->GetOrder()+2;
    ir2 = &rule2.Get(elGeomType, order2);
    m_assembleDgpSmoothError->SetIntegrationRule(ir2);
    m_vE->ProjectCoefficient(*m_vE_coeff);
    errv = m_assembleDgpSmoothError->Fspace(v.get(), m_vE_coeff.get());

    return (errp + errv);
}

//! Computes DG^{+} error
std::tuple <double, double> WaveO1Observer
:: eval_xtDgpError (BlockVector& W) const
{
    double errDgFtime=0;
    double errDgFspace=0;

    int Nt = m_tWspace->GetNE();
    int xdimW1 = m_xW1space->GetTrueVSize();
    int xdimW2 = m_xW2space->GetTrueVSize();

    Vector& p = W.GetBlock(0);
    Vector& v = W.GetBlock(1);

    Vector pSol(xdimW1);
    std::shared_ptr<GridFunction> pGSol
            = std::make_shared<GridFunction>
            (m_xW1space, pSol);

    Vector vSol(xdimW2);
    std::shared_ptr<GridFunction> vGSol
            = std::make_shared<GridFunction>
            (m_xW2space, vSol);

    // medium
    auto invSqMed = std::make_shared<WaveO1InvSqMediumCoeff>
            (m_testCase);

    ElementTransformation *tTrans = nullptr;
    const FiniteElement *tFe = nullptr;
    Vector tShape;
    Array<int> tVdofs;

    // DG^{+} error for time-like faces
    Eigen::VectorXd bufErrFtime(Nt);
    bufErrFtime.setZero();

    double errFtime;
    for (int n=0; n<Nt; n++)
    {
        m_tWspace->GetElementVDofs(n, tVdofs);
        tTrans = m_tWspace->GetElementTransformation(n);

        tFe = m_tWspace->GetFE(n);
        int tNdofs = tFe->GetDof();
        tShape.SetSize(tNdofs);

        int order = 2*tFe->GetOrder()+2;
        const IntegrationRule *ir
                = &IntRules.Get(tFe->GetGeomType(), order);

        for (int i = 0; i < ir->GetNPoints(); i++)
        {
            const IntegrationPoint &ip = ir->IntPoint(i);
            tTrans->SetIntPoint(&ip);
            tFe->CalcShape(ip, tShape);

            // build solution at time t
            Vector t;
            tTrans->Transform(ip, t);
            build_xSol_FG(p, tShape, tVdofs, pSol);
            build_xSol_FG(v, tShape, tVdofs, vSol);

            errFtime = eval_dgpFtimeError(pGSol, vGSol, t(0));

            double w = ip.weight*tTrans->Weight();
            bufErrFtime(n) += w*errFtime;
        }
    }
    errDgFtime = std::sqrt(bufErrFtime.sum());

    // DG^{+} error for space-like faces
    Eigen::VectorXd bufErrFspace(Nt);
    bufErrFspace.setZero();

    Vector t;
    IntegrationPoint ip0;
    ip0.Set1w(0, 1);

    for (int n=0; n<Nt; n++)
    {
        int np = n;

        // build solution at tn^{+}
        m_tWspace->GetElementVDofs(np, tVdofs);
        tTrans = m_tWspace->GetElementTransformation(np);
        tFe = m_tWspace->GetFE(np);
        int tNdofs = tFe->GetDof();
        tShape.SetSize(tNdofs);

        tTrans->SetIntPoint(&ip0);
        tFe->CalcShape(ip0, tShape);
        tTrans->Transform(ip0, t);
        build_xSol_FG(p, tShape, tVdofs, pSol);
        build_xSol_FG(v, tShape, tVdofs, vSol);

        // compute error
        bufErrFspace(n) = eval_dgpFspaceError
                (pGSol, vGSol, t(0), invSqMed);
    }
    errDgFspace = std::sqrt(2*bufErrFspace.sum());

    return {errDgFtime, errDgFspace};
}

// End of file