// Copyright (c) 1995-1999 Matra Datavision
// Copyright (c) 1999-2012 OPEN CASCADE SAS
//
// The content of this file is subject to the Open CASCADE Technology Public
// License Version 6.5 (the "License"). You may not use the content of this file
// except in compliance with the License. Please obtain a copy of the License
// at http://www.opencascade.org and read it completely before using this file.
//
// The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
//
// The Original Code and all software distributed under the License is
// distributed on an "AS IS" basis, without warranty of any kind, and the
// Initial Developer hereby disclaims all such warranties, including without
// limitation, any warranties of merchantability, fitness for a particular
// purpose or non-infringement. Please see the License for the specific terms
// and conditions governing the rights and limitations under the License.

#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);
    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_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_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)
    Standard_TypeMismatch::Raise("No init");
  
  myU = U;
  Vec D1c;
  Tool::D1(*((Curve*)myC),myU,myPc,D1c);
  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)
    {
#ifdef DEB
    cout << "+++Function Extrema_FuncExtPC::Value(...)." << endl;
    cout << "Warning: 1st derivative is equal to zero!---"<<endl;
#endif
    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) Standard_TypeMismatch::Raise();
  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)
    Standard_TypeMismatch::Raise("No init");
    
  Pnt myPc_old = myPc, myP_old = myP;

  if(Value(U,F) == Standard_False)
    {
#ifdef DEB
    cout << "+++Function Extrema_FuncExtPC::Values(...)." << endl;
    cout << "Warning: No function value found!---"<<endl;
#endif

    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, U2 = myU+aDelta, U3 = myU+2*aDelta;
      
      if(!((Value(U2,F2)) && (Value(U3,F3))))
        {
#ifdef DEB
        cout << "+++ Function Extrema_FuncExtPC::Values(...)" << endl;
        cout << "There are many points close to singularity points "
            "and which have zero-derivative." << endl;
        cout << "Try to decrease aDelta variable's value. ---" << endl;
#endif
        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-2*aDelta, U2 = myU-aDelta, U3 = myU;
      
      if(!((Value(U2,F2)) && (Value(U1,F1))))
        {
#ifdef DEB
        cout << "+++ Function Extrema_FuncExtPC::Values(...)" << endl;
        cout << "There are many points close to singularity points "
            "and which have zero-derivative." << endl;
        cout << "Try to decrease aDelta variable's value. ---" << endl;
#endif
        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) Standard_TypeMismatch::Raise();
  mySqDist.Append(myPc.SquareDistance(myP));
  Standard_Integer IntVal;
  if (!myD1Init) {
    myD1Init = Standard_True;
    Standard_Real FF, DD;
    Values(myU, FF, DD);
  }
  if (!myD1Init) IntVal = 0;
  else {
    if (myD1f > 0.) { IntVal = 1; }
    else { IntVal = 0; }
  }
  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) Standard_TypeMismatch::Raise();
  return mySqDist.Value(N);
}
//=============================================================================
Standard_Boolean Extrema_FuncExtPC::IsMin (const Standard_Integer N) const
{
  if (!myPinit || !myCinit) Standard_TypeMismatch::Raise();
  return (myIsMin.Value(N) == 1);
}
//=============================================================================
POnC Extrema_FuncExtPC::Point (const Standard_Integer N) const
{
  if (!myPinit || !myCinit) Standard_TypeMismatch::Raise();
  return myPoint.Value(N);
}
//=============================================================================

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