IndexingUtils.cpp

#include "MantidGeometry/Crystal/IndexingUtils.h"
#include "MantidGeometry/Crystal/NiggliCell.h"
#include "MantidKernel/Quat.h"
#include <algorithm>
#include <cmath>
#include <boost/math/special_functions/round.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <stdexcept>

extern "C" {
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sys.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fft_real.h>
}

using namespace Mantid::Geometry;
using Mantid::Kernel::V3D;
using Mantid::Kernel::Matrix;
using Mantid::Kernel::DblMatrix;
using Mantid::Kernel::Quat;

namespace {
const constexpr double DEG_TO_RAD = M_PI / 180.;
const constexpr double RAD_TO_DEG = 180. / M_PI;
}

/**
  STATIC method Find_UB: Calculates the matrix that most nearly indexes
  the specified q_vectors, given the lattice parameters.  The sum of the
  squares of the residual errors is returned.  This method first sorts the
  specified q_vectors in order of increasing magnitude.  It then searches
  through all possible orientations to find an initial UB that indexes the
  lowest magnitude peaks.
     The resolution of the search through possible orientations is specified
  by the degrees_per_step parameter.  Approximately 2-4 degrees_per_step is
  usually adequate.  NOTE: This is an expensive calculation which takes
  approximately 1 second using 2 degrees_per_step.  However, the execution
  time is O(n^3) so decreasing the resolution to 1 degree per step will take
  about 8 seconds, etc.  It should not be necessary to decrease this value
  below 1 degree per step, and users will have to be VERY patient, if it is
  decreased much below 1 degree per step.
    The number of peaks used to obtain an initial indexing is specified by
  the "NumInitial" parameter.  Good values for this are typically around
  10-15, though with accurate peak positions, and good values for the lattice
  paramters, as few as 2 can be used.  Using substantially more than 15 peaks
  initially typically has no benefit and increases execution time.

  @param  UB                  3x3 matrix that will be set to the UB matrix
  @param  q_vectors           std::vector of V3D objects that contains the
                              list of q_vectors that are to be indexed
                              NOTE: There must be at least 2 q_vectors.
  @param  lattice             The orientated lattice with the lattice
                              parameters a,b,c and alpha, beta, gamma. The found
                              UB and errors will be set on this lattice.
  @param  required_tolerance  The maximum allowed deviation of Miller indices
                              from integer values for a peak to be indexed.
  @param  base_index          The sequence number of the peak that should
                              be used as the central peak.  On the first
                              scan for a UB matrix that fits the data,
                              the remaining peaks in the list of q_vectors
                              will be shifted by -base_peak, where base_peak
                              is the q_vector with the specified base index.
                              If fewer than 5 peaks are specified in the
                              q_vectors list, this parameter is ignored.
                              If this parameter is -1, and there are at least
                              four peaks in the q_vector list, then a base
                              index will be calculated internally.  In most
                              cases, it should suffice to set this to -1.
  @param  num_initial         The number of low |Q| peaks that should be
                              used to scan for an initial orientation matrix.
  @param  degrees_per_step    The number of degrees between different
                              orientations used during the initial scan.
  @param  fixAll              Fix the lattice parameters and do not optimise
                              the UB matrix.

  @return  This will return the sum of the squares of the residual errors.

  @throws  std::invalid_argument exception if UB is not a 3X3 matrix,
                                 if there are not at least 2 q vectors,
                                 if num_initial is < 2, or
                                 if the required_tolerance or degrees_per_step
                                 is <= 0.
*/
double IndexingUtils::Find_UB(DblMatrix &UB, const std::vector<V3D> &q_vectors,
                              OrientedLattice &lattice,
                              double required_tolerance, int base_index,
                              size_t num_initial, double degrees_per_step,
                              bool fixAll) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Find_UB(): UB matrix NULL or not 3X3");
  }

  if (q_vectors.size() < 2) {
    throw std::invalid_argument(
        "Find_UB(): Two or more indexed peaks needed to find UB");
  }

  if (required_tolerance <= 0) {
    throw std::invalid_argument(
        "Find_UB(): required_tolerance must be positive");
  }

  if (num_initial < 2) {
    throw std::invalid_argument(
        "Find_UB(): number of peaks for inital scan must be > 2");
  }

  if (degrees_per_step <= 0) {
    throw std::invalid_argument("Find_UB(): degrees_per_step must be positive");
  }

  // get list of peaks, sorted on |Q|
  std::vector<V3D> sorted_qs;
  sorted_qs.reserve(q_vectors.size());

  if (q_vectors.size() > 5) // shift to be centered on peak (we lose
                            // one peak that way, so require > 5)
  {
    std::vector<V3D> shifted_qs(q_vectors);
    size_t mid_ind = q_vectors.size() / 3;
    // either do an initial sort and use
    // default mid index, or use the index
    // specified by the base_peak parameter
    if (base_index < 0 || base_index >= static_cast<int>(q_vectors.size())) {
      std::sort(shifted_qs.begin(), shifted_qs.end(), V3D::CompareMagnitude);
    } else {
      mid_ind = base_index;
    }
    V3D mid_vec(shifted_qs[mid_ind]);

    for (size_t i = 0; i < shifted_qs.size(); i++) {
      if (i != mid_ind) {
        V3D shifted_vec(shifted_qs[i]);
        shifted_vec -= mid_vec;
        sorted_qs.push_back(shifted_vec);
      }
    }
  } else {
    for (const auto &q_vector : q_vectors)
      sorted_qs.push_back(q_vector);
  }

  std::sort(sorted_qs.begin(), sorted_qs.end(), V3D::CompareMagnitude);

  if (num_initial > sorted_qs.size())
    num_initial = sorted_qs.size();

  std::vector<V3D> some_qs;
  some_qs.reserve(q_vectors.size());

  for (size_t i = 0; i < num_initial; i++)
    some_qs.push_back(sorted_qs[i]);

  ScanFor_UB(UB, some_qs, lattice, degrees_per_step, required_tolerance);

  double fit_error = 0;
  std::vector<V3D> miller_ind;
  std::vector<V3D> indexed_qs;
  miller_ind.reserve(q_vectors.size());
  indexed_qs.reserve(q_vectors.size());

  // now gradually bring in the remaining
  // peaks and re-optimize the UB to index
  // them as well
  size_t count = 0;
  while (!fixAll && num_initial < sorted_qs.size()) {
    count++;
    num_initial = std::lround(1.5 * static_cast<double>(num_initial + 3));
    // add 3, in case we started with
    // a very small number of peaks!
    if (num_initial >= sorted_qs.size())
      num_initial = sorted_qs.size();

    for (size_t i = some_qs.size(); i < num_initial; i++)
      some_qs.push_back(sorted_qs[i]);

    try {
      GetIndexedPeaks(UB, some_qs, required_tolerance, miller_ind, indexed_qs,
                      fit_error);
      Matrix<double> temp_UB(3, 3, false);
      fit_error = Optimize_UB(temp_UB, miller_ind, indexed_qs);
      UB = temp_UB;
    } catch (...) {
      // failed to fit using these peaks, so add some more and try again
    }
  }

  std::vector<double> sigabc(7);
  if (!fixAll &&
      q_vectors.size() >= 3) // try one last refinement using all peaks
  {
    try {
      GetIndexedPeaks(UB, q_vectors, required_tolerance, miller_ind, indexed_qs,
                      fit_error);
      Matrix<double> temp_UB = UB;
      fit_error = Optimize_UB(temp_UB, miller_ind, indexed_qs, sigabc);
      UB = temp_UB;
    } catch (...) {
      // failed to improve UB using these peaks, so just return the current UB
    }
  }
  // Regardless of how we got the UB, find the
  // sum-squared errors for the indexing in
  // HKL space.
  GetIndexedPeaks(UB, q_vectors, required_tolerance, miller_ind, indexed_qs,
                  fit_error);
  // set the error on the lattice parameters
  lattice.setUB(UB);
  lattice.setError(sigabc[0], sigabc[1], sigabc[2], sigabc[3], sigabc[4],
                   sigabc[5]);
  return fit_error;
}

