#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <utils.h>
#include <convexpoly.h>

typedef struct {
    double      vx, vy, c;     /* edge equation  vx*X + vy*Y + c = 0 */
    int         ext_flag;      /* TRUE == exterior edge of polygon */
} PlaneSet, *pPlaneSet ;

pPlaneSet PlaneSetup( double **pgon, int numverts );
int PlaneTest( pPlaneSet p_plane_set, int numverts, double point[2] );

ConvexPoly::ConvexPoly( VertexList *vlist )
{
    Vector3 normal;
    int i;

    numpts = vlist->size();

    ptlist = new Point3[ numpts ];
    for( i = 0; i < numpts; i++ ) {
        ptlist[ i ] = (*vlist)[ i ];
    }

    Vector3 v1 = ptlist[ 1 ].subtract( ptlist[ 0 ] ).normalize();
    Vector3 v2 = ptlist[ 2 ].subtract( ptlist[ 0 ] ).normalize();
    normal = v1.cross( v2 ).normalize();

    double bx, by, bz;
    bx = fabs( normal.getX() );
    by = fabs( normal.getY() );
    bz = fabs( normal.getZ() );
    bigx = ( bx >= by ) && ( bx >= bz );
    bigy = ( by >= bx ) && ( by >= bz );
    bigz = ( bz >= bx ) && ( bz >= by );

    if( bigx ) {
        double **pgon = new double*[ numpts ];
        for( i = 0; i < numpts; i++ ) {
            pgon[ i ] = new double[ 2 ];
            pgon[ i ][ 0 ] = ptlist[ i ].getY();
            pgon[ i ][ 1 ] = ptlist[ i ].getZ();
        }
        pset = PlaneSetup( pgon, numpts );
    } else if( bigy ) {
        double **pgon = new double*[ numpts ];
        for( i = 0; i < numpts; i++ ) {
            pgon[ i ] = new double[ 2 ];
            pgon[ i ][ 0 ] = ptlist[ i ].getX();
            pgon[ i ][ 1 ] = ptlist[ i ].getZ();
        }
        pset = PlaneSetup( pgon, numpts );
    } else {
        double **pgon = new double*[ numpts ];
        for( i = 0; i < numpts; i++ ) {
            pgon[ i ] = new double[ 2 ];
            pgon[ i ][ 0 ] = ptlist[ i ].getX();
            pgon[ i ][ 1 ] = ptlist[ i ].getY();
        }
        pset = PlaneSetup( pgon, numpts );
    }

    a = normal.getX();
    b = normal.getY();
    c = normal.getZ();
    d =   ( -a * ptlist[ 0 ].getX() )
        - (  b * ptlist[ 0 ].getY() )
        - (  c * ptlist[ 0 ].getZ() );
}

ConvexPoly::~ConvexPoly( void )
{
    delete[] ptlist;
}

bool ConvexPoly::closestIntersect( const Point3 &origin, const Vector3 &dir,
                                   double *t, Vector3 *norm, Material **mat ) const
{
    Vector3 v1, v2;
    Point3 intpoint;
    double intpointd[2];

    if( !planeIntersect( origin, dir, t, norm ) ) {
        return false;
    }

    intpoint = origin.add( dir.scalarMult( *t ) );

    if( bigx ) {
        intpointd[ 0 ] = intpoint.getY();
        intpointd[ 1 ] = intpoint.getZ();
    } else if( bigy ) {
        intpointd[ 0 ] = intpoint.getX();
        intpointd[ 1 ] = intpoint.getZ();
    } else {
        intpointd[ 0 ] = intpoint.getX();
        intpointd[ 1 ] = intpoint.getY();
    }

    if( PlaneTest( (pPlaneSet) pset, numpts, intpointd ) ) {
        *mat = getMaterial();
        return true;
    }
    return false;
}



/* ======= Triangle half-plane algorithm ================================== */

/* Split the polygon into a fan of triangles and for each triangle test if
 * the point is inside of the three half-planes formed by the triangle's edges.
 *
 * Call setup with 2D polygon _pgon_ with _numverts_ number of vertices,
 * which returns a pointer to a plane set array.
 * Call testing procedure with a pointer to this array, _numverts_, and
 * test point _point_, returns 1 if inside, 0 if outside.
 * Call cleanup with pointer to plane set array to free space.
 *
 * SORT and CONVEX can be defined for this test.
 */


/* Size sorting structure for half-planes */
typedef struct {
    double      size ;
    pPlaneSet   pps ;
} SizePlanePair, *pSizePlanePair ;

int CompareSizePlanePairs( const void *a1, const void *a2 )
{
    pSizePlanePair p_sp0 = (pSizePlanePair) a1;
    pSizePlanePair p_sp1 = (pSizePlanePair) a2;
    if ( p_sp0->size == p_sp1->size ) {
        return( 0 ) ;
    } else {
        return( p_sp0->size > p_sp1->size ? -1 : 1 ) ;
    }
}

