#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include "triangulation.h"

/**
 * TODO:
 * 1. add test for colinearity before swap.
 * 2. split triangulation and interpolation code:
 *    use different pixel structs!
 */

typedef struct edge_s edge_t;
typedef struct pixel_s pixel_t;
typedef struct triangle_s triangle_t;

struct edge_s
{
    pixel_t *vert;    /* vertex at the end of the half-edge */
    edge_t *pair;     /* oppositely oriented adjacent half-edge */
    edge_t *next;     /* next half-edge around the face */
    double cost;
};

struct pixel_s
{
    int x;
    int y;
    double value;
    double r, g, b;

    /**
     * Each pixel should know about at least
     * one of the edges out of it.
     */
    edge_t *edge;

    /* Doing this greatly speeds up our intersection routine. */
    triangle_t *tristart;
    triangle_t *triend;
};

struct triangle_s
{
    edge_t *edge;
    triangle_t *next;
};

struct triangulation_s
{
    pixel_t *data;
    int width;
    int height;

    int numtriangles;
    int listgenerated;

    /* The current cost function. */
    int costfunc;

    /* We need a list of unique edges for our algorithm. */
    int numedges;
    edge_t **edgelist;

    /* Since we need alot of edges, allocate them in a pool here. */
    edge_t *edgepool;
};

int is_interior( edge_t *edge ) { return ( edge->pair != 0 ); }
int get_triangulation_width( triangulation_t *image ) { return image->width; }
int get_triangulation_height( triangulation_t *image ) { return image->height; }
edge_t *get_triangle( triangulation_t *image, int i ) { return &(image->edgepool[ i * 3 ]); }
double get_distance( pixel_t *v1, pixel_t *v2 )
{
    double rdist = ( v1->r - v2->r );
    double gdist = ( v1->g - v2->g );
    double bdist = ( v1->b - v2->b );
    double ret = sqrt( (rdist * rdist) + (gdist * gdist) + (bdist * bdist) );
    if( isnan( ret ) ) return 0.0;
    // assert( !isnan( ret ) );
    return ret;
}

void add_triangle( pixel_t *v, edge_t *edge )
{
    triangle_t *tri = (triangle_t *) malloc( sizeof( triangle_t ) );
    tri->edge = edge;
    tri->next = 0;

    if( !v->triend ) {
        v->triend = tri;
        v->tristart = tri;
    } else {
        v->triend->next = tri;
        v->triend = tri;
    }
}

void get_normal( double *nx, double *ny, double *nz,
                 pixel_t *v1, pixel_t *v2, pixel_t *v3 )
{
    double e1x = (double) ( v2->x - v1->x );
    double e1y = (double) ( v2->y - v1->y );
    double e1z = v2->value - v1->value;

    double e2x = (double) ( v3->x - v1->x );
    double e2y = (double) ( v3->y - v1->y );
    double e2z = v3->value - v1->value;

    double nl;

    *nx  = (e1y * e2z) - (e1z * e2y);
    *ny  = (e1z * e2x) - (e1x * e2z);
    *nz  = (e1x * e2y) - (e1y * e2x);
    nl  = sqrt( ((*nx)*(*nx))+((*ny)*(*ny))+((*nz)*(*nz)) );

    *nx /= nl;
    *ny /= nl;
    *nz /= nl;
}

void get_interpolating_poly_vector( double *a, double *b,
                                    pixel_t *v1, pixel_t *v2, pixel_t *v3,
                                    int which )
{
    double e1x = (double) ( v2->x - v1->x );
    double e1y = (double) ( v2->y - v1->y );
    //double e1z = v2->value - v1->value;
    //double e1z = get_distance( v2, v1 );
    //double e1z = ( v2->r - v1->r ) + ( v2->g - v1->g ) + ( v2->b - v1->b );
    double e1z;

    double e2x = (double) ( v3->x - v1->x );
    double e2y = (double) ( v3->y - v1->y );
    //double e2z = v3->value - v1->value;
    //double e2z = get_distance( v3, v1 );
    //double e2z = ( v3->r - v1->r ) + ( v3->g - v1->g ) + ( v3->b - v1->b );
    double e2z;

    double nx;
    double ny;

    if( which == 0 ) {
        e1z = v2->r - v1->r;
        e2z = v3->r - v1->r;
    } else if( which == 1 ) {
        e1z = v2->g - v1->g;
        e2z = v3->g - v1->g;
    } else if( which == 2 ) {
        e1z = v2->b - v1->b;
        e2z = v3->b - v1->b;
    } else {
        e1z = v2->value - v1->value;
        e2z = v3->value - v1->value;
    }

    nx  = (e1y * e2z) - (e1z * e2y);
    ny  = (e1z * e2x) - (e1x * e2z);

    *a = -nx;
    *b = -ny;

    /**
     * Alternatively, we could use the normalized vector and
     * get the results as below, but we don't need them!  Just
     * using nx and ny is sufficient for the scale of the
     * vector.

    double nz;
    double nl;

    nz  = (e1x * e2y) - (e1y * e2x);
    nl  = sqrt( (nx*nx)+(ny*ny)+(nz*nz) );

    nx /= nl;
    ny /= nl;
    nz /= nl;

    *a = -(nx / nz);
    *b = -(ny / nz);

     */
}

