/*
 * axisfinder.cpp
 *
 * Purpose: Finds the settings of a gonimeter that aligns
 *          the crystal and lab coordinate systems.
 *
 * Richard Jones, University of Connecticut
 * November 2, 2006
 *
 * Notes:
 * 1. The lab coordinate system is defined by the following rules.
 *       a. the beam axis is along the z axis
 *       b. the origin is at the center of the X-ray scattering target
 *       c. X-ray scattering is in the yz plane
 *       d. the y axis points vertically upward
 *       e. the x,y,z axes form a right-handed orthogonal system
 *
 * 2. The goniometer mount system is defined to coincide with the lab
 *    frame when the goniometer settings are all zero.  There is no prior
 *    assumption about how those axes are aligned with the mount fixture
 *    because the goniometer motors have arbitrary offsets.  The goniometer
 *    mount coordinates are intermediate between the lab and crystal frames.
 *
 * 3. The crystal axes are defined by the following rules.
 *       a. the user assigns the h,k,l indices that identify the crystal
 *          vector q = 2pi/a (h,k,l) responsible for the scattering, such
 *          that q = p' - p with incident X-ray momentum vector p and
 *          scattered momentum vector p'
 *       b. the indices h,k,l belong to a right-handed orthogonal system
 *       c. the basic unit cell of the crystal is cubic
 *
 * 4. Goniometer settings are recorded at the maximum of the rocking curve
 *    when the spot is aligned in the upper zy laboratory half-plane.
 *    A goniometer setting is defined by the following three angles,
 *    whose sense is defined by an active rotation of the mount.
 *       theta - rotation around the lab x axis, direction z->y, [-pi/2,pi/2]
 *       chi - rotation around the z' axis, direction x->y, [-pi,pi]
 *       phi - rotation around the x'' axis, direction z->y, [-pi,pi]
 *
 * 5. Two diffraction settings {h,k,l -> theta,chi,phi} for scattering
 *    in a monochromatic X-ray beam of known energy E are accepted as inputs.
 *       E (real) - X-ray beam energy (GeV)
 *       hkl (integer[3]) - Miller indices of scattering plane
 *       theta,chi,phi (double) - goniometer setting at peak (degrees)
 *
 * 5a. In case the diamond orientation is not known, the user may not have
 *    a clear idea what hkl to assign a given diffraction peak.  An option
 *    has been added to allow the user to enter the twotheta values for the
 *    settings instead of hkl, and tell the program to list the possible
 *    hkl assignments, from which the user can chose one.
 *
 * 6. After the above inputs have been entered, the user can then ask for
 *    the settings that will set up the goniometer for diffraction from an
 *    arbitrary crystal vector hkl.  In addition, the user is also asked to
 *    provide a "wobble" angle to completely fix the crystal orientation
 *    (see point 7 below for details).  Outputs are as follows.
 *       twotheta (real) - goniometer setting (degrees)
 *       theta (real) - goniometer setting (degrees)
 *       chi (real) - goniometer setting (degrees)
 *       psi (real) - goniometer setting (degrees)
 *    By reporting the results as gonimeter settings, the program makes
 *    it easy for the user to compute goniometer offsets to align with
 *    other arbitrary crystal planes.
 *
 * 7. Ambiguities
 *    There is an ambiguity in aligning a crystal for X-ray scattering
 *    from a particular h,k,l plane: any rotation within the plane
 *    perpendicular to the q vector preserves the scattering condition.
 *    In general this does not correspond to one of the goniometer axes,
 *    but rather a mixture of the three here called a "wobble".  Sometimes
 *    the geometry of the detector setup prevents the goniometer from
 *    rotating over the full range in theta,chi,phi so it is useful to
 *    exploit the wobble to find an allowed goniometer setting that can
 *    reach a particular set of planes.  The user is able to enter arbitrary
 *    values for the wobble to search for the most convenient choice.
 */

const double hbarc=1.97326e-7;     // hbar*c in eV.m
const double kX=15.0e3;            // X-ray beam energy (eV)
const double chisquare_cutoff=100; // only consider hypotheses better than this

#include <iostream>
#include <math.h>
#include <stdio.h>
#include <time.h>
#include <vector>
#include <algorithm>

#define use_diamond 1
//#define use_silicon 1

#if use_diamond
double aLattice=3.5597e-10;  // diamond lattice constant (m)
char crystal[]="diamond";
#elif use_silicon
double aLattice=5.43095e-10; // silicon lattice constant (m)
char crystal[]="silicon";
#else
# error "You must select either silicon or diamond"
#endif

struct gset_ {       // angles in degrees
   double theta;
   double chi;
   double phi;
};

struct miller_ {
   int h;
   int k;
   int l;
};

struct xpeak_ {
   struct miller_ miller;
   struct gset_ goni;
   double twotheta;
};

typedef struct miller_ miller_t;
typedef struct gset_ gset_t;
typedef struct xpeak_ xpeak_t;
typedef std::vector<double> vector_t;

class rmatrix {

 // Class rmatrix provides the basic linear algebra
 // functions necessary to interpret and apply
 // the 4-circle goniometer settings

