// File:	NLPlate_NLPlate.cxx
// Created:	Thu Apr  9 18:33:41 1998
// Author:	Andre LIEUTIER
//		<alr@sgi63>


#include <math_Matrix.hxx>
#include <math_Vector.hxx>
#include <math_Gauss.hxx>
#include <gp_Vec.hxx>
#include <gp_Pnt.hxx>

#include <Plate_D1.hxx>
#include <Plate_D2.hxx>
#include <Plate_D3.hxx>
#include <Plate_Plate.hxx>
#include <Plate_PinpointConstraint.hxx>
#include <Plate_FreeGtoCConstraint.hxx>

#include <NLPlate_StackIteratorOfStackOfPlate.hxx>

#include <NLPlate_NLPlate.ixx>


NLPlate_NLPlate::NLPlate_NLPlate(const Handle(Geom_Surface)& InitialSurface) :
 myInitialSurface(InitialSurface),OK(Standard_False) {}
//=======================================================================

 void NLPlate_NLPlate::Load(const Handle(NLPlate_HGPPConstraint)& GConst) 
{
  if(!GConst.IsNull()) myHGPPConstraints.Append(GConst);
  OK = Standard_False;
}

//=======================================================================
//function : Solve
//purpose  : 
//=======================================================================
 void NLPlate_NLPlate::Solve(const Standard_Integer ord, const Standard_Integer InitialConsraintOrder) 
{
  Standard_Integer maxOrder = MaxActiveConstraintOrder();
  Standard_Integer ordre = ord;
  if(ordre<maxOrder+2) ordre = maxOrder+2;

  for(Standard_Integer iterOrder=InitialConsraintOrder;iterOrder<=maxOrder;iterOrder++)
    {
     if(!Iterate(iterOrder,ordre+iterOrder-maxOrder))
       {
	OK = Standard_False;
	break;
       }
    }
  OK = Standard_True;
}
//=======================================================================
//function : Solve2
//purpose  : 
//=======================================================================
 void NLPlate_NLPlate::Solve2(const Standard_Integer ord, const Standard_Integer InitialConsraintOrder) 
{
  Standard_Integer maxOrder = MaxActiveConstraintOrder();
  Standard_Integer ordre = ord;
  if(ordre<maxOrder+2) ordre = maxOrder+2;
  if(Iterate(0,ord)) 
    {
      mySOP.ChangeTop().SetPolynomialPartOnly(Standard_True);
      ConstraintsSliding();
    }
      

  for(Standard_Integer iterOrder=InitialConsraintOrder;iterOrder<=maxOrder;iterOrder++)
    {
     if(!Iterate(iterOrder,ordre+iterOrder-maxOrder))
       {
	OK = Standard_False;
	break;
       }
    }
  OK = Standard_True;
}

//=======================================================================
//function : IncrementalSolve
//purpose  : 
//=======================================================================
 void NLPlate_NLPlate::IncrementalSolve(const Standard_Integer ord, const Standard_Integer InitialConsraintOrder,
			     const Standard_Integer NbIncrements, const Standard_Boolean UVSliding) 
{
  Standard_Integer maxOrder = MaxActiveConstraintOrder();
  Standard_Integer ordre = ord;
  if(ordre<maxOrder+2) ordre = maxOrder+2;
  Standard_Real IncrementalLoad = 1.;

  for(Standard_Integer increment=0;increment < NbIncrements;increment++)
    {
      IncrementalLoad = 1./Standard_Real(NbIncrements-increment);
//      for(Standard_Integer iterOrder=InitialConsraintOrder;iterOrder<=maxOrder;iterOrder++)
      Standard_Integer iterOrder=maxOrder;
	{
	  if(!Iterate(iterOrder,ordre+iterOrder-maxOrder,IncrementalLoad))
	    {
	      OK = Standard_False;
	      return;
	    }
	}
      if(UVSliding) ConstraintsSliding();
    }
  OK = Standard_True;
}

