/* Major update 11/99 to clean up scaling, spot finding and other steps
and add multiple rectification iterations.  Distorted spots in wide angle
views should be easier to find in partially recified images.
12/99
Added a pre-rectification option for better initial spot
finding. Program doesn't breath hard even on 120 deg images.
 -- hpm */

/* To Do List 11/99

$ Make local maximum test neighborhood proportional to spot size

$ Include a "don't care" ring between spot and background in spot template

$ Tighten range for adjacent spot location in spot linking code
  (Skip unlinked spots in spot-coordinate calculations!!!! - fixed)

$ Replace rectification S->P polynomial by inverse of P->S polynomial
  to improve extrapolation properties.
  (Substituted atan basis function that excellently models lens instead)

$ Expand horizontal/vertical spot linking to diagonals

$ Fix redistort display

$ Investigate why red curve, green inverse curve overlap colors cut out

$ save first calibration, but replace whenever #points grows by more than 4
  even if error rises

$ stop iterations if function coeffs become too large

$ save rectified pixel coords when calculating original coords

$ Replace spotpixelcoord from simple bilinear to plane fit on
  distance-weighted rectified pixel coordinates
  (in later iterations, simply use prior rectification)
  ** All above replaced by adding pixel center values to
  scatter-minimization search

$ Display center crosses on rectified pixel location

* Make spot size range in iteration 0 proportional to tan(FOV/2)

* Eliminate unnecessary display images with Gaudy=0 switch

*/



/* flatfish.c  last updated 8/10/1999 mcm - scale pixels amounts by
   image width */

/* First version completed 4/96 at DB Berlin.  -- hpm */

/* Radial distortion finder and rectifier for solid state cameras
   with wide angle lenses. Created 5/95
                              Hans Moravec <hpm@cmu.edu>
                              Carnegie Mellon University
			      Pittsburgh, PA  15213

   In calibration phase, the program assumes the camera
   is perpendicularly aimed at a square, horizontally oriented,
   grid of round dark spots, with an additional spot at a half
   grid position near the center:

   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * *_* * * * * * * * * * * *
   * * * * * * * * * * * *^* * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *

   (the center "spot" made of an underbar paired with a hat is
   supposed to look like all the others, represented by stars,
   and all spots should be black circular disks: excuse the
   limitations of ascii graphics).  The program was developed
   with a test pattern made of a magnetic white board 4 feet high
   and six feet wide, covered with a 16 x 24 + 1 grid of dark
   magnetic disks 1.5 inches in diameter at 3 inch spacing.
   The object of the calibration was a stereo bar of three Sony
   XC-75 black and white cameras with 90 degree wide angle lenses
   exhibiting strong fish eye distortion, with a radial image
   compression factor between the center and the edges of at least
   2:1. The cameras were mounted parallel at 15 cm spacing, and
   connected to the R, G and B channels of a color digitizer. The
   camera bar was placed so the lens iris rings were 22.5 inches
   in front of the calibration pattern.  This filled each camera's
   field of view with a majority of the pattern.   The center spot
   was approximately centered in the "green" camera's image, to
   the left of center in the "blue" image and to the right in the
   "red" image.
 
   The program finds the spots, in pixel (distorted) and spot grid
   (ideal) coordinates, and finds a least-squares best fit distortion
   center, assumed to indicate lens axis, a digitizer aspect ratio,
   and a distortion correcting function that maps distorted to ideal
   radius from that center.  In our setup the result RMS deviation for
   all (approximately 300/image) spots is better than 0.5 pixel.

   In our setup, we note that the lens center between calibrations
   jitters only a few hundredths of a pixel.

   As of 11/95 the program could rectify images from the cameras using
   the distortion polynomial into ones with ideal projective geometry.

   The program is not complete, as of 3/96.  Bugs and missing:

   When the distorion causes a corner spot to appear in the calibration
   image whose horizontal and vertical neighbors are off the edge of the
   image, the program often links it with a diagonal neighbor, giving it
   the wrong ideal coordinates.  Tests need to be added to handle this
   situation. (fixed 3/15/96 - hpm).

   The center spot is intended to relate the three cameras, in a not
   yet implemented later calibration step, which produces a full
   geometric camera calibration.  (completed 4/24/96 - hpm)

*/


/* The Great Aspect Ratio Saga

   Concerning aspect ratio of Sony XC-75 camera module in conjunction
   with our K2T digitizer: the camera specs say the camera pixels are
   8.4 um horizontal by 9.8 um vertical.  Horizontal clock rate is
   14.318 MHz.  Vertical drive frequency is 15.734 kHz +- 1%.
   The camera has 768 horizontal by 494 vertical pixels.
   Sensing area is 1/2 inch wide.  (Optical blank 43 elements
   on each horizontal line?)  EIA system.  Chip size is 7.95 (h) by
   6.45 (v) mm.  525 lines.
---
    Turns out that the resolution of AQDOTFREQ in the k2t package
    is limited  - I think it's 80ns and the 12.2 MHz is really 12.256
    which is as close as we can get to perfectly square...   Bill Ross

12.256 makes the horizontal pixel size 9.813, vs.  9.8 um vertical, an
error of only 0.135 percent, about 0.86 pixels total in the horizontal
direction, if you use the vertical as the reference.  That's
good, it means that there is no effect on things like template
matches, where the template spans only a fraction of the image, and
thus has a maximum aspect stretch much less than a pixel.  In numeric
computations involving pixel coordinates, I can now multiply in the
aspect ratio of 1.001352107, and have complete peace of mind.
A camera solver would never have been able to come up with an
estimate that accurate.  

hpm 9/95 Hah!!!! Minimizing over aspect ratio with the distortion-finding
 code gives a repeatable aspect ratio of 1.020 +/- .0006.  Using that
 found aspect ratio reduces the rms error of the best fit curve of spot
 index radius to pixel radius to about 0.5 pixel, compared to much worse
 2.5 pixels using the calculated aspect ratio.  Another good intention
 leads to hell!

*/


static int letterdefs[340] ={
	00000000000, 00000000000,	/* null */
	00010101010, 05234100000,	/* 1 downarrow */
	00000003244, 04444320000,	/* 2 alpha */
	00000003442, 07442744040,	/* 3 beta */
	00000001024, 04200000000,	/* 4 or */
	00000000076, 00200000000,	/* 5 not */
	00000001420, 03420140000,	/* 6 epsilon */
	00000007624, 02424240000,	/* 7 pi */
	00000404020, 01024420000,	/* 8 lambda */
	07020202000, 01412141214,	/* tab */
	04040407000, 01610141000,	/* lf */
	00004121010, 01010502000,	/* vt */
	00010107610, 01076000000,	/* ff */
	03440403400, 03422342222,	/* cr */
	00000002452, 05224000000,	/* 16 infinity */
	00030040236, 04242340000,	/* 17 partial */
	00000364040, 04036000000,	/* 20 subset */
	00000740202, 00274000000,	/* 21 superset */
	00000344242, 04200000000,	/* 22 intersection */
	00000424242, 03400000000,	/* 23 union */
	00042427642, 02424100000,	/* 24 forall */
	00076020236, 00202760000,	/* 25 thereexists */
	00000346652, 06634000000,	/* 26 circlestar */
	00010047604, 01020762010,	/* 27 doublearrow  */
	00000000000, 00000000076,	/* underbar */
	00000100476, 00410000000,	/* 31 rightarrow */
	03254000000, 00000000000,	/* 32 tilde */
	00002047610, 07620400000,	/* 33 not equal */
	00004102010, 00400340000,	/* 34 lt or eq */
	00020100410, 02000340000,	/* 35 gt or eq */
	00000760076, 00076000000,	/* 36 equivalence */
	00000004224, 01000000000,	/* 37 or */
	00000000000, 00000000000,	/* space */
	00010101010, 01000100000,	/* ! */
	02424240000, 00000000000,	/* " "*/
	00000247624, 02476240000,	/* # */
	01034525034, 01252341000,	/* $ */
	00076620410, 02046460000,	/* % */
	00020505020, 05244320000,	/* & */
	03030600000, 00000000000,	/* ' */
	00002041010, 01004020000,	/* ( */
	00040201010, 01020400000,	/* ) */
	00010523410, 03452100000,	/* * */
	00000101076, 01010000000,	/* + */
	00000000000, 00030306000,	/* , */
	00000000076, 00000000000,	/* - */
	00000000000, 00030300000,	/* . */
	00000020410, 02040000000,	/* / */
	00034424652, 06242340000,	/* 0 */
	00010301010, 01010340000,	/* 1 */
	00034420204, 01020760000,	/* 2 */
	00034420214, 00242340000,	/* 3 */
	00004142444, 07604040000,	/* 4 */
	00076407402, 00242340000,	/* 5 */
	00014204074, 04242340000,	/* 6 */
	00076020404, 01010100000,	/* 7 */
	00034424234, 04242340000,	/* 8 */
	00034424236, 00204300000,	/* 9 */
	00000003030, 00030300000,	/* : */
	00000003030, 00030306000,	/* ; */
	00000041020, 01004000000,	/* < */
	00000007600, 07600000000,	/* = */
	00000201004, 01020000000,	/* > */
	00034420410, 01000100000,	/* ? */
	00034425652, 05640340000,	/* @ */
	00034424276, 04242420000,	/* A */
	00074424274, 04242740000,	/* B */
	00034424040, 04042340000,	/* C */
	00074222222, 02222740000,	/* D */
	00076404074, 04040760000,	/* E */
	00076404074, 04040400000,	/* F */
	00034424040, 04642340000,	/* G */
	00042424276, 04242420000,	/* H */
	00034101010, 01010340000,	/* I */
	00002020202, 00242340000,	/* J */
	00042445060, 05044420000,	/* K */
	00040404040, 04040760000,	/* L */
	00042665242, 04242420000,	/* M */
	00042426252, 04642420000,	/* N */
	00034424242, 04242340000,	/* O */
	00074424274, 04040400000,	/* P */
	00034424242, 05244320000,	/* Q */
	00074424274, 05044420000,	/* R */
	00034424034, 00242340000,	/* S */
	00076101010, 01010100000,	/* T */
	00042424242, 04242340000,	/* U */
	00042424242, 02424100000,	/* V */
	00042424242, 05266420000,	/* W */
	00042422410, 02442420000,	/* X */
	00042422410, 01010100000,	/* Y */
	00076020476, 02040760000,	/* Z */
	01610101010, 01010101600,	/* [ */
	00000402010, 00402000000,	/* \ */
	07010101010, 01010107000,	/* ] */
	00010345210, 01010100000,	/* 136 uparrow */
	00000102076, 02010000000,	/* 137 leftarrow */
	01414060000, 00000000000,	/* ` */
	00000003402, 03642360000,	/* a */
	00040407442, 04242740000,	/* b */
	00000003442, 04040360000,	/* c */
	00002023642, 04242360000,	/* d */
	00000003442, 07440340000,	/* e */
	00014222070, 02020200000,	/* f */
	00000003442, 04242360234,	/* g */
	00040407442, 04242420000,	/* h */
	00000100010, 01010100000,	/* i */
	00000020002, 00202024234,	/* j */
	00040404244, 07044420000,	/* k */
	00010101010, 01010100000,	/* l */
	00000006452, 05252520000,	/* m */
	00000005462, 04242420000,	/* n */
	00000003442, 04242340000,	/* o */
	00000007442, 04242744040,	/* p */
	00000003442, 04242360202,	/* q */
	00000005462, 04040400000,	/* r */
	00000003640, 03402740000,	/* s */
	00010107610, 01010060000,	/* t */
	00000004242, 04242340000,	/* u */
	00000004242, 04224100000,	/* v */
	00000004242, 05252240000,	/* w */
	00000004224, 01024420000,	/* x */
	00000004242, 04224102040,	/* y */
	00000007604, 03420760000,	/* z */
	00204040410, 00404040200,	/* { */
	01010101010, 01010101010,	/* | */
	04020202010, 02020204000,	/* alt (quad) */
	00010102442, 02410100000,	/* ~ */
	07777777777, 07777777777,	/*  */
	00036424236, 01222420000,	/*  */
	00000003642, 03622420000,	/*  */
	00034420276, 00242340000,	/*  */
	00000003442, 01642160000,	/*  */
	00042424236, 00202020000,	/*  */
	00000004242, 03602020000,	/*  */
	00044525272, 05252440000,	/*  */
	00000004452, 07252440000,	/*  */
	00052525252, 05252760000,	/*  */
	00000005252, 05252760000,	/*  */
	00052525252, 05252760600,	/*  */
	00000005252, 05252760600,	/*  */
	00042424242, 04242760600,	/*  */
	00000004242, 04242760600,	/*  */
	00076222222, 04242420000,	/*  */
	00000007622, 02242420000,	/*  */
	00000006060, 03422340000,	/*  */
	00042424272, 04646720000,	/*  */
	00000004242, 07246720000,	/*  */
	00042424652, 06242420000,	/*  */
	00000004246, 05262420000,	/*  */
	03400424246, 05262420000,	/*  */
	00034004246, 05262420000,	/*  */
	00076424040, 04040400000,	/*  */
	00000007642, 04040400000,	/*  */
	00076222222, 04242764200,	/*  */
	00000007622, 02242764200,	/*  */
	00000004040, 07442740000,	/*  */
	00052525234, 05252520000,	/*  */
	00000005252, 03452520000,	/*  */
	00074404074, 04242740000,	/*  */
	00000003440, 07442740000,	/*  */
	00034103442, 03410340000,	/*  */
	00000101034, 04234101000,	/*  */
	00074020276, 00202760000,	/*  */
	00000007402, 07602740000,	/*  */
	02476404074, 04040760000,	/*  */
	00024003442, 07440340000,	/*  */
	00000007442, 07442740000,	/*  */
	00076242424, 02424240000,	/*  */
	00000004242, 07642420000,	/*  */
	00000004450, 06050440000 	/*  */ };

#define CompareCenters 1 /* whether to check out old Spot->Pixel converters */
#define IDline  "Calibration File Version Number: 3.0"

#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "ppm.h"
#include <assert.h>

clock_t  timeA;     /* often-needed time variable */

#define PGM_ASSIGN(p,v) (p = (v)) /* for symmetry with PPM_ASSIGN */

