/*
 *  OrientedBoundingRect.hpp
 *
 *  Created by Thant on 1/6/2011
 *  Copyright 2011 TeleNav Inc. All rights reserved.
 *
 *  Implements oriented bounding rectangle along with functions to
 *  transform and test for intersection. Allows for 3D transformation,
 *  but the idea is that the test for collision will always happen in
 *  ground space or screen space, so the collision is tested in the
 *  z=0 plane. This is done by internally converting an
 *  OrientedBoundingRect to the 2D helper class OrientedBoundingRect2.
 *
 *  An oriented bounding rect is a rectangular region represented by a
 *  center point and two (presumably orthogonal) vectors. The length
 *  of the vectors indicate the dimensions along those axes.
 *
 */

#ifndef Foundation_OrientedBoundingRect_INCLUDED
#define Foundation_OrientedBoundingRect_INCLUDED

#include <assert.h>
#include <deque>

#include <foundation/Math/Point.hpp>
#include <foundation/Math/Vec.hpp>
#include <foundation/Math/Matrix.hpp>
#include <foundation/Math/transform.hpp>

namespace Tn
{

namespace math
{

template <typename T>
struct OrientedBoundingRect
{

    OrientedBoundingRect(Point<3,T> const & center, T width, T height);

    // This way of constructing an oriented bounding rectangle assume
    // the bounding box is flat in the z axis (i.e. the z coordinates
    // match). In practice, this constructor finds the average of the
    // two z coordinates and uses that as the rectangle's z coordinate.
    OrientedBoundingRect(Point<3,T> const & lower_left, Point<3,T> const & upper_right);

    // For construction of non-axis-aligned bounding rect
    OrientedBoundingRect(Point<3,T> const & center, 
                         Vec<3,T> const & x_axis,
                         Vec<3,T> const & y_axis);

    Point<3,T> center;
    Vec<3,T> axes[2];
};


typedef OrientedBoundingRect<float> OrientedBoundingRectf;
typedef OrientedBoundingRect<double> OrientedBoundingRectd;


// Collision happens in the z=0 plane by converting an oriented
// bounding rect into a 2D oriented bounding rect, and doing the
// collision with those.

template <typename T>
struct OrientedBoundingRect2
{
    OrientedBoundingRect2(Point<2,T> const & center,
                          Vec<2,T> const & x_axis,
                          Vec<2,T> const & y_axis)
        : center(center)
        {
            axes[0] = x_axis;
            axes[1] = y_axis;
        }

    OrientedBoundingRect2(OrientedBoundingRect<T> const & that)
        : center(Point2<T>(that.center[0], that.center[1]))
        {
            axes[0] = Vec2<T>(that.axes[0][0], that.axes[0][1]);
            axes[1] = Vec2<T>(that.axes[1][0], that.axes[1][1]);
        }

    Point<2,T> center;
    Vec<2,T> axes[2];

    // 0-3
    Point<2,T> corner(int i) const;
};


typedef OrientedBoundingRect2<float> OrientedBoundingRect2f;
typedef OrientedBoundingRect2<double> OrientedBoundingRect2d;

//bounding box face projection into screen, can not keep the nature of the parallel, so can not rebuilt by center and axes
//tetragon bounding: projection from rectangle bounding
//bring in 4 corner points
//keep axes, to get which edges face to the specified tetragon, because projection can not change positional relation of 2 tetragons
template <typename T>
struct OrientedBoundingTetragon
{

    OrientedBoundingTetragon(Point<3,T> const & vCenter, Vec<3,T> const & x_axis,
                          Vec<3,T> const & y_axis, Point<3,T> vCorners[])
        : center(vCenter)
    {
        axes[0] = x_axis;
        axes[1] = y_axis;
        corners[0] = vCorners[0];
        corners[1] = vCorners[1];
        corners[2] = vCorners[2];
        corners[3] = vCorners[3];
    }

    OrientedBoundingTetragon(OrientedBoundingRect<T> const & rectBounds)
        : center(rectBounds.center)
    {
        axes[0] = rectBounds.axes[0];
        axes[1] = rectBounds.axes[1];
        corners[0] = rectBounds.center - rectBounds.axes[0] - rectBounds.axes[1];
        corners[1] = rectBounds.center - rectBounds.axes[0] + rectBounds.axes[1];
        corners[2] = rectBounds.center + rectBounds.axes[0] + rectBounds.axes[1];
        corners[3] = rectBounds.center + rectBounds.axes[0] - rectBounds.axes[1];
    }