/**
    STATIC method Find_UB: This method will attempt to calculate the matrix
  that most nearly indexes the specified q_vectors, given only a range of
  possible unit cell edge lengths.  If successful, the matrix should
  correspond to the Niggli reduced cell.
     The resolution of the search through possible orientations is specified
  by the degrees_per_step parameter.  Approximately 1-3 degrees_per_step is
  usually adequate.  NOTE: This is an expensive calculation which takes
  approximately 1 second using 1 degree_per_step.  However, the execution
  time is O(n^3) so decreasing the resolution to 0.5 degree per step will take
  about 8 seconds, etc.  It should not be necessary to decrease this value
  below 1 degree per step, and users will have to be VERY patient, if it is
  decreased much below 1 degree per step.
    The number of peaks used to obtain an initial indexing is specified by
  the "NumInitial" parameter.  Good values for this are typically around
  15-25.  The specified q_vectors must correspond to a single crystal.  If
  several crystallites are present or there are other sources of "noise"
  leading to invalid peaks, this method will not work well.  The method that
  uses lattice parameters may be better in such cases.  Alternatively, adjust
  the list of specified q_vectors so it does not include noise peaks or peaks
  from more than one crystal, by increasing the threshold for what counts
  as a peak, or by other methods.

  @param  UB                  3x3 matrix that will be set to the UB matrix
  @param  q_vectors           std::vector of V3D objects that contains the
                              list of q_vectors that are to be indexed
                              NOTE: There must be at least 3 q_vectors.
  @param  min_d               Lower bound on shortest unit cell edge length.
                              This does not have to be specified exactly but
                              must be strictly less than the smallest edge
                              length, in Angstroms.
  @param  max_d               Upper bound on longest unit cell edge length.
                              This does not have to be specified exactly but
                              must be strictly more than the longest edge
                              length in angstroms.
  @param  required_tolerance  The maximum allowed deviation of Miller indices
                              from integer values for a peak to be indexed.
  @param  base_index          The sequence number of the peak that should
                              be used as the central peak.  On the first
                              scan for a UB matrix that fits the data,
                              the remaining peaks in the list of q_vectors
                              will be shifted by -base_peak, where base_peak
                              is the q_vector with the specified base index.
                              If fewer than 6 peaks are specified in the
                              q_vectors list, this parameter is ignored.
                              If this parameter is -1, and there are at least
                              five peaks in the q_vector list, then a base
                              index will be calculated internally.  In most
                              cases, it should suffice to set this to -1.
  @param  num_initial         The number of low |Q| peaks that should be
                              used to scan for an initial orientation matrix.
  @param  degrees_per_step    The number of degrees between different
                              orientations used during the initial scan.

  @return  This will return the sum of the squares of the residual errors.

  @throws  std::invalid_argument exception if UB is not a 3X3 matrix,
                                 if there are not at least 3 q vectors,
                                 if min_d >= max_d or min_d <= 0
                                 if num_initial is < 3, or
                                 if the required_tolerance or degrees_per_step
                                 is <= 0.
*/
double IndexingUtils::Find_UB(DblMatrix &UB, const std::vector<V3D> &q_vectors,
                              double min_d, double max_d,
                              double required_tolerance, int base_index,
                              size_t num_initial, double degrees_per_step) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Find_UB(): UB matrix NULL or not 3X3");
  }

  if (q_vectors.size() < 3) {
    throw std::invalid_argument(
        "Find_UB(): Three or more indexed peaks needed to find UB");
  }

  if (min_d >= max_d || min_d <= 0) {
    throw std::invalid_argument("Find_UB(): Need 0 < min_d < max_d");
  }

  if (required_tolerance <= 0) {
    throw std::invalid_argument(
        "Find_UB(): required_tolerance must be positive");
  }

  if (num_initial < 3) {
    throw std::invalid_argument(
        "Find_UB(): number of peaks for inital scan must be > 2");
  }

  if (degrees_per_step <= 0) {
    throw std::invalid_argument("Find_UB(): degrees_per_step must be positive");
  }

  // get list of peaks, sorted on |Q|
  std::vector<V3D> sorted_qs;
  sorted_qs.reserve(q_vectors.size());

  if (q_vectors.size() > 5) // shift to be centered on peak (we lose
                            // one peak that way, so require > 5)
  {
    std::vector<V3D> shifted_qs(q_vectors);
    size_t mid_ind = q_vectors.size() / 2;
    // either do an initial sort and use
    // default mid index, or use the index
    // specified by the base_peak parameter
    if (base_index < 0 || base_index >= static_cast<int>(q_vectors.size())) {
      std::sort(shifted_qs.begin(), shifted_qs.end(), V3D::CompareMagnitude);
    } else {
      mid_ind = base_index;
    }
    V3D mid_vec(shifted_qs[mid_ind]);

    for (size_t i = 0; i < shifted_qs.size(); i++) {
      if (i != mid_ind) {
        V3D shifted_vec(shifted_qs[i]);
        shifted_vec -= mid_vec;
        sorted_qs.push_back(shifted_vec);
      }
    }
  } else {
    for (const auto &q_vector : q_vectors)
      sorted_qs.push_back(q_vector);
  }

  std::sort(sorted_qs.begin(), sorted_qs.end(), V3D::CompareMagnitude);

  if (num_initial > sorted_qs.size())
    num_initial = sorted_qs.size();

  std::vector<V3D> some_qs;
  some_qs.reserve(q_vectors.size());

  for (size_t i = 0; i < num_initial; i++)
    some_qs.push_back(sorted_qs[i]);
  std::vector<V3D> directions;
  ScanFor_Directions(directions, some_qs, min_d, max_d, required_tolerance,
                     degrees_per_step);

  if (directions.size() < 3) {
    throw std::runtime_error(
        "Find_UB(): Could not find at least three possible lattice directions");
  }

  std::sort(directions.begin(), directions.end(), V3D::CompareMagnitude);

  if (!FormUB_From_abc_Vectors(UB, directions, 0, min_d, max_d)) {
    throw std::runtime_error(
        "Find_UB(): Could not find independent a, b, c directions");
  }
  // now gradually bring in the remaining
  // peaks and re-optimize the UB to index
  // them as well
  std::vector<V3D> miller_ind;
  std::vector<V3D> indexed_qs;
  miller_ind.reserve(q_vectors.size());
  indexed_qs.reserve(q_vectors.size());

  Matrix<double> temp_UB(3, 3, false);
  double fit_error = 0;
  while (num_initial < sorted_qs.size()) {
    num_initial = std::lround(1.5 * static_cast<double>(num_initial + 3));
    // add 3, in case we started with
    // a very small number of peaks!
    if (num_initial >= sorted_qs.size())
      num_initial = sorted_qs.size();

    for (size_t i = some_qs.size(); i < num_initial; i++)
      some_qs.push_back(sorted_qs[i]);

    GetIndexedPeaks(UB, some_qs, required_tolerance, miller_ind, indexed_qs,
                    fit_error);
    try {
      fit_error = Optimize_UB(temp_UB, miller_ind, indexed_qs);
      UB = temp_UB;
    } catch (...) {
      // failed to improve with these peaks, so continue with more peaks
      // if possible
    }
  }

  if (q_vectors.size() >= 5) // try one last refinement using all peaks
  {
    GetIndexedPeaks(UB, q_vectors, required_tolerance, miller_ind, indexed_qs,
                    fit_error);
    try {
      fit_error = Optimize_UB(temp_UB, miller_ind, indexed_qs);
      UB = temp_UB;
    } catch (...) {
      // failed to improve with all peaks, so return the UB we had
    }
  }

  if (NiggliCell::MakeNiggliUB(UB, temp_UB))
    UB = temp_UB;

  return fit_error;
}

