/*****************************************************************************
 *
 * ccalib.c
 *
 * Performs camera calibration.
 *
 *
 *  by Corey Tripp <crt17@columbia.edu>
 *     Naveed Hasan <naveed@cs.columbia.edu>
 *     Paul Blaer <psb15@columbia.edu>
 *     Lenny Volchok <lv71@columbia.edu>
 *
 *     Team Eclipse
 *
 *
 * The super cool function you might want to call:
 *       void calib(char *imagefile, int len, char *cdata, char *cparam)
 *
 *      Where:
 *          *imagefile  - image file name
 *          len         - length of squares
 *          *cdata      - data file name
 *          *cparam     - params file name
 *
 *
 * Date: 2000/05/05
 *
 ****************************************************************************/

#include<stdio.h>
#include<stdlib.h>
#include<math.h>        /* sqrt, cos, sin functions */

#include "pgm_helper.h"
#include "disjset.h"

/* tsai code structs and functions */
#include <cal_main.h>

/* tsai code functions for saving data */
extern void dump_cd_data();
extern void dump_cp_cc_data();

/* pgm_helper line drawing */
extern int line(struct image *im, int x0, int y0, int x1, int y1, int color);

/* useful definitions */
#define PI2 6.28318530717958647692528676655901
#define THETA_RES 1000
#define MABS(X)  ( (X) < 0 ? -(X) : (X) )

/* Structures */
typedef struct {
    int x;
    int y;
} Point;

typedef struct {
    int xa;
    int ya;
    int xb;
    int yb;
} Line_End_Points;

/* lookup tables for hough */
double theta_lookup[THETA_RES];
double cos_lookup[THETA_RES];
double sin_lookup[THETA_RES];

/* initialize lookup tables for hough transform */
void init() {
    int i;
    double theta_inc = PI2 / (double)THETA_RES;

    for( i = 0; i < THETA_RES; i++ ) {
        theta_lookup[i] = i * theta_inc;
        cos_lookup[i] = cos( theta_lookup[i] );
        sin_lookup[i] = sin( theta_lookup[i] );
    }
}

/**
 * Finds the edges of a given image.
 *
 * img - image struct
 * *value - holds the grayscale value used for thresholding the edge image
 *
 * returns the edge image of the given image
 */
struct image edge( struct image img, int* value ) {
    struct image out;
    int histogram[256];
    int i, j, k, l, w, h;
    int sum, val, index;
    int lmin_val, lmin_i, lmax_val, lmax_i;
    int min_i, min_val, max_val, max_i;

    out.rows = h = img.rows;
    out.columns = w = img.columns;
    out.colors = img.colors;

    out.data = (unsigned char*) malloc( sizeof( unsigned char )
            * out.rows * out.columns );

    for(i = 0; i < 256; i++) {
        histogram[i] = 0;
    }

    for(i = 0; i < h; i++) {
        for( j = 0; j < w; j++) {
            index = i * w + j;
            if( i != 0 && j != 0 && (i + 1) < h && (j + 1) < w) {
                sum = 0;
                for(k = i - 1; k <= i + 1; k++) {
                    for(l = j - 1; l <= j + 1; l++) {
                        val = img.data[k*w+l] - img.data[index];
                        sum += ( val < 0 ? -val : val );
                    }
                }
                out.data[index] = (unsigned char)( sum / 8.0 );
                histogram[ out.data[index] ]++;
            } else {
                out.data[ index ] = 0;
                histogram[ out.data[index] ]++;
            }
        }
    }

