// Created on: 1998-12-21
// Created by: Igor FEOKTISTOV
// Copyright (c) 1998-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 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.

//  Modified by skv - Fri Apr  8 14:58:12 2005 OCC8559

#include <PLib_Base.hxx>
#include <PLib_JacobiPolynomial.hxx>
#include <PLib_HermitJacobi.hxx>
#include <FEmTool_Curve.hxx>
#include <math_Vector.hxx>
#include <TColStd_Array1OfReal.hxx>

//=======================================================================
//function : InitSmoothCriterion
//purpose  : Initializes the SmoothCriterion
//=======================================================================
void AppParCurves_Variational::InitSmoothCriterion()
{
  
  const Standard_Real Eps2 = 1.e-6, Eps3 = 1.e-9;
//  const Standard_Real J1 = .01, J2 = .001, J3 = .001;



  Standard_Real Length;

  InitParameters(Length);

  mySmoothCriterion->SetParameters(myParameters);

  Standard_Real E1, E2, E3;

  InitCriterionEstimations(Length, E1,  E2,  E3);
/*
  J1 = 1.e-8; J2 = J3 = (E1 + 1.e-8) * 1.e-6;

  if(E1 < J1) E1 = J1;
  if(E2 < J2) E2 = J2;
  if(E3 < J3) E3 = J3;
*/
  mySmoothCriterion->EstLength() = Length;
  mySmoothCriterion->SetEstimation(E1, E2, E3);

  Standard_Real WQuadratic, WQuality;

  if(!myWithMinMax && myTolerance != 0.) 
    WQuality = myTolerance;
  else if (myTolerance == 0.)
    WQuality = 1.;
  else
    WQuality = Max(myTolerance, Eps2 * Length);

  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  WQuadratic = Sqrt((Standard_Real)(myNbPoints - NbConstr)) * WQuality;
  if(WQuadratic > Eps3) WQuadratic = 1./ WQuadratic;

  if(WQuadratic == 0.) WQuadratic = Max(Sqrt(E1), 1.);


  mySmoothCriterion->SetWeight(WQuadratic, WQuality, 
			       myPercent[0], myPercent[1], myPercent[2]);


  Handle(PLib_Base) TheBase = new PLib_HermitJacobi(myMaxDegree, myContinuity);
  Handle(FEmTool_Curve) TheCurve;
  Standard_Integer NbElem;
  Standard_Real CurvTol = Eps2 * Length / myNbPoints;

  // Decoupe de l'intervalle en fonction des contraintes
  if(myWithCutting == Standard_True && NbConstr != 0) {

    InitCutting(TheBase, CurvTol, TheCurve);

  }
  else {

    NbElem = 1;
    TheCurve = new FEmTool_Curve(myDimension, NbElem, TheBase, CurvTol);
    TheCurve->Knots().SetValue(TheCurve->Knots().Lower(), 
			       myParameters->Value(myFirstPoint));
    TheCurve->Knots().SetValue(TheCurve->Knots().Upper(), 
			       myParameters->Value(myLastPoint));

  }
			     
  mySmoothCriterion->SetCurve(TheCurve);

  return;
  
}

//
//=======================================================================
//function : InitParameters
//purpose  : Calculation of initial estimation of the multicurve's length
//           and parameters for multipoints.
//=======================================================================
//
void AppParCurves_Variational::InitParameters(Standard_Real& Length)
{

  const Standard_Real Eps1 = Precision::Confusion() * .01;

  Standard_Real aux, dist;
  Standard_Integer i, i0, i1 = 0, ipoint;


  Length = 0.;  
  myParameters->SetValue(myFirstPoint, Length);

  for(ipoint = myFirstPoint + 1; ipoint <= myLastPoint; ipoint++) {
    i0 = i1; i1 += myDimension;
    dist = 0;
    for(i = 1; i <= myDimension; i++) {
      aux = myTabPoints->Value(i1 + i) - myTabPoints->Value(i0 + i);
      dist += aux * aux;
    }
    Length += Sqrt(dist);
    myParameters->SetValue(ipoint, Length);
  }


  if(Length <= Eps1) 
    Standard_ConstructionError::Raise("AppParCurves_Variational::InitParameters");


  for(ipoint = myFirstPoint + 1; ipoint <= myLastPoint - 1; ipoint++) 
    myParameters->SetValue(ipoint, myParameters->Value(ipoint) / Length);
  
  myParameters->SetValue(myLastPoint, 1.);
  
  // Avec peu de point il y a sans doute sous estimation ...
  if(myNbPoints < 10) Length *= (1. + 0.1 / (myNbPoints - 1));
}

