/*Copyright (C) 2010-2023 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 "PhysicsWorld.hpp"
#include "PhysicsObject.hpp"
#include "PhysicsRoutines.hpp"

#include "Structs/Vector.hpp"
#include <algorithm>

namespace
{
    /// Tolerated margin of error for collision detection
    constexpr float PHYSICS_EPSILON = .08f;
}

PhysicsWorld::PhysicsWorld() : PhysicsWorld(Box(), 0.f, 0.f) {}

PhysicsWorld::PhysicsWorld(const Box &worldBox, float gravity,
                           float airFriction) :
m_worldBox(worldBox), m_gravity(gravity), m_airFriction(airFriction),
m_groundMaterial(MATERIAL_NONE)
{

}

void PhysicsWorld::update(float frameTime)
{
    float elapsedTime = frameTime / 1000.f;

    for (PhysicsObject *object : m_objectsList)
    {
        if (object->isFixed())
            continue;

        //Alterations
        Vector altered = object->getVelocity();

        //Air friction (x-axis)
        ParabolaStep airFrictionUpdate;
        if (altered.x > PHYSICS_EPSILON || altered.x < -PHYSICS_EPSILON)
        {
            const float airFriction = m_airFriction *
                (1.f - object->getAirPenetration());

            airFrictionUpdate =
                PhysicsRoutines::computeParabolaStep(
                    altered.x,
                    (altered.x > 0.f) ? -airFriction : airFriction,
                    elapsedTime);

            if (altered.x > 0.f)
            {
                altered.x += airFrictionUpdate.velocityDelta;
                if (altered.x < 0.f) altered.x = 0.f;
            }
            else if (altered.x < 0.f)
            {
                altered.x += airFrictionUpdate.velocityDelta;
                if (altered.x > 0.f) altered.x = 0.f;
            }
        }

        //Gravity (y-axis)
        const float gravity = m_gravity * (1.f - object->getAirPenetration()/2.f);
        ParabolaStep gravityUpdate =
            PhysicsRoutines::computeParabolaStep(altered.y, gravity, elapsedTime);

        altered.y += gravityUpdate.velocityDelta;

        object->moveXPosition(airFrictionUpdate.positionDelta);
        object->moveYPosition(gravityUpdate.positionDelta);

        if (object->isOnGround()) altered.y = 0.f;
        object->setVelocity(altered);
    }
}

void PhysicsWorld::addObject(PhysicsObject &object)
{
    if (!isKnown(&object))
        m_objectsList.push_back(&object);
}

void PhysicsWorld::removeObject(const PhysicsObject &object)
{
    std::vector<PhysicsObject*>::iterator it = std::find(m_objectsList.begin(),
                                                         m_objectsList.end(),
                                                         &object);
    if (it != m_objectsList.end())
        m_objectsList.erase(it);
}

const std::vector<PhysicsObject*> &PhysicsWorld::getObjects() const
{
    return m_objectsList;
}

void PhysicsWorld::pulseObject(PhysicsObject &object,
                               float xPulse, float yPulse)
{
    if (isKnown(&object))
    {
        Vector pulse = object.getVelocity();
        pulse.x += xPulse;
        pulse.y += yPulse;

        object.setVelocity(pulse);
    }
}

void PhysicsWorld::resetVelocities()
{
    for (PhysicsObject *object : m_objectsList)
        object->setVelocity(Vector(0.f, 0.f));
}

std::vector<PhysicsObject*> PhysicsWorld::hitTest(const PhysicsObject &object) const
{
    return hitTest(object, object.getBox());
}

std::vector<PhysicsObject*> PhysicsWorld::hitTest(const PhysicsObject &object,
                                                  const Box &customBox) const
{
    std::vector<PhysicsObject*> hitObjects;
    for (PhysicsObject *knownObject : m_objectsList)
    {
        if (knownObject != &object &&
            customBox.hits(knownObject->getBox(), PHYSICS_EPSILON))
            hitObjects.push_back(knownObject);
    }

    return hitObjects;
}

bool PhysicsWorld::worldContains(const Box &box) const
{
    return m_worldBox.contains(box, PHYSICS_EPSILON);
}

bool PhysicsWorld::isFree(const Box &box) const
{
    if (!worldContains(box))
        return false;

    for (const PhysicsObject *object : m_objectsList)
    {
        if (box.hits(object->getBox(), PHYSICS_EPSILON))
            return false;
    }

    return true;
}

bool PhysicsWorld::isKnown(const PhysicsObject *object) const
{
    return (std::find(m_objectsList.begin(), m_objectsList.end(), object) !=
        m_objectsList.end());
}

void PhysicsWorld::setWorldBox(const Box &worldBox)
{
    m_worldBox = worldBox;
}

const Box &PhysicsWorld::getWorldBox() const
{
    return m_worldBox;
}

void PhysicsWorld::setGroundMaterial(const std::string &material)
{
    m_groundMaterial = material;
}

const std::string &PhysicsWorld::getGroundMaterial() const
{
    return m_groundMaterial;
}

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

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