    /* find the value from the histogram */
    lmin_val = 10000;
    lmin_i = 0;
    lmax_val = 0;
    lmax_i = 0;
    min_val = 1000;
    min_i = 0;
    max_val = 0;
    max_i = 0;
    j = 0;
    k = 0;
    i = 255;
    while( histogram[i--] == 0 );
    for(; i >= 0; i--) {
        if( max_val < histogram[i] ) {
            max_val = histogram[i];
            max_i = i;
        }
        if( min_val > histogram[i] ) {
            min_val = histogram[i];
            min_i = i;
        }
    }
    lmin_val = max_val;
    lmin_i = max_i;
    for(i = max_i; i < min_i; i++) {
        if( lmin_val > histogram[i] && (i - lmin_i) <= 10) {
            lmin_val = histogram[i];
            lmin_i = i;
        }
    }
    lmax_val = lmin_val;
    lmax_i = lmin_i;
    for(i = lmin_i; i < min_i; i++) {
        if( lmax_val < histogram[i] && ( i - lmax_i ) <= 10) {
            lmax_val = histogram[i];
            lmax_i = i;
        }
    }

    (*value) = lmax_i;

    return out;
}

/**
 * Computes the hough transform of a given image.
 *
 * img       - the edge image
 * threshold - the threshold value for edges
 *
 * returns an image of the hough transform
 */
struct image hough( struct image img, int threshold ) {
    unsigned int* hough;
    struct image out;
    int i, w, h, t, x, y, wh_total, total;
    int rho_max, max, rho, index;

    w = img.columns;
    h = img.rows;
    wh_total = w * h;

    rho_max = (int) sqrt( w * w + h * h );

    total = rho_max * THETA_RES;

    hough = (unsigned int*) malloc(sizeof(unsigned int) * total );

    for( i = 0; i < total; i++ ) {
        hough[i] = 0;
    }

    threshold -= 5;

    max = 0;
    for( i = 0; i < wh_total; i++ ) {
        if( img.data[i] >= threshold ) {
            y = i / w;
            x = i - ( y * w );

            for(t = 0; t < THETA_RES; t++) {
                rho = (int)( y * cos_lookup[t] - x * sin_lookup[t] );
                if( rho >= 0 && rho < rho_max ) {
                    index = rho * THETA_RES + t;
                    hough[index]++;
                    max = ( max < hough[index] ? hough[index] : max);
                }
            }
        }
    }

    out.rows = rho_max;
    out.columns = THETA_RES;
    out.colors = 255;
    out.data = (unsigned char*) malloc( sizeof(unsigned char)*total );

    max /= 3;

    for( i = 0; i < total; i++ ) {
        if( hough[i] > max ) {
            out.data[i] = 255;
        } else {
            out.data[i] = 0;
        }
    }

    free( hough );

    return out;
}


/**
 * naveed's label_hough function
 *
 * hough - image of the hough transform, (regions are labeled after)
 */
void label_hough( struct image *hough ) {
    int x, y, A, B, C, D, label, P, i, newlabel;
    Disjoint_Set S;
    SetType Uset, Vset, Pset;
    int maxlabel = 10240;
    int renumber[10240];
    int *tempdata;
    int w, h;

    /* initialize labels, sets, tables and tempdata */
    label = newlabel = 0;
    S = DS_Create(maxlabel);

    w = hough->columns;
    h = hough->rows;
    tempdata = (int*) malloc( sizeof(int) * w * h );

    for( i = 0; i < w * h; i++ ) {
        if( hough->data[i] == 255 ) {
            tempdata[i] = 1;
        } else {
            tempdata[i] = 0;
        }
    }

    /* Sequential labeling algorithm */
    for( y = 0; y < h; y++ ) {
        for( x = 0; x < w; x++ ) {

            if( x + 1 < w && y + 1 < h ) {
                A = tempdata[(y + 1) * w + (x + 1)];
                B = tempdata[ y * w + (x + 1)];
                C = tempdata[(y + 1) * w + x ];
                D = tempdata[ y * w + x ];

                if( D == 0 && B == 0 && C == 0 && A == 1 ) {
                    label++;
                    tempdata[(y + 1) * w + (x + 1)] = label;
                } else if( A == 0 ) {
                    ;
                } else if( D != 0 && A == 1 ) {
                    tempdata[(y + 1) * w + (x + 1)] = D;
                } else if( D == 0 && B == 0 && C != 0 && A == 1 ) {
                    tempdata[(y + 1) * w + (x + 1)] = C;
                } else if( D == 0 && B != 0 && C == 0 && A == 1 ) {
                    tempdata[(y + 1) * w + (x + 1)] = B;
                } else if( D==0 && B!=0 && C!=0 && A==1 ) {
                    if( B == C ) {
                        tempdata[(y + 1) * w + (x + 1)] = B;
                    } else {

                        Uset = DS_Find(S, B);
                        Vset = DS_Find(S, C);
        
                        if( Uset != Vset ){
                            DS_SetUnion(S, Uset, Vset);
                        }
        
                        /*fix in next pass*/
                        tempdata[(y + 1) * w + (x + 1)] = C;
                    }
                }
            }
        }
    } /* end first pass */

    for( i = 0; i < maxlabel; i++ ) {
        renumber[i] = -1;
    }
    renumber[0] = 0;

    for( y=0; y < h; y++ ) {
        for( x=0; x < w; x++ ) {

            P = tempdata[y * w + x];

            if( P == 0 ) {
                Pset = 0;
            } else {
                Pset = DS_Find(S, P);
            }

            if( renumber[Pset] == -1 ) {
                newlabel++;
                renumber[Pset] = newlabel;

                hough->data[ y * w + x ] = renumber[Pset];
            } else {
                hough->data[ y * w + x ] = renumber[Pset];
            }
        }
    } /* end second pass */

    hough->colors = newlabel;

    free( tempdata );
}