double get_edge_cost_abn( edge_t *edge )
{
    pixel_t *v1, *v2, *v3, *v4, *v5, *v6;
    double n1x, n1y, n1z;
    double n2x, n2y, n2z;
    double angle;

    v1 = edge->vert;
    v2 = edge->next->vert;
    v3 = edge->next->next->vert;

    v4 = edge->pair->vert;
    v5 = edge->pair->next->vert;
    v6 = edge->pair->next->next->vert;

    get_normal( &n1x, &n1y, &n1z, v1, v2, v3 );
    get_normal( &n2x, &n2y, &n2z, v4, v5, v6 );

    angle = acos( (n1x * n2x) + (n1y * n2y) + (n1z * n2z) );
    if( isnan( angle ) ) {
        return 0.0;
    }

    return angle;
}

double get_edge_cost_rand( edge_t *edge )
{
    return ( (double) rand() / (double) RAND_MAX );
}

double get_edge_cost_yms( edge_t *edge, int which )
{
    pixel_t *v1, *v2, *v3, *v4, *v5, *v6;
    double a1, b1, a2, b2;

    v1 = edge->vert;
    v2 = edge->next->vert;
    v3 = edge->next->next->vert;

    v4 = edge->pair->vert;
    v5 = edge->pair->next->vert;
    v6 = edge->pair->next->next->vert;

    get_interpolating_poly_vector( &a1, &b1, v1, v2, v3, which );
    get_interpolating_poly_vector( &a2, &b2, v4, v5, v6, which );

    if( isinf( a1 ) || isinf( a2 ) || isnan( a1 ) || isnan( a2 ) ||
        isinf( b1 ) || isinf( b2 ) || isnan( b1 ) || isnan( b2 ) ) {
        assert( 0 );
        return 0.0;
    }

    return ( sqrt( (a1*a1) + (b1*b1) ) * sqrt( (a2*a2) + (b2*b2) ) ) -
           ( (a1*a2) + (b1*b2) );
}

/*
double get_edge_cost_cool2( edge_t *edge )
{
    pixel_t *v1, *v2, *v3, *v4, *v5, *v6;
    double a1, b1, a2, b2;
    double a3, b3, a4, b4;
    double a5, b5, a6, b6;

    v1 = edge->vert;
    v2 = edge->next->vert;
    v3 = edge->next->next->vert;

    v4 = edge->pair->vert;
    v5 = edge->pair->next->vert;
    v6 = edge->pair->next->next->vert;

    get_interpolating_poly_vector( &a1, &b1, v1, v2, v3, 0 );
    get_interpolating_poly_vector( &a2, &b2, v4, v5, v6, 0 );

    get_interpolating_poly_vector( &a3, &b3, v1, v2, v3, 1 );
    get_interpolating_poly_vector( &a4, &b4, v4, v5, v6, 1 );

    get_interpolating_poly_vector( &a5, &b5, v1, v2, v3, 2 );
    get_interpolating_poly_vector( &a6, &b6, v4, v5, v6, 2 );

    return ( sqrt( (a1*a1) + (b1*b1) ) * sqrt( (a2*a2) + (b2*b2) ) *
             sqrt( (a3*a3) + (b3*b3) ) * sqrt( (a4*a4) + (b4*b4) ) *
             sqrt( (a5*a5) + (b5*b5) ) * sqrt( (a6*a6) + (b6*b6) ) * ) -
           ( (a1*a2) + (b1*b2) + (a );

 ) -
           ( (a1*a2) + (b1*b2) );
    //return get_distance( edge->vert, edge->next->vert );
}
*/

double get_edge_cost_cool( edge_t *edge )
{
    pixel_t *v1, *v2, *v3, *v4, *v5, *v6;
    double a1, b1, a2, b2;
    double dist;
    double size1, size2, dot, angle;

    v1 = edge->vert;
    v2 = edge->next->vert;
    v3 = edge->next->next->vert;

    v4 = edge->pair->vert;
    v5 = edge->pair->next->vert;
    v6 = edge->pair->next->next->vert;

    get_interpolating_poly_vector( &a1, &b1, v1, v2, v3, 3 );
    get_interpolating_poly_vector( &a2, &b2, v4, v5, v6, 3 );

    if( isinf( a1 ) || isinf( a2 ) || isnan( a1 ) || isnan( a2 ) ||
        isinf( b1 ) || isinf( b2 ) || isnan( b1 ) || isnan( b2 ) ) {
        assert( 0 );
        return 0.0;
    }

    size1 = sqrt( (a1*a1) + (b1*b1) );
    size2 = sqrt( (a2*a2) + (b2*b2) );
    dot = ( (a1*a2) + (b1*b2) );

    if( size1 == 0.0 || size2 == 0.0 ) {
        angle = 1.0;
    } else {
        angle = 1.0 - ( dot / ( size1 * size2 ) );
    }
    dist  = get_distance( edge->vert, edge->next->vert );
/*
    //dist += get_distance( edge->next->vert, edge->next->next->vert ) / 2.0;
    //dist += get_distance( edge->vert, edge->next->next->vert ) / 2.0;
    dist = fabs( 1.0 - ( edge->vert->value / edge->next->vert->value ) );

    dist = result * ( 1.0 + dist );

    return dist;
*/
    return dist * angle;
}

