/*
 * 
 * This file is part of ARToolKit.
 * 
 * ARToolKit is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * ARToolKit is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with ARToolKit; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 * 
 */

// ============================================================================
//	Includes
// ============================================================================

#ifdef _WIN32
#  include <windows.h>
#endif
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#ifndef __APPLE__
#  include <GL/glut.h>
#  ifdef GL_VERSION_1_2
#    include <GL/glext.h>
#  endif
#else
#  include <GLUT/glut.h>
#  include <OpenGL/glext.h>
#endif
#include <AR/config.h>
#include <AR/video.h>
#include <AR/param.h>
#include <AR/matrix.h>
#include <AR/gsub_lite.h>
#include "calib_camera.h"

// ============================================================================
//	Constants
// ============================================================================

#define CALIB_CAMERA2_DEBUG 0

// ============================================================================
//	Global variables
// ============================================================================

/* set up the video format globals */

#if defined(__sgi)
static char            *vconf = "-size=FULL";
#elif defined(__linux)
#  if defined(AR_INPUT_GSTREAMER)
char 			*vconf = "videotestsrc ! capsfilter caps=video/x-raw-rgb,bpp=24 ! identity name=artoolkit ! fakesink";
#  elif defined(AR_INPUT_V4L)
static char            *vconf = "-width=640 -height=480";
#  elif defined(AR_INPUT_1394CAM)
static char            *vconf = "-mode=640x480_YUV411";
#  elif defined(AR_INPUT_DV)
static char            *vconf = "";
#  endif
#elif defined(_WIN32)
static char			*vconf = "Data\\WDM_camera_flipV.xml";
#elif defined(__APPLE__)
static char			*vconf = "-width=640 -height=480";
#else
static char			*vconf = "";
#endif

static ARUint8		*gARTImage = NULL;
static ARParam		gARTCparam; // Dummy parameter, to supply to gsub_lite.
static ARGL_CONTEXT_SETTINGS_REF gArglSettings = NULL;


static int             gWin;
static int             gXsize = 0;
static int             gYsize = 0;
static int             gThresh = THRESH;
static unsigned char   *gClipImage = NULL;

static CALIB_PATT_T    gPatt;          
static double          dist_factor[4];
static double          mat[3][4];

static int             point_num;
static int             gDragStartX = -1, gDragStartY = -1, gDragEndX = -1, gDragEndY = -1;	// x and y coordinates of start and end of mouse drag.

static int             gStatus;	// 0 = Waiting to grab image, 1 = Drawing bounding boxes, 2 = Placing warp lines.
static int             check_num;

// ============================================================================
//	Functions
// ============================================================================

int             main(int argc, char *argv[]);
static int      init(int argc, char *argv[]);
static void     Mouse(int button, int state, int x, int y);
static void     Motion(int x, int y);
static void     Keyboard(unsigned char key, int x, int y);
static void     Quit(void);
static void     Idle(void);
static void     Visibility(int visible);
static void     Reshape(int w, int h);
static void     Display(void);
static void     draw_warp_line(double a, double b , double c);
static void     draw_line(void);
static void     draw_line2(double *x, double *y, int num);
static void     draw_warp_line(double a, double b , double c);
static void     print_comment(int status);
static void     save_param(void);