/**
    STATIC method Find_UB: This method will attempt to calculate the matrix
  that most nearly indexes the specified q_vectors, using FFTs to find
  patterns in projected Q-vectors, given only a range of possible unit cell
  edge lengths. If successful, the resulting matrix should correspond to
  the Niggli reduced cell.
    The resolution of the search through possible orientations is specified
  by the degrees_per_step parameter.  One to two degrees per step is usually
  adequate.
  NOTE: The execution time is O(n^3) where n is the number of degrees per
  step, so decreasing the resolution to 0.5 degree per step will take
  about 8 times longer than using 1 degree per step.  It should not be
  necessary to decrease this value below 1 degree per step, and users will
  have to be VERY patient, if it is decreased much below 1 degree per step.
    The specified q_vectors should correspond to a single crystal, for this
  to work reliably.

  @param  UB                  3x3 matrix that will be set to the UB matrix
  @param  q_vectors           std::vector of V3D objects that contains the
                              list of q_vectors that are to be indexed
                              NOTE: There must be at least 4 q_vectors and it
                              really should have at least 10 or more peaks
                              for this to work quite consistently.
  @param  min_d               Lower bound on shortest unit cell edge length.
                              This does not have to be specified exactly but
                              must be strictly less than the smallest edge
                              length, in Angstroms.
  @param  max_d               Upper bound on longest unit cell edge length.
                              This does not have to be specified exactly but
                              must be strictly more than the longest edge
                              length in angstroms.
  @param  required_tolerance  The maximum allowed deviation of Miller indices
                              from integer values for a peak to be indexed.
  @param  degrees_per_step    The number of degrees between different
                              orientations used during the initial scan.

  @return  This will return the sum of the squares of the residual errors.

  @throws  std::invalid_argument exception if UB is not a 3X3 matrix,
                                 if there are not at least 3 q vectors,
                                 if min_d >= max_d or min_d <= 0,
                                 if the required_tolerance or degrees_per_step
                                 is <= 0,
                                 if at least three possible a,b,c directions
                                 were not found,
                                 or if a valid UB matrix could not be formed
                                 from the a,b,c directions that were found.
*/
double IndexingUtils::Find_UB(DblMatrix &UB, const std::vector<V3D> &q_vectors,
                              double min_d, double max_d,
                              double required_tolerance,
                              double degrees_per_step) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Find_UB(): UB matrix NULL or not 3X3");
  }

  if (q_vectors.size() < 4) {
    throw std::invalid_argument(
        "Find_UB(): Four or more indexed peaks needed to find UB");
  }

  if (min_d >= max_d || min_d <= 0) {
    throw std::invalid_argument("Find_UB(): Need 0 < min_d < max_d");
  }

  if (required_tolerance <= 0) {
    throw std::invalid_argument(
        "Find_UB(): required_tolerance must be positive");
  }

  if (degrees_per_step <= 0) {
    throw std::invalid_argument("Find_UB(): degrees_per_step must be positive");
  }

  std::vector<V3D> directions;

  // NOTE: we use a somewhat higher tolerance when
  // finding individual directions since it is easier
  // to index one direction individually compared to
  // indexing three directions simultaneously.
  size_t max_indexed =
      FFTScanFor_Directions(directions, q_vectors, min_d, max_d,
                            0.75f * required_tolerance, degrees_per_step);

  if (max_indexed == 0) {
    throw std::invalid_argument(
        "Find_UB(): Could not find any a,b,c vectors to index Qs");
  }

  if (directions.size() < 3) {
    throw std::invalid_argument(
        "Find_UB(): Could not find enough a,b,c vectors");
  }

  std::sort(directions.begin(), directions.end(), V3D::CompareMagnitude);

  double min_vol = min_d * min_d * min_d / 4.0;

  if (!FormUB_From_abc_Vectors(UB, directions, q_vectors, required_tolerance,
                               min_vol)) {
    throw std::invalid_argument(
        "Find_UB(): Could not form UB matrix from a,b,c vectors");
  }

  Matrix<double> temp_UB(3, 3, false);
  double fit_error = 0;
  if (q_vectors.size() >= 5) // repeatedly refine UB
  {
    std::vector<V3D> miller_ind;
    std::vector<V3D> indexed_qs;
    miller_ind.reserve(q_vectors.size());
    indexed_qs.reserve(q_vectors.size());

    GetIndexedPeaks(UB, q_vectors, required_tolerance, miller_ind, indexed_qs,
                    fit_error);

    for (int counter = 0; counter < 4; counter++) {
      try {
        fit_error = Optimize_UB(temp_UB, miller_ind, indexed_qs);
        UB = temp_UB;
        GetIndexedPeaks(UB, q_vectors, required_tolerance, miller_ind,
                        indexed_qs, fit_error);
      } catch (std::exception &) {
        // failed to improve with all peaks, so just keep the UB we had
      }
    }
  }

  if (NiggliCell::MakeNiggliUB(UB, temp_UB))
    UB = temp_UB;

  return fit_error;
}

