// zygo2root.cc - tool for use with Zygo's MetroPro package.
//
// purpose: Reads in a standard coords.crd file produced by the MetroPro
//          stitch script, packs the surface map into a TH2D root histogram,
//	    and saves the histogram to a root file.  The x,y axes of the
//          histogram are in mm, and the vertical axis is in microns.
//
// author: richard.t.jones at uconn.edu
// version: November 15, 2011

#include <fstream>
#include <string>
#include <sstream>
#include <iomanip>
#include <vector>
#include <math.h>
#include "MetroProMap.h"

#include <TFile.h>
#include <TVectorD.h>
#include <TMatrixD.h>
#include <TMatrixDSym.h>

#define REWRITE_ABSOLUTE_TO_LOCAL_PATHS 1
#define DO_HITCH_NOT_STITCH 1

int overlay_number(0);
std::string rootfile("zygo.root");
int overlay_frames(std::vector<double> &x, std::vector<double> &y,
                   std::vector<double> &z, std::vector<TH2D*> &map,
                   TVectorD *Z0=0, TVectorD *A0=0, TVectorD *B0=0);
int stitch_frames(std::vector<double> &x, std::vector<double> &y,
                  std::vector<double> &z, std::vector<TH2D*> &map);
int hitch_frames(std::vector<double> &x, std::vector<double> &y,
                 std::vector<double> &z, std::vector<TH2D*> &map);

void usage()
{
   std::cerr << "Usage: zygo2root -o [<output_file>] <input file 1> ..."
             << std::endl
             << " where <output_file> is the name of a root file in which"
             << " the histogram is saved,"
             << std::endl
             << " (default zygo.root)"
             << " and <input file> is a text file in .crd format,"
             << std::endl
             << " as in \"coords.crd\"."
             << std::endl;
}