int main(int argc, char *argv[])
{
	glutInit(&argc, argv);
    if (!init(argc, argv)) exit(-1);
	
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
    glutInitWindowSize(gXsize, gYsize);
    glutInitWindowPosition(100,100);
    gWin = glutCreateWindow("Calibrate camera parameters");
	
	// Setup argl library for current context.
	// Turn off distortion compensation.. we don't know distortion yet!
	if ((gArglSettings = arglSetupForCurrentContext()) == NULL) {
		fprintf(stderr, "main(): arglSetupForCurrentContext() returned error.\n");
		exit(-1);
	}
	arglDistortionCompensationSet(gArglSettings, FALSE);
	
	// Make a dummy camera parameter to supply when calling arglDispImage().
	gARTCparam.xsize = gXsize;
	gARTCparam.ysize = gYsize;
	
	// Register GLUT event-handling callbacks.
	// NB: Idle() is registered by Visibility.
    glutDisplayFunc(Display);
	glutReshapeFunc(Reshape);
	glutVisibilityFunc(Visibility);
	glutKeyboardFunc(Keyboard);
    glutMouseFunc(Mouse);
    glutMotionFunc(Motion);
	
	// Start grabbing.
	if (arVideoCapStart() != 0) {
    	fprintf(stderr, "init(): Unable to begin camera data capture.\n");
		return (FALSE);		
	}
	point_num = 0;
    gStatus = 0;
    print_comment(0);
	
    glutMainLoop();
	
	return (0);
}

static int init(int argc, char *argv[])
{
    double  length;
    char    line[512];
    int     i, j;

    gPatt.h_num    = H_NUM;
    gPatt.v_num    = V_NUM;
    gPatt.loop_num = 0;
    if (gPatt.h_num < 3 || gPatt.v_num < 3) exit(0);

    printf("Input the distance between each marker dot, in millimeters: ");
    scanf("%lf", &length);
	while (getchar() != '\n');

    gPatt.world_coord = (CALIB_COORD_T *)malloc(sizeof(CALIB_COORD_T) * gPatt.h_num * gPatt.v_num);
    for (j = 0; j < gPatt.v_num; j++) {
        for (i = 0; i < gPatt.h_num; i++) {
            gPatt.world_coord[j*gPatt.h_num+i].x_coord = length * i;
            gPatt.world_coord[j*gPatt.h_num+i].y_coord = length * j;
        }
    }
		
	// Add command-line arguments to vconf string.
    strcpy(line, vconf);
    for (i = 1; i < argc; i++) {
        strcat(line, " ");
        strcat(line, argv[i]);
    }
	
	// Open the video path.
    if (arVideoOpen(line) < 0) {
    	fprintf(stderr, "init(): Unable to open connection to camera.\n");
    	return (FALSE);
	}
	
	// Find the size of the window.
    if (arVideoInqSize(&gXsize, &gYsize) < 0) return (FALSE);
    fprintf(stdout, "Camera image size (x,y) = (%d,%d)\n", gXsize, gYsize);
	
	// Allocate space for a clipping image (luminance only).
	arMalloc(gClipImage, unsigned char, gXsize * gYsize);

	return (TRUE);
}

static void grabImage(void) {
	ARUint8 *image;
	
	// Processing a new image.
	// Copy an image to saved image buffer.
	do {
		image = arVideoGetImage();
	} while (image == NULL);
	gPatt.loop_num++;
	if ((gPatt.arglSettings[gPatt.loop_num-1] = arglSetupForCurrentContext()) == NULL) {
		fprintf(stderr, "grabImage(): arglSetupForCurrentContext() returned error.\n");
		exit(-1);
	}
	arglDistortionCompensationSet(gPatt.arglSettings[gPatt.loop_num-1], FALSE);
	arMalloc((gPatt.savedImage)[gPatt.loop_num-1], unsigned char, gXsize*gYsize*AR_PIX_SIZE_DEFAULT);
	memcpy((gPatt.savedImage)[gPatt.loop_num-1], image, gXsize*gYsize*AR_PIX_SIZE_DEFAULT);
	printf("Grabbed image %d.\n", gPatt.loop_num);
	arMalloc(gPatt.point[gPatt.loop_num-1], CALIB_COORD_T, gPatt.h_num*gPatt.v_num);
}

