IndexingUtils.cpp

Go to the documentation of this file.

// Mantid Repository : https://github.com/mantidproject/mantid
//
// Copyright © 2018 ISIS Rutherford Appleton Laboratory UKRI,
//   NScD Oak Ridge National Laboratory, European Spallation Source,
//   Institut Laue - Langevin & CSNS, Institute of High Energy Physics, CAS
// SPDX - License - Identifier: GPL - 3.0 +
#include "MantidGeometry/Crystal/IndexingUtils.h"
#include "MantidGeometry/Crystal/NiggliCell.h"
#include "MantidKernel/EigenConversionHelpers.h"
#include "MantidKernel/Quat.h"
 
#include <boost/math/special_functions/round.hpp>
#include <boost/numeric/conversion/cast.hpp>
 
#include <gsl/gsl_errno.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_sys.h>
#include <gsl/gsl_vector.h>
 
#include <algorithm>
#include <cmath>
 
using namespace Mantid::Geometry;
using Mantid::Kernel::DblMatrix;
using Mantid::Kernel::Matrix;
using Mantid::Kernel::Quat;
using Mantid::Kernel::V3D;
 
namespace {
const constexpr double DEG_TO_RAD = M_PI / 180.;
const constexpr double RAD_TO_DEG = 180. / M_PI;
} // namespace
 
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, int iterations) {
  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.emplace_back(shifted_vec);
      }
    }
  } else {
    std::copy(q_vectors.cbegin(), q_vectors.cend(), std::back_inserter(sorted_qs));
  }
 
  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.emplace_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
  while (!fixAll && 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.emplace_back(sorted_qs[i]);
    for (int counter = 0; counter < iterations; counter++) {
      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
  {
    for (int counter = 0; counter < iterations; counter++) {
      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;
}
 
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.emplace_back(shifted_vec);
      }
    }
  } else {
    std::copy(q_vectors.cbegin(), q_vectors.cend(), std::back_inserter(sorted_qs));
  }
 
  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.emplace_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.emplace_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;
}
 
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, int iterations) {
  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 < iterations; 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;
}
 
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;
}
 
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;
}
 
double IndexingUtils::Optimize_6dUB(DblMatrix &UB, DblMatrix &ModUB, const std::vector<V3D> &hkl_vectors,
                                    const std::vector<V3D> &mnp_vectors, const int &ModDim,
                                    const std::vector<V3D> &q_vectors, std::vector<double> &sigabc,
                                    std::vector<double> &sigq) {
 
  if (ModDim != 0 && ModDim != 1 && ModDim != 2 && ModDim != 3)
    throw std::invalid_argument("invalid Value for Modulation Dimension");
 
  double result = 0;
  result = Optimize_6dUB(UB, ModUB, hkl_vectors, mnp_vectors, ModDim, q_vectors);
 
  if (sigabc.size() < static_cast<size_t>(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];
      }
 
  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]);
 
  DblMatrix UBinv = UB;
  UBinv.Invert();
 
  if (sigq.size() < static_cast<size_t>(3)) {
    sigq.clear();
    return result;
  }
 
  else {
    sigq[0] = sqrt(delta) * latOrig[0];
    sigq[1] = sqrt(delta) * latOrig[1];
    sigq[2] = sqrt(delta) * latOrig[2];
  }
 
  return delta;
}
 