int SPOTSACROSS = 25;  /* approximate number of horizontal spot spacings across image */
int SPOTRADIUS = 15;
int SPOTRADIUSMIN = 5;	/* minimum radius of spot to consider */
int SPOTRADIUSMAX = 18;	/* maximum radius of spot to consider */

int DISTORPOLYDEG = 6;	/* number of coefficients of radial distortion function */

#define PhMax	1000  /* Maximum picture height */
#define PwMax	2000  /* Maximum picture width */
#define Pad 100  /* Picture padding border, pixels */
#define Sad 50   /* How far beyond boundaries to allow spot centers */
#define Paspect 1.000 /* Width/height ratio of pixels */

char PreRectify = 1;	/* 1 to blindly pre-rectify image using atan assumption */
int IdentityMap = 0;	/* 1 to leave picture unchanged in current rectification */
int Iteration = 0;	/* Each rectification iteration uses last to better find spots */
int BasisSet = 0 ;	/* Which basis functions to use in least squares fit */
double FitRad = 1.0;  	/* Radius in spot spacings of neighborhood for SpotToPixelSimple */

int weirdcount = 0, clashcount = 0;
/* in spot linking, count of aberrant local link coordinates and number
   of attempted spot coord labellings inconsistent with previous labellings */

gray **picin;  		/* raw incoming picture */
pixel **picout;		/* outgoing display picture */
#define PICOUT_(I,J) picout[(I)+Pad][(J)+Pad]
pixel **curves;		/* color curve fit display picture */
#define CURVES(i,j) curves[Curveswid-1-(i)][j]
int Curveswid;		/* size of Curves image */

gray **graphout;	/* outgoing plot picture */
int	Ph,		/* Picture height, pixels (576 or 480 typical) */
        Pw;		/* Picture width, pixels  (768 or 640 typical) */

unsigned char **picpad;    /* working image, with border padding */     

/* IMGid specifies the destination image for Text and perhaps other
   graphic routines */
#define IMGpicin 0     /* specifies picin as graphic destination */
#define IMGpicout 1    /* specifies picout as graphic destination */
#define IMGgraphout 2  /* specifies graphout as graphic destination */
#define IMGpicpad 3    /* specifies picpad as graphic destination */
#define IMGcurves 4    /* specifies curves as graphic destination */
int IMGid = IMGpicout;


#ifndef PI
#define PI 3.14159265358979323  /* Pies are round */
#endif


float **flP;  /* raster spotness value */
unsigned char **chP;  /* raster spotness radius */

int MaxNspots = 2000;   /* maximum number of spots expected + safety margin */
int Nspots = 0;		/* number of spots found */
int NspotsLinked = 0;	/* number of spots linked into grid */
int MaxHood = 12;	/* neighborhood size for adjacent spot candidates */
#define Adjacencies 8   
/* immediate neighbors: horizontal, vertical are 4 and maybe 2 diagonals make 8 */
int Inb[8] = {1,-1,0,0,1,-1,1,-1},  Jnb[8] = {0,0,1,-1,1,-1,-1,1};
/* displacement of the eight adjacent spots, in I and J spot coords */

double *Ispots, *Jspots;   /* stores spot's pixel coordinates */
double *Vspots, *Rspots;  /* spotness value, radius lists */
double *IIspots, *IJspots, *JIspots, *JJspots; /* spot local coordinate system */
double *Itemp, *Jtemp, *Ktemp, *Ltemp;  /* working storage for spot least-square fits */
/* IIspots and JJspots also used later to store spot's spot coordinates
 IJspots and JIspots to store spot's rectified pixel coordinates */

int *Aspots;  /* spot coordinate status: -1 untouched, 0 inited, 1 done 2 linked */

float **Dspots;  int **DPspots, **DPerror;
int Cspots, Pspots;   /*  Pattern and image center spots */
float *Dsp;    int *DPsp;  /* Arrays for spot distance sort */


double SpotSpacing, FOVrad;
float Yaim = 0.0, Xaim = -2.62, Distance = 8.5, FOV = 90.0;
/* Daimler Benz old camera calibration setup 
      Yaim = 0.0, Xaim = 0.0, Distance = 40.33, FOV = 30.0; */
/* CMU red camera
      Yaim = 0.0, Xaim = -2.62, Distance = 8.5, FOV = 90.0; */

