// Created on: 2015-03-16
// Created by: Varvara POSKONINA
// Copyright (c) 2005-2014 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 <NCollection_Vector.hxx>
#include <Poly_Array1OfTriangle.hxx>
#include <Standard_Assert.hxx>

// =======================================================================
// function : isSeparated
// purpose  : Checks if AABB and frustum are separated along the given axis.
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::isSeparated (const SelectMgr_Vec3& theBoxMin,
                                                    const SelectMgr_Vec3& theBoxMax,
                                                    const gp_XYZ&         theDirect,
                                                    Standard_Boolean*     theInside) const
{
  const Standard_Real aMinB =
    theDirect.X() * (theDirect.X() < 0.0 ? theBoxMax.x() : theBoxMin.x()) +
    theDirect.Y() * (theDirect.Y() < 0.0 ? theBoxMax.y() : theBoxMin.y()) +
    theDirect.Z() * (theDirect.Z() < 0.0 ? theBoxMax.z() : theBoxMin.z());

  const Standard_Real aMaxB =
    theDirect.X() * (theDirect.X() < 0.0 ? theBoxMin.x() : theBoxMax.x()) +
    theDirect.Y() * (theDirect.Y() < 0.0 ? theBoxMin.y() : theBoxMax.y()) +
    theDirect.Z() * (theDirect.Z() < 0.0 ? theBoxMin.z() : theBoxMax.z());

  Standard_ASSERT_RAISE (aMaxB >= aMinB, "Error! Failed to project box");

  // frustum projection
  Standard_Real aMinF =  DBL_MAX;
  Standard_Real aMaxF = -DBL_MAX;

  for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
  {
    const Standard_Real aProj = myVertices[aVertIdx].XYZ().Dot (theDirect);

    aMinF = Min (aMinF, aProj);
    aMaxF = Max (aMaxF, aProj);

    if (aMinF <= aMaxB && aMaxF >= aMinB)
    {
      if (theInside == NULL || !(*theInside)) // only overlap test
      {
        return Standard_False;
      }
    }
  }

  if (aMinF > aMaxB || aMaxF < aMinB)
  {
    return Standard_True; // fully separated
  }
  else if (theInside != NULL) // to check for inclusion?
  {
    *theInside &= aMinB >= aMinF && aMaxB <= aMaxF;
  }

  return Standard_False;
}

// =======================================================================
// function : isSeparated
// purpose  : Checks if triangle and frustum are separated along the
//            given axis
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::isSeparated (const gp_Pnt& thePnt1,
                                                    const gp_Pnt& thePnt2,
                                                    const gp_Pnt& thePnt3,
                                                    const gp_XYZ& theAxis) const
{
  // frustum projection
  Standard_Real aMinF = RealLast();
  Standard_Real aMaxF = RealFirst();

  // triangle projection
  Standard_Real aMinTr = RealLast();
  Standard_Real aMaxTr = RealFirst();

  Standard_Real aTriangleProj;

  aTriangleProj = theAxis.Dot (thePnt1.XYZ());
  aMinTr = Min (aMinTr, aTriangleProj);
  aMaxTr = Max (aMaxTr, aTriangleProj);

  aTriangleProj = theAxis.Dot (thePnt2.XYZ());
  aMinTr = Min (aMinTr, aTriangleProj);
  aMaxTr = Max (aMaxTr, aTriangleProj);

  aTriangleProj = theAxis.Dot (thePnt3.XYZ());
  aMinTr = Min (aMinTr, aTriangleProj);
  aMaxTr = Max (aMaxTr, aTriangleProj);

  for (Standard_Integer aVertIter = 0; aVertIter < N * 2; ++aVertIter)
  {
    const Standard_Real aProj = myVertices[aVertIter].XYZ().Dot (theAxis);

    aMinF = Min (aMinF, aProj);
    aMaxF = Max (aMaxF, aProj);

    if (aMinF <= aMaxTr && aMaxF >= aMinTr)
    {
      return Standard_False;
    }
  }

  return aMinF > aMaxTr || aMaxF < aMinTr;
}

