// Created by: Eugeny MALTCHIKOV
// Created on: 2019-04-17
// Copyright (c) 2019 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

#include <QABugs.hxx>

#include <Bnd_Tools.hxx>

#include <BRep_Builder.hxx>

#include <BRepBndLib.hxx>

#include <BVH_Box.hxx>
#include <BVH_DistanceField.hxx>
#include <BVH_Geometry.hxx>
#include <BVH_IndexedBoxSet.hxx>
#include <BVH_LinearBuilder.hxx>
#include <BVH_PairDistance.hxx>
#include <BVH_Traverse.hxx>
#include <BVH_Triangulation.hxx>

#include <DBRep.hxx>
#include <Draw.hxx>

#include <Precision.hxx>

#include <TopExp.hxx>
#include <TopExp_Explorer.hxx>

#include <TopoDS.hxx>
#include <TopoDS_Compound.hxx>
#include <TopoDS_Face.hxx>
#include <TopoDS_Shape.hxx>

#include <TopTools_IndexedMapOfShape.hxx>

//=======================================================================
// function : ShapeSelector
// purpose : Implement the simplest shape's selector
//=======================================================================
class ShapeSelector : public BVH_Traverse<Standard_Real,
                                          3,
                                          BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>,
                                          Standard_Boolean>
{
public:
  //! Constructor
  ShapeSelector() {}

  //! Sets the Box for selection
  void SetBox(const Bnd_Box& theBox) { myBox = Bnd_Tools::Bnd2BVH(theBox); }

  //! Returns the selected shapes
  const NCollection_List<TopoDS_Shape>& Shapes() const { return myShapes; }

public:
  //! Defines the rules for node rejection by bounding box
  virtual Standard_Boolean RejectNode(const BVH_Vec3d&  theCornerMin,
                                      const BVH_Vec3d&  theCornerMax,
                                      Standard_Boolean& theIsInside) const Standard_OVERRIDE
  {
    Standard_Boolean hasOverlap;
    theIsInside = myBox.Contains(theCornerMin, theCornerMax, hasOverlap);
    return !hasOverlap;
  }

  //! Defines the rules for leaf acceptance
  virtual Standard_Boolean AcceptMetric(const Standard_Boolean& theIsInside) const Standard_OVERRIDE
  {
    return theIsInside;
  }

  //! Defines the rules for leaf acceptance
  virtual Standard_Boolean Accept(const Standard_Integer  theIndex,
                                  const Standard_Boolean& theIsInside) Standard_OVERRIDE
  {
    if (theIsInside || !myBox.IsOut(myBVHSet->Box(theIndex)))
    {
      myShapes.Append(myBVHSet->Element(theIndex));
      return Standard_True;
    }
    return Standard_False;
  }

protected:
  BVH_Box<Standard_Real, 3>      myBox;    //!< Selection box
  NCollection_List<TopoDS_Shape> myShapes; //!< Selected shapes
};

//=======================================================================
// function : ShapeSelector
// purpose : Implement the simplest shape's selector
//=======================================================================
class ShapeSelectorVoid : public BVH_Traverse<Standard_Real, 3, void, Standard_Boolean>
{
public:
  //! Constructor
  ShapeSelectorVoid() {}

  //! Sets the Box for selection
  void SetBox(const Bnd_Box& theBox) { myBox = Bnd_Tools::Bnd2BVH(theBox); }

  //! Returns the selected shapes
  const NCollection_List<TopoDS_Shape>& Shapes() const { return myShapes; }

public:
  //! Sets the Box Set
  void SetShapeBoxSet(
    const opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>>& theBoxSet)
  {
    myBoxSet = theBoxSet;
  }

public:
  //! Defines the rules for node rejection by bounding box
  virtual Standard_Boolean RejectNode(const BVH_Vec3d&  theCornerMin,
                                      const BVH_Vec3d&  theCornerMax,
                                      Standard_Boolean& theIsInside) const Standard_OVERRIDE
  {
    Standard_Boolean hasOverlap;
    theIsInside = myBox.Contains(theCornerMin, theCornerMax, hasOverlap);
    return !hasOverlap;
  }

  //! Defines the rules for leaf acceptance
  virtual Standard_Boolean AcceptMetric(const Standard_Boolean& theIsInside) const Standard_OVERRIDE
  {
    return theIsInside;
  }