double get_gradient_angle( edge_t *edge )
{
    pixel_t *v1, *v2, *v3, *v4, *v5, *v6;
    double a1, b1, a2, b2;
    double result;

    v1 = edge->vert;
    v2 = edge->next->vert;
    v3 = edge->next->next->vert;

    v4 = edge->pair->vert;
    v5 = edge->pair->next->vert;
    v6 = edge->pair->next->next->vert;

    get_interpolating_poly_vector( &a1, &b1, v1, v2, v3, 3 );
    get_interpolating_poly_vector( &a2, &b2, v4, v5, v6, 3 );

    if( isinf( a1 ) || isinf( a2 ) || isnan( a1 ) || isnan( a2 ) ||
        isinf( b1 ) || isinf( b2 ) || isnan( b1 ) || isnan( b2 ) ) {
        assert( 0 );
        return 0.0;
    }

    result = 1.0 - ( ( (a1*a2) + (b1*b2) ) /
                   ( sqrt( (a1*a1) + (b1*b1) ) * sqrt( (a2*a2) + (b2*b2) ) ) );
    if( isinf( result ) || isnan( result ) ) return 1.0;
    return result;
}

double get_edge_cost_distance( edge_t *edge )
{
    return get_distance( edge->vert, edge->next->vert );
}

double get_edge_cost_test( edge_t *edge )
{
    double distance = get_edge_cost_distance( edge );
    //double yms = get_edge_cost_yms( edge, 3 );
    double angle = get_gradient_angle( edge );

    return distance * angle;
}

double get_edge_cost_dude( edge_t *edge )
{
    pixel_t *v1, *v2, *v3, *v4, *v5, *v6;
    double a1, b1, a2, b2;
    double result;

    v1 = edge->vert;
    v2 = edge->next->vert;
    v3 = edge->next->next->vert;

    v4 = edge->pair->vert;
    v5 = edge->pair->next->vert;
    v6 = edge->pair->next->next->vert;

    get_interpolating_poly_vector( &a1, &b1, v1, v2, v3, 3 );
    get_interpolating_poly_vector( &a2, &b2, v4, v5, v6, 3 );

    if( isinf( a1 ) || isinf( a2 ) || isnan( a1 ) || isnan( a2 ) ||
        isinf( b1 ) || isinf( b2 ) || isnan( b1 ) || isnan( b2 ) ) {
        assert( 0 );
        return 0.0;
    }

    result =( (a1*a2) + (b1*b2) ) /
            ( sqrt( ( (a1*a1) + (b1*b1) + 1.0 ) * ( (a2*a2) + (b2*b2) ) ) );
    if( isinf( result ) || isnan( result ) ) return 1.0;
    return result;
}

double get_edge_cost( triangulation_t *image, edge_t *edge )
{
    if( !edge ) return 0.0;

    if( !is_interior( edge ) ) return 0.0;

    if( edge->cost < 0.0 ) {
        edge->cost = 0.0;

        if( image->costfunc == COST_FUNC_DISTANCE ) {
            edge->cost = get_edge_cost_distance( edge );
        } else if( image->costfunc == COST_FUNC_YMS ) {
            edge->cost = get_edge_cost_yms( edge, 3 );
        } else if( image->costfunc == COST_FUNC_ABN ) {
            edge->cost = get_edge_cost_abn( edge );
        } else if( image->costfunc == COST_FUNC_RAND ) {
            edge->cost = get_edge_cost_rand( edge );
        } else if( image->costfunc == COST_FUNC_TEST ) {
            edge->cost = get_edge_cost_test( edge );
        } else if( image->costfunc == COST_FUNC_DUDE ) {
            edge->cost = get_edge_cost_dude( edge );
        }

        /* XXX: Is this correct? */
        if( edge->cost <= 0.0 ) edge->cost = 0.0;

        if( edge->pair ) edge->pair->cost = edge->cost;
    }

    return edge->cost;
}

void invalidate_cost( edge_t *edge )
{
    if( !edge ) { fprintf( stderr, "fucked.\n" ); return; }

    edge->cost = -1.0;
    if( edge->pair ) edge->pair->cost = -1.0;
}

int orientation( pixel_t *p, pixel_t *q, pixel_t *r )
{
    return ((q->y - p->y)*(r->x - p->x) - (q->x - p->x)*(r->y - p->y));
}

double orientation_xy( pixel_t *p, pixel_t *q, double x, double y )
{
    return (((double) (q->y - p->y))*(x - ((double) p->x)))
           - (((double) (q->x - p->x))*(y - ((double) p->y)));
}

int is_colinear( edge_t *edge )
{
    pixel_t *v[ 4 ];
    int i;

    v[ 0 ] = edge->vert;
    v[ 1 ] = edge->pair->next->next->vert;
    v[ 2 ] = edge->next->vert;
    v[ 3 ] = edge->next->next->vert;

    for( i = 0; i < 4; i++ ) {
        pixel_t *pt[ 3 ];

        pt[ 0 ] = v[ i ];
        pt[ 1 ] = v[ (i + 1) % 4 ];
        pt[ 2 ] = v[ (i + 2) % 4 ];

        if( orientation( pt[ 0 ], pt[ 1 ], pt[ 2 ] ) == 0 ) return 1;
    }

    return 0;
}


