/*Copyright (C) 2010-2012 Duncan Deveaux

This file is part of Hikou no mizu.

Hikou no mizu is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

Hikou no mizu is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Hikou no mizu.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "PhysicsEngine.hpp"

PhysicsEngine::PhysicsEngine()
{
    
}

PhysicsEngine::PhysicsEngine(const Box &physicsWorld, float gravity, float airFriction) :
m_physicsWorld(physicsWorld), m_gravity(gravity), m_airFriction(airFriction)
{
    
}

void PhysicsEngine::addObject(PhysicsObject *object)
{
    m_physicsObjectsList.push_back( std::make_pair(object, Vector()) );
}

void PhysicsEngine::removeObject(PhysicsObject *object)
{
    std::vector< std::pair<PhysicsObject*, Vector> >::iterator it = m_physicsObjectsList.begin();
    
    while (it != m_physicsObjectsList.end())
    {
        if (it->first == object)
            it = m_physicsObjectsList.erase(it);
        else
            it++;
    }
}

void PhysicsEngine::update(float frameTime)
{
    for (unsigned int i = 0; i < m_physicsObjectsList.size(); i++)
    {
        float elapsedTime = frameTime / 1000;
        
        PhysicsObject *physicsObject = m_physicsObjectsList[i].first;
        Vector &pulseVector = m_physicsObjectsList[i].second;
        
        //Pulse
        if (pulseVector.x != 0) moveObjectX(physicsObject, pulseVector.x * elapsedTime);
        if (pulseVector.y != 0) moveObjectY(physicsObject, pulseVector.y * elapsedTime);
        
            //alterations
        float alteredX = pulseVector.x;
        float alteredY = pulseVector.y;
        
        float airFriction = m_airFriction * elapsedTime;
        if (alteredX > 0)
        {
            if (alteredX - airFriction < 0) alteredX = 0;
            else alteredX -= airFriction;
        }
        else if (alteredX < 0)
        {
            if (alteredX + airFriction > 0) alteredX = 0;
            else alteredX += airFriction;
        }
        
        if (!isOnGround(physicsObject)) alteredY += m_gravity * elapsedTime;
        else alteredY = 0;
        
        m_physicsObjectsList[i].second = Vector(alteredX, alteredY);
    }
}

void PhysicsEngine::moveObjectX(PhysicsObject *objectToMove, float distance)
{
    std::vector<PhysicsObject*> initialHits = objectHitTest(objectToMove);
    
    float currentPosition = objectToMove->getXPosition();
    objectToMove->setXPosition(currentPosition + distance);
    
    //Objects
    std::vector<PhysicsObject*> finalHits = objectHitTest(objectToMove);
    PhysicsRoutines::soustractVectors(finalHits, initialHits);
    
    if (finalHits.size() > 0)
    {
        int checkedSide = 0;
        if (distance < 0) checkedSide = 1;
        
        PhysicsRoutines::sortObjectsByX(finalHits, checkedSide);
        
        PhysicsObject *nearestObject = finalHits.front();
        if (distance < 0) nearestObject = finalHits.back();
        
        float hitX = nearestObject->getXPosition() - nearestObject->getXOrigin();
        float hitWidth = nearestObject->getWidth();
        
        if (distance > 0)
            objectToMove->setXPosition(hitX + (objectToMove->getXOrigin() - objectToMove->getWidth()) - .1);
        else if (distance < 0)
            objectToMove->setXPosition(hitX + hitWidth + objectToMove->getXOrigin() + .1);
    }
    
    //World
    if (!worldHitTest(objectToMove))
    {
        if (distance > 0)
            objectToMove->setXPosition(m_physicsWorld.left + m_physicsWorld.width - (objectToMove->getWidth() - objectToMove->getXOrigin()) - .1);
        else if (distance < 0)
            objectToMove->setXPosition(m_physicsWorld.left + objectToMove->getXOrigin() + .1);
    }
}

void PhysicsEngine::moveObjectY(PhysicsObject *objectToMove, float distance)
{
    std::vector<PhysicsObject*> initialHits = objectHitTest(objectToMove);
    
    float currentPosition = objectToMove->getYPosition();
    objectToMove->setYPosition(currentPosition + distance);
    
    //Objects
    std::vector<PhysicsObject*> finalHits = objectHitTest(objectToMove);
    PhysicsRoutines::soustractVectors(finalHits, initialHits);
    
    if (finalHits.size() > 0)
    {
        int checkedSide = 0;
        if (distance < 0) checkedSide = 1;
        
        PhysicsRoutines::sortObjectsByY(finalHits, checkedSide);
        
        PhysicsObject *nearestObject = finalHits.front();
        if (distance < 0) nearestObject = finalHits.back();
        
        float hitY = nearestObject->getYPosition() - nearestObject->getYOrigin();
        float hitHeight = nearestObject->getHeight();
        
        if (distance > 0)
            objectToMove->setYPosition(hitY + (objectToMove->getYOrigin() - objectToMove->getHeight()) - .1);
        else if (distance < 0)
            objectToMove->setYPosition(hitY + hitHeight + objectToMove->getYOrigin() + .1);
    }
    
    //World
    if (!worldHitTest(objectToMove))
    {
        if (distance > 0)
            objectToMove->setYPosition(m_physicsWorld.top + m_physicsWorld.height - (objectToMove->getHeight() - objectToMove->getYOrigin()) - .1);
        else if (distance < 0)
            objectToMove->setYPosition(m_physicsWorld.top + objectToMove->getYOrigin() + .1);
    }
}

void PhysicsEngine::pulseObject(PhysicsObject *objectToPulse, float xForce, float yForce)
{
    std::vector< std::pair<PhysicsObject*, Vector> >::iterator it;
    
    for (it = m_physicsObjectsList.begin(); it != m_physicsObjectsList.end(); it++)
    {
        if (it->first == objectToPulse)
        {
            it->second.x += xForce;
            it->second.y += yForce;
        }
    }
}

std::vector<PhysicsObject*> PhysicsEngine::objectHitTest(PhysicsObject *objectToTest)
{
    Box objectSize;
    objectSize.left = objectToTest->getXPosition() - objectToTest->getXOrigin();
    objectSize.top = objectToTest->getYPosition() - objectToTest->getYOrigin();
    objectSize.width = objectToTest->getWidth();
    objectSize.height = objectToTest->getHeight();
    
    return objectHitTest(objectToTest, objectSize);
}

std::vector<PhysicsObject*> PhysicsEngine::objectHitTest(PhysicsObject *objectToTest, const Box &patchedSize)
{
    std::vector<PhysicsObject*> hitObjects;
    
    for (unsigned int i = 0; i < m_physicsObjectsList.size(); i++)
    {
        PhysicsObject *physicsObject = m_physicsObjectsList[i].first;
        
        if (physicsObject != objectToTest)
        {
            float testedXPosition = physicsObject->getXPosition() - physicsObject->getXOrigin();
            float testedYPosition = physicsObject->getYPosition() - physicsObject->getYOrigin();
            float testedWidth = physicsObject->getWidth();
            float testedHeight = physicsObject->getHeight();
            
            //Hit test condition
            if ((patchedSize.left + patchedSize.width > testedXPosition && patchedSize.left < testedXPosition + testedWidth) && (patchedSize.top + patchedSize.height > testedYPosition && patchedSize.top < testedYPosition + testedHeight))
                hitObjects.push_back(physicsObject);
        }
    }
    
    return hitObjects;
}

bool PhysicsEngine::worldHitTest(PhysicsObject *objectToTest)
{
    //Position using the object's top-left corner
    float xPosition = objectToTest->getXPosition() - objectToTest->getXOrigin();
    float yPosition = objectToTest->getYPosition() - objectToTest->getYOrigin();
    float width = objectToTest->getWidth();
    float height = objectToTest->getHeight();
    
    float testedXPosition = m_physicsWorld.left;
    float testedYPosition = m_physicsWorld.top;
    float testedWidth = m_physicsWorld.width;
    float testedHeight = m_physicsWorld.height;
    
    //Hit test condition
    if ((xPosition + width <= testedXPosition + testedWidth && xPosition >= testedXPosition) && (yPosition + height <= testedYPosition + testedHeight && yPosition >= testedYPosition))
        return true;
    
    return false;
}

bool PhysicsEngine::isOnGround(PhysicsObject *objectToTest)
{
    float originalYPosition = objectToTest->getYPosition();
    float topYPosition = objectToTest->getYPosition() - objectToTest->getYOrigin();
    
    if (topYPosition + objectToTest->getHeight() + .1 < m_physicsWorld.top + m_physicsWorld.height) // The object doesn't touch the ground
    {
        std::vector<PhysicsObject*> initialHits = objectHitTest(objectToTest);
        objectToTest->setYPosition(originalYPosition + 1.1);
        
        std::vector<PhysicsObject*> hitObjects = objectHitTest(objectToTest);
        PhysicsRoutines::soustractVectors(hitObjects, initialHits);
        
        objectToTest->setYPosition(originalYPosition);
        if (hitObjects.size() == 0) //The object doesn't touch another one
            return false;
    }
    
    return true;
}

//Get and set methods
void PhysicsEngine::setPhysicsWorld(const Box &physicsWorld)
{
    m_physicsWorld = physicsWorld;
}

const Box &PhysicsEngine::getPhysicsWorld() const
{
    return m_physicsWorld;
}

void PhysicsEngine::setGravityForce(float gravity)
{
    m_gravity = gravity;
}

float PhysicsEngine::getGravityForce() const
{
    return m_gravity;
}

void PhysicsEngine::setAirFriction(float airFriction)
{
    m_airFriction = airFriction;
}

float PhysicsEngine::getAirFriction() const
{
    return m_airFriction;
}
