/**
 * Creates a Cube
 * @param Point1 :: first point of the cube
 * @param Point2 :: second point of the cube
 * @param Point3 :: thrid point of the cube
 * @param Point4 :: fourth point of the cube
 */
    void GluGeometryRenderer::CreateCube(const V3D& Point1,const V3D& Point2,const V3D& Point3,const V3D& Point4)
    {
      V3D vec0=Point1;
      V3D vec1=Point2-Point1;
      V3D vec2=Point3-Point1;
      V3D vec3=Point4-Point1;
      V3D vertex[8];
      vertex[0]=vec0;
      vertex[1]=vec0+vec3;
      vertex[2]=vec0+vec3+vec1;
      vertex[3]=vec0+vec1;
      vertex[4]=vec0+vec2;
      vertex[5]=vec0+vec2+vec3;
      vertex[6]=vec0+vec2+vec3+vec1;
      vertex[7]=vec0+vec1+vec2;
      //int faceindex[6][4]={{0,1,2,3},{0,3,7,4},{3,2,6,7},{2,1,5,6},{0,4,5,1},{4,7,6,5}};
      //int faceindex[6][4]={{0,3,2,1},{0,4,7,3},{3,7,6,2},{2,6,5,1},{0,1,5,4},{4,5,6,7}};
      int faceindex[6][4]={
	{0,1,2,3}, //top
	{0,3,7,4}, //left
	{3,2,6,7}, //back
	{2,1,5,6}, //right
	{0,4,5,1}, //front
	{4,7,6,5}, //bottom
      };
      V3D normal;
      //first face
      glBegin(GL_QUADS);
      for(int i=0;i<6;i++){
	normal=(vertex[faceindex[i][0]]-vertex[faceindex[i][1]]).cross_prod((vertex[faceindex[i][0]]-vertex[faceindex[i][2]]));
	normal.normalize();
	glNormal3d(normal[0],normal[1],normal[2]);
	for(int j=0;j<4;j++)
	{
	  int ij = faceindex[i][j];
	  if (ij == 0) glTexCoord2i(0, 0);
	  if (ij == 1) glTexCoord2i(1, 0);
	  if (ij == 2) glTexCoord2i(1, 1);
	  if (ij == 3) glTexCoord2i(0, 1);
	  if (ij == 4) glTexCoord2i(0, 0);
	  if (ij == 5) glTexCoord2i(1, 0);
	  if (ij == 6) glTexCoord2i(1, 1);
	  if (ij == 7) glTexCoord2i(0, 1);
	  glVertex3d(vertex[ij][0],vertex[ij][1],vertex[ij][2]);
	}
      }
      glEnd();
    }