#define CalibPNMax 100    /* Scatter polynomial maximum size */
double CalibPi=240.0, CalibPj=320.0, CalibAS = 1.0;
/* Calibrated pixel image center and aspect ratio */
double CalibSi = 0.0, CalibSj = 0.0;   /* Calibrated spot image center */
/* Set up distortion polynomial for identity */
int CalibPN = 1;  /* Distortion function degree */
double CalibP[CalibPNMax] = {1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
			     0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
/* Spot->Pixel radius scatter plot function */
double CalibS[CalibPNMax] = {1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
			     0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
/* Pixel->Spot radius scatter plot function */
double CalibAN = 0.0;	     /* Angle of the gid image from horizontal (vertical) */

/* Map_Spot_Pixel_Setup set versions of distortion parameters */
int CalibPN_s;
double CalibP_s[CalibPNMax], CalibS_s[CalibPNMax]; 
double SpotSpacing_s, Yaim_s, Xaim_s;
double CalibSj_s, CalibSi_s;
double CalibAN_s, CalibAS_s, rootCalibAS_s;
double CalibPj_s, CalibPi_s;
int Ph_s, Pw_s;

double Poly[CalibPNMax];  /* Snapshot of "fitbasis" function */
double isc, jsc;     /* Image axis center */
double ipc, jpc, res, aspect, rootaspect; 
double bestisc, bestjsc, bestipc, bestjpc;
double bestfit;       /* RMS scatter from most recent minimization */
double bestaspect, Bestbestfit;
int dpass;

/* indexed color map, reflecting some old conventions */
#define x 255
#define y 128
unsigned char cmap[15][3] =
{{0,0,0},{x,0,0},{0,x,0},{0,0,x},{0,x,x},
 {x,0,x},{x,x,0},{x,x,x},{x,y,0},{x,0,y},
 {y,x,0},{y,0,x},{0,x,y},{0,y,x},{y,y,y}};
#undef x
#undef y

/* spot pixels, from center out, maximally balanced */
#define SPX 5000	/* maximum length of prototype spot pixel list */
#define ROX 100		/* maximum spot prototype radius */
int spi[SPX], spj[SPX];	/* i and j coords of the pixels */
int sprpos[ROX];	/* starting index for each whole pixel ring */
int spn = 0;		/* actual number of pixels in list */
int sprmax = 0;		/* maximum spot radius in list */


#define IMGNMAX 200	/* maximum length of calibration image file names */
char img[2*IMGNMAX];  int imgn;  int imgl;	     /* calibration image name */
char imgname[2*IMGNMAX];  /* extra copy of name to annotate displays */

FILE *logfile;
char slog[500];  /* logfile, slog: file and string for run log output */

void plog(void);
void plog(void)      /* write out message in log file and stdout */
{   fprintf(logfile,"%s",slog); printf("%s",slog);   }

void llog(char *s);
void llog(char *s) /* write fixed string in log file */
{   fprintf(logfile,s); printf(s);   }


void Text(int kolor, int ip, int jp, char* txt);
void Text(int kolor, int ip, int jp, char* txt)
     /* insert text txt into image at ip jp into IMGid specified image */
{ 
  char *tc;
  int i, j, l, ic, jc, jl, icp, jcp;

  tc=txt;  ic = ip;  jl = jc = jp;

  while ((i = *tc) != '\0')
    {
      if (i == '\n') { jc = jl;  ic += 11; }
      else
	if (i == '\b') jc -= 7; else
	  if (i == '\v') ic -= 11; else
	    if (i == ' ')  jc += 7; else
	      {
		icp = ic - 9;
		for (l=0; l<=1; ++l)
		  for (j=0; j<30; ++j)
		    {
		      if ((j % 6) == 0) { icp++;  jcp = jc; }
		      if (((1 << (29-j)) & letterdefs[2*i+l]) != 0)
			switch (IMGid)
			  {
			  case IMGpicout:
			    if ( jcp >= -Pad && jcp < Pw+Pad &&
				 icp >= -Pad && icp < Ph+Pad )
			      PPM_ASSIGN(PICOUT_(icp,jcp),cmap[kolor-1][0],
					 cmap[kolor-1][1],cmap[kolor-1][2]);
			    break;

			  case IMGpicin:
			    if ( jcp >= 0 && jcp < Pw && icp >= 0 && icp < Ph )
			      PGM_ASSIGN(picin[icp][jcp],cmap[kolor-1][1]);
			    break;

			  case IMGgraphout:
			    if ( jcp >= 0 && jcp < Pw && icp >= 0 && icp < Ph )
			      PGM_ASSIGN(graphout[icp][jcp],cmap[kolor-1][1]);
			    break;

			  case IMGpicpad:
			    if ( jcp >= -Pad && jcp < Pw+Pad &&
				 icp >= -Pad && icp < Ph+Pad )
			      picpad[icp][jcp] = cmap[kolor-1][1];
			    break;

			  case IMGcurves:
			    if ( jcp >= 0 && jcp < Curveswid &&
				 icp >= 0 && icp < Curveswid )
			      PPM_ASSIGN(curves[icp][jcp],cmap[kolor-1][0],
					 cmap[kolor-1][1],cmap[kolor-1][2]);
			    break;

			  default: {};
			  }
		      jcp++;
		    }
		jc += 7; 
	      }
      ++tc; 
    }
}


void WriteCalibFile(char *CalibFilename);
void WriteCalibFile(char *CalibFilename)  /* Write out calibration  file */
{
  FILE *fp;
  clock_t  time1;
  int i;
  
  assert(fp = fopen(CalibFilename,"w"));
  /* write out the calibration setup, 
     Ph, Pw, Xaim, Yaim, Distance, FOV,
     The function degree+1,
     the distortion center i and j pixel coordinates, the aspect ratio,
     the distortion center i and j spot coordinates, the image rotation angle,
     the Spot->Pixel function coefficients,
     the Pixel->Spot function coefficients,
     space for fine displacement and rotation adjustments */
  
  time1 = time(NULL);
  fprintf(fp,IDline "\n"
	  "Creation Time: %s\n"
	  "Image Height (pixels): %d\nImage Width (pixels): %d\n"
	  "Aimed At (spot coords): Y=%.6g  X=%.6g\n"
	  "Distance to Calibration Grid (spot spacings): %.6g\n"
	  "Horizontal Field Of View (degrees): %.6g\n",
	  ctime(&time1),
	  Ph, Pw, Yaim, Xaim, Distance, FOV);
  fprintf(fp,"Function Degree: %d\n"
	  "Distortion Center (pixels): Y=%.6g X=%.6g\n"
	  "Aspect Ratio: %.6g\n"
	  "Distortion Center (spot coords): Y=%.6g X=%.6g\n"
	  "Rotation (degrees): %.6g\n\n"
	  "Spot->Pixel Function Coefficients:\n", 
	  CalibPN,CalibPi, CalibPj,CalibAS,CalibSi,CalibSj,CalibAN*180./PI);
  for (i=0; i<CalibPN; i++) fprintf(fp,"%.6g ",CalibP[i]);
  fprintf(fp,"\nPixel->Spot Function Coefficients:\n");
  for (i=0; i<CalibPN; i++) fprintf(fp,"%.6g ",CalibS[i]);
  fprintf(fp,"\n\n"
	  "Vernier Angle Offset (radians): 0.0\n"
	  "Vernier Y Offset (pixels): 0.0\n"
	  "Vernier X Offset (pixels): 0.0\n");
  fclose(fp);
}


void **padmat(int row, int col, int padrow, int padcol, int typesize);
void **padmat(int row, int col, int padrow, int padcol, int typesize)
  /* make padded 2D array of typesize elements */
{	
    int i, sizfac;
    char *ar, **arp;
    assert(arp = (char **) calloc(row+2*padrow,sizeof(char *)));
    assert(ar =  (char *) calloc((row+2*padrow)*(col+2*padcol),typesize));
    sizfac = typesize/sizeof(char);
    for (i=0; i<row+2*padrow; i++)
      arp[i] = &(ar[(i*(col+2*padcol)+padcol)*sizfac]);
    return (void **) ( &(arp[padrow]));
}


/* basis function for general least-squares fit   i = 0..n */
double basis(int i, double x);
double basis(int i, double x)
{
  double v, c;
  
  if (BasisSet==1) /* for P->S lens distortion */
    {
      c = Distance;
      v = x*FOVrad/Pw;
      v = c*pow(v,(double) 2*i+1);
      /* series expansion for tan(v) is made of odd powers of v */
    }
  else if (BasisSet==2)  /* for S->P lens distortion */
    {
      c = Pw/FOVrad;
      v = x/Distance;  /* projective scale, from calibration setup */
      v = c*pow(((i&1)==0)?atan(v):v,(double) (1+(i>>1)));
      /* Powers of atan(v) and v
	 v models simple projective geometry.
	 atan(v) is appropriate for distortion that gives
	 uniform light intensity across image.
	 Function is ready for either, with higher order adjustments. */
    }
  else v = pow(x,(double) i);  /* generic */
  return(v);
}


void Map_Spot_Pixel_Setup(void);
void Map_Spot_Pixel_Setup(void)
     /* setu up to transform spot coords [i,j] into pixel coords [*ii,*jj]
	and vice versa using last calculated distortion setup */
  {
    int i;

    CalibPN_s = CalibPN;   CalibPi_s = CalibPi;     CalibPj_s = CalibPj;
    CalibAS_s = CalibAS;   CalibSi_s = CalibSi;  CalibSj_s = CalibSj;
    CalibAN_s = CalibAN;   Pw_s = Pw;   Ph_s = Ph;
    Xaim_s = Xaim;   Yaim_s = Yaim;    SpotSpacing_s = SpotSpacing;
    for (i=0; i< CalibPN_s; i++) CalibS_s[i] = CalibS[i];
    for (i=0; i< CalibPN_s; i++) CalibP_s[i] = CalibP[i];
    
    rootCalibAS_s = sqrt(CalibAS_s);
    
    sprintf(slog,"   Rectified image spot spacing %.4g pixels\n",SpotSpacing_s); plog();
    sprintf(slog,"   Rectification parameters:\n"); plog();
    sprintf(slog,"   Ph %d Pw %d Yaim %.4g  Xaim %.4g  Distance %.4g  FOV %.4g\n",
	    Ph_s, Pw_s, Yaim_s, Xaim_s, Distance, FOV);
    plog();
    sprintf(slog,"   CalibPi %.4g  CalibPj %.4g  CalibAS %.4g  "
	    "CalibSi %.4g  CalibSj %.4g  CalibAN(d) %.4g\n",
	    CalibPi,CalibPj,CalibAS,CalibSi,CalibSj,CalibAN_s*180.0/PI);
    plog();
    llog("   S->P Function CalibP:");
    for (i=0; i<CalibPN; i++)
      { sprintf(slog,"  %.4g",CalibP_s[i]); plog(); }
    llog("\n");   llog("   P->S Function CalibS:");
    for (i=0; i<CalibPN; i++)
      { sprintf(slog,"  %.4g",CalibS_s[i]); plog(); }
    llog("\n");
  }
   

double Pixel_Radius_from_Spot(double rs);
double Pixel_Radius_from_Spot(double rs)
{
  double rp;  int k;
  /* transform spot radius to pixel radius */
  BasisSet = 2;
  rp = 0;  for (k = 0; k<CalibPN_s; k++)  rp += CalibP_s[k]*basis(k,rs);
  return(rp);
}


double Spot_Radius_from_Pixel(double rp);
double Spot_Radius_from_Pixel(double rp)
{
  double rs;  int k;
  /* transform pixel radius to spot radius */
  BasisSet = 1;
   rs = 0;  for (k = 0; k<CalibPN_s; k++)  rs += CalibS_s[k]*basis(k,rp);
  return(rs);
}


void Map_Spot_to_Pixel(double i, double j, double *ii, double *jj);
void Map_Spot_to_Pixel(double i, double j, double *ii, double *jj)
     /* transform spot coords [i,j] into pixel coords [*ii,*jj]
	using last calculated distortion setup */
{
  double is, js, rs, rp, ang, sang, cang, ip, jp;  

  /* calculate spot coordinates desired for this destination pixel */
  is = (i - (Ph_s-1)/2.0)/SpotSpacing_s + Yaim_s;
  js = (j - (Pw_s-1)/2.0)/SpotSpacing_s + Xaim_s;
  
  /* calculate radius from spot optical axis */
  is -= CalibSi_s;    js -= CalibSj_s;
  rs = sqrt(is*is + js*js);
  
  /* transform spot radius to pixel radius */
  rp = Pixel_Radius_from_Spot(rs);
  
  /* project pixel radius back onto to spot radius direction,
     rotated by grid angle from horizontal  */
  ang = atan2(is,js);
  sang = sin(ang-CalibAN_s);  cang = cos(ang-CalibAN_s);
  
  ip = rp*sang/rootCalibAS_s;
  jp = rp*cang*rootCalibAS_s;
  
  /* and add pixel optical axis to get source pixel */
  *ii = ip + CalibPi_s;     *jj = jp + CalibPj_s;

  if (IdentityMap>0) /* see if identity transform really is */
    { 
      is = *ii - i;  if (is<0) is = -is;
      js = *jj - j;  if (js<0) js = -js;
      if (is>0.001 || js>0.001) IdentityMap++;
      *ii = i;    *jj = j;
    }
}


void Map_Pixel_to_Spot(double ii, double jj, double *i, double *j);
void Map_Pixel_to_Spot(double ii, double jj, double *i, double *j)
     /* transform pixel coords [ii,jj] into spot coords [*i,*j]
	using last calculated distortion setup */
{
  double is, js, rs, rp, ang, sang, cang, ip, jp;  

  /* subtract pixel optical axis from source pixel */
  ip = ii - CalibPi_s;     jp = jj - CalibPj_s;

  ip *= rootCalibAS_s;   jp /= rootCalibAS_s;

  rp = sqrt(ip*ip+jp*jp);

  /* transform pixel radius to spot radius */
  rs = Spot_Radius_from_Pixel(rp);

  /* project spot radius back onto to pixel radius direction,
     rotated by grid angle from horizontal  */
  ang = atan2(ip,jp);
  sang = sin(ang+CalibAN_s);  cang = cos(ang+CalibAN_s);
  
  is = rs*sang;    js = rs*cang;
 
  /* add in axis spot center */
  is += CalibSi_s;    js += CalibSj_s;

  /* rectified spot image coordinates for this source pixel */

  *i = (is - Yaim_s)*SpotSpacing_s + (Ph_s-1)/2.0;
  *j = (js - Xaim_s)*SpotSpacing_s + (Pw_s-1)/2.0;

  if (IdentityMap>0) /* see if identity transform really is */
    { 
      is = *i - ii;  if (is<0) is = -is;
      js = *j - jj;  if (js<0) js = -js;
      if (is>0.001 || js>0.001) IdentityMap++; /* count up violations */
      *i = ii;    *j = jj;
    }
}


void ScatterGraph(char *name);
void ScatterGraph(char *name) 
  /* draw basic scatter graph of Pixel versus Spot radii */
{
  double RpMax = 0.0, RsMax = 0.0;  /* max radii, for scaling plots */
  int k, ii, jj, i, j;
  FILE *fp;

  /* clear scatter graph */
  for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) graphout[i][j] = 255;

  /* draw reference diagonal line */
  for (j=0; j<Pw; j+=3) graphout[Ph-1-(j*Ph)/Pw][j] = 128;

  /* calculate radius maxima */
  RpMax = RsMax = 0;
  for (k=0; k<NspotsLinked; k++)
    {  
      if (Itemp[k]>RpMax) RpMax = Itemp[k];
      if (Jtemp[k]>RsMax) RsMax = Jtemp[k];
    };

  /* draw spot/pixel radius scatter points */
  for (k=0; k<NspotsLinked; k++)
    {  
      ii = (Itemp[k]/RpMax)*(Ph-4);
      jj = (Jtemp[k]/RsMax)*(Pw-4);
      if (ii>0 && ii<Ph-5 && jj>0 && jj<Pw-5)
	for (i=0; i<4; i++) for (j=0; j<4; j++)
	  graphout[Ph-1-ii-i][jj+j] = 0;
    };

  strcat(img,".");  strcat(img,name);  strcat(img,".pgm");

  IMGid = IMGgraphout;  
  {
    char temp[500];
    sprintf(temp,"%s    --   RMS scatter %.4g pixels",img,bestfit);
    Text(1,Ph-20,Pw/8,temp);
  };

  {
    char temp[500];
    timeA = time(NULL);
    sprintf(temp,"flatfish %s %.4g %.4g %.4g %.4g %d      %s",
	    imgname,Yaim,Xaim,Distance,FOV,SPOTSACROSS,ctime(&timeA) );
    Text(1,Ph-5,Pw/8,temp);
  };

  Text(1,Ph-20,Pw-80,PreRectify?"PRE-RECT":"ORIGINAL");
			    
  assert(fp = fopen(img,"w"));
  img[imgl] = 0;
  pgm_writepgm(fp,graphout,Pw,Ph,255,0);       /* and write out graph */
  fclose(fp);
}


void plottransforms(void);
void plottransforms(void)  /* make fancy image of P->S and S->P functions */
{ 
  int rmax, i, j, l, ii, jj;  double k;
  FILE *fp;

  rmax = sqrt(Pw*Pw+Ph*Ph)/2.0+1;
  Curveswid = 1.4*rmax;
  assert(curves = ppm_allocarray(Curveswid,Curveswid));
  
  for (i=0; i<Curveswid; i++) for (j=0; j<Curveswid; j++)
    PPM_ASSIGN(CURVES(i,j),255,255,255);

  for (j=0; j<Curveswid; j++)
    {
      PPM_ASSIGN(CURVES(Pw/2,j),192,192,192);
      PPM_ASSIGN(CURVES(Ph/2,j),192,192,192);
      PPM_ASSIGN(CURVES(rmax,j),192,192,192);
      PPM_ASSIGN(CURVES(j,Pw/2),192,192,192);
      PPM_ASSIGN(CURVES(j,Ph/2),192,192,192);
      PPM_ASSIGN(CURVES(j,rmax),192,192,192);
    };

  for (k=0; k<Curveswid; k+=0.01)  /* draw S->P curve in red */
    {
      j = floor(k+0.5);
      i = floor(Pixel_Radius_from_Spot(k/SpotSpacing)+0.5);
      if (j>=0 && j<Curveswid && i>=0 && i<Curveswid)
	PPM_ASSIGN(CURVES(i,j),255,0,0);
    };

  for (k=0; k<Curveswid; k+=0.01)  /* draw P->S curve in green */
    {
      i = k+0.5;
      j = floor(SpotSpacing*Spot_Radius_from_Pixel(k)+0.5);

      if (j>=0 && j<Curveswid && i>=0 && i<Curveswid)
	{
	if (PPM_GETG(CURVES(i,j))==255)
	  PPM_ASSIGN(CURVES(i,j),0,255,0);
	else PPM_ASSIGN(CURVES(i,j),64,64,0);
	};
    };
  
  for (l=0; l<NspotsLinked; l++)  /* draw scatter points in blue spotsL*/
    {  
      i = Itemp[l];
      j = SpotSpacing*Jtemp[l];
      for (ii=-1; ii<=1; ii++) for (jj=-1; jj<=1; jj++)
	if (i+ii>=0 && i+ii<Curveswid && j+jj>=0 && j+jj<Curveswid)
	  {
	  if (ii==0 || jj==0) {
	    if (PPM_GETB(CURVES(i+ii,j+jj))==255) {
	      PPM_ASSIGN(CURVES(i+ii,j+jj),0,0,255);
	    } else if (PPM_GETG(CURVES(i+ii,j+jj))==0) {
	      PPM_ASSIGN(CURVES(i+ii,j+jj),64,0,64);
	    } else if (PPM_GETR(CURVES(i+ii,j+jj))==0) {
	      PPM_ASSIGN(CURVES(i+ii,j+jj),0,64,64);
	    } else PPM_ASSIGN(CURVES(i+ii,j+jj),28,28,28);
	   } 
	  };
    };
  
  assert(fp = fopen(strcat(img,".curves.ppm"),"w")); /* and write out display */
  IMGid = IMGcurves;  
    {
    char temp[500];
    sprintf(temp,"%s    --   RMS scatter %.4g pixels",img,bestfit);
    Text(1,Curveswid-20,Curveswid/8,temp);
    };
    {
      char temp[500];
      timeA = time(NULL);
      sprintf(temp,"flatfish %s %.4g %.4g %.4g %.4g %d      %s",
	      imgname,Yaim,Xaim,Distance,FOV,SPOTSACROSS,ctime(&timeA) );
      Text(1,Curveswid-5,Curveswid/8,temp);
    };
  Text(1,Curveswid-20,Curveswid-80,PreRectify?"PRE-RECT":"ORIGINAL");

  img[imgl] = 0;
  ppm_writeppm(fp,curves,Curveswid,Curveswid,255,0);
  fclose(fp);

  ppm_freearray( curves, Curveswid );

}


void rings(int ro);
void rings(int ro)  /* make bullseye for spots, radius ro pixels */
{
    int i, j, jbst,sumi,sumj,t;  double is,js,ros,ti,tj,dc,err;

    assert(ro+1<ROX);
    sprmax=ro;

    for (i=0; i<ro+1; i++) sprpos[i]=0;

    ros = (ro+1)*(ro+1);
    assert(ros<SPX);

    spn = 0;     
    for (i = -ro-1; i <= ro+1; i++)  /* mark out cells in circle */
      {   is = i*i;
	  for (j = -ro-1; j <= ro+1; j++)
	    {   js = j*j;
	 	if (is+js <= ros) 
		  { spi[spn] = i;  spj[spn] = j;  spn++; }
	    }
      }

    sumi = sumj = 0;
    for (i=0; i<spn-1; i++)  /* accrete to keep centroid at origin */
      {   err = 1e30; jbst = i;
	  for (j=i+1; j<spn; j++)
	    { 
		ti = sumi+spi[j];   tj = sumj+spj[j];
		dc = ti*ti + tj*tj;
		if (dc<err) { err=dc;  jbst=j; };
	    }
	  t = spi[i];  spi[i] = spi[jbst];  spi[jbst] = t;
	  t = spj[i];  spj[i] = spj[jbst];  spj[jbst] = t;
	  sumi += spi[i];   sumj += spj[i];
	  ti = spi[i];  tj = spj[i];  t = sqrt(ti*ti+tj*tj);
	  if (sprpos[t]==0) sprpos[t]=i;
      };

}


double fitspot(int pi, int pj, int lor, int hir, int *rbest);
double fitspot(int pi, int pj, int lor, int hir, int *rbest)
  /* match spot prototype to image location pi,pj */
  /* consider spot radii from lor to hir */
{
    int i, k, kk, outr, kout, ant, allnt, avt, it;
    double sn[ROX], sv[ROX], sq[ROX], san[ROX];
    double t, an, av, aq, dn, dv, dq, on, ov, oq;
    double dotq, dotbest, dmn, dvr, omn, ovr;

    if (lor<1) return(0.0);

    outr = floor(1.4*hir+0.5);
    if (outr<=hir) outr=hir+1;
    
    assert(outr<sprmax);

    aq = avt = ant = allnt = 0;

    for (i = 0; i<=outr; i++)
      {
	  for (k=sprpos[i]; k<sprpos[i+1]; k++)
	    { allnt++;
	    if ((it=picpad[pi+spi[k]][pj+spj[k]]) != 128)
	      {  t=it;  aq += t*t;   avt += it;    ant++; }
	    };

	  sq[i] = aq;	  sv[i] = avt;	  sn[i] = ant;   san[i] = allnt;
      };

    an = ant;  av = avt;
    /* compute count, sum and sum of squares of pixel values
       in concentric rings around candidate spot (dot) position */

    dotbest = -1000000.0;  *rbest = 0;

    /* scan range of candidate radii */
    for (k = lor; k<=hir; k++) 
      {
	kout = floor(k*1.3+0.5);
	if (2*sn[kout]>san[kout])  /* eliminate mostly unknown areas */
	  { 
	    kk = k>1?k-1:k;
	    dn = sn[kk];  dv = sv[kk];  dq = sq[kk];
	    /* dot averages: 
	       if possible omit outermost pixels for distortion tolerance */

	    on = sn[kout]-sn[k];  ov = sv[kout]-sv[k];  oq = sq[kout]-sq[k];
	    /* outer ring averages */
	  
	    dmn = dv/dn;	  dvr = (dq - dv*dmn);
	    omn = ov/on;	  ovr = (oq - ov*omn);
	  
	    dotq = omn-dmn - sqrt(4.0*dvr/(dn-1.0)+2.0*ovr/(on-1.0));
	    if (dotq>dotbest) { dotbest = dotq;  *rbest = k;};
	  }
      };

    return(dotbest*(*rbest));  /* bias answer towards largest spots */
}
    

void paintspots(int lor, int hir);
void paintspots(int lor, int hir)  /* calculate "spotness" across image */
{ int i,j,l;

    flP = (float **) padmat(Ph,Pw,Pad,Pad,sizeof(float));
    chP = (unsigned char **) padmat(Ph,Pw,Pad,Pad,sizeof(char));

    for (i= -Sad; i<Ph+Sad; i++) for (j= -Sad; j<Pw+Sad; j++)
      {
	  flP[i][j] = fitspot(i,j,lor,hir,&l);  chP[i][j] = l;
      }	
}


void spotmaxlink(void);
void spotmaxlink(void)  /* link maxima of paintspots operator into grid */
{
    double v, spsep;  
    int i, j, k, l, ndone;  double di, dj, ddi, ddj, d;
    
    int Hood = MaxHood;     /* number of nearest spot neighbors to consider
			 when searching for adjacent neighbors */


    Nspots = 0;  /* count and store spot locations (spot operator local maxima) */
    for (i=-Sad; i<Ph+Sad; i++) for (j=-Sad; j<Pw+Sad; j++) if (flP[i][j]>0.0)
      { 
	di = i;  dj = j;
	Ispots[Nspots] = di;  Jspots[Nspots] = dj;
	Vspots[Nspots] = flP[i][j];
	Rspots[Nspots] = chP[i][j];   Nspots++; 
	assert(Nspots<MaxNspots);  /* are spot coordinate arrays big enough? */
      };

    /* print histogram of found spot sizes */
    {
    int sizes[100], szmax, szmin;
    szmin = 99;  szmax = 0; for (i=0; i<100; i++) sizes[i]=0;
    for (i=0; i<Nspots; i++)
      {	sizes[(int) Rspots[i]]++;
	if (Rspots[i]<szmin) szmin=Rspots[i];
	if (Rspots[i]>szmax) szmax=Rspots[i]; };
    llog("   Spot maxima histogram: \n       ");
    for (i=szmin; i<=szmax; i++) { sprintf(slog," %d(%d)",i,sizes[i]); plog(); };
    llog("\n");
    }

    llog("\n* Linking neighboring spots into grid *\n\n");

    llog("   Creating and propagating spot-local coordinate systems\n");

    /* scan spot list to find neighbors of each spot */
    if (Nspots < Hood) Hood = Nspots;

    for (i = 0; i<Nspots; i++)  /* for each spot */
      {
	  for (j=0; j<Nspots; j++)
	    { /* calculate spot-size normalized distance to other spots */
		DPsp[j] = j;
		di = Ispots[j]-Ispots[i];  dj = Jspots[j]-Jspots[i];
		Dsp[j]  = sqrt(di*di + dj*dj) / (Rspots[i]+Rspots[j]+1);
		/* spot size sets scale */
	    };

	  /* find and save Hood closest neighbors (sort them, sort of) */
	  for (j=0; j<Hood; j++) for (k=j+1; k<Nspots; k++)
	    if (Dsp[k] < Dsp[j])
	      {
		  v = Dsp[k];  Dsp[k] = Dsp[j];   Dsp[j] = v;
		  l = DPsp[k]; DPsp[k] = DPsp[j]; DPsp[j] = l;
	      };

	  for (j=0; j<Hood; j++)
	    {  Dspots[i][j] = Dsp[j];  DPspots[i][j] = DPsp[j]; };

	  /* note: for each spot i, the 0th entry [i][0] will be the spot itself */
      }

    llog("   Pick out and isolate pattern center spot\n");

    Cspots = 0;  /* find spot cosiest to 4 others -- better be pattern center */
    for (i=1; i<Nspots; i++)
	  if (Dspots[i][4]<Dspots[Cspots][4]) Cspots = i;

    sprintf(slog,
	    "   Pattern Center spot %d (spacing %.3g spot diameters, %.3g pixels)\n",
	    Cspots,Dspots[Cspots][4],
	    Dspots[Cspots][4]*(Rspots[Cspots]+Rspots[DPspots[Cspots][4]]+1));
    plog();

    /* remove center spot from neighbor's lists */

    for (i = 0; i < Nspots; i++) if (i != Cspots)
      for (j = 1; j<Hood; j++) if (DPspots[i][j] == Cspots)
	for (k=j+1; k<Hood; k++)
	  { Dspots[i][k-1] = Dspots[i][k];
	    DPspots[i][k-1] = DPspots[i][k]; }

    Aspots[Cspots] = -1;  /* mark it unlinked, just in case ... */

    spsep = l = 0;
    for (i=0; i<Nspots; i++) if (i != Cspots) /* trim and average spot spacing */
      {  spsep += Dspots[i][1];  l++;  }
    /* accumulate mean spots spacing */
    spsep /= l;  /* find spot spacing mean */
    sprintf(slog,"   Mean measured spacing %.3g spot diameters\n",spsep); plog();

    Pspots = 0;   ddi = 1<<30;   /* find spot nearest image centroid */
    for (i=0; i<Nspots; i++) if (i != Cspots)
      {
	Aspots[i] = -1;  /* indicate every spot has no coordinate frame yet */
	di = Ispots[i] - (Ph-1)/2.0;   dj = Jspots[i] - (Pw-1)/2.0;
	d = di*di + dj*dj;
	if (d<ddi) {ddi = d;  Pspots = i;};
      };
    sprintf(slog,"   Centroidmost spot %d, at pixel center + (%g,%g)\n",Pspots,
	   Ispots[Pspots]-(Ph-1)/2.0,Jspots[Pspots]-(Pw-1)/2.0);  plog();

    /* set up initial local coord axes for first spot */
    IIspots[Pspots] = JJspots[Pspots] = spsep*2*Rspots[Pspots];
    IJspots[Pspots] = JIspots[Pspots] = 0.0;  /* straight up! */
    Aspots[Pspots] = 0;  /* first spot now has initial coordinates */

    weirdcount = 0;  /* to count distorted local linking coordinates */
    llog("   Spot linking iterations:");
    do
      {   
	double axpr[Adjacencies][2], axbs[Adjacencies][2], axvl[Adjacencies];
	int axid[Adjacencies];
	/* local coord axes,  prototype, and best found, value, and id */

	ndone = 0;
	for (i=0; i<Nspots; i++) if (i != Cspots)
	  if (Aspots[i] == 0)  /* propagate coords for newly-linked spot i */
	    {  
	      ndone++;  /* set up expected displacement to eight neighbors */
	      for (j=0; j<Adjacencies; j++)
		{
		  axpr[j][0] = IIspots[i]*Inb[j]+JIspots[i]*Jnb[j];   
		  axpr[j][1] = JJspots[i]*Jnb[j]+IJspots[i]*Inb[j];
		};
	      
	      /* test and note if this spot's spacing aspect ratio is weird */
	      ddi = sqrt(axpr[0][0]*axpr[0][0] + axpr[0][1]*axpr[0][1]);
	      ddj = sqrt(axpr[3][0]*axpr[3][0] + axpr[3][1]*axpr[3][1]);
	      if (ddi/ddj > 1.4 || ddj/ddi > 1.4) weirdcount++;
	      
	      for (j=0; j<Adjacencies; j++) /* initialize best axis spots */
		{   
		  /* fits must be 0.5 to 1.5 way to prototype */
		  di = axpr[j][0]*0.5;
		  dj = axpr[j][1]*0.5;   
		  ddi = axpr[j][0] - di;     ddj = axpr[j][1] - dj;
		  axvl[j] = ddi*ddi + ddj*ddj;
		  axid[j] = -1;
		};
	      
	      /* search for best fit for each axis direction among neighbors */
	      for (j=1; j<Hood-1; j++)
		{
		  l = DPspots[i][j];
		  di = Ispots[l]-Ispots[i];  dj = Jspots[l]-Jspots[i];
		  for (k=0; k<Adjacencies; k++)
		    {
		      ddi = axpr[k][0] - di;   ddj = axpr[k][1] - dj;
		      d = ddi*ddi + ddj*ddj;
		      if (d<axvl[k])
			{ 
			  axvl[k] = d;  axid[k] = l;
			  axbs[k][0] = di;  axbs[k][1] = dj;
			};
		    };
		};
	      
	      for (j=0; j<Adjacencies; j++) /* mix best coord fits into prototype */
		if (axid[j]>=0) for (k=0; k<2; k++) axpr[j][k] = axbs[j][k];
	      
	      for (j=0; j<Adjacencies; j++)  DPspots[i][j+1] = axid[j];
	      Aspots[i] = 1;  /* this spot has finalized coordinates! */
	      

	      if (axid[0]>=0 && axid[1]>=0)
	      { IIspots[i] = (axpr[0][0]-axpr[1][0])/4.0 + IIspots[i]/2.0; 
	      IJspots[i] = (axpr[0][1]-axpr[1][1])/4.0 + IJspots[i]/2.0; };

	      if (axid[2]>=0 && axid[3]>=0)
	      { JIspots[i] = (axpr[2][0]-axpr[3][0])/4.0 + JIspots[i]/2.0;
	      JJspots[i] = (axpr[2][1]-axpr[3][1])/4.0 + JJspots[i]/2.0; };
	      
	      for (j=0; j<Adjacencies; j++)  /* propagate to neighbors */
		if ((k=axid[j])>=0) 
		  {
		    if (Aspots[k]<0) /* an uninitialized spot */
		      {
			Aspots[k] = 0;
			IIspots[k] = IIspots[i];  IJspots[k] = IJspots[i];
			JIspots[k] = JIspots[i];  JJspots[k] = JJspots[i];
		      }
		    else if (Aspots[k]==0) /* already has initial coordinates */
		      { 
			/* average in this spot's coordinates */
			IIspots[k] = (IIspots[i] + IIspots[k])/2;
			IJspots[k] = (IJspots[i] + IJspots[k])/2;
			JIspots[k] = (JIspots[i] + JIspots[k])/2;
			JJspots[k] = (JJspots[i] + JJspots[k])/2;
		      }; 
		  };
	      
	      for (j=0; j<Adjacencies; j++) DPspots[i][j+1] = axid[j];
	      /* replace near neighbor with coordinate neighbor links */
	    };
	sprintf(slog," %d",ndone); plog();
      }  while (ndone>0);
    llog("\n");

    if (weirdcount>0)
      {
	sprintf(slog,"   %d spots got weird labelling axis ratios !!!\n",weirdcount);
	plog();
      };
    
    llog("   Assigning ideal coordinates to spots\n");
    
    /* label spots with ideal coordinates */
    /*    for (i=0; i<Nspots; i++) Aspots[i] = 0;  !!!!!! */

    IIspots[Pspots] = JJspots[Pspots] = 0;
    Aspots[Pspots] = 2;  /* image centroidmost spot is now defined as ideal [0,0] */

    clashcount = 0;  /* to count up any spot coordinate labelling disagreements */
    llog("   Ideal labelling iterations:"); 
    do
      {   
	/* label adjoining spots with ideal coordinates */ 
	ndone = 0;
	for (i=0; i<Nspots; i++) if (i != Cspots && Aspots[i] == 2)
	  for (j=1; j<=Adjacencies; j++) 
	    { 
	      k = DPspots[i][j];
	      if (k>=0 && Aspots[k]>=1) /* only do validly linked spots! */
		{   
		  di = IIspots[i] + Inb[j-1];  dj = JJspots[i] + Jnb[j-1];

		  if (Aspots[k]==2)
		    { 
		      if (IIspots[k] != di || JJspots[k] != dj)
			{  
			  clashcount++;  /* note labelling clash */
			  Aspots[k] = -1; /* trouble - unlink clashing spot */
			}
		    }
		  else
		    { 
		      Aspots[k]=2; ndone++;
		      IIspots[k] = di;  JJspots[k] = dj;
		    };
		};
	    };
	sprintf(slog," %d",ndone); plog();
      } 
    while (ndone>0);
    llog("\n");

    if (clashcount>0)
      {
	sprintf(slog,"   %d spot coordinate labels clashed !!!\n",clashcount);
	plog();
      };
    
    /* calculate ideal coords for pattern center spot */
    IIspots[Cspots]=JJspots[Cspots]=0;
    for (j=1; j<=4; j++)
      {
	IIspots[Cspots] += IIspots[DPspots[Cspots][j]];
	JJspots[Cspots] += JJspots[DPspots[Cspots][j]];
      };
    IIspots[Cspots] /= 4.0;       JJspots[Cspots] /= 4.0;
    sprintf(slog,"   Pattern center spot, at Centroidmost spot + [%.3g,%.3g]\n",
	    IIspots[Cspots],JJspots[Cspots]); plog();
	    
    /* shift spot coordinates so pattern center spot is at (0,0) */
    di = IIspots[Cspots];  dj = JJspots[Cspots];
    for (i=0; i<Nspots; i++) { IIspots[i] -= di;  JJspots[i] -= dj; };
}

#if(CompareCenters)

void spotpixelcoord(double *ipos, double *jpos);
void spotpixelcoord(double *ipos, double *jpos)
/* convert spot coords to pixel coords, by interpolating between spots */
     /* No longer needed: instead use SpotToPixelSimple in 
	first iteration	and Map_Spot_to_Pixel later */
{
    int i;
    double is, js;
    double is0, is1, js0, js1;   int p00, p01, p10, p11;

    is = *ipos;  js = *jpos;   /* set up spot coordinates */

    /* set up bounding spot coordinates */
    is0 = floor(is-0.5) + .5;  is1 = is0+1.0;
    js0 = floor(js-0.5) + .5;  js1 = js0+1.0;

    /* find array index of bounding spots */
    p00 = p01 = p10 = p11 = -1;
    for (i =0; i<Nspots; i++) if (i != Cspots && Aspots[i]==2)
      {
	  if (IIspots[i]==is0 && JJspots[i]==js0) p00 = i;
	  if (IIspots[i]==is0 && JJspots[i]==js1) p01 = i;
	  if (IIspots[i]==is1 && JJspots[i]==js0) p10 = i;
	  if (IIspots[i]==is1 && JJspots[i]==js1) p11 = i;
      };
    
    if (p00<0 || p01<0 || p10<0 || p11<0)
      { 
	if ((i = p00>p01?p00:p01) < 0) i = p10>p11?p10:p11;
	if (i<0) return;
	*ipos = Ispots[i];  *jpos  = Jspots[i];
	return; 
      };

    is -= is0;  js -= js0;  /* fractional position within bounding box */

    /* Bilinear interpolation of spot pixel coordinates */

    *ipos  = (Ispots[p00]*(1.0-is) + Ispots[p10]*is)*(1.0-js) +
      (Ispots[p01]*(1.0-is) + Ispots[p11]*is)*js ;

    *jpos  = (Jspots[p00]*(1.0-is) + Jspots[p10]*is)*(1.0-js) +
      (Jspots[p01]*(1.0-is) + Jspots[p11]*is)*js ;

}

#endif


#define NN 1000 /* cc doesn't allow variables in declarations.  Use
                 NN as a substitute for n in declarations */

int SimulSolve(double a[], int n);
int SimulSolve(double a[], int n)  /* solve n simultaneous linear equations */
/* a is an [n * (n+1)] array with rhs constants in rightmost column.
   Solved values are returned in a[0:n-1]. Rest of a turns into a mess. */
    { int i, ii, j, jj, k, it;  double t, tt;    int perm[NN];

    assert(NN>n);
      /* initialize pivot permutation */
      perm[0]=0;  for (i=1; i<n; i++) perm[i] = perm[i-1]+n+1;
      
      for (ii=0; ii<n; ii++)  /* elimination loop */
        { if ((tt = a[perm[ii]+ii])<0.0) tt = -tt;
          for (i=ii+1; i<n; i++)  /* pivot */
            { if ((t = a[perm[i]+ii])<0.0) t = -t;
                if (t>tt)
               {it = perm[ii]; perm[ii] = perm[i]; perm[i] = it; tt=t;}; }
          i = perm[ii]; if ((t=a[i+ii]) == 0.0) return(0); /* singular */
          if (t != 1.0)
            { t = 1.0/t;  /* eliminate variable perm[ii] */
              for (k=ii+1; k<=n; k++) a[i+k] *= t; }
          for (jj=ii+1; jj<n; jj++)
             { j = perm[jj]; if ((t=a[j+ii])!=0.0)
                for (k=ii+1; k<=n; k++) a[j+k] -= t*a[i+k]; }
         }
       for (ii=n-1; ii>0; ii--)   /* backsolve */
         { i = perm[ii]; if ((t=a[i+n]) != 0.0)
              for (jj=0; jj<ii; jj++)  
                {j = perm[jj]; a[j+n] -= a[j+ii]*t;}
         }
       /* unscramble pivot permutation */
       for (ii=0; ii<n; ii++) a[ii]=a[perm[ii]+n];
       return(1);
    }


void fitplane(int n, double x[], double y[], double z[], double w[],
		double *A, double *B, double *C);
void fitplane(int n, double x[], double y[], double z[], double w[],
		double *A, double *B, double *C)
     /* fit least-squares plane   z = A + Bx + Cy
	to n weighted points f[x,y] -> z (weighting w),
	and return its root mean square error */
{	
  double Sz, Sxz, Syz, S1, Sx, Sy, Sxx, Sxy, Syy;
  double E[12];
  int i;
  
  Sz = Sxz = Syz = 0;
  S1 = Sx = Sy = Sxx = Sxy = Syy = 0;
  
  for (i=0; i<n; i++) /* compute power sums for matrix and rowsums */
    { 
      Sz += w[i]*z[i];
      Sxz += w[i]*x[i]*z[i];
      Syz += w[i]*y[i]*z[i];
      
      S1 += w[i];
      Sx += w[i]*x[i];
      Sy += w[i]*y[i];
      Sxx += w[i]*x[i]*x[i];
      Sxy += w[i]*x[i]*y[i];
      Syy += w[i]*y[i]*y[i];
    };
  
  /* set up equations */

  E[0] = S1;   E[1] =  Sx;   E[2]  = Sy;    E[3]  =  Sz;
  E[4] = Sx;   E[5] = Sxx;   E[6]  = Sxy;   E[7]  =  Sxz;
  E[8] = Sy;   E[9] = Sxy;   E[10] = Syy;   E[11] =  Syz;
  
  SimulSolve(E,3);	/* and solve them */
  
  *A = E[0];  *B = E[1];  *C = E[2];
  
}


/* cc doesn't allow variables in array declaration dimensions
   use DEGG as a substitute for deg in declarations */
#define DEGG 100

/* r[0 .. n-1] and rp[0 .. n-1] hold the x and y points to fit to.
   poly[0 .. deg-1] holds the coefficients of the polynomial; the
   final polynomical is:

   poly[0]*1 + poly[1]*x^1 + poly[2]*x^2 + ... + poly[deg-1]*x^(deg-1).

   */

double fitpoly(int deg, int n, double r[], double rp[], double poly[]);
double fitpoly(int deg, int n, double r[], double rp[], double poly[])
/* fit least-squares degree deg-1 polynomial to n points r -> rp,
   and return its root mean square error */
{	/* double S[2*DEGG-1], R[DEGG], A[DEGG*(DEGG+1)];  */
	double *S, *R, *A;
	int i, j, k;   double v, x, xp, sum;

	/* assert(DEGG>deg);  */
	
	assert(S = (double *) calloc(2*deg-1,sizeof(double)));
	assert(R = (double *) calloc(deg,sizeof(double)));
	assert(A = (double *) calloc(deg*(deg+1),sizeof(double)));

	for (k=0; k<2*deg-1; k++) S[k]=0;
	for (k=0; k<deg; k++) R[k]=0;

	for (i=0; i<n; i++) /* compute power sums for matrix and rowsums */
	  { v = 1.0;  /* 1.0 becomes w[i] if weighted sums needed */
	    x = r[i];  xp = rp[i];
	    for (k=0; k<2*deg-1; k++)
	      { S[k] += v;  if (k<deg) R[k] += v*xp;  v *= x; }; };

	k = 0;  /* set up equations */
	for (i = 0; i<deg; i++)
	  {  for (j=0;  j<deg;  j++) A[k+j] = S[i+j];
	     k += deg+1;  A[k-1] = R[i];  /* right hand side */ };

	SimulSolve(A,deg);	/* and solve them */

	for (i=0; i<deg; i++) poly[i] = A[i];  /* return the polynomial */

	sum = 0;	/* compute root mean square error */
	for (i=0; i<n; i++)
	  {   x = r[i];  xp = A[deg-1];   /* evaluate polynomial */
	      for (j = deg-2; j>=0; j--)  xp = xp*x + A[j];
	      xp -= rp[i];
	      sum += xp*xp;  /* *w[i] if weighted sum */   };
	      
	free(A);  free(R);  free(S);
	return (sqrt(sum/n)); /* /Sum(w[i]), not n, if weighted sum */
}



double fitbasis(int deg, int n, double r[], double rp[], double coeff[]);
double fitbasis(int deg, int n, double r[], double rp[], double coeff[])
/* least-squares fit weighted basis functions to n points r -> rp,
   and return its root mean square error */
{	
  /* double S[DEGG+1], A[DEGG*(DEGG+1)];  */
  double *S, *A;
  int i, j, k, l;   double xp, sum;
  
  /* assert(DEGG>deg); */
  assert(S = (double *) calloc(deg+1,sizeof(double)));
	assert(A = (double *) calloc(deg*(deg+1),sizeof(double)));

  k = 0;  /* initialize sums in matrix */
  for (i = 0; i<deg; i++)
    {  
      for (j=i;  j<deg+1;  j++) A[k+j] = 0.0;
      k += deg+1;
    };
  
  /* accumulate basis sums for matrix and rowsums */
  for (l = 0; l<n;  l++)
    {
      for (i=0; i<deg; i++) S[i] = basis(i,r[l]);
      
      k = 0;  /* add lth term to upper half diagonal sums in matrix */
      for (i = 0; i<deg; i++)
	{  
	  for (j=i;  j<deg;  j++) A[k+j] += S[i]*S[j];
	  A[k+deg] += S[i]*rp[l];
	  k += deg+1;
	};
    };

  /* reflect sums in upper half to lower half of the matrix */

  k = 0;
  for (i = 1; i<deg; i++)
    {  
      k += deg+1;
      for (j=0;  j<i;  j++) A[k+j] = A[j*(deg+1)+i];
    };

  SimulSolve(A,deg);	/* and solve the equations */
  
  for (i=0; i<deg; i++) coeff[i] = A[i];  /* return the coefficients */
  
  sum = 0.0;	/* compute root mean square error */
  for (l=0; l<n; l++)
    {
      xp = -rp[l];   /* evaluate function for point l */
      for (j = 0; j<deg; j++)  xp += coeff[j]*basis(j,r[l]);
      sum += xp*xp;  /* *w[i] if weighted sum */
    };
  
  free(A); free(S);  
  return (sqrt(sum/n)); /* /Sum(w[i]), not n, if weighted sum */
}


void PixelToSpotSimple(double ipixel, double jpixel, double *ispot, double *jspot);
void PixelToSpotSimple(double ipixel, double jpixel, double *ispot, double *jspot)
/* convert a pixel coord location to spot coords, by best-fitting a plane to spot
coordinates wrt pixel locations, weighting by distance from desired
point. */
{
  int i, k;
  double di, dj, r, A, B, C, ipx, jpx;

  /* set up independent (pixel coordinate) variables and weighting */
  k = 0;
  for (i =0; i<Nspots; i++) if (i == Cspots || Aspots[i]==2)
    {
      Itemp[k] = Ispots[i];   Jtemp[k] = Jspots[i];
      di = Itemp[k] - ipixel;   dj = Jtemp[k] - jpixel;
      r = sqrt(di*di + dj*dj);
      r = (r/SpotSpacing)/FitRad;
      Ltemp[k] = r>10?4e-44:exp(-r*r);  /* gaussian in pixel radius */
      k++;
    };

  ipx = ipixel;   jpx = jpixel;  
  /* in case pixel and spot arguments are same variables */
    
  /* fit plane for I spot coordinate */

  k = 0;
  for (i =0; i<Nspots; i++) if (i == Cspots || Aspots[i]==2)
    {
      Ktemp[k] = IIspots[i];  /* spot I coordinate */
      k++;
    };

  fitplane(k, Itemp, Jtemp, Ktemp, Ltemp, &A, &B, &C);
  *ispot = A + B*ipx + C*jpx;

  /* fit plane for J spot coordinate */

  k = 0;
  for (i =0; i<Nspots; i++) if (i == Cspots || Aspots[i]==2)
    {
      Ktemp[k] = JJspots[i];  /* spot J coordinate */
      k++;
    };

  fitplane(k, Itemp, Jtemp, Ktemp, Ltemp, &A, &B, &C);
  *jspot = A + B*ipx + C*jpx;
  
}


void SpotToRectPixelSimple(double ispot, double jspot, double *ipixel, double *jpixel);
void SpotToRectPixelSimple(double ispot, double jspot, double *ipixel, double *jpixel)
/* convert a spot coord location to rectified pixel coords, after spot linking but
before full transformation is available, by best-fitting a plane to pixel
coordinates wrt spot locations, weighting by distance from desired
point. */
{
  int i, k;
  double di, dj, r, A, B, C, isp, jsp;

  /* set up independent (spot coordinate) variables and weighting */
  k = 0;
  for (i =0; i<Nspots; i++) if (i == Cspots || Aspots[i]==2)
    {
      Itemp[k] = IIspots[i];   Jtemp[k] = JJspots[i];
      di = Itemp[k] - ispot;   dj = Jtemp[k] - jspot;
      r = sqrt(di*di + dj*dj);
      r = r/FitRad;
      Ltemp[k] = r>10?4e-44:exp(-r*r);  /* gaussian in spot radius */
      k++;
    };
    
  isp = ispot;   jsp = jspot;  
  /* in case pixel and spot arguments are same variables */

  /* fit plane for I pixel coordinate */

  k = 0;
  for (i =0; i<Nspots; i++) if (i == Cspots || Aspots[i]==2)
    {
      Ktemp[k] = IJspots[i];  /* rectified pixel I coordinate */
      k++;
    };

  fitplane(k, Itemp, Jtemp, Ktemp, Ltemp, &A, &B, &C);
  *ipixel = A + B*isp + C*jsp;

  /* fit plane for J pixel coordinate */

  k = 0;
  for (i =0; i<Nspots; i++) if (i == Cspots || Aspots[i]==2)
    {
      Ktemp[k] = JIspots[i];  /* rectified pixel J coordinate */
      k++;
    };

  fitplane(k, Itemp, Jtemp, Ktemp, Ltemp, &A, &B, &C);
  *jpixel = A + B*isp + C*jsp;
  
}


void showspots(int lor, int hir);
void showspots(int lor, int hir) /* color spots found by paintspots */
{
    int i, j, k, l, szmax;  double flmax;
    int sizes[ROX];

    assert(lor <= hir);

    for (i=0; i<100; i++) sizes[i]=0;  flmax = 0.0;  szmax = 0;

    for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) if (flP[i][j]>0)
      {   
	if (flP[i][j]>flmax) flmax=flP[i][j];
	if (chP[i][j]>szmax) szmax=chP[i][j];
	sizes[chP[i][j]]++; 
      };

    if (hir>szmax) hir=szmax;
    if (lor>hir) lor=hir;

    llog("   Spot operator spot size histogram:\n       ");
    l=0; while (sizes[l]==0 && l<100) l++;
    for (i=l; i<=szmax; i++) 
      { sprintf(slog," %d(%d)",i,sizes[i]); plog(); };  llog("\n");

    for (i=-Pad; i<Ph+Pad; i++) for (j=-Pad; j<Pw+Pad; j++) if (flP[i][j]>0)
      {   
	k = chP[i][j];  if (k<lor) k = lor;  if (k>hir) k = hir;
	l = sqrt(sqrt(flP[i][j]/flmax))*255.0;
	PPM_ASSIGN(PICOUT_(i,j),255-l,l,l);
      };
}