 public:
   rmatrix() {};
   rmatrix(const rmatrix& src);
   rmatrix& setRotateX(double theta);
   rmatrix& setRotateY(double theta);
   rmatrix& setRotateZ(double theta);
   rmatrix& setAlignment(const vector_t q1, const vector_t q2);
   rmatrix& setDiffraction(const miller_t& indi, const double phi=0);
   rmatrix& setGoniometer(const gset_t& goni);
   gset_t getGoniometer() const;
   double getBraggAngle(const miller_t indi) const;
   vector_t getCrystalVector(const miller_t indi) const;
   vector_t getLabVector(const miller_t indi) const;
   rmatrix inverse() const;
   vector_t rotate(const vector_t vector) const;
   rmatrix operator*(const rmatrix& src) const;
   rmatrix& operator=(const rmatrix& src);
   rmatrix& operator*=(const rmatrix& src);
   void dump();
   double& operator()(int i, int j) { return rmat[i-1][j-1]; }

 private:
   double rmat[3][3];
};

class hypothesis {

 // Class hypothesis contains a unique identification
 // of which crystal planes give rise to a given pair
 // of diffraction peaks

 public:
   inline bool operator<(const hypothesis& h) const {
      return (chisquare < h.chisquare);
   };
   xpeak_t peak1;
   xpeak_t perr1;
   xpeak_t peak2;
   xpeak_t perr2;
   double chisquare;
};

rmatrix::rmatrix(const rmatrix& src)
{
   // Copy constructor

   rmat[0][0] = src.rmat[0][0];
   rmat[1][0] = src.rmat[1][0];
   rmat[2][0] = src.rmat[2][0];
     rmat[0][1] = src.rmat[0][1];
     rmat[1][1] = src.rmat[1][1];
     rmat[2][1] = src.rmat[2][1];
       rmat[0][2] = src.rmat[0][2];
       rmat[1][2] = src.rmat[1][2];
       rmat[2][2] = src.rmat[2][2];
}

rmatrix& rmatrix::setRotateX(double theta)
{
   // Rotate y->z, passive, degrees

   double thetar=theta*M_PI/180;
   rmat[0][0] = 1;
   rmat[1][0] = 0;
   rmat[2][0] = 0;
     rmat[0][1] = 0;
     rmat[1][1] = cos(thetar);
     rmat[2][1] = -sin(thetar);
       rmat[0][2] = 0;
       rmat[1][2] = sin(thetar);
       rmat[2][2] = cos(thetar);
   return *this;
}

rmatrix& rmatrix::setRotateY(double theta)
{
   // Rotate z->x, passive, degrees

   double thetar=theta*M_PI/180;
   rmat[0][0] = cos(thetar);
   rmat[1][0] = 0;
   rmat[2][0] = sin(thetar);
     rmat[0][1] = 0;
     rmat[1][1] = 1;
     rmat[2][1] = 0;
       rmat[0][2] = -sin(thetar);
       rmat[1][2] = 0;
       rmat[2][2] = cos(thetar);
   return *this;
}

rmatrix& rmatrix::setRotateZ(double theta)
{
   // Rotate x->y, passive, degrees

   double thetar=theta*M_PI/180;
   rmat[0][0] = cos(thetar);
   rmat[1][0] = -sin(thetar);
   rmat[2][0] = 0;
     rmat[0][1] = sin(thetar);
     rmat[1][1] = cos(thetar);
     rmat[2][1] = 0;
       rmat[0][2] = 0;
       rmat[1][2] = 0;
       rmat[2][2] = 1;
   return *this;
}

rmatrix& rmatrix::setAlignment(const vector_t q1, const vector_t q2)
{
   // Set rmatrix to rotate the vector q1 into
   // the direction (0,0,1) and at the same time
   // rotate the vector q2 into the xz half-plane
   // with x>0.

   double q1perp=sqrt(q1[0]*q1[0]+q1[1]*q1[1]);
   double theta=atan2(q1perp,q1[2])*180/M_PI;
   double phi=atan2(q1[1],q1[0])*180/M_PI;
   setRotateY(theta);
   rmatrix stage;
   *this *= stage.setRotateZ(phi);
   vector_t q2r=rotate(q2);
   double psi=atan2(q2r[1],q2r[0])*180/M_PI;
   *this = stage.setRotateZ(psi)*(*this);
   return *this;
}

rmatrix& rmatrix::setDiffraction(const miller_t& indi, const double phi)
{
   // Set rmatrix to transform from the crystal axis frame
   // to the frame in which the lattice vector indicated by indi
   // is aligned for diffraction in the lab system.  To make the
   // solution unique, the goniometer phi is specified as an input.

   vector_t q=getCrystalVector(indi);

   // Last rotation step appears first in the product
   setRotateX(getBraggAngle(indi));

   // Take the phi offset out of q,
   // we will stick it back onto R at the end
   rmatrix Rphi;
   Rphi.setRotateX(phi);
   vector_t qq=Rphi.rotate(q);
   double qqperp=sqrt(qq[0]*qq[0]+qq[1]*qq[1]);

   // This combination puts q in the lab (0,1,0) direction
   rmatrix stage;
   if (qqperp > 1e-10) {
      double theta = atan2(qqperp,qq[2])*180/M_PI;
      double chi = atan2(qq[1],qq[0])*180/M_PI;
      *this *= stage.setRotateX(90-theta);
      *this *= stage.setRotateZ(chi-90);
   }
   else {
      *this *= stage.setRotateX(90);
   }

   // Now stick the phi rotation back on the end
   *this *= Rphi;
   return *this;
}

