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

PhysicsObject::PhysicsObject() : m_physicsWorld(NULL), m_airPenetration(0.f), m_fixed(false)
{
    
}

PhysicsObject::~PhysicsObject()
{
    
}

void PhysicsObject::setPhysicsWorld(PhysicsWorld &physicsWorld)
{
    m_physicsWorld = &physicsWorld;
}

PhysicsWorld &PhysicsObject::getPhysicsWorld() const
{
    return (*m_physicsWorld);
}

void PhysicsObject::setPosition(float xPosition, float yPosition)
{
    if (m_physicsWorld != NULL)
    {
        Box initialBox = m_box;
        
        m_box.left = xPosition;
        m_box.top = yPosition;
        
        const Box &worldBox = m_physicsWorld->getWorldBox();
        
        if (!worldBox.contains(m_box) || hitTest().size() > 0)
            m_box = initialBox;
    }
}

void PhysicsObject::moveXPosition(float distance, bool push)
{
    if (m_physicsWorld != NULL && distance != 0)
    {
        // To avoid crossing others object without noticing,
        // We make the object move by steps of its width,
        // So it covers the whole area it's moving through.
        
        int stepsNo_signed = distance / m_box.width; /// Number of times it's gonna move by its width.
        float remainder = distance - stepsNo_signed * m_box.width; /// Remainder to move to reach the expected distance.
        
        int stepsNo = (stepsNo_signed > 0) ? stepsNo_signed : -stepsNo_signed;
        for (int i = 0; i < stepsNo; i++)
        {
            if (!attempt_moveXPosition((stepsNo_signed > 0) ? m_box.width : -m_box.width, push))
                return;
        }
        
        attempt_moveXPosition(remainder, push);
    }
}

bool PhysicsObject::attempt_moveXPosition(float distance, bool push)
{
    if (m_physicsWorld != NULL && distance != 0)
    {
        float initialPosition = m_box.left;
        
        std::vector<PhysicsObject*> initialHits = hitTest();
        m_box.left += distance;
        
        //Objects
        std::vector<PhysicsObject*> finalHits = hitTest();
        PhysicsRoutines::soustractVectors(finalHits, initialHits);
        
        if (push)
        {
            std::vector<PhysicsObject*>::iterator it;
            
            for (it = finalHits.begin(); it != finalHits.end(); it++) //Move hit objects
            {
                PhysicsObject *hitObject = (*it);
                
                if (!hitObject->isFixed())
                {
                    const Box &hitBox = hitObject->getBox();
                    
                    if (distance > 0)
                        hitObject->moveXPosition((m_box.left + m_box.width) - hitBox.left, true);
                    else if (distance < 0)
                        hitObject->moveXPosition(m_box.left - (hitBox.left + hitBox.width), true);
                }
            }
            
            //Update hits
            finalHits = hitTest();
            PhysicsRoutines::soustractVectors(finalHits, initialHits);
        }
        
        if (finalHits.size() > 0)
        {
            m_box.left = initialPosition; //Return to initial position.
            
            int checkedSide = 0;
            if (distance < 0) checkedSide = 1;
            
            PhysicsRoutines::sortObjectsByX(finalHits, checkedSide);
            
            // 'Collide' callback
            for (std::vector<PhysicsObject*>::iterator it = finalHits.begin(); it != finalHits.end(); it++)
            {
                (*it)->collide( (*this) ); //The object collided with myself
                collide( (*(*it)) ); //I collided with the object
            }
            
            // Attempt to fix the position.
            PhysicsObject *nearestObject = finalHits.front();
            if (distance < 0) nearestObject = finalHits.back();
            
            const Box &hitBox = nearestObject->getBox();
            
            if (distance > 0)
                setPosition(hitBox.left - m_box.width, m_box.top);
            else if (distance < 0)
                setPosition(hitBox.left + hitBox.width, m_box.top);
            
            return false; /// Move was corrected not to induce a collision.
        }
        
        //World
        const Box &worldBox = m_physicsWorld->getWorldBox();
        
        if (!worldBox.contains(m_box))
        {
            m_box.left = initialPosition; //Return to initial position.
            
            // Attempt to fix the position.
            if (distance > 0)
                setPosition(worldBox.left + worldBox.width - m_box.width, m_box.top);
            else if (distance < 0)
                setPosition(worldBox.left, m_box.top);
            
            collideWorld(); //Notify collision with the world box.
            
            return false; /// Move was corrected not to induce a collision.
        }
        
        return true; /// Move happened as asked.
    }
    
    return false;
}

void PhysicsObject::moveYPosition(float distance, bool push)
{
    if (m_physicsWorld != NULL && distance != 0)
    {
        // To avoid crossing others object without noticing,
        // We make the object move by steps of its width,
        // So it covers the whole area it's moving through.
        int stepsNo_signed = distance / m_box.height; /// Number of times it's gonna move by its height.
        float remainder = distance - stepsNo_signed * m_box.height; /// Remainder to move to reach the expected distance.
        
        int stepsNo = (stepsNo_signed > 0) ? stepsNo_signed : -stepsNo_signed;
        for (int i = 0; i < stepsNo; i++)
        {
            if (!attempt_moveYPosition((stepsNo_signed > 0) ? m_box.height : -m_box.height, push))
                return;
        }
        
        attempt_moveYPosition(remainder, push);
    }
}