  //! Defines the rules for leaf acceptance
  virtual Standard_Boolean Accept(const Standard_Integer  theIndex,
                                  const Standard_Boolean& theIsInside) Standard_OVERRIDE
  {
    if (theIsInside || !myBox.IsOut(myBoxSet->Box(theIndex)))
    {
      myShapes.Append(myBoxSet->Element(theIndex));
      return Standard_True;
    }
    return Standard_False;
  }

protected:
  opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>> myBoxSet; //!< ShapeBoxSet
  BVH_Box<Standard_Real, 3>                                       myBox;    //!< Selection box
  NCollection_List<TopoDS_Shape>                                  myShapes; //!< Selected shapes
};

//=======================================================================
// function : QABVH_ShapeSelect
// purpose : Test the work of BVH on the simple example of shapes selection
//=======================================================================
static Standard_Integer QABVH_ShapeSelect(Draw_Interpretor& theDI,
                                          Standard_Integer  theArgc,
                                          const char**      theArgv)
{
  if (theArgc < 4)
  {
    theDI.PrintHelp(theArgv[0]);
    return 1;
  }

  // Get the shape to add its sub-shapes into BVH
  TopoDS_Shape aShape = DBRep::Get(theArgv[2]);
  if (aShape.IsNull())
  {
    std::cout << theArgv[2] << " does not exist" << std::endl;
    return 1;
  }

  // Get the shape to get the Box for selection
  TopoDS_Shape aBShape = DBRep::Get(theArgv[3]);
  if (aBShape.IsNull())
  {
    std::cout << theArgv[3] << " does not exist" << std::endl;
    return 1;
  }

  // Which selector to use
  Standard_Boolean useVoidSelector = Standard_False;
  if (theArgc > 4)
    useVoidSelector = !strcmp(theArgv[4], "-void");

  // Define BVH Builder
  opencascade::handle<BVH_LinearBuilder<Standard_Real, 3>> aLBuilder =
    new BVH_LinearBuilder<Standard_Real, 3>();

  // Create the ShapeSet
  opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>> aShapeBoxSet =
    !useVoidSelector ? new BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>(aLBuilder)
                     : new BVH_IndexedBoxSet<Standard_Real, 3, TopoDS_Shape>(aLBuilder);

  // Add elements into BVH

  // Map the shape
  TopTools_IndexedMapOfShape aMapShapes;
  TopExp::MapShapes(aShape, TopAbs_VERTEX, aMapShapes);
  TopExp::MapShapes(aShape, TopAbs_EDGE, aMapShapes);
  TopExp::MapShapes(aShape, TopAbs_FACE, aMapShapes);

  for (Standard_Integer iS = 1; iS <= aMapShapes.Extent(); ++iS)
  {
    const TopoDS_Shape& aS = aMapShapes(iS);

    Bnd_Box aSBox;
    BRepBndLib::Add(aS, aSBox);

    aShapeBoxSet->Add(aS, Bnd_Tools::Bnd2BVH(aSBox));
  }

  // Build BVH
  aShapeBoxSet->Build();

  TopTools_ListOfShape aSelectedShapes;

  // Prepare a Box for selection
  Bnd_Box aSelectionBox;
  BRepBndLib::Add(aBShape, aSelectionBox);

  // Perform selection
  if (!useVoidSelector)
  {
    ShapeSelector aSelector;
    aSelector.SetBox(aSelectionBox);
    aSelector.SetBVHSet(aShapeBoxSet.get());
    aSelector.Select();
    aSelectedShapes = aSelector.Shapes();
  }
  else
  {
    ShapeSelectorVoid aSelector;
    aSelector.SetBox(aSelectionBox);
    aSelector.SetShapeBoxSet(aShapeBoxSet);
    aSelector.Select(aShapeBoxSet->BVH());
    aSelectedShapes = aSelector.Shapes();
  }

  // Draw the selected shapes
  TopoDS_Compound aResult;
  BRep_Builder().MakeCompound(aResult);

  for (TopTools_ListOfShape::Iterator it(aSelectedShapes); it.More(); it.Next())
    BRep_Builder().Add(aResult, it.Value());

  DBRep::Set(theArgv[1], aResult);
  return 0;
}