double IndexingUtils::Optimize_6dUB(DblMatrix &UB, DblMatrix &ModUB, const std::vector<V3D> &hkl_vectors,
                                    const std::vector<V3D> &mnp_vectors, const int &ModDim,
                                    const std::vector<V3D> &q_vectors) {
 
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Optimize_6dUB(): UB matrix NULL or not 3X3");
  }
 
  if (ModUB.numRows() != 3 || ModUB.numCols() != 3) {
    throw std::invalid_argument("Optimize_6dUB(): ModUB matrix NULL or not 3X3");
  }
 
  if (hkl_vectors.size() < 3) {
    throw std::invalid_argument("Optimize_6dUB(): Three or more indexed peaks needed to find UB");
  }
 
  if (hkl_vectors.size() != q_vectors.size() || hkl_vectors.size() != mnp_vectors.size()) {
    throw std::invalid_argument("Optimize_6dUB(): Number of hkl_vectors "
                                "doesn't match number of mnp_vectors or number "
                                "of q_vectors");
  }
 
  if (ModDim != 0 && ModDim != 1 && ModDim != 2 && ModDim != 3)
    throw std::invalid_argument("invalid Value for Modulation Dimension");
 
  gsl_matrix *H_transpose = gsl_matrix_alloc(hkl_vectors.size(), ModDim + 3);
  gsl_vector *tau = gsl_vector_alloc(ModDim + 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]);
    for (size_t col = 0; col < static_cast<size_t>(ModDim); col++)
      gsl_matrix_set(H_transpose, row, col + 3, (mnp_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 and mnp 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(ModDim + 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 < static_cast<size_t>(ModDim + 3); i++) {
      double value = gsl_vector_get(UB_row, i);
      if (!std::isfinite(value))
        found_UB = false;
    }
 
    V3D hklrow_values(gsl_vector_get(UB_row, 0), gsl_vector_get(UB_row, 1), gsl_vector_get(UB_row, 2));
    V3D mnprow_values(0, 0, 0);
 
    if (ModDim == 1)
      mnprow_values(gsl_vector_get(UB_row, 3), 0, 0);
    if (ModDim == 2)
      mnprow_values(gsl_vector_get(UB_row, 3), gsl_vector_get(UB_row, 4), 0);
    if (ModDim == 3)
      mnprow_values(gsl_vector_get(UB_row, 3), gsl_vector_get(UB_row, 4), gsl_vector_get(UB_row, 5));
 
    UB.setRow(row, hklrow_values);
    ModUB.setRow(row, mnprow_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;
}
 
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;
}
 
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();
 
  // Precompute required trigonometric functions
  const auto cosAlpha = std::cos(cell.alpha() * DEG_TO_RAD);
  const auto cosBeta = std::cos(cell.beta() * DEG_TO_RAD);
  const auto cosGamma = std::cos(cell.gamma() * DEG_TO_RAD);
  const auto sinGamma = std::sin(cell.gamma() * DEG_TO_RAD);
  const auto gamma_degrees = cell.gamma();
 
  V3D a_dir;
  V3D b_dir;
  V3D c_dir;
 
  double num_a_steps = std::round(90.0 / degrees_per_step);
  int num_b_steps = boost::math::iround(4.0 * sinGamma * 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_degrees);
 
    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 = makeCDir(a_dir_temp, b_dir_temp, c, cosAlpha, cosBeta, cosGamma, sinGamma);
      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;
      }
      if (num_indexed == max_indexed) {
        selected_a_dirs.emplace_back(a_dir_temp);
        selected_b_dirs.emplace_back(b_dir_temp);
        selected_c_dirs.emplace_back(c_dir_temp);
      }
    }
  }
  // now, for each such direction, find
  // the one that indexes closes to
  // integer values
  double min_error = 1.0e50;
  for (size_t dir_num = 0; dir_num < selected_a_dirs.size(); dir_num++) {
    a_dir_temp = selected_a_dirs[dir_num];
    b_dir_temp = selected_b_dirs[dir_num];
    c_dir_temp = selected_c_dirs[dir_num];
 
    double sum_sq_error = 0.0;
    for (const auto &q_vector : q_vectors) {
      q_vec = q_vector / (2.0 * M_PI);
      dot_prod = a_dir_temp.scalar_prod(q_vec);
      nearest_int = std::round(dot_prod);
      error = dot_prod - nearest_int;
      sum_sq_error += error * error;
 
      dot_prod = b_dir_temp.scalar_prod(q_vec);
      nearest_int = std::round(dot_prod);
      error = dot_prod - nearest_int;
      sum_sq_error += error * error;
 
      dot_prod = c_dir_temp.scalar_prod(q_vec);
      nearest_int = std::round(dot_prod);
      error = dot_prod - nearest_int;
      sum_sq_error += error * error;
    }
 
    if (sum_sq_error < min_error) {
      min_error = sum_sq_error;
      a_dir = a_dir_temp;
      b_dir = b_dir_temp;
      c_dir = c_dir_temp;
    }
  }
 
  if (!OrientedLattice::GetUB(UB, a_dir, b_dir, c_dir)) {
    throw std::runtime_error("UB could not be formed, invert matrix failed");
  }
 
  return min_error;
}
 
size_t IndexingUtils::ScanFor_Directions(std::vector<V3D> &directions, const std::vector<V3D> &q_vectors, double min_d,
                                         double max_d, double required_tolerance, double degrees_per_step) {
  double error;
  double fit_error;
  double dot_prod;
  double nearest_int;
  int max_indexed = 0;
  V3D q_vec;
  // first, make hemisphere of possible directions
  // with specified resolution.
  int num_steps = boost::math::iround(90.0 / degrees_per_step);
  std::vector<V3D> full_list = MakeHemisphereDirections(num_steps);
  // Now, look for possible real-space unit cell edges
  // by checking for vectors with length between
  // min_d and max_d that would index the most peaks,
  // in some direction, keeping the shortest vector
  // for each direction where the max peaks are indexed
  double delta_d = 0.1f;
  int n_steps = boost::math::iround(1.0 + (max_d - min_d) / delta_d);
 
  std::vector<V3D> selected_dirs;
  V3D dir_temp;
 
  for (const auto &current_dir : full_list) {
    for (int step = 0; step <= n_steps; step++) {
      dir_temp = current_dir;
      dir_temp *= (min_d + step * delta_d); // increasing size
 
      int num_indexed = 0;
      for (const auto &q_vector : q_vectors) {
        q_vec = q_vector / (2.0 * M_PI);
        dot_prod = dir_temp.scalar_prod(q_vec);
        nearest_int = std::round(dot_prod);
        error = fabs(dot_prod - nearest_int);
        if (error <= required_tolerance)
          num_indexed++;
      }
 
      if (num_indexed > max_indexed) // only keep those directions that
      {                              // index the max number of peaks
        selected_dirs.clear();
        max_indexed = num_indexed;
      }
      if (num_indexed >= max_indexed) {
        selected_dirs.emplace_back(dir_temp);
      }
    }
  }
  // Now, optimize each direction and discard possible
  // unit cell edges that are duplicates, putting the
  // new smaller list in the vector "directions"
  std::vector<int> index_vals;
  std::vector<V3D> indexed_qs;
  index_vals.reserve(q_vectors.size());
  indexed_qs.reserve(q_vectors.size());
 
  directions.clear();
  V3D current_dir;
  V3D diff;
  for (auto &selected_dir : selected_dirs) {
    current_dir = selected_dir;
 
    GetIndexedPeaks_1D(current_dir, q_vectors, required_tolerance, index_vals, indexed_qs, fit_error);
 
    Optimize_Direction(current_dir, index_vals, indexed_qs);
 
    double length = current_dir.norm();
    if (length >= min_d && length <= max_d) // only keep if within range
    {
      bool duplicate = false;
      for (auto &direction : directions) {
        dir_temp = direction;
        diff = current_dir - dir_temp;
        // discard same direction
        if (diff.norm() < 0.001) {
          duplicate = true;
        } else {
          diff = current_dir + dir_temp;
          // discard opposite direction
          if (diff.norm() < 0.001) {
            duplicate = true;
          }
        }
      }
      if (!duplicate) {
        directions.emplace_back(current_dir);
      }
    }
  }
  return max_indexed;
}
 