/**
  STATIC method Optimize_UB: Calculates the matrix that most nearly maps
  the specified hkl_vectors to the specified q_vectors.  The calculated
  UB minimizes the sum squared differences between UB*(h,k,l) and the
  corresponding (qx,qy,qz) for all of the specified hkl and Q vectors.
  The sum of the squares of the residual errors is returned.  This method is
  used to optimize the UB matrix once an initial indexing has been found.

  @param  UB           3x3 matrix that will be set to the UB matrix
  @param  hkl_vectors  std::vector of V3D objects that contains the
                       list of hkl values
  @param  q_vectors    std::vector of V3D objects that contains the list of
                       q_vectors that are indexed by the corresponding hkl
                       vectors.
  @param  sigabc      error in the crystal lattice parameter values if length
                      is at least 6. NOTE: Calculation of these errors is based
  on
                      SCD FORTRAN code base at IPNS. Contributors to the least
                      squares application(1979) are J.Marc Overhage, G.Anderson,
                      P. C. W. Leung, R. G. Teller, and  A. J. Schultz
  NOTE: The number of hkl_vectors and q_vectors must be the same, and must
        be at least 3.

  @return  This will return the sum of the squares of the residual differences
           between the Q vectors provided and the UB*hkl values, in
           reciprocal space.

  @throws  std::invalid_argument exception if there are not at least 3
                                 hkl and q vectors, or if the numbers of
                                 hkl and q vectors are not the same, or if
                                 the UB matrix is not a 3x3 matrix.

  @throws  std::runtime_error    exception if the QR factorization fails or
                                 the UB matrix can't be calculated or if
                                 UB is a singular matrix.
*/
double IndexingUtils::Optimize_UB(DblMatrix &UB,
                                  const std::vector<V3D> &hkl_vectors,
                                  const std::vector<V3D> &q_vectors,
                                  std::vector<double> &sigabc) {
  double result = 0;
  result = Optimize_UB(UB, hkl_vectors, q_vectors);

  if (sigabc.size() < 6) {
    sigabc.clear();
    return result;
  } else
    for (int i = 0; i < 6; i++)
      sigabc[i] = 0.0;

  size_t nDOF = 3 * (hkl_vectors.size() - 3);
  DblMatrix HKLTHKL(3, 3);
  for (int r = 0; r < 3; r++)
    for (int c = 0; c < 3; c++)
      for (const auto &hkl_vector : hkl_vectors) {
        HKLTHKL[r][c] +=
            hkl_vector[r] * hkl_vector[c]; // rounded??? to nearest integer
      }

  HKLTHKL.Invert();

  double SMALL = 1.525878906E-5;
  Matrix<double> derivs(3, 7);
  std::vector<double> latOrig, latNew;
  GetLatticeParameters(UB, latOrig);

  for (int r = 0; r < 3; r++) {
    for (int c = 0; c < 3; c++) {

      UB[r][c] += SMALL;
      GetLatticeParameters(UB, latNew);
      UB[r][c] -= SMALL;

      for (size_t l = 0; l < 7; l++)
        derivs[c][l] = (latNew[l] - latOrig[l]) / SMALL;
    }

    for (size_t l = 0;
         l < std::min<size_t>(static_cast<size_t>(7), sigabc.size()); l++)
      for (int m = 0; m < 3; m++)
        for (int n = 0; n < 3; n++)
          sigabc[l] += (derivs[m][l] * HKLTHKL[m][n] * derivs[n][l]);
  }

  double delta = result / static_cast<double>(nDOF);

  for (size_t i = 0; i < std::min<size_t>(7, sigabc.size()); i++)
    sigabc[i] = sqrt(delta * sigabc[i]);

  return result;
}