/**
 * Check to make sure that the line found are 'nice' lines (lines extend
 *      from one side to the other  left->right, top->bottom).
 *
 * points  - end points of found lines
 * *num    - number of lines, before and after
 * width   - width of image
 * height  - height of image
 *
 * returns a new array of lines ( *num contains the number )
 */
Line_End_Points* check_lines( Line_End_Points* points, int *num,
                              int width, int height )
{
    int i;
    int index;
    Line_End_Points* return_points;

    width--;
    height--;

    return_points = (Line_End_Points*) malloc(sizeof(Line_End_Points)*(*num));

    index = 0;
    for( i = 0; i < *num; i++ ) {
        if( (points[i].xa == 0 && points[i].xb != width)
            || (points[i].xa != 0 && points[i].xb == width)
            || (points[i].ya == 0 && points[i].yb != height)
            || (points[i].ya != 0 && points[i].yb == height)
            || (points[i].xa >= width-10 && points[i].ya == 0
                && points[i].xb == points[i].xa) )
        {
            /* found a bad line, move on to the next */
            continue;
        }

        /* add this line */
        return_points[index].xa = points[i].xa;
        return_points[index].ya = points[i].ya;
        return_points[index].xb = points[i].xb;
        return_points[index].yb = points[i].yb;
        index++;
    }

    *num = index;

    return return_points;
}

/**
 * naveed's line stuff.
 *
 * Gets the lines from the hough transform.
 *
 * hough  - labeled hough image
 * w      - width of original image
 * h      - height of original image
 * *num   - after contains the number of lines found
 *
 * returns the lines found
 */