void stroke(int kolor, int is, int js, int id, int jd);
void stroke(int kolor, int is, int js, int id, int jd) 
     /* draw a line from is,js to id,jd */
{
  int i,j,k,l;

  l = sqrt((double) ((is-id)*(is-id)+(js-jd)*(js-jd))) + 1;
  for (k=0; k<=l; k++)
    {
	i = (k*id + (l-k)*is)/l;   j = (k*jd + (l-k)*js)/l;
	if (j >= -Pad && j < Pw+Pad) if (i >= -Pad && i<Ph+Pad)
	  PPM_ASSIGN(PICOUT_(i,j),cmap[kolor-1][0],
		     cmap[kolor-1][1],cmap[kolor-1][2]);
    };
}


void crosshair(int kolor, int ip, int jp, int w);
void crosshair(int kolor, int ip, int jp, int w) 
     /* draw an X of size w at ip,jp */
{ int i;

  for (i=-w; i<=w; i++) if (jp+i >= -Pad && jp+i < Pw+Pad)
      { if (ip-i >= -Pad && ip-i < Ph+Pad)
	  PPM_ASSIGN(PICOUT_(ip-i,jp+i),cmap[kolor-1][0],
		     cmap[kolor-1][1],cmap[kolor-1][2]);
	if (ip+i >= -Pad && ip+i < Ph+Pad)
	  PPM_ASSIGN(PICOUT_(ip+i,jp+i),cmap[kolor-1][0],
		     cmap[kolor-1][1],cmap[kolor-1][2]);
    };
}