int is_convex( edge_t *edge )
{
    pixel_t *v[ 4 ];
    int i, sign;

    v[ 0 ] = edge->vert;
    v[ 1 ] = edge->pair->next->next->vert;
    v[ 2 ] = edge->next->vert;
    v[ 3 ] = edge->next->next->vert;

    sign = orientation( v[ 0 ], v[ 1 ], v[ 2 ] );

    for( i = 1; i < 4; i++ ) {
        int s;

        s = orientation( v[ i ], v[ (i + 1) % 4 ], v[ (i + 2) % 4 ] );
        if( ( sign < 0 ) && !( s < 0 ) ) return 0;
        if( ( sign > 0 ) && !( s > 0 ) ) return 0;
    }

    return 1;
}

void do_swap( edge_t *edge )
{
    edge_t *a, *b, *c, *d, *e, *f;
    pixel_t *v1, *v2, *v3, *v4;

    /**
     * We need to reconstruct the two adjacent triangles.
     *        v2
     *         >
     *      /a   b\              /a   b\
     *   v1  <-c-   v3    -->      c |
     *        -d->              ^  | d  v
     *      \e   f/              \e   f/
     *         <
     *        v4
     * Changes:
     *   c->vert = v2
     *   d->vert = v4
     *   c->next = e
     *   d->next = b
     *   a->next = c
     *   f->next = d
     */
    c = edge;
    d = c->pair;

    a = c->next;
    b = a->next;

    f = d->next;
    e = f->next;

    v1 = d->vert;
    v2 = b->vert;
    v3 = c->vert;
    v4 = e->vert;

    assert( c->vert != d->vert );
    assert( e->next == d );
    assert( b->next == c );
    assert( c->vert == f->vert );
    assert( d->vert == a->vert );

    c->vert = v2;
    d->vert = v4;

    a->next = c;
    c->next = e;
    e->next = a;

    d->next = b;
    b->next = f;
    f->next = d;

    v1->edge = a;
    v2->edge = b;
    v3->edge = f;
    v4->edge = e;

    invalidate_cost( a );
    invalidate_cost( b );
    invalidate_cost( c );
    invalidate_cost( d );
    invalidate_cost( e );
    invalidate_cost( f );
}

void swap_edge( edge_t *edge )
{
    if( !is_interior( edge ) ) return;
    assert( is_interior( edge ) );

    if( !is_convex( edge ) ) return;
    assert( is_convex( edge ) );

    assert( !is_colinear( edge ) );

    /* FIXME: We can predict if the result will be colinear after the swap! */

    do_swap( edge );
    if( is_colinear( edge ) ) do_swap( edge );

    assert( !is_colinear( edge ) );
    assert( is_convex( edge ) );
}

double get_image_cost( triangulation_t *image )
{
    double cost = 0.0;
    int i;

    for( i = 0; i < image->numedges; i++ ) {
        cost += get_edge_cost( image, image->edgelist[ i ] );
    }

    return cost;
}