// =======================================================================
// function : hasOverlap
// purpose  : Returns true if selecting volume is overlapped by
//            axis-aligned bounding box with minimum corner at point
//            theMinPnt and maximum at point theMaxPnt
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const SelectMgr_Vec3& theMinPnt,
                                                   const SelectMgr_Vec3& theMaxPnt,
                                                   Standard_Boolean*     theInside) const
{
  for (Standard_Integer anAxis = 0; anAxis < 3; ++anAxis)
  {
    if (theMinPnt[anAxis] > myMaxOrthoVertsProjections[anAxis]
     || theMaxPnt[anAxis] < myMinOrthoVertsProjections[anAxis])
    {
      return Standard_False; // fully separated
    }
    else if (theInside != NULL) // to check for inclusion?
    {
      *theInside &= theMinPnt[anAxis] >= myMinOrthoVertsProjections[anAxis]
                 && theMaxPnt[anAxis] <= myMaxOrthoVertsProjections[anAxis];
    }
  }

  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;

  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    const gp_XYZ& aPlane = myPlanes[aPlaneIdx].XYZ();

    const Standard_Real aBoxProjMin =
      aPlane.X() * (aPlane.X() < 0.f ? theMaxPnt.x() : theMinPnt.x()) +
      aPlane.Y() * (aPlane.Y() < 0.f ? theMaxPnt.y() : theMinPnt.y()) +
      aPlane.Z() * (aPlane.Z() < 0.f ? theMaxPnt.z() : theMinPnt.z());

    const Standard_Real aBoxProjMax =
      aPlane.X() * (aPlane.X() < 0.f ? theMinPnt.x() : theMaxPnt.x()) +
      aPlane.Y() * (aPlane.Y() < 0.f ? theMinPnt.y() : theMaxPnt.y()) +
      aPlane.Z() * (aPlane.Z() < 0.f ? theMinPnt.z() : theMaxPnt.z());

    Standard_ASSERT_RAISE (aBoxProjMax >= aBoxProjMin, "Error! Failed to project box");

    if (aBoxProjMin > myMaxVertsProjections[aPlaneIdx]
     || aBoxProjMax < myMinVertsProjections[aPlaneIdx])
    {
      return Standard_False; // fully separated
    }
    else if (theInside != NULL) // to check for inclusion?
    {
      *theInside &= aBoxProjMin >= myMinVertsProjections[aPlaneIdx]
                 && aBoxProjMax <= myMaxVertsProjections[aPlaneIdx];
    }
  }

  for (Standard_Integer aDim = 0; aDim < 3; ++aDim)
  {
    // the following code performs a speedup of cross-product
    // of vector with 1.0 at the position aDim and myEdgeDirs[aVolDir]
    const Standard_Integer aNext = (aDim + 1) % 3;
    const Standard_Integer aNextNext = (aDim + 2) % 3;
    for (Standard_Integer aVolDir = 0, aDirectionsNb = myIsOrthographic ? 4 : 6; aVolDir < aDirectionsNb; ++aVolDir)
    {
      gp_XYZ aDirection (DBL_MAX, DBL_MAX, DBL_MAX);
      aDirection.ChangeData()[aDim]      = 0;
      aDirection.ChangeData()[aNext]     = -myEdgeDirs[aVolDir].XYZ().GetData()[aNextNext];
      aDirection.ChangeData()[aNextNext] = myEdgeDirs[aVolDir].XYZ().GetData()[aNext];

      if (isSeparated (theMinPnt, theMaxPnt, aDirection, theInside))
      {
        return Standard_False;
      }
    }
  }

  return Standard_True;
}

// =======================================================================
// function : hasOverlap
// purpose  : SAT intersection test between defined volume and given point
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& thePnt) const
{
  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;

  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    const Standard_Real aPointProj = myPlanes[aPlaneIdx].XYZ().Dot (thePnt.XYZ());

    if (aPointProj > myMaxVertsProjections[aPlaneIdx]
     || aPointProj < myMinVertsProjections[aPlaneIdx])
    {
      return Standard_False;
    }
  }

  return Standard_True;
}