void starburst(int kolor, int ip, int jp, int w);
void starburst(int kolor, int ip, int jp, int w) 
     /* draw a * of size w at ip,jp */
{
  int i, k, ki, kj;
  double di, dj;

  for (i = 1; i < 16; i+=2)
    {
      di = sin(2*PI*i/16);  dj = cos(2*PI*i/16);  
      for (k=0; k<w; k++)
	{ 
	  ki = floor(k*di + ip + 0.5);
	  kj = floor(k*dj + jp + 0.5);
	  if (ki >= -Pad && ki < Ph+Pad && kj >= -Pad && kj < Pw+Pad)
	    PPM_ASSIGN(PICOUT_(ki,kj),cmap[kolor-1][0],
		     cmap[kolor-1][1],cmap[kolor-1][2]);
	};

    };
}


void ringdraw(int kolor, int ip, int jp, int r);
void ringdraw(int kolor, int ip, int jp, int r) 
     /* draw a ring overlay around ip,jp  */
{ int i,j,k;

  for (k=sprpos[r]; k<sprpos[r+1]; k++)
    {
	i = ip + spi[k];   j = jp + spj[k];
	if (i >= -Pad && i < Ph+Pad && j >= -Pad && j < Pw+Pad)
	  { 
	    if (cmap[kolor-1][0]) 
	      PPM_PUTR(PICOUT_(i,j),cmap[kolor-1][0]);
	    if (cmap[kolor-1][1]) 
	      PPM_PUTG(PICOUT_(i,j),cmap[kolor-1][1]);
	    if (cmap[kolor-1][2]) 
	      PPM_PUTB(PICOUT_(i,j),cmap[kolor-1][2]);
	  };
    };
}


