// Created on: 1995-07-18
// Created by: Modelistation
// Copyright (c) 1995-1999 Matra Datavision
// Copyright (c) 1999-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 <algorithm>

#include <Extrema_GlobOptFuncCC.hxx>
#include <math_GlobOptMin.hxx>
#include <Standard_NullObject.hxx>
#include <Standard_OutOfRange.hxx>
#include <StdFail_NotDone.hxx>
#include <TColStd_Array1OfReal.hxx>
#include <TColStd_ListOfInteger.hxx>
#include <Precision.hxx>
#include <NCollection_Vector.hxx>
#include <NCollection_CellFilter.hxx>
#include <GCPnts_AbscissaPoint.hxx>

// Comparator, used in std::sort.
static Standard_Boolean comp(const gp_XY& theA,
                             const gp_XY& theB)
{
  if (theA.X() < theB.X())
  {
    return Standard_True;
  }
  else
  {
    if (theA.X() == theB.X())
    {
      if (theA.Y() < theB.Y())
        return Standard_True;
    }
  }

  return Standard_False;
}

static void ChangeIntervals(Handle(TColStd_HArray1OfReal)& theInts, const Standard_Integer theNbInts)
{
  Standard_Integer aNbInts = theInts->Length() - 1;
  Standard_Integer aNbAdd = theNbInts - aNbInts;
  Handle(TColStd_HArray1OfReal) aNewInts = new TColStd_HArray1OfReal(1, theNbInts + 1);
  Standard_Integer aNbLast = theInts->Length();
  Standard_Integer i;
  if (aNbInts == 1)
  {
    aNewInts->SetValue(1, theInts->First());
    aNewInts->SetValue(theNbInts + 1, theInts->Last());
    Standard_Real dt = (theInts->Last() - theInts->First()) / theNbInts;
    Standard_Real t = theInts->First() + dt;
    for (i = 2; i <= theNbInts; ++i, t += dt)
    {
      aNewInts->SetValue(i, t);
    }
    theInts = aNewInts;
    return;
  }
  //
  for (i = 1; i <= aNbLast; ++i)
  {
    aNewInts->SetValue(i, theInts->Value(i));
  }
  //
  while (aNbAdd > 0)
  {
    Standard_Real anLIntMax = -1.;
    Standard_Integer aMaxInd = -1;
    for (i = 1; i < aNbLast; ++i)
    {
      Standard_Real anL = aNewInts->Value(i + 1) - aNewInts->Value(i);
      if (anL > anLIntMax)
      {
        anLIntMax = anL;
        aMaxInd = i;
      }
    }

    Standard_Real t = (aNewInts->Value(aMaxInd + 1) + aNewInts->Value(aMaxInd)) / 2.;
    for (i = aNbLast; i > aMaxInd; --i)
    {
      aNewInts->SetValue(i + 1, aNewInts->Value(i));
    }
    aNbLast++;
    aNbAdd--;
    aNewInts->SetValue(aMaxInd + 1, t);
  }
  theInts = aNewInts;
}
class Extrema_CCPointsInspector : public NCollection_CellFilter_InspectorXY
{
public:
  typedef gp_XY Target;
  //! Constructor; remembers the tolerance
  Extrema_CCPointsInspector (const Standard_Real theTol)
  {
    myTol = theTol * theTol;
    myIsFind = Standard_False;
  }

  void ClearFind()
  {
    myIsFind = Standard_False;
  }

  Standard_Boolean isFind()
  {
    return myIsFind;
  }

  //! Set current point to search for coincidence
  void SetCurrent (const gp_XY& theCurPnt) 
  { 
    myCurrent = theCurPnt;
  }

  //! Implementation of inspection method
  NCollection_CellFilter_Action Inspect (const Target& theObject)
  {
    gp_XY aPt = myCurrent.Subtracted(theObject);
    const Standard_Real aSQDist = aPt.SquareModulus();
    if(aSQDist < myTol)
    {
      myIsFind = Standard_True;
    }

    return CellFilter_Keep;
  }

private:
  Standard_Real myTol;
  gp_XY myCurrent;
  Standard_Boolean myIsFind;
};

