// geopix.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <vector>
#include "matrix3.h"
using std::vector;

#define RAD2DEG 57.29578f
#define DEG2RAD 0.017453293f

struct LCM_param {
  float a,b,c,d;
  int parity;
};

struct GeoPoint : Vector3 {
  static float radius_h,radius_v;
  const char *fscanf(FILE *);
};

static void backtrans(const Vector3& input, const Vector3& north,
		      const Vector3& east, const Vector3& up,
		      Vector3& output)
{
  for (unsigned i=0; i<3; ++i)
    output.value[i]=
	north.value[i]*input.value[0]+
	east .value[i]*input.value[1]+
	up   .value[i]*input.value[2];
}

const char *GeoPoint::fscanf(FILE *f)
{
  float latd,lond,alt;
  if (::fscanf(f,"%f%f%f",&latd,&lond,&alt)<3)
    return "Lat/lon/alt read failed";
  set_unit(latd*DEG2RAD,lond*DEG2RAD);
  operator*=(1+alt/radius_v);
  return 0;
}

float GeoPoint::radius_h=0,GeoPoint::radius_v=0;

struct PixPoint {
  float pixX,pixY,azim,elev,h_dist2;
  GeoPoint location;
  Vector3 sight;
  char *name;
  const char *fscanf(FILE *);
  const char *fscanf(FILE *, float&);
  const char *scan_pix_name(FILE *);
  void set_sight(const Vector3&, const Vector3&,
		 const Vector3&, const Vector3&);
  void set_sight(const Vector3&, Vector3, Vector3,
		 const LCM_param&, float, float, float);
  void set_refrac();
  PixPoint() {name=0;}
  PixPoint(const PixPoint& p) : location(p.location),sight(p.sight),
    pixX(p.pixX),pixY(p.pixY),azim(p.azim),elev(p.elev),h_dist2(p.h_dist2),
    name(strdup(p.name)) {}
  ~PixPoint() {delete[] name;}
};

const char *PixPoint::scan_pix_name(FILE *f)
{
  char buf[64],c;
  if (::fscanf(f,"%f%f",&pixX,&pixY)<2)
    return strerror(errno);
  while (isspace(c=fgetc(f)));
  if (c==EOF) return "No name for point";
  ungetc(c,f);
  fgets(buf,sizeof buf,f);
  char *nl=strchr(buf,'\n');
  if (!nl) return "Background point name too long or not terminated";
  *nl='\0';
  delete[] name; // just in case there was already something there
  name=new char[strlen(buf)+1];
  strcpy(name,buf);
  return 0;
}

const char *PixPoint::fscanf(FILE *f)
{
  const char *error=location.fscanf(f);
  if (error) return error;
  return scan_pix_name(f);
}

const char *PixPoint::fscanf(FILE *f, float& dist)
{
  if (::fscanf(f,"%f",&dist)<1)
    return strerror(errno);
  dist/=GeoPoint::radius_h;
  return scan_pix_name(f);
}

void PixPoint::set_refrac()
{
  h_dist2=sight.value[0]*sight.value[0]+sight.value[1]*sight.value[1];
}

void PixPoint::set_sight(const Vector3& photo_point, const Vector3& up,
			 const Vector3& north, const Vector3& east)
{
  // compute the sight vector
  Vector3 sight_ECEF=location;
  sight_ECEF-=photo_point;
  sight.value[0]=sight_ECEF*north;
  sight.value[1]=sight_ECEF*east;
  sight.value[2]=sight_ECEF*up;
  set_refrac();
}

void PixPoint::set_sight(const Vector3& photo_axis, Vector3 photo_up,
			 Vector3 photo_cross, const LCM_param& lcmp,
			 float camres2i, float dist, float rf_curv)
{
  // compute azimuth and elevation angle of pixel coordinates
  float dx=pixX-lcmp.c,dy=pixY-lcmp.d;
  azim=( dx*lcmp.a+lcmp.parity*dy*lcmp.b)*camres2i;
  elev=(-dx*lcmp.b+lcmp.parity*dy*lcmp.a)*camres2i;
  // compute sight vector based on pixel coordinates and distance
  photo_up*=elev;
  photo_cross*=azim;
  sight=photo_axis;
  sight+=photo_up;
  sight+=photo_cross;
  sight*=dist/(float)sqrt(sight*sight);
  set_refrac();
  sight.value[2]-=h_dist2*rf_curv;
}

static float mod_diff(float z, const Vector3& y)
{
  Vector3 xy=y;
  xy.value[2]+=z;
  float D=z+(float)sqrt(xy*xy);
  xy.value[2]+=z;
  return (xy*y)/D;
}

static int LCM(float ax, float ay, float ax2, float ay2,
		float au, float av, float aux, float auy, float avx, float avy,
		LCM_param& param)
{
  float Di=1/(ax2+ay2);
  int p_rec=aux*avy>auy*avx?1:-1;
  if (param.parity==0) param.parity=p_rec;
  param.a=(aux+param.parity*avy)*Di;
  param.b=(-auy+param.parity*avx)*Di;
  param.c=au-param.a*ax+param.b*ay;
  param.d=av-param.parity*(param.a*ay+param.b*ax);
  return p_rec;
}