double RadiusSum::getMaxDistance(const V3D &centre,
                                 const std::vector<double> &boundary_limits) {

  std::array<double, 2> Xs = {{boundary_limits[0], boundary_limits[1]}};
  std::array<double, 2> Ys = {{boundary_limits[2], boundary_limits[3]}};
  std::array<double, 2> Zs = {{0., 0.}};

  if (boundary_limits.size() == 6) {
    Zs[0] = boundary_limits[4];
    Zs[1] = boundary_limits[5];
  }

  double max_distance = 0;
  for (auto &x : Xs)
    for (auto &y : Ys)
      for (auto &z : Zs) {
        // define all the possible combinations for the limits

        double curr_distance = centre.distance(V3D(x, y, z));

        if (curr_distance > max_distance)
          max_distance = curr_distance; // keep the maximum distance.
      }
  return max_distance;
}

 void PeaksOnSurface::checkTouchPoint(const V3D& touchPoint,const  V3D& normal,const  V3D& faceVertex) const
 {
    if( normal.scalar_prod(touchPoint - faceVertex) != 0)
    {
      throw std::runtime_error("Debugging. Calculation is wrong. touch point should always be on the plane!"); // Remove this line later. Check that geometry is setup properly.
    }
 }

  MLPTesting(): c_(&camera_){
    static_assert(imgSize_*dx_*dy_<255,"imgSize to large for gradients");
    pixel_ = cv::Point2f(patchSize_/2+1,patchSize_/2+1);
    bearing_ = V3D(patchSize_/2+1,patchSize_/2+1,1);
    bearing_.normalize();
    c_.set_c(pixel_);
    warp_c_ << 0.1, 0.5, 0.7, -0.2;
    c_.set_warp_c(warp_c_);
    warp_nor_ = c_.get_warp_nor();

    stat_.localQualityRange_ = 3;
    stat_.localVisibilityRange_ = 4;
    stat_.minGlobalQualityRange_ = 5;

    img1_ = cv::Mat::zeros(imgSize_,imgSize_,CV_8UC1);
    uint8_t* img_ptr = (uint8_t*) img1_.data;
    for(int i=0;i<imgSize_;i++){
      for(int j=0;j<imgSize_;j++, ++img_ptr){
        *img_ptr = i*dy_+j*dx_;
      }
    }
    img2_ = cv::Mat::zeros(imgSize_,imgSize_,CV_8UC1);
    img_ptr = (uint8_t*) img2_.data;
    for(int i=0;i<imgSize_;i++){
      for(int j=0;j<imgSize_;j++, ++img_ptr){
        if(j<imgSize_/2 & i<imgSize_/2){
          *img_ptr = 0;
        } else {
          *img_ptr = 255;
        }
      }
    }
    pyr1_.computeFromImage(img1_);
    pyr2_.computeFromImage(img2_);
  }

/**
    Check if a,b,c cell has angles satifying Niggli condition within epsilon.
    Specifically, check if all angles are strictly less than 90 degrees,
    or all angles are greater than or equal to 90 degrees.  The inequality
    requirements are relaxed by an amount specified by the paramter epsilon
    to accommodate some experimental and/or rounding error in the calculated
    angles.

    @param a_dir    Vector in the direction of the real cell edge vector 'a'
    @param b_dir    Vector in the direction of the real cell edge vector 'b'
    @param c_dir    Vector in the direction of the real cell edge vector 'c'
    @param epsilon  Tolerance (in degrees) around 90 degrees.  For example
                    an angle theta will be considered strictly less than 90
                    degrees, if it is less than 90+epsilon.
    @return true if all angles are less than 90 degrees, or if all angles
            are greater than or equal to 90 degrees.
 */
bool NiggliCell::HasNiggliAngles(const V3D &a_dir, const V3D &b_dir,
                                 const V3D &c_dir, double epsilon) {
  double alpha = b_dir.angle(c_dir) * RAD_TO_DEG;
  double beta = c_dir.angle(a_dir) * RAD_TO_DEG;
  double gamma = a_dir.angle(b_dir) * RAD_TO_DEG;

  if (alpha < 90 + epsilon && beta < 90 + epsilon && gamma < 90 + epsilon) {
    return true;
  }

  if (alpha >= 90 - epsilon && beta >= 90 - epsilon && gamma >= 90 - epsilon) {
    return true;
  }

  return false;
}

/**
 * Provide the collimation length which is associated with the instrument
 * @param workspace: the input workspace
 * @returns the collimation length
 */
