#include <algorithm>
#include <iostream>

#include "PhysicsEngine.hpp"
#include "TinyMath.hpp"

// These methods intentionally left blank
PhysicsEngine::PhysicsEngine(){};
PhysicsEngine::~PhysicsEngine(){};

void PhysicsEngine::AddComponent(IPhysicsComponent* newComponent){
    m_components.push_back(newComponent);
}

// TODO: Work in progress
// Calculates all relevant forces on an object in total so there's only one time each of the components are affected
void PhysicsEngine::Update(){
    // Fill an array with new velocities to be applied to all the components
    Vec2D velocities[m_components.size()];

    // Apply gravity first so it can be cancelled out if on a surface
    for(int i = 0; i < m_components.size(); ++i){
        velocities[i] = m_components[i]->GetVelocity();
        if(m_components[i]->UsesGravity()){
            velocities[i] += Vec2D(0, m_gravity);
        }
    }

    // Apply collisions to all components
    for(int i = 0; i < m_components.size(); ++i){
        IPhysicsComponent* component = m_components.at(i);

        // Check for collisions with all unhandled components
        for(int j = i + 1; j < m_components.size(); ++j){
            IPhysicsComponent* other = m_components.at(j);
            if(component->IsColliding(other)){
                if(component->IsKinematic()){
                    if(other->IsKinematic()){
                        // https://imada.sdu.dk/u/rolf/Edu/DM842/E14/2dcollisions.pdf
                        // https://en.wikipedia.org/wiki/Elastic_collision
                        Vec2D velA = velocities[i];
                        Vec2D posA = component->GetPosition() + component->GetDimensions()/2;
                        float massA = component->GetMass();
                        Vec2D velB = velocities[j];
                        Vec2D posB = other->GetPosition() + other->GetDimensions()/2;
                        float massB = component->GetMass();

                        Vec2D newVelA = Vec2D(0,0);
                        if(component->IsElastic()){
                            newVelA = velA - ((2 * massB)/(massA + massB)) * 
                                    (Dot(velA - velB, posA - posB) / 
                                        Magnitude(posA - posB)*Magnitude(posA-posB)) *
                                    (posA - posB);
                        } else {
                            // get component of velocity that's colliding and cancel it
                            // TODO: placeholder
                            newVelA = Vec2D(0,0);
                        }
                        Vec2D newVelB = Vec2D(0,0);
                        if(other->IsElastic()){
                            newVelB = velB - ((2 * massA)/(massA + massB)) * 
                                    (Dot(velB - velA, posB - posA) / 
                                        Magnitude(posB - posA)*Magnitude(posB-posA)) *
                                    (posB - posA);
                        } else {
                            newVelB = Vec2D(0,0);
                        }                        
                        // TODO: This needs significant testing with multi-collision effects
                        velocities[i] = newVelA;
                        velocities[j] = newVelB;
                    } else {
                        // TODO: Broadcast collision, but don't affect velocity
                    }
                } else {
                    // TODO: Broadcast collision, but don't affect velocity
                }
            }
        }
    }
    // Apply the array of velocities to the array of components
    for(int i = 0; i < m_components.size(); ++i){
        m_components[i]->SetVelocity(velocities[i]);
        m_components[i]->ApplyVelocity();
    }
}

void PhysicsEngine::SetGravity(float newGravity){
    m_gravity = newGravity;
}