#pragma once

#include <hk_game_object.hpp>

#define GLM_FORCE_RADIANS
#define GLM_FORCE_DEPTH_ZERO_TO_ONE
#include <glm/glm.hpp>
#include <glm/gtc/constants.hpp>

// std
#include <memory>
#include <random>
#include <vector>

namespace hk
{
    class GravityPhysicsSystem
    {
    public:
        GravityPhysicsSystem(float strength) :m_strengthGravity(strength) {}

        void update(std::vector<GameObject> &objs, float dt, unsigned int substeps = 1)
        {
            const float stepDelta = dt / substeps;

            for (int i = 0; i < substeps; i++)
            {
                stepSimulation(objs, stepDelta);
            }
        }

        glm::vec2 computeForce(GameObject &fromObj, GameObject &toObj) const 
        {
            auto offset = fromObj.m_transform2d.translation - toObj.m_transform2d.translation;
            float distanceSquared = glm::dot(offset, offset);

            if (glm::abs(distanceSquared < 1e-10f))
            {
                return {.0f, .0f};
            }
            float force = m_strengthGravity * toObj.m_rigidBody2d.mass * fromObj.m_rigidBody2d.mass / distanceSquared;

            return force * offset / glm::sqrt(distanceSquared);
        }

        const float m_strengthGravity;
    private:
        void stepSimulation(std::vector<GameObject> &physicsObjs, float dt)
        {
            // Loops throught all pairs of objects and applies attractive force between them
            for (auto iterA = physicsObjs.begin(); iterA != physicsObjs.end(); ++iterA)
            {
                auto &objA = *iterA;
                for (auto iterB = iterA; iterB != physicsObjs.end(); ++iterB)
                {
                    if (iterA == iterB) continue;

                    auto &objB = *iterB;
                    auto force = computeForce(objA, objB);
                    objA.m_rigidBody2d.velocity += dt * -force / objA.m_rigidBody2d.mass;
                    objB.m_rigidBody2d.velocity += dt * force / objB.m_rigidBody2d.mass;
                }
            }

            // update each objects position based on its final velocity
            for (auto &obj : physicsObjs)
            {
                obj.m_transform2d.translation += dt * obj.m_rigidBody2d.velocity;
            }
        }
    };

    class Vec2FieldSystem
    {
    public:
        void update(const GravityPhysicsSystem &physicsSystem, std::vector<GameObject> &physicsObjs, std::vector<GameObject> &vectorField)
        {
            for (auto &vf : vectorField)
            {
                glm::vec2 direction{};
                for (auto &obj : physicsObjs)
                {
                    direction += physicsSystem.computeForce(obj, vf);
                }

                vf.m_transform2d.scale.x = 0.005f + 0.045f * glm::clamp(glm::log(glm::length(direction) + 1) / 3.f, 0.f, 1.f);
                vf.m_transform2d.rotation = atan2(direction.y, direction.x);
            }
        }
    };

    std::unique_ptr<Model> createSquareModel(Device &device, glm::vec2 offset)
    {
        std::vector<Model::Vertex> vertices = {
            {{-0.5f, -0.5f}}, {{0.5f, 0.5f}},
            {{-0.5f, 0.5f}}, {{-0.5f, -0.5f}},
            {{0.5f, -0.5f}}, {{0.5f, 0.5f}},
        };

        for (auto &v : vertices)
        {
            v.posision += offset;
        }

        return std::make_unique<Model>(device, vertices);
    }

    std::unique_ptr<Model> createCircleModel(Device &device, unsigned int numSides)
    {
        std::vector<Model::Vertex> uniqueVertices{};
        for(int i = 0; i < numSides; i++)
        {
            float angle = i * glm::two_pi<float>() / numSides;
            uniqueVertices.push_back({{glm::cos(angle), glm::sin(angle)}});
        }

        uniqueVertices.push_back({});

        std::vector<Model::Vertex> vertices{};
        for(int i = 0; i < numSides; i++)
        {
            vertices.push_back(uniqueVertices[i]);
            vertices.push_back(uniqueVertices[(i+1) % numSides]);
            vertices.push_back(uniqueVertices[numSides]);
        }
        return std::make_unique<Model>(device, vertices);
    }

}