// 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 <Standard_TypeMismatch.hxx>
#include <Precision.hxx>
static const Standard_Real TolFactor = 1.e-12;
static const Standard_Real MinTol    = 1.e-20;
static const Standard_Real MinStep   = 1e-7;

static const Standard_Integer MaxOrder = 3;



/*-----------------------------------------------------------------------------
Fonction permettant de rechercher une distance extremale entre un point P et
une courbe C (en partant d'un point approche C(u0)).
Cette classe herite de math_FunctionWithDerivative et est utilisee par
les algorithmes math_FunctionRoot et math_FunctionRoots.
Si on note D1c et D2c les derivees premiere et seconde:
{ F(u) = (C(u)-P).D1c(u)/ ||Du||}
{ DF(u) = ||Du|| + (C(u)-P).D2c(u)/||Du|| - F(u)*Duu*Du/||Du||**2


{ F(u) = (C(u)-P).D1c(u) }
{ DF(u) = D1c(u).D1c(u) + (C(u)-P).D2c(u)
= ||D1c(u)|| ** 2 + (C(u)-P).D2c(u) }
----------------------------------------------------------------------------*/

Standard_Real Extrema_FuncExtPC::SearchOfTolerance()
{
  const Standard_Integer NPoint = 10;
  const Standard_Real aStep = (myUsupremum - myUinfium)/(Standard_Real)NPoint;

  Standard_Integer aNum = 0;
  Standard_Real aMax = -Precision::Infinite();  //Maximum value of 1st derivative
  //(it is computed with using NPoint point)

  do
  {
    Standard_Real u = myUinfium + aNum*aStep;    //parameter for every point
    if(u > myUsupremum)
      u = myUsupremum;

    Pnt Ptemp;  //empty point (is not used below)
    Vec VDer;   // 1st derivative vector
    Tool::D1(*((Curve*)myC), u, Ptemp, VDer);

    if(Precision::IsInfinite(VDer.X()) || Precision::IsInfinite(VDer.Y()))
    {
      continue;
    }

    Standard_Real vm = VDer.Magnitude();
    if(vm > aMax)
      aMax = vm;
  }
  while(++aNum < NPoint+1);

  return Max(aMax*TolFactor,MinTol);

}

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

Extrema_FuncExtPC::Extrema_FuncExtPC():
myU(0.),
myD1f(0.) 
{ 
  myPinit = Standard_False;
  myCinit = Standard_False;
  myD1Init = Standard_False;

  SubIntervalInitialize(0.0,0.0);
  myMaxDerivOrder = 0;
  myTol=MinTol;
}

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

Extrema_FuncExtPC::Extrema_FuncExtPC (const Pnt& P, 
                                      const Curve& C): myU(0.), myD1f(0.)
{
  myP = P;
  myC = (Standard_Address)&C;
  myPinit = Standard_True;
  myCinit = Standard_True;
  myD1Init = Standard_False;

  SubIntervalInitialize(Tool::FirstParameter(*((Curve*)myC)),
    Tool::LastParameter(*((Curve*)myC)));

  switch(Tool::GetType(*((Curve*)myC)))
  {
  case GeomAbs_BezierCurve:
  case GeomAbs_BSplineCurve:
  case GeomAbs_OffsetCurve:
  case GeomAbs_OtherCurve:
    myMaxDerivOrder = MaxOrder;
    myTol = SearchOfTolerance();
    break;
  default:
    myMaxDerivOrder = 0;
    myTol=MinTol;
    break;
  }
}
//=============================================================================

void Extrema_FuncExtPC::Initialize(const Curve& C)
{
  myC = (Standard_Address)&C;
  myCinit = Standard_True;
  myPoint.Clear();
  mySqDist.Clear();
  myIsMin.Clear();

  SubIntervalInitialize(Tool::FirstParameter(*((Curve*)myC)),
    Tool::LastParameter(*((Curve*)myC)));

  switch(Tool::GetType(*((Curve*)myC)))
  {
  case GeomAbs_BezierCurve:
  case GeomAbs_BSplineCurve:
  case GeomAbs_OffsetCurve:
  case GeomAbs_OtherCurve:
    myMaxDerivOrder = MaxOrder;
    myTol = SearchOfTolerance();
    break;
  default:
    myMaxDerivOrder = 0;
    myTol=MinTol;
    break;
  }
}

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