    Point<3,T> center;
    Vec<3,T> axes[2];
    Point<3,T> corners[4];
};

typedef OrientedBoundingTetragon<float> OrientedBoundingTetragonf;
typedef OrientedBoundingTetragon<double> OrientedBoundingTetragond;


//2d tetragon bounding
template <typename T>
struct OrientedBoundingTetragon2
{
    OrientedBoundingTetragon2(Point<2,T> const & vCenter, Vec<2,T> const & x_axis,
                          Vec<2,T> const & y_axis, Point<2,T> vCorners[])
        : center(vCenter)
    {
        axes[0] = x_axis;
        axes[1] = y_axis;
        corners[0] = vCorners[0];
        corners[1] = vCorners[1];
        corners[2] = vCorners[2];
        corners[3] = vCorners[3];
    }

    OrientedBoundingTetragon2(OrientedBoundingRect2<T> const & rectBounds)
        : center(rectBounds.center)
    {
        axes[0] = rectBounds.axes[0];
        axes[1] = rectBounds.axes[1];
        corners[0] = rectBounds.center - rectBounds.axes[0] - rectBounds.axes[1];
        corners[1] = rectBounds.center - rectBounds.axes[0] + rectBounds.axes[1];
        corners[2] = rectBounds.center + rectBounds.axes[0] + rectBounds.axes[1];
        corners[3] = rectBounds.center + rectBounds.axes[0] - rectBounds.axes[1];
    }

    OrientedBoundingTetragon2(OrientedBoundingTetragon<T> const & tetragonBounds)
    {
        center = Point2<T>(tetragonBounds.center[0], tetragonBounds.center[1]);
        axes[0] = Vec2<T>(tetragonBounds.axes[0][0], tetragonBounds.axes[0][1]);
        axes[1] = Vec2<T>(tetragonBounds.axes[1][0], tetragonBounds.axes[1][1]);
        corners[0] = Point2<T>(tetragonBounds.corners[0][0], tetragonBounds.corners[0][1]);
        corners[1] = Point2<T>(tetragonBounds.corners[1][0], tetragonBounds.corners[1][1]);
        corners[2] = Point2<T>(tetragonBounds.corners[2][0], tetragonBounds.corners[2][1]);
        corners[3] = Point2<T>(tetragonBounds.corners[3][0], tetragonBounds.corners[3][1]);
    }