Line_End_Points* get_lines(struct image *hough, int w, int h, int *num ) {
    double rbar, tbar, area;
    int i, j, xa, ya, xb, yb;
    int hw, hh;
    int r, t;
    Line_End_Points* points;
    Line_End_Points* return_points;

    hw = hough->columns;
    hh = hough->rows;

    points = (Line_End_Points*)malloc(sizeof(Line_End_Points)*hough->colors-1);
    (*num) = hough->colors - 1;

    /* skipping 0 and 1 -- not sure why, but it is better if we do*/
    /* for each label (0=background) */
    for( i = 2; i <= hough->colors; i++) {
        area = 0.0;
        rbar = 0.0;
        tbar = 0.0;

        for( j = 0; j < hw * hh; j++ ) {
            if( hough->data[j] == i ) {
                r = j / hw;
                t = j - ( r * hw );
                area++;
                rbar += (double)r;
                tbar += (double)t;
            }
        }

        rbar /= area;
        tbar /= area;

        /* calculate endpoints in (x, y) space using this (theta, rho) pair */
        ya = (int)(  rbar / cos_lookup[ (int)tbar ]);
        xa = (int)( -rbar / sin_lookup[ (int)tbar ]);
        yb = (int)(((w*sin_lookup[(int)tbar])+rbar)/cos_lookup[(int)tbar]);
        xb = (int)(((h*cos_lookup[(int)tbar])-rbar)/sin_lookup[(int)tbar]);

        if( (xa >= 0 && xa < w) && ( ya < 0 || ya >= h) ) {
            ya = 0;
        } else if( (ya >= 0 && ya < h) && (xa < 0 || xa >= w) ) {
            xa = 0;
        } else if( (xa < 0 || xa >= w) && (ya < 0 || ya >= h) ) {
            xa = 0;
            ya = 0;
        }
        if( (xb >= 0 && xb < w) && ( yb < 0 || yb >= h) ) {
            yb = h - 1;
        } else if( (yb >= 0 && yb < h) && (xb < 0 || xb >= w) ) {
            xb = w - 1;
        } else if( (xb < 0 || xb >= w) && (yb < 0 || yb >= h) ) {
            xb = w - 1;
            yb = h - 1;
        }

        points[i-2].xa = xa;
        points[i-2].ya = ya;
        points[i-2].xb = xb;
        points[i-2].yb = yb;
    } /* all labels done */

    /* check that the lines found make sense for us */
    return_points = check_lines( points, num, w, h );

    free( points );

    return return_points;
}

/**
 * Filter the lines by checking to see if two lines are too close to each other.
 *
 * points   - array of lines
 * *num     - number of lines
 */
void filter_lines( Line_End_Points* lines, int *num ) {
    int i, j;
    int dxa, dya, dxb, dyb;
    Line_End_Points* mark;

    mark = (Line_End_Points*) malloc( sizeof( Line_End_Points ) * (*num) );
    j = 0;
    for(i = 0; i < *num - 1; i++) {

        dxa = lines[i].xa - lines[i+1].xa;
        dya = lines[i].ya - lines[i+1].ya;
        dxb = lines[i].xb - lines[i+1].xb;
        dyb = lines[i].yb - lines[i+1].yb;

        dxa = MABS( dxa );
        dya = MABS( dya );
        dxb = MABS( dxb );
        dyb = MABS( dyb );

        if( !(dxa <= 6 && dya <= 6 && dxb <= 6 && dyb <= 6) ) {
            mark[j].xa = lines[i].xa;
            mark[j].ya = lines[i].ya;
            mark[j].xb = lines[i].xb;
            mark[j].yb = lines[i].yb;
            j++;
        }
    }
    mark[j].xa = lines[*num-1].xa;
    mark[j].ya = lines[*num-1].ya;
    mark[j].xb = lines[*num-1].xb;
    mark[j].yb = lines[*num-1].yb;
    j++;

    for(i = 0; i < *num; i++) {
        if( i < j ) {
            lines[i].xa = mark[i].xa;
            lines[i].ya = mark[i].ya;
            lines[i].xb = mark[i].xb;
            lines[i].yb = mark[i].yb;
        } else {
            lines[i].xa = 0;
            lines[i].ya = 0;
            lines[i].xb = 0;
            lines[i].yb = 0;
        }
    }
    *num = j;

    free( mark );
}

/**
 * Computes the intersection between two lines.
 *
 * x1, y1, x2, y2   - end points for the first line
 * x3, y3, x4, y4   - end points for the second line
 * *x               - x pos of the intersection
 * *y               - y pos of the intersection
 *
 * (*x,*y) of (-1,-1) indicates a bad intersection
 */