double SANSCollimationLengthEstimator::provideCollimationLength(
    Mantid::API::MatrixWorkspace_sptr workspace) {
  // If the instrument does not have a correction specified then set the length
  // to 4
  const double defaultLColim = 4.0;
  auto collimationLengthID = "collimation-length-correction";

  if (!workspace->getInstrument()->hasParameter(collimationLengthID)) {
    g_log.error("Error in SANSCollimtionLengthEstimator: The instrument "
                "parameter file does not contain a collimation length "
                "correction,"
                "a default of 4 is provided. Please update the instrument "
                "parameter file.");
    return defaultLColim;
  }

  // Get the L1 length
  const V3D samplePos = workspace->getInstrument()->getSample()->getPos();
  const V3D sourcePos = workspace->getInstrument()->getSource()->getPos();
  const V3D SSD = samplePos - sourcePos;
  const double L1 = SSD.norm();

  auto collimationLengthCorrection =
      workspace->getInstrument()->getNumberParameter(collimationLengthID);

  if (workspace->getInstrument()->hasParameter(
          "special-default-collimation-length-method")) {
    auto specialCollimationMethod =
        workspace->getInstrument()->getStringParameter(
            "special-default-collimation-length-method");
    if (specialCollimationMethod[0] == "guide") {
      try {
        return getCollimationLengthWithGuides(workspace, L1,
                                              collimationLengthCorrection[0]);
      } catch (std::invalid_argument &ex) {
        g_log.notice() << ex.what();
        g_log.notice()
            << "SANSCollimationLengthEstimator: Not using any guides";
        return L1 - collimationLengthCorrection[0];
      }
    } else {
      throw std::invalid_argument("Error in SANSCollimationLengthEstimator: "
                                  "Unknown special collimation method.");
    }
  }
  return L1 - collimationLengthCorrection[0];
}

 void evalTransform(mtOutput& output, const mtInput& input) const{
   input.updateMultiCameraExtrinsics(mpMultiCamera_);
   mpMultiCamera_->transformFeature(outputCamID_,input.CfP(ID_),input.dep(ID_),output.c(),output.d());
   if(input.CfP(ID_).trackWarping_ && input.CfP(ID_).com_warp_nor()){
     const int& camID = input.CfP(ID_).camID_;
     const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation
     const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID)));
     const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec();
     const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD));
     const double d_out = DrDP.norm();
     const LWF::NormalVectorElement nor_out(DrDP);
     const Eigen::Matrix<double,2,3> J_nor_DrDP = nor_out.getM().transpose()/d_out;
     const Eigen::Matrix<double,3,3> J_DrDP_CrCP = MPD(qDC).matrix();
     const Eigen::Matrix<double,3,2> J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM();
     output.c().set_warp_nor(J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor*input.CfP(ID_).get_warp_nor());
   }
 }

/**
  @param  inname       Name of workspace containing peaks
  @param  params       optimized cell parameters
  @param  out          residuals from optimization
*/
void OptimizeLatticeForCellType::optLattice(std::string inname,
                                            std::vector<double> &params,
                                            double *out) {
  PeaksWorkspace_sptr ws = boost::dynamic_pointer_cast<PeaksWorkspace>(
      AnalysisDataService::Instance().retrieve(inname));
  const std::vector<Peak> &peaks = ws->getPeaks();
  size_t n_peaks = ws->getNumberPeaks();
  std::vector<V3D> q_vector;
  std::vector<V3D> hkl_vector;

  for (size_t i = 0; i < params.size(); i++)
    params[i] = std::abs(params[i]);
  for (size_t i = 0; i < n_peaks; i++) {
    q_vector.push_back(peaks[i].getQSampleFrame());
    hkl_vector.push_back(peaks[i].getHKL());
  }

  Mantid::API::IAlgorithm_sptr alg = createChildAlgorithm("CalculateUMatrix");
  alg->setPropertyValue("PeaksWorkspace", inname);
  alg->setProperty("a", params[0]);
  alg->setProperty("b", params[1]);
  alg->setProperty("c", params[2]);
  alg->setProperty("alpha", params[3]);
  alg->setProperty("beta", params[4]);
  alg->setProperty("gamma", params[5]);
  alg->executeAsChildAlg();

  ws = alg->getProperty("PeaksWorkspace");
  OrientedLattice latt = ws->mutableSample().getOrientedLattice();
  DblMatrix UB = latt.getUB();
  DblMatrix A = aMatrix(params);
  DblMatrix Bc = A;
  Bc.Invert();
  DblMatrix U1_B1 = UB * A;
  OrientedLattice o_lattice;
  o_lattice.setUB(U1_B1);
  DblMatrix U1 = o_lattice.getU();
  DblMatrix U1_Bc = U1 * Bc;

  for (size_t i = 0; i < hkl_vector.size(); i++) {
    V3D error = U1_Bc * hkl_vector[i] - q_vector[i] / (2.0 * M_PI);
    out[i] = error.norm2();
  }

  return;
}