static void ungrabImage(void) {
	if (gPatt.loop_num == 0) {printf("error!!\n"); exit(0);}
	free(gPatt.point[gPatt.loop_num-1]);
	gPatt.point[gPatt.loop_num-1] = NULL;
	free(gPatt.savedImage[gPatt.loop_num-1]);
	gPatt.savedImage[gPatt.loop_num-1] = NULL;
	arglCleanup(gPatt.arglSettings[gPatt.loop_num-1]);
	gPatt.loop_num--;			
}

void checkFit(void) {
    printf("\n-----------\n");
	if (check_num < gPatt.loop_num) {
		printf("Checking fit on image %3d of %3d.\n", check_num + 1, gPatt.loop_num);
		if (check_num + 1 < gPatt.loop_num) {
			printf("Press mouse button to check fit of next image.\n");
		} else {
			printf("Press mouse button to calculate parameter.\n");
		}
		glutPostRedisplay();
	} else {
		if (gPatt.loop_num > 1) {
			if (calc_inp(&gPatt, dist_factor, gXsize, gYsize, mat) < 0) {
				printf("Calibration failed. Exiting.\n");
				exit(0);
			} else {
				printf("Calibration succeeded. Enter filename to save camera parameter to below.\n");
				save_param();
			}
		} else {
			printf("Not enough images to proceed with calibration. Exiting.\n");
		}
		Quit();
	}				
}

static void eventCancel(void) {
	// What was cancelled?
	if (gStatus == 0) {
		// Cancelled grabbing.
		// Live video will not be needed from here on.
		arVideoCapStop();
		if (gPatt.loop_num == 0) {
			// No images with all features identified, so quit.
			Quit();
		} else {
			// At least one image with all features identified,
			// so calculate distortion.
			calc_distortion(&gPatt, gXsize, gYsize, dist_factor);
			printf("--------------\n");
			printf("Center X: %f\n", dist_factor[0]);
			printf("       Y: %f\n", dist_factor[1]);
			printf("Dist Factor: %f\n", dist_factor[2]);
			printf("Size Adjust: %f\n", dist_factor[3]);
			printf("--------------\n");
			// Distortion calculation done. Check fit.
			gStatus = 2;
			check_num = 0;
			checkFit();
		}
	} else if (gStatus == 1) {
		// Cancelled rubber-bounding.
		ungrabImage();
		// Restart grabbing.
		point_num = 0;
		arVideoCapStart();
		gStatus = 0;
		if (gPatt.loop_num == 0) print_comment(0);
		else                     print_comment(4);	
	}
}