void intersection( int x1, int y1, int x2, int y2,
                   int x3, int y3, int x4, int y4, int* x, int* y )
{
    double b1, b2, m1, m2;

    if( !(x1 == x3 && x2 == x4 && y1 == y3 && y2 == y4) ) {

        if( x1 != x2 && x3 != x4 ) {
            m1 = (double)(y2 - y1) / (double)(x2 - x1);
            b1 = (double) y1 - (double) x1 * m1;

            m2 = (double)(y4 - y3) / (double)(x4 - x3);
            b2 = (double) y3 - (double) x3 * m2;

            (*x) = (int)( (b2 - b1) / (m1 - m2) );

            (*y) = (int)( m1 * (*x) + b1 );
        } else {
            if( x3 == x4 ) {
                m1 = (double)(y2 - y1) / (double)(x2 - x1);
            } else {
                m1 = (double)(y4 - y3) / (double)(x4 - x3);
                x1 = x3;
                y1 = y3;
            }

            b1 = (double) y1 - (double) x1 * m1;

            (*x) = x3;
            (*y) = (int)( m1 * (double) (*x) + b1 );
        }
    } else {
        (*x) = -1;
        (*y) = -1;
    }
}

/** comparison functions */
/** horizontal comparison */
int horz_comp( const void* a, const void* b ) {
    return ( ((Line_End_Points*)a)->ya - ((Line_End_Points*)b)->ya );
}
/** vertical comparison */
int vert_comp( const void* a, const void* b ) {
    return ( ((Line_End_Points*)a)->xa - ((Line_End_Points*)b)->xa );
}

/**
 * Sorts the given lines into horizontal and vertical lines.
 *
 * lines      - original line array
 * num        - number of lines in *lines
 * vertical   - vertical lines found  ( pre allocated to num )
 * *vnum      - number of vertical lines found
 * horizontal - horizontal lines found ( pre allocated to num )
 * *hnum      - number of horizontal lines found
 */
void sort_lines( Line_End_Points* lines, int num,
                 Line_End_Points* vertical, int* vnum,
                 Line_End_Points* horizontal, int* hnum )
{
    double m;
    int i;

    (*hnum) = 0;
    (*vnum) = 0;
    for( i = 0; i < num; i++ ) {

        if( lines[i].xa == lines[i].xb ) {
            vertical[(*vnum)].xa = lines[i].xa;
            vertical[(*vnum)].ya = lines[i].ya;
            vertical[(*vnum)].xb = lines[i].xb;
            vertical[(*vnum)].yb = lines[i].yb;
            (*vnum)++;
        } else {

            m = (lines[i].yb - lines[i].ya ) / ( lines[i].xb - lines[i].xa );

            m = ( m < 0 ? -m : m );

            if( m <= 1 ) {
                horizontal[(*hnum)].xa = lines[i].xa;
                horizontal[(*hnum)].ya = lines[i].ya;
                horizontal[(*hnum)].xb = lines[i].xb;
                horizontal[(*hnum)].yb = lines[i].yb;
                (*hnum)++;
            } else {
                vertical[(*vnum)].xa = lines[i].xa;
                vertical[(*vnum)].ya = lines[i].ya;
                vertical[(*vnum)].xb = lines[i].xb;
                vertical[(*vnum)].yb = lines[i].yb;
                (*vnum)++;
            }
        }
    }

    qsort( horizontal, *hnum, sizeof( Line_End_Points ), horz_comp);
    qsort( vertical, *vnum, sizeof( Line_End_Points ), vert_comp);
}

/**
 * Finds the raw corner points from line intersections.
 *
 * lines - line array
 * num - number of lines
 * *cnum - the number of corner points found
 *
 * returns array of corner points
 */