void RectifyImage(void);
void RectifyImage(void)		/* Rectify display image using Map_Spot_to_Pixel*/
{ 
  double is, js;    int i, j, k, ii, jj, errc;   double di, dj;
  FILE *fp;  
  
  Map_Spot_Pixel_Setup();
  errc = 0;
  
  plottransforms();  /* draw S->P and P->S radial correction curves */
  
  /* Make grid spacing SpotSpacing pixels between spot centers */
  
  for (i=-Pad; i<Ph+Pad; i++)  for (j=-Pad; j<Pw+Pad; j++)
    {   
      Map_Spot_to_Pixel((double) i,(double) j,&di,&dj); 
      ii = floor(di+0.5);  jj = floor(dj+0.5);
      
      if (ii<0 || ii>Ph-1 || jj<0 || jj>Pw-1)
	{  errc++; PPM_ASSIGN(PICOUT_(i,j),128,128,128); }
      else
	PPM_ASSIGN(PICOUT_(i,j),picin[ii][jj],picin[ii][jj],picin[ii][jj]);
    };
  sprintf(slog,"   Display rectify: %d pixels from beyond the original image edge\n",errc);
  plog();
  
  /* Overlay a grid with SpotSpacing-sized cells */
  for (k=-1; k<=1; k+=2)
    {
      for (is=0; is<Ph/2; is+=SpotSpacing)
	{
	  i = floor(is + 0.5);  
	  stroke(i?4:2,k*i+(Ph-1)/2.0,0,k*i+(Ph-1)/2.0,Pw-1);
	};
      for (js=0; js<Pw/2; js+=SpotSpacing)
	{ 
	  j = floor(js + 0.5);  
	  stroke(j?4:2,0,k*j+(Pw-1)/2.0,Ph-1,k*j+(Pw-1)/2.0);
	};
    };

  /* mark original (unpadded) image extent with yellow outlines */
  for (i = -Pad; i < Ph+Pad; i++)  for (j = -Pad; j < Pw+Pad; j++)
    { k = PPM_GETG(PICOUT_(i,j));
    if(i == -1 || i==Ph || j == -1 || j==Pw)
      PPM_ASSIGN(PICOUT_(i,j),128+(k>>1),128+(k>>1),(k>>1));
    };

  assert(fp = fopen(strcat(img,".rect.ppm"),"w")); /* and write out display */
  IMGid = IMGpicout;  
  Text(1,Ph+Pad-20,(Pw+2*Pad)/8-Pad,img);  /* annotate display */
  {
    char temp[500];
    timeA = time(NULL);
    sprintf(temp,"flatfish %s %.4g %.4g %.4g %.4g %d      %s",
	    imgname,Yaim,Xaim,Distance,FOV,SPOTSACROSS,ctime(&timeA) );
    Text(1,Ph+Pad-5,(Pw+2*Pad)/8-Pad,temp);
  };
  Text(1,Ph+Pad-20,Pw+Pad-80,PreRectify?"PRE-RECT":"ORIGINAL");

  img[imgl] = 0;
  ppm_writeppm(fp,picout,Pw+Pad+Pad,Ph+Pad+Pad,255,0);
  fclose(fp);
  
  /* Rectify image into working array */
  
  errc = 0;
  for (i=-Pad; i<Ph+Pad; i++)  for (j=-Pad; j<Pw+Pad; j++)
    {
      Map_Spot_to_Pixel((double) i,(double) j,&di,&dj); 
      ii = floor(di+0.5);  jj = floor(dj+0.5);
      
      if (ii<0 || ii>Ph-1 || jj<0 || jj>Pw-1)
	{  errc++; picpad[i][j] = 128; }
      else
	picpad[i][j] = picin[ii][jj];
    }
  
  sprintf(slog,
	  "   Working image rectify: %d pixels from beyond original image edge\n"
	  ,errc);
  plog();
  if (IdentityMap > 1)
    {
      sprintf(slog,"   !!!!! Identity transform failed %d times !!!!!\n",
	      IdentityMap/2);
      plog();
    };
  
  /* Redistort image using Map_Pixel_to_Spot */
  
  errc = 0;
  for (i=-Pad; i<Ph+Pad; i++)  for (j=-Pad; j<Pw+Pad; j++)
    {   
      Map_Pixel_to_Spot((double) i,(double) j,&di,&dj); 
      ii = floor(di+0.5);  jj = floor(dj+0.5);
      
      if (ii<-Pad || ii>Ph+Pad-1 || jj<-Pad || jj>Pw+Pad-1)
	{  errc++; PPM_PUTB(PICOUT_(i,j),128); }
      else
	{  PPM_PUTB(PICOUT_(i,j),PPM_GETG(PICOUT_(ii,jj)));  };
    };
  sprintf(slog,"   Redistort: %d pixels from beyond the padded image edge\n",errc); plog();
  
  for (i=-Pad; i<Ph+Pad; i++)  for (j=-Pad; j<Pw+Pad; j++)
    {
      PPM_PUTR(PICOUT_(i,j),(i>=0 && i<Ph && j>=0 && j<Pw)?picin[i][j]:128);
      PPM_PUTG(PICOUT_(i,j),PPM_GETB(PICOUT_(i,j)));
    };
  


  assert(fp = fopen(strcat(img,".redistort.ppm"),"w")); /* and write out display */

  IMGid = IMGpicout;  
  Text(1,-Pad+15,(Pw+2*Pad)/2-Pad,img);  /* annotate display */
  {
    char temp[500];
    timeA = time(NULL);
    sprintf(temp,"flatfish %s %.4g %.4g %.4g %.4g %d      %s",
	    imgname,Yaim,Xaim,Distance,FOV,SPOTSACROSS,ctime(&timeA) );
    Text(1,-Pad+30,(Pw+2*Pad)/2-Pad,temp);
  };
  Text(1,-Pad+15,Pw+Pad-80,PreRectify?"PRE-RECT":"ORIGINAL");

  img[imgl] = 0;
  ppm_writeppm(fp,picout,Pw+Pad+Pad,Ph+Pad+Pad,255,0);
  fclose(fp);
  
}


/* test a trial optical center, return RMS scatter of pixel coordinates on spot axis */
double TryCenter(double isc, double jsc, double ipc, double jpc, double aspect);
double TryCenter(double isc, double jsc, double ipc, double jpc, double aspect)
  {
    double di, dj, rootaspect;
    int k;

    rootaspect = sqrt(aspect);
    NspotsLinked = 0;
    for (k=0; k<Nspots; k++) if (Aspots[k]==2 || k==Cspots) 
      /* linked or center spot? */
      {   /* calculate pixel and spot radii */
	di = (Ispots[k] - ipc)*rootaspect; 
	dj = (Jspots[k] - jpc)/rootaspect;
	Itemp[NspotsLinked] = sqrt(di*di+dj*dj);
	di = IIspots[k] - isc; 
	dj = JJspots[k] - jsc;
	Jtemp[NspotsLinked] = sqrt(di*di+dj*dj);
      if (Itemp[NspotsLinked]<0 || Jtemp[NspotsLinked]<0)
	printf("TryCenter Linked spot %d radii %.4g %.4g\n",
	       NspotsLinked,Itemp[NspotsLinked],Jtemp[NspotsLinked]);
	NspotsLinked++;
      };
    
    BasisSet = 2;
    di = fitbasis(CalibPN,NspotsLinked,Jtemp,Itemp,Poly);

    return(di);
  }


