/*
 * 
 * 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/ar.h>
#include <AR/video.h>
#include <AR/param.h>
#include <AR/gsub_lite.h>
#include "calib_cparam.h"

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

#define CALIB_CPARAM_DEBUG 0

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

/* set up the video format globals */

#if defined(__sgi)
char            *vconf = "-size=FULL";
#elif defined(__linux)
#  if defined(AR_INPUT_GSTREAMER)
char 			*vconf = "videotestsrc";
#  elif defined(AR_INPUT_V4L)
char            *vconf = "-width=640 -height=480";
#  elif defined(AR_INPUT_1394CAM)
char            *vconf = "-mode=640x480_YUV411";
#  elif defined(AR_INPUT_DV)
char            *vconf = "";
#  endif
#elif defined(_WIN32)
char			*vconf = "Data\\WDM_camera_flipV.xml";
#elif defined(__APPLE__)
char			*vconf = "-width=640 -height=480";
#else
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;
int             refresh;

static unsigned char   *gSaveARTImage = NULL;
static ARGL_CONTEXT_SETTINGS_REF gSaveArglSettings = NULL;

static ARParam         param;
static double          dist_factor[4];

int             line_num;
int             loop_num;
int             line_mode[LINE_MAX];
double          inter_coord[LOOP_MAX][LINE_MAX][LINE_MAX][3];
double          line_info[LOOP_MAX][LINE_MAX][4];

int             mode;
int             line_num_current;
int             loop_num_current;
double          theta;
double          radius;
double          gStartX, gStartY, gEndX, gEndY;

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

int             main(int argc, char *argv[]);
static int      init(int argc, char *argv[]);
static void		Keyboard(unsigned char key, int x, int y);
static void		Special(int key, int x, int y);
static void		Mouse(int button, int state, 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		drawPrevLine(void);
static void		drawNextLine(void);
static void		draw_warp_line(double sx, double ex, double sy, double ey);
static void		getCpara(void);
static void		intersection( double line1[4], double line2[4], double *screen_coord );


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("Camera calibration");
	
	// Setup argl library for current context.
	// Turn off distortion compensation.. we are calibrating.
	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;
	
	// Setup a space to save our captured image.
	if ((gSaveArglSettings = arglSetupForCurrentContext()) == NULL) {
		fprintf(stderr, "grabImage(): arglSetupForCurrentContext() returned error.\n");
		exit(-1);
	}
	arglDistortionCompensationSet(gSaveArglSettings, FALSE);
	arMalloc(gSaveARTImage, unsigned char, gXsize*gYsize*AR_PIX_SIZE_DEFAULT);

	// Register GLUT event-handling callbacks.
	// NB: Idle() is registered by Visibility.
    glutDisplayFunc(Display);
	glutReshapeFunc(Reshape);
	glutVisibilityFunc(Visibility);
	glutKeyboardFunc(Keyboard);
	glutSpecialFunc(Special);
    glutMouseFunc(Mouse);
	
	if (arVideoCapStart() != 0) {
    	fprintf(stderr, "init(): Unable to begin camera data capture.\n");
		return (FALSE);		
	}
	mode = 0;
    line_num_current = 0;
    loop_num_current = 0;

    glutMainLoop();
	return (0);
}