//
//=======================================================================
//function : InitCriterionEstimations
//purpose  : Calculation of initial estimation of the linear criteria
//           
//=======================================================================
//
void AppParCurves_Variational::InitCriterionEstimations(const Standard_Real Length, 
							Standard_Real& E1,  
							Standard_Real& E2,  
							Standard_Real& E3) const
{
  E1 = Length * Length;

  const Standard_Real Eps1 = Precision::Confusion() * .01;

  math_Vector VTang1(1, myDimension), VTang2(1, myDimension), VTang3(1, myDimension),
              VScnd1(1, myDimension), VScnd2(1, myDimension), VScnd3(1, myDimension);

  // ========== Treatment of first point =================

  Standard_Integer ipnt = myFirstPoint;

  EstTangent(ipnt, VTang1);
  ipnt++;
  EstTangent(ipnt, VTang2);
  ipnt++;
  EstTangent(ipnt, VTang3);

  ipnt = myFirstPoint;
  EstSecnd(ipnt, VTang1, VTang2, Length, VScnd1);
  ipnt++;
  EstSecnd(ipnt, VTang1, VTang3, Length, VScnd2);

//  Modified by skv - Fri Apr  8 14:58:12 2005 OCC8559 Begin
//   Standard_Real Delta = .5 * (myParameters->Value(ipnt) - myParameters->Value(--ipnt));
  Standard_Integer anInd = ipnt;
  Standard_Real    Delta = .5 * (myParameters->Value(anInd) - myParameters->Value(--ipnt));
//  Modified by skv - Fri Apr  8 14:58:12 2005 OCC8559 End

  if(Delta <= Eps1) Delta = 1.;

  E2 = VScnd1.Norm2() * Delta;

  E3 = (Delta > Eps1) ? VScnd2.Subtracted(VScnd1).Norm2() / (4. * Delta) : 0.;
  // ========== Treatment of internal points =================

  Standard_Integer CurrPoint = 2;

  for(ipnt = myFirstPoint + 1; ipnt < myLastPoint; ipnt++) {

    Delta = .5 * (myParameters->Value(ipnt + 1) - myParameters->Value(ipnt - 1));

    if(CurrPoint == 1) {
      if(ipnt + 1 != myLastPoint) {
	EstTangent(ipnt + 2, VTang3);
	EstSecnd(ipnt + 1, VTang1, VTang3, Length, VScnd2);
      }
      else
	EstSecnd(ipnt + 1, VTang1, VTang2, Length, VScnd2);

      E2 += VScnd1.Norm2() * Delta;
      E3 += (Delta > Eps1) ? VScnd2.Subtracted(VScnd3).Norm2() / (4. * Delta) : 0.;

    }
    else if(CurrPoint == 2) {
      if(ipnt + 1 != myLastPoint) {
	EstTangent(ipnt + 2, VTang1);
	EstSecnd(ipnt + 1, VTang2, VTang1, Length, VScnd3);
      }
      else
	EstSecnd(ipnt + 1, VTang2, VTang3, Length, VScnd3);

      E2 += VScnd2.Norm2() * Delta;
      E3 += (Delta > Eps1) ? VScnd3.Subtracted(VScnd1).Norm2() / (4. * Delta) : 0.;

    }
    else {
      if(ipnt + 1 != myLastPoint) {
	EstTangent(ipnt + 2, VTang2);
	EstSecnd(ipnt + 1, VTang3, VTang2, Length, VScnd1);
      }
      else
	EstSecnd(ipnt + 1, VTang3, VTang1, Length, VScnd1);
	
      E2 += VScnd3.Norm2() * Delta;
      E3 += (Delta > Eps1) ? VScnd1.Subtracted(VScnd2).Norm2() / (4. * Delta) : 0.;

    }

    CurrPoint++; if(CurrPoint == 4) CurrPoint = 1;
  }

  // ========== Treatment of last point =================

  Delta = .5 * (myParameters->Value(myLastPoint) - myParameters->Value(myLastPoint - 1));
  if(Delta <= Eps1) Delta = 1.;

  Standard_Real aux;

  if(CurrPoint == 1) {

    E2 += VScnd1.Norm2() * Delta;
    aux = VScnd1.Subtracted(VScnd3).Norm2();
    E3 += (Delta > Eps1) ? aux / (4. * Delta) : aux;
    
  }
  else if(CurrPoint == 2) {

    E2 += VScnd2.Norm2() * Delta;
    aux = VScnd2.Subtracted(VScnd1).Norm2();
    E3 += (Delta > Eps1) ? aux / (4. * Delta) : aux;

  }
  else {
    
    E2 += VScnd3.Norm2() * Delta;
    aux = VScnd3.Subtracted(VScnd2).Norm2();
    E3 += (Delta > Eps1) ? aux / (4. * Delta) : aux;
    
  }

  aux = Length * Length;

  E2 *= aux; E3 *= aux;

  
}