//=======================================================================
// function : PairShapeSelector
// purpose : Implement the simplest shape's selector
//=======================================================================
class PairShapesSelector
    : public BVH_PairTraverse<Standard_Real, 3, BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>>
{
public:
  //! Constructor
  PairShapesSelector() {}

  //! Returns the selected pairs of shapes
  const NCollection_List<std::pair<TopoDS_Shape, TopoDS_Shape>>& Pairs() const { return myPairs; }

public:
  //! Defines the rules for node rejection
  virtual Standard_Boolean RejectNode(const BVH_Vec3d& theCornerMin1,
                                      const BVH_Vec3d& theCornerMax1,
                                      const BVH_Vec3d& theCornerMin2,
                                      const BVH_Vec3d& theCornerMax2,
                                      Standard_Real&) const Standard_OVERRIDE
  {
    return BVH_Box<Standard_Real, 3>(theCornerMin1, theCornerMax1)
      .IsOut(BVH_Box<Standard_Real, 3>(theCornerMin2, theCornerMax2));
  }

  //! Defines the rules for leaf acceptance
  virtual Standard_Boolean Accept(const Standard_Integer theIndex1,
                                  const Standard_Integer theIndex2) Standard_OVERRIDE
  {
    BVH_Box<Standard_Real, 3> aBox1 = myBVHSet1->Box(theIndex1);
    BVH_Box<Standard_Real, 3> aBox2 = myBVHSet2->Box(theIndex2);
    if (!aBox1.IsOut(aBox2))
    {
      myPairs.Append(std::pair<TopoDS_Shape, TopoDS_Shape>(myBVHSet1->Element(theIndex1),
                                                           myBVHSet2->Element(theIndex2)));
      return Standard_True;
    }
    return Standard_False;
  }

protected:
  NCollection_List<std::pair<TopoDS_Shape, TopoDS_Shape>> myPairs; //!< Selected pairs
};

//=======================================================================
// function : PairShapeSelector
// purpose : Implement the simplest shape's selector
//=======================================================================
class PairShapesSelectorVoid : public BVH_PairTraverse<Standard_Real, 3>
{
public:
  //! Constructor
  PairShapesSelectorVoid() {}

  //! Returns the selected pairs of shapes
  const NCollection_List<std::pair<TopoDS_Shape, TopoDS_Shape>>& Pairs() const { return myPairs; }

public:
  //! Sets the sets to access the elements
  void SetShapeBoxSets(
    const opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>>& theSBSet1,
    const opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>>& theSBSet2)
  {
    mySBSet1 = theSBSet1;
    mySBSet2 = theSBSet2;
  }

public:
  //! Defines the rules for node rejection
  virtual Standard_Boolean RejectNode(const BVH_Vec3d& theCornerMin1,
                                      const BVH_Vec3d& theCornerMax1,
                                      const BVH_Vec3d& theCornerMin2,
                                      const BVH_Vec3d& theCornerMax2,
                                      Standard_Real&) const Standard_OVERRIDE
  {
    return BVH_Box<Standard_Real, 3>(theCornerMin1, theCornerMax1)
      .IsOut(BVH_Box<Standard_Real, 3>(theCornerMin2, theCornerMax2));
  }

  //! Defines the rules for leaf acceptance
  virtual Standard_Boolean Accept(const Standard_Integer theIndex1,
                                  const Standard_Integer theIndex2) Standard_OVERRIDE
  {
    BVH_Box<Standard_Real, 3> aBox1 = mySBSet1->Box(theIndex1);
    BVH_Box<Standard_Real, 3> aBox2 = mySBSet2->Box(theIndex2);
    if (!aBox1.IsOut(aBox2))
    {
      myPairs.Append(std::pair<TopoDS_Shape, TopoDS_Shape>(mySBSet1->Element(theIndex1),
                                                           mySBSet2->Element(theIndex2)));
      return Standard_True;
    }
    return Standard_False;
  }

protected:
  opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>> mySBSet1; //!< First ShapeBoxSet
  // clang-format off
  opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>> mySBSet2; //!< Second ShapeBoxSet
  // clang-format on
  NCollection_List<std::pair<TopoDS_Shape, TopoDS_Shape>> myPairs; //!< Selected pairs
};

