/*
  Last edited on 2003-12-23 15:53:41 by stolfi

  This program allows a user to interactively
  convert 3D world coordinates into 2D image
  coordinates. The perform the conversion the
  program requires a calibrated camera model of
  the form generated by ccal, ccal_fo, ncal, or
  ncal_fo.

  History
  -------

  01-Apr-95  Reg Willson (rgwillson@mmm.com) at 
        3M St. Paul, MN Filename changes for DOS port.

  30-May-93  Reg Willson (rgw@cs.cmu.edu) at 
        Carnegie-Mellon University Original implementation.

*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cal_main.h>

#define MAXLIN 256

main (int argc, char **argv)
{
  char *cal_file_name, *dat_file_name;

  FILE *cal_fd, *dat_fd;
  double Xw, Yw, Zw, Xfd, Yfd;
  char *rest;
  int interactive;
  char temp[256];

  if (argc == 2) 
    { cal_file_name = argv[1];
      dat_file_name = NULL;
      interactive = 1;
    }
  else if (argc == 3) 
    { cal_file_name = argv[1];
      dat_file_name = argv[2];
      interactive = 0;
    }
  else
    { fprintf 
        ( stderr, "syntax: %s cal_file [dat_file]\n",
          argv[0]
        );
      exit(1);
    }
    
  /* load up the camera calibration parameters: */
  fprintf
    ( stderr, "\n Camera calibration file: %s\n\n",
      cal_file_name
    );
  if ((cal_fd = fopen (cal_file_name, "r")) == NULL)
    { fprintf
        ( stderr, "%s: unable to open file \"%s\"\n",
          argv[0], cal_file_name
        );
      exit(1);
    }
  load_cp_cc_data (cal_fd, &cp, &cc);
  fclose (cal_fd);
  print_cp_cc_data (stdout, &cp, &cc);

  /* Process points: */
  if (interactive)
    { 
      while (1)
        {
          /* Prompt for and get the world coordinates: */
          fprintf(stdout, "\n Enter Xw [mm] : ");
          if (fgets(temp, MAXLIN, stdin) == NULL) break;
          Xw = strtod(temp, &rest);

          fprintf(stdout, "\n Enter Yw [mm] : ");
          if (fgets(temp, MAXLIN, stdin) == NULL) break;
          Yw = strtod(temp, &rest);

          fprintf(stdout, "\n Enter Zw [mm] : ");
          if (fgets(temp, MAXLIN, stdin) == NULL) break;
          Zw = strtod(temp, &rest);

          /* Map it to image coordinates: */
          fprintf
            ( stdout, "\n    [%.2lf, %.2lf, %.2lf]",
              Xw, Yw, Zw
            );
          world_coord_to_image_coord
            ( Xw, Yw, Zw, &Xfd, &Yfd);

          fprintf
            ( stdout, "  -->  [%.2lf, %.2lf]\n",
              Xfd, Yfd
            );
        }
      fprintf(stdout, "\n\n");
    }
  else
    { 
      fprintf
        ( stderr, "\n Point file: %s\n\n",
          dat_file_name
        );
      if ((dat_fd = fopen (dat_file_name, "r")) == NULL)
        { fprintf
            ( stderr, "%s: unable to open file \"%s\"\n",
              argv[0], dat_file_name
            );
          exit(1);
        }
      while (1)
        {
          /* Read the world coordinates of a point: */
          if (fgets (temp, MAXLIN, dat_fd) == NULL) break;
          rest = temp;
          Xw = strtod(rest, &rest);
          Yw = strtod(rest, &rest);
          Zw = strtod(rest, &rest);

          /* Map to image coordinates: */
          fprintf
            ( stdout, "\n    [%.2lf, %.2lf, %.2lf]",
              Xw, Yw, Zw
            );
          world_coord_to_image_coord
            ( Xw, Yw, Zw, &Xfd, &Yfd);

          fprintf
            ( stdout, "  -->  [%.2lf, %.2lf]\n",
              Xfd, Yfd
            );
        }
      fprintf(stdout, "\n\n");
      
      fclose (dat_fd);
    }

  return 0;
}