int main(int argc, char *argv[])
{
   if (argc == 1) {
      usage();
      exit(1);
   }

   TFile *rfile=0;
   std::vector<TH2D*> mapset;
   std::vector<double> xorig,yorig,zorig;
   for (int arg=1; arg<argc; ++arg) {
      std::string args(argv[arg]);
      if (args.compare("-o") == 0) {
         rootfile = std::string(argv[++arg]);
         continue;
      }
      std::ifstream infile(args.c_str());
      if (! infile.is_open()) {
         std::cerr << "Error - unable to open input file "
                   << args << std::endl;
         continue;
      }
      std::string inpath(args.c_str());
      size_t tpos = inpath.find("/stch_");
      if (tpos != std::string::npos) {
         inpath = inpath.substr(0,inpath.find("/stch_"));
      }
      else {
         inpath = inpath.substr(0,inpath.rfind("/"));
      }
      int frame=0;
      std::string dfilename;
      while (! infile.eof()) {
         std::string line;
         getline(infile,line);
         size_t epos = line.rfind(".dat");
         if (epos != std::string::npos) {
            std::cout << line << std::endl;
            dfilename = line;
            if (dfilename.substr(dfilename.length()-1) == "\r") {
               dfilename = dfilename.substr(0,dfilename.length()-1);
            }
#if REWRITE_ABSOLUTE_TO_LOCAL_PATHS
            size_t rpos = dfilename.find("MetroPro");
            if (rpos != std::string::npos) {
               dfilename = dfilename.substr(rpos);
               for (size_t pos=dfilename.find("\\"); pos != std::string::npos;
                        pos=dfilename.find("\\")) {
                  dfilename[pos] = '/';
               }
               dfilename = dfilename.replace(0,8,inpath);
            }
#endif
            continue;
         }
         if (line.find("x y and z position in inches") == std::string::npos) {
            continue;
         }
         ++frame;
         double x,y,z;
         std::stringstream ss(line);
         ss >> x >> y >> z;
         std::ifstream dfile(dfilename.c_str());
         if (! dfile.is_open()) {
            std::cerr << "Error - unable to open data file "
                      << dfilename << std::endl;
            exit(2);
         }
         MetroProMap map;
         dfile >> map;
         std::cout << "MetroProMap is "
                   << map.getWidth() << "x"
                   << map.getHeight()
                   << " with origin at "
                   << map.getXorig() << ","
                   << map.getYorig()
                   << std::endl;
         if (rfile == 0) {
            rfile = new TFile(rootfile.c_str(),"update");
            if (! rfile->IsOpen()) {
               std::cerr << "Error - unable to open output file " << rootfile
                         << " for writing, giving up!" << std::endl;
               exit(2);
            }
         }
         std::string rdir(args.c_str());
         size_t ncut = rdir.rfind("MetroPro/");
         if (ncut != std::string::npos) {
            rdir = rdir.substr(ncut+9);
         }
         TDirectory *dir = rfile->GetDirectory("/");
         for (size_t ncut=0, nslash=rdir.find("/");
              nslash != std::string::npos;
              ncut=nslash+1,nslash=rdir.find("/",ncut))
         {
            std::string subdir = rdir.substr(ncut,nslash-ncut);
            if (! dir->GetDirectory(subdir.c_str())) {
	       dir = dir->mkdir(subdir.c_str());
               if (! dir) {
                  std::cerr << "Error - unable to create directory "
                            << subdir << " in root output file, giving up!"
                            << std::endl;
                  exit(2);
               }
            }
            else {
	       dir = dir->GetDirectory(subdir.c_str());
            }
         }
         rdir = rdir.substr(0,rdir.rfind("/"));
         rfile->cd(rdir.c_str());

         // If this x,y frame position already appears in the frame list,
         // overlay the non-zero pixels in this image on the previous one.

         int repeat_frame = -1;
         for (size_t i=0; i < xorig.size(); ++i) {
            if (fabs(xorig[i] - x*map.inches2mm) < 0.001 &&
                fabs(yorig[i] - y*map.inches2mm) < 0.001) 
            {
               repeat_frame = i;
               break;
            }
         }
         if (repeat_frame >= 0) {
            int fi = repeat_frame;
            double zshift = z*map.inches2mm - zorig[fi];
            TH2D *h2 = map.makeHist2D("h2_temp","");
            for (int ix = 1; ix <= h2->GetNbinsX(); ++ix) {
               for (int iy = 1; iy <= h2->GetNbinsY(); ++iy) {
                  double old_height = mapset[fi]->GetBinContent(ix,iy);
                  double new_height = h2->GetBinContent(ix,iy);
                  if (old_height == 0 && new_height != 0) {
                     mapset[fi]->SetBinContent(ix,iy,new_height+zshift);
                  }
               }
            }
            delete h2;
         }
         else {
            xorig.push_back(x*map.inches2mm);
            yorig.push_back(y*map.inches2mm);
            zorig.push_back(z*map.inches2mm);
            std::string hname = dfilename.substr(dfilename.rfind("/")+1);
            std::string htitle = dfilename;
            TH2D *h2 = map.makeHist2D(hname,htitle);
            mapset.push_back(h2);
            h2->SetDirectory(0);
         }
      }
      for (size_t ih = 0; ih < mapset.size(); ++ih) {
         mapset[ih]->Write();
      }
      if (dfilename.find("*") != 0) {
         inpath += "/";
         inpath += dfilename;
         std::ifstream dfile(inpath.c_str());
         if (! dfile.is_open()) {
            std::cerr << "Error - unable to open data file "
                      << inpath << std::endl;
            exit(2);
         }
         MetroProMap map;
         dfile >> map;
         std::cout << "MetroProMap is "
                   << map.getWidth() << "x"
                   << map.getHeight()
                   << " with origin at "
                   << map.getXorig() << ","
                   << map.getYorig()
                   << std::endl;
         std::string hname = dfilename;
         std::string htitle = dfilename;
         TH2D *h2 = map.makeHist2D(hname,htitle);
         h2->Write();
         delete h2;
      }
   }

   overlay_frames(xorig,yorig,zorig,mapset);
#if DO_HITCH_NOT_STITCH
   std::cout << "hitching not stitching\n";
   hitch_frames(xorig,yorig,zorig,mapset);
#else
   stitch_frames(xorig,yorig,zorig,mapset);
   std::cout << "stitching not hitching\n";
#endif
   
   for (size_t ih = 0; ih < mapset.size(); ++ih) {
     delete mapset[ih];
   }
   rfile->Close();
   delete rfile;
   return 0;
}