//=======================================================================
// function : QABVH_PairSelect
// purpose : Test the work of BVH on the simple example of pairs of shapes selection
//=======================================================================
static Standard_Integer QABVH_PairSelect(Draw_Interpretor& theDI,
                                         Standard_Integer  theArgc,
                                         const char**      theArgv)
{
  if (theArgc < 4)
  {
    theDI.PrintHelp(theArgv[0]);
    return 1;
  }

  TopoDS_Shape aShape[2];
  // Get the first shape
  aShape[0] = DBRep::Get(theArgv[2]);
  if (aShape[0].IsNull())
  {
    std::cout << theArgv[2] << " does not exist" << std::endl;
    return 1;
  }

  // Get the second shape
  aShape[1] = DBRep::Get(theArgv[3]);
  if (aShape[1].IsNull())
  {
    std::cout << theArgv[3] << " does not exist" << std::endl;
    return 1;
  }

  // Which selector to use
  Standard_Boolean useVoidSelector = Standard_False;
  if (theArgc > 4)
    useVoidSelector = !strcmp(theArgv[4], "-void");

  // Define BVH Builder
  opencascade::handle<BVH_LinearBuilder<Standard_Real, 3>> aLBuilder =
    new BVH_LinearBuilder<Standard_Real, 3>();

  // Create the ShapeSet
  opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>> aShapeBoxSet[2];

  for (Standard_Integer i = 0; i < 2; ++i)
  {
    aShapeBoxSet[i] = new BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>(aLBuilder);
    // Add elements into set
    TopTools_IndexedMapOfShape aMapShapes;
    TopExp::MapShapes(aShape[i], TopAbs_VERTEX, aMapShapes);
    TopExp::MapShapes(aShape[i], TopAbs_EDGE, aMapShapes);
    TopExp::MapShapes(aShape[i], TopAbs_FACE, aMapShapes);

    for (Standard_Integer iS = 1; iS <= aMapShapes.Extent(); ++iS)
    {
      const TopoDS_Shape& aS = aMapShapes(iS);

      Bnd_Box aSBox;
      BRepBndLib::Add(aS, aSBox);

      aShapeBoxSet[i]->Add(aS, Bnd_Tools::Bnd2BVH(aSBox));
    }
    // Build BVH
    aShapeBoxSet[i]->Build();
  }

  NCollection_List<std::pair<TopoDS_Shape, TopoDS_Shape>> aPairs;
  if (!useVoidSelector)
  {
    // Initialize selector
    PairShapesSelector aSelector;
    // Select the elements
    aSelector.SetBVHSets(aShapeBoxSet[0].get(), aShapeBoxSet[1].get());
    aSelector.Select();
    aPairs = aSelector.Pairs();
  }
  else
  {
    // Initialize selector
    PairShapesSelectorVoid aSelector;
    // Select the elements
    aSelector.SetShapeBoxSets(aShapeBoxSet[0], aShapeBoxSet[1]);
    aSelector.Select(aShapeBoxSet[0]->BVH(), aShapeBoxSet[1]->BVH());
    aPairs = aSelector.Pairs();
  }

  // Draw the selected shapes
  TopoDS_Compound aResult;
  BRep_Builder().MakeCompound(aResult);

  for (NCollection_List<std::pair<TopoDS_Shape, TopoDS_Shape>>::Iterator it(aPairs); it.More();
       it.Next())
  {
    TopoDS_Compound aPair;
    BRep_Builder().MakeCompound(aPair);

    BRep_Builder().Add(aPair, it.Value().first);
    BRep_Builder().Add(aPair, it.Value().second);

    BRep_Builder().Add(aResult, aPair);
  }

  DBRep::Set(theArgv[1], aResult);
  return 0;
}

//=======================================================================
// function : Triangle
// purpose : Auxiliary structure to keep the nodes of the triangle
//=======================================================================
struct Triangle
{
  Triangle() {}

  Triangle(const BVH_Vec3d& theP1, const BVH_Vec3d& theP2, const BVH_Vec3d& theP3)
      : _Node1(theP1),
        _Node2(theP2),
        _Node3(theP3)
  {
  }

  BVH_Vec3d _Node1;
  BVH_Vec3d _Node2;
  BVH_Vec3d _Node3;
};