rmatrix& rmatrix::setGoniometer(const gset_t& goni)
{
   // Set for active transformation (theta,chi,phi)
   // from goniometer mount coordinates to the lab
   rmatrix stage;
   this->setRotateX(+goni.theta);         // theta is about x, direction z->y
   *this *= stage.setRotateZ(-goni.chi);  // chi is about z', direction x->y
   *this *= stage.setRotateX(+goni.phi);  // phi is about x'', direction z->y
   return *this;
}

gset_t rmatrix::getGoniometer() const
{
   // inverse of setGoniometer
   gset_t solution;

   // Extract theta,chi by transforming xhat to the lab frame
   // ghat = R xhat
   //      = ( cos(chi), sin(chi) cos(theta), -sin(chi) sin(theta) )
   // Note: if chi is exactly zero or +/-pi then the solution is singular
   // pointing to the fact that theta and phi are redundant.  In this case
   // I arbitrarily assign theta to 0 degrees to break the ambiguity.
   solution.theta = -atan(rmat[2][0]/(rmat[1][0]+1e-35))*180/M_PI;
   solution.chi = atan2(rmat[1][0]/cos(solution.theta*M_PI/180),rmat[0][0])
                  *180/M_PI;

   // Extract phi by transforming yhat to the lab frame
   // and then undoing the chi and theta parts of the transformation
   // ghat = Rz(chi) Rx(-theta) R yhat
   //      = ( 0, cos(phi), sin(phi) )
   rmatrix result;
   rmatrix stage;
   result.setRotateZ(solution.chi);
   result *= stage.setRotateX(-solution.theta);
   result *= *this;
   solution.phi = atan2(result.rmat[1][2],result.rmat[2][2])*180/M_PI;
   return solution;
}

double rmatrix::getBraggAngle(const miller_t indi) const
{
   // Find the Bragg angle for diffraction from the lattice planes
   // specified by the specified Miller indices.

   double qmax=kX*aLattice/(M_PI*hbarc);
   double qmag=sqrt(indi.h*indi.h+indi.k*indi.k+indi.l*indi.l);
   double thetaB=asin(qmag/qmax)*180/M_PI;
   return thetaB;
}

vector_t rmatrix::getCrystalVector(const miller_t indi) const
{
   // Create a crystal vector containing Miller indices as components

   vector_t q(3);
   q[0]=indi.h;
   q[1]=indi.k;
   q[2]=indi.l;
   return q;
}

vector_t rmatrix::getLabVector(const miller_t indi) const
{
   // Create a lab vector representing a crystal vector
   // in the position for its diffraction.

   vector_t g(3);
   double gmag=sqrt(indi.h*indi.h+indi.k*indi.k+indi.l*indi.l);
   double thetaBr=getBraggAngle(indi)*M_PI/180;
   g[0]=0;
   g[1]=gmag*cos(thetaBr);
   g[2]=-gmag*sin(thetaBr);
   return g;
}

rmatrix rmatrix::inverse() const
{
   // Invert the sense of rotation, passive <-> active
   rmatrix res;
   res.rmat[0][0] = rmat[0][0];
   res.rmat[1][0] = rmat[0][1];
   res.rmat[2][0] = rmat[0][2];
     res.rmat[0][1] = rmat[1][0];
     res.rmat[1][1] = rmat[1][1];
     res.rmat[2][1] = rmat[1][2];
       res.rmat[0][2] = rmat[2][0];
       res.rmat[1][2] = rmat[2][1];
       res.rmat[2][2] = rmat[2][2];
   return res;
}

vector_t rmatrix::rotate(const vector_t vector) const
{
   // Rotate vector according to rmatrix
   vector_t result(3);
   result[0] = rmat[0][0]*vector[0]
             + rmat[0][1]*vector[1]
             + rmat[0][2]*vector[2];
   result[1] = rmat[1][0]*vector[0]
             + rmat[1][1]*vector[1]
             + rmat[1][2]*vector[2];
   result[2] = rmat[2][0]*vector[0]
             + rmat[2][1]*vector[1]
             + rmat[2][2]*vector[2];
   return result;
}