Point* find_corners( Line_End_Points* lines, int num, int* cnum ) {
    int i, j, n;
    int hnum, vnum;
    Line_End_Points* horizontal;
    Line_End_Points* vertical;
    Point* corners;

    horizontal = (Line_End_Points*) malloc( sizeof(Line_End_Points) * num );
    vertical = (Line_End_Points*) malloc( sizeof(Line_End_Points) * num );

    printf( "Filtering ... " );
    filter_lines( lines, &num );
    printf( "done " );

    printf( "Sorting ... " );
    sort_lines( lines, num, vertical, &vnum, horizontal, &hnum );
    printf( "h=%d v=%d done ", hnum, vnum );

    corners = (Point*) malloc( sizeof(Point) * vnum * hnum );

    n = 0;
    for(i = 0; i < hnum; i++) {
        for(j = 0; j < vnum; j++) {
            intersection( horizontal[i].xa, horizontal[i].ya,
                          horizontal[i].xb, horizontal[i].yb,
                          vertical[j].xa, vertical[j].ya,
                          vertical[j].xb, vertical[j].yb,
                          &(corners[n].x), &(corners[n].y) );
            if( !(corners[n].x == -1 && corners[n].y == -1) ) {
                n++;
            }
        }
    }

    (*cnum) = n;

    free( horizontal );
    free( vertical );

    return corners;
}

/**
 * Moves a point closer to the real corner of calibration square.
 *
 * *x   - x position
 * *y   - y position
 * xinc - x increment
 * yinc - y increment
 * dir  - direction
 * img  - calibration image
 *
 * returns   -1 if the point wasn't moved, 1 if the point was moved
 */
int move_point(int *x, int *y, int xinc, int yinc, int dir, struct image img) {
    int window_size = 20;
    int diff_size = 80;
    int win = 6;
    int i;
    int stop, d, dabs;
    int mx, my;

    i = 0;
    mx = *x;
    my = *y;

    if( dir == 0 ) {
        d = img.data[my*img.columns+(mx-win)]-img.data[my*img.columns+(mx+win)];
    } else if( dir == 1 ) {
        d = img.data[(my-win)*img.columns+mx]-img.data[(my+win)*img.columns+mx];
    }

    stop = d;
    dabs = MABS( d );

    if( dabs < 20 ) {
        return -1;
    }

    while( i < window_size ) {

        if( stop > 0 && d < 0 && dabs > diff_size ) {
            break;
        } else if( stop < 0 && d > 0 && dabs > diff_size ) {
            break;
        }

        mx += xinc;
        my += yinc;

        if( dir == 0 ) {
            d = img.data[my*img.columns+(mx-win)]
                    - img.data[my*img.columns+(mx+win)];
        } else if( dir == 1 ) {
            d = img.data[(my-win)*img.columns+mx]
                    - img.data[(my+win)*img.columns+mx];
        }

        dabs = MABS( d );
        i++;
    }

    if( i >= window_size ) {
        return -1;
    }

    *x = mx;
    *y = my;
    return 1;
}

/**
 * Returns the distance between two points.
 *
 * x1, y1  -  first point
 * x2, y2  - second point
 *
 * returns the distance
 */
double distance( int x1, int y1, int x2, int y2 ) {
    return sqrt( (x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1) );
}

/**
 * Refines the corner points by looking for close neighbors
 *
 * *points - array of points
 * *num    - number of points before and after
 *
 * return new array of points
 */
Point* refine_points( Point* points, int* num ) {
    Point* newpoints;
    int* point_index;
    double d;
    double maxdis = 20.0;
    int index;
    int i, j;

    point_index = (int*) malloc( sizeof( int ) * (*num) );
    for( i = 0; i < *num; i++ ) {
        point_index[i] = 0;
    }

    for( i = 0; i < *num; i++ ) {
        for( j = i; j < *num; j++ ) {
            if( point_index[j] == 0 && j != i ) {
                d = distance(points[i].x,points[i].y,points[j].x,points[j].y);

                if( d <= maxdis ) {
                    point_index[j] = -1;
                }
            }
        }
    }

    newpoints = (Point*) malloc( sizeof( Point ) * (*num) );
    index = 0;
    for( i = 0; i < *num; i++ ) {
        if( point_index[i] == 0 ) {
            newpoints[index].x = points[i].x;
            newpoints[index].y = points[i].y;
            index++;
        }
    }
    *num = index;

    free( point_index );

    return newpoints;
}

/**
 * Refines the corners points found.
 *
 * *corners  - corner point array
 * *cnum     - number of corner points (before and after)
 * img       - calibration image
 *
 * returns a new array of corner points that are refined
 */