//=======================================================================
// function : TriangleTriangleSqDistance
// purpose : Computes the Triangle-Triangle square distance
//=======================================================================
static Standard_Real TriangleTriangleSqDistance(const BVH_Vec3d& theNode11,
                                                const BVH_Vec3d& theNode12,
                                                const BVH_Vec3d& theNode13,
                                                const BVH_Vec3d& theNode21,
                                                const BVH_Vec3d& theNode22,
                                                const BVH_Vec3d& theNode23)
{
  Standard_Real aDist, aMinDist = RealLast();

  BVH_Vec3d aNodes[2][3] = {{theNode11, theNode12, theNode13}, {theNode21, theNode22, theNode23}};

  const Standard_Integer aNbSeg = 100; // number of segments on edge
  for (Standard_Integer iT = 0; iT < 2; ++iT)
  {
    // projecting points of one triangle on the opposite triangle
    for (Standard_Integer iP = 0; iP < 3; ++iP)
    {
      aDist = BVH_Tools<Standard_Real, 3>::PointTriangleSquareDistance(aNodes[iT][iP],
                                                                       aNodes[(iT + 1) % 2][0],
                                                                       aNodes[(iT + 1) % 2][1],
                                                                       aNodes[(iT + 1) % 2][2]);
      if (aDist < aMinDist)
      {
        aMinDist = aDist;
        if (aMinDist == 0)
          return aMinDist;
      }
    }

    // projecting edges on the opposite triangle
    std::pair<BVH_Vec3d, BVH_Vec3d> anEdges[3] = {
      std::pair<BVH_Vec3d, BVH_Vec3d>(aNodes[iT][0], aNodes[iT][1]),
      std::pair<BVH_Vec3d, BVH_Vec3d>(aNodes[iT][1], aNodes[iT][2]),
      std::pair<BVH_Vec3d, BVH_Vec3d>(aNodes[iT][2], aNodes[iT][0])};

    for (Standard_Integer iE = 0; iE < 3; ++iE)
    {
      const BVH_Vec3d &aPFirst = anEdges[iE].first, aPLast = anEdges[iE].second;
      BVH_Vec3d        anEdge  = (aPLast - aPFirst);
      Standard_Real    aLength = anEdge.Modulus();
      if (aLength < Precision::Confusion())
        continue;
      anEdge /= aLength;

      Standard_Real aDelta = aLength / aNbSeg;

      for (Standard_Integer iP = 1; iP < aNbSeg; ++iP)
      {
        BVH_Vec3d aPE = aPFirst + anEdge.Multiplied(iP * aDelta);
        aDist         = BVH_Tools<Standard_Real, 3>::PointTriangleSquareDistance(aPE,
                                                                         aNodes[(iT + 1) % 2][0],
                                                                         aNodes[(iT + 1) % 2][1],
                                                                         aNodes[(iT + 1) % 2][2]);
        if (aDist < aMinDist)
        {
          aMinDist = aDist;
          if (aMinDist == 0)
            return aMinDist;
        }
      }
    }
  }
  return aMinDist;
}

//=======================================================================
// function : MeshMeshDistance
// purpose : Class to compute the distance between two meshes
//=======================================================================
class MeshMeshDistance
    : public BVH_PairDistance<Standard_Real, 3, BVH_BoxSet<Standard_Real, 3, Triangle>>
{
public:
  //! Constructor
  MeshMeshDistance() {}

public:
  //! Defines the rules for leaf acceptance
  virtual Standard_Boolean Accept(const Standard_Integer theIndex1,
                                  const Standard_Integer theIndex2) Standard_OVERRIDE
  {
    const Triangle& aTri1 = myBVHSet1->Element(theIndex1);
    const Triangle& aTri2 = myBVHSet2->Element(theIndex2);

    Standard_Real aDistance = TriangleTriangleSqDistance(aTri1._Node1,
                                                         aTri1._Node2,
                                                         aTri1._Node3,
                                                         aTri2._Node1,
                                                         aTri2._Node2,
                                                         aTri2._Node3);
    if (aDistance < myDistance)
    {
      myDistance = aDistance;
      return Standard_True;
    }
    return Standard_False;
  }
};