int overlay_frames(std::vector<double> &x, std::vector<double> &y,
                   std::vector<double> &z, std::vector<TH2D*> &map,
                   TVectorD *Z0, TVectorD *A0, TVectorD *B0)
{
   double xmin=999;
   double ymin=999;
   double xmax=-999;
   double ymax=-999;
   double xdelta=0;
   double ydelta=0;
   for (size_t i=0; i < x.size(); ++i) {
      xmin = (x[i] < xmin)? x[i] : xmin;
      xmax = (x[i] > xmax)? x[i] : xmax;
      ymin = (y[i] < ymin)? y[i] : ymin;
      ymax = (y[i] > ymax)? y[i] : ymax;
      if (xdelta==0 && y[i]==y[i+1]) {
         xdelta = fabs(x[i]-x[i+1]);
      }
      if (ydelta==0 && x[i]==x[i+1]) {
         ydelta = fabs(y[i]-y[i+1]);
      }
   }
   int fwidth = map[0]->GetNbinsX();
   int fheight = map[0]->GetNbinsY();
   double pixel_dx = map[0]->GetXaxis()->GetBinWidth(1);
   double pixel_dy = map[0]->GetYaxis()->GetBinWidth(1);
   xmin -= fwidth*pixel_dx;
   ymax += fheight*pixel_dy;
   int nxbins = int((xmax-xmin)/pixel_dx + 0.9999);
   int nybins = int((ymax-ymin)/pixel_dy + 0.9999);
   
   std::stringstream ss;
   ss << "over" << ++overlay_number;
   TH2D *hov = new TH2D(ss.str().c_str(),ss.str().c_str(),
               nxbins,0,xmax-xmin,nybins,0,ymax-ymin);
   TH2D *hov1 = new TH2D(*hov);

   for (size_t frame=0; frame < map.size(); ++frame) {
      int xbin0 = int((xmax-x[frame])/pixel_dx + 1.5);
      int ybin0 = int((y[frame]-ymin)/pixel_dy + 1.5);
      for (int xpix=0; xpix < fwidth; ++xpix) {
         for (int ypix=0; ypix < fheight; ++ypix) {
            double zelev = map[frame]->GetBinContent(xpix,ypix);
            if (zelev != 0) {
               const double zref = -28.460; // mm
               zelev += z[frame]-zref;
               if (Z0) {
                  zelev += (*Z0)(frame);
               }
               if (A0) {
                  zelev += (*A0)(frame)*xpix;
               }
               if (B0) {
                  zelev += (*B0)(frame)*ypix;
               }
               if (hov1->GetBinContent(xbin0+xpix,ybin0+ypix) == 0) {
                  hov->SetBinContent(xbin0+xpix,ybin0+ypix,
                       hov->GetBinContent(xbin0+xpix,ybin0+ypix)+zelev*1000);
                  hov1->SetBinContent(xbin0+xpix,ybin0+ypix,
                       hov1->GetBinContent(xbin0+xpix,ybin0+ypix)+1);
               }
            }
         }
      }
   }
   hov->Divide(hov1);
   hov->Write();
   delete hov;
   return 0;
}

// This function solves the general problem of adding an arbitrary z offset
// to each frame, together with arbitrary tilts along the x and y axes,
// such that the disagreement between the surface heights in the regions of
// overlap between the frames is minimized. The problem is ambiguous up to
// a global shift and rotation, but the solution is well-defined if the
// average tilts and shifts of all the frames is constrained to zero.