/* split polygons along set of x axes - call preprocess once */
pPlaneSet PlaneSetup( double **pgon, int numverts )
{
    int i, p1, p2;
    double tx, ty, vx0, vy0;
    pPlaneSet pps, pps_return;
    double len[3], len_temp;
    int j;
    PlaneSet ps_temp;
    pPlaneSet pps_new;
    pSizePlanePair p_size_pair;

    pps = pps_return = (pPlaneSet)malloc( 3 * (numverts-2) * sizeof( PlaneSet ));
    // MALLOC_CHECK( pps ) ;
    p_size_pair = (pSizePlanePair)malloc( (numverts-2) * sizeof( SizePlanePair ) );
    // MALLOC_CHECK( p_size_pair ) ;

    vx0 = pgon[0][0] ;
    vy0 = pgon[0][1] ;

    for ( p1 = 1, p2 = 2 ; p2 < numverts ; p1++, p2++ ) {
        pps->vx = vy0 - pgon[p1][1] ;
        pps->vy = pgon[p1][0] - vx0 ;
        pps->c = pps->vx * vx0 + pps->vy * vy0 ;
        len[0] = pps->vx * pps->vx + pps->vy * pps->vy ;
        pps->ext_flag = ( p1 == 1 );

        /* Sort triangles by areas, so compute (twice) the area here */
        p_size_pair[p1-1].pps = pps ;
        p_size_pair[p1-1].size =
                        ( pgon[0][0] * pgon[p1][1] ) +
                        ( pgon[p1][0] * pgon[p2][1] ) +
                        ( pgon[p2][0] * pgon[0][1] ) -
                        ( pgon[p1][0] * pgon[0][1] ) -
                        ( pgon[p2][0] * pgon[p1][1] ) -
                        ( pgon[0][0] * pgon[p2][1] ) ;
        pps++ ;
        pps->vx = pgon[p1][1] - pgon[p2][1] ;
        pps->vy = pgon[p2][0] - pgon[p1][0] ;
        pps->c = pps->vx * pgon[p1][0] + pps->vy * pgon[p1][1] ;
        len[1] = pps->vx * pps->vx + pps->vy * pps->vy ;
        pps->ext_flag = 1;
        pps++ ;
        pps->vx = pgon[p2][1] - vy0 ;
        pps->vy = vx0 - pgon[p2][0] ;
        pps->c = pps->vx * pgon[p2][0] + pps->vy * pgon[p2][1] ;
        len[2] = pps->vx * pps->vx + pps->vy * pps->vy ;
        pps->ext_flag = ( p2 == numverts-1 );

        /* find an average point which must be inside of the triangle */
        tx = ( vx0 + pgon[p1][0] + pgon[p2][0] ) / 3.0 ;
        ty = ( vy0 + pgon[p1][1] + pgon[p2][1] ) / 3.0 ;

        /* check sense and reverse if test point is not thought to be inside
         * first triangle
         */
        if ( pps->vx * tx + pps->vy * ty >= pps->c ) {
            /* back up to start of plane set */
            pps -= 2 ;
            /* point is thought to be outside, so reverse sense of edge
             * normals so that it is correctly considered inside.
             */
            for ( i = 0 ; i < 3 ; i++ ) {
                pps->vx = -pps->vx ;
                pps->vy = -pps->vy ;
                pps->c  = -pps->c ;
                pps++ ;
            }
        } else {
            pps++ ;
        }

        /* sort the planes based on the edge lengths */
        pps -= 3 ;
        for ( i = 0 ; i < 2 ; i++ ) {
            for ( j = i+1 ; j < 3 ; j++ ) {
                if ( len[i] < len[j] ) {
                    ps_temp = pps[i] ;
                    pps[i] = pps[j] ;
                    pps[j] = ps_temp ;
                    len_temp = len[i] ;
                    len[i] = len[j] ;
                    len[j] = len_temp ;
                }
            }
        }
        pps += 3 ;
    }

    /* sort the triangles based on their areas */
    qsort( p_size_pair, numverts-2,
            sizeof( SizePlanePair ), CompareSizePlanePairs ) ;

    /* make the plane sets match the sorted order */
    for ( i = 0, pps = pps_return
        ; i < numverts-2
        ; i++ ) {

        pps_new = p_size_pair[i].pps ;
        for ( j = 0 ; j < 3 ; j++, pps++, pps_new++ ) {
            ps_temp = *pps ;
            *pps = *pps_new ;
            *pps_new = ps_temp ;
        }
    }
    free( p_size_pair ) ;

    return( pps_return ) ;
}


/* check point for inside of three "planes" formed by triangle edges */
int PlaneTest( pPlaneSet p_plane_set, int numverts, double point[2] )
{
    register pPlaneSet ps;
    register int p2;
    register double tx, ty;

    tx = point[0] ;
    ty = point[1] ;


    for ( ps = p_plane_set, p2 = numverts-1 ; --p2 ; ) {

        if ( ps->vx * tx + ps->vy * ty < ps->c ) {
            ps++ ;
            if ( ps->vx * tx + ps->vy * ty < ps->c ) {
                ps++ ;
                /* note: we make the third edge have a slightly different
                 * equality condition, since this third edge is in fact
                 * the next triangle's first edge.  Not fool-proof, but
                 * it doesn't hurt (better would be to keep track of the
                 * triangle's area sign so we would know which kind of
                 * triangle this is).  Note that edge sorting nullifies
                 * this special inequality, too.
                 */
                if ( ps->vx * tx + ps->vy * ty <= ps->c ) {
                    /* point is inside polygon */
                    return( 1 ) ;
                }
                /* check if outside exterior edge */
                else if ( ps->ext_flag ) return( 0 );
                ps++ ;
            } else {
                /* check if outside exterior edge */
                if ( ps->ext_flag ) return( 0 );
                /* get past last two plane tests */
                ps += 2 ;
            }
        } else {
            /* check if outside exterior edge */
            if ( ps->ext_flag ) return( 0 );
            /* get past all three plane tests */
            ps += 3 ;
        }
    }

    /* for convex, if we make it to here, all triangles were missed */
    return( 0 ) ;
}

void PlaneCleanup( pPlaneSet p_plane_set )
{
    free( p_plane_set ) ;
}