/**
 * Utility to place detector in space, according to data file
 */
void LoadILLReflectometry::placeDetector(double distance /* meter */,
                                         double angle /* degree */) {
  const double deg2rad = M_PI / 180.0;
  std::string componentName("uniq_detector");
  V3D pos = m_loader.getComponentPosition(m_localWorkspace, componentName);
  //      double r, theta, phi;
  //      pos.getSpherical(r, theta, phi);

  double angle_rad = angle * deg2rad;
  V3D newpos(distance * sin(angle_rad), pos.Y(), distance * cos(angle_rad));
  m_loader.moveComponent(m_localWorkspace, componentName, newpos);

  // Apply a local rotation to stay perpendicular to the beam
  const V3D axis(0.0, 1.0, 0.0);
  Quat rotation(angle, axis);
  m_loader.rotateComponent(m_localWorkspace, componentName, rotation);
}

/**
* Query whether the given point is inside the bounding box within a tolerance
* defined by Mantid::Geometry::Tolerance.
* @param point :: The point to query
* @returns True if the point is within the bounding box, false otherwise
*/
bool BoundingBox::isPointInside(const V3D &point) const {
  if (!this->isAxisAligned()) {
    throw(Kernel::Exception::NotImplementedError(
        "this function has not been modified properly"));
  }

  if (point.X() <= xMax() + Kernel::Tolerance &&
      point.X() >= xMin() - Kernel::Tolerance &&
      point.Y() <= yMax() + Kernel::Tolerance &&
      point.Y() >= yMin() - Kernel::Tolerance &&
      point.Z() <= zMax() + Kernel::Tolerance &&
      point.Z() >= zMin() - Kernel::Tolerance) {
    return true;
  } else {
    return false;
  }
}

bool PeakBackground::isBackground(Mantid::API::IMDIterator *iterator) const {
  if (!HardThresholdBackground::isBackground(iterator)) {
    const VMD &center = iterator->getCenter();
    V3D temp(center[0], center[1], center[2]); // This assumes dims 1, 2, and 3
                                               // in the workspace correspond to
                                               // positions.

    for (int i = 0; i < m_peaksWS->getNumberPeaks(); ++i) {
      const IPeak &peak = m_peaksWS->getPeak(i);
      V3D coords = m_coordFunction(&peak);
      if (coords.distance(temp) < m_radiusEstimate) {
        return false;
      }
    }
  }
  return true;
}

 /** \brief Initializes the FilterState \ref Base::state_ with the Acceleration-Vector.
  *
  *  @param fMeasInit - Acceleration-Vector which should be used for initializing the attitude of the IMU.
  */
 void initWithAccelerometer(const V3D& fMeasInit){
   V3D unitZ(0,0,1);
   if(fMeasInit.norm()>1e-6){
     state_.qWM().setFromVectors(unitZ,fMeasInit);
   } else {
     state_.qWM().setIdentity();
   }
 }

/**
 * Generate a random position within the final detector in the lab frame
 * @param nominalPos The poisiton of the centre point of the detector
 * @param energy The final energy of the neutron
 * @param scatterPt The position of the scatter event that lead to this
 * detector
 * @param direcBeforeSc Directional vector that lead to scatter point that hit
 * this detector
 * @param scang [Output] The value of the scattering angle for the generated
 * point
 * @param distToExit [Output] The distance covered within the object from
 * scatter to exit
 * @return A new position in the detector
 */