void apply_alg( triangulation_t *image, edge_t *edge )
{
    edge_t *e1, *e2, *e3, *e4;
    double cur_cost, swap_cost;

    /* if it's not an exterior edge, do nothing. */
    if( !is_interior( edge ) ) return;

    /* if the quad is not convex, do nothing. */
    if( !is_convex( edge ) ) return;

    e1 = edge->next;
    e2 = edge->next->next;
    e3 = edge->pair->next;
    e4 = edge->pair->next->next;

    /* figure out the cost of this edge. */
    cur_cost = get_edge_cost( image, e1 ) +
               get_edge_cost( image, e2 ) +
               get_edge_cost( image, e3 ) +
               get_edge_cost( image, e4 ) +
               get_edge_cost( image, edge );

    /* swap, and check the new cost. */
    swap_edge( edge );
    swap_cost = get_edge_cost( image, e1 ) +
                get_edge_cost( image, e2 ) +
                get_edge_cost( image, e3 ) +
                get_edge_cost( image, e4 ) +
                get_edge_cost( image, edge );

    if( swap_cost >= cur_cost ) {
        /* our swap sucked. try the look-aheads */
     
        /**   
         *        v2
         *         >
         *      /a   b\              /a   b\
         *   v1  <-c-   v3    -->      c |
         *        -d->              ^  | d  v
         *      \e   f/              \e   f/
         *         <
         *        v4
         */
        edge_t *e1 = edge->next;
        edge_t *e2 = edge->next->next;
        edge_t *e3 = edge->pair->next;
        edge_t *e4 = edge->pair->next->next;

        edge_t *e5 = e1->pair ? e1->pair->next : 0;
        edge_t *e6 = e1->pair ? e1->pair->next->next: 0;

        edge_t *e7 = e2->pair ? e2->pair->next : 0;
        edge_t *e8 = e2->pair ? e2->pair->next->next : 0;

        edge_t *e9  = e3->pair ? e3->pair->next : 0;
        edge_t *e10 = e3->pair ? e3->pair->next->next : 0;

        edge_t *e11 = e4->pair ? e4->pair->next : 0;
        edge_t *e12 = e4->pair ? e4->pair->next->next : 0;

        swap_edge( edge );
        cur_cost = get_edge_cost( image, e1 ) +
                   get_edge_cost( image, e2 ) +
                   get_edge_cost( image, e3 ) +
                   get_edge_cost( image, e4 ) +
                   get_edge_cost( image, e5 ) +
                   get_edge_cost( image, e6 ) +
                   get_edge_cost( image, e7 ) +
                   get_edge_cost( image, e8 ) +
                   get_edge_cost( image, e9 ) +
                   get_edge_cost( image, e10 ) +
                   get_edge_cost( image, e11 ) +
                   get_edge_cost( image, e12 ) +
                   get_edge_cost( image, edge );
        swap_edge( edge );

        /* op 1 */
        swap_edge( e1 );
        swap_cost = get_edge_cost( image, e1 ) +
                    get_edge_cost( image, e2 ) +
                    get_edge_cost( image, e3 ) +
                    get_edge_cost( image, e4 ) +
                    get_edge_cost( image, e5 ) +
                    get_edge_cost( image, e6 ) +
                    get_edge_cost( image, e7 ) +
                    get_edge_cost( image, e8 ) +
                    get_edge_cost( image, e9 ) +
                    get_edge_cost( image, e10 ) +
                    get_edge_cost( image, e11 ) +
                    get_edge_cost( image, e12 ) +
                    get_edge_cost( image, edge );
        if( swap_cost < cur_cost ) return;
        swap_edge( e1 );

        /* op 2 */
        swap_edge( e2 );
        swap_cost = get_edge_cost( image, e1 ) +
                    get_edge_cost( image, e2 ) +
                    get_edge_cost( image, e3 ) +
                    get_edge_cost( image, e4 ) +
                    get_edge_cost( image, e5 ) +
                    get_edge_cost( image, e6 ) +
                    get_edge_cost( image, e7 ) +
                    get_edge_cost( image, e8 ) +
                    get_edge_cost( image, e9 ) +
                    get_edge_cost( image, e10 ) +
                    get_edge_cost( image, e11 ) +
                    get_edge_cost( image, e12 ) +
                    get_edge_cost( image, edge );
        if( swap_cost < cur_cost ) return;
        swap_edge( e2 );

        /* op 3 */
        swap_edge( e3 );
        swap_cost = get_edge_cost( image, e1 ) +
                    get_edge_cost( image, e2 ) +
                    get_edge_cost( image, e3 ) +
                    get_edge_cost( image, e4 ) +
                    get_edge_cost( image, e5 ) +
                    get_edge_cost( image, e6 ) +
                    get_edge_cost( image, e7 ) +
                    get_edge_cost( image, e8 ) +
                    get_edge_cost( image, e9 ) +
                    get_edge_cost( image, e10 ) +
                    get_edge_cost( image, e11 ) +
                    get_edge_cost( image, e12 ) +
                    get_edge_cost( image, edge );
        if( swap_cost < cur_cost ) return;
        swap_edge( e3 );

        /* op 4 */
        swap_edge( e4 );
        swap_cost = get_edge_cost( image, e1 ) +
                    get_edge_cost( image, e2 ) +
                    get_edge_cost( image, e3 ) +
                    get_edge_cost( image, e4 ) +
                    get_edge_cost( image, e5 ) +
                    get_edge_cost( image, e6 ) +
                    get_edge_cost( image, e7 ) +
                    get_edge_cost( image, e8 ) +
                    get_edge_cost( image, e9 ) +
                    get_edge_cost( image, e10 ) +
                    get_edge_cost( image, e11 ) +
                    get_edge_cost( image, e12 ) +
                    get_edge_cost( image, edge );
        if( swap_cost < cur_cost ) return;
        swap_edge( e4 );

        /* they all sucked, go back. */
        swap_edge( edge );
        return;
    }

    /* swap rules!  we're done! */
}

edge_t *get_edge( pixel_t *v1, pixel_t *v2 )
{
     edge_t *edge = v1->edge;

     if( !edge ) return 0;

     do {
          if( edge->next->vert == v2 ) return edge;
          if( !edge->pair ) return 0;
          edge = edge->pair->next;
     } while( edge != v1->edge );

     return 0;
}

