/*         +------------------------------------------------------+ 
  ____/  \____   /| - Open source game framework licensed to freely use, |
  \          /  / |   copy, modify and sell without restriction          |
+--\ ^__^   /--+  |                                                      |
| ~/        \~ |  | - created for <https://foam.shampoo.ooo>             |
| ~~~~~~~~~~~~ |  +------------------------------------------------------+
| SPACE ~~~~~  | /
|  ~~~~~~~ BOX |/
+--------------+

[math.hpp]

For math helper functions that require only GLM primitives, especially trigonometric functions. Angle values are in radians. 0 is
directly up on the screen in GL coordinates, and the angle increases clockwise.

This test of points on a square and one point below the square should print the output below.

    glm::vec2 a {5.0f, 5.0f}, b {1.0f, 1.0f}, c {1.0f, 5.0f}, d {5.0f, 1.0f}, e {5.0f, -1.0f};
    std::cout << glm::degrees(sb::angle_between(a, b)) << " " << glm::degrees(sb::angle_between(b, a)) << " " <<
        glm::degrees(sb::angle_between(a, c)) << " " << glm::degrees(sb::angle_between(c, a)) << " " <<
        glm::degrees(sb::angle_between(b, e)) << " " << glm::degrees(sb::angle_between(e, b)) << std::endl <<
        sb::angle_to_vector(sb::angle_between(a, b), 4.0f * glm::sqrt(2.0f)) << " " <<
        sb::angle_to_vector(sb::angle_between(b, a), 4.0f * glm::sqrt(2.0f)) << " " <<
        sb::angle_to_vector(sb::angle_between(d, a), 4.0f) << " " <<
        sb::angle_to_vector(sb::angle_between(b, e), 1.0f) << std::endl;

Should print,

    -135 45 -90 90 116.565 -63.435
    {-4, -4} {4, 4} {0, 4} {0.894427, -0.447214}

This test of angle differences should print the output below.

    float a = 0.0f, b = glm::pi<float>(), c = glm::half_pi<float>(), d = 0.5f, e = glm::pi<float>() * 4;
    std::cout << sb::angle_difference(a, b) << " " << sb::angle_difference(a, c) << " " << sb::angle_difference(b, c) << " " <<
        sb::angle_difference(a, d) << " " << sb::angle_difference(c, d) << " " << sb::angle_difference(a, e) << " " <<
        sb::angle_difference(c, e) << " " << sb::angle_difference(d, e) << " " << sb::angle_difference(c, c) << std::endl <<
        sb::angle_ratio(a, b) << " " << sb::angle_ratio(a, c) << " " << sb::angle_ratio(b, c) << " " << sb::angle_ratio(a, d) << " " <<
        sb::angle_ratio(c, d) << " " << sb::angle_ratio(a, e) << " " << sb::angle_ratio(c, e) << " " << sb::angle_ratio(d, e) << " " <<
        sb::angle_ratio(c, c) << std::endl;

Should print,

    -3.14159 1.5708 -1.5708 0.5 -1.0708 3.49691e-07 -1.5708 -0.5 0
    -1 0.5 -0.5 0.159155 -0.340845 1.1131e-07 -0.5 -0.159155 0
*/

#pragma once

#include <vector>
#include <stdexcept>

/* GLM */
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/glm.hpp>
#include <glm/gtx/compatibility.hpp>

namespace sb
{
    static inline const glm::vec3 ZAXIS {0.0f, 0.0f, 1.0f};

    /*!
     * Get a 2D vector from an angle and magnitude
     *
     * @param angle      angle of a vector in radians, with 0 being up on the screen and increasing values going clockwise
     * @param magnitude  magnitude (length, speed, etc.) of a vector
     * @return           2D vector of the change in X and Y for the given angle and magnitude
     */
    glm::vec2 angle_to_vector(float angle, float magnitude = 1.0f);

    /*!
     * Get the angle between two vectors, or the angle the first would rotate to to point toward the second.
     *
     * @param start   X/Y coordinates
     * @param end     X/Y coordinates
     * @return        an angle in radians
     */
    float angle_between(glm::vec2 start, glm::vec2 end);

