#include <SFML/Graphics.hpp>
#include <vector>
#include <cmath>
#include <random>
#include <memory>
#include <string>
#include <algorithm>
#include <iostream>



//physics engine class
class PhysicsEngine
{
private:
    std::vector<std::unique_ptr<RigidBody>> bodies;
    sf::Vector2f gravity;

    float worldHeight;
    float worldWidth;

public:
    PhysicsEngine(float width, float height) : worldWidth(width), worldHeight(height), gravity(0.0f, 500.0f)
    {

    }


    void addBody(std::unique_ptr<RigidBody> body)
    {
        bodies.push_back(std::move(body));
    }

    void clearDynamicBodies()
    {
        bodies.erase(
            std::remove_if(
                bodies.begin(),
                bodies.end(),
                [](const std::unique_ptr<RigidBody>& body) { return !body->isStatic; }),
            bodies.end());
    }


    void update(float deltaTime)
    {
        //first we applied gravity to aall bodies
        for (auto& body : bodies)
        {
            if (!body->isStatic && !body->isResting)
            {
                body->applyForce(gravity * body->mass);
            }
        }


        //update  each particles position
        for (auto& body : bodies)
        {
            body->update(deltaTime);
            body->checkBoundaryCollision(worldWidth, worldHeight);           
        }

        //check for collisions between bodies O(n^2) 
        for (size_t i = 0; i < bodies.size(); ++i)
        {
            for (size_t j = i + 1; j < bodies.size(); ++j)
            {
                //at this point here we will do the collision checks where we compare every particle to every other particle
                checkCollision(*bodies[i], *bodies[j]);
            }
        }
    }

    void checkCollision(RigidBody& body1, RigidBody& body2)
    {
        sf::Vector2f difference = body2.position - body1.position;

        float distance = length(difference);

        float minDistance = body1.radius + body2.radius; 

        if (distance < minDistance)
        {

            if (distance < 0.001f) //prevent division by zero for any overlapping bodies
            {
                distance = 0.001f;
                difference = sf::Vector2f(1.0f, 0.0f) * minDistance;
            }
            //wake up resting bodies
            body1.wake();
            body2.wake();

            //collision detected
            sf::Vector2f normal = normalise(difference);

            //seperate the bodies
            float overlap = minDistance - distance;
            float seperationFactor = 1.01f;

            if (!body1.isStatic && !body2.isStatic)
            {
                body1.position -= normal * (overlap * 0.5f * seperationFactor);
                body2.position += normal * (overlap * 0.5f * seperationFactor);
            }
            else if (!body1.isStatic)
            {
                body1.position -= normal * (overlap);
            }
            else if (!body2.isStatic)
            {

                body2.position += normal * (overlap);
            }


            //calculate relative velocity 
            sf::Vector2f relativeVelocity = body2.velocity - body1.velocity;
            float velocityAlongNormal = dot(relativeVelocity, normal);
            if (velocityAlongNormal < 0)
            {
                return;
            }

            //calculate the restitution
            float e = std::min(body1.restitution, body2.restitution);


            //calculate the impulse scalar
            float j = -(1 + e) * velocityAlongNormal;
            if (!body1.isStatic && !body2.isStatic)
            {
                j /= (1 / body1.mass + 1 / body2.mass);
            }
            else if (!body1.isStatic)
            {
                j /= (1 / body2.mass);
            }
            else if (!body2.isStatic)
            {
                j /= (1 / body1.mass);
            }

            sf::Vector2f impulse = j * normal;

            if (!body1.isStatic)
            {
                body1.velocity -= impulse / body1.mass;
            }
            if (!body2.isStatic)
            {
                body2.velocity += impulse / body2.mass;
            }


            //apply friction (tangential component)
            sf::Vector2f tangent = relativeVelocity - normal * velocityAlongNormal;
            if (length(tangent) > 0.001f)
            {
                tangent = normalise(tangent);

                float frictionCoEff = (body1.friction + body2.friction) * 0.5f;
                float jt = -dot(relativeVelocity, tangent);

                if (!body1.isStatic && !body2.isStatic)
                {
                    jt /= (1 / body1.mass + 1 / body2.mass);
                }
                else if (!body1.isStatic)
                {
                    jt /= (1 / body2.mass);
                }
                else if (!body2.isStatic)
                {
                    jt /= (1 / body1.mass);
                }

                //coulombs law: limit friction
                float frictionLimit = std::abs(j * frictionCoEff);
                jt = std::clamp(jt, -frictionLimit, frictionLimit);

                sf::Vector2f frictionImpulse = jt * tangent;
                if (!body1.isStatic)
                {
                    body1.velocity -= frictionImpulse / body1.mass;
                }
                if (!body2.isStatic)
                {
                    body2.velocity += frictionImpulse / body2.mass;
                }

            }
        }

    }

    void draw(sf::RenderWindow& window)
    {
        for (auto& body : bodies)
        {
            body->draw(window);
        }
    }

    void setGravity(const sf::Vector2f& g)
    {
        gravity = g;
        for (auto& body : bodies)
        {
            body->wake();
        }
    }


    //next session we will figure out the call to finding bodies at specific locations