struct Parameter {
  unsigned n;
  float *x_deriv,*y_deriv,a_deriv,b_deriv,sx,sy;
  Parameter() : n(0),x_deriv(0) {}
  ~Parameter() {delete[] x_deriv;}
  void init(unsigned n_)
    {n=n_; x_deriv=new float[2*n_]; y_deriv=x_deriv+n_; sx=sy=0;}
  void set_xy_deriv(unsigned i, float x, float y)
    {sx+=x_deriv[i]=x; sy+=y_deriv[i]=y;}
  void subtract_avg();
  void set_ab_deriv(const vector<PixPoint>&, float, float, float, float,
		    const LCM_param&, float);
  void predict_deriv(unsigned, const LCM_param&, float, float, float&, float&) const;
  float Q_deriv(const vector<PixPoint>&, float, float, float, float,
		const LCM_param&);
};

void Parameter::subtract_avg()
{
  float ax=sx/n,ay=sy/n;
  for (unsigned i=0; i<n; ++i) {
    x_deriv[i]-=ax;
    y_deriv[i]-=ay;
  }
}

void Parameter::set_ab_deriv(const vector<PixPoint>& bgpts,
			     float au, float av, float ax, float ay,
			     const LCM_param& lcmp, float D)
{
  float du,dv,dx,dy,ca=0,cb=0,c0=0;
  for (unsigned i=0; i<n; ++i) {
    const PixPoint& pixp=bgpts[i];
    du=pixp.pixX-au;
    dv=(pixp.pixY-av)*lcmp.parity;
    dx=pixp.azim-ax;
    dy=pixp.elev-ay;
    float &xd=x_deriv[i],&yd=y_deriv[i];
    ca+=du*xd+dv*yd;
    cb+=dv*xd-du*yd;
    c0+=dx*xd+dy*yd;
  }
  D=1/(D*n);
  a_deriv=(ca-2*lcmp.a*c0)*D;
  b_deriv=(cb-2*lcmp.b*c0)*D;
}

void Parameter::predict_deriv(unsigned i, const LCM_param& lcmp,
			      float dx, float dy,
			      float& u_deriv, float& v_deriv) const
{
  u_deriv=lcmp.a*x_deriv[i]+a_deriv*dx-lcmp.b*y_deriv[i]-b_deriv*dy;
  v_deriv=lcmp.b*x_deriv[i]+b_deriv*dx+lcmp.a*y_deriv[i]+a_deriv*dy;
}

float Parameter::Q_deriv(const vector<PixPoint>& bgpts,
			 float au, float av, float ax, float ay,
			 const LCM_param& lcmp)
{
  float du,dv,dx,dy,Qd=0;
  for (unsigned i=0; i<n; ++i) {
    const PixPoint& pixp=bgpts[i];
    du=pixp.pixX-au;
    dv=(pixp.pixY-av)*lcmp.parity;
    dx=pixp.azim-ax;
    dy=pixp.elev-ay;
    Qd+=(lcmp.a*dx-lcmp.b*dy-du)*
	(lcmp.a*x_deriv[i]+a_deriv*dx-lcmp.b*y_deriv[i]-b_deriv*dy)
       +(lcmp.b*dx+lcmp.a*dy-dv)*
	(lcmp.b*x_deriv[i]+b_deriv*dx+lcmp.a*y_deriv[i]+a_deriv*dy);
  }
  return 2*Qd;
}

struct ParamPair : Parameter {
  void set_ab_deriv(const Parameter&, const Parameter&,
		    const vector<PixPoint>&, float, float, float, float,
		    const LCM_param&, float);
  float Q_deriv(const Parameter&, const Parameter&,
		const vector<PixPoint>&, float, float, float, float,
		const LCM_param&);
};

void ParamPair::set_ab_deriv(const Parameter& p1, const Parameter& p2,
			     const vector<PixPoint>& bgpts,
			     float au, float av, float ax, float ay,
			     const LCM_param& lcmp, float D)
{
  float du,dv,dx,dy,ca=0,cb=0,c0=0,c1=0,c2=0;
  for (unsigned i=0; i<n; ++i) {
    const PixPoint& pixp=bgpts[i];
    du=pixp.pixX-au;
    dv=(pixp.pixY-av)*lcmp.parity;
    dx=pixp.azim-ax;
    dy=pixp.elev-ay;
    float &p1x=p1.x_deriv[i],&p1y=p1.y_deriv[i],
      &p2x=p2.x_deriv[i],&p2y=p2.y_deriv[i],
      &xd=x_deriv[i],&yd=y_deriv[i];
    ca+=du*xd+dv*yd;
    cb+=dv*xd-du*yd;
    c0+=dx*xd+dy*yd+p1x*p2x+p1y*p2y;
    c1+=dx*p2x+dy*p2y;
    c2+=dx*p1x+dy*p1y;
  }
  D=1/(D*n);
  a_deriv=(ca-2*(lcmp.a*c0+p1.a_deriv*c1+p2.a_deriv*c2))*D;
  b_deriv=(cb-2*(lcmp.b*c0+p1.b_deriv*c1+p2.b_deriv*c2))*D;
}