// =======================================================================
// function : hasOverlap
// purpose  : SAT intersection test between defined volume and given segment
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& theStartPnt,
                                                   const gp_Pnt& theEndPnt) const
{
  const gp_XYZ& aDir = theEndPnt.XYZ() - theStartPnt.XYZ();
  if (aDir.Modulus() < Precision::Confusion())
    return Standard_True;

  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    Standard_Real aMinSegm = RealLast(), aMaxSegm = RealFirst();
    Standard_Real aMinF    = RealLast(), aMaxF    = RealFirst();

    Standard_Real aProj1 = myPlanes[aPlaneIdx].XYZ().Dot (theStartPnt.XYZ());
    Standard_Real aProj2 = myPlanes[aPlaneIdx].XYZ().Dot (theEndPnt.XYZ());
    aMinSegm = Min (aProj1, aProj2);
    aMaxSegm = Max (aProj1, aProj2);

    aMaxF = myMaxVertsProjections[aPlaneIdx];
    aMinF = myMinVertsProjections[aPlaneIdx];

    if (aMinSegm > aMaxF
      || aMaxSegm < aMinF)
    {
      return Standard_False;
    }
  }

  Standard_Real aMin1 = DBL_MAX, aMax1 = -DBL_MAX;
  Standard_Real aMin2 = DBL_MAX, aMax2 = -DBL_MAX;
  for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
  {
    Standard_Real aProjection = aDir.Dot (myVertices[aVertIdx].XYZ());
    aMax2 = Max (aMax2, aProjection);
    aMin2 = Min (aMin2, aProjection);
  }
  Standard_Real aProj1 = aDir.Dot (theStartPnt.XYZ());
  Standard_Real aProj2 = aDir.Dot (theEndPnt.XYZ());
  aMin1 = Min (aProj1, aProj2);
  aMax1 = Max (aProj1, aProj2);
  if (aMin1 > aMax2
    || aMax1 < aMin2)
  {
    return Standard_False;
  }

  Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
  for (Standard_Integer aEdgeDirIdx = 0; aEdgeDirIdx < aDirectionsNb; ++aEdgeDirIdx)
  {
    Standard_Real aMinSegm = DBL_MAX, aMaxSegm = -DBL_MAX;
    Standard_Real aMinF    = DBL_MAX, aMaxF    = -DBL_MAX;

    const gp_XYZ aTestDir = aDir.Crossed (myEdgeDirs[aEdgeDirIdx].XYZ());

    Standard_Real Proj1 = aTestDir.Dot (theStartPnt.XYZ());
    Standard_Real Proj2 = aTestDir.Dot (theEndPnt.XYZ());
    aMinSegm = Min (Proj1, Proj2);
    aMaxSegm = Max (Proj1, Proj2);

    for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
    {
      Standard_Real aProjection = aTestDir.Dot (myVertices[aVertIdx].XYZ());
      aMaxF = Max (aMaxF, aProjection);
      aMinF = Min (aMinF, aProjection);
    }

    if (aMinSegm > aMaxF
      || aMaxSegm < aMinF)
    {
      return Standard_False;
    }
  }

  return Standard_True;
}