rmatrix rmatrix::operator*(const rmatrix& src) const
{
   rmatrix res;
   res.rmat[0][0] = rmat[0][0]*src.rmat[0][0]
                  + rmat[0][1]*src.rmat[1][0]
                  + rmat[0][2]*src.rmat[2][0];
   res.rmat[0][1] = rmat[0][0]*src.rmat[0][1]
                  + rmat[0][1]*src.rmat[1][1]
                  + rmat[0][2]*src.rmat[2][1];
   res.rmat[0][2] = rmat[0][0]*src.rmat[0][2]
                  + rmat[0][1]*src.rmat[1][2]
                  + rmat[0][2]*src.rmat[2][2];

   res.rmat[1][0] = rmat[1][0]*src.rmat[0][0]
                  + rmat[1][1]*src.rmat[1][0]
                  + rmat[1][2]*src.rmat[2][0];
   res.rmat[1][1] = rmat[1][0]*src.rmat[0][1]
                  + rmat[1][1]*src.rmat[1][1]
                  + rmat[1][2]*src.rmat[2][1];
   res.rmat[1][2] = rmat[1][0]*src.rmat[0][2]
                  + rmat[1][1]*src.rmat[1][2]
                  + rmat[1][2]*src.rmat[2][2];

   res.rmat[2][0] = rmat[2][0]*src.rmat[0][0]
                  + rmat[2][1]*src.rmat[1][0]
                  + rmat[2][2]*src.rmat[2][0];
   res.rmat[2][1] = rmat[2][0]*src.rmat[0][1]
                  + rmat[2][1]*src.rmat[1][1]
                  + rmat[2][2]*src.rmat[2][1];
   res.rmat[2][2] = rmat[2][0]*src.rmat[0][2]
                  + rmat[2][1]*src.rmat[1][2]
                  + rmat[2][2]*src.rmat[2][2];
   return res;
}

rmatrix& rmatrix::operator*=(const rmatrix& src)
{
   rmatrix res = (*this)*src;
   return *this = res;
}

rmatrix& rmatrix::operator=(const rmatrix& src)
{
   rmat[0][0] = src.rmat[0][0];
   rmat[1][0] = src.rmat[1][0];
   rmat[2][0] = src.rmat[2][0];
   rmat[0][1] = src.rmat[0][1];
   rmat[1][1] = src.rmat[1][1];
   rmat[2][1] = src.rmat[2][1];
   rmat[0][2] = src.rmat[0][2];
   rmat[1][2] = src.rmat[1][2];
   rmat[2][2] = src.rmat[2][2];
   return *this;
}

void rmatrix::dump()
{
   printf(" / %12.6f  %12.6f  %12.6f \\\n", rmat[0][0], rmat[0][1], rmat[0][2]);
   printf("|  %12.6f  %12.6f  %12.6f  |\n", rmat[1][0], rmat[1][1], rmat[1][2]);
   printf(" \\ %12.6f  %12.6f  %12.6f /\n", rmat[2][0], rmat[2][1], rmat[2][2]);
}

double gaussian(const double mu, const double sigma)
{
   // Generate a pseudo-random number from a normal distribution
   // with mean mu and rms sigma.  This function is not thread-safe.

   static int nleft=0;
   const int maxvalues=200;
   static double values[maxvalues];

   static unsigned int seed=0;
   if (seed == 0) {
      seed = (int)time(0);
      srand48(seed);
   }

   if (nleft == 0) {
      while (nleft < maxvalues) {
         double u=drand48();
         double r=sqrt(-2*log(u));
         double phir=drand48()*2*M_PI;
         values[nleft++] = r*cos(phir);
         values[nleft++] = r*sin(phir);
      }
   }
   return sigma*values[--nleft]+mu;
}

