#include <stdio.h>
#include <iostream.h>
#include <stdlib.h>
#include <math.h>
#include "TsaiCal.h"

int main (int argc, char** argv)
{
    double    Xw,
              Yw,
              Zw,
              Xfd,
              Yfd;

    char      temp[256];

    if (argc != 2) {
	(void) fprintf (stderr, "syntax: %s datafile\n", argv[0]);
	exit (-1);
    }
    /* load up the camera parameters and calibration constants from the given data file */
	TsaiCal* proj = new TsaiCal(argv[1]);

    while (1) {
	/* prompt for the world coordinates */
	(void) fprintf (stdout, "\n Enter Xw [mm] : ");
	if (gets (temp) == NULL)
	    break;
	Xw = atof (temp);

	(void) fprintf (stdout, "\n Enter Yw [mm] : ");
	if (gets (temp) == NULL)
	    break;
	Yw = atof (temp);

	(void) fprintf (stdout, "\n Enter Zw [mm] : ");
	if (gets (temp) == NULL)
	    break;
	Zw = atof (temp);

	/* determine the corresponding image coordinates */
	proj->world_coord_to_image_coord (Xw, Yw, Zw, &Xfd, &Yfd);

	(void) fprintf (stdout,
			"\n    [Xw,Yw,Zw] = [%.2lf, %.2lf, %.2lf]  -->  [Xf,Yf] = [%.2lf, %.2lf]\n",
			Xw, Yw, Zw, Xfd, Yfd);
    }

    (void) fprintf (stdout, "\n\n");

    return 0;
}