    /*!
     * Get the signed shortest angle difference between two angles, or how much the first angle would have to rotate
     * to be equivalent to the second angle. Negative is counter-clockwise, positive clockwise.
     *
     * @param start   X/Y coordinates of the angle which the difference will be relative to
     * @param end     X/Y coordinates of the angle which the difference will be rotated to
     * @return        angle difference relative to the start parameter in radians
     */
    float angle_difference(float start, float end);

    /*!
     * Get the angle difference between two angles as a signed ratio of how much of a 180 degree turn it is. Negative
     * is counter-clockwise, positive clockwise.
     * 
     * @param start   X/Y coordinates of the angle which the difference will be relative to
     * @param end     X/Y coordinates of the angle which the difference will be rotated to
     * @return        angle difference relative to the start parameter as a signed ratio of how much of a 180 degree
     *                turn it is.
     */
    float angle_ratio(float start, float end);

    /*!
     * Return the coordinates of a point on a circle at a given angle. The circle is passed by giving a center and radius.
     *
     * @param center   vector representing the center of the circle
     * @param angle    angle along the circle, 0 being up, going clockwise
     * @param radius   radius of the circle
     * @return         a vector representing a point along the circle's edge at the given angle
     */
    glm::vec2 point_on_circle(const glm::vec2& center, float angle, float radius = 1.0f);

    /*!
     * Fill a vector with count number of points evenly spaced around a circle starting at the angle offset (defaults to 0).
     *
     * @param points   vector that will be filled with 2D vectors representing the requested points
     * @param count    number of points
     * @param radius   radius of the circle
     * @param center   2D vector representing the center of the circle
     * @param offset   start calculating the points offset from 0 degrees (up on the screen)
     */
    void points_on_circle(std::vector<glm::vec2>& points, int count, float radius = 1.0f, const glm::vec2& center = {0.0f, 0.0f}, float offset = 0.0f);

    /*!
     * Create and return a vector of count number of points evenly spaced around a circle starting at the angle offset (defaults to 0).
     *
     * @see points_on_circle(std::vector<glm::vec2>&, int, float, const glm::vec2&, float)
     */
    std::vector<glm::vec2> points_on_circle(int count, float radius = 1.0f, const glm::vec2& center = {0.0f, 0.0f}, float offset = 0.0f);

    /*!
     * Calculate a 2D bezier curve from four 2D control points.
     * 
     * Adapted from public domain released code, 2007 Victor Blomqvist, originally at https://www.pygame.org/wiki/BezierCurve.
     *
     * @param vertices      four 2D control points
     * @param resolution    number of points that will be in the computed curve
     */
    std::vector<glm::vec2> bezier(const std::vector<glm::vec2>& vertices, int resolution = 30);

    /*!
     * Wrap a point so that it is translated into the clip space as if it entered the opposite side of the clip upon exiting it.
     *
     * Running the same algorithm with PyGLM,
     *
     *     In [42]: V = glm.vec3(1.5, 8.88, 3.2)
     *     In [43]: C0 = glm.vec3(-1.0, -1.0, -1.0)
     *     In [44]: C1 = glm.vec3(1.0, 1.0, 1.0)
     *     In [45]: Vw = ((V - C1) % (C1 - C0)) + C0
     *     In [46]: Vw
     *     Out[46]: vec3( -0.5, 0.88, -0.8 )
     *
     * @param vertex       point to wrap
     * @param clip_lower   the lower corner of the clip space to wrap into
     * @param clip_upper   the upper corner of the clip space to wrap into
     * @return             wrapped point
     */
    template<glm::length_t dimensions, typename Type, glm::qualifier qualifier>
    glm::vec<dimensions, Type, qualifier> wrap_point(const glm::vec<dimensions, Type, qualifier>& vertex,
                         const glm::vec<dimensions, Type, qualifier>& clip_lower,
                         const glm::vec<dimensions, Type, qualifier>& clip_upper)
    {
        /* If any clip dimension is zero, throw an error because it will cause NaN to appear in the output. */
        glm::vec<dimensions, Type, qualifier> clip_delta = clip_upper - clip_lower;
        for (glm::length_t ii = 0; ii < clip_delta.length(); ii++)
        {
            if (clip_delta[ii] == 0)
            {
                throw std::invalid_argument("Submitted clip area contains a dimension of size zero.");
            }
        }

        return glm::mod(vertex - clip_upper, clip_delta) + clip_lower;
    }