//
//=======================================================================
//function : EstTangent
//purpose  : Calculation of  estimation of the Tangent
//           (see fortran routine MMLIPRI)
//=======================================================================
//

void AppParCurves_Variational::EstTangent(const Standard_Integer ipnt, 
					  math_Vector& VTang) const

{
  Standard_Integer i ;
  const Standard_Real Eps1 = Precision::Confusion() * .01;
  const Standard_Real EpsNorm = 1.e-9;

  Standard_Real Wpnt = 1.;


  if(ipnt == myFirstPoint) {
    // Estimation at first point
    if(myNbPoints < 3) 
      Wpnt = 0.;
    else {

      Standard_Integer adr1 = 1, adr2 = adr1 + myDimension, 
                       adr3 = adr2 + myDimension;

      math_Vector Pnt1((Standard_Real*)&myTabPoints->Value(adr1), 1, myDimension);
      math_Vector Pnt2((Standard_Real*)&myTabPoints->Value(adr2), 1, myDimension);
      math_Vector Pnt3((Standard_Real*)&myTabPoints->Value(adr3), 1, myDimension);

      // Parabolic interpolation
      // if we have parabolic interpolation: F(t) = A0 + A1*t + A2*t*t,
      // first derivative for t=0 is A1 = ((d2-1)*P1 + P2 - d2*P3)/(d*(1-d))
      //       d= |P2-P1|/(|P2-P1|+|P3-P2|), d2 = d*d
      Standard_Real V1 = Pnt2.Subtracted(Pnt1).Norm();
      Standard_Real V2 = 0.;
      if(V1 > Eps1) V2 = Pnt3.Subtracted(Pnt2).Norm();
      if(V2 > Eps1) {
	Standard_Real d = V1 / (V1 + V2), d1;
	d1 = 1. / (d * (1 - d)); d *= d;
	VTang = ((d - 1.) * Pnt1 + Pnt2 - d * Pnt3) * d1;
      }
      else {
	// Simple 2-point estimation

	VTang = Pnt2 - Pnt1;
      }
    }
  }
  else if (ipnt == myLastPoint) {
    // Estimation at last point
    if(myNbPoints < 3) 
      Wpnt = 0.;
    else {

      Standard_Integer adr1 = (myLastPoint - 3) * myDimension + 1, 
                       adr2 = adr1 + myDimension, 
                       adr3 = adr2 + myDimension;

      math_Vector Pnt1((Standard_Real*)&myTabPoints->Value(adr1), 1, myDimension);
      math_Vector Pnt2((Standard_Real*)&myTabPoints->Value(adr2), 1, myDimension);
      math_Vector Pnt3((Standard_Real*)&myTabPoints->Value(adr3), 1, myDimension);

      // Parabolic interpolation
      // if we have parabolic interpolation: F(t) = A0 + A1*t + A2*t*t,
      // first derivative for t=1 is 2*A2 + A1 = ((d2+1)*P1 - P2 - d2*P3)/(d*(1-d))
      //       d= |P2-P1|/(|P2-P1|+|P3-P2|), d2 = d*(d-2)
      Standard_Real V1 = Pnt2.Subtracted(Pnt1).Norm();
      Standard_Real V2 = 0.;
      if(V1 > Eps1) V2 = Pnt3.Subtracted(Pnt2).Norm();
      if(V2 > Eps1) {
	Standard_Real d = V1 / (V1 + V2), d1;
	d1 = 1. / (d * (1 - d)); d *= d - 2;
	VTang = ((d + 1.) * Pnt1 - Pnt2 - d * Pnt3) * d1;
      }
      else {
	// Simple 2-point estimation
	
	VTang = Pnt3 - Pnt2;
      }
    }
  }
  else {

    Standard_Integer adr1 = (ipnt - myFirstPoint - 1) * myDimension + 1, 
                     adr2 = adr1 + 2 * myDimension;
    
    math_Vector Pnt1((Standard_Real*)&myTabPoints->Value(adr1), 1, myDimension);
    math_Vector Pnt2((Standard_Real*)&myTabPoints->Value(adr2), 1, myDimension);

    VTang = Pnt2 - Pnt1;

  }

  Standard_Real Vnorm = VTang.Norm();
  
  if(Vnorm <= EpsNorm)
    VTang.Init(0.);
  else 
    VTang /= Vnorm;

  // Estimation with constraints
  
  Standard_Real Wcnt = 0.;
  Standard_Integer IdCnt = 1;

// Warning!! Here it is suppoused that all points are in range [myFirstPoint, myLastPoint]
  
  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  math_Vector VCnt(1, myDimension, 0.);

  if(NbConstr > 0) {

    while(myTypConstraints->Value(2 * IdCnt - 1) < ipnt &&
	  IdCnt <= NbConstr) IdCnt++;
    if((myTypConstraints->Value(2 * IdCnt - 1) == ipnt) &&
       (myTypConstraints->Value(2 * IdCnt) >= 1)) {
      Wcnt = 1.;
      Standard_Integer i0 = 2 * myDimension * (IdCnt - 1), k = 0;
      for( i = 1; i <= myNbP3d; i++) {
	for(Standard_Integer j = 1; j <= 3; j++) 
	  VCnt(++k) = myTabConstraints->Value(++i0);
	i0 += 3;
      }
      for(i = 1; i <= myNbP2d; i++) {
	for(Standard_Integer j = 1; j <= 2; j++) 
	  VCnt(++k) = myTabConstraints->Value(++i0);
	i0 += 2;
      }
    }
  }

  // Averaging of estimation

  Standard_Real Denom = Wpnt + Wcnt;
  if(Denom == 0.) Denom = 1.;
  else            Denom = 1. / Denom;

  VTang = (Wpnt * VTang + Wcnt * VCnt) * Denom;

  Vnorm = VTang.Norm();

  if(Vnorm <= EpsNorm)
    VTang.Init(0.);
  else 
    VTang /= Vnorm;

    
}