//=======================================================================
 Standard_Boolean NLPlate_NLPlate::IsDone() const
{
  return OK;
}
//=======================================================================

 void NLPlate_NLPlate::destroy() 
{
  Init();
}
//=======================================================================

 void NLPlate_NLPlate::Init() 
{
  mySOP.Clear();
  myHGPPConstraints.Clear();
}
//=======================================================================

 gp_XYZ NLPlate_NLPlate::Evaluate(const gp_XY& point2d) const
{
  return EvaluateDerivative(point2d,0,0);
}
//=======================================================================

 gp_XYZ NLPlate_NLPlate::EvaluateDerivative(const gp_XY& point2d,const Standard_Integer iu,const Standard_Integer iv) const
{
  gp_XYZ Value(0.,0.,0.);
  if((iu==0)&&(iv==0))
    Value = myInitialSurface->Value(point2d.X(),point2d.Y()).XYZ();
  else
    Value = myInitialSurface->DN(point2d.X(),point2d.Y(),iu,iv).XYZ();

  for(NLPlate_StackIteratorOfStackOfPlate SI(mySOP);SI.More();SI.Next())
    {
      if(SI.Value().IsDone())
	Value += SI.Value().EvaluateDerivative(point2d,iu,iv);
    }
  return Value;
}
//=======================================================================

 Standard_Integer NLPlate_NLPlate::Continuity() const
{
  Standard_Integer cont ;
  for( cont=-1; cont<10;cont++)
    {
      if(!(myInitialSurface->IsCNu(cont+1)&&myInitialSurface->IsCNv(cont+1)))break;
    }
  for(NLPlate_StackIteratorOfStackOfPlate SI(mySOP);SI.More();SI.Next())
    {
      if((SI.Value().IsDone())&&(cont > SI.Value().Continuity())) cont = SI.Value().Continuity();
    }
  return cont;
}
//=======================================================================