    /*!
     * Wrap a curve so that all the points on the curve are translated into the given clip space as if they entered the opposite
     * side of the clip upon exiting it. The curve is traversed from start to end. At wrap points, the curve splits, creating two
     * disjoint segments. All the segments generated are returned in a vector.
     *
     * When a curve splits, an unwrapped version of the point at the split is added to the first segment, and an unwrapped version
     * of the point preceeding the split is added to the start of the second segment (technically, the points are still wrapped but
     * offset by a sector to match with their respective segments). This is done to allow the curve to be drawn up to the edge of
     * the clip (and past it).
     *
     * @warning Because this causes points to lie outside the clip at the edges, this function may change in the future to create
     * new points that intersect exactly with the edge of the clip.
     *
     * Example, testing 2D and 3D vertices,
     *
     *     std::vector<glm::vec3> test = {
     *         {0.0f, 0.5f, 3.0f}, {0.5f, 0.5f, 3.5f}, {1.0f, 0.5f, 3.2f}, {1.5f, 0.75f, 2.8f}, {2.0f, 1.25f, 3.5f}, {3.0f, 2.0f, 4.5f}};
     *     std::cout << test << " -> " << sb::wrap_curve(test, {-2.0f, -1.0f, 3.0f}, {2.0f, 1.0f, 4.0f}) << std::endl;
     *     std::vector<glm::vec2> test2 = {{0.0f, 0.5f}, {0.5f, 0.5f}, {1.0f, 0.5f}, {1.5f, 0.75f}, {2.0f, 1.25f}, {3.0f, 2.0f}};
     *     std::cout << test2 << " -> " << sb::wrap_curve(test2, {-(16.0f / 9.0f), -1.0f}, {16.0f / 9.0f, 1.0f}) << std::endl;
     *
     * Prints,
     *
     *     { {0, 0.5, 3} {0.5, 0.5, 3.5} {1, 0.5, 3.2} {1.5, 0.75, 2.8} {2, 1.25, 3.5} {3, 2, 4.5} } -> { { \
     *         {0, 0.5, 3} {0.5, 0.5, 3.5} {1, 0.5, 3.2} {1.5, 0.75, 2.8} } { {1, 0.5, 4.2} {1.5, 0.75, 3.8} {2, 1.25, 4.5} } \
     *         { {-2.5, -1.25, 2.8} {-2, -0.75, 3.5} {-1, 0, 4.5} } { {-2, -0.75, 2.5} {-1, 0, 3.5} } }
     *     { {0, 0.5} {0.5, 0.5} {1, 0.5} {1.5, 0.75} {2, 1.25} {3, 2} } -> { { {0, 0.5} {0.5, 0.5} {1, 0.5} {1.5, 0.75} {2, 1.25} } \
     *         { {-2.05556, -1.25} {-1.55556, -0.75} {-0.555556, 0} } }
     *
     * @param vertices     a vector of vertices of any dimension defining a curve
     * @param clip_lower   the lower corner of the clip space to wrap into
     * @param clip_upper   the upper corner of the clip space to wrap into
     * @return             a vector of curves wrapped to fit
     */
    template<glm::length_t dimensions, typename Type, glm::qualifier qualifier>
    std::vector<std::vector<glm::vec<dimensions, Type, qualifier>>> wrap_curve(const std::vector<glm::vec<dimensions, Type, qualifier>>& vertices,
                                                   const glm::vec<dimensions, Type, qualifier>& clip_lower,
                                                   const glm::vec<dimensions, Type, qualifier>& clip_upper)
    {
        /* Create vector of vectors to store the segments. */
        std::vector<std::vector<glm::vec<dimensions, Type, qualifier>>> segments = {{}};

        /* Create vertices for tracking which sector the unwrapped point falls in. Which sector a vertex falls in represents how many clip-sized
         * spaces away from the target clip space a vertex is. When a vertex is in a different sector than the previous vertex, the wrapped
         * curve needs to split into a new disjoint segment. */
        glm::vec<dimensions, Type, qualifier> sector, sector_prev;

        /* Create vertices for per-vertex wrapping operations. */
        glm::vec<dimensions, Type, qualifier> vertex, vertex_prev, wrapped;

        /* Get the difference between upper and lower clips to define the range of a clip as a vector. */
        glm::vec<dimensions, Type, qualifier> clip_delta = clip_upper - clip_lower;

        /* If any clip dimension is zero, throw an error because it will cause NaN to appear in the output. */
        for (glm::length_t ii = 0; ii < clip_delta.length(); ii++)
        {
            if (clip_delta[ii] == 0)
            {
                throw std::invalid_argument("Submitted clip area contains a dimension of size zero.");
            }
        }

        /* Iterate over all input vertices, wrapping each one. */
        for (std::size_t ii = 0; ii < vertices.size(); ii++)
        {
            vertex = vertices[ii];
            sector = glm::floor((vertex - clip_lower) / clip_delta);
            wrapped = wrap_point(vertex, clip_lower, clip_upper);

            /* A mismatch in sector means the most recent vertex wrapped, so add a new disjoint segment. */
            if (ii > 0 && sector != sector_prev)
            {
                /* Use the difference in sector to calculate an unwrapped version of the point to extend the end of the segment to the edge of the clip
                 * and past. */
                segments.back().push_back(wrapped + (sector - sector_prev) * clip_delta);

                /* Add a new disjoint segment. */
                segments.push_back({});

                /* Use the difference in sector to calculate an unwrapped version of the previous vertex so that the new segment begins slightly
                 * outside of the clip. */
                segments.back().push_back(wrap_point(vertex_prev, clip_lower, clip_upper) + (sector_prev - sector) * clip_delta);
            }

            segments.back().push_back(wrapped);
            sector_prev = sector;
            vertex_prev = vertex;
        }
        
        return segments;
    }