void initialize_triangulation( triangulation_t *image )
{
    edge_t **edgelist = image->edgelist;
    edge_t *edgepool = image->edgepool;
    int i, j;

    for( i = 0; i < (image->height - 1); i++ ) {
        for( j = 0; j < (image->width - 1); j++ ) {
            /**
             * Create two triangles interconnected with each other
             * on the diagonal.  Make edgelist entries for the first
             * triangle only.
             *
             *  v1   v2 -a  v3
             *        b -c
             *      g     d-e 
             *  v4   v5  f- v6
             */
            pixel_t *v2 = &(image->data[ (i * image->width) + j ]);
            pixel_t *v3 = &(image->data[ (i * image->width) + j + 1 ]);
            pixel_t *v5 = &(image->data[ ((i + 1) * image->width) + j ]);
            pixel_t *v6 = &(image->data[ ((i + 1) * image->width) + j + 1 ]);
            edge_t *a, *b, *c, *d, *e, *f, *g, *h;

            /**
             * These are allocated in a specific order.
             * The key edges are the first ones of each group-of-3.
             */
            c = edgepool++;
            a = edgepool++;
            b = edgepool++;

            d = edgepool++;
            e = edgepool++;
            f = edgepool++;

            /* Remember the created edges and triangles in the main struct */
            *edgelist++ = a;
            *edgelist++ = b;
            *edgelist++ = c;

            assert( a && b && c && d && e && f );

            a->cost = -1.0;
            b->cost = -1.0;
            c->cost = -1.0;
            d->cost = -1.0;
            e->cost = -1.0;
            f->cost = -1.0;

            g = get_edge( v2, v5 );
            h = get_edge( v3, v2 );

            /**
             *           h
             *  v1   v2 -a  v3
             *        b -c
             *      g     d-e 
             *  v4   v5  f- v6
             */

            a->vert = v2;
            a->next = c;
            a->pair = h;
            v2->edge = a;
            if( h ) h->pair = a;

            c->vert = v3;
            c->next = b;
            c->pair = d;
            v3->edge = c;

            b->vert = v5;
            b->next = a;
            b->pair = g;
            v5->edge = b;
            if( g ) g->pair = b;

            d->vert = v5;
            d->next = e;
            d->pair = c;
            v5->edge = d;

            e->vert = v3;
            e->next = f;
            e->pair = 0;
            v3->edge = e;

            f->vert = v6;
            f->next = d;
            f->pair = 0;
            v6->edge = f;
        }
    }
}

triangulation_t *initialize_image( double *data, double *pixeldata,
                                   int width, int height )
{
    triangulation_t *image = (triangulation_t *) malloc( sizeof( triangulation_t ) );
    pixel_t *cur;
    int i, j;

    if( !image ) {
        fprintf( stderr, "tri: can't allocate image structure.\n" );
        return 0;
    }

    image->width = width;
    image->height = height;
    image->listgenerated = 0;

    image->data = (pixel_t *) malloc( width * height * sizeof( pixel_t ) );
    if( !image->data ) {
        fprintf( stderr, "tri: can't allocate pixel data.\n" );
        free( image );
        return 0;
    }

    cur = image->data;
    for( i = 0; i < height; i++ ) {
        for( j = 0; j < width; j++ ) {
            cur->x = j;
            cur->y = i;
            cur->value = *data;
            cur->edge = 0;
            if( pixeldata ) {
                cur->r = *pixeldata++;
                cur->g = *pixeldata++;
                cur->b = *pixeldata++;
            }
            cur->tristart = 0;
            cur->triend = 0;
            cur++;
            data++;
        }
    }

    image->numedges = ( width - 1 ) * ( height - 1 ) * 3;
    image->edgelist = (edge_t **) malloc( image->numedges * sizeof( edge_t * ) );
    if( !image->edgelist ) {
        fprintf( stderr, "tri: can't allocate edgelist.\n" );
        free( image->data );
        free( image );
        return 0;
    }
    image->numtriangles = ( width - 1 ) * ( height - 1 ) * 2;

    image->edgepool = (edge_t *) malloc( image->numedges * 2 * sizeof( edge_t ) );
    if( !image->edgepool ) {
        fprintf( stderr, "tri: can't allocate edgepool.\n" );
        free( image->edgelist );
        free( image->data );
        free( image );
        return 0;
    }

    memset( image->edgelist, 0, sizeof( image->edgelist ) );
    initialize_triangulation( image );

    return image;
}

int point_between( pixel_t *v1, pixel_t *v2, double x, double y )
{
    double x1 = (double) v1->x;
    double y1 = (double) v1->y;
    double x2 = (double) v2->x;
    double y2 = (double) v2->y;

    if( x <= x2 && x >= x1 && y <= y2 && y >= y1 ) return 1;
    if( x <= x1 && x >= x2 && y <= y1 && y >= y2 ) return 1;
    if( x <= x1 && x >= x2 && y <= y2 && y >= y1 ) return 1;
    if( x <= x2 && x >= x1 && y <= y1 && y >= y2 ) return 1;
    return 0;
}

int triangle_intersect( edge_t *edge, double x, double y )
{
    pixel_t *v[ 3 ];
    double sign;
    int i;

    v[ 0 ] = edge->vert;
    v[ 1 ] = edge->next->vert;
    v[ 2 ] = edge->next->next->vert;

    if( v[ 0 ]->x < x && v[ 1 ]->x < x && v[ 2 ]->x < x ) return 0;
    if( v[ 0 ]->y < y && v[ 1 ]->y < y && v[ 2 ]->y < y ) return 0;
    if( v[ 0 ]->x > x && v[ 1 ]->x > x && v[ 2 ]->x > x ) return 0;
    if( v[ 0 ]->y > y && v[ 1 ]->y > y && v[ 2 ]->y > y ) return 0;

    sign = orientation_xy( v[ 0 ], v[ 1 ], x, y );
    if( sign == 0.0 ) return point_between( v[ 0 ], v[ 1 ], x, y );

    for( i = 1; i < 3; i++ ) {
        double s;

        s = orientation_xy( v[ i ], v[ (i + 1) % 3 ], x, y );
        if( s == 0.0 ) return point_between( v[ i ], v[ (i + 1) % 3 ], x, y );
        if( ( sign < 0.0 ) && !( s < 0.0 ) ) return 0;
        if( ( sign > 0.0 ) && !( s > 0.0 ) ) return 0;
    }

    return 1;
}