    Point<2,T> center;
    Vec<2,T> axes[2];
    Point<2,T> corners[4];
};

typedef OrientedBoundingTetragon2<float> OrientedBoundingTetragon2f;
typedef OrientedBoundingTetragon2<double> OrientedBoundingTetragon2d;


// This gets a given point's location in terms of a bounding rect.

template <typename T>
Point<2,T> transform(OrientedBoundingRect<T> const & rect, Point<2,T> const & p)
{
    OrientedBoundingRect2<T> rect2 = OrientedBoundingRect2<T>(rect);

    Vec<2,T> v = p - rect2.center;

    T len_x_axis = magnitude(rect2.axes[0]);
    T len_y_axis = magnitude(rect2.axes[1]);

    T x = dot(rect2.axes[0] / len_x_axis, v) / len_x_axis;
    T y = dot(rect2.axes[1] / len_y_axis, v) / len_y_axis;

    return Point2<T>(x, y);
}


template <typename T>
OrientedBoundingRect<T>::OrientedBoundingRect(Point<3,T> const & center_,
                                              Vec<3,T> const & x_axis,
                                              Vec<3,T> const & y_axis)
    : center(center_)
{
    axes[0] = x_axis;
    axes[1] = y_axis;
}


template <typename T>
OrientedBoundingRect<T>::OrientedBoundingRect(Point<3,T> const & center_, T width, T height) 
  : center(center_)
{
    axes[0] = Vec<3,T>(width/T(2),        T(0),       T(0));
    axes[1] = Vec<3,T>(      T(0), height/T(2),       T(0));
}


template <typename T>
OrientedBoundingRect<T>::OrientedBoundingRect(Point<3,T> const & llf, Point<3,T> const & urn)
    : center(Point3<T>((llf[0]+urn[0])/T(2),
                       (llf[1]+urn[1])/T(2),
                       (llf[2]+urn[2])/T(2)))
{
    T width  = urn[0] - llf[0];
    T height = urn[1] - llf[1];

    axes[0] = Vec3<T>(width/T(2),        T(0),       T(0));
    axes[1] = Vec3<T>(      T(0), height/T(2),       T(0));
}


template <typename T>
OrientedBoundingRect<T> transform(Vec<3,T> const & v, OrientedBoundingRect<T> const & rect)
{
    return OrientedBoundingRect<T>(rect.center + v,
                                   rect.axes[0],
                                   rect.axes[1]);
}


template <typename T>
OrientedBoundingRect<T> transform(Matrix<T> const & m, OrientedBoundingRect<T> const & rect)
{
    Point<3,T> new_center = transform(m, rect.center);

    return OrientedBoundingRect<T>(new_center,
                                   transform(m, rect.center + rect.axes[0]) - new_center,
                                   transform(m, rect.center + rect.axes[1]) - new_center);

}


template <typename T>
OrientedBoundingTetragon<T> transform(Matrix<T> const & m, OrientedBoundingTetragon<T> const & tetragon)
{
    Point<3,T> center = transform(m, tetragon.center);
    Vec<3,T> x_axis = transform(m, tetragon.axes[0]);
    Vec<3,T> y_axis = transform(m, tetragon.axes[1]);
    Point<3,T> corners[4];
    corners[0] = transform(m, tetragon.corners[0]);
    corners[1] = transform(m, tetragon.corners[1]);
    corners[2] = transform(m, tetragon.corners[2]);
    corners[3] = transform(m, tetragon.corners[3]);

    return OrientedBoundingTetragon<T>(center, x_axis, y_axis, corners);
}


template <typename T>
OrientedBoundingRect<T> operator*(OrientedBoundingRect<T> const & a, T s)
{
    return OrientedBoundingRect<T>(a.center, a.axes[0] * s, a.axes[1] * s);
}


template <typename T>
Point<2,T> OrientedBoundingRect2<T>::corner(int i) const
{
    assert((i>=0) && (i<4));
    return
        center
        + ((i & (0x1 << 0)) ? axes[0] : -axes[0])
        + ((i & (0x1 << 1)) ? axes[1] : -axes[1]);
}


template <typename T>
OrientedBoundingRect2<T> operator*(OrientedBoundingRect2<T> const & a, T s)
{
    return OrientedBoundingRect2<T>(a.center, a.axes[0] * s, a.axes[1] * s);
}



template <typename T>
bool collide(OrientedBoundingRect2<T> const & rect, Point<2,T> const & face, Vec<2,T> const & axis)
{
    for (int i=0; i<4; ++i)
    {
        Vec<2,T> v = rect.corner(i) - face;
        if (dot(v, axis) < 0) return true;
    }
    return false;
}


template <typename T>
bool collide(OrientedBoundingRect2<T> const & rect, Point<2,T> const & face, Vec<2,T> const & axis, 
             Point<2, T> const & corner0, Point<2, T> const & corner1)
{
    for (int i=0; i<4; ++i)
    {
        Vec<2,T> v = rect.corner(i) - face;
        Vec<2,T> border = corner1 - corner0;
        T v_test = cross(v, border);
        T axis_test = cross(axis, border);
        if (axis_test * v_test <= 0)    // they are facing different direction
        {
            return true;
        }

    }
    return false;
}


template <typename T>
bool collide(OrientedBoundingRect<T> const & a_3d, OrientedBoundingRect<T> const & b_3d)
{
    // Project onto z=0 plane and do our collision testing as edges.

    OrientedBoundingRect2<T> a = OrientedBoundingRect2<T>(a_3d);
    OrientedBoundingRect2<T> b = OrientedBoundingRect2<T>(b_3d);
    return collide(a, b);
}



template <typename T>
bool collide(OrientedBoundingRect2<T> const & a, OrientedBoundingRect2<T> const & b)
{
    // First do a bounding circle test.

    T r_a = magnitude(a.axes[0] + a.axes[1]);
    T r_b = magnitude(b.axes[0] + b.axes[1]);

    Vec<2,T> a2b = b.center - a.center;

    T r = magnitude(a2b);

    if (r > r_a + r_b) return false;

    // Test a's separation axes against b's corners. If we find one
    // that works, we know the bounding rectes don't intersect.

    for (int i=0; i<2; ++i)
    {
        Vec<2,T> axis = (0 < dot(a.axes[i], a2b)) ? a.axes[i] : -a.axes[i];
        Point<2,T> face = a.center + axis;
        Point<2,T> corner0 = face - a.axes[1-i];
        Point<2,T> corner1 = face + a.axes[1-i];
        if (!collide(b, face, axis, corner0, corner1)) return false;  // found one that works
    }

    // Test b's separation axes against a's corners.

    for (int i=0; i<2; ++i)
    {
        Vec<2,T> axis = (0 < dot(b.axes[i], -a2b)) ? b.axes[i] : -b.axes[i];
        Point<2,T> face = b.center + axis;
        Point<2,T> corner0 = face - b.axes[1-i];
        Point<2,T> corner1 = face + b.axes[1-i];
        if (!collide(a, face, axis, corner0, corner1)) return false;  // found one that works
    }

    return true;
}


template <typename T>
bool collide(OrientedBoundingTetragon2<T> const & tetragon, Vec<2,T> const & axis, 
             Point<2, T> const & corner0, Point<2, T> const & corner1)
{
    for (int i=0; i<4; ++i)
    {
        Vec<2,T> v = tetragon.corners[i]- corner0;
        Vec<2,T> border = corner1 - corner0;
        T v_test = cross(v, border);
        T axis_test = cross(axis, border);
        if (axis_test * v_test <= 0)    // they are facing different direction
        {
            return true;
        }
    }
    return false;
}


template <typename T>
bool collide(OrientedBoundingTetragon<T> const & a_3d, OrientedBoundingTetragon<T> const & b_3d)
{
    // Project onto z=0 plane and do our collision testing as edges.

    OrientedBoundingTetragon2<T> a = OrientedBoundingTetragon2<T>(a_3d);
    OrientedBoundingTetragon2<T> b = OrientedBoundingTetragon2<T>(b_3d);
    return collide(a, b);
}


template <typename T>
bool collide(OrientedBoundingTetragon2<T> const & a, OrientedBoundingTetragon2<T> const & b)
{
    // First do a bounding circle test.
    //get radius of a
    T r_a = T();
    for (int i = 0; i < 4; i++)
    {
        T dis = magnitude(a.center - a.corners[i]);
        if (r_a < dis)
        {
            r_a = dis;
        }
    }
    
    //get radius of b
    T r_b = T();
    for (int i = 0; i < 4; i++)
    {
        T dis = magnitude(b.center - b.corners[i]);
        if (r_b < dis)
        {
            r_b = dis;
        }
    }

    Vec<2,T> a2b = b.center - a.center;

    T r = magnitude(a2b);

    if (r > r_a + r_b) return false;

    //calc which axis points to edge 1->2, which points to 2->3
    //then can get which edges face to b
    bool swap_a = cross(a.axes[0], a.axes[1]) <= 0;

    // Test a's separation axes against b's corners. If we find one
    // that works, we know the bounding tetragons don't intersect.
    for (int i=0; i<2; ++i)
    {
        Vec<2,T> axis;
        Point<2,T> corner0, corner1;
        if (0 < dot((swap_a ? a.axes[i] : a.axes[1-i]), a2b))
        {
            axis = swap_a ? a.axes[i] : a.axes[1-i];
            corner0 = a.corners[i+1];
            corner1 = a.corners[i+2];
        }
        else
        {
            axis = -(swap_a ? a.axes[i] : a.axes[1-i]);
            corner0 = a.corners[(i+3)%4];
            corner1 = a.corners[i];
        }

        if (!collide(b, axis, corner0, corner1)) return false;
    }

    //calc which axis points to edge 1->2, which points to 2->3
    //then can get which edges face to a
    bool swap_b = cross(b.axes[0], b.axes[1]) <= 0;

    // Test b's separation axes against a's corners.
    for (int i=0; i<2; ++i)
    {
        Vec<2,T> axis;
        Point<2,T> corner0, corner1;
        if (0 < dot((swap_b ? b.axes[i] : b.axes[1-i]), -a2b))
        {
            axis = swap_b ? b.axes[i] : b.axes[1-i];
            corner0 = b.corners[i+1];
            corner1 = b.corners[i+2];
        }
        else
        {
            axis = -(swap_b ? b.axes[i] : b.axes[1-i]);
            corner0 = b.corners[(i+3)%4];
            corner1 = b.corners[i];
        }

        if (!collide(a, axis, corner0, corner1)) return false;
    }

    return true;
}


} // namespace math

} // namespace Tn

#endif