double evaluate(const hypothesis &hypo)
{
   // Compare the measured goniometer settings in peak1 and peak2 with
   // the hypothesis contained in peak1 and peak2 for the corresponding
   // lattice vectors.  The measurement errors are taken from perr1 and
   // perr2.  A chisquare for the hypothesis is estimated based on a
   // Monte Carlo method and a chisquare is returned.  The chisquare has
   // 3 degrees of freedom and should be on the order of unity in order
   // to support the hypothesis, assuming the errors are reliable.

   rmatrix R1,R2;

   // Define the vectors q1, q2 and their dot product
   vector_t q1=R1.getCrystalVector(hypo.peak1.miller);
   vector_t q2=R2.getCrystalVector(hypo.peak2.miller);
   double q1_q2=q1[0]*q2[0]+q1[1]*q2[1]+q1[2]*q2[2];

   // Define the vectors g1, g2
   double thetaB1=R1.getBraggAngle(hypo.peak1.miller);
   vector_t g1=R1.getLabVector(hypo.peak1.miller);
   double thetaB2=R2.getBraggAngle(hypo.peak2.miller);
   vector_t g2=R2.getLabVector(hypo.peak2.miller);

   // Now compare g1.g2 with q1.q2 and compute a chisquare
   // between them using the errors on the measured angles.
   double moment[3] = {0,0,0};
   for (int s=0; s < 25; ++s) {
      gset_t set1;
      gset_t set2;
      set1.theta = gaussian(hypo.peak1.goni.theta,hypo.perr1.goni.theta);
      set1.chi = gaussian(hypo.peak1.goni.chi,hypo.perr1.goni.chi);
      set1.phi = gaussian(hypo.peak1.goni.phi,hypo.perr1.goni.phi);
      set2.theta = gaussian(hypo.peak2.goni.theta,hypo.perr2.goni.theta);
      set2.chi = gaussian(hypo.peak2.goni.chi,hypo.perr2.goni.chi);
      set2.phi = gaussian(hypo.peak2.goni.phi,hypo.perr2.goni.phi);

      R1.setGoniometer(set1);
      R2.setGoniometer(set2);
      vector_t g1t=R1.inverse().rotate(g1);
      vector_t g2t=R2.inverse().rotate(g2);
      double g1t_g2t=g1t[0]*g2t[0]+g1t[1]*g2t[1]+g1t[2]*g2t[2];
      moment[0] += 1;
      moment[1] += g1t_g2t;
      moment[2] += g1t_g2t*g1t_g2t;
   }
   double g1_g2t_mean = moment[1]/moment[0];
   double g1_g2t_rms = sqrt(moment[2]/moment[0]-g1_g2t_mean*g1_g2t_mean);

   // The chisquare has three degrees of freedom
   //  1. from comparison of goniometer settings with lattice vectors
   //  2. from match of 2theta for peak 1
   //  3. from match of 2theta for peak 2
   double pull[3];
   pull[0] = (g1_g2t_mean-q1_q2)/(g1_g2t_rms+1e-10);
   pull[1] = (hypo.peak1.twotheta-2*thetaB1)/(hypo.perr1.twotheta+1e-10);
   pull[2] = (hypo.peak2.twotheta-2*thetaB2)/(hypo.perr2.twotheta+1e-10);
   double chisquare=(pull[0]*pull[0]+pull[1]*pull[1]+pull[2]*pull[2])/3;

   // Print diagnostic information if desired
#define verbose_evaluate 0
#if verbose_evaluate
   std::cout << "Evaluating " << peak1.miller.h << ", "
                              << peak1.miller.k << ", "
                              << peak1.miller.l << " and "
                              << peak2.miller.h << ", "
                              << peak2.miller.k << ", "
                              << peak2.miller.l << ":"
             << std::endl;
   std::cout << "   Peak 1: thetaB=" << thetaB1 << ", 2theta=" << 2*thetaB1
             << ", measured=" << peak1.twotheta << ", pull=" << pull[1]
             << std::endl;
   std::cout << "   Peak 2: thetaB=" << thetaB2 << ", 2theta=" << 2*thetaB2
             << ", measured=" << peak2.twotheta << ", pull=" << pull[2]
             << std::endl;
   std::cout << "   q1.q2=" << q1_q2 << ", g1.g2t=" << g1_g2t_mean
             << ", error=" << g1_g2t_rms << ", pull=" << pull[0]
             << std::endl;
   std::cout << "   Overall chisquare=" << chisquare << std::endl;
#endif

   return chisquare;
}