Standard_Boolean NLPlate_NLPlate::Iterate(const Standard_Integer ConstraintOrder,
const Standard_Integer ResolutionOrder,
const Standard_Real IncrementalLoading) 
{
  Plate_Plate EmptyPlate;
  mySOP.Push(EmptyPlate);
  Plate_Plate &TopP = mySOP.ChangeTop();
  for(Standard_Integer index =1; index <= myHGPPConstraints.Length(); index++)
    {
      const Handle(NLPlate_HGPPConstraint) &HGPP = myHGPPConstraints(index);
      Standard_Integer Order = HGPP->ActiveOrder();
      if(ConstraintOrder<Order) Order = ConstraintOrder;
      const gp_XY &UV = HGPP->UV();
      
      if((Order >=0)&& HGPP->IsG0())
	{
	  if(HGPP->IncrementalLoadAllowed())
	    TopP.Load(Plate_PinpointConstraint(UV, (HGPP->G0Target()-Evaluate(UV))*IncrementalLoading));
	  else
	    TopP.Load(Plate_PinpointConstraint(UV, HGPP->G0Target()-Evaluate(UV)));
	}
      
      if((IncrementalLoading != 1.) && HGPP->IncrementalLoadAllowed() && (Order>=1))
	{
	  switch(Order)
	    {
	    case 1:
	      {
		Plate_D1 D1S(EvaluateDerivative(UV,1,0),EvaluateDerivative(UV,0,1));
		TopP.Load(Plate_FreeGtoCConstraint(UV,D1S,HGPP->G1Target(),IncrementalLoading,HGPP->Orientation()));
	      }
	      break;
	    case 2:
	      {
		Plate_D1 D1S(EvaluateDerivative(UV,1,0),EvaluateDerivative(UV,0,1));
		Plate_D2 D2S(EvaluateDerivative(UV,2,0),EvaluateDerivative(UV,1,1),EvaluateDerivative(UV,0,2));
		TopP.Load(Plate_FreeGtoCConstraint(UV,D1S,HGPP->G1Target(),D2S,HGPP->G2Target(),
						   IncrementalLoading,HGPP->Orientation()));
	      }
	      break;
	    case 3:
	      {
		Plate_D1 D1S(EvaluateDerivative(UV,1,0),EvaluateDerivative(UV,0,1));
		Plate_D2 D2S(EvaluateDerivative(UV,2,0),EvaluateDerivative(UV,1,1),EvaluateDerivative(UV,0,2));
		Plate_D3 D3S(EvaluateDerivative(UV,3,0),EvaluateDerivative(UV,2,1),
			     EvaluateDerivative(UV,1,2),EvaluateDerivative(UV,0,3));
		TopP.Load(Plate_FreeGtoCConstraint(UV,D1S,HGPP->G1Target(),D2S,HGPP->G2Target(),D3S,HGPP->G3Target(),
						   IncrementalLoading,HGPP->Orientation()));
	      }
	      break;
	    default:
	      break;
	    }
	}
      else
	{
	  switch(Order)
	    {
	    case 1:
	      {
		Plate_D1 D1S(EvaluateDerivative(UV,1,0),EvaluateDerivative(UV,0,1));
		TopP.Load(Plate_FreeGtoCConstraint(UV,D1S,HGPP->G1Target()));
	      }
	      break;
	    case 2:
	      {
		Plate_D1 D1S(EvaluateDerivative(UV,1,0),EvaluateDerivative(UV,0,1));
		Plate_D2 D2S(EvaluateDerivative(UV,2,0),EvaluateDerivative(UV,1,1),EvaluateDerivative(UV,0,2));
		TopP.Load(Plate_FreeGtoCConstraint(UV,D1S,HGPP->G1Target(),D2S,HGPP->G2Target()));
	      }
	      break;
	    case 3:
	      {
		Plate_D1 D1S(EvaluateDerivative(UV,1,0),EvaluateDerivative(UV,0,1));
		Plate_D2 D2S(EvaluateDerivative(UV,2,0),EvaluateDerivative(UV,1,1),EvaluateDerivative(UV,0,2));
		Plate_D3 D3S(EvaluateDerivative(UV,3,0),EvaluateDerivative(UV,2,1),
			     EvaluateDerivative(UV,1,2),EvaluateDerivative(UV,0,3));
		TopP.Load(Plate_FreeGtoCConstraint(UV,D1S,HGPP->G1Target(),D2S,HGPP->G2Target(),D3S,HGPP->G3Target()));
	      }
	      break;
	    default:
	      break;
	    }
	}
    }
  
  TopP.SolveTI(ResolutionOrder);
  if(!TopP.IsDone())
    {
      mySOP.Pop();
      return Standard_False;
    }
  else
    return Standard_True;
}
//=======================================================================

 void NLPlate_NLPlate::ConstraintsSliding(const Standard_Integer NbIterations) 
{
  for(Standard_Integer index =1; index <= myHGPPConstraints.Length(); index++)
    {
      const Handle(NLPlate_HGPPConstraint) &HGPP = myHGPPConstraints(index);
      if(HGPP->UVFreeSliding() && HGPP->IsG0())
	{
	  gp_XY UV = HGPP->UV();
	  gp_XYZ P0 = Evaluate(UV);
	  const gp_XYZ P1 = HGPP->G0Target();
	  for(Standard_Integer iter=1;iter<=NbIterations;iter++)
	    {
	      // on itere au premier ordre, ce qui suffit si on est assez pres de la surface ??
	      gp_XYZ DP = P1 - P0;
	      gp_XYZ Du = EvaluateDerivative(UV,1,0);
	      gp_XYZ Dv = EvaluateDerivative(UV,0,1);
	      math_Matrix mat(0,1,0,1);
	      mat(0,0) = Du*Du;
	      mat(0,1) = Du*Dv;
	      mat(1,0) = Du*Dv;
	      mat(1,1) = Dv*Dv;
	      math_Gauss gauss(mat);
	      if(!gauss.IsDone()) break;

	      math_Vector vec(0,1);
	      vec(0) = Du*DP;
	      vec(1) = Dv*DP;
	      math_Vector sol(0,1);
	      gauss.Solve(vec,sol);
	      UV.SetX(UV.X()+sol(0));
	      UV.SetY(UV.Y()+sol(1));
	      P0 = Evaluate(UV);
	    }
	  HGPP->SetUV(UV);
	}
    }
}

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

 Standard_Integer NLPlate_NLPlate::MaxActiveConstraintOrder() const
{
  Standard_Integer MaxOrder = -1;
  for(Standard_Integer index =1; index <= myHGPPConstraints.Length(); index++)
    {
      Standard_Integer CAOrder = myHGPPConstraints(index)->ActiveOrder();
      if(CAOrder > MaxOrder) MaxOrder = CAOrder;
    }
  return MaxOrder;
}