#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <pbm.h>
#include <pgm.h>
#include <assert.h>
#include <cmacs.h>
#include <fishhead.h>

int main(argc, argv)
int argc;
char *argv[];
{
    gray **picin;  	/* raw incoming picture */
    gray **picout;	/* rectified outgoing picture */
    int	Ph,	/* Picture height, pixels (576 or 480 typical) */
        Pw;	/* Picture width, pixels  (768 or 640 typical) */
    Calib Cal;  /* calibration parameters and pointers */
    int time1, time2 ;

    int m;  gray maxval;  int rows, cols;
    FILE *fp;  char img[100], cal[100];  int imgl;	     /*  image name */

    if (argc < 2 || argc > 3)
      {
	  printf("Usage: fastfish image.pgm <cam.calib>\n");
	  printf("image.pgm contains a picture to be rectified\n");
	  printf("using the 'flatfish' calibration parameters in cam.calib\n");
	  printf("if cam.calib is not given, the program tries image.calib\n");
	  
	  exit(0);
      };

    strcpy(img,argv[1]); imgl = strlen(img);
    if (imgl>5) if (img[imgl-4]=='.') imgl -= 4;
    img[imgl] = 0;

    pgm_init(&argc, argv);

    /* Read in raw image */

    fp = fopen(argv[1],"r");
    if (fp == NULL) {printf("No %s file!!\n",argv[1]); exit(0); };
    picin = pgm_readpgm(fp, &cols, &rows, &maxval );
    fclose(fp);
    Pw = cols;   Ph = rows;  
    if (maxval > 255)
      { printf("Digitized pic pixels too big: %d instead of %d!!\n",
	       maxval,255);  exit(0); };
    
    if (argc >= 3) strcpy(cal,argv[2]); else strcpy(cal,strcat(img,".calib"));
    img[imgl] = 0;  /* reset file name to base */

    printf(argv[1]);  printf("\n");
    printf(" %d high by %d wide, max pixel %d\n\n",Ph,Pw,maxval);

    /* Create display image array */
    picout = pgm_allocarray(Pw,Ph);

	
    time1 = clock() ;  /* time the rectification precalculation */
 
   if (!getfish(cal, &Cal, picin)) 
      {printf("%s calibration file didn't work!\n",cal); exit(0); };
	
    time2 = clock() ;
    printf("Rectification setup time: %g s\n", (time2 - time1)/1000000.0);

    if (Cal.Ph != Ph || Cal.Pw != Pw)
      printf("Calibration size [%d,%d] different from Image size [%d,%d]\n",
	     Cal.Ph,Cal.Pw,Ph,Pw);

    if (Cal.Yaim>10 || Cal.Yaim<-10 || Cal.Xaim>15 || Cal.Xaim<-15 
	|| Cal.Distance<3 || Cal.Distance>50 || Cal.FOV < 10 || Cal.FOV > 180)
      {
	  printf("(Yaim %.3g Xaim %.3g Distance %.3g FOV %.3g) seem unreasonable\n",
		 Cal.Yaim, Cal.Xaim, Cal.Distance, Cal.FOV);
	  exit(0);
      };

    time1 = clock();    /* time the rectification - needs to be fast */
    for (m = 0; m<100; m++) /* repeat, to average cache effects */
  	  applyfish(picin, &Cal,picout);
  
    time2 = clock();
    printf("Rectification Time %g seconds\n",(time2 - time1)/1e8);

    fp = fopen(strcat(img,".frect.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,Pw,Ph,256,0);
    fclose(fp);
 
    exit(0);
};