int stitch_frames(std::vector<double> &x, std::vector<double> &y,
                  std::vector<double> &z, std::vector<TH2D*> &map)
{
   double fxdim = map[0]->GetXaxis()->GetXmax() -
                  map[0]->GetXaxis()->GetXmin();
   double fydim = map[0]->GetYaxis()->GetXmax() -
                  map[0]->GetYaxis()->GetXmin();
   double pixel_dx = map[0]->GetXaxis()->GetBinWidth(1);
   double pixel_dy = map[0]->GetYaxis()->GetBinWidth(1);

   int frames = x.size();
   TMatrixD ZsZsprime(frames,frames);
   TMatrixD ZsAsprime(frames,frames);
   TMatrixD ZsBsprime(frames,frames);
   TMatrixD AsZsprime(frames,frames);
   TMatrixD AsAsprime(frames,frames);
   TMatrixD AsBsprime(frames,frames);
   TMatrixD BsZsprime(frames,frames);
   TMatrixD BsAsprime(frames,frames);
   TMatrixD BsBsprime(frames,frames);
   TMatrixD EsEsprime(frames,3);
   ZsZsprime.Zero();
   ZsAsprime.Zero();
   ZsBsprime.Zero();
   AsZsprime.Zero();
   AsAsprime.Zero();
   AsBsprime.Zero();
   BsZsprime.Zero();
   BsAsprime.Zero();
   BsBsprime.Zero();
   EsEsprime.Zero();
   for (int frame1=0; frame1 < frames; ++frame1) {
      for (int frame2=0; frame2 < frames; ++frame2) {
         if (frame2 == frame1) {
            continue;
         }
         int i1start,i2start,iwidth;
         int j1start,j2start,jwidth;
         if (x[frame2] == x[frame1]) {
            i1start = i2start = 0;
            iwidth = map[frame1]->GetNbinsX();
         }
         else if (x[frame2] < x[frame1] && x[frame2] > x[frame1]-fxdim) {
            i1start = int((x[frame1]-x[frame2])/pixel_dx + 0.5);
            iwidth = map[frame1]->GetNbinsX() - i1start;
            i2start = 0;
         }
         else if (x[frame1] < x[frame2] && x[frame1] > x[frame2]-fxdim) {
            i2start = int((x[frame2]-x[frame1])/pixel_dx + 0.5);
            iwidth = map[frame2]->GetNbinsX() - i2start;
            i1start = 0;
         }
         else {
            continue;
         }
         if (y[frame2] == y[frame1]) {
            j1start = j2start = 0;
            jwidth = map[frame1]->GetNbinsY();
         }
         else if (y[frame2] < y[frame1] && y[frame2] > y[frame1]-fydim) {
            j2start = int((y[frame1]-y[frame2])/pixel_dy + 0.5);
            jwidth = map[frame2]->GetNbinsY() - j2start;
            j1start = 0;
         }
         else if (y[frame1] < y[frame2] && y[frame1] > y[frame2]-fydim) {
            j1start = int((y[frame2]-y[frame1])/pixel_dy + 0.5);
            jwidth = map[frame1]->GetNbinsY() - j1start;
            j2start = 0;
         }
         else {
            continue;
         }

#if 0
         std::cout << "found overlap region of size " << iwidth << "x" << jwidth 
                   << " starting at " << i1start << "," << j1start
                   << " in frame " << frame1
                   << " and " << i2start << "," << j2start
                   << " in frame " << frame2
                   << std::endl;
#endif

         for (int i=0; i < iwidth; ++i) {
            for (int j=0; j < jwidth; ++j) {
               double elev1 = map[frame1]->GetBinContent(i1start+i,j1start+j);
               double elev2 = map[frame2]->GetBinContent(i2start+i,j2start+j);
               if (elev1 == 0 || elev2 == 0) {
                  continue;
               }
               ZsZsprime(frame1,frame1) += 1;
               ZsZsprime(frame1,frame2) -= 1;
               ZsAsprime(frame1,frame1) += i1start+i;
               ZsAsprime(frame1,frame2) -= i2start+i;
               ZsBsprime(frame1,frame1) += j1start+j;
               ZsBsprime(frame1,frame2) -= j2start+j;
               AsZsprime(frame1,frame1) += i1start+i;
               AsZsprime(frame1,frame2) -= i1start+i;
               AsAsprime(frame1,frame1) += (i1start+i)*(i1start+i);
               AsAsprime(frame1,frame2) -= (i1start+i)*(i2start+i);
               AsBsprime(frame1,frame1) += (i1start+i)*(j1start+j);
               AsBsprime(frame1,frame2) -= (i1start+i)*(j2start+j);
               BsZsprime(frame1,frame1) += j1start+j;
               BsZsprime(frame1,frame2) -= j1start+j;
               BsAsprime(frame1,frame1) += (j1start+j)*(i1start+i);
               BsAsprime(frame1,frame2) -= (j1start+j)*(i2start+i);
               BsBsprime(frame1,frame1) += (j1start+j)*(j1start+j);
               BsBsprime(frame1,frame2) -= (j1start+j)*(j2start+j);
               double zdiff = (elev1+z[frame1])-(elev2+z[frame2]);
               EsEsprime(frame1,0) += zdiff;
               EsEsprime(frame1,1) += (i1start+i)*zdiff;
               EsEsprime(frame1,2) += (j1start+j)*zdiff;
            }
         }
      }
   }

   // Assemble the matrix M multiplying the transformation coefficients

   int ndim = 3*frames;
   TMatrixD M(ndim,ndim);
   TMatrixD C(ndim,1);
   int zset=0;
   int aset=frames;
   int bset=2*frames;
   for (int frame1=0; frame1 < frames; ++frame1) {
      for (int frame2=0; frame2 < frames; ++frame2) {
         M(frame1+zset,frame2+zset) = ZsZsprime(frame1,frame2);
         M(frame1+zset,frame2+aset) = ZsAsprime(frame1,frame2);
         M(frame1+zset,frame2+bset) = ZsBsprime(frame1,frame2);
         M(frame1+aset,frame2+zset) = AsZsprime(frame1,frame2);
         M(frame1+aset,frame2+aset) = AsAsprime(frame1,frame2);
         M(frame1+aset,frame2+bset) = AsBsprime(frame1,frame2);
         M(frame1+bset,frame2+zset) = BsZsprime(frame1,frame2);
         M(frame1+bset,frame2+aset) = BsAsprime(frame1,frame2);
         M(frame1+bset,frame2+bset) = BsBsprime(frame1,frame2);
      }
      C(frame1+zset,0) = -EsEsprime(frame1,0);
      C(frame1+aset,0) = -EsEsprime(frame1,1);
      C(frame1+bset,0) = -EsEsprime(frame1,2);
   }

   if (! M.IsSymmetric()) {
      std::cerr << "Weird - matrix M is not symmetric!"
                << "  Cannot continue, giving up."
                << std::endl;
      for (int frame1=0; frame1 < ndim; ++frame1) {
         for (int frame2=0; frame2 < ndim; ++frame2) {
            if (M(frame1,frame2) != M(frame2,frame1)) {
               std::cerr << "mismatch found at "
                         << "M(" << frame1 << "," << frame2 << ") = "
                         << M(frame1,frame2) << " != "
                         << "M(" << frame2 << "," << frame1 << ") = "
                         << M(frame2,frame1) << std::endl;
            }
         }
      }
      return 8;
   }

   // Solve the system of linear equations

   TVectorD evalues(ndim);
   TMatrixD evectors(ndim,ndim);
   evectors = M.EigenVectors(evalues);
   if (evalues(ndim-1) < -1e-3 || evalues(ndim-1) > 1e-3 ||
       evalues(ndim-2) < -1e-3 || evalues(ndim-2) > 1e-3 ||
       evalues(ndim-3) < -1e-3 || evalues(ndim-3) > 1e-3) {
      std::cerr << "Error in stitch_frames - lowest 3 eigenvalues of M"
                << " should be consistent with zero, cannot continue."
                << std::endl
                << "  " << evalues(ndim-3)
                << ", " << evalues(ndim-2)
                << ", " << evalues(ndim-1)
                << std::endl;
      exit(15);
   }
   int nzeros;
   for (nzeros=0; nzeros < ndim; ++nzeros) {
       if (evalues(ndim-nzeros-1) > 1e-3) {
          break;
       }
   }
   if (nzeros > 3) {
      std::cerr << "Warning in stitch_frames - the first 3 eigenvalues "
                << "of the stitching matrix should be zero, but you have "
                << nzeros << " eigenvalues less than 0.001.  This is normal "
                << "whenever there are empty frames in the image."
                << std::endl;
   }

#if 0
   std::cout << "Here is a list of the eigenvalues of M:" << std::endl;
   for (int i=0; i < ndim; ++i) {
      std::cout << "   " << i << " : " << evalues(i) << std::endl;
   }
   std::cout << "Verifying that the last eigenvector is valid:" << std::endl;
   double enorm=0;
   for (int ir=0; ir < ndim; ++ir) {
      double prod=0;
      for (int ic=0; ic < ndim; ++ic) {
         prod += M(ir,ic)*evectors(ic,ndim-1);
      }
      enorm += evectors(ir,ndim-1)*evectors(ir,ndim-1);
      std::cout << "   " << ir << " : " << prod << " / "
                << evectors(ir,ndim-1) << " = "
                << prod/evectors(ir,ndim-1)
                << std::endl;
   }
   std::cout << "norm of eigenvector is " << enorm << std::endl;
#endif

   TMatrixD S(ndim,1);
   S.TMult(evectors,C);
   for (int i=0; i < ndim-nzeros; ++i) {
      S(i,0) /= evalues(i);
   }
   TMatrixD sol(ndim,1);
   sol.Mult(evectors,S);

   // Make the solution unique by requiring the correction
   // factors (z,alpha,beta) each to average zero.

   TMatrixD V(3,3);
   TMatrixD U(3,1);
   V.Zero();
   U.Zero();
   for (int frame=0; frame < frames; ++frame) {
      V(0,0) += evectors(frame+zset,ndim-3);
      V(1,0) += evectors(frame+aset,ndim-3);
      V(2,0) += evectors(frame+bset,ndim-3);
      V(0,1) += evectors(frame+zset,ndim-2);
      V(1,1) += evectors(frame+aset,ndim-2);
      V(2,1) += evectors(frame+bset,ndim-2);
      V(0,2) += evectors(frame+zset,ndim-1);
      V(1,2) += evectors(frame+aset,ndim-1);
      V(2,2) += evectors(frame+bset,ndim-1);
      U(0,0) += sol(frame+zset,0);
      U(1,0) += sol(frame+aset,0);
      U(2,0) += sol(frame+bset,0);
   }
   TMatrixD A(3,1);
   A.Mult(V.Invert(),U);
   for (int i=0; i < ndim; ++i) {
      sol(i,0) -= A(0,0)*evectors(i,ndim-3)
                + A(1,0)*evectors(i,ndim-2)
                + A(2,0)*evectors(i,ndim-1);
   }

#if 0
   std::cout << "Solution is:" << std::endl;
   for (int frame=0; frame < frames; ++frame) {
      std::cout << "   " << sol(zset+frame,0)
                << "   " << sol(aset+frame,0)
                << "   " << sol(bset+frame,0) << std::endl;
   }
#endif
  
#if 0
   std::cout << "Check the solution:" << std::endl;
   for (int i=0; i < ndim; ++i) {
      double sum = 0;
      for (int j=0; j < ndim; ++j) {
         sum += M(i,j)*sol(j,0);
      }
      std::cout << "   " << sum << " - " << C(i,0)
                << " = " << sum-C(i,0) << std::endl;
   }
#endif

   TH1D hZ0("hZ0","zshift distribution",100,-2e-2,2e-2);
   TH1D hA0("hA0","alpha distribution",100,-2e-6,2e-6);
   TH1D hB0("hB0","beta distribution",100,-2e-6,2e-6);
   TVectorD Z0(frames);
   TVectorD A0(frames);
   TVectorD B0(frames);
   for (int frame=0; frame < frames; ++frame) {
      Z0(frame) = sol(zset+frame,0);
      A0(frame) = sol(aset+frame,0);
      B0(frame) = sol(bset+frame,0);
      hZ0.Fill(Z0(frame));
      hA0.Fill(A0(frame));
      hB0.Fill(B0(frame));
   }
   hZ0.Write();
   hA0.Write();
   hB0.Write();
   return overlay_frames(x,y,z,map,&Z0,&A0,&B0);
}