size_t IndexingUtils::FFTScanFor_Directions(std::vector<V3D> &directions, const std::vector<V3D> &q_vectors,
                                            double min_d, double max_d, double required_tolerance,
                                            double degrees_per_step) {
  constexpr size_t N_FFT_STEPS = 512;
  constexpr size_t HALF_FFT_STEPS = 256;
 
  double fit_error;
  int max_indexed = 0;
 
  // first, make hemisphere of possible directions
  // with specified resolution.
  int num_steps = boost::math::iround(90.0 / degrees_per_step);
  std::vector<V3D> full_list = MakeHemisphereDirections(num_steps);
 
  // find the maximum magnitude of Q to set range
  // needed for FFT
  double max_mag_Q = 0;
  for (size_t q_num = 1; q_num < q_vectors.size(); q_num++) {
    double mag_Q = q_vectors[q_num].norm() / (2.0 * M_PI);
    if (mag_Q > max_mag_Q)
      max_mag_Q = mag_Q;
  }
 
  max_mag_Q *= 1.1f; // allow for a little "headroom" for FFT range
 
  // apply the FFT to each of the directions, and
  // keep track of their maximum magnitude past DC
  double max_mag_fft;
  std::vector<double> max_fft_val;
  max_fft_val.resize(full_list.size());
 
  double projections[N_FFT_STEPS];
  double magnitude_fft[HALF_FFT_STEPS];
 
  double index_factor = N_FFT_STEPS / max_mag_Q; // maps |proj Q| to index
 
  for (size_t dir_num = 0; dir_num < full_list.size(); dir_num++) {
    V3D current_dir = full_list[dir_num];
    max_mag_fft = GetMagFFT(q_vectors, current_dir, N_FFT_STEPS, projections, index_factor, magnitude_fft);
 
    max_fft_val[dir_num] = max_mag_fft;
  }
  // find the directions with the 500 largest
  // fft values, and place them in temp_dirs vector
  int N_TO_TRY = 500;
 
  std::vector<double> max_fft_copy;
  max_fft_copy.resize(full_list.size());
  for (size_t i = 0; i < max_fft_copy.size(); i++) {
    max_fft_copy[i] = max_fft_val[i];
  }
 
  std::sort(max_fft_copy.begin(), max_fft_copy.end());
 
  size_t index = max_fft_copy.size() - 1;
  max_mag_fft = max_fft_copy[index];
 
  double threshold = max_mag_fft;
  while ((index > max_fft_copy.size() - N_TO_TRY) && threshold >= max_mag_fft / 2) {
    index--;
    threshold = max_fft_copy[index];
  }
 
  std::vector<V3D> temp_dirs;
  for (size_t i = 0; i < max_fft_val.size(); i++) {
    if (max_fft_val[i] >= threshold) {
      temp_dirs.emplace_back(full_list[i]);
    }
  }
  // now scan through temp_dirs and use the
  // FFT to find the cell edge length that
  // corresponds to the max_mag_fft.  Only keep
  // directions with length nearly in bounds
  V3D temp;
  std::vector<V3D> temp_dirs_2;
 
  for (auto &temp_dir : temp_dirs) {
    GetMagFFT(q_vectors, temp_dir, N_FFT_STEPS, projections, index_factor, magnitude_fft);
 
    double position = GetFirstMaxIndex(magnitude_fft, HALF_FFT_STEPS, threshold);
    if (position > 0) {
      double q_val = max_mag_Q / position;
      double d_val = 1 / q_val;
      if (d_val >= 0.8 * min_d && d_val <= 1.2 * max_d) {
        temp = temp_dir * d_val;
        temp_dirs_2.emplace_back(temp);
      }
    }
  }
  // look at how many peaks were indexed
  // for each of the initial directions
  max_indexed = 0;
  int num_indexed;
  V3D current_dir;
  for (auto &dir_num : temp_dirs_2) {
    current_dir = dir_num;
    num_indexed = NumberIndexed_1D(current_dir, q_vectors, required_tolerance);
    if (num_indexed > max_indexed)
      max_indexed = num_indexed;
  }
 
  // only keep original directions that index
  // at least 50% of max num indexed
  temp_dirs.clear();
  for (auto &dir_num : temp_dirs_2) {
    current_dir = dir_num;
    num_indexed = NumberIndexed_1D(current_dir, q_vectors, required_tolerance);
    if (num_indexed >= 0.50 * max_indexed)
      temp_dirs.emplace_back(current_dir);
  }
  // refine directions and again find the
  // max number indexed, for the optimized
  // directions
  max_indexed = 0;
  std::vector<int> index_vals;
  std::vector<V3D> indexed_qs;
  for (auto &temp_dir : temp_dirs) {
    num_indexed = GetIndexedPeaks_1D(temp_dir, q_vectors, required_tolerance, index_vals, indexed_qs, fit_error);
    try {
      int count = 0;
      while (count < 5) // 5 iterations should be enough for
      {                 // the optimization to stabilize
        Optimize_Direction(temp_dir, index_vals, indexed_qs);
 
        num_indexed = GetIndexedPeaks_1D(temp_dir, q_vectors, required_tolerance, index_vals, indexed_qs, fit_error);
        if (num_indexed > max_indexed)
          max_indexed = num_indexed;
 
        count++;
      }
    } catch (...) {
      // don't continue to refine if the direction fails to optimize properly
    }
  }
  // discard those with length out of bounds
  temp_dirs_2.clear();
  for (auto &temp_dir : temp_dirs) {
    current_dir = temp_dir;
    double length = current_dir.norm();
    if (length >= 0.8 * min_d && length <= 1.2 * max_d)
      temp_dirs_2.emplace_back(current_dir);
  }
  // only keep directions that index at
  // least 75% of the max number of peaks
  temp_dirs.clear();
  for (auto &dir_num : temp_dirs_2) {
    current_dir = dir_num;
    num_indexed = NumberIndexed_1D(current_dir, q_vectors, required_tolerance);
    if (num_indexed > max_indexed * 0.75)
      temp_dirs.emplace_back(current_dir);
  }
 
  std::sort(temp_dirs.begin(), temp_dirs.end(), V3D::compareMagnitude);
 
  // discard duplicates:
  double len_tol = 0.1; // 10% tolerance for lengths
  double ang_tol = 5.0; // 5 degree tolerance for angles
  DiscardDuplicates(directions, temp_dirs, q_vectors, required_tolerance, len_tol, ang_tol);
 
  return max_indexed;
}
 