//=======================================================================
// function : QABVH_PairDistance
// purpose : Computes the distance between two meshes of the given shapes
//=======================================================================
static Standard_Integer QABVH_PairDistance(Draw_Interpretor& theDI,
                                           Standard_Integer  theArgc,
                                           const char**      theArgv)
{
  if (theArgc != 3)
  {
    theDI.PrintHelp(theArgv[0]);
    return 1;
  }

  TopoDS_Shape aShape[2];
  // Get the first shape
  aShape[0] = DBRep::Get(theArgv[1]);
  if (aShape[0].IsNull())
  {
    std::cout << theArgv[1] << " does not exist" << std::endl;
    return 1;
  }

  // Get the second shape
  aShape[1] = DBRep::Get(theArgv[2]);
  if (aShape[1].IsNull())
  {
    std::cout << theArgv[2] << " does not exist" << std::endl;
    return 1;
  }

  // Define BVH Builder
  opencascade::handle<BVH_LinearBuilder<Standard_Real, 3>> aLBuilder =
    new BVH_LinearBuilder<Standard_Real, 3>();

  // Create the ShapeSet
  opencascade::handle<BVH_BoxSet<Standard_Real, 3, Triangle>> aTriangleBoxSet[2];

  for (Standard_Integer i = 0; i < 2; ++i)
  {
    aTriangleBoxSet[i] = new BVH_BoxSet<Standard_Real, 3, Triangle>(aLBuilder);

    TopTools_IndexedMapOfShape aMapShapes;
    TopExp::MapShapes(aShape[i], TopAbs_FACE, aMapShapes);

    for (Standard_Integer iS = 1; iS <= aMapShapes.Extent(); ++iS)
    {
      const TopoDS_Face&                aF = TopoDS::Face(aMapShapes(iS));
      TopLoc_Location                   aLoc;
      const Handle(Poly_Triangulation)& aTriangulation = BRep_Tool::Triangulation(aF, aLoc);

      const int aNbTriangles = aTriangulation->NbTriangles();
      for (int iT = 1; iT <= aNbTriangles; ++iT)
      {
        const Poly_Triangle aTriangle = aTriangulation->Triangle(iT);
        // Nodes indices
        Standard_Integer id1, id2, id3;
        aTriangle.Get(id1, id2, id3);

        const gp_Pnt aP1 = aTriangulation->Node(id1).Transformed(aLoc.Transformation());
        const gp_Pnt aP2 = aTriangulation->Node(id2).Transformed(aLoc.Transformation());
        const gp_Pnt aP3 = aTriangulation->Node(id3).Transformed(aLoc.Transformation());

        BVH_Vec3d aBVHP1(aP1.X(), aP1.Y(), aP1.Z());
        BVH_Vec3d aBVHP2(aP2.X(), aP2.Y(), aP2.Z());
        BVH_Vec3d aBVHP3(aP3.X(), aP3.Y(), aP3.Z());

        BVH_Box<Standard_Real, 3> aBox;
        aBox.Add(aBVHP1);
        aBox.Add(aBVHP2);
        aBox.Add(aBVHP3);

        aTriangleBoxSet[i]->Add(Triangle(aBVHP1, aBVHP2, aBVHP3), aBox);
      }
    }
    // Build BVH
    aTriangleBoxSet[i]->Build();
  }

  // Initialize selector
  MeshMeshDistance aDistTool;
  // Select the elements
  aDistTool.SetBVHSets(aTriangleBoxSet[0].get(), aTriangleBoxSet[1].get());
  Standard_Real aSqDist = aDistTool.ComputeDistance();
  if (!aDistTool.IsDone())
    std::cout << "Not Done" << std::endl;
  else
    theDI << "Distance " << sqrt(aSqDist) << "\n";

  return 0;
}

//=======================================================================
// function : QABVH_TriangleSet
// purpose : Auxiliary class to contain triangulation of a face
//=======================================================================
class QABVH_TriangleSet : public BVH_Triangulation<Standard_Real, 3>
{
public:
  QABVH_TriangleSet()
      : BVH_Triangulation<Standard_Real, 3>()
  {
  }

public:
  //! Creates the triangulation from a face
  void Build(const TopoDS_Face& theFace)
  {
    TopLoc_Location                   aLoc;
    const Handle(Poly_Triangulation)& aTriangulation = BRep_Tool::Triangulation(theFace, aLoc);

    const int aNbTriangles = aTriangulation->NbTriangles();
    for (int iT = 1; iT <= aNbTriangles; ++iT)
    {
      const Poly_Triangle aTriangle = aTriangulation->Triangle(iT);
      // Nodes indices
      Standard_Integer id1, id2, id3;
      aTriangle.Get(id1, id2, id3);

      const gp_Pnt aP1 = aTriangulation->Node(id1).Transformed(aLoc.Transformation());
      const gp_Pnt aP2 = aTriangulation->Node(id2).Transformed(aLoc.Transformation());
      const gp_Pnt aP3 = aTriangulation->Node(id3).Transformed(aLoc.Transformation());

      BVH_Vec3d aBVHP1(aP1.X(), aP1.Y(), aP1.Z());
      BVH_Vec3d aBVHP2(aP2.X(), aP2.Y(), aP2.Z());
      BVH_Vec3d aBVHP3(aP3.X(), aP3.Y(), aP3.Z());

      Standard_Integer id = static_cast<Standard_Integer>(Vertices.size()) - 1;
      Vertices.push_back(aBVHP1);
      Vertices.push_back(aBVHP2);
      Vertices.push_back(aBVHP3);

      Elements.push_back(BVH_Vec4i(id + 1, id + 2, id + 3, iT));
    }

    MarkDirty();
    BVH();
  }
};