//
//=======================================================================
//function : EstSecnd
//purpose  : Calculation of  estimation of the second derivative
//           (see fortran routine MLIMSCN)
//=======================================================================
//
void AppParCurves_Variational::EstSecnd(const Standard_Integer ipnt, 
					const math_Vector& VTang1, 
					const math_Vector& VTang2, 
					const Standard_Real Length, 
					math_Vector& VScnd) const
{
  Standard_Integer i ;

  const Standard_Real Eps = 1.e-9;

  Standard_Real Wpnt = 1.;

  Standard_Real aux;

  if(ipnt == myFirstPoint)
    aux = myParameters->Value(ipnt + 1) - myParameters->Value(ipnt);
  else if(ipnt == myLastPoint)
    aux = myParameters->Value(ipnt) - myParameters->Value(ipnt - 1);
  else
    aux = myParameters->Value(ipnt + 1) - myParameters->Value(ipnt - 1);

  if(aux <= Eps)
    aux = 1.;
  else
    aux = 1. / aux;

  VScnd = (VTang2 - VTang1) * aux;
    
  // Estimation with constraints
  
  Standard_Real Wcnt = 0.;
  Standard_Integer IdCnt = 1;

// Warning!! Here it is suppoused that all points are in range [myFirstPoint, myLastPoint]

  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  math_Vector VCnt(1, myDimension, 0.);

  if(NbConstr > 0) {

    while(myTypConstraints->Value(2 * IdCnt - 1) < ipnt &&
	  IdCnt <= NbConstr) IdCnt++;

    if((myTypConstraints->Value(2 * IdCnt - 1) == ipnt) &&
       (myTypConstraints->Value(2 * IdCnt) >= 2))  {
      Wcnt = 1.;
      Standard_Integer i0 = 2 * myDimension * (IdCnt - 1) + 3, k = 0;
      for( i = 1; i <= myNbP3d; i++) {
	for(Standard_Integer j = 1; j <= 3; j++) 
	  VCnt(++k) = myTabConstraints->Value(++i0);
	i0 += 3;
      }
      i0--;
      for(i = 1; i <= myNbP2d; i++) {
	for(Standard_Integer j = 1; j <= 2; j++) 
	  VCnt(++k) = myTabConstraints->Value(++i0);
	i0 += 2;
      }
    }
  }

  // Averaging of estimation

  Standard_Real Denom = Wpnt + Wcnt;
  if(Denom == 0.) Denom = 1.;
  else            Denom = 1. / Denom;

  VScnd = (Wpnt * VScnd + (Wcnt * Length) * VCnt) * Denom;

}