static int init(int argc, char *argv[])
{
    printf("Input center coordinates: X = ");
    scanf("%lf", &dist_factor[0]);
    while (getchar() != '\n');
    printf("                        : Y = ");
    scanf("%lf", &dist_factor[1]);
    while (getchar() != '\n');
    printf("Input distortion ratio: F = ");
    scanf("%lf", &dist_factor[2]);
    while (getchar() != '\n');
    printf("Input size adjustment factor: S = ");
    scanf("%lf", &dist_factor[3]);
    while (getchar() != '\n');

    initLineModel(&line_num, &loop_num, line_mode, inter_coord);

	// Open the video path.
    if (arVideoOpen(vconf) < 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 save image.
	arMalloc(gSaveARTImage, unsigned char, gXsize * gYsize * AR_PIX_SIZE_DEFAULT);
	
    param.xsize = gXsize;
    param.ysize = gYsize;
    param.dist_factor[0] = dist_factor[0];
    param.dist_factor[1] = dist_factor[1];
    param.dist_factor[2] = dist_factor[2];
    param.dist_factor[3] = dist_factor[3];

	return (TRUE);
}

static void thetaRadiusSet(void) {
	if (line_num_current == 0) {
		if (line_mode[line_num_current] == L_HORIZONTAL) {
			theta  = 90;
			radius = gYsize/2;
		} else {
			theta  = 0;
			radius = gXsize/2;
		}
	} else {
		if (line_mode[line_num_current] == L_HORIZONTAL) {
			if (line_mode[line_num_current] == line_mode[line_num_current-1]) {
				radius += 10.0;
			} else {
				theta  = 90;
				radius = gYsize/2;
			}
		} else {
			if (line_mode[line_num_current] == line_mode[line_num_current-1]) {
				radius += 10.0;
			} else {
				theta  = 0;
				radius = gXsize/2;
			}
		}
	}
}

static void eventCancel(void) {
	if (mode == 0) {
		// Cancel from mode 0 should quit program.
		arVideoCapStop();
		Quit();
	} else if (mode == 1) {
		if (line_num_current == 0) {
			// Cancel from mode 1, first line, should go back to mode 0.
			arVideoCapStart();
			mode = 0;			
		} else {
			// Cancel from mode 1, non-first line, should go back to first line.
			line_num_current = 0;
			thetaRadiusSet();
			glutPostRedisplay();
		}
	}
}

static void Mouse(int button, int state, int x, int y)
{
	ARUint8 *image;
	
	if (state == GLUT_DOWN) {
		if (button == GLUT_RIGHT_BUTTON) {
			eventCancel();
		} else if (button == GLUT_LEFT_BUTTON && mode == 0) {
			// Processing a new image.
			// Copy an image to saved image buffer.
			do {
				image = arVideoGetImage();
			} while (image == NULL);
			memcpy(gSaveARTImage, image, gXsize*gYsize*AR_PIX_SIZE_DEFAULT);
			printf("Grabbed image.\n");
			arVideoCapStop();
			mode = 1;
			line_num_current = 0;
			thetaRadiusSet();
			glutPostRedisplay();
		}
	}
}

static void Keyboard(unsigned char key, int x, int y)
{
    ARParam  iparam;
    double   trans[3][4];
    char     name[256];
    int      k = 1;

    if( mode == 0 ) return;

    switch( key ) {
        case 0x1B:
            gStartX = -1.0;
            gStartY = -1.0;
            gEndX = -1.0;
            gEndY = -1.0;
			// fall through..
        case 0x0D:
            line_info[loop_num_current][line_num_current][0] = gStartX;
            line_info[loop_num_current][line_num_current][1] = gStartY;
            line_info[loop_num_current][line_num_current][2] = gEndX;
            line_info[loop_num_current][line_num_current][3] = gEndY;
            line_num_current++;
			if (line_num_current < line_num) {
				thetaRadiusSet();
            } else {
				// Completed placements for this image.
                loop_num_current++;
                if (loop_num_current < loop_num) {
                    arVideoCapStart();
                    mode = 0;
                } else {
					// All done. Calculate parameter, save it, and exit.
					getCpara();
					arParamDecomp( &param, &iparam, trans );
					arParamDisp( &iparam );
					
					printf("Input filename: ");
					scanf("%s", name);
					arParamSave( name, 1, &iparam );
					
					Quit();
				}
            }
            break;
        default:
            k = 0;
            break;
    }
    if (k) glutPostRedisplay();
}


static void Special(int key, int x, int y)
{
    double   mx, my;
    int      k = 1;

    if (mode == 0) return;

    if (line_mode[line_num_current] == L_HORIZONTAL) {
        switch (key) {
            case GLUT_KEY_UP:
                radius -= 1.0;
                break;
            case GLUT_KEY_DOWN:
                radius += 1.0;
                break;
            case GLUT_KEY_LEFT:
                mx = (gStartX + gEndX)/ 2.0;
                my = (gStartY + gEndY)/ 2.0;
                theta -= 0.25;
                radius = cos( (double)(theta*3.141592/180.0) ) * mx
                       + sin( (double)(theta*3.141592/180.0) ) * my;
                break;
            case GLUT_KEY_RIGHT:
                mx = (gStartX + gEndX)/ 2.0;
                my = (gStartY + gEndY)/ 2.0;
                theta += 0.25;
                radius = cos( (double)(theta*3.141592/180.0) ) * mx
                       + sin( (double)(theta*3.141592/180.0) ) * my;
                break;
            default:
                k = 0;
                break;
        }
    } else {
        switch (key) {
            case GLUT_KEY_UP:
                mx = (gStartX + gEndX)/ 2.0;
                my = (gStartY + gEndY)/ 2.0;
                theta += 0.25;
                radius = cos( (double)(theta*3.141592/180.0) ) * mx
                       + sin( (double)(theta*3.141592/180.0) ) * my;
                break;
            case GLUT_KEY_DOWN:
                mx = (gStartX + gEndX)/ 2.0;
                my = (gStartY + gEndY)/ 2.0;
                theta -= 0.25;
                radius = cos( (double)(theta*3.141592/180.0) ) * mx
                       + sin( (double)(theta*3.141592/180.0) ) * my;
                break;
            case GLUT_KEY_LEFT:
                radius -= 1.0;
                break;
            case GLUT_KEY_RIGHT:
                radius += 1.0;
                break;
            default:
                k = 0;
                break;
        }
    }

    if (k) glutPostRedisplay();
}

static void Quit(void)
{
	if (gSaveArglSettings) arglCleanup(gSaveArglSettings);
	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 (mode == 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)
{
	// 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 (mode == 0) {
		
		arglDispImage(gARTImage, &gARTCparam, 1.0, gArglSettings);	// zoom = 1.0.
		arVideoCapNext();
		gARTImage = NULL;
		
    } else if (mode == 1) {

		arglDispImage(gSaveARTImage, &gARTCparam, 1.0, gSaveArglSettings);

        drawPrevLine();
        drawNextLine();
	}
	
	endOrtho2D();
    glutSwapBuffers();
}

static void drawPrevLine(void)
{
    int     i;

    glColor3f(0.0,0.0,1.0);
    for (i = 0; i < line_num_current; i++) {
        if (line_info[loop_num_current][i][0] == -1.0) continue;
        draw_warp_line(line_info[loop_num_current][i][0], line_info[loop_num_current][i][2],
                       line_info[loop_num_current][i][1], line_info[loop_num_current][i][3]);
    }
}

static void drawNextLine(void)
{
    double sin_theta, cos_theta;
    double x1, x2;
    double y1, y2;

    sin_theta = sin( (double)(theta*3.141592/180.0) );
    cos_theta = cos( (double)(theta*3.141592/180.0) );

    if (cos_theta != 0) {
        x1 = radius / cos_theta;
        x2 = (radius - (gYsize-1)*sin_theta) / cos_theta;
    } else {
        x1 = x2 = -1.0;
    }

    if (sin_theta != 0) {
        y1 = radius / sin_theta;
        y2 = (radius - (gXsize-1)*cos_theta) / sin_theta;
    } else {
        y1 = y2 = -1.0;
    }

    gEndY = -1;
    if (x1 >= 0 && x1 <= gXsize-1) {
         gStartX = x1;
         gStartY = 0;
         if (x2 >= 0 && x2 <= gXsize-1) {
             gEndX = x2;
             gEndY = gYsize-1;
         } else if (y1 >= 0 && y1 <= gYsize-1) {
             gEndX = 0;
             gEndY = y1;
         } else if (y2 >= 0 && y2 <= gYsize-1) {
             gEndX = gXsize-1;
             gEndY = y2;
         } else {
			 printf("???\n");
		 }
    } else if (y1 >= 0 && y1 <= gYsize-1) {
         gStartX = 0;
         gStartY = y1;
         if (x2 >= 0 && x2 <= gXsize-1) {
             gEndX = x2;
             gEndY = gYsize-1;
         } else if (y2 >= 0 && y2 <= gYsize-1) {
             gEndX = gXsize-1;
             gEndY = y2;
         } else {
			 printf("???\n");
		 }
    } else if (x2 >= 0 && x2 <= gXsize-1) {
         gStartX = x2;
         gStartY = gYsize-1;
         if (y2 >= 0 && y2 <= gYsize-1) {
             gEndX = gXsize-1;
             gEndY = y2;
         } else {
			 printf("???\n");
		 }
    }

    glColor3f(1.0,1.0,1.0);
    if (gEndY != -1) {
        draw_warp_line(gStartX, gEndX, gStartY, gEndY);
    }
}

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

    a = ey - sy;
    b = sx - ex;
    c = sy*ex - sx*ey;

    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 getCpara(void)
{
    double  *world_coord;
    double  *screen_coord;
    int     point_num;
    int     i, j, k;

    point_num = 0;
    for (k = 0; k < loop_num; k++) {
        for (i = 0; i < line_num; i++) {
            for (j = 0; j < line_num; j++) {
                if (inter_coord[k][i][j][0] != -10000.0) point_num++;
            }
        }
    }
	arMalloc(world_coord, double, point_num * 3);
	arMalloc(screen_coord, double, point_num * 2);

    point_num = 0;
    for (k = 0; k < loop_num; k++) {
        for (i = 0; i < line_num; i++) {
            if (line_info[k][i][0] == -1.0) continue;
            for (j = 0; j < line_num; j++) {
                if (line_info[k][j][0] == -1.0) continue;
                if (inter_coord[k][i][j][0] == -10000.0) continue;

                world_coord[point_num*3+0] = inter_coord[k][i][j][0];
                world_coord[point_num*3+1] = inter_coord[k][i][j][1];
                world_coord[point_num*3+2] = inter_coord[k][i][j][2];
                intersection(line_info[k][i], line_info[k][j],
                             &(screen_coord[point_num*2]));

                point_num++;
            }
        }
    }
    printf("point_num = %d\n", point_num);
    if (arParamGet((double (*)[3])world_coord, (double (*)[2])screen_coord, point_num, param.mat) < 0) {
        printf("ddd error!!\n");
        exit(0);
    }

    free(world_coord);
    free(screen_coord);
}

static void intersection(double line1[4], double line2[4], double *screen_coord)
{
    double a, b, c, d, e, f, g;

    a = line1[1] - line1[3];
    b = line1[2] - line1[0];
    c = line1[0] * a + line1[1] * b;
    d = line2[1] - line2[3];
    e = line2[2] - line2[0];
    f = line2[0] * d + line2[1] * e;

    g = a*e - b*d;
    if (g == 0.0) { printf("???\n"); exit(0); }

    screen_coord[0] = (c * e - b * f) / g;
    screen_coord[1] = (a * f - c * d) / g;
}