    /*!
     * @param world            world coordinates
     * @param transformation   transformation to apply to the given world coordinates, if any
     */
    template<glm::length_t dimensions, typename Type, glm::qualifier qualifier>
    glm::vec4 world_to_clip(const glm::vec<dimensions, Type, qualifier>& world, const glm::mat4& transformation = glm::mat4{1.0f})
    {
        /* Create a 4-D vertex with the w-coordinate normalized to 1.0f. */
        glm::vec4 _world {world.x, world.y, 0.0f, 1.0f};

        /* If a third dimension was passed, get it from the caller. */
        if constexpr (dimensions > 2)
        {
            _world.z = world.z;
        }

        /* Apply projection and view transformations */
        return transformation * _world;
    }

    /*!
     * @param world            world coordinates
     * @param transformation   transformation to apply to the given world coordinates, if any
     */
    template<glm::length_t dimensions, typename Type, glm::qualifier qualifier>
    glm::vec3 world_to_ndc(const glm::vec<dimensions, Type, qualifier>& world, const glm::mat4& transformation = glm::mat4{1.0f})
    {
        glm::vec4 clip = world_to_clip(world, transformation);
        return glm::vec3{clip.x, clip.y, clip.z} / clip.w;
    }

    /*!
     * @param world            world coordinates
     * @param transformation   transformation to apply to the given world coordinates, if any
     */
    template<glm::length_t dimensions, typename Type, glm::qualifier qualifier>
    glm::vec2 world_to_viewport(const glm::vec<dimensions, Type, qualifier>& world, const glm::vec2& viewport, const glm::mat4& transformation = glm::mat4{1.0f})
    {
        glm::vec3 ndc = world_to_ndc(world, transformation);
        return (glm::vec2{ndc.x, ndc.y} + 1.0f) / 2.0f * viewport;
    }
}