//
//=======================================================================
//function : InitCutting
//purpose  : Realisation of curve's cutting a priory accordingly to 
//           constraints (see fortran routine MLICUT)
//=======================================================================
//
void AppParCurves_Variational::InitCutting(const Handle(PLib_Base)& aBase,
					   const Standard_Real CurvTol, 
					   Handle(FEmTool_Curve)& aCurve) const
{

    // Definition of number of elements
    Standard_Integer ORCMx = -1, NCont = 0, i, kk, NbElem;
    Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;

    for(i = 1; i <= NbConstr; i++) {
      kk = Abs(myTypConstraints->Value(2 * i)) + 1;
      ORCMx = Max(ORCMx, kk);
      NCont += kk;
    }

    if(ORCMx > myMaxDegree - myNivCont) 
      Standard_ConstructionError::Raise("AppParCurves_Variational::InitCutting");

    Standard_Integer NLibre = Max(myMaxDegree - myNivCont - (myMaxDegree + 1) / 4,
				  myNivCont + 1);

    NbElem = (NCont % NLibre == 0) ? NCont / NLibre : NCont / NLibre + 1;

    while((NbElem > myMaxSegment) && (NLibre < myMaxDegree - myNivCont)) {

      NLibre++;
      NbElem = (NCont % NLibre == 0) ? NCont / NLibre : NCont / NLibre + 1;

    }


    if(NbElem > myMaxSegment) 
      Standard_ConstructionError::Raise("AppParCurves_Variational::InitCutting");


    aCurve = new FEmTool_Curve(myDimension, NbElem, aBase, CurvTol);

    Standard_Integer NCnt = (NCont - 1) / NbElem + 1;
    Standard_Integer NPlus = NbElem - (NCnt * NbElem - NCont);

    TColStd_Array1OfReal& Knot = aCurve->Knots();

    Standard_Integer IDeb = 0, IFin = NbConstr + 1,
                     NDeb = 0, NFin = 0,
                     IndEl = Knot.Lower(), IUpper = Knot.Upper(),
                     NbEl = 0;


    Knot(IndEl) = myParameters->Value(myFirstPoint);
    Knot(IUpper) = myParameters->Value(myLastPoint);

    while(NbElem - NbEl > 1) {

      IndEl++; NbEl++;
      if(NPlus == 0) NCnt--;

      while(NDeb < NCnt && IDeb < IFin) {
	IDeb++;
	NDeb += Abs(myTypConstraints->Value(2 * IDeb)) + 1;
      }

      if(NDeb == NCnt) {
	NDeb = 0;
	if(NPlus == 1 && 
	   myParameters->Value(myTypConstraints->Value(2 * IDeb - 1)) > Knot(IndEl - 1))
	  
	  Knot(IndEl) = myParameters->Value(myTypConstraints->Value(2 * IDeb - 1));
	else
	  Knot(IndEl) = (myParameters->Value(myTypConstraints->Value(2 * IDeb - 1)) +
			 myParameters->Value(myTypConstraints->Value(2 * IDeb + 1))) / 2;

      }
      else {
	NDeb -= NCnt;
	Knot(IndEl) = myParameters->Value(myTypConstraints->Value(2 * IDeb - 1));
      }

      NPlus--;
      if(NPlus == 0) NCnt--;

      if(NbElem - NbEl == 1) break;

      NbEl++;

      while(NFin < NCnt && IDeb < IFin) {
	IFin--;
	NFin += Abs(myTypConstraints->Value(2 * IFin)) + 1;
      }

      if(NFin == NCnt) {
	NFin = 0;
	Knot(IUpper + 1 - IndEl) = (myParameters->Value(myTypConstraints->Value(2 * IFin - 1)) +
				myParameters->Value(myTypConstraints->Value(2 * IFin - 3))) / 2;
      }
      else {
	NFin -= NCnt;
	if(myParameters->Value(myTypConstraints->Value(2 * IFin - 1)) < Knot(IUpper - IndEl + 1))
	  Knot(IUpper + 1 - IndEl) = myParameters->Value(myTypConstraints->Value(2 * IFin - 1));
	else
	  Knot(IUpper + 1 - IndEl) = (myParameters->Value(myTypConstraints->Value(2 * IFin - 1)) +
				  myParameters->Value(myTypConstraints->Value(2 * IFin - 3))) / 2;
      }

    }


}