bool PhysicsObject::attempt_moveYPosition(float distance, bool push)
{
    if (m_physicsWorld != NULL && distance != 0)
    {
        float initialPosition = m_box.top;
        
        std::vector<PhysicsObject*> initialHits = hitTest();
        m_box.top += distance;
        
        //Objects
        std::vector<PhysicsObject*> finalHits = hitTest();
        PhysicsRoutines::soustractVectors(finalHits, initialHits);
        
        if (push)
        {
            std::vector<PhysicsObject*>::iterator it;
            
            for (it = finalHits.begin(); it != finalHits.end(); it++) //Move hit objects
            {
                PhysicsObject *hitObject = (*it);
                
                if (!hitObject->isFixed())
                {
                    const Box &hitBox = hitObject->getBox();
                    
                    if (distance > 0)
                        hitObject->moveYPosition((m_box.top + m_box.height) - hitBox.top, true);
                    else if (distance < 0)
                        hitObject->moveYPosition(m_box.top - (hitBox.top + hitBox.height), true);
                }
            }
            
            //Update hits
            finalHits = hitTest();
            PhysicsRoutines::soustractVectors(finalHits, initialHits);
        }
        
        if (finalHits.size() > 0)
        {
            m_box.top = initialPosition; //Return to initial position.
            
            int checkedSide = 0;
            if (distance < 0) checkedSide = 1;
            
            PhysicsRoutines::sortObjectsByY(finalHits, checkedSide);
            
            // 'Collide' callback
            for (std::vector<PhysicsObject*>::iterator it = finalHits.begin(); it != finalHits.end(); it++)
            {
                (*it)->collide( (*this) ); //The object collided with myself
                collide( (*(*it)) ); //I collided with the object
            }
            
            // Attempt to fix the position.
            PhysicsObject *nearestObject = finalHits.front();
            if (distance < 0) nearestObject = finalHits.back();
            
            const Box &hitBox = nearestObject->getBox();
            
            if (distance > 0)
                setPosition(m_box.left, hitBox.top - m_box.height);
            else if (distance < 0)
                setPosition(m_box.left, hitBox.top + hitBox.height);
            
            return false; /// Move was corrected not to induce a collision.
        }
        
        //World
        const Box &worldBox = m_physicsWorld->getWorldBox();
        
        if (!worldBox.contains(m_box))
        {
            m_box.top = initialPosition; //Return to initial position.
            
            if (distance > 0)
                setPosition(m_box.left, worldBox.top + worldBox.height - m_box.height);
            else if (distance < 0)
                setPosition(m_box.left, worldBox.top);
            
            collideWorld(); //Notify collision with the world box.
            
            return false; /// Move was corrected not to induce a collision.
        }
        
        return true; /// Move happened as asked.
    }
    
    return false;
}

void PhysicsObject::setWidth(float width, bool reverseSide)
{
    if (m_physicsWorld != NULL && width > 0)
    {
        Box initialBox = m_box;
        m_box.width = width;
        if (reverseSide) m_box.left += initialBox.width - m_box.width;
        
        //Lower width : no collision, change & abort
        if (initialBox.width >= width)
            return;
        
        //Leave world : abort
        const Box &worldBox = m_physicsWorld->getWorldBox();
        
        if (!worldBox.contains(m_box))
        {
            m_box = initialBox;
            return;
        }
        
        //Potential Object hit
        std::vector<PhysicsObject*> initialHits = hitTest(initialBox);
        std::vector<PhysicsObject*> finalHits = hitTest();
        
        PhysicsRoutines::soustractVectors(finalHits, initialHits);
        
        if (finalHits.size() > 0) //Hits
        {
            // If fixed objects are hit abort to avoid moving objects past the fixed objects.
            if (PhysicsRoutines::containsFixedObject(finalHits))
            {
                m_box = initialBox;
                return;
            }
            
            // Otherwise, try to move the objects.
            std::vector<PhysicsObject*>::iterator it;
            
            for (it = finalHits.begin(); it != finalHits.end(); it++) //Move hit objects
            {
                if (!(*it)->isFixed())
                {
                    const Box &hitBox = (*it)->getBox();
                    
                    if (!reverseSide)
                        (*it)->moveXPosition((m_box.left + m_box.width) - hitBox.left, true);
                    else
                        (*it)->moveXPosition(m_box.left - (hitBox.left + hitBox.width), true);
                }
            }
            
            // If still hit (one of the objects hit a fixed object for example) : abort
            finalHits = hitTest();
            PhysicsRoutines::soustractVectors(finalHits, initialHits);
            
            if (finalHits.size() > 0)
                m_box = initialBox;
        }
    }
}