double IndexingUtils::GetMagFFT(const std::vector<V3D> &q_vectors, const V3D &current_dir, const size_t N,
                                double projections[], double index_factor, double magnitude_fft[]) {
  for (size_t i = 0; i < N; i++) {
    projections[i] = 0.0;
  }
  // project onto direction
  V3D q_vec;
  for (const auto &q_vector : q_vectors) {
    q_vec = q_vector / (2.0 * M_PI);
    double dot_prod = current_dir.scalar_prod(q_vec);
    auto index = static_cast<size_t>(fabs(index_factor * dot_prod));
    if (index < N)
      projections[index] += 1;
    else
      projections[N - 1] += 1; // This should not happen, but trap it in
  }                            // case of rounding errors.
 
  // get the |FFT|
  gsl_fft_real_radix2_transform(projections, 1, N);
  for (size_t i = 1; i < N / 2; i++) {
    magnitude_fft[i] = sqrt(projections[i] * projections[i] + projections[N - i] * projections[N - i]);
  }
 
  magnitude_fft[0] = fabs(projections[0]);
 
  size_t dc_end = 5; // we may need a better estimate of this
  double max_mag_fft = 0.0;
  for (size_t i = dc_end; i < N / 2; i++)
    if (magnitude_fft[i] > max_mag_fft)
      max_mag_fft = magnitude_fft[i];
 
  return max_mag_fft;
}
 
double IndexingUtils::GetFirstMaxIndex(const double magnitude_fft[], size_t N, double threshold) {
  // find first local min below threshold
  size_t i = 2;
  bool found_min = false;
  double val;
  while (i < N - 1 && !found_min) {
    val = magnitude_fft[i];
    if (val < threshold && val <= magnitude_fft[i - 1] && val <= magnitude_fft[i + 1])
      found_min = true;
    i++;
  }
 
  if (!found_min)
    return -1;
  // find next local max above threshold
  bool found_max = false;
  while (i < N - 1 && !found_max) {
    val = magnitude_fft[i];
    if (val >= threshold && val >= magnitude_fft[i - 1] && val >= magnitude_fft[i + 1])
      found_max = true;
    else
      i++;
  }
 
  if (found_max) {
    double sum = 0;
    double w_sum = 0;
    for (size_t j = i - 2; j < std::min(N, i + 3); j++) {
      sum += static_cast<double>(j) * magnitude_fft[j];
      w_sum += magnitude_fft[j];
    }
    return sum / w_sum;
  } else
    return -1;
}
 
bool IndexingUtils::FormUB_From_abc_Vectors(DblMatrix &UB, const std::vector<V3D> &directions, size_t a_index,
                                            double min_d, double max_d) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Find_UB(): UB matrix NULL or not 3X3");
  }
 
  size_t index = a_index;
  V3D a_dir = directions[index];
  index++;
  // the possible range of d-values
  // implies a bound on the minimum
  // angle between a,b, c vectors.
  double min_deg = (RAD_TO_DEG)*atan(2.0 * std::min(0.2, min_d / max_d));
 
  double epsilon = 5; //  tolerance on right angle (degrees)
  V3D b_dir;
  bool b_found = false;
  while (!b_found && index < directions.size()) {
    V3D vec = directions[index];
    double gamma = a_dir.angle(vec) * RAD_TO_DEG;
 
    if (gamma >= min_deg && (180. - gamma) >= min_deg) {
      b_dir = vec;
      if (gamma > 90 + epsilon) // try for Nigli cell with angles <= 90
        b_dir *= -1.0;
      b_found = true;
      index++;
    } else
      index++;
  }
 
  if (!b_found)
    return false;
 
  V3D c_dir;
  bool c_found = false;
 
  const V3D perp = normalize(a_dir.cross_prod(b_dir));
  double perp_ang;
  double alpha;
  double beta;
  while (!c_found && index < directions.size()) {
    V3D vec = directions[index];
    int factor = 1;
    while (!c_found && factor >= -1) // try c in + or - direction
    {
      c_dir = vec;
      c_dir *= factor;
      perp_ang = perp.angle(c_dir) * RAD_TO_DEG;
      alpha = b_dir.angle(c_dir) * RAD_TO_DEG;
      beta = a_dir.angle(c_dir) * RAD_TO_DEG;
      // keep a,b,c right handed by choosing
      // c in general directiion of a X b
      if (perp_ang < 90. - epsilon && alpha >= min_deg && (180. - alpha) >= min_deg && beta >= min_deg &&
          (180. - beta) >= min_deg) {
        c_found = true;
      }
      factor -= 2;
    }
    if (!c_found)
      index++;
  }
 
  if (!c_found) {
    return false;
  }
  // now build the UB matrix from a,b,c
  if (!OrientedLattice::GetUB(UB, a_dir, b_dir, c_dir)) {
    throw std::runtime_error("UB could not be formed, invert matrix failed");
  }
 
  return true;
}
 
