//! Base implementation of a physical AABB for the Sju engine
/*!
 * The physical Axis-Aligned Bounding Box class is intended to be used for
 * wrapping non-immovable objects. This limitation will be removed in the near
 * future.
 *
 * It is useful to create "force fields" such as wind.
 */
/* Copyright (C) 2009-2011  Alejandro Valenzuela Roca, <lanjoe9 at mexinetica NOSPAMPLEASE dot com>
 *
 * This file is part of MotorJ, a free framework for videogame creation.
 * <http://wiki.lidsol.net/wiki/index.php?title=MotorJ >
 * <http://motorj.mexinetica.com/ >
 *
 * MotorJ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * MotorJ 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with MotorJ.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "paabb-class.h"

pAABB::pAABB()
{
	v3_0(&corners[0]);
	v3_0(&corners[1]);
	cGeometry = CG_AABB;
	cGeometryData = (void *) corners;
	v3_0(&v);
	v3_0(&a);
	v3_0(&rotv);
	v3_0(&rota);
	hasCollision = true;
	hasGravitation = false;
	hasGravity = false;
	hasCharge = false;
	movable = false;

}

void pAABB::SetData(vector3 a_min, vector3 a_max)
{
	float t;
	// Flip coordinates around in case they weren't specified correctly
	if (a_min.x > a_max.x)
	{
		t = a_min.x;
		a_min.x = a_max.x;
		a_max.x = t;
	}
	if (a_min.y > a_max.y)
	{
		t = a_min.y;
		a_min.y = a_max.y;
		a_max.y = t;
	}
	if (a_min.z > a_max.z)
	{
		t = a_min.z;
		a_min.z = a_max.z;
		a_max.z = t;
	}

	vec_copy(a_min, &corners[0]);
	vec_copy(a_max, &corners[1]);

	// Calculate the halfWidths of the aabb
	// Store them in halfWidths
	// Calculate the center of the aabb
	// Store it in "p"

	vec_copy(corners[1], &halfWidths);
	vec_sum(-1.0, corners[0], &halfWidths);
	mul_scal(0.5, &halfWidths);
	vec_copy(halfWidths, &p);
	vec_sum2(corners[0], &p);
}
pAABB::~pAABB()
{
}
void pAABB::ApplyEffect(vector3 effect, vector3 &pointOfAction, sjuPETypes effectType, IPhysicalObject* interactionObject)
{
    if (movable)
    {
        if (interactionObject && interactionObject->cGeometry == CG_AABB)
        {
            //collStatus = COLR_EXACT;
            // Neutralise both accel and velocity
            // In the direction of the effect
            if ((fabs(effect.x) > 0.001) && (sign(effect.x) != sign(v.x)))
            {
                v.x = 0;
            }
            if ((fabs(effect.y) > 0.001) && (sign(effect.y) != sign(v.y)))
            {
                v.y = 0;
            }
            if ((fabs(effect.z) > 0.001) && (sign(effect.z) != sign(v.z)))
            {
                v.z = 0;
            }

            //v3_0(&a);
            //v3_0(&v);
            MoveTo(pointOfAction);
        } else
        {
            // Respond to effect
            if (effectType == PE_C_IMMOVABLE)
            {
                // Collision reaction needs to be delayed until the speed
                // has been recalculated
                //collisionDetected = true;

                //vec_copy(effect, &f_1);
                //normalize(&f_1);

            } else if (effectType == PE_ACCEL)
            {
                // Acceleration response


                vec_sum2(effect, &a);
            } else if (effectType == PE_FRFIELD)
            {
                v.x *= 1-effect.x;
                v.y *= 1-effect.y;
                v.z *= 1-effect.z;

            } else if (effectType == PE_ROTACCEL)
            {
                vec_sum2(effect, &rota);
            } else if (effectType == PE_TORQUE)
            {
                //vec_sum(invMInertia, effect, &rota);
            } else if (effectType == PE_CONTACT)
            {
                vec_copy(effect, &v);
            } else
            {
                //printf("%3.3f: %3.3f, %3.3f, %3.3f\n", invmass, effect.x, effect.y, effect.z);
                //vec_sum(invmass, effect, &a);
            }
        }
    }
}
void pAABB::Move(vector3 displacement)
{
	vec_sum2(displacement, &p);
	vec_sum2(displacement, &corners[0]);
	vec_sum2(displacement, &corners[1]);
}
void pAABB::MoveTo(vector3 position)
{
	vec_copy(position, &p);
	vec_copy(p,&corners[0]);
	vec_copy(p,&corners[1]);
	vec_sum2(halfWidths,&corners[1]);
	vec_sum(-1, halfWidths, &corners[0]);
}
void pAABB::ResetAccels()
{
    if (movable)
    {
        v3_0(&a);
        v3_0(&rota);
        collisionDetected = false;
        collStatus = COLR_NOCOLLISION;
    }
}
void pAABB::Update(float t_elapsed)
{
    if (movable)
    {
        //if (collStatus == COLR_NOCOLLISION)

        //vec_sum2(g, &a);

        vec_sum(t_elapsed, a, &v);
        //vec_sum(t_elapsed, rota, &rotv); by definition it CANNOT rotate.


        // Do the dragFactor scaling
        mul_scal(1.0-dragFactor, &v);
        //mul_scal(1.0-rotDragFactor, &rotv);
        vec_sum(t_elapsed, v, &corners[0]);
        vec_sum(t_elapsed, v, &corners[1]);
        vec_sum(t_elapsed, v, &p);
    }
}