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

#include "math.hpp"

glm::vec2 sb::velocity_to_delta(float angle, float magnitude)
{
    return {glm::sin(angle) * magnitude, glm::cos(angle) * magnitude};
}

glm::vec2 sb::velocity_to_delta(glm::vec2 velocity)
{
    return velocity_to_delta(velocity.x, velocity.y);
}

float sb::angle_between(glm::vec2 start, glm::vec2 end)
{
    return glm::atan(end.x - start.x, end.y - start.y);
}

float sb::angle_difference(float start, float end)
{
    return glm::atan(glm::sin(end - start), glm::cos(end - start));
}

float sb::angle_ratio(float start, float end)
{
    return sb::angle_difference(start, end) / glm::pi<float>();
}

std::vector<glm::vec2> sb::bezier(const std::vector<glm::vec2>& control, int resolution)
{
    std::vector<glm::vec2> points;
    points.reserve(resolution);

    float b0x = control[0][0];
    float b0y = control[0][1];
    float b1x = control[1][0];
    float b1y = control[1][1];
    float b2x = control[2][0];
    float b2y = control[2][1];
    float b3x = control[3][0];
    float b3y = control[3][1];

    float ax = -b0x + 3.0f * b1x + -3.0f * b2x + b3x;
    float ay = -b0y + 3.0f * b1y + -3.0f * b2y + b3y;
    float bx = 3.0f * b0x + -6.0f * b1x + 3.0f * b2x;
    float by = 3.0f * b0y + -6.0f * b1y + 3.0f * b2y;
    float cx = -3.0f * b0x + 3.0f * b1x;
    float cy = -3.0f * b0y + 3.0f * b1y;
    float dx = b0x;
    float dy = b0y;

    float steps = resolution - 1;
    float h = 1.0f / steps;

    float pointX = dx;
    float pointY = dy;

    float firstFDX = ax * std::pow(h, 3.0f) + bx * std::pow(h, 2.0f) + cx * h;
    float firstFDY = ay * std::pow(h, 3.0f) + by * std::pow(h, 2.0f) + cy * h;
    float secondFDX = 6 * ax * std::pow(h, 3.0f) + 2 * bx * std::pow(h, 2.0f);
    float secondFDY = 6 * ay * std::pow(h, 3.0f) + 2 * by * std::pow(h, 2.0f);
    float thirdFDX = 6 * ax * std::pow(h, 3.0f);
    float thirdFDY = 6 * ay * std::pow(h, 3.0f);

    points.push_back({pointX, pointY});

    for (int ii = 0; ii < steps; ii++)
    {
        pointX += firstFDX;
        pointY += firstFDY;
        firstFDX += secondFDX;
        firstFDY += secondFDY;
        secondFDX += thirdFDX;
        secondFDY += thirdFDY;
        points.push_back({pointX, pointY});
    }
    
    return points;
}