static void Mouse(int button, int state, int x, int y)
{
    unsigned char   *p;
    int             ssx, ssy, eex, eey;
    int             i, j, k;

	if (button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN) {
		eventCancel();
	} else if (button == GLUT_LEFT_BUTTON) {
		if (state == GLUT_DOWN) {
			if (gStatus == 0 && gPatt.loop_num < LOOP_MAX) {
				// End grabbing.
				// Begin waiting for drag for rubber-bounding of a feature.
				grabImage();
				gDragStartX = gDragStartY = gDragEndX = gDragEndY = -1;
				arVideoCapStop();
				gStatus = 1;
				print_comment(1);				
			} else if (gStatus == 1) {
				if (point_num < gPatt.h_num*gPatt.v_num) {
					// Drag for rubber-bounding of a feature has begun.
					gDragStartX = gDragEndX = x;
					gDragStartY = gDragEndY = y;
				} else {
					// Feature point locations have been accepted.
					printf("### Image no.%d ###\n", gPatt.loop_num);
					for( j = 0; j < gPatt.v_num; j++ ) {
						for( i = 0; i < gPatt.h_num; i++ ) {
							printf("%2d, %2d: %6.2f, %6.2f\n", i+1, j+1,
								   gPatt.point[gPatt.loop_num-1][j*gPatt.h_num+i].x_coord,
								   gPatt.point[gPatt.loop_num-1][j*gPatt.h_num+i].y_coord);
						}
					}
					printf("\n\n");
					// Restart grabbing.
					point_num = 0;
					arVideoCapStart();
					gStatus = 0;
					if (gPatt.loop_num < LOOP_MAX) print_comment(4);
					else                           print_comment(5);
				}
			} else if (gStatus == 2) {
				check_num++;
				checkFit();
			}
		} else if (state == GLUT_UP) {
			if (gStatus == 1
				 && gDragStartX != -1 && gDragStartY != -1
				 && gDragEndX != -1 && gDragEndY != -1
				 && point_num < gPatt.h_num*gPatt.v_num) {
				// Drag for rubber-bounding of a feature has finished. Begin identification
				// of center of white region in gClipImage.
				if (gDragStartX < gDragEndX) { ssx = gDragStartX; eex = gDragEndX; }
				else         { ssx = gDragEndX; eex = gDragStartX; }
				if (gDragStartY < gDragEndY) { ssy = gDragStartY; eey = gDragEndY; }
				else         { ssy = gDragEndY; eey = gDragStartY; }
				
				gPatt.point[gPatt.loop_num-1][point_num].x_coord = 0.0;
				gPatt.point[gPatt.loop_num-1][point_num].y_coord = 0.0;
				p = gClipImage;
				k = 0;
				for (j = 0; j < (eey-ssy+1); j++) {
					for (i = 0; i < (eex-ssx+1); i++) {
						gPatt.point[gPatt.loop_num-1][point_num].x_coord += i * *p;
						gPatt.point[gPatt.loop_num-1][point_num].y_coord += j * *p;
						k += *p;
						p++;
					}
				}
				if (k != 0) {
					gPatt.point[gPatt.loop_num-1][point_num].x_coord /= k;
					gPatt.point[gPatt.loop_num-1][point_num].y_coord /= k;
					gPatt.point[gPatt.loop_num-1][point_num].x_coord += ssx;
					gPatt.point[gPatt.loop_num-1][point_num].y_coord += ssy;
					point_num++;
					printf("Marked feature position %3d of %3d\n", point_num, gPatt.h_num*gPatt.v_num);
					if (point_num == gPatt.h_num*gPatt.v_num) print_comment(2);
				}
				gDragStartX = gDragStartY = gDragEndX = gDragEndY = -1;
				glutPostRedisplay();
			}
		}
	}
}