//=======================================================================
//function : ProjPOnC
//purpose  : Projects the point on the curve and returns the minimal
//           projection distance
//=======================================================================
static Standard_Real ProjPOnC(const Pnt& theP,
                              Extrema_GExtPC& theProjTool)
{
  Standard_Real aDist = ::RealLast();
  theProjTool.Perform(theP);
  if (theProjTool.IsDone() && theProjTool.NbExt())
  {
    for (Standard_Integer i = 1; i <= theProjTool.NbExt(); ++i)
    {
      Standard_Real aD = theProjTool.SquareDistance(i);
      if (aD < aDist)
        aDist = aD;
    }
    aDist = sqrt(aDist);
  }
  return aDist;
}

//=======================================================================
//function : Extrema_GenExtCC
//purpose  : 
//=======================================================================
Extrema_GenExtCC::Extrema_GenExtCC()
: myIsFindSingleSolution(Standard_False),
  myParallel(Standard_False),
  myCurveMinTol(Precision::PConfusion()),
  myLowBorder(1,2),
  myUppBorder(1,2),
  myDone(Standard_False)
{
  myC[0] = myC[1] = 0;
}

//=======================================================================
//function : Extrema_GenExtCC
//purpose  : 
//=======================================================================
Extrema_GenExtCC::Extrema_GenExtCC(const Curve1& C1,
                                   const Curve2& C2)
: myIsFindSingleSolution(Standard_False),
  myParallel(Standard_False),
  myCurveMinTol(Precision::PConfusion()),
  myLowBorder(1,2),
  myUppBorder(1,2),
  myDone(Standard_False)
{
  myC[0] = (Standard_Address)&C1;
  myC[1] = (Standard_Address)&C2;
  myLowBorder(1) = C1.FirstParameter();
  myLowBorder(2) = C2.FirstParameter();
  myUppBorder(1) = C1.LastParameter();
  myUppBorder(2) = C2.LastParameter();
}

//=======================================================================
//function : Extrema_GenExtCC
//purpose  : 
//=======================================================================
Extrema_GenExtCC::Extrema_GenExtCC(const Curve1& C1,
                                   const Curve2& C2,
                                   const Standard_Real Uinf,
                                   const Standard_Real Usup,
                                   const Standard_Real Vinf,
                                   const Standard_Real Vsup)
: myIsFindSingleSolution(Standard_False),
  myParallel(Standard_False),
  myCurveMinTol(Precision::PConfusion()),
  myLowBorder(1,2),
  myUppBorder(1,2),
  myDone(Standard_False)
{
  myC[0] = (Standard_Address)&C1;
  myC[1] = (Standard_Address)&C2;
  myLowBorder(1) = Uinf;
  myLowBorder(2) = Vinf;
  myUppBorder(1) = Usup;
  myUppBorder(2) = Vsup;
}

//=======================================================================
//function : SetParams
//purpose  : 
//=======================================================================
void Extrema_GenExtCC::SetParams(const Curve1& C1,
                                 const Curve2& C2,
                                 const Standard_Real Uinf,
                                 const Standard_Real Usup,
                                 const Standard_Real Vinf,
                                 const Standard_Real Vsup)
{
  myC[0] = (Standard_Address)&C1;
  myC[1] = (Standard_Address)&C2;
  myLowBorder(1) = Uinf;
  myLowBorder(2) = Vinf;
  myUppBorder(1) = Usup;
  myUppBorder(2) = Vsup;
}

//=======================================================================
//function : SetTolerance
//purpose  : 
//=======================================================================
void Extrema_GenExtCC::SetTolerance(Standard_Real theTol)
{
  myCurveMinTol = theTol;
}

