#ifndef __MATH_OPERATIONS__
#define __MATH_OPERATIONS__

#include "Utilities/string_operations.hpp"

#include <GEL/CGLA/CGLA.h>

#include <vector>

// Macros
#define sgn(x) ((x) >= (0.0) ? (1.0) : (-1.0))

/** Collection of usefull utilities.
 *
 * A number of utilities, usefull for debugging, mathematics and other areas.
 */
namespace Utilities {
    // ====================================================================== //
    // Generic vector operations

    /** Rotate the vector v around normal n by angle theta
     *
     * @param v
     * @param n
     * @param theta
     * @return CGLA::Vec3d
     */
    CGLA::Vec3d rotate(const CGLA::Vec3d &v, const CGLA::Vec3d &n,
                       const double &theta);

    // ====================================================================== //
    // Combinatorials

    /** Combinatorial list of K integers from [0, N)
     *
     * @param N
     * @param K
     * @return std::vector<std::vector<int>>
     */
    std::vector<std::vector<int>> combinatorial_list(const int &N,
                                                     const int &K);

    // ====================================================================== //
    // Minimum and maximum for vector types

    /**
     *
     * @tparam T
     * @param in
     * @return T
     */
    template <typename T> inline T sum(const std::vector<T> &in) {
        T val = 0;
        for (const T &v : in) val += v;
        return val;
    }
    /**
     *
     * @tparam T
     * @param in
     * @return T
     */
    template <typename T> inline T max(const std::vector<T> &in) {
        return *std::max_element(in.begin(), in.end());
    }
    /**
     *
     * @tparam T
     * @param in
     * @return T
     */
    template <typename T> inline T min(const std::vector<T> &in) {
        return *std::min_element(in.begin(), in.end());
    }
    /**
     *
     * @tparam T
     * @param in
     * @return size_t
     */
    template <typename T> inline size_t max_idx(const std::vector<T> &in) {
        return std::max_element(in.begin(), in.end()) - in.begin();
    }
    /**
     *
     * @tparam T
     * @param in
     * @return size_t
     */
    template <typename T> inline size_t min_idx(const std::vector<T> &in) {
        return std::min_element(in.begin(), in.end()) - in.begin();
    }

    /**
     *
     * @tparam T
     * @tparam V
     * @tparam N
     * @param in
     * @return T
     */
    template <typename T, typename V, unsigned int N>
    inline T max_idx(const CGLA::ArithVec<T, V, N> &in) {
        return std::max_element(in.begin(), in.end()) - in.begin();
    }
    /**
     *
     * @tparam T
     * @tparam V
     * @tparam N
     * @param in
     * @return T
     */
    template <typename T, typename V, unsigned int N>
    inline T min_idx(const CGLA::ArithVec<T, V, N> &in) {
        return std::min_element(in.begin(), in.end()) - in.begin();
    }

    // ====================================================================== //
    // Interpolations

    /** Compute the trilinear coordinates of a point given the list of
     * corners
     *
     * @param p
     * @param c
     * @return CGLA::Vec3d
     */
    inline CGLA::Vec3d trilinear(const CGLA::Vec3d &             p,
                                 const std::vector<CGLA::Vec3d> &c) {

        // Computing the trilinear coordinates
        CGLA::Vec3d tril = (p - c[0]) / (c[7] - c[0]);

        return tril;
    }
    /** Compute trilinear interpolation between the 8 input corners
     *
     * @tparam T
     * @param xyz
     * @param input
     * @return T
     */
    template <typename T>
    inline T interpolate_trilinear(const CGLA::Vec3d &   xyz,
                                   const std::vector<T> &input) {

        const T interX_Y0Z0 = (1.0 - xyz[0]) * input[0] + xyz[0] * input[1];
        const T interX_Y1Z0 = (1.0 - xyz[0]) * input[2] + xyz[0] * input[3];
        const T interX_Y0Z1 = (1.0 - xyz[0]) * input[4] + xyz[0] * input[5];
        const T interX_Y1Z1 = (1.0 - xyz[0]) * input[6] + xyz[0] * input[7];

        const T interY_Z0 = (1.0 - xyz[1]) * interX_Y0Z0 + xyz[1] * interX_Y1Z0;
        const T interY_Z1 = (1.0 - xyz[1]) * interX_Y0Z1 + xyz[1] * interX_Y1Z1;

        return (1.0 - xyz[2]) * interY_Z0 + xyz[2] * interY_Z1;
    }

    /** Compute the barycentric coordinates of a point
     *
     * @param p
     * @param c
     * @return CGLA::Vec4d
     */
    inline CGLA::Vec4d barycentric(const CGLA::Vec3d &             p,
                                   const std::vector<CGLA::Vec3d> &c) {

        // Computing the barycentric coordinates by the forumla
        // provided by
        // https://en.wikipedia.org/wiki/Barycentric_coordinate_system

        //! Version 1
        // CGLA::Mat3x3d Tm = {c[0] - c[3], c[1] - c[3], c[2] - c[3]};
        // Tm               = CGLA::transpose(Tm);
        // CGLA::Vec4d bary =
        //     CGLA::Vec4d(CGLA::Vec3d(CGLA::invert(Tm) * (p - c[3])), 0.0);
        // bary[3] = 1.0 - bary[0] - bary[1] - bary[2];

        //! Version 2
        CGLA::Mat4x4d R  = {CGLA::Vec4d(c[0], 1.0), CGLA::Vec4d(c[1], 1.0),
                           CGLA::Vec4d(c[2], 1.0), CGLA::Vec4d(c[3], 1.0)};
        R                = CGLA::transpose(R);
        CGLA::Vec4d bary = CGLA::invert(R) * CGLA::Vec4d(p, 1.0);

        return bary;
    }
    /** Compute barycentric interpolation between the 4 input corners
     *
     * @tparam T
     * @param p
     * @param c
     * @param input
     * @return T
     */
    template <typename T>
    inline T interpolate_barycentric(const CGLA::Vec3d &             p,
                                     const std::vector<CGLA::Vec3d> &c,
                                     const std::vector<T> &          input) {
        CGLA::Vec4d bary = barycentric(p, c);

        return bary[0] * input[0] + bary[1] * input[1] + bary[2] * input[2] +
               bary[3] * input[3];
    }
    /** Compute barycentric interpolation between the 4 input corners
     *
     * @tparam T
     * @param bary
     * @param input
     * @return T
     */
    template <typename T>
    inline T interpolate_barycentric(const CGLA::Vec4d &   bary,
                                     const std::vector<T> &input) {
        return bary[0] * input[0] + bary[1] * input[1] + bary[2] * input[2] +
               bary[3] * input[3];
    }
    /** Compute barycentric interpolation between the 4 input corners
     *
     * @tparam T
     * @param bary
     * @param input
     * @return T
     */
    template <typename T>
    inline T interpolate_barycentric(const std::vector<double> &bary,
                                     const std::vector<T> &     input) {
        return bary[0] * input[0] + bary[1] * input[1] + bary[2] * input[2] +
               bary[3] * input[3];
    }
} // namespace Utilities
#endif /* __MATH_OPERATIONS__ */