bool IndexingUtils::FormUB_From_abc_Vectors(DblMatrix &UB, const std::vector<V3D> &directions,
                                            const std::vector<V3D> &q_vectors, double req_tolerance, double min_vol) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Find_UB(): UB matrix NULL or not 3X3");
  }
 
  int max_indexed = 0;
  V3D a_dir(0, 0, 0);
  V3D b_dir(0, 0, 0);
  V3D c_dir(0, 0, 0);
  V3D a_temp;
  V3D b_temp;
  V3D c_temp;
  V3D acrossb;
  double vol;
 
  for (size_t i = 0; i < directions.size() - 2; i++) {
    a_temp = directions[i];
    for (size_t j = i + 1; j < directions.size() - 1; j++) {
      b_temp = directions[j];
      acrossb = a_temp.cross_prod(b_temp);
      for (size_t k = j + 1; k < directions.size(); k++) {
        c_temp = directions[k];
        vol = fabs(acrossb.scalar_prod(c_temp));
        if (vol > min_vol) {
          const auto num_indexed = NumberIndexed_3D(a_temp, b_temp, c_temp, q_vectors, req_tolerance);
 
          // Requiring 20% more indexed with longer edge lengths, favors
          // the smaller unit cells.
          if (num_indexed > 1.20 * max_indexed) {
            max_indexed = num_indexed;
            a_dir = a_temp;
            b_dir = b_temp;
            c_dir = c_temp;
          }
        }
      }
    }
  }
 
  if (max_indexed <= 0) {
    return false;
  }
  // force a,b,c to be right handed
  acrossb = a_dir.cross_prod(b_dir);
  if (acrossb.scalar_prod(c_dir) < 0) {
    c_dir = c_dir * (-1.0);
  }
  // now build the UB matrix from a,b,c
  if (!OrientedLattice::GetUB(UB, a_dir, b_dir, c_dir)) {
    throw std::runtime_error("UB could not be formed, invert matrix failed");
  }
 
  return true;
}
 
V3D IndexingUtils::makeCDir(const V3D &a_dir, const V3D &b_dir, const double c, const double cosAlpha,
                            const double cosBeta, const double cosGamma, const double sinGamma) {
 
  double c1 = c * cosBeta;
  double c2 = c * (cosAlpha - cosGamma * cosBeta) / sinGamma;
  double V =
      sqrt(1 - cosAlpha * cosAlpha - cosBeta * cosBeta - cosGamma * cosGamma + 2 * cosAlpha * cosBeta * cosGamma);
  double c3 = c * V / sinGamma;
 
  auto basis_1 = Mantid::Kernel::toVector3d(a_dir).normalized();
  auto basis_3 = Mantid::Kernel::toVector3d(a_dir).cross(Mantid::Kernel::toVector3d(b_dir)).normalized();
  auto basis_2 = basis_3.cross(basis_1).normalized();
 
  return Mantid::Kernel::toV3D(basis_1 * c1 + basis_2 * c2 + basis_3 * c3);
}
 
void IndexingUtils::DiscardDuplicates(std::vector<V3D> &new_list, std::vector<V3D> &directions,
                                      const std::vector<V3D> &q_vectors, double required_tolerance, double len_tol,
                                      double ang_tol) {
  new_list.clear();
  std::vector<V3D> temp;
 
  V3D current_dir;
  V3D next_dir;
  V3D zero_vec(0, 0, 0);
 
  double next_length;
  double length_diff;
  double angle;
  size_t dir_num = 0;
  size_t check_index;
  bool new_dir;
 
  while (dir_num < directions.size()) // put sequence of similar vectors
  {                                   // in list temp
    current_dir = directions[dir_num];
    double current_length = current_dir.norm();
    dir_num++;
 
    if (current_length > 0) // skip any zero vectors
    {
      temp.clear();
      temp.emplace_back(current_dir);
      check_index = dir_num;
      new_dir = false;
      while (check_index < directions.size() && !new_dir) {
        next_dir = directions[check_index];
        next_length = next_dir.norm();
        if (next_length > 0) {
          length_diff = fabs(next_dir.norm() - current_length);
          if ((length_diff / current_length) < len_tol) // continue scan
          {
            angle = current_dir.angle(next_dir) * RAD_TO_DEG;
            if ((std::isnan)(angle))
              angle = 0;
            if ((angle < ang_tol) || (angle > (180.0 - ang_tol))) {
              temp.emplace_back(next_dir);
              directions[check_index] = zero_vec; // mark off this direction
            }                                     // since it was duplicate
 
            check_index++; // keep checking all vectors with
          }                // essentially the same length
          else
            new_dir = true; // we only know we have a new
                            // direction if the length is
                            // different, since list is
        }                   // sorted by length !
        else
          check_index++; // just move on
      }
      // now scan through temp list to
      int max_indexed = 0; // find the one that indexes most
 
      long max_i = -1;
      for (size_t i = 0; i < temp.size(); i++) {
        int num_indexed = NumberIndexed_1D(temp[i], q_vectors, required_tolerance);
        if (num_indexed > max_indexed) {
          max_indexed = num_indexed;
          max_i = static_cast<long>(i);
        }
      }
 
      if (max_indexed > 0) // don't bother to add any direction
      {                    // that doesn't index anything
        new_list.emplace_back(temp[max_i]);
      }
    }
  }
 
  directions.clear();
}
 
void IndexingUtils::RoundHKL(Mantid::Kernel::V3D &hkl) {
  for (size_t i = 0; i < 3; i++) {
    hkl[i] = std::round(hkl[i]);
  }
}
 
void IndexingUtils::RoundHKLs(std::vector<V3D> &hkl_list) {
  for (auto &entry : hkl_list) {
    RoundHKL(entry);
  }
}
 
inline bool withinTol(const double value, const double tolerance) {
  double myVal = fabs(value);
  if (myVal - floor(myVal) < tolerance)
    return true;
  if (floor(myVal + 1.) - myVal < tolerance)
    return true;
  return false;
}
 
bool IndexingUtils::ValidIndex(const V3D &hkl, double tolerance) {
  if ((hkl[0] == 0.) && (hkl[1] == 0.) && (hkl[2] == 0.))
    return false;
 
  return (withinTol(hkl[0], tolerance) && withinTol(hkl[1], tolerance) && withinTol(hkl[2], tolerance));
}
 