Point* refine_corners( Point* corners, int* cnum, struct image img ) {
    Point* newcorners;
    Point* return_corners;
    int i, x, y;
    int index;
    int updown, leftright;

    index = 0;
    newcorners = (Point*) malloc( sizeof( Point ) * (*cnum) );

    /* first move points */
    for(i = 0; i < *cnum; i++) {

        x = corners[i].x;
        y = corners[i].y;

        updown = move_point( &x, &y, 0, 1, 0, img ); 

        if( updown == -1 ) {
            updown = move_point( &x, &y, 0, -1, 0, img );
        }

        leftright = move_point( &x, &y, 1, 0, 1, img );

        if( leftright == -1 ) {
            leftright = move_point( &x, &y, -1, 0, 1, img );
        }

        if( updown == 1 || leftright == 1 ) {
            /* we made it */
            newcorners[index].x = x;
            newcorners[index++].y = y;
        }
    }

    /* now remove dups */
    *cnum = index;
    return_corners = refine_points( newcorners, cnum );

    free( newcorners );

    return return_corners;
}

/** functions for visualizations */
void paint_lines( Line_End_Points* points, int num, struct image *orig ) {
    int i;

    for(i = 0; i < num; i++) {
        line(orig,points[i].ya,points[i].xa,points[i].yb,points[i].xb,64);
    }
}

void paint_corners( struct image *img, Point* corners, int num ) {
    int i;

    for(i = 0; i < num; i++) {
        img->data[ corners[i].y * img->columns + corners[i].x ] = 255;
    }
}

void scale( struct image *img, double scale ) {
    int i;
    int total;

    total = img->rows * img->columns;
    for(i = 0; i < total; i++) {
        img->data[i] = (int)( img->data[i] * scale );
    }
}

/**
 * Forces the corner points to lie in a square. Assumes that there is
 *  only a difference in either one row or column.
 *
 * *corners - array of corner points
 * *num     - size of array (before and after)
 *
 * returns a new array of the square corner points
 */
Point* make_corners_square( Point* corners, int* num ) {
    Point* new_corners;
    int len, max, i;
    int count;

    len = (int) sqrt( (*num) );

    new_corners = (Point*) malloc( sizeof( Point ) * (*num) );

    i = 0;
    max = corners[i++].x;
    while( max < corners[i].x ) {
        max = corners[i++].x;
    }
    max = i;

    if( max > len ) {
        /*
        * the points are wider than tall, cut off the last column
        */
        count = 0;
        for(i = 0; i < *num && count < len*len; i++) {
            if( i % max != len ) {
                new_corners[ count ].x = corners[i].x;
                new_corners[ count ].y = corners[i].y;
                count++;
            }
        }
    } else if( max == len ) {
        /*
        * the points are taller than wide, cut off the first row
        */
        count = 0;
        for( i = len; i < *num && count < len*len; i++ ) {
            new_corners[ count ].x = corners[i].x;
            new_corners[ count ].y = corners[i].y;
            count++;
        }
    }

    (*num) = count;

    return new_corners;
}

/* calibration code -------------------------------------------------- */
/* some of these numbers need to change */
void camera_params() {
    cp.Ncx = 768;                       /* [sel]        */
    cp.Nfx = 640;                       /* [pix]        */
    cp.dx = 0.01145833333;              /* [mm/sel]     */
    cp.dy = 0.01338742394;              /* [mm/sel]     */
    cp.dpx = cp.dx * cp.Ncx / cp.Nfx;   /* [mm/pix]     */
    cp.dpy = cp.dy;                     /* [mm/pix]     */
    cp.Cx = 319.5;                      /* [pix]        */
    cp.Cy = 239.5;                      /* [pix]        */
    cp.sx = 1.0;                        /* []           */
}

/**
 * Get the calibration.
 *
 * square_len_mm - len of a side of squares in mm
 * *corners      - corner point array
 * num           - number of points
 * dumpdata      - flag for saving data
 * outdata       - file pointer for data
 * outparam      - file pointer for params 
 */