/**
  STATIC method Optimize_UB: Calculates the matrix that most nearly maps
  the specified hkl_vectors to the specified q_vectors.  The calculated
  UB minimizes the sum squared differences between UB*(h,k,l) and the
  corresponding (qx,qy,qz) for all of the specified hkl and Q vectors.
  The sum of the squares of the residual errors is returned.  This method is
  used to optimize the UB matrix once an initial indexing has been found.

  @param  UB           3x3 matrix that will be set to the UB matrix
  @param  hkl_vectors  std::vector of V3D objects that contains the
                       list of hkl values
  @param  q_vectors    std::vector of V3D objects that contains the list of
                       q_vectors that are indexed by the corresponding hkl
                       vectors.
  NOTE: The number of hkl_vectors and q_vectors must be the same, and must
        be at least 3.

  @return  This will return the sum of the squares of the residual differences
           between the Q vectors provided and the UB*hkl values, in
           reciprocal space.

  @throws  std::invalid_argument exception if there are not at least 3
                                 hkl and q vectors, or if the numbers of
                                 hkl and q vectors are not the same, or if
                                 the UB matrix is not a 3x3 matrix.

  @throws  std::runtime_error    exception if the QR factorization fails or
                                 the UB matrix can't be calculated or if
                                 UB is a singular matrix.
*/
double IndexingUtils::Optimize_UB(DblMatrix &UB,
                                  const std::vector<V3D> &hkl_vectors,
                                  const std::vector<V3D> &q_vectors) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Optimize_UB(): UB matrix NULL or not 3X3");
  }

  if (hkl_vectors.size() < 3) {
    throw std::invalid_argument(
        "Optimize_UB(): Three or more indexed peaks needed to find UB");
  }

  if (hkl_vectors.size() != q_vectors.size()) {
    throw std::invalid_argument(
        "Optimize_UB(): Number of hkl_vectors != number of q_vectors");
  }

  gsl_matrix *H_transpose = gsl_matrix_alloc(hkl_vectors.size(), 3);
  gsl_vector *tau = gsl_vector_alloc(3);

  double sum_sq_error = 0;
  // Make the H-transpose matrix from the
  // hkl vectors and form QR factorization
  for (size_t row = 0; row < hkl_vectors.size(); row++) {
    for (size_t col = 0; col < 3; col++) {
      gsl_matrix_set(H_transpose, row, col, (hkl_vectors[row])[col]);
    }
  }

  int returned_flag = gsl_linalg_QR_decomp(H_transpose, tau);

  if (returned_flag != 0) {
    gsl_matrix_free(H_transpose);
    gsl_vector_free(tau);
    throw std::runtime_error(
        "Optimize_UB(): gsl QR_decomp failed, invalid hkl values");
  }
  // solve for each row of UB, using the
  // QR factorization of and accumulate the
  // sum of the squares of the residuals
  gsl_vector *UB_row = gsl_vector_alloc(3);
  gsl_vector *q = gsl_vector_alloc(q_vectors.size());
  gsl_vector *residual = gsl_vector_alloc(q_vectors.size());

  bool found_UB = true;

  for (size_t row = 0; row < 3; row++) {
    for (size_t i = 0; i < q_vectors.size(); i++) {
      gsl_vector_set(q, i, (q_vectors[i])[row] / (2.0 * M_PI));
    }

    returned_flag =
        gsl_linalg_QR_lssolve(H_transpose, tau, q, UB_row, residual);
    if (returned_flag != 0) {
      found_UB = false;
    }

    for (size_t i = 0; i < 3; i++) {
      double value = gsl_vector_get(UB_row, i);
      if (!std::isfinite(value))
        found_UB = false;
    }

    V3D row_values(gsl_vector_get(UB_row, 0), gsl_vector_get(UB_row, 1),
                   gsl_vector_get(UB_row, 2));
    UB.setRow(row, row_values);

    for (size_t i = 0; i < q_vectors.size(); i++) {
      sum_sq_error += gsl_vector_get(residual, i) * gsl_vector_get(residual, i);
    }
  }

  gsl_matrix_free(H_transpose);
  gsl_vector_free(tau);
  gsl_vector_free(UB_row);
  gsl_vector_free(q);
  gsl_vector_free(residual);

  if (!found_UB) {
    throw std::runtime_error(
        "Optimize_UB(): Failed to find UB, invalid hkl or Q values");
  }

  if (!CheckUB(UB)) {
    throw std::runtime_error("Optimize_UB(): The optimized UB is not valid");
  }

  return sum_sq_error;
}