void finder(const xpeak_t peak1, const xpeak_t perr1,
            const xpeak_t peak2, const xpeak_t perr2)
{
   // Look for plausible candidates for a pair of diffraction peaks
   // which are assumed to diffract in the upper yz laboratory plane.

   const int listsize=9999;
   hypothesis hypolist[listsize];
   int hypotheses=0;
   
   // For a cubic structure like this, it makes no sense to consider every
   // possible sign combination of h,k,l.  Many of them are equivalent,
   // and correspond to different choices for which crystal axis is
   // labeled x, y and z.  For a pair of lattice vectors there are up to
   // 24 equivalent ways to assign them to h1,k1,l1 and h2,k2,l2.  This
   // is seen by fixing the vectors in space and counting the ways that
   // the axes can be assigned: 6 choices for x, and for each of these
   // 4 choices for y, and for each of these only one choice for z.  I
   // want to list only the distinct possibilities, so I adopt some rules
   // for including a pair of vectors in the list hypotheses.
   //
   //  1. Indices h1,k1,l1 for the first vector are all non-negative.
   //  2. For any position in the first vector that contains a zero,
   //     apply rule 1 to the corresponding component of vector 2.
   //  3. No member of the set h1,k1,l1 is larger than h1.
   //  4. For the case where the largest of the set h1,k1,l1 is not
   //     unique, the smallest value in the set is l1.
   //  5. For the case where all three values h1,k1,l1 are the same,
   //     apply rules 3-4 to vector 2.
   //
   // Loop through all allowed lattice vectors for diamond structure.
   // The rules for allowed lattice vectors in diamond are as follows,
   // either h, k, l are all odd or their sum is a multiple of 4.

   double hmax = kX*aLattice/(M_PI*hbarc);
   for (int h1=1; h1 <= hmax; ++h1) {
      for (int k1=0; k1 <= h1; ++k1) {
         for (int l1=0; l1 < h1 || k1 == h1 && l1 == h1; ++l1) {
            if (h1/2*2 == h1 || k1/2*2 == k1 || l1/2*2 == l1) {
               if (h1/2*2 != h1 || k1/2*2 != k1 || l1/2*2 != l1) {
                  continue;
               }
               if ((h1+k1+l1)/4*4 != h1+k1+l1) {
                  continue;
               }
            }
            double hmag1=sqrt(h1*h1+k1*k1+l1*l1);
            if (hmag1 > hmax) {
               continue;
            }
            double thetaB1=asin(hmag1/hmax)*180/M_PI;
            if (fabs(peak1.twotheta-2*thetaB1) > 5*perr1.twotheta) {
               continue;
            }
            for (int h2=-int(hmax); h2 <= hmax; ++h2) {
               if (h1 == 0 && h2 < 0) {
                  continue;
               }
               for (int k2=-int(hmax); k2 <= hmax; ++k2) {
                  if (k1 == 0 && k2 < 0) {
                     continue;
                  }
                  for (int l2=-int(hmax); l2 <= hmax; ++l2) {
                     if (h2 == 0 && k2 == 0 && l2 == 0) {
                        continue;
                     }
                     if (l1 == 0 && l2 < 0) {
                        continue;
                     }
                     if (h2/2*2 == h2 || k2/2*2 == k2 || l2/2*2 == l2) {
                        if (h2/2*2 != h2 || k2/2*2 != k2 || l2/2*2 != l2) {
                           continue;
                        }
                        else if ((h2+k2+l2)/4*4 != h2+k2+l2) {
                           continue;
                        }
                     }
                     if (sqrt(h2*h2+k2*k2+l2*l2) > hmax) {
                        continue;
                     }
                     double hmag2=sqrt(h2*h2+k2*k2+l2*l2);
                     if (hmag2 > hmax) {
                        continue;
                     }
                     double thetaB2=asin(hmag2/hmax)*180/M_PI;
                     if (fabs(peak2.twotheta-2*thetaB2) > 5*perr2.twotheta) {
                        continue;
                     }
                     if (h1 == k1 && h1 == l1) {
                        if (h2 < k2 || h2 < l2 || h2 == l2) {
                           continue;
                        }
                     }
                     if ((h1 == h2 && k1 == k2 && l1 == l2) ||
                         (h1 == -h2 && k1 == -k2 && l1 == -l2)) {
                        continue;
                     }
                     hypothesis &hyp=hypolist[hypotheses];
                     hyp.peak1 = peak1;
                     hyp.peak1.miller.h = h1;
                     hyp.peak1.miller.k = k1;
                     hyp.peak1.miller.l = l1;
                     hyp.perr1 = perr1;
                     hyp.peak2 = peak2;
                     hyp.peak2.miller.h = h2;
                     hyp.peak2.miller.k = k2;
                     hyp.peak2.miller.l = l2;
                     hyp.perr2 = perr2;
                     hyp.chisquare = evaluate(hyp);
                     if (hyp.chisquare < chisquare_cutoff) {
                        ++hypotheses;
                     }
                  }
               }
            }
         }
      }
   }

   if (hypotheses == 0) {
      std::cout << "   There is no plausible hypothesis for these two planes."
                << std::endl
                << "   Please check the errors and try again."
                << std::endl;
   }
   else {
      std::sort(hypolist,hypolist+hypotheses);
      for (int h=0; h < hypotheses && h < 25; h++) {
         rmatrix R;
         hypothesis &hyp=hypolist[h];
         double thetaB1=R.getBraggAngle(hyp.peak1.miller);
         double thetaB2=R.getBraggAngle(hyp.peak2.miller);
         std::cout << "     (" << hyp.peak1.miller.h << ","
                   << hyp.peak1.miller.k << "," << hyp.peak1.miller.l
                   << ") 2theta=" << 2*thetaB1 << " and ("
                   << hyp.peak2.miller.h << ","
                   << hyp.peak2.miller.k << "," << hyp.peak2.miller.l
                   << ") 2theta=" << 2*thetaB2
                   << ": chisquare=" << hyp.chisquare
                   << std::endl;
      }
   }
}