// This function is a simplified version of stitch_frames which only
// varies the z shift of each frame, while constraining the tilts to zero.

int hitch_frames(std::vector<double> &x, std::vector<double> &y,
                 std::vector<double> &z, std::vector<TH2D*> &map)
{
   double fxdim = map[0]->GetXaxis()->GetXmax() -
                  map[0]->GetXaxis()->GetXmin();
   double fydim = map[0]->GetYaxis()->GetXmax() -
                  map[0]->GetYaxis()->GetXmin();
   double pixel_dx = map[0]->GetXaxis()->GetBinWidth(1);
   double pixel_dy = map[0]->GetYaxis()->GetBinWidth(1);

   int frames = x.size();
   TMatrixD ZsZsprime(frames,frames);
   TMatrixD EsEsprime(frames,1);
   ZsZsprime.Zero();
   EsEsprime.Zero();
   for (int frame1=0; frame1 < frames; ++frame1) {
      for (int frame2=0; frame2 < frames; ++frame2) {
         if (frame2 == frame1) {
            continue;
         }
         int i1start,i2start,iwidth;
         int j1start,j2start,jwidth;
         if (x[frame2] == x[frame1]) {
            i1start = i2start = 0;
            iwidth = map[frame1]->GetNbinsX();
         }
         else if (x[frame2] < x[frame1] && x[frame2] > x[frame1]-fxdim) {
            i1start = int((x[frame1]-x[frame2])/pixel_dx + 0.5);
            iwidth = map[frame1]->GetNbinsX() - i1start;
            i2start = 0;
         }
         else if (x[frame1] < x[frame2] && x[frame1] > x[frame2]-fxdim) {
            i2start = int((x[frame2]-x[frame1])/pixel_dx + 0.5);
            iwidth = map[frame2]->GetNbinsX() - i2start;
            i1start = 0;
         }
         else {
            continue;
         }
         if (y[frame2] == y[frame1]) {
            j1start = j2start = 0;
            jwidth = map[frame1]->GetNbinsY();
         }
         else if (y[frame2] < y[frame1] && y[frame2] > y[frame1]-fydim) {
            j2start = int((y[frame1]-y[frame2])/pixel_dy + 0.5);
            jwidth = map[frame2]->GetNbinsY() - j2start;
            j1start = 0;
         }
         else if (y[frame1] < y[frame2] && y[frame1] > y[frame2]-fydim) {
            j1start = int((y[frame2]-y[frame1])/pixel_dy + 0.5);
            jwidth = map[frame1]->GetNbinsY() - j1start;
            j2start = 0;
         }
         else {
            continue;
         }

#if 0
         std::cout << "found overlap region of size " << iwidth << "x" << jwidth 
                   << " starting at " << i1start << "," << j1start
                   << " in frame " << frame1
                   << " and " << i2start << "," << j2start
                   << " in frame " << frame2
                   << std::endl;
#endif

         for (int i=0; i < iwidth; ++i) {
            for (int j=0; j < jwidth; ++j) {
               double elev1 = map[frame1]->GetBinContent(i1start+i,j1start+j);
               double elev2 = map[frame2]->GetBinContent(i2start+i,j2start+j);
               if (elev1 == 0 || elev2 == 0) {
                  continue;
               }
               ZsZsprime(frame1,frame1) += 1;
               ZsZsprime(frame1,frame2) -= 1;
               double zdiff = (elev1+z[frame1])-(elev2+z[frame2]);
               EsEsprime(frame1,0) += zdiff;
            }
         }
      }
   }

   // Assemble the matrix M multiplying the transformation coefficients

   int ndim = frames;
   TMatrixD M(ndim,ndim);
   TMatrixD C(ndim,1);
   for (int frame1=0; frame1 < frames; ++frame1) {
      for (int frame2=0; frame2 < frames; ++frame2) {
         M(frame1,frame2) = ZsZsprime(frame1,frame2);
      }
      C(frame1,0) = -EsEsprime(frame1,0);
   }

   if (! M.IsSymmetric()) {
      std::cerr << "Weird - matrix M is not symmetric!"
                << "  Cannot continue, giving up."
                << std::endl;
      for (int frame1=0; frame1 < ndim; ++frame1) {
         for (int frame2=0; frame2 < ndim; ++frame2) {
            if (M(frame1,frame2) != M(frame2,frame1)) {
               std::cerr << "mismatch found at "
                         << "M(" << frame1 << "," << frame2 << ") = "
                         << M(frame1,frame2) << " != "
                         << "M(" << frame2 << "," << frame1 << ") = "
                         << M(frame2,frame1) << std::endl;
            }
         }
      }
      return 8;
   }

   // Solve the system of linear equations

   TVectorD evalues(ndim);
   TMatrixD evectors(ndim,ndim);
   evectors = M.EigenVectors(evalues);
   if (evalues(ndim-1) < -1e-3 || evalues(ndim-1) > 1e-3) {
      std::cerr << "Error in hitch_frames - lowest eigenvalue of M"
                << " should be consistent with zero, cannot continue."
                << std::endl
                << evalues(ndim-1)
                << std::endl;
      exit(15);
   }
   int nzeros;
   for (nzeros=0; nzeros < ndim; ++nzeros) {
       if (evalues(ndim-nzeros-1) > 1e-3) {
          break;
       }
   }
   if (nzeros > 1) {
      std::cerr << "Warning in hitch_frames - the first eigenvalue "
                << "of the hitching matrix should be zero, but you have "
                << nzeros << " eigenvalues less than 0.001.  This is normal "
                << "whenever there are empty frames in the image."
                << std::endl;
   }

#if 0
   std::cout << "Here is a list of the eigenvalues of M:" << std::endl;
   for (int i=0; i < ndim; ++i) {
      std::cout << "   " << i << " : " << evalues(i) << std::endl;
   }
   std::cout << "Verifying that the last eigenvector is valid:" << std::endl;
   double enorm=0;
   for (int ir=0; ir < ndim; ++ir) {
      double prod=0;
      for (int ic=0; ic < ndim; ++ic) {
         prod += M(ir,ic)*evectors(ic,ndim-1);
      }
      enorm += evectors(ir,ndim-1)*evectors(ir,ndim-1);
      std::cout << "   " << ir << " : " << prod << " / "
                << evectors(ir,ndim-1) << " = "
                << prod/evectors(ir,ndim-1)
                << std::endl;
   }
   std::cout << "norm of eigenvector is " << enorm << std::endl;
#endif

   TMatrixD S(ndim,1);
   S.TMult(evectors,C);
   for (int i=0; i < ndim-nzeros; ++i) {
      S(i,0) /= evalues(i);
   }
   TMatrixD sol(ndim,1);
   sol.Mult(evectors,S);

   // Make the solution unique by requiring the correction
   // factors z to average zero.

   double Vsum = 0;
   double Usum = 0;
   for (int frame=0; frame < frames; ++frame) {
      Vsum += evectors(frame,ndim-1);
      Usum += sol(frame,0);
   }
   double a = Usum / Vsum;
   for (int i=0; i < ndim; ++i) {
      sol(i,0) -= a * evectors(i,ndim-1);
   }

#if 0
   std::cout << "Solution is:" << std::endl;
   for (int frame=0; frame < frames; ++frame) {
      std::cout << "   " << sol(zset+frame,0)
                << "   " << sol(aset+frame,0)
                << "   " << sol(bset+frame,0) << std::endl;
   }
#endif
  
#if 0
   std::cout << "Check the solution:" << std::endl;
   for (int i=0; i < ndim; ++i) {
      double sum = 0;
      for (int j=0; j < ndim; ++j) {
         sum += M(i,j)*sol(j,0);
      }
      std::cout << "   " << sum << " - " << C(i,0)
                << " = " << sum-C(i,0) << std::endl;
   }
#endif

   TH1D hZ0("hZ0","zshift distribution",100,-2e-2,2e-2);
   TH1D hA0("hA0","alpha distribution",100,-2e-6,2e-6);
   TH1D hB0("hB0","beta distribution",100,-2e-6,2e-6);
   TVectorD Z0(frames);
   TVectorD A0(frames);
   TVectorD B0(frames);
   for (int frame=0; frame < frames; ++frame) {
      Z0(frame) = sol(frame,0);
      A0(frame) = 0;
      B0(frame) = 0;
      hZ0.Fill(Z0(frame));
      hA0.Fill(A0(frame));
      hB0.Fill(B0(frame));
   }
   hZ0.Write();
   hA0.Write();
   hB0.Write();
   return overlay_frames(x,y,z,map,&Z0,&A0,&B0);
}