void Extrema_FuncExtPC::SetPoint(const Pnt& P)
{
  myP = P;
  myPinit = Standard_True;
  myPoint.Clear();
  mySqDist.Clear();
  myIsMin.Clear();
}

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


Standard_Boolean Extrema_FuncExtPC::Value (const Standard_Real U, Standard_Real& F)
{
  if (!myPinit || !myCinit)
    throw Standard_TypeMismatch("No init");

  myU = U;
  Vec D1c;
  Tool::D1(*((Curve*)myC),myU,myPc,D1c);

  if(Precision::IsInfinite(D1c.X()) || Precision::IsInfinite(D1c.Y()))
  {
    F = Precision::Infinite();
    return Standard_False;
  }

  Standard_Real Ndu = D1c.Magnitude();

  if(myMaxDerivOrder != 0)
  {
    if (Ndu <= myTol) // Cas Singulier (PMN 22/04/1998)
    {
      const Standard_Real DivisionFactor = 1.e-3;
      Standard_Real du;
      if((myUsupremum >= RealLast()) || (myUinfium <= RealFirst()))
        du = 0.0;
      else
        du = myUsupremum-myUinfium;

      const Standard_Real aDelta = Max(du*DivisionFactor,MinStep);
      //Derivative is approximated by Taylor-series

      Standard_Integer n = 1; //Derivative order
      Vec V;
      Standard_Boolean IsDeriveFound;

      do
      {
        V = Tool::DN(*((Curve*)myC),myU,++n);
        Ndu = V.Magnitude();
        IsDeriveFound = (Ndu > myTol);
      }
      while(!IsDeriveFound && n < myMaxDerivOrder);

      if(IsDeriveFound)
      {
        Standard_Real u;

        if(myU-myUinfium < aDelta)
          u = myU+aDelta;
        else
          u = myU-aDelta;

        Pnt P1, P2;
        Tool::D0(*((Curve*)myC),Min(myU, u),P1);
        Tool::D0(*((Curve*)myC),Max(myU, u),P2);

        Vec V1(P1,P2);
        Standard_Real aDirFactor = V.Dot(V1);

        if(aDirFactor < 0.0)
          D1c = -V;
        else
          D1c = V;
      }//if(IsDeriveFound)
      else
      {
        //Derivative is approximated by three points

        Pnt Ptemp; //(0,0,0)-coordinate
        Pnt P1, P2, P3;
        Standard_Boolean IsParameterGrown;

        if(myU-myUinfium < 2*aDelta)
        {
          Tool::D0(*((Curve*)myC),myU,P1);
          Tool::D0(*((Curve*)myC),myU+aDelta,P2);
          Tool::D0(*((Curve*)myC),myU+2*aDelta,P3);
          IsParameterGrown = Standard_True;
        }
        else
        {
          Tool::D0(*((Curve*)myC),myU-2*aDelta,P1);
          Tool::D0(*((Curve*)myC),myU-aDelta,P2);
          Tool::D0(*((Curve*)myC),myU,P3);
          IsParameterGrown = Standard_False;
        }

        Vec V1(Ptemp,P1), V2(Ptemp,P2), V3(Ptemp,P3);

        if(IsParameterGrown)
          D1c=-3*V1+4*V2-V3;
        else
          D1c=V1-4*V2+3*V3;
      }
      Ndu = D1c.Magnitude();
    }//(if (Ndu <= myTol)) condition
  }//if(myMaxDerivOrder != 0)

  if (Ndu <= MinTol)
  {
    //Warning: 1st derivative is equal to zero!
    return Standard_False;
  }

  Vec PPc (myP,myPc);
  F = PPc.Dot(D1c)/Ndu;
  return Standard_True;
}

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

Standard_Boolean Extrema_FuncExtPC::Derivative (const Standard_Real U, Standard_Real& D1f)
{
  if (!myPinit || !myCinit) throw Standard_TypeMismatch();
  Standard_Real F;
  return Values(U,F,D1f);  /* on fait appel a Values pour simplifier la
                           sauvegarde de l'etat. */
}
//=============================================================================