/**
  STATIC method Optimize_Direction: Calculates the vector for which the
  dot product of the the vector with each of the specified Qxyz vectors
  is most nearly the corresponding integer index.  The calculated best_vec
  minimizes the sum squared differences between best_vec dot (qx,qy,z)
  and the corresponding index for all of the specified Q vectors and
  indices.  The sum of the squares of the residual errors is returned.
  NOTE: This method is similar the Optimize_UB method, but this method only
        optimizes the plane normal in one direction.  Also, this optimizes
        the mapping from (qx,qy,qz) to one index (Q to index), while the
        Optimize_UB method optimizes the mapping from three (h,k,l) to
        (qx,qy,qz) (3 indices to Q).

  @param  best_vec     V3D vector that will be set to a vector whose
                       direction most nearly corresponds to the plane
                       normal direction and whose magnitude is d.  The
                       corresponding plane spacing in reciprocal space
                       is 1/d.
  @param  index_values std::vector of ints that contains the list of indices
  @param  q_vectors    std::vector of V3D objects that contains the list of
                       q_vectors that are indexed in one direction by the
                       corresponding index values.
  NOTE: The number of index_values and q_vectors must be the same, and must
        be at least 3.

  @return  This will return the sum of the squares of the residual errors.

  @throws  std::invalid_argument exception if there are not at least 3
                                 indices and q vectors, or if the numbers of
                                 indices and q vectors are not the same.

  @throws  std::runtime_error    exception if the QR factorization fails or
                                 the best direction can't be calculated.
*/