edge_t *get_nearest_to_vertex( pixel_t *pixel, double x, double y )
{
    edge_t *cur = pixel->edge;

    do {
        if( triangle_intersect( cur, x, y ) ) return cur;

        if( !cur->pair ) {
            cur = 0;
        } else {
            cur = cur->pair->next;
        }
    } while( cur && cur != pixel->edge );

    return 0;
}

edge_t *get_nearest_to_vertex_list( pixel_t *pixel, double x, double y )
{
    triangle_t *tri = pixel->tristart;

    while( tri ) {
        if( triangle_intersect( tri->edge, x, y ) ) return tri->edge;
        tri = tri->next;
    }

    return 0;
}

void generate_pixel_triangle_list( triangulation_t *image )
{
    int i;

    for( i = 0; i < image->numtriangles; i++ ) {
        edge_t *curedge = get_triangle( image, i );
        int minx = curedge->vert->x;
        int miny = curedge->vert->y;
        int maxx = minx;
        int maxy = miny;
        int x, y;

        if( curedge->next->vert->x < minx ) minx = curedge->next->vert->x;
        if( curedge->next->vert->x > maxx ) maxx = curedge->next->vert->x;
        if( curedge->next->vert->y < miny ) miny = curedge->next->vert->y;
        if( curedge->next->vert->y > maxy ) maxy = curedge->next->vert->y;

        if( curedge->next->next->vert->x < minx ) minx = curedge->next->next->vert->x;
        if( curedge->next->next->vert->x > maxx ) maxx = curedge->next->next->vert->x;
        if( curedge->next->next->vert->y < miny ) miny = curedge->next->next->vert->y;
        if( curedge->next->next->vert->y > maxy ) maxy = curedge->next->next->vert->y;

        for( y = miny; y <= maxy; y++ ) {
            for( x = minx; x <= maxx; x++ ) {
                add_triangle( &(image->data[ (y * image->width) + x ]),
                              curedge );
            }
        }
    }
    image->listgenerated = 1;
}

edge_t *get_nearest_triangle( triangulation_t *image, double x, double y )
{
    int nearx = (int) floor( x );
    int neary = (int) floor( y );
    edge_t *cur;

    if( !image->listgenerated ) generate_pixel_triangle_list( image );

    /* XXX: FIX THIS! */
    if( nearx >= image->width - 1 ) return 0;
    if( neary >= image->height - 1 ) return 0;
    if( nearx < 0.0 ) return 0;
    if( neary < 0.0 ) return 0;

    cur = get_nearest_to_vertex_list( &(image->data[ (neary * image->width) + nearx ]), x, y );
    if( cur ) return cur;
    if( nearx < (image->width - 1) ) {
        cur = get_nearest_to_vertex_list( &(image->data[ (neary * image->width) + nearx + 1 ]), x, y );
        if( cur ) return cur;
    }
    if( neary < (image->height - 1) ) {
        cur = get_nearest_to_vertex_list( &(image->data[ ((neary+1) * image->width) + nearx ]), x, y );
        if( cur ) return cur;
    }
    if( (nearx < (image->width - 1)) && (neary < (image->height - 1)) ) {
        cur = get_nearest_to_vertex_list( &(image->data[ ((neary+1) * image->width) + nearx + 1 ]), x, y );
        if( cur ) return cur;
    }

    assert( 0 );

    return 0;
}