int main(int argc, char *argv[])
{
  int i, j, k, ii, jj;  gray maxval;  int rows, cols;
  int NspotsLast, IterateMore; 
  /* how many spots linked last time, to stop if no improvement */
  double di, dj;
  double v; int rmax;
  FILE *fp;  
  char line[1000];
  clock_t  time1, time2;
  char PreRectifyChar = 'y';

	strcpy(imgname,"spot.pgm");
	Xaim = 0;  Yaim = 0;  Distance = 8.5;  FOV = 90.0;
	SPOTSACROSS = 0;   PreRectify = 1;

  if (argc < 2)
    {
      printf(
      "Usage: flatfish <spot>.pgm <Yaim> <Xaim> "
      "<Distance> <FOV> <SpotsAcross> <PreRectify>\n"
	     "spot.pgm: an aligned image of a calibration spot grid\n"
	     "Y,Xaim: down,right spot coords of camera aim, center spot = (0,0)\n"
	     "Distance: spot spacings between lens center and the spot grid\n"
	     "FOV: Degrees horizontal field of view of rectified image\n"
	     "SpotsAcross: number of spot spacings across image (0=automatic)\n"
	     "PreRectify: precorrect assumed distortion? (defaults to Y)\n" );    
      
     	printf("\n\nflatfish "); fflush(stdout);
	fgets(line,300,stdin);
     	sscanf(line,"%s%g%g%g%g%d%c\n",
     		imgname,&Yaim,&Xaim,&Distance,&FOV,&SPOTSACROSS,&PreRectifyChar);
     	printf("\n\n");
	   }
   else
    {
      strcpy(imgname,argv[1]);
      if (argc > 2) sscanf(argv[2],"%g",&Yaim);
  		if (argc > 3) sscanf(argv[3],"%g",&Xaim);
  		if (argc > 4) sscanf(argv[4],"%g",&Distance);
  		if (argc > 5) sscanf(argv[5],"%g",&FOV);
  		if (argc > 6) sscanf(argv[6],"%d",&SPOTSACROSS);
  		if (argc > 7) sscanf(argv[7],"%c",&PreRectifyChar);
  	};
  	
  /* set up file name extension system for trace files */
  strcpy(img,imgname); imgl = strlen(img);
  assert(imgl<IMGNMAX);
  if (imgl>5) if (img[imgl-4]=='.') imgl -= 4;
  imgn = imgl;
  img[imgl] = 0;
  
  pgm_init(&argc, argv);
  ppm_init(&argc, argv);
  
  /* Read in raw spot image */
  
  fp = fopen(imgname,"r");
  if (fp == NULL) {printf("No %s file!!\n",imgname); exit(1); };
  picin = pgm_readpgm(fp, &cols, &rows, &maxval );
  fclose(fp);
  Pw = cols;   Ph = rows;  
  
  /* check to see if picture will fit */
  assert(Ph<PhMax);   assert(Pw<PwMax);   assert(maxval<256);
  
  if (Yaim>10 || Yaim<-10 || Xaim>15 || Xaim<-15 
      || Distance<3 || Distance>50 || FOV < 10 || FOV > 170)
    {
      printf("(Yaim %.3g Xaim %.3g Distance %.3g FOV %.3g) seem unreasonable\n",
	     Yaim, Xaim, Distance, FOV);
      exit(1);
    };

  FOVrad = FOV*PI/180.0;   /* FOV in radians */
  
  /* Desired spot spacing calculated from user FOV request and Distance */
  SpotSpacing = Pw/(Distance*2.0*tan(FOVrad/2.0));
  
  /* estimate number of spots across image from SpotSpacing and picture width */
  if (SPOTSACROSS<=0) SPOTSACROSS = Pw/SpotSpacing+1;

  PreRectify = (PreRectifyChar == 'Y' || PreRectifyChar =='y');

  logfile = fopen(strcat(img,".log"),"w");  /* open log file */
  img[imgl] = 0;  /* reset file name to base */

  llog("\n");
  j = strlen(imgname);  /* pretty print file name */
  for (i=0; i<j+6; i++) slog[i]='*'; slog[j+6]=0;
  llog("    "); llog(slog); llog("\n");
  llog("    "); llog("*  ");  llog(imgname);  llog("  *\n");
  llog("    "); llog(slog); llog("\n");
  
  llog("\n***   Making " IDline "\n");  
  
  time1 = time(NULL);
  sprintf(slog,"      Image: %s   Date: %s\n",imgname,ctime(&time1));
  plog();

  sprintf(slog,"* Calibration Setup *\n\n");
  plog();
  
  sprintf(slog,"   Image %d high by %d wide, max pixel %d\n",Ph,Pw,maxval);
  plog();
  sprintf(slog,"   Imaging setup  Yaim %.3g  Xaim %.3g  Distance %.3g  FOV %.3g\n",
	  Yaim, Xaim, Distance, FOV);  
  plog();
  
  sprintf(slog,"   Approximate number of horizontal spot spacings across image %d\n",
	  SPOTSACROSS); 
  plog();
  
  if (PreRectify)
    {
      /* set up blindly assumed atan distortion correction */
      CalibPN = 2;   CalibPi = (Ph-1)/2.0;     CalibPj = (Pw-1)/2.0;
      CalibAS = 1.0;    CalibSi = Yaim;   CalibSj = Xaim;
      CalibP[0] = 1.0;  CalibP[1] = 0;   /* use built-in arctangent basis function */
      CalibS[0] = 1.0;  CalibS[1] = 1.0/3.0;  /* first two terms of tangent series */
      CalibAN = 0;
      IdentityMap = 0;  /* and use it to pre-rectify image */
    }
  else
    {
      /* set up identity (null) distortion transform */
      CalibPN = 2;   CalibPi = (Ph-1)/2.0;     CalibPj = (Pw-1)/2.0;
      CalibAS = 1.0;    CalibSi = Yaim;   CalibSj = Xaim;
      CalibP[0] = 0.0;  CalibP[1] = (FOVrad/2)/tan(FOVrad/2);
      CalibS[0] = tan(FOVrad/2)/(FOVrad/2);  CalibS[1] = 0.0;
      CalibAN = 0;
      IdentityMap = 1;  /* forces identity: reports if above not really identity */
    };
  
  llog(PreRectify?"\n   Pre-rectifying assumed distortion\n":
       "\n   Starting with original image: No pre-rectification\n");

   /* Create padded working array, clear it all to "unknown" = 0x80 */
   assert(picpad = (unsigned char **) padmat(Ph,Pw,Pad,Pad,sizeof(unsigned char)));
   for (i=-Pad; i<Ph+Pad; i++) for (j=-Pad; j<Pw+Pad; j++) picpad[i][j] = 0x80;
  
   rings(SPOTRADIUSMAX*2);		/* prepare variable size spot template */

   MaxNspots = Pw*Ph/(4*SPOTRADIUS*SPOTRADIUS)+400;  /* 4 times nominal + 400 */
   
    /* create arrays for spot parameter lists */
    assert(Ispots = (double *) calloc(MaxNspots,sizeof(double)));
    assert(Jspots = (double *) calloc(MaxNspots,sizeof(double)));
    assert(Vspots = (double *) calloc(MaxNspots,sizeof(double)));
    assert(Rspots = (double *) calloc(MaxNspots,sizeof(double)));

    /* create spot local coordinate system lists */
    assert(IIspots = (double *) calloc(MaxNspots,sizeof(double)));
    assert(IJspots = (double *) calloc(MaxNspots,sizeof(double)));
    assert(JIspots = (double *) calloc(MaxNspots,sizeof(double)));
    assert(JJspots = (double *) calloc(MaxNspots,sizeof(double)));

    /* make spot list working storage */
    assert(Itemp = (double *) calloc(MaxNspots,sizeof(double)));
    assert(Jtemp = (double *) calloc(MaxNspots,sizeof(double)));
    assert(Ktemp = (double *) calloc(MaxNspots,sizeof(double)));
    assert(Ltemp = (double *) calloc(MaxNspots,sizeof(double)));

    assert(Aspots = (int *) calloc(MaxNspots,sizeof(int)));

    /* create arrays for spot neighborhoods */
    assert(Dspots = (float **) padmat(MaxNspots,MaxHood,0,0,sizeof(float)));
    assert(DPspots = (int **) padmat(MaxNspots,MaxHood,0,0,sizeof(int)));
    assert(DPerror = (int **) padmat(MaxNspots,5,0,0,sizeof(int)));

    /* make arrays for spot distance sort */
    assert(Dsp = (float *) calloc(MaxNspots,sizeof(float)));
    assert(DPsp = (int *) calloc(MaxNspots,sizeof(int)));
   
    /* Create display image arrays */
    assert(picout = ppm_allocarray(Pw+Pad+Pad,Ph+Pad+Pad));
    assert(graphout = pgm_allocarray(Pw,Ph));  /* make raster for scatter graph */

   /* Transfer in image into working array, fiddle any accidental "unknowns" */
   for (i=0; i<Ph; i++)  for (j=0; j<Pw; j++)
     if ((picpad[i][j] = picin[i][j]) == 0x80) picpad[i][j] = 127+(((i+j) & 1)<<1);

   img[imgn] = '.';  img[imgn+1] = '0';  img[imgn+2] = 0;   imgl = imgn+2;
   Iteration = -1;
   RectifyImage();

   Iteration = 0;  NspotsLast = 10;   Bestbestfit = 1e10;
    /* Stop when too many iterations, or number of found spots stops growing */
    /* Save best fitted function found so far */
    do
      {

	if (Iteration==0) /* wider spot size range */
	  {
	    SPOTRADIUS = (Pw/SPOTSACROSS)/4+1;
	    SPOTRADIUSMAX = floor(SPOTRADIUS*1.5+0.5);
	    SPOTRADIUSMIN = SPOTRADIUS*0.5;
	    if (SPOTRADIUSMIN<1) SPOTRADIUSMIN=1;
	  }
	else  /* narrow spot size range */
	  {
	    SPOTRADIUS = floor(SpotSpacing*0.25 + 0.5);
	    if (SPOTRADIUS<1) SPOTRADIUS=1;
	    SPOTRADIUSMAX = SPOTRADIUS+1;
	    SPOTRADIUSMIN = SPOTRADIUS-1;
	    if (SPOTRADIUSMIN<1) SPOTRADIUSMIN=1;
	  };

	sprintf(slog,"\n\n** ITERATION %c **\n",Iteration+'A');
	plog();

	img[imgn] = '.';  img[imgn+1] = Iteration + 'A';  img[imgn+2] = 0;
	imgl = imgn+2;
	
	/* mark original (unpadded) image extent with yellow outlines */
	for (i = -Pad; i < Ph+Pad; i++)  for (j = -Pad; j < Pw+Pad; j++)
	  { k = picpad[i][j];
	    if(i == -1 || i==Ph || j == -1 || j==Pw)
	      PPM_ASSIGN(PICOUT_(i,j),128+(k>>1),128+(k>>1),(k>>1));
	    else
	      PPM_ASSIGN(PICOUT_(i,j),k,k,k);
	  };

	IMGid = IMGpicout;

	Text(1,Ph+Pad-20,(Pw+2*Pad)/4-Pad,img);  /* annotate display */
	{
	  char temp[500];
	  timeA = time(NULL);
	  sprintf(temp,"flatfish %s %.4g %.4g %.4g %.4g %d      %s",
		  imgname,Yaim,Xaim,Distance,FOV,SPOTSACROSS,ctime(&timeA) );
	  Text(1,Ph+Pad-5,(Pw+2*Pad)/4-Pad,temp);
	};
	Text(1,Ph+Pad-20,Pw+Pad-80,PreRectify?"PRE-RECT":"ORIGINAL");

	/*****************************************************\
	  fit dot templates - find spots with spot operator
	\*****************************************************/
	
	sprintf(slog,"\n* Applying spot operator at every pixel,"
		" radius range %d to %d *\n\n",
		SPOTRADIUSMIN,SPOTRADIUSMAX);
	plog();
	
	time1 = clock() ;  /* time it too - this is the expensive part */
	
	paintspots(SPOTRADIUSMIN, SPOTRADIUSMAX);  /* apply spot operator */
	
	time2 = clock() ;
	sprintf(slog,"   Spot finder time: %g s\n",
	 (time2 - time1)/((double) CLOCKS_PER_SEC));
	plog();
	
	showspots(SPOTRADIUSMIN, SPOTRADIUSMAX); 
	/* overlay the results of the spot operator */
	
	assert(fp = fopen(strcat(img,".spots.ppm"),"w")); /* and write out display */
	img[imgl] = 0;
	ppm_writeppm(fp,picout,Pw+Pad+Pad,Ph+Pad+Pad,255,0);
	fclose(fp);
	
	
	/************************************************************\
	  link up found spots (identified by local max of spot
	  operator) into their natural grid + center spot
	\************************************************************/
	
	
	llog("\n* Finding local maxima of spot operator *\n\n");
	for (i= -Sad; i<Ph+Sad; i++) for (j=-Sad; j<Pw+Sad; j++)
	  if ((v=flP[i][j])>0.0)
	    { rmax = (chP[i][j]*3)/2+1;  /* radius of local max test */
	      for (k=1; k<sprpos[rmax+1]; k++)
		if ((ii = i + spi[k]) >= -Sad) if (ii<Ph+Sad)
		  if ((jj = j + spj[k]) >= -Sad) if (jj<Pw+Sad)
		    if (flP[ii][jj] <= v) flP[ii][jj] = 0.0;  /* erase weaker spots */
	    };
	
	spotmaxlink();  /* link up spot maxima */
	
	/* display rings of appropriate size around the found spots */
	
	for (i=0; i<Nspots; i++)
	  {  
	    ii = floor(Ispots[i] + 0.5);
	    jj = floor(Jspots[i] + 0.5);
	    ringdraw(i==Cspots?3:9,ii,jj,k=chP[ii][jj]);  };
	
	assert(fp = fopen(strcat(img,".rings.ppm"),"w")); /* and write out display */
	img[imgl] = 0;
	ppm_writeppm(fp,picout,Pw+Pad+Pad,Ph+Pad+Pad,255,0);
	fclose(fp);
	
	/* display links between spots determined to be adjacent */
	
	NspotsLinked = 0;
   	for (i=0; i<Nspots; i++) if (i != Cspots && Aspots[i]==2)
	  {
	    NspotsLinked++;
	    for (j=1; j<=Adjacencies; j++) if ((k=DPspots[i][j])>=0)
	      {
		stroke(j,Ispots[i],Jspots[i],
		       (Ispots[k]+Ispots[i]+1)/2,(Jspots[k]+Jspots[i]+1)/2);
	      };
	  };
	
	assert(fp = fopen(strcat(img,".links.ppm"),"w")); /* and write out display */
	img[imgl] = 0;
	ppm_writeppm(fp,picout,Pw+Pad+Pad,Ph+Pad+Pad,255,0);
	fclose(fp);

	if (NspotsLinked < 2*CalibPN+1)  /* can't go on without enough data! */
	  {
	    sprintf(slog,
		    "\n!! Too few spots (%d) linked for degree %d fit - quitting!!\n",
		    NspotsLinked,CalibPN);
	    plog();
	    llog("\n");
	    img[imgn]=0;
	    llog("Calibration file is: ");
	    llog(strcat(img,".calib"));
	    llog("\n\n");
	    fclose(logfile);  /* close log file */
	    exit(0);
	  };
		      
	/***********************************************************\
	  fit distortion curve, and find center to minimize scatter
	\***********************************************************/
	
	llog("\n* Determining optical center and radial distortion *\n\n");
	
	Map_Spot_Pixel_Setup();
	/* done with using old transform now: can muck with the parameters */

	/* replace rectified pixel coords with raw coords */
        for (k=0; k<Nspots; k++) 
	  {
	    /* save coordinates of spots found in rectified image */
	    IJspots[k] = Ispots[k];  JIspots[k] = Jspots[k];
	    /* and also map coordinated back to original image */
	    Map_Spot_to_Pixel(IJspots[k],JIspots[k],&Ispots[k],&Jspots[k]);
	  };

	{ /* check out rectification */
	  int k, kb;

	  kb = 0;
	  for (k = 0; k < Nspots; k++)
	    if (pow(Ispots[k]-Ph/4,2.0)+pow(Jspots[k]-Pw/4,2.0) <
		pow(Ispots[kb]-Ph/4,2.0)+pow(Jspots[kb]-Pw/4,2.0)) kb = k;
	  
	  sprintf(slog,"   Test spot (%d) at %.4g %.4g, rectified from %g %g\n",
		 kb,IJspots[kb],JIspots[kb],Ispots[kb],Jspots[kb]);
	  plog();
	};


	if (Iteration+2>DISTORPOLYDEG) CalibPN = DISTORPOLYDEG;
	else CalibPN = Iteration+2;

	assert(CalibPN < CalibPNMax);

	/* choose initial center, in spot coords, at pixel center of image */
	CalibPi = (Ph-1.0)/2.0;   CalibPj = (Pw-1.0)/2.0;
	PixelToSpotSimple(CalibPi, CalibPj, &CalibSi, &CalibSj);
	CalibAS = 1.0;  /* and initial aspect ratio */

	bestfit = 1e10;  /* Any answer is better than none! */
	bestisc = CalibSi;  bestjsc = CalibSj;  bestaspect = CalibAS;
	bestipc = CalibPi;  bestjpc = CalibPj;

	for (dpass = -1; dpass <= 0; dpass++) /* first guess, then fit */
	  { 
	    int scan;   /* extent of search */
	    double reslo, reshi;
	    
	    /* search for a better image center */

	    scan = dpass?0:1;    /* 1st pass: use initial guess */
	    reslo = 4;  reshi = dpass?reslo:1e-6; /* 2nd pass: optimize */

	    	    
	    for (res = reslo; res >= reshi; res /= 1.414)
	      {
					int l;
					for (l = -scan; l<=scan; l+=1)
		  		for (i = -scan; i<=scan; i+=1) for (j=-scan; j<=scan; j+=1)
		  		for (ii = -scan; ii<=scan; ii+=1) for (jj=-scan; jj<=scan; jj+=1)
		    		{  
		      		/* search for spot center, pixel center, aspect simultaneously */
		      		rootaspect = sqrt(aspect = bestaspect + l*res/10.0);
		      		isc = bestisc + i*res/8.0;
		      		jsc = bestjsc + j*res/8.0;
		      		ipc = bestipc + ii*res*SpotSpacing/8.0;
		      		jpc = bestjpc + jj*res*SpotSpacing/8.0;

		      		di = TryCenter(isc,jsc,ipc,jpc,aspect);

		      		if (di<bestfit)
								{ /* save new best, and its fitted function */
			  					int m;
			  
			  					bestfit = di;  CalibAS = aspect;
			  					CalibSi = isc;  CalibSj = jsc; 
			  					CalibPi = ipc;  CalibPj = jpc;
			  					for (m=0; m<CalibPN; m++) CalibP[m] = Poly[m];  

			  					/* FOR DEBUGGING SEARCH
			    				printf("dpass %d   res %f   Spot r %.4g %.4g   "
			    				"Pixel r %.4g %.4g   As %.4g   Error %g\n",
			    				dpass, res, CalibSi, CalibSj, 
			    				CalibPi, CalibPj, CalibAS, bestfit);   */
		
			  					BasisSet = 1;
			  					dj = fitbasis(CalibPN,NspotsLinked,Itemp,Jtemp,CalibS);
								};
		    		};
					bestisc = CalibSi;  bestjsc = CalibSj; bestaspect = CalibAS;
					bestipc = CalibPi;  bestjpc = CalibPj;
				};

#if (CompareCenters)	    
	    if (dpass==0)
		{ /* compare historical pixel from spot center finders with Search */
		  double ipixA, jpixA, ipixB, jpixB, ipixC, jpixC;
		  
		  /* Fist version: bilinear interpolates 4 neighbor spots,
		     thus very sensitive to spot position errors */
		  ipixA = CalibSi;  jpixA = CalibSj;  spotpixelcoord(&ipixA,&jpixA);

		  /* Second vestion: fits plane to I and to J pixel coordinates
		     as function of spot coords, weighted in vicinity of center */
		  SpotToRectPixelSimple(CalibSi, CalibSj, &ipixB, &jpixB);
		  /* convert rectified to raw coordinates */
		  Map_Spot_to_Pixel(ipixB,jpixB,&ipixB,&jpixB);
 

		  /* Third version: general distortion spot: pixel relation */
		  Map_Spot_to_Pixel((CalibSi-Yaim)*SpotSpacing+(Ph-1)/2.0,
				    (CalibSj-Xaim)*SpotSpacing+(Pw-1)/2.0,
				    &ipixC,&jpixC);

		  /* New version searches for least scatter center in pixel
		     coords as well as spot coords */
		  sprintf(slog,"\n   Spot center [%.4g %.4g] "
			  "to Pixel Center conversion comparison\n\n",
			  CalibSi,CalibSj);
		  plog();
		  llog("     Routine             Pixel Center       	Pixels Offset\n");
		  llog("     --------------	 --------------    	----------------\n");
		  sprintf(slog,"     spotpixelcoord      [%.4g %.4g]"
			  "      	[%.4g %.4g]\n",
			  ipixA,jpixA,ipixA-CalibPi,jpixA-CalibPj);
		  plog();
		  sprintf(slog,"     SpotToPixelSimple   [%.4g %.4g]"
			  "      	[%.4g %.4g]\n",
			  ipixB,jpixB,ipixB-CalibPi,jpixB-CalibPj);
		  plog();
		  sprintf(slog,"     Map_Spot_to_Pixel   [%.4g %.4g]"
			  "      	[%.4g %.4g]\n",
			  ipixC,jpixC,ipixC-CalibPi,jpixC-CalibPj);
		  plog();
		  sprintf(slog,"     Search              [%.4g %.4g]\n",CalibPi,CalibPj);
		  plog();
		};
#endif

	    llog(dpass==0?"\n   Best Center\n":"\n   Guessed Center\n");
	    sprintf(slog,"   Fit function degree %d to %d points\n",
		    CalibPN,NspotsLinked);
	    plog();
	    sprintf(slog,"   aspect %.4g, center S[%.4g %.4g]==P[%.4g %.4g]",
		    CalibAS,CalibSi,CalibSj,CalibPi,CalibPj);
	    plog();
	    sprintf(slog," --> %.4g (%.4g) pixel rms error\n",
		    bestfit,bestfit*NspotsLinked/(NspotsLinked-CalibPN));
	    plog();
	    llog("   S->P Function: ");
	    for (i=0; i<CalibPN; i++) 
	      { sprintf(slog,"  %.4g",CalibP[i]); plog(); }
	    llog("\n");  llog("   P->S Function: ");
	    for (i=0; i<CalibPN; i++)  { sprintf(slog,"  %.4g",CalibS[i]); plog(); }
	    llog("\n");

	    SpotToRectPixelSimple(isc,jsc,&di,&dj);
	    /* Map raw center coordinates to recent rectified diaplay coords */
	    if (dpass) starburst(4,di,dj,7);  /* draw in initial guess of lens axis */
	    else starburst(2,di,dj,12);	/* draw in best fit lens axis */

	    /* recalculate pixel and spot radii of best fit, and make scatter graph */

	    TryCenter(CalibSi,CalibSj,CalibPi,CalibPj,CalibAS);
	    ScatterGraph(dpass?"guess":"fit");

	  };
	  
	/* write out display of centers, with center offset annotation */
	{
	  char temp[500];
	  IMGid = IMGpicout;
	  sprintf(temp,"%s    --   Optical center offset [%.4g %.4g] pixels",img,
		  CalibPi-(Ph-1)/2.0,CalibPj-(Pw-1)/2.0);
	  Text(1,Ph+Pad-20,(Pw+2*Pad)/4-Pad,temp);
	};

	assert(fp = fopen(strcat(img,".center.ppm"),"w")); 
	img[imgl] = 0;
	ppm_writeppm(fp,picout,Pw+Pad+Pad,Ph+Pad+Pad,255,0);
	fclose(fp);

	if (Iteration>0)
	  for (i=1; i<CalibPN;  i++)
	    if (CalibP[i] > 2 || CalibP[i] < -2 || CalibS[i] > 4 || CalibS[i] < -4)
	      {
		llog("\n   Polynomial coefficient too large\n"
		    "   Probably nearing singularity\n"
		    "   Quitting while ahead!\n");
		llog("\n");
		img[imgn] = 0;
		llog("Calibration file is: ");
		llog(strcat(img,".calib"));
		llog("\n\n");
		fclose(logfile);  /* close log file */
		exit(0);
	      };

	/* At this point, [Ispots,Jspots] holds pixel coordinates of spots
	   [IIspots,JJspots] holds spot coordinates of spots
	   Itemp holds pixel radius from optical axis for each spot
	   Jtemp holds spot radius from optical axis for each spot   */
	
	/*****************************************************************\
	Determine angle from plumb of radially corrected spot grid 
	\*****************************************************************/
	
	llog("\n* Determining angle of grid from horizontal (vertical) *\n\n");
	{ 
	  double x, y, Sx, Sy, Sxx, Syy, Sxy, A, B;
	  double rratio;
	  double Cmin, Cmax, Cur, t;
	  int pass;
	  
	  /* clear scatter graph */
	  for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) graphout[i][j] = 128;
	  
	  Sxx = Syy = Sxy = 0.0;  /* quadratic sums, for angle solution */
	  for (pass = 0; pass <= 1; pass++)  /* do rows, then columns */
	    {
	      /* find bounds of spot coordinates */
	      Cmin = Cmax = pass?JJspots[0]:IIspots[0];
	      for (k=0; k<Nspots; k++) if (Aspots[k]==2 && k!=Cspots)
		{   
		  t = pass?JJspots[k]:IIspots[k];
		  if (t<Cmin) Cmin = t;  if (t>Cmax) Cmax = t;  
		};
	      
	      llog(pass?"   Cols":"   Rows"); 
	      sprintf(slog," [%g:%g] ",Cmin,Cmax); 
	      plog();
	      for (Cur = Cmin; Cur <= Cmax; Cur += 0.5)
		{
		  int n;
		  
		  n = 0;  Sx = Sy = 0;
		  for (k=0; k<Nspots; k++) if (Aspots[k]==2 && k!=Cspots)
		    { /* survey, and compute means */
		      if ((pass?JJspots[k]:IIspots[k]) == Cur)
			{   /* project spot radius onto pixel angle,
			       thus removing radial distortion, but
			       keeping rotation */

			  rratio = 
			    sqrt( (pow(IIspots[k]-CalibSi,2.0)
				   +pow(JJspots[k]-CalibSj,2.0))/
				  (pow(Ispots[k]-CalibPi,2.0)*CalibAS
				   +pow(Jspots[k]-CalibPj,2.0)/CalibAS)    );
			  x = (Jspots[k]-CalibPj)*rratio;
			  y = (Ispots[k]-CalibPi)*rratio;
			  Sx += x;  Sy += y;  n++;  
			};
		    };
		  
		  if (n>3) /* if row/column has enough members, add in its sums */
		    {   
		      sprintf(slog," %d",n); plog();
		      Sx /= n;   Sy /= n;
		      for (k=0; k<Nspots; k++) if (Aspots[k]==2 && k!=Cspots)
			{ /* subtract means, compute sums */
			  if ((pass?JJspots[k]:IIspots[k]) == Cur)
			    {   /* project spot radius onto pixel angle,
				   thus removing radial distortion, but
				   keeping rotation */
			      rratio = 
				sqrt( (pow(IIspots[k]-CalibSi,2.0)
				       +pow(JJspots[k]-CalibSj,2.0))/
				      (pow(Ispots[k]-CalibPi,2.0)*CalibAS
				       +pow(Jspots[k]-CalibPj,2.0)/CalibAS)    );
			      x = (Jspots[k]-CalibPj)*rratio;
			      y = (Ispots[k]-CalibPi)*rratio;
			      x -= Sx;  y -= Sy;
			      if (pass) { t = -x; x = y; y = t;};
			      i = y*Pw/20 + (Ph-1)/2.0;  j = x*Pw/20 + (Pw-1)/2.0;
			      if (i>=0 && i < Ph && j>=0 && j<Pw)
				graphout[i][j] = pass?255:0;
			      Sxx += x*x;  Syy += y*y;  Sxy += x*y;	       
			    };
			};
		    };
		};
	      llog("\n");
	    };
	  A = Sxy;   B = Syy - Sxx;  /* closed-form solution, in terms of sums */
	  
	  assert(fp = fopen(strcat(img,".tilt.pgm"),"w"));

	  IMGid = IMGgraphout;
	  Text(1,Ph-20,Pw/4,img);
	  {
	    char temp[500];
	    timeA = time(NULL);
	    sprintf(temp,"flatfish %s %.4g %.4g %.4g %.4g %d      %s",
		    imgname,Yaim,Xaim,Distance,FOV,SPOTSACROSS,ctime(&timeA) );
	    Text(1,Ph-5,Pw/4,temp);
	  };
	  Text(1,Ph-20,Pw-80,PreRectify?"PRE-RECT":"ORIGINAL");

	  img[imgl] = 0;
	  pgm_writepgm(fp,graphout,Pw,Ph,255,0);       /* and write out graph */
	  fclose(fp);
	  
	  CalibAN = atan2(-B+(B<0?1:-1)*sqrt(B*B+4.0*A*A),2.0*A);
	  /* out of all the minimum, maximum, flipped, solutions, pick the one
	     that requires the least rotation of the original grid */
	  while (CalibAN>PI/4) CalibAN -= PI/2;
	  while (CalibAN<-PI/4) CalibAN += PI/2;
	  
	  sprintf(slog,"   Grid angle = %.4g degrees\n",CalibAN*180/PI); plog();
	  
	};
	
	/* Write out all calibration parameters into .calib file */
	llog("\n* Writing out calibration file *");

	WriteCalibFile(strcat(img,".calib"));   img[imgl] = 0;
	
	/* If best calibration so far, write it out under plain .calib name also */
	if (bestfit*NspotsLinked/(NspotsLinked-CalibPN)<Bestbestfit
	    || NspotsLinked>NspotsLast+4)
	  {
	    char imgcpy[2*IMGNMAX];

	    llog("    (best so far)");

	    strcpy(imgcpy,img);  imgcpy[imgn] = 0;
	    WriteCalibFile(strcat(imgcpy,".calib"));

	    Bestbestfit = bestfit*NspotsLinked/(NspotsLinked-CalibPN);
	  };

	llog("\n");
	llog("\n* Rectifying image *\n\n");
	
	IdentityMap = 0;  /* Now using calculated rectifications for sure */
	RectifyImage();

	Iteration++;  
	IterateMore = Iteration<3 || (Iteration<5 && NspotsLinked>NspotsLast);
	NspotsLast = NspotsLinked;
      }  
    while (IterateMore);

    if (Iteration>=5) 
      llog("   \n Enough iterations, already!\n");
    else llog("\n   Spot count didn't grow: we're done!\n");
    llog("\n");
    img[imgn]=0;
    llog("Calibration file is: ");
    llog(strcat(img,".calib"));
    llog("\n\n");
    fclose(logfile);  /* close log file */
    return(0);


}