double IndexingUtils::Optimize_Direction(V3D &best_vec,
                                         const std::vector<int> &index_values,
                                         const std::vector<V3D> &q_vectors) {
  if (index_values.size() < 3) {
    throw std::invalid_argument(
        "Optimize_Direction(): Three or more indexed values needed");
  }

  if (index_values.size() != q_vectors.size()) {
    throw std::invalid_argument(
        "Optimize_Direction(): Number of index_values != number of q_vectors");
  }

  gsl_matrix *H_transpose = gsl_matrix_alloc(q_vectors.size(), 3);
  gsl_vector *tau = gsl_vector_alloc(3);

  double sum_sq_error = 0;
  // Make the H-transpose matrix from the
  // q vectors and form QR factorization

  for (size_t row = 0; row < q_vectors.size(); row++) {
    for (size_t col = 0; col < 3; col++) {
      gsl_matrix_set(H_transpose, row, col,
                     (q_vectors[row])[col] / (2.0 * M_PI));
    }
  }
  int returned_flag = gsl_linalg_QR_decomp(H_transpose, tau);

  if (returned_flag != 0) {
    gsl_matrix_free(H_transpose);
    gsl_vector_free(tau);
    throw std::runtime_error(
        "Optimize_Direction(): gsl QR_decomp failed, invalid hkl values");
  }
  // solve for the best_vec, using the
  // QR factorization and accumulate the
  // sum of the squares of the residuals
  gsl_vector *x = gsl_vector_alloc(3);
  gsl_vector *indices = gsl_vector_alloc(index_values.size());
  gsl_vector *residual = gsl_vector_alloc(index_values.size());

  bool found_best_vec = true;

  for (size_t i = 0; i < index_values.size(); i++) {
    gsl_vector_set(indices, i, index_values[i]);
  }

  returned_flag = gsl_linalg_QR_lssolve(H_transpose, tau, indices, x, residual);
  if (returned_flag != 0) {
    found_best_vec = false;
  }

  for (size_t i = 0; i < 3; i++) {
    double value = gsl_vector_get(x, i);
    if (!std::isfinite(value))
      found_best_vec = false;
  }

  best_vec(gsl_vector_get(x, 0), gsl_vector_get(x, 1), gsl_vector_get(x, 2));

  for (size_t i = 0; i < index_values.size(); i++) {
    sum_sq_error += gsl_vector_get(residual, i) * gsl_vector_get(residual, i);
  }

  gsl_matrix_free(H_transpose);
  gsl_vector_free(tau);
  gsl_vector_free(x);
  gsl_vector_free(indices);
  gsl_vector_free(residual);

  if (!found_best_vec) {
    throw std::runtime_error("Optimize_Direction(): Failed to find best_vec, "
                             "invalid indexes or Q values");
  }

  return sum_sq_error;
}