// =======================================================================
// function : hasOverlap
// purpose  : SAT intersection test between frustum given and planar convex
//            polygon represented as ordered point set
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const TColgp_Array1OfPnt& theArrayOfPnts,
                                                   gp_Vec& theNormal) const
{
  Standard_Integer aStartIdx = theArrayOfPnts.Lower();
  Standard_Integer anEndIdx = theArrayOfPnts.Upper();

  const gp_XYZ& aPnt1 = theArrayOfPnts.Value (aStartIdx).XYZ();
  const gp_XYZ& aPnt2 = theArrayOfPnts.Value (aStartIdx + 1).XYZ();
  const gp_XYZ& aPnt3 = theArrayOfPnts.Value (aStartIdx + 2).XYZ();
  const gp_XYZ aVec1 = aPnt1 - aPnt2;
  const gp_XYZ aVec2 = aPnt3 - aPnt2;
  theNormal = aVec2.Crossed (aVec1);
  const gp_XYZ& aNormal = theNormal.XYZ();
  Standard_Real aPolygProjection = aNormal.Dot (aPnt1);

  Standard_Real aMax = RealFirst();
  Standard_Real aMin = RealLast();
  for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
  {
    Standard_Real aProjection = aNormal.Dot (myVertices[aVertIdx].XYZ());
    aMax = Max (aMax, aProjection);
    aMin = Min (aMin, aProjection);
  }
  if (aPolygProjection > aMax
    || aPolygProjection < aMin)
  {
    return Standard_False;
  }

  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx <  N + 1; aPlaneIdx += anIncFactor)
  {
    Standard_Real aMaxF = RealFirst();
    Standard_Real aMinF = RealLast();
    Standard_Real aMaxPolyg = RealFirst();
    Standard_Real aMinPolyg = RealLast();
    const gp_XYZ& aPlane = myPlanes[aPlaneIdx].XYZ();
    for (Standard_Integer aPntIter = aStartIdx; aPntIter <= anEndIdx; ++aPntIter)
    {
      Standard_Real aProjection = aPlane.Dot (theArrayOfPnts.Value (aPntIter).XYZ());
      aMaxPolyg = Max (aMaxPolyg, aProjection);
      aMinPolyg = Min (aMinPolyg, aProjection);
    }
    aMaxF = myMaxVertsProjections[aPlaneIdx];
    aMinF = myMinVertsProjections[aPlaneIdx];
    if (aMinPolyg > aMaxF
      || aMaxPolyg < aMinF)
    {
      return Standard_False;
    }
  }

  Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
  for (Standard_Integer aPntsIter = 0, aLastIdx = anEndIdx - aStartIdx, aLen = theArrayOfPnts.Length(); aPntsIter <= aLastIdx; ++aPntsIter)
  {
    const gp_XYZ aSegmDir = theArrayOfPnts.Value ((aPntsIter + 1) % aLen + aStartIdx).XYZ()
      - theArrayOfPnts.Value (aPntsIter + aStartIdx).XYZ();
    for (Standard_Integer aVolDir = 0; aVolDir < aDirectionsNb; ++aVolDir)
    {
      Standard_Real aMaxPolyg = RealFirst();
      Standard_Real aMinPolyg = RealLast();
      Standard_Real aMaxF = RealFirst();
      Standard_Real aMinF = RealLast();
      const gp_XYZ aTestDir = aSegmDir.Crossed (myEdgeDirs[aVolDir].XYZ());

      for (Standard_Integer aPntIter = aStartIdx; aPntIter <= anEndIdx; ++aPntIter)
      {
        Standard_Real aProjection = aTestDir.Dot (theArrayOfPnts.Value (aPntIter).XYZ());
        aMaxPolyg = Max (aMaxPolyg, aProjection);
        aMinPolyg = Min (aMinPolyg, aProjection);
      }

      for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
      {
        Standard_Real aProjection = aTestDir.Dot (myVertices[aVertIdx].XYZ());
        aMaxF = Max (aMaxF, aProjection);
        aMinF = Min (aMinF, aProjection);
      }

      if (aMinPolyg > aMaxF
        || aMaxPolyg < aMinF)
      {
        return Standard_False;
      }
    }
  }

  return Standard_True;
}