void camera_calibration( int square_len_mm, Point* corners, int num,
                         int dumpdata, FILE* outdata, FILE* outparam )
{
    int i;
    int len;
    camera_params();

    len = (int) sqrt( num );

    if( len*len != num ) {
        printf("Points are not in a square\n" );
        corners = make_corners_square( corners, &num );
    }

    cd.point_count = 0;
    for(i = 0; i < num; i++) {
        cd.xw[i] = ( i % len ) * square_len_mm;
        cd.yw[i] = len * square_len_mm - ( i / len ) * square_len_mm;
        cd.zw[i] = 0.0;
        cd.Xf[i] = corners[i].x;
        cd.Yf[i] = corners[i].y;
        cd.point_count++;

        /*printf("(%d, %d, %d) => (%d, %d)\n", (int)cd.xw[i], (int)cd.yw[i],
                    (int)cd.zw[i], (int)cd.Xf[i], (int)cd.Yf[i]);*/
    }

    /* call tsai code for calibration */
    coplanar_calibration();

    print_cp_cc_data (stdout, &cp, &cc);

    if( dumpdata ) {
        dump_cd_data( outdata, &cd );
        dump_cp_cc_data( outparam, &cp, &cc );
    }
}
/* end calibration code ---------------------------------------------- */


/**
 * The Super Cool Function. Starts the ball rolling.
 *
 * *imagefile  - image file name
 * len         - length of squares
 * *cdata      - data file name
 * *cparam     - params file name
 *
 */
void calib(char *imagefile, int len, char *cdata, char *cparam) {
    struct image img;
    struct image edgeimg;
    struct image houghimg;
    Point* corners;
    Line_End_Points *points;
    FILE *cdatafile, *cparamfile;
    int val, num, cnum;

    if( readImage( &img, imagefile ) == 0 ) {
        init();

        printf( "Finding edges ... " );
        edgeimg = edge( img, &val );
        printf( "val=%d  done\n", val );

        /*
	printf( "Writing edge img -> edge.pgm ... ");
        writeImage( &edgeimg, "edge.pgm" );
        printf( "done\n" );
	*/

        printf( "Finding hough ... " );
        houghimg = hough( edgeimg, val );
        printf( "done\n" );

	/*
        printf( "Writing hough img -> hough.pgm ... " );
        writeImage( &houghimg, "hough.pgm" );
        printf( "done\n" );
	*/
        
        printf( "Labeling hough ... " );
        label_hough( &houghimg );
        printf( "done\n" );

        printf( "Getting lines ... " );
        points = get_lines( &houghimg, img.columns, img.rows, &num );
        printf( "n=%d  done\n", num );

        printf( "Finding corners ... " );
        corners = find_corners( points, num, &cnum );
        printf( "cn=%d done \n", cnum );

        printf( "Refining corners ... " );
        corners = refine_corners( corners, &cnum, img );
        printf( "cn=%d done\n", cnum );

        scale( &img, .5 );
        paint_lines( points, num, &img );
        paint_corners( &img, corners, cnum );
	/*
        writeImage( &img, "out.pgm" );
	*/

        cdatafile = fopen( cdata, "wt" );
        cparamfile= fopen( cparam, "wt" );
        printf("Camera calibration (%s) (%s) ...\n", cdata, cparam);
        camera_calibration( len, corners, cnum, 1, cdatafile, cparamfile );
        fclose( cdatafile );
        fclose( cparamfile );

        free( points );
        free( corners );
    }
}

int main( int argc, char** argv ) {
    char *cal_file;
    int len;

    if( argc <= 1 ) {
        printf( "Usage: %s <image> <len mm> [data] [param]\n", argv[0] );
        return 0;
    }

    cal_file = argv[1];
    len = atoi( argv[2] );

    if( argc >= 4 ) {
        calib( cal_file, len, argv[3], argv[4] );
    } else {
        calib( cal_file, len, "data", "param" );
    }

    return 0;
}