V3D VesuvioCalculateMS::generateDetectorPos(
    const V3D &nominalPos, const double energy, const V3D &scatterPt,
    const V3D &direcBeforeSc, double &scang, double &distToExit) const {
  // Inverse attenuation length (m-1) for vesuvio det.
  const double mu = 7430.0 / sqrt(energy);
  // Probability of detection in path thickness.
  const double ps = 1.0 - exp(-mu * m_detThick);
  V3D detPos;
  scang = 0.0;
  distToExit = 0.0;
  size_t ntries(0);
  do {
    // Beam direction by moving to front of "box"define by detector dimensions
    // and then
    // computing expected distance travelled based on probability
    detPos[m_beamIdx] = (nominalPos[m_beamIdx] - 0.5 * m_detThick) -
                        (log(1.0 - m_randgen->flat() * ps) / mu);
    // perturb away from nominal position
    detPos[m_acrossIdx] =
        nominalPos[m_acrossIdx] + (m_randgen->flat() - 0.5) * m_detWidth;
    detPos[m_upIdx] =
        nominalPos[m_upIdx] + (m_randgen->flat() - 0.5) * m_detHeight;

    // Distance to exit the sample for this order
    V3D scToDet = detPos - scatterPt;
    scToDet.normalize();
    Geometry::Track scatterToDet(scatterPt, scToDet);
    if (m_sampleShape->interceptSurface(scatterToDet) > 0) {
      scang = direcBeforeSc.angle(scToDet);
      const auto &link = scatterToDet.cbegin();
      distToExit = link->distInsideObject;
      break;
    }
    // if point is very close surface then there may be no valid intercept so
    // try again
    ++ntries;
  } while (ntries < MAX_SCATTER_PT_TRIES);
  if (ntries == MAX_SCATTER_PT_TRIES) {
    // Assume it is very close to the surface so that the distance travelled
    // would
    // be a neglible contribution
    distToExit = 0.0;
  }
  return detPos;
}

      /**
       * Transform the box. Permanent  change the box left, right, top, bottom, front and back according to the transform.
       * @param transform : Transform to use.
       */
      void PeakBoundingBox::transformBox(Mantid::Geometry::PeakTransform_sptr transform)
      {
        using Mantid::Kernel::V3D;
        // Front bottom left
        V3D newBottomLeft = transform->transformBack(V3D(m_left(), m_bottom(), m_front()));
        // Back top right
        V3D newTopRight = transform->transformBack(V3D(m_right(), m_top(), m_back()));
        // SlicePoint
        V3D newSlicePoint = transform->transformBack(V3D(0, 0, m_slicePoint()));

        m_left = Left(newBottomLeft.X());
        m_bottom = Bottom(newBottomLeft.Y());
        m_right = Right(newTopRight.X());
        m_top = Top(newTopRight.Y());
        m_front = Front(newBottomLeft.Z());
        m_back = Back(newTopRight.Z());
        m_slicePoint = SlicePoint(newSlicePoint.Z());
      }

/** Calculates the angle between this and another vector.
 *
 *  @param v :: The other vector
 *  @return The angle between the vectors in radians (0 < theta < pi)
 */
double V3D::angle(const V3D &v) const {
  double ratio = this->scalar_prod(v) / (this->norm() * v.norm());

  if (ratio >= 1.0)       // NOTE: Due to rounding errors, if v is
    return 0.0;           //       is nearly the same as "this" or
  else if (ratio <= -1.0) //       as "-this", ratio can be slightly
    return M_PI;          //       more than 1 in absolute value.
                          //       That causes acos() to return NaN.
  return acos(ratio);
}

/// Try to extract a V3D from the given string with different separators.
V3D V3DFromHKLColumnExtractor::getHKLFromString(
    const std::string &hklString) const {
  auto delimiters = boost::is_any_of(" ,[];");

  std::string workingCopy = boost::trim_copy_if(hklString, delimiters);
  std::vector<std::string> indicesStr;
  boost::split(indicesStr, workingCopy, delimiters);

  if (indicesStr.size() != 3) {
    throw std::invalid_argument("Input string cannot be parsed as HKL.");
  }

  V3D hkl;
  hkl.setX(boost::lexical_cast<double>(indicesStr[0]));
  hkl.setY(boost::lexical_cast<double>(indicesStr[1]));
  hkl.setZ(boost::lexical_cast<double>(indicesStr[2]));

  return hkl;
}