int IndexingUtils::NumberOfValidIndexes(const std::vector<V3D> &hkls, double tolerance, double &average_error) {
 
  double h_error;
  double k_error;
  double l_error;
  double total_error = 0;
  int count = 0;
  for (const auto &hkl : hkls) {
    if (ValidIndex(hkl, tolerance)) {
      count++;
      h_error = fabs(round(hkl[0]) - hkl[0]);
      k_error = fabs(round(hkl[1]) - hkl[1]);
      l_error = fabs(round(hkl[2]) - hkl[2]);
      total_error += h_error + k_error + l_error;
    }
  }
 
  if (count > 0)
    average_error = total_error / (3.0 * static_cast<double>(count));
  else
    average_error = 0.0;
 
  return count;
}
 
double IndexingUtils::IndexingError(const DblMatrix &UB, const std::vector<V3D> &hkls,
                                    const std::vector<V3D> &q_vectors) {
  DblMatrix UB_inverse(UB);
  if (CheckUB(UB)) {
    UB_inverse.Invert();
  } else {
    throw std::runtime_error("The UB in IndexingError() is not valid");
  }
  if (hkls.size() != q_vectors.size()) {
    throw std::runtime_error("Different size hkl and q_vectors in IndexingError()");
  }
 
  double total_error = 0;
  V3D hkl;
  for (size_t i = 0; i < hkls.size(); i++) {
    hkl = UB_inverse * q_vectors[i] / (2.0 * M_PI);
 
    double h_error = fabs(hkl[0] - round(hkl[0]));
    double k_error = fabs(hkl[1] - round(hkl[1]));
    double l_error = fabs(hkl[2] - round(hkl[2]));
    total_error += h_error + k_error + l_error;
  }
 
  if (!hkls.empty())
    return total_error / (3.0 * static_cast<double>(hkls.size()));
  else
    return 0;
}
 
bool IndexingUtils::CheckUB(const DblMatrix &UB) {
  if (UB.numRows() != 3 || UB.numCols() != 3)
    return false;
 
  for (size_t row = 0; row < 3; row++)
    for (size_t col = 0; col < 3; col++) {
      if (!std::isfinite(UB[row][col])) {
        return false;
      }
    }
 
  double det = UB.determinant();
 
  double abs_det = fabs(det);
 
  return !(abs_det > 10 || abs_det < 1e-12);
}
 
int IndexingUtils::NumberIndexed(const DblMatrix &UB, const std::vector<V3D> &q_vectors, double tolerance) {
  int count = 0;
 
  DblMatrix UB_inverse(UB);
  if (CheckUB(UB)) {
    UB_inverse.Invert();
  } else {
    throw std::runtime_error("The UB in NumberIndexed() is not valid");
  }
 
  V3D hkl;
  for (const auto &q_vector : q_vectors) {
    hkl = UB_inverse * q_vector / (2.0 * M_PI);
    if (ValidIndex(hkl, tolerance)) {
      count++;
    }
  }
 
  return count;
}
 
int IndexingUtils::NumberIndexed_1D(const V3D &direction, const std::vector<V3D> &q_vectors, double tolerance) {
  if (direction.norm() == 0)
    return 0;
 
  int count = 0;
 
  for (const auto &q_vector : q_vectors) {
    double proj_value = direction.scalar_prod(q_vector) / (2.0 * M_PI);
    double error = fabs(proj_value - std::round(proj_value));
    if (error <= tolerance) {
      count++;
    }
  }
 
  return count;
}
 
int IndexingUtils::NumberIndexed_3D(const V3D &a_dir, const V3D &b_dir, const V3D &c_dir,
                                    const std::vector<V3D> &q_vectors, double tolerance) {
  if (a_dir.norm() == 0 || b_dir.norm() == 0 || c_dir.norm() == 0)
    return 0;
 
  V3D hkl_vec;
  int count = 0;
 
  for (const auto &q_vector : q_vectors) {
    hkl_vec[0] = a_dir.scalar_prod(q_vector) / (2.0 * M_PI);
    hkl_vec[1] = b_dir.scalar_prod(q_vector) / (2.0 * M_PI);
    hkl_vec[2] = c_dir.scalar_prod(q_vector) / (2.0 * M_PI);
    if (ValidIndex(hkl_vec, tolerance)) {
      count++;
    }
  }
 
  return count;
}
 
int IndexingUtils::CalculateMillerIndices(const DblMatrix &UB, const std::vector<V3D> &q_vectors, double tolerance,
                                          std::vector<V3D> &miller_indices, double &ave_error) {
  DblMatrix UB_inverse(UB);
 
  if (CheckUB(UB)) {
    UB_inverse.Invert();
  } else {
    throw std::runtime_error("The UB in CalculateMillerIndices() is not valid");
  }
 
  miller_indices.clear();
  miller_indices.reserve(q_vectors.size());
 
  int count = 0;
  ave_error = 0.0;
  for (const auto &q_vector : q_vectors) {
    V3D hkl;
    if (CalculateMillerIndices(UB_inverse, q_vector, tolerance, hkl)) {
      count++;
      ave_error += hkl.hklError();
    }
    miller_indices.emplace_back(hkl);
  }
 
  if (count > 0) {
    ave_error /= (3.0 * count);
  }
 
  return count;
}
 
bool IndexingUtils::CalculateMillerIndices(const DblMatrix &inverseUB, const V3D &q_vector, double tolerance,
                                           V3D &miller_indices) {
  miller_indices = CalculateMillerIndices(inverseUB, q_vector);
  if (ValidIndex(miller_indices, tolerance)) {
    return true;
  } else {
    miller_indices = V3D(0, 0, 0);
    return false;
  }
}
 