void test_rmatrix()
{
   // diagnostics - test rmatrix class

   std::cout << "axisfinder diagnostics: testing rmatrix class" << std::endl;

   while (1 > 0) {
      gset_t goni;
      rmatrix R;
      std::cout << "Enter goniometer theta chi phi: ";
      std::cin >> goni.theta >> goni.chi >> goni.phi;
      R.setGoniometer(goni);
      std::cout << "  R matrix is as follows: " << std::endl;
      R.dump();
      gset_t goni2 = R.getGoniometer();
      std::cout << "  Goniometer settings seemed to be theta,chi,phi = " 
                << goni2.theta << ","
                << goni2.chi << ","
                << goni2.phi << std::endl;
      std::cout << "  R matrix inverse is as follows: " << std::endl;
      rmatrix Rinv = R.inverse();
      Rinv.dump();
      gset_t goni3 = Rinv.getGoniometer();
      std::cout << "  Inverted goniometer settings seemed to be theta,chi,phi = " 
                << goni3.theta << "," << goni3.chi << "," << goni3.phi
                << std::endl;
      std::cout << "  Product of R and Rtranspose is as follows:" << std::endl;
      (Rinv*R).dump();
      miller_t indi;
      std::cout << "Enter h k l for a diffraction peak: ";
      std::cin >> indi.h >> indi.k >> indi.l;
      vector_t q=R.getCrystalVector(indi);
      rmatrix Rx;
      double phi=118;  //arbitrary test value
      Rx.setDiffraction(indi,phi);
      std::cout << "  Rx puts that lattice vector in the diffraction condition"
                << std::endl;
      Rx.dump();
      std::cout << "  So let's see if it actually does so..." << std::endl;
      vector_t g=Rx.rotate(q);
      std::cout << "    Rx q = ( " << g[0] << ", " << g[1] << ", " << g[2]
                << " )" << std::endl;
      goni = Rx.getGoniometer();
      std::cout << "  The goniometer settings for Rx are theta chi phi = "
                << goni.theta << " " << goni.chi << " " << goni.phi
                << std::endl;
      rmatrix Rg,Rq;
      std::cout << "  When I set alignment of g with q then I get"
                << std::endl;
      Rg.setAlignment(g,q).dump();
      vector_t gg=Rg.rotate(g);
      vector_t qg=Rg.rotate(q);
      std::cout << "  with g=" << gg[0] << "," << gg[1] << "," << gg[2]
                << "  q=" << qg[0] << "," << qg[1] << "," << qg[2]
                << std::endl;
      std::cout << "  whereas when I set alignment of q with g then I get"
                << std::endl;
      Rq.setAlignment(q,g).dump();
      vector_t gq=Rq.rotate(g);
      vector_t qq=Rq.rotate(q);
      std::cout << "  with g=" << gq[0] << "," << gq[1] << "," << gq[2]
                << "  q=" << qq[0] << "," << qq[1] << "," << qq[2]
                << std::endl;
   }
}

void test_finder()
{
   // diagnostics -- test finder function

   std::cout << "axisfinder diagnostics: testing finder" << std::endl;

   double g=gaussian(0,0); // make sure drand48 is initialized

   while (1 > 0) {
      xpeak_t peak1,peak2;
      xpeak_t perr1,perr2;
      std::cout << "Enter h k l for diffraction peak 1: ";
      std::cin >> peak1.miller.h >> peak1.miller.k >> peak1.miller.l;
      std::cout << "Enter h k l for diffraction peak 2: ";
      std::cin >> peak2.miller.h >> peak2.miller.k >> peak2.miller.l;
      std::cout << "Enter measurement errors for theta chi phi 2theta: ";
      std::cin >> perr1.goni.theta >> perr1.goni.chi >> perr1.goni.phi
               >> perr1.twotheta;
      perr2.goni = perr1.goni;
      perr2.twotheta = perr1.twotheta;

      // generate a random crystal orientation in its mount
      // Rx transforms from the crystal frame to the goniometer mount
      gset_t offset;
      offset.theta = 180*(drand48()-0.5);
      offset.chi = 360*(drand48()-0.5);
      offset.phi = 360*(drand48()-0.5);
      rmatrix Rx;
      Rx.setGoniometer(offset);

      // Generate the rotation that orients crystal vectors q1 and q2
      // to diffract the beam in the upper vertical plane in the lab.
      // R1 transforms from the goniometer mount to the lab frame for q1
      // R2 transforms from the goniometer mount to the lab frame for q2
      vector_t q1=Rx.getCrystalVector(peak1.miller);
      vector_t q2=Rx.getCrystalVector(peak2.miller);
      double thetaB1=Rx.getBraggAngle(peak1.miller);
      double thetaB2=Rx.getBraggAngle(peak2.miller);

      // Compute the goniometer rotations that
      // set up diffraction for the two vectors
      double phi=90;
      rmatrix R1,R2;
      R1.setDiffraction(peak1.miller,phi);
      R2.setDiffraction(peak2.miller,phi);
      R1 *= Rx.inverse();
      R2 *= Rx.inverse();

      // Smear these goniometer settings and pass them to finder
      peak1.goni = R1.getGoniometer();
      peak1.goni.theta = gaussian(peak1.goni.theta,perr1.goni.theta);
      peak1.goni.chi = gaussian(peak1.goni.chi,perr1.goni.chi);
      peak1.goni.phi = gaussian(peak1.goni.phi,perr1.goni.phi);
      peak1.twotheta = gaussian(2*thetaB1,perr1.twotheta);
      peak2.goni = R2.getGoniometer();
      peak2.goni.theta = gaussian(peak2.goni.theta,perr2.goni.theta);
      peak2.goni.chi = gaussian(peak2.goni.chi,perr2.goni.chi);
      peak2.goni.phi = gaussian(peak2.goni.phi,perr2.goni.phi);
      peak2.twotheta = gaussian(2*thetaB2,perr2.twotheta);
      finder(peak1,perr1,peak2,perr2);
   }
}