static void Motion(int x, int y)
{
    unsigned char   *p, *p1;
    int             ssx, ssy, eex, eey;
    int             i, j;
	
	// During mouse drag.
    if (gStatus == 1 && gDragStartX != -1 && gDragStartY != -1) {
		
		// Set x,y of end of mouse drag region.
        gDragEndX = x;
        gDragEndY = y;
		
		// Check that start is above and to left of end.
		if (gDragStartX < gDragEndX) { ssx = gDragStartX; eex = gDragEndX; }
		else         { ssx = gDragEndX; eex = gDragStartX; }
		if (gDragStartY < gDragEndY) { ssy = gDragStartY; eey = gDragEndY; }
		else         { ssy = gDragEndY; eey = gDragStartY; }
		
		// Threshold clipping area, copy it into gClipImage.
        p1 = gClipImage;
        for (j = ssy; j <= eey; j++) {
            p = &(gPatt.savedImage[gPatt.loop_num-1][(j*gXsize+ssx)*AR_PIX_SIZE_DEFAULT]);
            for (i = ssx; i <= eex; i++) {
#if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGRA)
                *p1 = (((255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ABGR)
                *p1 = (((255*3 - (*(p+1) + *(p+2) + *(p+3))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ARGB)
                *p1 = (((255*3 - (*(p+1) + *(p+2) + *(p+3))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR)
                *p1 = (((255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGBA)
                *p1 = (((255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB)
                *p1 = (((255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_2vuy)
                *p1 = ((255 - *(p+1)) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_yuvs)
                *p1 = ((255 - *(p+0)) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_MONO)
                *p1 = ((255 - *(p)) < gThresh ? 0 : 255);
#else
#  error Unknown default pixel format defined in config.h
#endif
                p  += AR_PIX_SIZE_DEFAULT;
                p1++;
            }
        }
		// Tell GLUT the display has changed.
		glutPostRedisplay();
    }
}

static void Keyboard(unsigned char key, int x, int y)
{
	switch (key) {
		case 0x1B:
			eventCancel();
			break;
		case 'T':
		case 't':
			printf("Enter new threshold value (now = %d): ", gThresh);
			scanf("%d",&gThresh); while( getchar()!='\n' );
			printf("\n");
			break;
		case '1':
			gThresh -= 5;
			if (gThresh < 0) gThresh = 0;
			break;
		case '2':
			gThresh += 5;
			if (gThresh > 255) gThresh = 255;
			break;
		default:
			break;
	}
}

static void Quit(void)
{
	if (gClipImage) {
		free(gClipImage);
		gClipImage = NULL;
	}
	if (gArglSettings) arglCleanup(gArglSettings);
	if (gWin) glutDestroyWindow(gWin);
	arVideoClose();
	exit(0);
}

static void Idle(void)
{
	static int ms_prev;
	int ms;
	float s_elapsed;
	ARUint8 *image;
	
	// Find out how long since Idle() last ran.
	ms = glutGet(GLUT_ELAPSED_TIME);
	s_elapsed = (float)(ms - ms_prev) * 0.001;
	if (s_elapsed < 0.01f) return; // Don't update more often than 100 Hz.
	ms_prev = ms;
	
	// Grab a video frame.
	if (gStatus == 0) {
		if ((image = arVideoGetImage()) != NULL) {
			gARTImage = image;
			// Tell GLUT the display has changed.
			glutPostRedisplay();
		}	
	}
}

//
//	This function is called on events when the visibility of the
//	GLUT window changes (including when it first becomes visible).
//
static void Visibility(int visible)
{
	if (visible == GLUT_VISIBLE) {
		glutIdleFunc(Idle);
	} else {
		glutIdleFunc(NULL);
	}
}

//
//	This function is called when the
//	GLUT window is resized.
//
static void Reshape(int w, int h)
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	
	// Call through to anyone else who needs to know about window sizing here.
}

static void beginOrtho2D(int xsize, int ysize) {
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	gluOrtho2D(0.0, xsize, 0.0, ysize);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();	
}

static void endOrtho2D(void) {
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
}

static void Display(void)
{
    double         x, y;
    int            ssx, eex, ssy, eey;
    int            i;

	// Select correct buffer for this context.
	glDrawBuffer(GL_BACK);
	glClear(GL_COLOR_BUFFER_BIT);
    glDisable(GL_DEPTH_TEST);
	glDisable(GL_LIGHTING);
	glDisable(GL_TEXTURE_2D);
	beginOrtho2D(gXsize, gYsize);

    if (gStatus == 0) {
		
		arglDispImage(gARTImage, &gARTCparam, 1.0, gArglSettings);	// zoom = 1.0.
		arVideoCapNext();
		gARTImage = NULL;
		
    } else if (gStatus == 1) {
		
        arglDispImage(gPatt.savedImage[gPatt.loop_num-1], &gARTCparam, 1.0, gPatt.arglSettings[gPatt.loop_num-1]);
		// Draw red crosses on the points in the image.
        for (i = 0; i < point_num; i++) {
            x = gPatt.point[gPatt.loop_num-1][i].x_coord;
            y = gPatt.point[gPatt.loop_num-1][i].y_coord;
            glColor3f(1.0f, 0.0f, 0.0f);
            glBegin(GL_LINES);
              glVertex2d(x-10.0, (GLdouble)(gYsize-1)-y);
              glVertex2d(x+10.0, (GLdouble)(gYsize-1)-y);
              glVertex2d(x, (GLdouble)(gYsize-1)-(y-10.0));
              glVertex2d(x, (GLdouble)(gYsize-1)-(y+10.0));
            glEnd();
        }
		
		// Draw the current mouse drag clipping area.
        if (gDragStartX != -1 && gDragStartY != -1
			&& gDragEndX != -1 && gDragEndY != -1) {
			if (gDragStartX < gDragEndX) { ssx = gDragStartX; eex = gDragEndX; }
			else         { ssx = gDragEndX; eex = gDragStartX; }
			if (gDragStartY < gDragEndY) { ssy = gDragStartY; eey = gDragEndY; }
			else         { ssy = gDragEndY; eey = gDragStartY; }
#if 1
			if (gClipImage) {
				glPixelZoom(1.0f, -1.0f);	// ARToolKit bitmap 0.0 is at upper-left, OpenGL bitmap 0.0 is at lower-left.
				glRasterPos2f((GLfloat)(ssx), (GLfloat)(gYsize-1-ssy));
				glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
				glDrawPixels(eex-ssx+1, eey-ssy+1, GL_LUMINANCE, GL_UNSIGNED_BYTE, gClipImage);
			}
#else
            glColor3f(0.0f, 0.0f, 1.0f);
            glBegin(GL_LINE_LOOP);
              glVertex2i(gDragStartX, (gYsize-1)-gDragStartY);
              glVertex2i(gDragEndX, (gYsize-1)-gDragStartY);
              glVertex2i(gDragEndX, (gYsize-1)-gDragEndY);
              glVertex2i(gDragStartX, (gYsize-1)-gDragEndY);
            glEnd();
#endif
        }
		
    } else if (gStatus == 2) {
		
        arglDispImage(gPatt.savedImage[check_num], &gARTCparam, 1.0, gPatt.arglSettings[check_num]);
        for (i = 0; i < gPatt.h_num*gPatt.v_num; i++) {
            x = gPatt.point[check_num][i].x_coord;
            y = gPatt.point[check_num][i].y_coord;
            glColor3f(1.0f, 0.0f, 0.0f);
            glBegin(GL_LINES);
			glVertex2d(x-10.0, (GLdouble)(gYsize-1)-y);
			glVertex2d(x+10.0, (GLdouble)(gYsize-1)-y);
			glVertex2d(x, (GLdouble)(gYsize-1)-(y-10.0));
			glVertex2d(x, (GLdouble)(gYsize-1)-(y+10.0));
            glEnd();
        }
        draw_line();
		
    }

	endOrtho2D();
    glutSwapBuffers();
}

static void draw_line(void)
{
    double   *x, *y;
    int      max;
    // int      num; // unreferenced
    int      i, j, k, l;
    int      p;

    max = (gPatt.v_num > gPatt.h_num) ? gPatt.v_num : gPatt.h_num;
    arMalloc(x, double, max);
    arMalloc(y, double, max);

    i = check_num;

    for (j = 0; j < gPatt.v_num; j++) {
        for (k = 0; k < gPatt.h_num; k++) {
            x[k] = gPatt.point[i][j*gPatt.h_num+k].x_coord;
            y[k] = gPatt.point[i][j*gPatt.h_num+k].y_coord;
        }
        draw_line2(x, y, gPatt.h_num);
    }

    for (j = 0; j < gPatt.h_num; j++) {
        for (k = 0; k < gPatt.v_num; k++) {
            x[k] = gPatt.point[i][k*gPatt.h_num+j].x_coord;
            y[k] = gPatt.point[i][k*gPatt.h_num+j].y_coord;
        }
        draw_line2(x, y, gPatt.v_num);
    }

    for (j = 3 - gPatt.v_num; j < gPatt.h_num - 2; j++) {
        p = 0;
        for (k = 0; k < gPatt.v_num; k++) {
            l = j+k;
            if (l < 0 || l >= gPatt.h_num) continue;
            x[p] = gPatt.point[i][k*gPatt.h_num+l].x_coord;
            y[p] = gPatt.point[i][k*gPatt.h_num+l].y_coord;
            p++;
        }
        draw_line2(x, y, p);
    }

    for (j = 2; j < gPatt.h_num + gPatt.v_num - 3; j++) {
        p = 0;
        for (k = 0; k < gPatt.v_num; k++) {
            l = j-k;
            if (l < 0 || l >= gPatt.h_num) continue;
            x[p] = gPatt.point[i][k*gPatt.h_num+l].x_coord;
            y[p] = gPatt.point[i][k*gPatt.h_num+l].y_coord;
            p++;
        }
        draw_line2(x, y, p);
    }

    free(x);
    free(y);
}

static void draw_line2( double *x, double *y, int num )
{
    ARMat    *input, *evec;
    ARVec    *ev, *mean;
    double   a, b, c;
    int      i;

    ev     = arVecAlloc(2);
    mean   = arVecAlloc(2);
    evec   = arMatrixAlloc(2, 2);

    input  = arMatrixAlloc(num, 2);
    for (i = 0; i < num; i++) {
        arParamObserv2Ideal(dist_factor, x[i], y[i],
                             &(input->m[i*2+0]), &(input->m[i*2+1]));
    }
    if (arMatrixPCA(input, evec, ev, mean) < 0) exit(0);
    a =  evec->m[1];
    b = -evec->m[0];
    c = -(a*mean->v[0] + b*mean->v[1]);

    arMatrixFree(input);
    arMatrixFree(evec);
    arVecFree(mean);
    arVecFree(ev);

    draw_warp_line(a, b, c);
}

static void draw_warp_line(double a, double b , double c)
{
    double   x, y;
    double   x1, y1;
    int      i;

    glLineWidth(1.0f);
    glBegin(GL_LINE_STRIP);
    if (a*a >= b*b) {
        for (i = -20; i <= gYsize+20; i+=10) {
            x = -(b*i + c)/a;
            y = i;

            arParamIdeal2Observ(dist_factor, x, y, &x1, &y1);
            glVertex2f(x1, gYsize-1-y1);
        }
    } else {
        for(i = -20; i <= gXsize+20; i+=10) {
            x = i;
            y = -(a*i + c)/b;

            arParamIdeal2Observ(dist_factor, x, y, &x1, &y1);
            glVertex2f(x1, gYsize-1-y1);
        }
    }
    glEnd();
}

static void print_comment(int status)
{
    printf("\n-----------\n");
    switch(status) {
        case 0:
			printf("Press mouse button to grab first image,\n");
			printf("or press right mouse button or [esc] to quit.\n");
			break;
        case 1:
			printf("Press mouse button and drag mouse to rubber-bound features (%d x %d),\n", gPatt.h_num, gPatt.v_num);
			printf("or press right mouse button or [esc] to cancel rubber-bounding & retry grabbing.\n");
			break;
        case 2:
			printf("Press mouse button to save feature positions,\n");
			printf("or press right mouse button or [esc] to discard feature positions & retry grabbing.\n");
			break;
        case 4:
			printf("Press mouse button to grab next image,\n");
			printf("or press right mouse button or [esc] to calculate distortion parameter.\n");
			break;
        case 5:
			printf("Press right mouse button or [esc] to calculate distortion parameter.\n");
			break;
    }
}

static void save_param(void)
{
    char     name[256];
    ARParam  param;
    int      i, j;
	
    param.xsize = gXsize;
    param.ysize = gYsize;
    for (i = 0; i < 4; i++) param.dist_factor[i] = dist_factor[i];
    for (j = 0; j < 3; j++) {
        for (i = 0; i < 4; i++) {
            param.mat[j][i] = mat[j][i];
        }
    }
    arParamDisp( &param );
	
    printf("Filename: ");
    scanf( "%s", name );
    arParamSave( name, 1, &param );
	
    return;
}