//=======================================================================
//function : Perform
//purpose  : 
//=======================================================================
void Extrema_GenExtCC::Perform()
{
  myDone = Standard_False;
  myParallel = Standard_False;

  Curve1 &C1 = *(Curve1*)myC[0];
  Curve2 &C2 = *(Curve2*)myC[1];

  Standard_Integer aNbInter[2];
  GeomAbs_Shape aContinuity = GeomAbs_C2;
  aNbInter[0] = C1.NbIntervals(aContinuity);
  aNbInter[1] = C2.NbIntervals(aContinuity);

  if (aNbInter[0] * aNbInter[1] > 100)
  {
    aContinuity = GeomAbs_C1;
    aNbInter[0] = C1.NbIntervals(aContinuity);
    aNbInter[1] = C2.NbIntervals(aContinuity);
  }

  Standard_Real anL[2];
  Standard_Integer indmax = -1, indmin = -1;
  const Standard_Real mult = 20.;
  if (!(Precision::IsInfinite(C1.FirstParameter()) || Precision::IsInfinite(C1.LastParameter()) ||
        Precision::IsInfinite(C2.FirstParameter()) || Precision::IsInfinite(C2.LastParameter())))
  {
    anL[0] = GCPnts_AbscissaPoint::Length(C1);
    anL[1] = GCPnts_AbscissaPoint::Length(C2);
    if (anL[0] / aNbInter[0] > mult * anL[1] / aNbInter[1])
    {
      indmax = 0;
      indmin = 1;
    }
    else if (anL[1] / aNbInter[1] > mult * anL[0] / aNbInter[0])
    {
      indmax = 1;
      indmin = 0;
    }
  }
  Standard_Integer aNbIntOpt = 0;
  if (indmax >= 0)
  {
    aNbIntOpt = RealToInt(anL[indmax] * aNbInter[indmin] / anL[indmin] / (mult / 4.))  + 1;
    if (aNbIntOpt > 100 || aNbIntOpt < aNbInter[indmax])
    {
      indmax = -1;
    }
    else
    {
      if (aNbIntOpt * aNbInter[indmin] > 100)
      {
        aNbIntOpt = 100 / aNbInter[indmin];
        if (aNbIntOpt < aNbInter[indmax])
        {
          indmax = -1;
        }
      }
    }
  }

  Handle(TColStd_HArray1OfReal) anIntervals1 = new TColStd_HArray1OfReal(1, aNbInter[0] + 1);
  Handle(TColStd_HArray1OfReal) anIntervals2 = new TColStd_HArray1OfReal(1, aNbInter[1] + 1);
  C1.Intervals(anIntervals1->ChangeArray1(), aContinuity);
  C2.Intervals(anIntervals2->ChangeArray1(), aContinuity);
  if (indmax >= 0)
  {
    if (indmax == 0)
    {
      //Change anIntervals1
      ChangeIntervals(anIntervals1, aNbIntOpt);
      aNbInter[0] = anIntervals1->Length() - 1;
    }
    else
    {
      //Change anIntervals2;
      ChangeIntervals(anIntervals2, aNbIntOpt);
      aNbInter[1] = anIntervals2->Length() - 1;
    }
  }

  // Lipchitz constant computation.
  const Standard_Real aMaxLC = 10000.;
  Standard_Real aLC = 9.0; // Default value.
  const Standard_Real aMaxDer1 = 1.0 / C1.Resolution(1.0);
  const Standard_Real aMaxDer2 = 1.0 / C2.Resolution(1.0);
  Standard_Real aMaxDer = Max(aMaxDer1, aMaxDer2) * Sqrt(2.0);
  if (aLC > aMaxDer)
    aLC = aMaxDer;

  // Change constant value according to the concrete curve types.
  Standard_Boolean isConstLockedFlag = Standard_False;
  //To prevent LipConst to became too small
  const Standard_Real aCR = 0.001;
  if (aMaxDer1 / aMaxDer < aCR || aMaxDer2 / aMaxDer < aCR)
  {
    isConstLockedFlag = Standard_True;
  }
  if (aMaxDer > aMaxLC)
  {
    aLC = aMaxLC;
    isConstLockedFlag = Standard_True;
  }
  if (C1.GetType() == GeomAbs_Line)
  {
    aMaxDer = 1.0 / C2.Resolution(1.0);
    if (aLC > aMaxDer)
    {
      isConstLockedFlag = Standard_True;
      aLC = aMaxDer;
    }
  }
  if (C2.GetType() == GeomAbs_Line)
  {
    aMaxDer = 1.0 / C1.Resolution(1.0);
    if (aLC > aMaxDer)
    {
      isConstLockedFlag = Standard_True;
      aLC = aMaxDer;
    }
  }

  Extrema_GlobOptFuncCCC2 aFunc (C1, C2);
  math_GlobOptMin aFinder(&aFunc, myLowBorder, myUppBorder, aLC);
  aFinder.SetLipConstState(isConstLockedFlag);
  aFinder.SetContinuity(aContinuity == GeomAbs_C2 ? 2 : 1);
  Standard_Real aDiscTol = 1.0e-2;
  Standard_Real aValueTol = 1.0e-2;
  Standard_Real aSameTol = myCurveMinTol / (aDiscTol);
  aFinder.SetTol(aDiscTol, aSameTol);
  aFinder.SetFunctionalMinimalValue(0.0); // Best distance cannot be lower than 0.0.

  // Size computed to have cell index inside of int32 value.
  const Standard_Real aCellSize = Max(Max(anIntervals1->Last() - anIntervals1->First(),
                                      anIntervals2->Last() - anIntervals2->First())
                                      * Precision::PConfusion() / (2.0 * Sqrt(2.0)),
                                      Precision::PConfusion());
  Extrema_CCPointsInspector anInspector(aCellSize);
  NCollection_CellFilter<Extrema_CCPointsInspector> aFilter(aCellSize);
  NCollection_Vector<gp_XY> aPnts;

  Standard_Integer i,j,k;
  math_Vector aFirstBorderInterval(1,2);
  math_Vector aSecondBorderInterval(1,2);
  Standard_Real aF = RealLast(); // Best functional value.
  Standard_Real aCurrF = RealLast(); // Current functional value computed on current interval.
  for(i = 1; i <= aNbInter[0]; i++)
  {
    for(j = 1; j <= aNbInter[1]; j++)
    {
      aFirstBorderInterval(1) = anIntervals1->Value(i);
      aFirstBorderInterval(2) = anIntervals2->Value(j); 
      aSecondBorderInterval(1) = anIntervals1->Value(i + 1);
      aSecondBorderInterval(2) = anIntervals2->Value(j + 1);

      aFinder.SetLocalParams(aFirstBorderInterval, aSecondBorderInterval);
      aFinder.Perform(GetSingleSolutionFlag());

      // Check that solution found on current interval is not worse than previous.
      aCurrF = aFinder.GetF();
      if (aCurrF >= aF + aSameTol * aValueTol)
      {
        continue;
      }

      // Clean previously computed solution if current one is better.
      if (aCurrF > aF - aSameTol * aValueTol)
      {
        if (aCurrF < aF)
          aF = aCurrF;
      }
      else
      {
        aF = aCurrF;
        aFilter.Reset(aCellSize);
        aPnts.Clear();
      }

      // Save found solutions avoiding repetitions.
      math_Vector sol(1,2);
      for(k = 1; k <= aFinder.NbExtrema(); k++)
      {
        aFinder.Points(k, sol);
        gp_XY aPnt2d(sol(1), sol(2));

        gp_XY aXYmin = anInspector.Shift(aPnt2d, -aCellSize);
        gp_XY aXYmax = anInspector.Shift(aPnt2d,  aCellSize);

        anInspector.ClearFind();
        anInspector.SetCurrent(aPnt2d);
        aFilter.Inspect(aXYmin, aXYmax, anInspector);
        if (!anInspector.isFind())
        {
          // Point is out of close cells, add new one.
          aFilter.Add(aPnt2d, aPnt2d);
          aPnts.Append(gp_XY(sol(1), sol(2)));
        }
      }
    }
  }

  const Standard_Integer aNbSol = aPnts.Length();
  if (aNbSol == 0)
  {
    // No solutions.
    myDone = Standard_False;
    return;
  }

  myDone = Standard_True;

  if (aNbSol == 1)
  {
    // Single solution
    const gp_XY& aSol = aPnts.First();
    myPoints1.Append(aSol.X());
    myPoints2.Append(aSol.Y());
    return;
  }

  // More than one solution is found.
  // Check for infinity solutions case, for this:
  // Sort points lexicographically and check midpoint between each two neighboring points.
  // If all midpoints functional value is acceptable then check the projection distances
  // of the bounding points of the curves onto the opposite curves.
  // If these distances are also acceptable set myParallel flag to true and return one solution.
  std::sort(aPnts.begin(), aPnts.end(), comp);

  // Solutions to pass into result.
  // If the parallel segment is found, save only extreme solutions on that segment.
  // The first and last solutions will always be the extreme ones, thus save them unconditionally.
  TColStd_ListOfInteger aSolutions;

  // Manages the addition of the solution into result.
  // Set it to TRUE to add the first solution.
  Standard_Boolean bSaveSolution = Standard_True;

  // Define direction of the second curve relatively the first one
  // (it will be needed for projection).
  Standard_Boolean bDirsCoinside = Standard_True;
  // Check also if the found solutions are not concentrated in one point
  // on any of the curves. And if they are, avoid marking the curves as parallel.
  Standard_Boolean bDifferentSolutions = Standard_False;

  Standard_Boolean isParallel = Standard_True;
  Standard_Real aVal = 0.0;
  math_Vector aVec(1, 2, 0.0);

  // Iterate on all solutions and collect the extreme solutions on all parallel segments.
  for (Standard_Integer anIdx = 0; anIdx < aNbSol - 1; anIdx++)
  {
    const gp_XY& aCurrent = aPnts(anIdx);
    const gp_XY& aNext    = aPnts(anIdx + 1);

    aVec(1) = (aCurrent.X() + aNext.X()) * 0.5;
    aVec(2) = (aCurrent.Y() + aNext.Y()) * 0.5;

    aFunc.Value(aVec, aVal);

    if (Abs(aVal - aF) < Precision::Confusion())
    {
      // It seems the parallel segment is found.
      // Save only extreme solutions on that segment.
      if (bSaveSolution)
      {
        // Add current solution as the beginning of the parallel segment.
        aSolutions.Append(anIdx);
        // Do not keep the next solution in current parallel segment.
        bSaveSolution = Standard_False;
      }
    }
    else
    {
      // Mid point does not satisfy the tolerance criteria, curves are not parallel.
      isParallel = Standard_False;
      // Add current solution as the last one in previous parallel segment.
      aSolutions.Append(anIdx);
      // Save also the next solution as the first one in next parallel segment.
      bSaveSolution = Standard_True;
    }

    if (!bDifferentSolutions)
    {
      if (aNext.X() > aCurrent.X())
      {
        if (aNext.Y() > aCurrent.Y())
        {
          bDifferentSolutions = Standard_True;
          bDirsCoinside = Standard_True;
        }
        else if (aNext.Y() < aCurrent.Y())
        {
          bDifferentSolutions = Standard_True;
          bDirsCoinside = Standard_False;
        }
      }
    }
  }
  // Save the last solution
  aSolutions.Append(aNbSol - 1);

  if (!bDifferentSolutions)
    isParallel = Standard_False;

  if (isParallel)
  {
    // For the check on parallel case it is also necessary to check additionally
    // if the ends of the curves do not diverge. For this, project the bounding
    // points of the curves on the opposite curves and check the distances.

    Standard_Real aT1[2] = {myLowBorder(1), myUppBorder(1)};
    Standard_Real aT2[2] = {bDirsCoinside ? myLowBorder(2) : myUppBorder(2),
                            bDirsCoinside ? myUppBorder(2) : myLowBorder(2)};

    Extrema_GExtPC anExtPC1, anExtPC2;
    anExtPC1.Initialize(C1, myLowBorder(1), myUppBorder(1));
    anExtPC2.Initialize(C2, myLowBorder(2), myUppBorder(2));

    for (Standard_Integer iT = 0; isParallel && (iT < 2); ++iT)
    {
      Standard_Real aDist1 = ProjPOnC(C1.Value(aT1[iT]), anExtPC2);
      Standard_Real aDist2 = ProjPOnC(C2.Value(aT2[iT]), anExtPC1);
      isParallel = (Abs(Min(aDist1, aDist2) - aF) < Precision::Confusion());
    }
  }

  if (isParallel)
  {
    // Keep only one solution
    const gp_XY& aSol = aPnts.First();
    myPoints1.Append(aSol.X());
    myPoints2.Append(aSol.Y());
    myParallel = Standard_True;
  }
  else
  {
    // Keep all saved solutions
    TColStd_ListIteratorOfListOfInteger aItSol(aSolutions);
    for (; aItSol.More(); aItSol.Next())
    {
      const gp_XY& aSol = aPnts(aItSol.Value());
      myPoints1.Append(aSol.X());
      myPoints2.Append(aSol.Y());
    }
  }
}