// =======================================================================
// function : hasOverlap
// purpose  : SAT intersection test between defined volume and given triangle
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& thePnt1,
                                                   const gp_Pnt& thePnt2,
                                                   const gp_Pnt& thePnt3,
                                                   gp_Vec& theNormal) const
{
  const gp_XYZ aTrEdges[3] = { thePnt2.XYZ() - thePnt1.XYZ(),
                               thePnt3.XYZ() - thePnt2.XYZ(),
                               thePnt1.XYZ() - thePnt3.XYZ() };

  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    const gp_XYZ& aPlane = myPlanes[aPlaneIdx].XYZ();
    Standard_Real aTriangleProj;

    aTriangleProj = aPlane.Dot (thePnt1.XYZ());
    Standard_Real aTriangleProjMin = aTriangleProj;
    Standard_Real aTriangleProjMax = aTriangleProj;

    aTriangleProj = aPlane.Dot (thePnt2.XYZ());
    aTriangleProjMin = Min (aTriangleProjMin, aTriangleProj);
    aTriangleProjMax = Max (aTriangleProjMax, aTriangleProj);

    aTriangleProj = aPlane.Dot (thePnt3.XYZ());
    aTriangleProjMin = Min (aTriangleProjMin, aTriangleProj);
    aTriangleProjMax = Max (aTriangleProjMax, aTriangleProj);

    Standard_Real aFrustumProjMax = myMaxVertsProjections[aPlaneIdx];
    Standard_Real aFrustumProjMin = myMinVertsProjections[aPlaneIdx];
    if (aTriangleProjMin > aFrustumProjMax
      || aTriangleProjMax < aFrustumProjMin)
    {
      return Standard_False;
    }
  }

  theNormal = aTrEdges[2].Crossed (aTrEdges[0]);
  if (isSeparated (thePnt1, thePnt2, thePnt3, theNormal.XYZ()))
  {
    return Standard_False;
  }

  Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
  for (Standard_Integer aTriangleEdgeIdx = 0; aTriangleEdgeIdx < 3; ++aTriangleEdgeIdx)
  {
    for (Standard_Integer aVolDir = 0; aVolDir < aDirectionsNb; ++aVolDir)
    {
      const gp_XYZ& aTestDirection =  myEdgeDirs[aVolDir].XYZ().Crossed (aTrEdges[aTriangleEdgeIdx]);

      if (isSeparated (thePnt1, thePnt2, thePnt3, aTestDirection))
      {
        return Standard_False;
      }
    }
  }

  return Standard_True;
}

//=======================================================================
//function : DumpJson
//purpose  : 
//=======================================================================
template <int N>
void SelectMgr_Frustum<N>::DumpJson (Standard_OStream& theOStream, Standard_Integer theDepth) const
{
  OCCT_DUMP_TRANSIENT_CLASS_BEGIN (theOStream)

  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    const gp_Vec& aPlane = myPlanes[aPlaneIdx];
    OCCT_DUMP_FIELD_VALUES_DUMPED (theOStream, theDepth, &aPlane)

    OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, myMaxVertsProjections[aPlaneIdx])
    OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, myMinVertsProjections[aPlaneIdx])
  }

  for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
  {
    const gp_Pnt& aVertex = myVertices[aVertIdx];
    OCCT_DUMP_FIELD_VALUES_DUMPED (theOStream, theDepth, &aVertex)
  }

  OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, myPixelTolerance)
  OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, myIsOrthographic)
  OCCT_DUMP_FIELD_VALUE_POINTER (theOStream, myBuilder)
  OCCT_DUMP_FIELD_VALUE_POINTER (theOStream, myCamera)

  for (Standard_Integer anIndex = 0; anIndex < 3; anIndex++)
  {
    Standard_Real aMaxOrthoVertsProjections = myMaxOrthoVertsProjections[anIndex];
    Standard_Real aMinOrthoVertsProjections = myMinOrthoVertsProjections[anIndex];

    OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, aMaxOrthoVertsProjections)
    OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, aMinOrthoVertsProjections)
  }

  for (Standard_Integer anIndex = 0; anIndex < 6; anIndex++)
  {
    const gp_Vec& anEdgeDir = myEdgeDirs[anIndex];
    OCCT_DUMP_FIELD_VALUES_DUMPED (theOStream, theDepth, &anEdgeDir)
  }
}