V3D IndexingUtils::CalculateMillerIndices(const DblMatrix &inverseUB, const V3D &q_vector) {
  return inverseUB * q_vector / (2.0 * M_PI);
}
 
int IndexingUtils::GetIndexedPeaks_1D(const V3D &direction, const std::vector<V3D> &q_vectors,
                                      double required_tolerance, std::vector<int> &index_vals,
                                      std::vector<V3D> &indexed_qs, double &fit_error) {
  int num_indexed = 0;
  index_vals.clear();
  indexed_qs.clear();
  index_vals.reserve(q_vectors.size());
  indexed_qs.reserve(q_vectors.size());
 
  fit_error = 0;
 
  if (direction.norm() == 0) // special case, zero vector will NOT index
    return 0;                // any peaks, even though dot product
                             // with Q vectors is always an integer!
 
  for (const auto &q_vector : q_vectors) {
    double proj_value = direction.scalar_prod(q_vector) / (2.0 * M_PI);
    double nearest_int = std::round(proj_value);
    double error = fabs(proj_value - nearest_int);
    if (error < required_tolerance) {
      fit_error += error * error;
      indexed_qs.emplace_back(q_vector);
      index_vals.emplace_back(boost::numeric_cast<int>(nearest_int));
      num_indexed++;
    }
  }
 
  return num_indexed;
}
 
int IndexingUtils::GetIndexedPeaks_3D(const V3D &direction_1, const V3D &direction_2, const V3D &direction_3,
                                      const std::vector<V3D> &q_vectors, double required_tolerance,
                                      std::vector<V3D> &miller_indices, std::vector<V3D> &indexed_qs,
                                      double &fit_error) {
  V3D hkl;
  int num_indexed = 0;
 
  miller_indices.clear();
  miller_indices.reserve(q_vectors.size());
 
  indexed_qs.clear();
  indexed_qs.reserve(q_vectors.size());
 
  fit_error = 0;
 
  for (const auto &q_vector : q_vectors) {
    double projected_h = direction_1.scalar_prod(q_vector) / (2.0 * M_PI);
    double projected_k = direction_2.scalar_prod(q_vector) / (2.0 * M_PI);
    double projected_l = direction_3.scalar_prod(q_vector) / (2.0 * M_PI);
 
    hkl(projected_h, projected_k, projected_l);
 
    if (ValidIndex(hkl, required_tolerance)) {
      double h_int = std::round(projected_h);
      double k_int = std::round(projected_k);
      double l_int = std::round(projected_l);
 
      double h_error = fabs(projected_h - h_int);
      double k_error = fabs(projected_k - k_int);
      double l_error = fabs(projected_l - l_int);
 
      fit_error += h_error * h_error + k_error * k_error + l_error * l_error;
 
      indexed_qs.emplace_back(q_vector);
 
      V3D miller_ind(h_int, k_int, l_int);
      miller_indices.emplace_back(miller_ind);
 
      num_indexed++;
    }
  }
 
  return num_indexed;
}
 
int IndexingUtils::GetIndexedPeaks(const DblMatrix &UB, const std::vector<V3D> &q_vectors, double required_tolerance,
                                   std::vector<V3D> &miller_indices, std::vector<V3D> &indexed_qs, double &fit_error) {
  double error;
  int num_indexed = 0;
  V3D hkl;
 
  miller_indices.clear();
  miller_indices.reserve(q_vectors.size());
 
  indexed_qs.clear();
  indexed_qs.reserve(q_vectors.size());
 
  fit_error = 0;
 
  DblMatrix UB_inverse(UB);
  if (CheckUB(UB)) {
    UB_inverse.Invert();
  } else {
    throw std::runtime_error("The UB in GetIndexedPeaks() is not valid");
  }
 
  for (const auto &q_vector : q_vectors) {
    hkl = UB_inverse * q_vector / (2.0 * M_PI);
 
    if (ValidIndex(hkl, required_tolerance)) {
      for (int i = 0; i < 3; i++) {
        error = hkl[i] - std::round(hkl[i]);
        fit_error += error * error;
      }
 
      indexed_qs.emplace_back(q_vector);
 
      V3D miller_ind(round(hkl[0]), round(hkl[1]), round(hkl[2]));
      miller_indices.emplace_back(miller_ind);
 
      num_indexed++;
    }
  }
 
  return num_indexed;
}
 
std::vector<V3D> IndexingUtils::MakeHemisphereDirections(int n_steps) {
  if (n_steps <= 0) {
    throw std::invalid_argument("MakeHemisphereDirections(): n_steps must be greater than 0");
  }
 
  std::vector<V3D> direction_list;
 
  double angle_step = M_PI / (2 * n_steps);
 
  for (int iPhi = 0; iPhi < n_steps + 1; ++iPhi) {
    double phi = static_cast<double>(iPhi) * angle_step;
    double r = sin(phi);
 
    int n_theta = boost::math::iround(2. * M_PI * r / angle_step);
 
    double theta_step;
    if (n_theta == 0) {            // n = ( 0, 1, 0 ).  Just
      theta_step = 2. * M_PI + 1.; // use one vector at the pole
      n_theta = 1;
    } else {
      theta_step = 2. * M_PI / static_cast<double>(n_theta);
    }
 
    // use half the equator to avoid vectors that are the negatives of other
    // vectors in the list.
    if (fabs(phi - M_PI / 2.) < angle_step / 2.) {
      n_theta /= 2;
    }
 
    for (int jTheta = 0; jTheta < n_theta; ++jTheta) {
      double theta = static_cast<double>(jTheta) * theta_step;
      direction_list.emplace_back(r * cos(theta), cos(phi), r * sin(theta));
    }
  }
  return direction_list;
}
 