//=======================================================================
// function : QABVH_Geometry
// purpose : Auxiliary class to contain triangulation of a shape
//=======================================================================
class QABVH_Geometry : public BVH_Geometry<Standard_Real, 3>
{
public:
  QABVH_Geometry()
      : BVH_Geometry<Standard_Real, 3>()
  {
  }

public:
  //! Creates the triangulation from a face
  void Build(const TopoDS_Shape& theShape)
  {
    TopExp_Explorer anExp(theShape, TopAbs_FACE);
    for (; anExp.More(); anExp.Next())
    {
      const TopoDS_Face&        aF      = TopoDS::Face(anExp.Current());
      Handle(QABVH_TriangleSet) aTriSet = new QABVH_TriangleSet();
      aTriSet->Build(aF);
      myObjects.Append(aTriSet);
    }

    MarkDirty();
    BVH();
  }
};

//=======================================================================
// function : QABVH_DistanceField
// purpose : Computes the distance field on a given shape
//=======================================================================
static Standard_Integer QABVH_DistanceField(Draw_Interpretor& theDI,
                                            Standard_Integer  theArgc,
                                            const char**      theArgv)
{
  if (theArgc < 2)
  {
    theDI.PrintHelp(theArgv[0]);
    return 1;
  }

  TopoDS_Shape aShape;
  // Get the first shape
  aShape = DBRep::Get(theArgv[1]);
  if (aShape.IsNull())
  {
    std::cout << theArgv[1] << " does not exist" << std::endl;
    return 1;
  }

  Standard_Integer aNbDim = 10;
  if (theArgc > 2)
  {
    aNbDim = Draw::Atoi(theArgv[2]);
  }

  QABVH_Geometry aGeometry;
  aGeometry.Build(aShape);

  BVH_DistanceField<Standard_Real, 3> aDField(aNbDim, Standard_True);
  aDField.Build(aGeometry);

  for (Standard_Integer iX = 0; iX < aDField.DimensionX(); ++iX)
  {
    for (Standard_Integer iY = 0; iY < aDField.DimensionY(); ++iY)
    {
      for (Standard_Integer iZ = 0; iZ < aDField.DimensionZ(); ++iZ)
      {
        Standard_Real aDist = aDField.Voxel(iX, iY, iZ);
        std::cout << "(" << iX << ", " << iY << ", " << iZ << "): " << aDist << std::endl;
      }
    }
  }
  return 0;
}

//=================================================================================================

void QABugs::Commands_BVH(Draw_Interpretor& theCommands)
{
  const char* group = "QABugs";

  theCommands.Add("QABVH_ShapeSelect",
                  "Tests the work of BHV_BoxSet algorithm on the simple example of selection of "
                  "shapes which boxes interfere with given box.\n"
                  "Usage: QABVH_ShapeSelect result shape box (defined as a solid) [-void]\n"
                  "\tResult should contain all sub-shapes of the shape interfering with given box",
                  __FILE__,
                  QABVH_ShapeSelect,
                  group);

  theCommands.Add("QABVH_PairSelect",
                  "Tests the work of BHV_BoxSet algorithm on the simple example of selection of "
                  "pairs of shapes with interfering bounding boxes.\n"
                  "Usage: QABVH_PairSelect result shape1 shape2 [-void]\n"
                  "\tResult should contain all interfering pairs (compound of pairs)",
                  __FILE__,
                  QABVH_PairSelect,
                  group);

  theCommands.Add("QABVH_PairDistance",
                  "Computes the distance between the meshes of the given shapes.\n"
                  "Usage: QABVH_PairDistance shape1 shape2\n"
                  "\tThe given shapes should contain triangulation\n",
                  __FILE__,
                  QABVH_PairDistance,
                  group);

  theCommands.Add("QABVH_DistanceField",
                  "Computes the distance field for a shape with triangulation\n"
                  "Usage: QABVH_DistanceField shape [nbSplit]\n",
                  __FILE__,
                  QABVH_DistanceField,
                  group);
}