Standard_Boolean Extrema_FuncExtPC::Values (const Standard_Real U, 
                                            Standard_Real& F,
                                            Standard_Real& D1f)
{
  if (!myPinit || !myCinit)
    throw Standard_TypeMismatch("No init");

  Pnt myPc_old = myPc, myP_old = myP;

  if(Value(U,F) == Standard_False)
  {
    //Warning: No function value found!;

    myD1Init = Standard_False;
    return Standard_False;
  }

  myU = U;
  myPc = myPc_old;
  myP = myP_old;

  Vec D1c,D2c;
  Tool::D2(*((Curve*)myC),myU,myPc,D1c,D2c);

  Standard_Real Ndu = D1c.Magnitude();
  if (Ndu <= myTol) // Cas Singulier (PMN 22/04/1998)
  {
    //Derivative is approximated by three points

    //Attention:  aDelta value must be greater than same value for "Value(...)"
    //            function to avoid of points' collisions.
    const Standard_Real DivisionFactor = 0.01;
    Standard_Real du;
    if((myUsupremum >= RealLast()) || (myUinfium <= RealFirst())) 
      du = 0.0;
    else
      du = myUsupremum-myUinfium;

    const Standard_Real aDelta = Max(du*DivisionFactor,MinStep);

    Standard_Real F1, F2, F3;

    if(myU-myUinfium < 2*aDelta)
    {
      F1=F;
      //const Standard_Real U1 = myU;
      const Standard_Real U2 = myU + aDelta;
      const Standard_Real U3 = myU + aDelta * 2.0;

      if(!((Value(U2,F2)) && (Value(U3,F3))))
      {
        //There are many points close to singularity points and
        //which have zero-derivative. Try to decrease aDelta variable's value.

        myD1Init = Standard_False;
        return Standard_False;
      }

      //After calling of Value(...) function variable myU will be redeterminated.
      //So we must return it previous value.
      D1f=(-3*F1+4*F2-F3)/(2.0*aDelta);
    }
    else
    {
      F3 = F;
      const Standard_Real U1 = myU - aDelta * 2.0;
      const Standard_Real U2 = myU - aDelta;
      //const Standard_Real U3 = myU;

      if(!((Value(U2,F2)) && (Value(U1,F1))))
      {
        //There are many points close to singularity points and
        //which have zero-derivative. Try to decrease aDelta variable's value.
        myD1Init = Standard_False;
        return Standard_False;
      }
      //After calling of Value(...) function variable myU will be redeterminated.
      //So we must return it previous value.
      D1f=(F1-4*F2+3*F3)/(2.0*aDelta);
    }
    myU = U;
    myPc = myPc_old;
    myP = myP_old;
  }
  else
  {
    Vec PPc (myP,myPc);
    D1f = Ndu + (PPc.Dot(D2c)/Ndu) - F*(D1c.Dot(D2c))/(Ndu*Ndu);
  }

  myD1f = D1f;

  myD1Init = Standard_True;
  return Standard_True;
}
//=============================================================================

Standard_Integer Extrema_FuncExtPC::GetStateNumber ()
{
  if (!myPinit || !myCinit) throw Standard_TypeMismatch();
  mySqDist.Append(myPc.SquareDistance(myP));

  // It is necessary to always compute myD1f.
  myD1Init = Standard_True;
  Standard_Real FF, DD;
  Values(myU, FF, DD);

  Standard_Integer IntVal = 0;
  if (myD1f > 0.0)
  {
    IntVal = 1;
  }

  myIsMin.Append(IntVal);
  myPoint.Append(POnC(myU,myPc));
  return 0;
}
//=============================================================================

Standard_Integer Extrema_FuncExtPC::NbExt () const { return mySqDist.Length(); }
//=============================================================================

Standard_Real Extrema_FuncExtPC::SquareDistance (const Standard_Integer N) const
{
  if (!myPinit || !myCinit) throw Standard_TypeMismatch();
  return mySqDist.Value(N);
}
//=============================================================================
Standard_Boolean Extrema_FuncExtPC::IsMin (const Standard_Integer N) const
{
  if (!myPinit || !myCinit) throw Standard_TypeMismatch();
  return (myIsMin.Value(N) == 1);
}
//=============================================================================
const POnC & Extrema_FuncExtPC::Point (const Standard_Integer N) const
{
  if (!myPinit || !myCinit) throw Standard_TypeMismatch();
  return myPoint.Value(N);
}
//=============================================================================

void Extrema_FuncExtPC::SubIntervalInitialize(const Standard_Real theUfirst, const Standard_Real theUlast)
{
  myUinfium = theUfirst;
  myUsupremum = theUlast;
}