void PhysicsObject::setHeight(float height, bool reverseSide)
{
    if (m_physicsWorld != NULL && height > 0)
    {
        Box initialBox = m_box;
        
        m_box.height = height;
        if (reverseSide) m_box.top += initialBox.height - m_box.height;
        
        //Lower height : no collision, abort
        if (initialBox.height >= height)
            return;
        
        //Leave world : abort
        const Box &worldBox = m_physicsWorld->getWorldBox();
        
        if (!worldBox.contains(m_box))
        {
            m_box = initialBox;
            return;
        }
        
        //Potential Object hit
        std::vector<PhysicsObject*> initialHits = hitTest(initialBox);
        std::vector<PhysicsObject*> finalHits = hitTest();
        
        PhysicsRoutines::soustractVectors(finalHits, initialHits);
        
        if (finalHits.size() > 0) //Hits
        {
            // If fixed objects are hit abort to avoid moving objects past the fixed objects.
            if (PhysicsRoutines::containsFixedObject(finalHits))
            {
                m_box = initialBox;
                return;
            }
            
            // Otherwise, try to move the objects.
            std::vector<PhysicsObject*>::iterator it;
            
            for (it = finalHits.begin(); it != finalHits.end(); it++) //Move hit objects
            {
                if (!(*it)->isFixed())
                {
                    const Box &hitBox = (*it)->getBox();
                    
                    if (!reverseSide)
                        (*it)->moveYPosition((m_box.top + m_box.height) - hitBox.top, true);
                    else
                        (*it)->moveYPosition(m_box.top - (hitBox.top + hitBox.height), true);
                }
            }
            
            // If still hit (one of the objects hit a fixed object for example) : abort
            finalHits = hitTest();
            PhysicsRoutines::soustractVectors(finalHits, initialHits);
            
            if (finalHits.size() > 0)
                m_box = initialBox;
        }
    }
}

void PhysicsObject::setXSpeed(float xSpeed)
{
    m_speed.x = xSpeed;
}

void PhysicsObject::setYSpeed(float ySpeed)
{
    m_speed.y = ySpeed;
}

void PhysicsObject::setSpeed(const Vector &speed)
{
    if (!isFixed())
        m_speed = speed;
}

const Vector &PhysicsObject::getSpeed() const
{
    return m_speed;
}

void PhysicsObject::setAirPenetration(float airPenetration)
{
    if (airPenetration >= 0.f && airPenetration <= 1.f)
        m_airPenetration = airPenetration;
}

float PhysicsObject::getAirPenetration() const
{
    return m_airPenetration;
}

void PhysicsObject::setFixed(bool fixed)
{
    m_fixed = fixed;
}

bool PhysicsObject::isFixed() const
{
    return m_fixed;
}

float PhysicsObject::shadowProjection() const
{
    if (m_physicsWorld == NULL)
        return 0.f;
    
    // The object touches the ground
    const Box &worldBox = m_physicsWorld->getWorldBox();
    if (m_box.top + m_box.height >= worldBox.top + worldBox.height)
        return 0.f;
    
    std::vector<PhysicsObject*> objectsUnder = hitTest(
        Box(m_box.left + 0.2*m_box.width, m_box.top, 0.6*m_box.width,
            worldBox.top + worldBox.height - m_box.top)
        );
    PhysicsRoutines::sortObjectsByY(objectsUnder, 0);
    
    // The object is over another object
    std::vector<PhysicsObject*>::iterator it;
    for (it = objectsUnder.begin(); it != objectsUnder.end(); it++)
    {
        if ((*it)->interceptsShadows())
            return (*it)->getBox().top - (m_box.top + m_box.height);
    }
    
    //The shadow should be projected on the ground
    return worldBox.top + worldBox.height - (m_box.top + m_box.height);
}

/// Whether the object should intercept shadows of other objects over it
bool PhysicsObject::interceptsShadows() const
{
    return false;
}

bool PhysicsObject::isOnGround() const
{
    if (m_physicsWorld != NULL)
    {
        const Box &worldBox = m_physicsWorld->getWorldBox();
        
        if (m_box.top + m_box.height < worldBox.top + worldBox.height) // The object doesn't touch the ground
        {
            Box upperBox = m_box; //Box 1px up compared to m_box
            upperBox.top += 1;
            
            std::vector<PhysicsObject*> initialHits = hitTest();
            std::vector<PhysicsObject*> hitObjects = hitTest(upperBox);
            
            PhysicsRoutines::soustractVectors(hitObjects, initialHits);
            
            if (hitObjects.size() == 0) //The object doesn't touch another one
                return false;
        }
        
        return true;
    }
    
    return true;
}

const Box &PhysicsObject::getBox() const
{
    return m_box;
}

std::vector<PhysicsObject*> PhysicsObject::hitTest() const
{
    return hitTest(m_box);
}

std::vector<PhysicsObject*> PhysicsObject::hitTest(const Box &box) const
{
    std::vector<PhysicsObject*> hitObjects;
    
    if (m_physicsWorld != NULL)
        hitObjects = m_physicsWorld->hitTest((*this), box);
    
    return hitObjects;
}