std::vector<V3D> IndexingUtils::MakeCircleDirections(int n_steps, const Mantid::Kernel::V3D &axis,
                                                     double angle_degrees) {
  if (n_steps <= 0) {
    throw std::invalid_argument("MakeCircleDirections(): n_steps must be greater than 0");
  }
  // first get a vector perpendicular to axis
  double max_component = fabs(axis[0]);
  double min_component = max_component;
  size_t min_index = 0;
  for (size_t i = 1; i < 3; i++) {
    if (fabs(axis[i]) < min_component) {
      min_component = fabs(axis[i]);
      min_index = i;
    }
    if (fabs(axis[i]) > max_component) {
      max_component = fabs(axis[i]);
    }
  }
 
  if (max_component == 0) {
    throw std::invalid_argument("MakeCircleDirections(): Axis vector must be non-zero!");
  }
 
  V3D second_vec(0, 0, 0);
  second_vec[min_index] = 1;
 
  const V3D perp_vec = normalize(second_vec.cross_prod(axis));
 
  // next get a vector that is the specified
  // number of degrees away from the axis
  Quat rotation_1(angle_degrees, perp_vec);
  V3D vector_at_angle(axis);
  rotation_1.rotate(vector_at_angle);
  vector_at_angle.normalize();
 
  // finally, form the circle of directions
  // consisting of vectors that are at the
  // specified angle from the original axis
  double angle_step = 360.0 / n_steps;
  Quat rotation_2(0, axis);
  std::vector<V3D> directions;
  for (int i = 0; i < n_steps; i++) {
    V3D vec(vector_at_angle);
    rotation_2.setAngleAxis(i * angle_step, axis);
    rotation_2.rotate(vec);
    directions.emplace_back(vec);
  }
 
  return directions;
}
 
int IndexingUtils::SelectDirection(V3D &best_direction, const std::vector<V3D> &q_vectors,
                                   const std::vector<V3D> &direction_list, double plane_spacing,
                                   double required_tolerance) {
  if (q_vectors.empty()) {
    throw std::invalid_argument("SelectDirection(): No Q vectors specified");
  }
 
  if (direction_list.empty()) {
    throw std::invalid_argument("SelectDirection(): List of possible directions has zero length");
  }
 
  double error;
  double min_sum_sq_error = 1.0e100;
 
  for (auto direction : direction_list) {
    double sum_sq_error = 0;
    direction /= plane_spacing;
    for (const auto &q_vector : q_vectors) {
      double dot_product = direction.scalar_prod(q_vector) / (2.0 * M_PI);
      error = std::abs(dot_product - std::round(dot_product));
      sum_sq_error += error * error;
    }
 
    if (sum_sq_error < min_sum_sq_error + DBL_EPSILON) {
      min_sum_sq_error = sum_sq_error;
      best_direction = direction;
    }
  }
 
  int num_indexed = 0;
  for (const auto &q_vector : q_vectors) {
    double proj_value = best_direction.scalar_prod(q_vector) / (2.0 * M_PI);
    error = std::abs(proj_value - std::round(proj_value));
    if (error < required_tolerance)
      num_indexed++;
  }
 
  best_direction.normalize();
 
  return num_indexed;
}
 
bool IndexingUtils::GetLatticeParameters(const DblMatrix &UB, std::vector<double> &lattice_par) {
  OrientedLattice o_lattice;
  o_lattice.setUB(UB);
 
  lattice_par.clear();
  lattice_par.emplace_back(o_lattice.a());
  lattice_par.emplace_back(o_lattice.b());
  lattice_par.emplace_back(o_lattice.c());
 
  lattice_par.emplace_back(o_lattice.alpha());
  lattice_par.emplace_back(o_lattice.beta());
  lattice_par.emplace_back(o_lattice.gamma());
 
  lattice_par.emplace_back(o_lattice.volume()); // keep volume > 0 even if
                                                // cell is left handed
  return true;
}
 
int IndexingUtils::GetModulationVectors(const DblMatrix &UB, const DblMatrix &ModUB, V3D &ModVec1, V3D &ModVec2,
                                        V3D &ModVec3) {
  OrientedLattice o_lattice;
  o_lattice.setUB(UB);
  o_lattice.setModUB(ModUB);
 
  ModVec1 = o_lattice.getModVec(0);
  ModVec2 = o_lattice.getModVec(1);
  ModVec3 = o_lattice.getModVec(2);
 
  int ModDim = 0;
  if (o_lattice.getdh(0) != 0.0 || o_lattice.getdk(0) != 0.0 || o_lattice.getdl(0) != 0.0)
    ModDim = 1;
  if (o_lattice.getdh(1) != 0.0 || o_lattice.getdk(1) != 0.0 || o_lattice.getdl(1) != 0.0)
    ModDim = 2;
  if (o_lattice.getdh(2) != 0.0 || o_lattice.getdk(2) != 0.0 || o_lattice.getdl(2) != 0.0)
    ModDim = 3;
 
  return ModDim;
}
 
bool IndexingUtils::GetModulationVector(const DblMatrix &UB, const DblMatrix &ModUB, V3D &ModVec, const int j) {
  OrientedLattice o_lattice;
  o_lattice.setUB(UB);
  o_lattice.setModUB(ModUB);
 
  ModVec = o_lattice.getModVec(j);
 
  if (ModVec[0] != 0.0 || ModVec[1] != 0.0 || ModVec[2] != 0.0)
    return true;
  else
    return false;
}
 
std::string IndexingUtils::GetLatticeParameterString(const DblMatrix &UB) {
  std::vector<double> lat_par;
  IndexingUtils::GetLatticeParameters(UB, lat_par);
 
  char buffer[100];
 
  sprintf(buffer, std::string(" %8.4f %8.4f %8.4f  %8.3f %8.3f %8.3f  %9.2f").c_str(), lat_par[0], lat_par[1],
          lat_par[2], lat_par[3], lat_par[4], lat_par[5], lat_par[6]);
 
  std::string result(buffer);
  return result;
}