/// Fills possibleHKLs with all HKLs from the supplied PeaksWorkspace.
void PredictPeaks::fillPossibleHKLsUsingPeaksWorkspace(
    const PeaksWorkspace_sptr &peaksWorkspace,
    std::vector<V3D> &possibleHKLs) const {
  possibleHKLs.clear();
  possibleHKLs.reserve(peaksWorkspace->getNumberPeaks());

  bool roundHKL = getProperty("RoundHKL");

  for (int i = 0; i < static_cast<int>(peaksWorkspace->getNumberPeaks()); ++i) {
    IPeak &p = peaksWorkspace->getPeak(i);
    // Get HKL from that peak
    V3D hkl = p.getHKL();

    if (roundHKL)
      hkl.round();

    possibleHKLs.push_back(hkl);
  } // for each hkl in the workspace
}

void EstimateResolutionDiffraction::retrieveInstrumentParameters() {
    double centrewavelength = getWavelength();
    g_log.notice() << "Centre wavelength = " << centrewavelength << " Angstrom\n";
    if (centrewavelength > WAVELENGTH_MAX) {
        throw runtime_error("unphysical wavelength used");
    }

    // Calculate centre neutron velocity
    m_centreVelocity = WAVELENGTH_TO_VELOCITY / centrewavelength;
    g_log.notice() << "Centre neutron velocity = " << m_centreVelocity << "\n";

    // Calculate L1 sample to source
    Instrument_const_sptr instrument = m_inputWS->getInstrument();
    V3D samplepos = instrument->getSample()->getPos();
    V3D sourcepos = instrument->getSource()->getPos();
    m_L1 = samplepos.distance(sourcepos);
    g_log.notice() << "L1 = " << m_L1 << "\n";

    return;
}

/**
 * @brief SortHKL::getUniqueReflections
 *
 * This method returns a map that contains a UniqueReflection-
 * object with 0 to n Peaks each. The key of the map is the
 * reflection index all peaks are equivalent to.
 *
 * @param peaks :: Vector of peaks to assign.
 * @param cell :: UnitCell to use for calculation of possible reflections.
 * @return Map of unique reflections.
 */
std::map<V3D, UniqueReflection>
SortHKL::getUniqueReflections(const std::vector<Peak> &peaks,
                              const UnitCell &cell) const {
  ReflectionCondition_sptr centering = getCentering();
  PointGroup_sptr pointGroup = getPointgroup();

  std::pair<double, double> dLimits = getDLimits(peaks, cell);

  std::map<V3D, UniqueReflection> uniqueReflectionInRange =
      getPossibleUniqueReflections(cell, dLimits, pointGroup, centering);

  for (auto const &peak : peaks) {
    V3D hkl = peak.getHKL();
    hkl.round();

    uniqueReflectionInRange.at(pointGroup->getReflectionFamily(hkl))
        .addPeak(peak);
  }

  return uniqueReflectionInRange;
}

void LoadILLSANS::moveDetectorVertical(double shift,
                                       const std::string &componentName) {

  API::IAlgorithm_sptr mover = createChildAlgorithm("MoveInstrumentComponent");
  V3D pos = getComponentPosition(componentName);
  try {
    mover->setProperty<MatrixWorkspace_sptr>("Workspace", m_localWorkspace);
    mover->setProperty("ComponentName", componentName);
    mover->setProperty("X", pos.X());
    mover->setProperty("Y", shift);
    mover->setProperty("Z", pos.Z());
    mover->setProperty("RelativePosition", false);
    mover->executeAsChildAlg();
    g_log.debug() << "Moving component '" << componentName
                  << "' to Y = " << shift << '\n';
  } catch (std::exception &e) {
    g_log.error() << "Cannot move the component '" << componentName
                  << "' to Y = " << shift << '\n';
    g_log.error() << e.what() << '\n';
  }
}