//=======================================================================
//function : IsDone
//purpose  : 
//=======================================================================
Standard_Boolean Extrema_GenExtCC::IsDone() const 
{
  return myDone; 
}

//=======================================================================
//function : IsParallel
//purpose  : 
//=======================================================================
Standard_Boolean Extrema_GenExtCC::IsParallel() const 
{
  if (!IsDone()) throw StdFail_NotDone();
  return myParallel;
}

//=======================================================================
//function : NbExt
//purpose  : 
//=======================================================================
Standard_Integer Extrema_GenExtCC::NbExt() const
{
  if (!IsDone()) throw StdFail_NotDone();
  return myPoints1.Length();
}

//=======================================================================
//function : SquareDistance
//purpose  : 
//=======================================================================
Standard_Real Extrema_GenExtCC::SquareDistance(const Standard_Integer N) const
{
  if (N < 1 || N > NbExt())
  {
    throw Standard_OutOfRange();
  }

  return Tool1::Value(*((Curve1*)myC[0]), myPoints1(N)).SquareDistance(Tool2::Value(*((Curve2*)myC[1]), myPoints2(N)));
}

//=======================================================================
//function : Points
//purpose  : 
//=======================================================================
void Extrema_GenExtCC::Points(const Standard_Integer N,
                              POnC& P1,
                              POnC& P2) const
{
  if (N < 1 || N > NbExt())
  {
    throw Standard_OutOfRange();
  }

  P1.SetValues(myPoints1(N), Tool1::Value(*((Curve1*)myC[0]), myPoints1(N)));
  P2.SetValues(myPoints2(N), Tool2::Value(*((Curve2*)myC[1]), myPoints2(N)));
}

//=======================================================================
//function : SetSingleSolutionFlag
//purpose  : 
//=======================================================================
void Extrema_GenExtCC::SetSingleSolutionFlag(const Standard_Boolean theFlag)
{
  myIsFindSingleSolution = theFlag;
}

//=======================================================================
//function : GetSingleSolutionFlag
//purpose  : 
//=======================================================================
Standard_Boolean Extrema_GenExtCC::GetSingleSolutionFlag() const
{
  return myIsFindSingleSolution;
}