/**
    The method uses two passes to scan across all possible directions and
    orientations to find the direction and orientation for the unit cell
    that best fits the specified list of peaks.
    On the first pass, only those sets of directions that index the
    most peaks are kept.  On the second pass, the directions that minimize
    the sum-squared deviations from integer indices are selected from that
    smaller set of directions.  This method should be most useful if number
    of peaks is on the order of 10-20, and most of the peaks belong to the
    same crystallite.
    @param UB                 This will be set to the UB matrix that best
                              indexes the supplied list of q_vectors.
    @param q_vectors          List of locations of peaks in "Q".
    @param cell               Unit cell defining the parameters a,b,c and
                              alpha, beta, gamma.
    @param degrees_per_step   The number of degrees per step used when
                              scanning through all possible directions and
                              orientations for the unit cell. NOTE: The
                              work required rises very rapidly as the number
                              of degrees per step decreases. A value of 1
                              degree leads to about 10 seconds of compute time.
                              while a value of 2 only requires a bit more than
                              1 sec.  The required time is O(n^3) where
                              n = 1/degrees_per_step.

    @param required_tolerance The maximum distance from an integer that the
                              calculated h,k,l values can have if a peak
                              is to be considered indexed.

  @throws std::invalid_argument exception if the UB matrix is not a 3X3 matrix.
  @throws std::runtime_error exception if the matrix inversion fails and UB
                             can't be formed

 */
double IndexingUtils::ScanFor_UB(DblMatrix &UB,
                                 const std::vector<V3D> &q_vectors,
                                 const UnitCell &cell, double degrees_per_step,
                                 double required_tolerance) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Find_UB(): UB matrix NULL or not 3X3");
  }

  auto a = cell.a();
  auto b = cell.b();
  auto c = cell.c();

  auto alpha = cell.alpha();
  auto beta = cell.beta();
  auto gamma = cell.gamma();

  V3D a_dir;
  V3D b_dir;
  V3D c_dir;

  double num_a_steps = std::round(90.0 / degrees_per_step);
  double gamma_radians = gamma * DEG_TO_RAD;

  int num_b_steps = boost::math::iround(4.0 * sin(gamma_radians) * num_a_steps);

  std::vector<V3D> a_dir_list =
      MakeHemisphereDirections(boost::numeric_cast<int>(num_a_steps));

  std::vector<V3D> b_dir_list;

  V3D a_dir_temp;
  V3D b_dir_temp;
  V3D c_dir_temp;

  double error;
  double dot_prod;
  double nearest_int;
  int max_indexed = 0;
  V3D q_vec;
  // first select those directions
  // that index the most peaks
  std::vector<V3D> selected_a_dirs;
  std::vector<V3D> selected_b_dirs;
  std::vector<V3D> selected_c_dirs;

  for (auto &a_dir_num : a_dir_list) {
    a_dir_temp = a_dir_num;
    a_dir_temp = V3D(a_dir_temp);
    a_dir_temp *= a;

    b_dir_list = MakeCircleDirections(boost::numeric_cast<int>(num_b_steps),
                                      a_dir_temp, gamma);

    for (auto &b_dir_num : b_dir_list) {
      b_dir_temp = b_dir_num;
      b_dir_temp = V3D(b_dir_temp);
      b_dir_temp *= b;
      c_dir_temp = Make_c_dir(a_dir_temp, b_dir_temp, c, alpha, beta, gamma);
      int num_indexed = 0;
      for (const auto &q_vector : q_vectors) {
        bool indexes_peak = true;
        q_vec = q_vector / (2.0 * M_PI);
        dot_prod = a_dir_temp.scalar_prod(q_vec);
        nearest_int = std::round(dot_prod);
        error = fabs(dot_prod - nearest_int);
        if (error > required_tolerance)
          indexes_peak = false;
        else {
          dot_prod = b_dir_temp.scalar_prod(q_vec);
          nearest_int = std::round(dot_prod);
          error = fabs(dot_prod - nearest_int);
          if (error > required_tolerance)
            indexes_peak = false;
          else {
            dot_prod = c_dir_temp.scalar_prod(q_vec);
            nearest_int = std::round(dot_prod);
            error = fabs(dot_prod - nearest_int);
            if (error > required_tolerance)
              indexes_peak = false;
          }
        }
        if (indexes_peak)
          num_indexed++;
      }

      if (num_indexed > max_indexed) // only keep those directions that
      {                              // index the max number of peaks
        selected_a_dirs.clear();
        selected_b_dirs.clear();
        selected_c_dirs.clear();
        max_indexed = num_indexed;
      }