int main()
{
//#define diagnostic_test_rmatrix
#if defined diagnostic_test_finder
   test_finder();
   return 0;
#elif defined diagnostic_test_rmatrix
   test_rmatrix();
   return 0;
#else

   xpeak_t inset[2];
   xpeak_t inerr[2];

   std::cout << " axisfinder: determine gonimeter settings that set up correct"
             << std::endl
             << " diffraction conditions for a given set of crystal planes in "
             << crystal << std::endl;
   std::cout << " The monochromated X-ray energy is set to "
             << kX/1e3 << " KeV" << std::endl;
   std::cout << std::endl;
   std::cout << " 1) Input two goniometer peak settings" 
             << " to determine offsets" << std::endl;
   std::cout << "    Enter gonimeter theta chi phi 2theta for peak 1: ";
   std::cin >> inset[0].goni.theta >> inset[0].goni.chi >> inset[0].goni.phi
            >> inset[0].twotheta;
   std::cout << "    Enter gonimeter theta chi phi 2theta errors for peak 1: ";
   std::cin >> inerr[0].goni.theta >> inerr[0].goni.chi >> inerr[0].goni.phi
            >> inerr[0].twotheta;
   std::cout << "    Enter gonimeter theta chi phi 2theta for peak 2: ";
   std::cin >> inset[1].goni.theta >> inset[1].goni.chi >> inset[1].goni.phi
            >> inset[1].twotheta;
   std::cout << "    Enter gonimeter theta chi phi 2theta errors for peak 2: ";
   std::cin >> inerr[1].goni.theta >> inerr[1].goni.chi >> inerr[1].goni.phi
            >> inerr[1].twotheta;

   std::cout << "    Below are listed some possible hypotheses "
                "for what these peaks might be." << std::endl;
   std::cout << "    Examine the chisquare and the twotheta values "
                "to chose the best hypothesis." << std::endl;
   finder(inset[0],inerr[0],inset[1],inerr[1]);

   std::cout << "    Enter Miller indices h k l for peak 1: ";
   std::cin >> inset[0].miller.h >> inset[0].miller.k >> inset[0].miller.l;
   std::cout << "    Enter Miller indices h k l for peak 2: "; 
   std::cin >> inset[1].miller.h >> inset[1].miller.k >> inset[1].miller.l;

   // create rotations for these two diffraction conditions
   rmatrix R1,R2;
   R1.setDiffraction(inset[0].miller);
   R2.setDiffraction(inset[1].miller);

   // find the q vectors for these two sets of planes
   vector_t q1=R1.getCrystalVector(inset[0].miller);
   vector_t q2=R2.getCrystalVector(inset[1].miller);

   // find the g vectors for these two sets of planes
   vector_t g1=R1.getLabVector(inset[0].miller);
   vector_t g2=R2.getLabVector(inset[1].miller);

   // find the p vectors for these two sets of planes
   vector_t p1=R1.inverse().rotate(g1);
   vector_t p2=R2.inverse().rotate(g2);

   // solve the system of simultaneous vector equations
   //    p1 = Rx q1
   //    p2 = Rx q2
   // for Rx subject to the condition that p1.p2=q1.q2

   // Strategy: find rotation Rp that puts p1 into a standard
   // alignment.  Find the rotation Rq that puts q1 into the
   // same alignment.  It doesn't matter what convention is
   // taken for the alignment, provided that it is unique.
   // If the condition p1.p2=q1.q2 holds then it follows that
   //    Rp p1 = Rq q1
   //    Rp p2 = Rq q2
   // which leads to the solution Rx = Rp.inverse()*Rq
   rmatrix Rp,Rq;
   Rp.setAlignment(p1,p2);
   Rq.setAlignment(q1,q2);
   rmatrix Rx = Rp.inverse()*Rq;

   std::cout << "   If you can give the goniometer settings"
             << " that place the crystal in a" << std::endl
             << "   standard orientation in the laboratory"
             << " then you can find out what" << std::endl
             << "   lattice vectors point along the standard"
             << " laboratory directions." << std::endl;
   std::cout << "   What theta chi phi orient put the crystal"
             << " in a standard orientation?" << std::endl;
   double theta0,chi0,phi0;
   std::cin >> theta0 >> chi0 >> phi0;
   gset_t set0;
   set0.theta = theta0;
   set0.chi = chi0;
   set0.phi = phi0;
   rmatrix R0;
   R0.setGoniometer(set0);
   vector_t xhat(3),yhat(3),zhat(3);
   xhat[0]=1; yhat[0]=0; zhat[0]=0;
   xhat[1]=0; yhat[1]=1; zhat[1]=0;
   xhat[2]=0; yhat[2]=0; zhat[2]=1;
   rmatrix Rlab_to_crystal=Rx.inverse()*R0.inverse();
   vector_t crystalXhat=Rlab_to_crystal.rotate(xhat);
   vector_t crystalYhat=Rlab_to_crystal.rotate(yhat);
   vector_t crystalZhat=Rlab_to_crystal.rotate(zhat);
   std::cout << "   Crystal vector " << crystalZhat[0] << ","
             << crystalZhat[1] << "," << crystalZhat[2]
             << " points in the beam direction" << std::endl;
   std::cout << "   Crystal vector " << crystalYhat[0] << ","
             << crystalYhat[1] << "," << crystalYhat[2]
             << " points vertically up" << std::endl;
   std::cout << "   Crystal vector " << crystalXhat[0] << ","
             << crystalXhat[1] << "," << crystalXhat[2]
             << " points beam-left" << std::endl;
   return 0;

#endif
}