#ifndef Foundation_visibility_INCLUDED
#define Foundation_visibility_INCLUDED

#include <iostream>

#include <foundation/SharedPtr.h>

#include <foundation/Math/Matrix.hpp>
#include <foundation/Math/Plane.hpp>
#include <foundation/Math/BoundingBox.hpp>

namespace Tn {

namespace math {

/*
 * These are routines to find the viewing frustum planes in world
 * coordinates given the concatenation of the projection and viewing
 * matrices. They can be used to perform visibility culling.
 *
 * http://www.cs.otago.ac.nz/postgrads/alexis/planeExtraction.pdf
 *
 * Note that rows and columns are reversed in our matrices compared to
 * the notation of the paper.
 */

#if 0

template <typename T>
Plane<T> getFrustumLeft(Matrix<T> const & m) {

    T a = m[3][0] + m[0][0];
    T b = m[3][1] + m[0][1];
    T c = m[3][2] + m[0][2];
    T d = m[3][3] + m[0][3];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumRight(Matrix<T> const & m) {

    T a = m[3][0] - m[0][0];
    T b = m[3][1] - m[0][1];
    T c = m[3][2] - m[0][2];
    T d = m[3][3] - m[0][3];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumBottom(Matrix<T> const & m) {

    T a = m[3][0] + m[1][0];
    T b = m[3][1] + m[1][1];
    T c = m[3][2] + m[1][2];
    T d = m[3][3] + m[1][3];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumTop(Matrix<T> const & m) {

    T a = m[3][0] - m[1][0];
    T b = m[3][1] - m[1][1];
    T c = m[3][2] - m[1][2];
    T d = m[3][3] - m[1][3];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumNear(Matrix<T> const & m) {

    T a = m[3][0] + m[2][0];
    T b = m[3][1] + m[2][1];
    T c = m[3][2] + m[2][2];
    T d = m[3][3] + m[2][3];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumFar(Matrix<T> const & m) {

    T a = m[3][0] - m[2][0];
    T b = m[3][1] - m[2][1];
    T c = m[3][2] - m[2][2];
    T d = m[3][3] - m[2][3];

    return Plane<T>(a, b, c, d);
}

#else
template <typename T>
Plane<T> getFrustumLeft(Matrix<T> const & m) {

    T a = m[0][3] + m[0][0];
    T b = m[1][3] + m[1][0];
    T c = m[2][3] + m[2][0];
    T d = m[3][3] + m[3][0];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumRight(Matrix<T> const & m) {

    T a = m[0][3] - m[0][0];
    T b = m[1][3] - m[1][0];
    T c = m[2][3] - m[2][0];
    T d = m[3][3] - m[3][0];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumBottom(Matrix<T> const & m) {

    T a = m[0][3] + m[0][1];
    T b = m[1][3] + m[1][1];
    T c = m[2][3] + m[2][1];
    T d = m[3][3] + m[3][1];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumTop(Matrix<T> const & m) {

    T a = m[0][3] - m[0][1];
    T b = m[1][3] - m[1][1];
    T c = m[2][3] - m[2][1];
    T d = m[3][3] - m[3][1];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumNear(Matrix<T> const & m) {

    T a = m[0][3] + m[0][2];
    T b = m[1][3] + m[1][2];
    T c = m[2][3] + m[2][2];
    T d = m[3][3] + m[3][2];

    return Plane<T>(a, b, c, d);
}


template <typename T>
Plane<T> getFrustumFar(Matrix<T> const & m) {

    T a = m[0][3] - m[0][2];
    T b = m[1][3] - m[1][2];
    T c = m[2][3] - m[2][2];
    T d = m[3][3] - m[3][2];

    return Plane<T>(a, b, c, d);
}
#endif

/* 'mat' is concatenation of projection and view matrix.
 */

template <typename T>
struct FrustumVisibilityTest {
        
    FrustumVisibilityTest(Matrix<T> const & mat);

    bool operator()(BoundingBox<T> const & box) const;

private:

    /* Tests whether the center of the bounding box is within the
     * viewing frustum planes. (One potential optimization is that if
     * it returns true, you know all four children are visible. (This
     * is an ASSUMPTION that happens to be true for a uniform
     * rectilinear grid. It is not true in the general case. To make
     * it true in the general case, we would need a different bounding
     * box representation specific to quad-tree subdivision.))
     */

    bool centerVisible(BoundingBox<T> const &) const;

    /* This returns true if the bounding sphere of the bounding box
     * intersects the viewing frustum. This means the tile is probably
     * visible, but not guaranteed to be. It may be good enough. */

    bool outsideFrustum(BoundingBox<T> const &) const;

    /* It is assumed the planes face inwards. */

    Plane<T> planes[6];
};

typedef FrustumVisibilityTest<double> FrustumVisibilityTestd;
typedef FrustumVisibilityTest<float> FrustumVisibilityTestf;


template <typename T>
FrustumVisibilityTest<T>::FrustumVisibilityTest(Matrix<T> const & mat) {
    planes[0] = normalize(getFrustumLeft(mat));
    planes[1] = normalize(getFrustumRight(mat));
    planes[2] = normalize(getFrustumBottom(mat));
    planes[3] = normalize(getFrustumTop(mat));
    planes[4] = normalize(getFrustumNear(mat));
    planes[5] = normalize(getFrustumFar(mat));
}


template <typename T>
bool FrustumVisibilityTest<T>::operator()(BoundingBox<T> const & box) const
{
    /* The centerVisible quickly includes tiles that are for sure
     * visible. The radiusTest quickly excludes tiles that are for
     * sure not visible. The rest are tiles that are 'close' so for
     * now assume they are visible. */

    if (centerVisible(box)) 
    {
        return true;
    }

    if (outsideFrustum(box)) return false;

    // Optionally put more refined tests here.

    return true;
}


template <typename T>
bool FrustumVisibilityTest<T>::centerVisible(BoundingBox<T> const & box) const {

    for (size_t i = 0; i<6; ++i) {

        T d = intersect(planes[i], box.center);
        if (d<T(0)) return false;
    }
    return true;
}


#if 0

/*  This test works, but seems overly generous.
 *
 */

template <typename T>
bool FrustumVisibilityTest<T>::outsideFrustum(BoundingBox<T> const & box) const {
    
    double x = box.dim[0]/2.0;
    double y = box.dim[1]/2.0;
    double z = box.dim[2]/2.0;
    
    T r = sqrt(x*x + y*y + z*z);

    for (size_t i = 0; i<6; ++i) {

        T d = intersect(planes[i], box.center);
        if (d < -r) return true;
    }

    return false;
}
#else

template <typename T> 
bool outsidePlane(Plane<T> const & plane, BoundingBox<T> const & box) {

    for (size_t i=0; i<8; ++i)
    {
        Point3d p = box.corner(i);
        T d = intersect(plane, p);
        if (d > 0) return false;
    }
    return true;
}


template <typename T>
bool FrustumVisibilityTest<T>::outsideFrustum(BoundingBox<T> const & box) const {
    
    for (size_t j = 0; j<6; ++j)
    {
        if (outsidePlane(planes[j], box))
        {
            return true;
        }
    }

    // More refined test here.

    return false;
}
#endif

} // namespace math

} // namespace Tn

#endif