float ParamPair::Q_deriv(const Parameter& p1, const Parameter& p2,
			 const vector<PixPoint>& bgpts,
			 float au, float av, float ax, float ay,
			 const LCM_param& lcmp)
{
  float du,dv,dx,dy,Qd=0;
  for (unsigned i=0; i<n; ++i) {
    const PixPoint& pixp=bgpts[i];
    du=pixp.pixX-au;
    dv=(pixp.pixY-av)*lcmp.parity;
    dx=pixp.azim-ax;
    dy=pixp.elev-ay;
    float &p1x=p1.x_deriv[i],&p1y=p1.y_deriv[i],
      &p2x=p2.x_deriv[i],&p2y=p2.y_deriv[i];
    Qd+=(lcmp.a*dx-lcmp.b*dy-du)*
	(lcmp.a*x_deriv[i]+p1.a_deriv*p2x+p2.a_deriv*p1x+a_deriv*dx
	  -lcmp.b*y_deriv[i]-p1.b_deriv*p2y-p2.b_deriv*p1y-b_deriv*dy)
       +(lcmp.a*p1x+p1.a_deriv*dx-lcmp.b*p1y-p1.b_deriv*dy)*
	(lcmp.a*p2x+p2.a_deriv*dx-lcmp.b*p2y-p2.b_deriv*dy)
       +(lcmp.b*dx+lcmp.a*dy-dv)*
	(lcmp.b*x_deriv[i]+p1.b_deriv*p2x+p2.b_deriv*p1x+b_deriv*dx
	  +lcmp.a*y_deriv[i]+p1.a_deriv*p2y+p2.a_deriv*p1y+a_deriv*dy)
       +(lcmp.b*p1x+p1.b_deriv*dx+lcmp.a*p1y+p1.a_deriv*dy)*
	(lcmp.b*p2x+p2.b_deriv*dx+lcmp.a*p2y+p2.a_deriv*dy);
  }
  return 2*Qd;
}

static bool cholesky(float a[3][3])
{
  unsigned i,j,k;
  for (i=0; i<3; ++i) {
    float& x=a[i][i];
    for (j=0; j<i; ++j)
      x-=a[i][j]*a[i][j];
    if (x<=0) return false;
    x=sqrt(x);
    for (j=i+1; j<3; ++j) {
      float& y=a[j][i];
      for (k=0; k<i; ++k) {
	y-=a[j][k]*a[i][k];
      }
      y/=x;
    }
  }
  return true;
}

static void solve(const float a[3][3], const float b[3], float x[3])
{
  int i,j;
  for (i=0; i<3; ++i) {
    float& y=x[i];
    y=b[i];
    for (j=0; j<i; ++j) {
      y-=a[i][j]*x[j];
    }
    y/=a[i][i];
  }
  for (i=2; i>=0; --i) {
    float& y=x[i];
    for (j=i+1; j<3; ++j) {
      y-=a[j][i]*x[j];
    }
    y/=a[i][i];
  }
}

static void solve(const float a[2][2], const float b[2], float x[2], float D)
{
  x[0]=(b[0]*a[1][1]-a[0][1]*b[1])/D;
  x[1]=(b[1]*a[0][0]-a[1][0]*b[0])/D;
}

static void transpose(float a[3][3])
{
  unsigned j,k;
  float y;
  for (j=1; j<3; ++j) {
    for (k=0; k<j; ++k) {
      y=a[j][k];
      a[j][k]=a[k][j];
      a[k][j]=y;
    }
  }
}
static void transpose(float a[2][2])
{
  float y=a[0][1];
  a[0][1]=a[1][0];
  a[1][0]=y;
}

static float rqf(const float a[3][3], const float x[3])
{
  unsigned i,j;
  float y=0;
  for (i=0; i<3; ++i)
    for (j=0; j<3; ++j)
      y+=x[i]*a[i][j]*x[j];
  return y;
}