    RigidBody* getBodyAt(const sf::Vector2f& point)
    {
        for(auto& body : bodies)
        {
            sf::Vector2f diff = body->position - point;
            if (length(diff) <= body->radius)
            {
                return body.get();
            }
        }
        return nullptr;
    }
};



int main()
{
    sf::RenderWindow window(sf::VideoMode({ 1200, 800 }), "Rigid Body");

    window.setFramerateLimit(60);

    //init physics engine
    PhysicsEngine physics(1200.f, 800.f);

    //random number generator 

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<float> posX(100.0f, 1100.0f);
    std::uniform_real_distribution<float> posY(100.0f, 300.0f);
    std::uniform_real_distribution<float> radiusDist(15.0f, 40.0f);
    std::uniform_real_distribution<float> massDist(1.f, 5.0f);
    std::uniform_int_distribution<int>   colourDist(100, 255);

    sf::Font font;
    font.openFromFile("c:/Windows/Fonts/arial.ttf");
    sf::Text it(font, "Cotnrols:\n "
        "Left click: Add a new body\n"
        "Right Click: drag a body\n"
        "Space: add 5 bodies\n"
        "C: Clear bodies\n"
        "G: Toggle Gravity");

    //add some static obstacles for our particles to collide with 
    physics.addBody(std::make_unique<RigidBody>(sf::Vector2f(300.f, 500.f), 50.f, 10.f, sf::Color(100, 100, 100), true));
    physics.addBody(std::make_unique<RigidBody>(sf::Vector2f(600.f, 400.f), 50.f, 10.f, sf::Color(100, 100, 100), true));
    physics.addBody(std::make_unique<RigidBody>(sf::Vector2f(900.f, 500.f), 50.f, 10.f, sf::Color(100, 100, 100), true));


    //add some initial dynamic rigid bodies

    for (int i = 0; i < 10; ++i)
    {
        float radius = radiusDist(gen);
        float mass = massDist(gen) * (radius / 20.f);

        sf::Color colour(colourDist(gen), colourDist(gen), colourDist(gen));
        sf::Vector2f position(posX(gen), posY(gen));
        physics.addBody(std::make_unique<RigidBody>(position, radius, mass, colour));
    }

    //mouse interaction variables
    RigidBody* draggedBody = nullptr;
    sf::Vector2f dragOffset;

    //clock for delta time 
    sf::Clock clock;
    while (window.isOpen())
    {
        float deltaTime = clock.restart().asSeconds();
        while (const std::optional event = window.pollEvent())
        {
            if (event->is<sf::Event::Closed>()) //close window on exit event
            {
                std::cout << "Closing window" << std::endl;
                window.close();
            }
            //the next event we want to handle is Mouse button pressed 

            if (const auto* mousePressed = event->getIf<sf::Event::MouseButtonPressed>())
            {
                sf::Vector2f mousePos(mousePressed->position.x, mousePressed->position.y);

                if (mousePressed->button == sf::Mouse::Button::Right)
                {
                    draggedBody = physics.getBodyAt(mousePos);
                    if (draggedBody)
                    {
                        dragOffset = draggedBody->position - mousePos;
                        draggedBody->wake();
                    }
                    
                }
                if (mousePressed->button == sf::Mouse::Button::Left)
                {
                    float radius = radiusDist(gen);
                    float mass = massDist(gen) * (radius / 20.f);

                    sf::Color colour(colourDist(gen), colourDist(gen), colourDist(gen));
                    physics.addBody(std::make_unique<RigidBody>(mousePos, radius, mass, colour));
                }
            }

            if (event->is<sf::Event::MouseButtonReleased>())
            {
                if (draggedBody) draggedBody = nullptr;
            }

            if (const auto* mouseMoved = event->getIf<sf::Event::MouseMoved>())
            {
                if (draggedBody && !draggedBody->isStatic)
                {
                    sf::Vector2f mousePos(mouseMoved->position.x, mouseMoved->position.y);

                    sf::Vector2f targetPos = mousePos + dragOffset;
                    draggedBody->velocity = (targetPos - draggedBody->position) * 10.f;
                }
            }

            if (const auto* keyPressed = event->getIf<sf::Event::KeyPressed>())
            {
                if (keyPressed->scancode == sf::Keyboard::Scancode::Space)
                {
                    for (int i = 0; i < 5; ++i)
                    {
                        float radius = radiusDist(gen);
                        float mass = massDist(gen) * (radius / 20.f);

                        sf::Color colour(colourDist(gen), colourDist(gen), colourDist(gen));
                        sf::Vector2f position(posX(gen), posY(gen));
                        physics.addBody(std::make_unique<RigidBody>(position, radius, mass, colour));
                    }
                }

                if (keyPressed->scancode == sf::Keyboard::Scancode::C)
                {
                    physics.clearDynamicBodies();
                }

                if (keyPressed->scancode == sf::Keyboard::Scancode::G)
                {
                    static bool gravityOn = true;
                    gravityOn = !gravityOn;

                    physics.setGravity(gravityOn ? sf::Vector2f(0.0f, 500.0f) : sf::Vector2f(0.0f, 0.0f));
                }

            }

        }

        physics.update(deltaTime);
        window.clear(sf::Color(20, 20, 30));
        physics.draw(window);
        window.draw(it);
        window.display();
    }
}