double interpolate_in_triangle( edge_t *edge, double x0, double y0,
                                double *r, double *g, double *b )
{
    double x1, y1, x2, y2, x3, y3;
    double b0, b1, b2, b3;
    double result;

    x1 = (double) edge->vert->x;
    y1 = (double) edge->vert->y;
    x2 = (double) edge->next->vert->x;
    y2 = (double) edge->next->vert->y;
    x3 = (double) edge->next->next->vert->x;
    y3 = (double) edge->next->next->vert->y;

    b0 =  (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
    b1 = ((x2 - x0) * (y3 - y0) - (x3 - x0) * (y2 - y0)) / b0;
    b2 = ((x3 - x0) * (y1 - y0) - (x1 - x0) * (y3 - y0)) / b0;
    b3 = ((x1 - x0) * (y2 - y0) - (x2 - x0) * (y1 - y0)) / b0;

/*
    if( b1 >= b2 && b1 >= b3 ) { b1 = 1.0; b2 = 0.0; b3 = 0.0; }
    else if( b2 >= b1 && b2 >= b3 ) { b1 = 0.0; b2 = 1.0; b3 = 0.0; }
    else { b1 = 0.0; b2 = 0.0; b3 = 1.0; }
*/

    result  = edge->vert->value * b1;
    result += edge->next->vert->value * b2;
    result += edge->next->next->vert->value * b3;

    if( r ) {
        *r = edge->vert->r * b1;
        *r = *r + edge->next->vert->r * b2;
        *r = *r + edge->next->next->vert->r * b3;

        *g = edge->vert->g * b1;
        *g = *g + edge->next->vert->g * b2;
        *g = *g + edge->next->next->vert->g * b3;

        *b = edge->vert->b * b1;
        *b = *b + edge->next->vert->b * b2;
        *b = *b + edge->next->next->vert->b * b3;
    }

    return result;
}

double interpolate_value( triangulation_t *image, double x, double y,
                          double *r, double *g, double *b )
{
    edge_t *edge;

    edge = get_nearest_triangle( image, x, y );
    if( !edge ) {
        //fprintf( stderr, "interpolator: can't find triangle for point %f,%f\n",
        //         x, y );
        return 0.0;
    }

    return interpolate_in_triangle( edge, x, y, r, g, b );
}

void print_edges( triangulation_t *image )
{
    int i;

    for( i = 0; i < image->numedges; i++ ) {
        if( image->edgelist[ i ] ) {
            fprintf( stdout, "%d -%d %d -%d\n\n",
                     image->edgelist[ i ]->vert->x,
                     image->edgelist[ i ]->vert->y,
                     image->edgelist[ i ]->next->vert->x,
                     image->edgelist[ i ]->next->vert->y );
            /*
            fprintf( stdout, "%2.2f %2.2f %2.2f %2.2f\n",
                     image->edgelist[ i ]->next->vert->x,
                     image->edgelist[ i ]->next->vert->y,
                     image->edgelist[ i ]->vert->x,
                     image->edgelist[ i ]->vert->y );
            */
        } else {
            fprintf( stdout, "null edge!\n" );
        }
    }
}

double get_cost( triangulation_t *image, int costfunc )
{
    int j;

    for( j = 0; j < image->numedges; j++ ) invalidate_cost( image->edgelist[ j ] );
    image->costfunc = costfunc;
    return get_image_cost( image );
}

void run_alg( triangulation_t *image, int costfunc, int numpasses )
{
    int i, j;

    image->costfunc = costfunc;

    fprintf( stderr, "tri: Running for %d passes on %d edges.\n",
             numpasses, image->numedges );
    for( i = 0; i < numpasses; i++ ) {
        fprintf( stderr, "\rtri: Pass %d: Cost %f\n",
                 i, get_image_cost( image ) );
        for( j = 0; j < image->numedges; j++ ) {
            assert( image->edgelist[ j ] );
            apply_alg( image, image->edgelist[ j ] );
            if( j % 10000 == 0 ) {
                fprintf( stderr, "\rtri: Checking edge: %d", j );
            }
        }
    }
    fprintf( stderr, "\rtri: Pass %d: Cost %f\n", i, get_image_cost( image ) );

    for( j = 0; j < image->numedges; j++ ) invalidate_cost( image->edgelist[ j ] );
}

void save_triangulation( triangulation_t *image, const char *filename )
{
    int i, fd;

    fd = open( filename, O_WRONLY|O_CREAT, S_IREAD|S_IWRITE|S_IRGRP|S_IROTH );
    if( fd < 0 ) {
        fprintf( stderr, "triangulation: can't open file '%s'\n", filename );
        return;
    }

    write( fd, &image->numedges, sizeof( int ) );

    for( i = 0; i < image->numedges * 2; i++ ) {
        int vertex = image->edgepool[ i ].vert - image->data;
        int pair = image->edgepool[ i ].pair - image->edgepool;
        int next = image->edgepool[ i ].next - image->edgepool;
        int pvertex = 0;
        int pnext = 0;

        if( image->edgepool[ i ].pair ) {
            pvertex = image->edgepool[ i ].pair->vert - image->data;
            pnext = image->edgepool[ i ].pair->next - image->edgepool;
        } else {
            pair = -1;
        }

        write( fd, &vertex, sizeof( int ) );
        write( fd, &pair, sizeof( int ) );
        write( fd, &next, sizeof( int ) );
        write( fd, &pvertex, sizeof( int ) );
        write( fd, &pnext, sizeof( int ) );
    }

    close( fd );
}

void load_triangulation( triangulation_t *image, const char *filename )
{
    int numedges;
    int i, fd;

    fd = open( filename, O_RDONLY );
    if( fd < 0 ) {
        fprintf( stderr, "triangulation: can't open file '%s'\n", filename );
        return;
    }

    read( fd, &numedges, sizeof( int ) );
    if( numedges != image->numedges ) {
        fprintf( stderr, "triangulation: loaded file '%s' not compatible with image!\n", filename );
        close( fd );
        return;
    }

    for( i = 0; i < numedges * 2; i++ ) {
        int vertex, pair, next, pvertex, pnext;

        read( fd, &vertex, sizeof( int ) );
        read( fd, &pair, sizeof( int ) );
        read( fd, &next, sizeof( int ) );
        read( fd, &pvertex, sizeof( int ) );
        read( fd, &pnext, sizeof( int ) );

        image->edgepool[ i ].vert = image->data + vertex;
        image->edgepool[ i ].next = image->edgepool + next;
        image->edgepool[ i ].cost = -1;

        if( pair < 0 ) {
            image->edgepool[ i ].pair = 0;
        } else {
            image->edgepool[ i ].pair = image->edgepool + pair;
            image->edgepool[ i ].pair->vert = image->data + pvertex;
            image->edgepool[ i ].pair->next = image->edgepool + pnext;
            image->edgepool[ i ].pair->pair = &(image->edgepool[ i ]);
            image->edgepool[ i ].pair->cost = -1;
        }
    }

    close( fd );
}