int main(int argc, char *argv[])
{
  char fname[FILENAME_MAX+1],c;
  const char *error=0;
  FILE *f;
  if (argv[1]) strcpy(fname,argv[1]);
  else {
    // prompt user for file name, if not provided in command line
    printf("Enter data file name: ");
    gets(fname);
    if (!*fname) {
#ifdef NDEBUG
      printf("\nPress <enter>");
      gets(fname);
#endif
      return 1;
    }
  }
  // open the file
  f=fopen(fname,"r");
  if (!f) {
    perror(fname);
#ifdef NDEBUG
    printf("\nPress <enter>");
    gets(fname);
#endif
    return 2;
  }

  float ax=0,ay=0,ax2=0,ay2=0,axy=0,
    axa=0,axe=0,aya=0,aye=0,aa=0,ae=0,aa2=0,ae2=0,aae=0,
    sig_aa,sig_ab,sig_bb,sig_azaz,sig_azel,sig_elel,
    da,de,dx,dy,ni,dist,rf_curv,camres,camresi,camres2i,camaz,camel,
    h_disti,a2,ab,b2;
  GeoPoint photo_point;
  Parameter cam_pos[3],pa_param[2],rf_param;
  ParamPair cam_pos2[3][3],pa_param2[2][2],rf_param2;
  float d1Qcp[3],d2Qcp[3][3],d1Qpa[2],d2Qpa[2][2],d1Qrf,d2Qrf;
  Vector3 photo_axis,photo_up,photo_cross,up,east,north,vec1;
  unsigned i,nbgpt=0,j,k,n_opt_par;
  int p_rec,nrdf;
  LCM_param lcmp;
  vector<PixPoint> bgpts;
  PixPoint test;

  // read global parameters
  if (fscanf(f,"%f%f%f%d%d",
      &GeoPoint::radius_h,&GeoPoint::radius_v,&rf_curv,
      &lcmp.parity,&n_opt_par)<5) {
    error=strerror(errno);
    goto exit;
  }
  rf_curv*=0.5;
  // read camera axis
  if (fscanf(f,"%f%f",&camaz,&camel)<2) {
    error=strerror(errno);
    goto exit;
  }

  // read photo point
  if (error=photo_point.fscanf(f))
    goto exit;
  up=photo_point;
  up.normalize();
  east.value[0]=-photo_point.value[1];
  east.value[1]=photo_point.value[0];
  east.value[2]=0;
  east.normalize();
  north.cross(up,east);
  photo_axis.value[0]=photo_axis.value[1]=photo_axis.value[2]=0;
  while (c=fgetc(f),c!='#') {
    ungetc(c,f);
    // read BG point
    bgpts.resize(++nbgpt);
    PixPoint& pixp=bgpts.back();
    if (error=pixp.fscanf(f))
      goto exit;
    pixp.set_sight(photo_point,up,north,east);
    // sum pixel positions for statistics analysis
    ax+=pixp.pixX;
    ay+=pixp.pixY;
    // sum the normalized sight vector into the initial photo axis
    vec1=pixp.sight;
    vec1.normalize();
    photo_axis+=vec1;
  }
  for (j=0; j<3; ++j) {
    cam_pos[j].init(nbgpt);
    for (k=0; k<3; ++k) cam_pos2[j][k].init(nbgpt);
  }
  for (j=0; j<2; ++j) {
    pa_param[j].init(nbgpt);
    for (k=0; k<2; ++k) pa_param2[j][k].init(nbgpt);
  }
  rf_param.init(nbgpt);
  rf_param2.init(nbgpt);
  ni=1.0f/nbgpt;
  ax*=ni;
  ay*=ni;
  if (camaz<0) {
    // user wants the photo axis to be the sight vector average
    photo_axis.normalize();
  }
  else {
    // user specifies photo axis
    photo_axis.set_unit(camel*DEG2RAD,camaz*DEG2RAD);
  }
  photo_up.value[0]=photo_up.value[1]=0;
  photo_up.value[2]=1;
  photo_up.orthog(photo_axis);
  photo_cross.cross(photo_up,photo_axis);
  // collect sums for statistics analysis
  for (i=0; i<nbgpt; ++i) {
    PixPoint& pixp=bgpts[i];
    Vector3& rf_sight=vec1;
    rf_sight=pixp.sight;
    rf_sight.value[2]+=pixp.h_dist2*rf_curv;
    float zi=rf_sight*photo_axis,zi2,xd,yd,xd2,yd2;
    if (zi<=0) {
      fprintf(stderr,"%s\n",pixp.name);
      error="Background point is on back side of camera";
      goto exit;
    }
    zi=1/zi;
    zi2=zi*zi;
    // set camera position derivatives
    aa+=pixp.azim=(rf_sight*photo_cross)*zi;
    ae+=pixp.elev=(rf_sight*photo_up)*zi;
    for (j=0; j<3; ++j) {
      float &paj=photo_axis.value[j],&pcj=photo_cross.value[j],
	&puj=photo_up.value[j];
      xd=(pcj-pixp.azim*paj)*zi;
      yd=(puj-pixp.elev*paj)*zi;
      cam_pos[j].set_xy_deriv(i,xd,yd);
      for (k=0; k<3; ++k) {
	float &pak=photo_axis.value[k],&pck=photo_cross.value[k],
	  &puk=photo_up.value[k];
	xd2=(2*pixp.azim*paj*pak-pck*paj-pcj*pak)*zi2;
	yd2=(2*pixp.elev*paj*pak-puk*paj-puj*pak)*zi2;
	cam_pos2[j][k].set_xy_deriv(i,xd2,yd2);
      }
    }
    // use values of xd, yd, xd2, yd2 left over from j=k=2
    // to set refraction curvature derivatives
    rf_param.set_xy_deriv(i,pixp.h_dist2*xd/2,pixp.h_dist2*yd/2);
    rf_param2.set_xy_deriv(i,pixp.h_dist2*xd2/2,pixp.h_dist2*yd2/2);
    // set photo axis derivatives
    pa_param[0].set_xy_deriv(i,-1-pixp.azim*pixp.azim,-pixp.elev*pixp.azim);
    pa_param[1].set_xy_deriv(i,-pixp.azim*pixp.elev,-1-pixp.elev*pixp.elev);
    pa_param2[0][0].set_xy_deriv(i,
      2*pixp.azim*(1+pixp.azim*pixp.azim),
      pixp.elev*(1+2*pixp.azim*pixp.azim));
    pa_param2[0][1].set_xy_deriv(i,
      pixp.elev*(0.5+2*pixp.azim*pixp.azim),
      pixp.azim*(0.5+2*pixp.elev*pixp.elev));
    pa_param2[1][0].set_xy_deriv(i,
      pixp.elev*(0.5+2*pixp.azim*pixp.azim),
      pixp.azim*(0.5+2*pixp.elev*pixp.elev));
    pa_param2[1][1].set_xy_deriv(i,
      pixp.azim*(1+2*pixp.elev*pixp.elev),
      2*pixp.elev*(1+pixp.elev*pixp.elev));
  }
  aa*=ni;
  ae*=ni;
  for (j=0; j<3; ++j) {
    cam_pos[j].subtract_avg();
    for (k=0; k<3; ++k)
      cam_pos2[j][k].subtract_avg();
  }
  for (j=0; j<2; ++j) {
    pa_param[j].subtract_avg();
    for (k=0; k<2; ++k)
      pa_param2[j][k].subtract_avg();
  }
  rf_param.subtract_avg();
  rf_param2.subtract_avg();
  for (i=0; i<nbgpt; ++i) {
    const PixPoint& pixp=bgpts[i];
    da=pixp.azim-aa;
    de=pixp.elev-ae;
    dx=pixp.pixX-ax;
    dy=pixp.pixY-ay;
    axa+=dx*da;
    axe+=dx*de;
    aya+=dy*da;
    aye+=dy*de;
    aa2+=da*da;
    ae2+=de*de;
    aae+=da*de;
  }
  axa*=ni;
  axe*=ni;
  aya*=ni;
  aye*=ni;
  aa2*=ni;
  ae2*=ni;
  aae*=ni;
  // statistical analysis
  p_rec=LCM(aa,ae,aa2,ae2,ax,ay,axa,axe,aya,aye,lcmp);
  a2=lcmp.a*lcmp.a;
  ab=lcmp.a*lcmp.b;
  b2=lcmp.b*lcmp.b;
  camres=(float)sqrt(a2+b2);
  camresi=1/camres;
  camres2i=1/(a2+b2);
  // user review of BG points
  printf("%d BACKGROUND POINTS\n",nbgpt);
  printf("                    ElevAngle              Residue\n"
	 "Azimuth  Distance  Actul Aprnt  Pix Res    X     Y    Name\n"
	 "=======  ========  ===== =====  =======  ===== =====  ================\n");
  for (i=0; i<nbgpt; ++i) {
    const PixPoint& pixp=bgpts[i];
    dist=(float)sqrt(pixp.sight*pixp.sight);
    h_disti=1/(float)sqrt(pixp.h_dist2);
    da=pixp.azim-aa;
    de=pixp.elev-ae;
    dx=lcmp.a*da-lcmp.b*de+ax-pixp.pixX;
    dy=(lcmp.b*da+lcmp.a*de)*lcmp.parity+ay-pixp.pixY;
    printf("%6.2f   %#7.3g   %5.2f %5.2f  %#6.3g   %5.1f %5.1f  %s\n",
	   RAD2DEG*pixp.sight.phi_positive(),
	   GeoPoint::radius_h*dist,
	   RAD2DEG*atan(pixp.sight.value[2]*h_disti),
	   RAD2DEG*atan((pixp.sight.value[2]+pixp.h_dist2*rf_curv)*h_disti),
	   GeoPoint::radius_v*dist*camresi,
	   dx,dy,pixp.name);
    ax2+=dx*dx;
    axy+=dx*dy;
    ay2+=dy*dy;
  }
  nrdf=2*nbgpt-4-n_opt_par;
  if (nrdf<=0) {
    printf("\nStatistical residues are not available because\n"
	   "there is no redundancy in the background points.\n");
  }
  else {
    float ndi=2.0f/nrdf;
    ax2*=ndi;
    ay2*=ndi;
    axy*=ndi;
    sig_azaz=(a2*ax2+2*ab*lcmp.parity*axy+b2*ay2);
    sig_elel=(b2*ax2-2*ab*lcmp.parity*axy+a2*ay2);
    sig_azel=(lcmp.parity*axy*(a2-b2)+ab*(ay2-ax2));
    float ax2r=sqrt(ax2),ay2r=sqrt(ay2),pm,D,
      a2r=sqrt(sig_azaz),e2r=sqrt(sig_elel);
    D=aa2+ae2;
    D=1/(D*D*nbgpt);
    sig_aa=(ax2*aa2+ay2*ae2+2*axy*aae*lcmp.parity)*D;
    sig_ab=((ay2-ax2)*aae+axy*(aa2-ae2))*D;
    sig_bb=(ax2*ae2+ay2*aa2-2*lcmp.parity*axy*aae)*D;
    D=(ax2-ay2)*0.5;
    D=sqrt(D*D+axy*axy);
    pm=(ax2+ay2)*0.5;
    printf("\nSTATISTICAL RESIDUES\n"
	   "            Degrees  Pixels\n"
	   "Total       %#7.4g  %#7.4g\n"
	   "Image X     %#7.4g  %#7.4g\n"
	   "Image Y     %#7.4g  %#7.4g\n"
	   "Corr(X,Y)       %#7.4g\n"
	   "Principals  %#7.4g  %#7.4g\n"
	   "            %#7.4g  %#7.4g\n"
	   "Azimuth     %#7.4g  %#7.4g\n"
	   "ElevAngle   %#7.4g  %#7.4g\n"
	   "Corr(A,E)       %#7.4g\n",
	   RAD2DEG*sqrt(ax2+ay2)*camresi,sqrt(ax2+ay2),
	   RAD2DEG*ax2r*camresi,ax2r,RAD2DEG*ay2r*camresi,ay2r,
	   axy/(ax2r*ay2r),
	   RAD2DEG*sqrt(pm+D)*camresi,sqrt(pm+D),
	   RAD2DEG*sqrt(pm-D)*camresi,sqrt(pm-D),
	   RAD2DEG*camres2i*a2r,a2r*camresi,
	   RAD2DEG*camres2i*e2r,e2r*camresi,
	   sig_azel/(a2r*e2r));
    float v2=GeoPoint::radius_v*GeoPoint::radius_v;
  } // redundant degrees of freedom available for error analysis
  printf("\nCAMERA INFORMATION\n");
  printf("Azimuth: %.2f\nElevAngle: %.2f\n"
	 "Tilt: %.2f, error %.2f\nResolution: %.2f pix/deg, error %.2f\n",
	 RAD2DEG*photo_axis.phi_positive(),
	 RAD2DEG*photo_axis.theta(),
	 RAD2DEG*atan(lcmp.b/lcmp.a),
	 nrdf>0 ? RAD2DEG*sqrt(a2*sig_bb+b2*sig_aa-2*ab*sig_ab)*camres2i : -1,
	 DEG2RAD*camres,
	 nrdf>0 ? DEG2RAD*sqrt(a2*sig_aa+b2*sig_bb+2*ab*sig_ab)*camresi : -1);
  printf("Pixel coordinates of photo axis: %.1f %.1f\n",lcmp.c,lcmp.d);
  printf("Image X-axis increases %s\n",lcmp.a>0?"right":"left");
  printf("Image Y-axis increases %s\n",lcmp.a*lcmp.parity>0?"up":"down");
  printf("Parity: ");
  if (lcmp.parity==p_rec) printf("%d\n",lcmp.parity);
  else printf("%d recommended, %d used\n",p_rec,lcmp.parity);
  if (nrdf>0) {
    printf("\nPARAMETER CORRECTIONS (%d redundant degrees of freedom)\n",nrdf);
    printf("Camera position:");
    // compute camera position gradients
    for (j=0; j<3; ++j) {
      cam_pos[j].set_ab_deriv(bgpts,ax,ay,aa,ae,lcmp,aa2+ae2);
      d1Qcp[j]=cam_pos[j].Q_deriv(bgpts,ax,ay,aa,ae,lcmp);
    }
    // compute camera position hessian
    for (j=0; j<3; ++j) {
      for (k=0; k<3; ++k) {
	cam_pos2[j][k].set_ab_deriv(cam_pos[j],cam_pos[k],
	  bgpts,ax,ay,aa,ae,lcmp,aa2+ae2);
	d2Qcp[j][k]=cam_pos2[j][k].
	  Q_deriv(cam_pos[j],cam_pos[k],bgpts,ax,ay,aa,ae,lcmp);
      }
    }
    if (cholesky(d2Qcp)) {
      // Camera position hessian is positive definite.
      // Proceed with correction & error analysis.
      float cam_shift[3],cam_error[3][3],Gff[3][3],fj,fk,gj,gk;
      Vector3 photo_shift=photo_point;
      solve(d2Qcp,d1Qcp,cam_shift);
      for (j=0; j<3; ++j) {
	const Parameter& pj=cam_pos[j];
	for (k=0; k<3; ++k) {
	  const Parameter& pk=cam_pos[k];
	  float& x=cam_error[j][k];
	  x=0;
	  for (i=0; i<nbgpt; ++i) {
	    da=bgpts[i].azim-aa;
	    de=bgpts[i].elev-ae;
	    pj.predict_deriv(i,lcmp,da,de,fj,gj);
	    pk.predict_deriv(i,lcmp,da,de,fk,gk);
	    x+=fj*fk*ax2+(fj*gk+gj*fk)*axy+gj*gk*ay2;
	  }
	}
      }
      for (j=0; j<3; ++j)
	solve(d2Qcp,cam_error[j],Gff[j]);
      transpose(Gff);
      for (j=0; j<3; ++j)
	solve(d2Qcp,Gff[j],cam_error[j]);
      printf("\n     adjustment    error\n");
      static const char *axis_name[] = {"North","East","Up"};
      static const Vector3 *horizon_basis[] = {&north,&east,&up};
      for (j=0; j<3; ++j) {
	printf("  %-7s %#7.4g  %#5.4g\n",axis_name[j],
	       cam_shift[j]*GeoPoint::radius_v,
	       2*sqrt(cam_error[j][j])*GeoPoint::radius_v);
	vec1=*horizon_basis[j];
	vec1*=cam_shift[j];
	photo_shift+=vec1;
      }
      printf("  Right   %#7.4g  %#5.4g\n",
	     (photo_cross*cam_shift)*GeoPoint::radius_v,
	     2*sqrt(rqf(cam_error,photo_cross.value))*GeoPoint::radius_v);
      Vector3 forward=photo_axis;
      forward.value[2]=0;
      forward.normalize();
      printf("  Forward %#7.4g  %#5.4g\n",
	     (forward*cam_shift)*GeoPoint::radius_v,
	     2*sqrt(rqf(cam_error,forward.value))*GeoPoint::radius_v);
      printf("  New position: lat=%.5f lon=%.5f alt=%.0f\n",
	     RAD2DEG*photo_shift.theta(),RAD2DEG*photo_shift.phi(),
	     (sqrt(photo_shift*photo_shift)-1)*GeoPoint::radius_v);
      if (nrdf<4)
	printf("  WARNING: due to insufficient data,\n"
	       "    you should not try to optimize the camera position.\n");
    } // if (camera position hessian is positive definite)
    else {
      printf(" not available because hessian is not positive definite\n");
    }
    printf("Photo axis:");
    // compute photo axis shift
    for (j=0; j<2; ++j) {
      pa_param[j].set_ab_deriv(bgpts,ax,ay,aa,ae,lcmp,aa2+ae2);
      d1Qpa[j]=pa_param[j].Q_deriv(bgpts,ax,ay,aa,ae,lcmp);
    }
    for (j=0; j<2; ++j) {
      for (k=0; k<2; ++k) {
	pa_param2[j][k].set_ab_deriv(pa_param[j],pa_param[k],
	  bgpts,ax,ay,aa,ae,lcmp,aa2+ae2);
	d2Qpa[j][k]=pa_param2[j][k].
	  Q_deriv(pa_param[j],pa_param[k],bgpts,ax,ay,aa,ae,lcmp);
      }
    }
    float D=d2Qpa[0][0]*d2Qpa[1][1]-d2Qpa[0][1]*d2Qpa[1][0];
    if (D>0 && d2Qpa[0][0]>0) {
      // Photo axis hessian is positive definite.
      // Proceed with correction & error analysis.
      float pa_shift[2],pa_error[2][2],Gff[2][2],fj,fk,gj,gk;
      solve(d2Qpa,d1Qpa,pa_shift,D);
      pa_shift[0]*=-1;
      pa_shift[1]*=-1;
      for (j=0; j<2; ++j) {
	const Parameter& pj=pa_param[j];
	for (k=0; k<2; ++k) {
	  const Parameter& pk=pa_param[k];
	  float& x=pa_error[j][k];
	  x=0;
	  for (i=0; i<nbgpt; ++i) {
	    da=bgpts[i].azim-aa;
	    de=bgpts[i].elev-ae;
	    pj.predict_deriv(i,lcmp,da,de,fj,gj);
	    pk.predict_deriv(i,lcmp,da,de,fk,gk);
	    x+=fj*fk*ax2+(fj*gk+gj*fk)*axy+gj*gk*ay2;
	  }
	}
      }
      for (j=0; j<2; ++j)
	solve(d2Qpa,pa_error[j],Gff[j],D);
      transpose(Gff);
      for (j=0; j<2; ++j)
	solve(d2Qpa,Gff[j],pa_error[j],D);
      printf("\n    adjustment     error\n");
      printf("  Right   %#7.4g  %#5.4g\n",pa_shift[0]*RAD2DEG,
	     2*sqrt(pa_error[0][0])*RAD2DEG);
      printf("  Up      %#7.4g  %#5.4g\n",pa_shift[1]*RAD2DEG,
	     2*sqrt(pa_error[1][1])*RAD2DEG);
      float u=sqrt(pa_shift[0]*pa_shift[0]+pa_shift[1]*pa_shift[1]),
	su=u?sin(u)/u:1;
      Vector3 new_pa_c(pa_shift[0]*su,pa_shift[1]*su,cos(u)),new_pa_h;
      backtrans(new_pa_c,photo_cross,photo_up,photo_axis,new_pa_h);
      printf("  New position: azim=%.2f elev=%.2f\n",
	     RAD2DEG*new_pa_h.phi_positive(),RAD2DEG*new_pa_h.theta());
    } // if (photo axis hessian is positive definite)
    else {
      printf(" not available because hessian is not positive definite\n");
    }

    printf("Refractive curvature:");
    // compute refractive curvature shift
    rf_param.set_ab_deriv(bgpts,ax,ay,aa,ae,lcmp,aa2+ae2);
    d1Qrf=rf_param.Q_deriv(bgpts,ax,ay,aa,ae,lcmp);
    rf_param2.set_ab_deriv(rf_param,rf_param,bgpts,ax,ay,aa,ae,lcmp,aa2+ae2);
    d2Qrf=rf_param2.Q_deriv(rf_param,rf_param,bgpts,ax,ay,aa,ae,lcmp);
    if (d2Qrf>0) {
      // Refractive curvature second derivative is positive.
      // Proceed with correction & error analysis.
      float rf_shift=-d1Qrf/d2Qrf,rf_error=0,f,g;
      for (i=0; i<nbgpt; ++i) {
	da=bgpts[i].azim-aa;
	de=bgpts[i].elev-ae;
	rf_param.predict_deriv(i,lcmp,da,de,f,g);
	rf_error+=f*f*ax2+2*f*g*axy+g*g*ay2;
      }
      rf_error/=d2Qrf*d2Qrf;
      printf("\n  Adjust by %#.4g, error %#.4g\n",rf_shift,2*rf_error);
      printf("  New value would be %#.4g\n",rf_curv*2+rf_shift);
      if (nrdf<2)
	printf("  WARNING: due to insufficient data,\n"
	       "    you should not try to optimize the refraction.\n");
    }
    else {
      printf(" not available because hessian is not positive definite\n");
    }
  }
  // analyze rival points
  printf("\nRIVAL POINTS\n");
  printf("    from position      from pix coords\n"
	 "----------------------  -------------\n"
	 "Azmuth ElvAng Distance  Azmuth ElvAng  D-Elev S-Elev Pix Res  Name\n"
	 "====== ====== ========  ====== ======  ====== ====== =======  ================\n");
  while (c=fgetc(f),c!='#' && c!='P' && c!=EOF) {
    ungetc(c,f);
    if (error=test.fscanf(f)) goto exit;
    // compute the sight vector and distance from input position
    test.set_sight(photo_point,up,north,east);
    dist=(float)sqrt(test.sight*test.sight);
    printf("%6.2f %6.2f %#7.3g   ",
	   RAD2DEG*test.sight.phi_positive(),RAD2DEG*test.sight.theta(),
	   GeoPoint::radius_h*dist);
    // Now recompute the sight vector from pixel coordinates
    test.set_sight(photo_axis,photo_up,photo_cross,lcmp,camres2i,dist,rf_curv);
    printf("%6.2f %6.2f  ",
	   RAD2DEG*test.sight.phi_positive(),RAD2DEG*test.sight.theta());
    // compute error parameters
    da=test.azim-aa;
    de=test.elev-ae;
    float aa_be=lcmp.a*da+lcmp.b*de,ba_ae=lcmp.b*da-lcmp.a*de;
    // compute elevation difference
    printf("%#6.3g %#6.3g %#6.3g   %s\n",
	   GeoPoint::radius_v*(mod_diff(sqrt(photo_point*photo_point),
					test.sight)),
	   nrdf>0 ?
	     sqrt(ba_ae*ba_ae*sig_aa+aa_be*aa_be*sig_bb-2*ba_ae*aa_be*sig_ab
		  +sig_elel*(1+ni)) * GeoPoint::radius_v*dist*camres2i
	     : -1,
	   GeoPoint::radius_v*dist*camresi,
	   test.name);
  }

  if (c=='P') {
    // compute prospective points
    printf("\nPROSPECTIVE POINTS\n");
    printf("                                          ElevAngle\n"
	   " Latitude  Longitude Elevation  Azimuth  Actul Aprnt  Pix Res  Name\n"
	   "========= ========== =========  =======  ===== =====  =======  ================\n");
    while (c=fgetc(f),c!='#' && c!=EOF) {
      ungetc(c,f);
      if (error=test.fscanf(f,dist)) goto exit;
      test.h_dist2=dist*dist;
      test.set_sight(photo_axis,photo_up,photo_cross,lcmp,
		     camres2i,dist,rf_curv);
      backtrans(test.sight,north,east,up,test.location);
      test.location+=photo_point;
      h_disti=1/(float)sqrt(test.h_dist2);
      printf("%9.5f %10.5f   %5.0f    %6.2f   %5.2f %5.2f  %#6.3g   %s\n",
	     RAD2DEG*test.location.theta(),RAD2DEG*test.location.phi(),
	     GeoPoint::radius_v*(sqrt(test.location*test.location)-1),
	     RAD2DEG*test.sight.phi_positive(),
	     RAD2DEG*atan(test.sight.value[2]*h_disti),
	     RAD2DEG*atan((test.sight.value[2]+test.h_dist2*rf_curv)*h_disti),
	     GeoPoint::radius_v*dist*camresi,test.name);
    }
  }

exit:
  fclose(f);
  if (error) {
    fprintf(stderr,"%s\n",error);
#ifdef NDEBUG
    printf("\nPress <enter>");
    gets(fname);
#endif
    return 3;
  }
#ifdef NDEBUG
  printf("\nPress <enter>");
  gets(fname);
#endif
  return 0;
}