/**
  @file ConvexPolyhedron.cpp
  
  @author Morgan McGuire, http://graphics.cs.williams.edu

  @created 2001-11-11
  @edited  2009-08-10
 
  Copyright 2000-2009, Morgan McGuire.
  All rights reserved.
 */

#include "G3D/platform.h"
#include "G3D/ConvexPolyhedron.h"
#include "G3D/debug.h"

namespace G3D {

ConvexPolygon::ConvexPolygon(const Array<Vector3>& __vertex) : _vertex(__vertex) {
    // Intentionally empty
}


ConvexPolygon::ConvexPolygon(const Vector3& v0, const Vector3& v1, const Vector3& v2) {
    _vertex.append(v0, v1, v2);
}


bool ConvexPolygon::isEmpty() const {
    return (_vertex.length() == 0) || (getArea() <= fuzzyEpsilon32);
}


float ConvexPolygon::getArea() const {

    if (_vertex.length() < 3) {
        return 0;
    }

    float sum = 0;

    int length = _vertex.length();
    // Split into triangle fan, compute individual area
    for (int v = 2; v < length; ++v) {
        int i0 = 0;
        int i1 = v - 1;
        int i2 = v;

        sum += (_vertex[i1] - _vertex[i0]).cross(_vertex[i2] - _vertex[i0]).magnitude() / 2; 
    }

    return sum;
}

void ConvexPolygon::cut(const Plane& plane, ConvexPolygon &above, ConvexPolygon &below) {
    DirectedEdge edge;
    cut(plane, above, below, edge);
}

void ConvexPolygon::cut(const Plane& plane, ConvexPolygon &above, ConvexPolygon &below, DirectedEdge &newEdge) {
    above._vertex.resize(0);
    below._vertex.resize(0);

    if (isEmpty()) {
        //debugPrintf("Empty\n");
        return;
    }

    int v = 0;
    int length = _vertex.length();


    Vector3 polyNormal = normal();
    Vector3 planeNormal= plane.normal();

    // See if the polygon is *in* the plane.
    if (planeNormal.fuzzyEq(polyNormal) || planeNormal.fuzzyEq(-polyNormal)) {
        // Polygon is parallel to the plane.  It must be either above,
        // below, or in the plane.

        double a, b, c, d;
        Vector3 pt = _vertex[0];

        plane.getEquation(a,b,c,d);
        float r = (float)(a * pt.x + b * pt.y + c * pt.z + d);

        if (fuzzyGe(r, 0)) {
            // The polygon is entirely in the plane.
            //debugPrintf("Entirely above\n");
            above = *this;
            return;
        } else {
            //debugPrintf("Entirely below (1)\n");
            below = *this;
            return;
        }
    }


    // Number of edges crossing the plane.  Used for 
    // debug assertions.
    int count = 0;

    // True when the last _vertex we looked at was above the plane
    bool lastAbove = plane.halfSpaceContains(_vertex[v]);

    if (lastAbove) {
        above._vertex.append(_vertex[v]);
    } else {
        below._vertex.append(_vertex[v]);
    }

    for (v = 1; v < length; ++v) {
        bool isAbove = plane.halfSpaceContains(_vertex[v]);
    
        if (lastAbove ^ isAbove) {
            // Switched sides.
            // Create an interpolated point that lies
            // in the plane, between the two points.
            Line line = Line::fromTwoPoints(_vertex[v - 1], _vertex[v]);
            Vector3 interp = line.intersection(plane);
            
            if (! interp.isFinite()) {

                // Since the polygon is not in the plane (we checked above), 
                // it must be the case that this edge (and only this edge)
                // is in the plane.  This only happens when the polygon is
                // entirely below the plane except for one edge.  This edge
                // forms a degenerate polygon, so just treat the whole polygon
                // as below the plane.
                below = *this;
                above._vertex.resize(0);
                //debugPrintf("Entirely below\n");
                return;
            }                

            above._vertex.append(interp);
            below._vertex.append(interp);
            if (lastAbove) {
                newEdge.stop = interp;
            } else {
                newEdge.start = interp;
            }
            ++count;
        }

        lastAbove = isAbove;
        if (lastAbove) {
            above._vertex.append(_vertex[v]);
        } else {
            below._vertex.append(_vertex[v]);
        }
    }

    // Loop back to the first point, seeing if an interpolated point is
    // needed.
    bool isAbove = plane.halfSpaceContains(_vertex[0]);
    if (lastAbove ^ isAbove) {
        Line line = Line::fromTwoPoints(_vertex[length - 1], _vertex[0]);
        Vector3 interp = line.intersection(plane);
        if (! interp.isFinite()) {
            // Since the polygon is not in the plane (we checked above), 
            // it must be the case that this edge (and only this edge)
            // is in the plane.  This only happens when the polygon is
            // entirely below the plane except for one edge.  This edge
            // forms a degenerate polygon, so just treat the whole polygon
            // as below the plane.
            below = *this;
            above._vertex.resize(0);
            //debugPrintf("Entirely below\n");
            return;
        }                

        above._vertex.append(interp);
        below._vertex.append(interp);
        debugAssertM(count < 2, "Convex polygons may only intersect planes at two edges.");
        if (lastAbove) {
            newEdge.stop = interp;
        } else {
            newEdge.start = interp;
        }
        ++count;
    }

    debugAssertM((count == 2) || (count == 0), "Convex polygons may only intersect planes at two edges.");
}


ConvexPolygon ConvexPolygon::inverse() const {
    ConvexPolygon result;
    int length = _vertex.length();
    result._vertex.resize(length);
    
    for (int v = 0; v < length; ++v) {
        result._vertex[v] = _vertex[length - v - 1];
    }

    return result;
}


void ConvexPolygon::removeDuplicateVertices(){
    // Any valid polygon should have 3 or more vertices, but why take chances?
    if (_vertex.size() >= 2){

        // Remove duplicate vertices.
        for (int i=0;i<_vertex.size()-1;++i){
            if (_vertex[i].fuzzyEq(_vertex[i+1])){
                _vertex.remove(i+1);
                --i; // Don't move forward.
            }
        }
    
        // Check the last vertex against the first.
        if (_vertex[_vertex.size()-1].fuzzyEq(_vertex[0])){
            _vertex.pop();
        }
    }
}

//////////////////////////////////////////////////////////////////////////////

ConvexPolyhedron::ConvexPolyhedron(const Array<ConvexPolygon>& _face) : face(_face) {
    // Intentionally empty
}


float ConvexPolyhedron::getVolume() const {

    if (face.length() < 4) {
        return 0;
    }

    // The volume of any pyramid is 1/3 * h * base area.
    // Discussion at: http://nrich.maths.org/mathsf/journalf/oct01/art1/

    float sum = 0;

    // Choose the first _vertex of the first face as the origin.
    // This lets us skip one face, too, and avoids negative heights.
    Vector3 v0 = face[0]._vertex[0];
    for (int f = 1; f < face.length(); ++f) {        
        const ConvexPolygon& poly = face[f];
        
        float height = (poly._vertex[0] - v0).dot(poly.normal());
        float base   = poly.getArea();

        sum += height * base;
    }

    return sum / 3;
}

bool ConvexPolyhedron::isEmpty() const {
    return (face.length() == 0) || (getVolume() <= fuzzyEpsilon32);
}

void ConvexPolyhedron::cut(const Plane& plane, ConvexPolyhedron &above, ConvexPolyhedron &below) {
    above.face.resize(0);
    below.face.resize(0);

    Array<DirectedEdge> edge;

    int f;
    
    // See if the plane cuts this polyhedron at all.  Detect when
    // the polyhedron is entirely to one side or the other.
    //{
        int numAbove = 0, numIn = 0, numBelow = 0;
        bool ruledOut = false;
        double d;
        Vector3 abc;
        plane.getEquation(abc, d);

        // This number has to be fairly large to prevent precision problems down
        // the road.
        const float eps = 0.005f;
        for (f = face.length() - 1; (f >= 0) && (!ruledOut); f--) {
            const ConvexPolygon& poly = face[f];
            for (int v = poly._vertex.length() - 1; (v >= 0) && (!ruledOut); v--) { 
                double r = abc.dot(poly._vertex[v]) + d;
                if (r > eps) {
                    ++numAbove;
                } else if (r < -eps) {
                    ++numBelow;
                } else {
                    ++numIn;
                }

                ruledOut = (numAbove != 0) && (numBelow !=0);
            }
        }

        if (numBelow == 0) {
            above = *this;
            return;
        } else if (numAbove == 0) {
            below = *this;
            return;
        }
    //}

    // Clip each polygon, collecting split edges.
    for (f = face.length() - 1; f >= 0; f--) {
        ConvexPolygon a, b;
        DirectedEdge e;
        face[f].cut(plane, a, b, e);

        bool aEmpty = a.isEmpty();
        bool bEmpty = b.isEmpty();

        //debugPrintf("\n");
        if (! aEmpty) {
            //debugPrintf(" Above %f\n", a.getArea());
            above.face.append(a);
        }

        if (! bEmpty) {
            //debugPrintf(" Below %f\n", b.getArea());
            below.face.append(b);
        }

        if (! aEmpty && ! bEmpty) {
            //debugPrintf(" == Split\n");
            edge.append(e);
        } else {
            // Might be the case that the polygon is entirely on 
            // one side of the plane yet there is an edge we need
            // because it touches the plane.
            // 
            // Extract the non-empty _vertex list and examine it.
            // If we find exactly one edge in the plane, add that edge.
            const Array<Vector3>& _vertex = (aEmpty ? b._vertex : a._vertex);
            int L = _vertex.length();
            int count = 0;
            for (int v = 0; v < L; ++v) {
                if (plane.fuzzyContains(_vertex[v]) && plane.fuzzyContains(_vertex[(v + 1) % L])) {
                    e.start = _vertex[v];
                    e.stop = _vertex[(v + 1) % L];
                    ++count;
                }
            }

            if (count == 1) {
                edge.append(e);
            }
        }
    }

    if (above.face.length() == 1) {
        // Only one face above means that this entire 
        // polyhedron is below the plane.  Move that face over.
        below.face.append(above.face[0]);
        above.face.resize(0);
    } else if (below.face.length() == 1) {
        // This shouldn't happen, but it arises in practice
        // from numerical imprecision.
        above.face.append(below.face[0]);
        below.face.resize(0);
    }

    if ((above.face.length() > 0) && (below.face.length() > 0)) {
        // The polyhedron was actually cut; create a cap polygon
        ConvexPolygon cap;

        // Collect the final polgyon by sorting the edges
        int numVertices = edge.length();
/*debugPrintf("\n");
for (int xx=0; xx < numVertices; ++xx) {
    std::string s1 = edge[xx].start.toString();
    std::string s2 = edge[xx].stop.toString();
    debugPrintf("%s -> %s\n", s1.c_str(), s2.c_str());
}
*/

        // Need at least three points to make a polygon
        debugAssert(numVertices >= 3);

        Vector3 last_vertex = edge.last().stop;
        cap._vertex.append(last_vertex);

        // Search for the next _vertex.  Because of accumulating
        // numerical error, we have to find the closest match, not 
        // just the one we expect.
        for (int v = numVertices - 1; v >= 0; v--) {
            // matching edge index
            int index = 0;
            int num = edge.length();
            double distance = (edge[index].start - last_vertex).squaredMagnitude();
            for (int e = 1; e < num; ++e) {
                double d = (edge[e].start - last_vertex).squaredMagnitude();

                if (d < distance) {
                    // This is the new closest one
                    index = e;
                    distance = d;
                }
            }

            // Don't tolerate ridiculous error.
            debugAssertM(distance < 0.02, "Edge missing while closing polygon.");

            last_vertex = edge[index].stop;
            cap._vertex.append(last_vertex);
        }
        
        //debugPrintf("\n");
        //debugPrintf("Cap (both) %f\n", cap.getArea());
        above.face.append(cap);
        below.face.append(cap.inverse());
    }

    // Make sure we put enough faces on each polyhedra
    debugAssert((above.face.length() == 0) || (above.face.length() >= 4));   
    debugAssert((below.face.length() == 0) || (below.face.length() >= 4));
}

///////////////////////////////////////////////

ConvexPolygon2D::ConvexPolygon2D(const Array<Vector2>& pts, bool reverse) : m_vertex(pts) {
    if (reverse) {
        m_vertex.reverse();
    }
}


bool ConvexPolygon2D::contains(const Vector2& p, bool reverse) const {
    // Compute the signed area of each polygon from p to an edge.  
    // If the area is non-negative for all polygons then p is inside 
    // the polygon.  (To adapt this algorithm for a concave polygon,
    // the *sum* of the areas must be non-negative).

    float r = reverse ? -1.0f : 1.0f;

    for (int i0 = 0; i0 < m_vertex.size(); ++i0) {
        int i1 = (i0 + 1) % m_vertex.size();
        const Vector2& v0 = m_vertex[i0];
        const Vector2& v1 = m_vertex[i1];

        Vector2 e0 = v0 - p;
        Vector2 e1 = v1 - p;

        // Area = (1/2) cross product, negated to be ccw in
        // a 2D space; we neglect the 1/2
        float area = -(e0.x * e1.y - e0.y * e1.x);

        if (area * r < 0) {
            return false;
        }
    }

    return true;
}


}