// Created on: 1996-01-22
// Created by: Philippe MANGIN
// Copyright (c) 1996-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.

#ifndef OCCT_DEBUG
#define No_Standard_RangeError
#define No_Standard_OutOfRange
#endif


#include <FairCurve_Energy.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_Vec2d.hxx>
#include <math_IntegerVector.hxx>
#include <math_Matrix.hxx>

//=======================================================================
FairCurve_Energy::FairCurve_Energy(const Handle(TColgp_HArray1OfPnt2d)& Poles, 
				   const Standard_Integer ContrOrder1, 
				   const Standard_Integer ContrOrder2, 
				   const Standard_Boolean WithAuxValue,
				   const Standard_Real Angle1, 
				   const Standard_Real Angle2,
				   const Standard_Integer Degree,
				   const Standard_Real Curvature1,
				   const Standard_Real Curvature2 )
//=======================================================================
                         : MyPoles (Poles), 
                           MyContrOrder1(ContrOrder1),     
			   MyContrOrder2(ContrOrder2),
                           MyWithAuxValue(WithAuxValue), 
                           MyNbVar(2*(Poles->Length()-2) - ContrOrder2 - ContrOrder1 + WithAuxValue),
                           MyNbValues(2*Poles->Length() +  WithAuxValue),
                           MyLinearForm(0, 1),
                           MyQuadForm(0, 1),
			   MyGradient( 0, MyNbValues),
                           MyHessian( 0, MyNbValues + MyNbValues*(MyNbValues+1)/2 )
{
  // chesk angles in reference (Ox,Oy)
  gp_XY L0 (Cos(Angle1), Sin(Angle1)), L1 (-Cos(Angle2), Sin(Angle2));
  MyLinearForm.SetValue(0, L0);
  MyLinearForm.SetValue(1, L1);
  gp_XY Q0(-Sin(Angle1), Cos(Angle1)), Q1 (Sin(Angle2), Cos(Angle2));
  MyQuadForm.SetValue(0, ((double)Degree) / (Degree-1) * Curvature1 * Q0);
  MyQuadForm.SetValue(1, ((double)Degree) / (Degree-1) * Curvature2 * Q1);
}

//=======================================================================
Standard_Boolean FairCurve_Energy::Value(const math_Vector& X, 
						 Standard_Real& E)
//=======================================================================
{
   Standard_Boolean IsDone;
   math_Vector Energie(0,0);
   ComputePoles(X);
   IsDone = Compute(0, Energie);
   E  = Energie(0);
   return IsDone;
}

//=======================================================================
Standard_Boolean FairCurve_Energy::Gradient(const math_Vector& X,
						  math_Vector& G)
//=======================================================================
{
   Standard_Boolean IsDone;
   Standard_Real E;
 
   IsDone = Values(X, E, G);
   return IsDone;    
}

//=======================================================================
void FairCurve_Energy::Gradient1(const math_Vector& Vect,
				       math_Vector& Grad)
//=======================================================================
{
  Standard_Integer ii,
                   DebG = Grad.Lower(), FinG = Grad.Upper();
  Standard_Integer Vdeb = 3, 
                   Vfin = 2*MyPoles->Length()-2;

// .... by calculation 
  if (MyContrOrder1 >= 1) {
     gp_XY DPole (Vect(Vdeb), Vect(Vdeb+1));
     Grad(DebG) = MyLinearForm(0) * DPole;
     Vdeb += 2;
     DebG += 1;
  }
  if(MyContrOrder1 == 2) {
     Standard_Real Lambda0 = MyPoles->Value(MyPoles->Lower())
                            .Distance( MyPoles->Value(MyPoles->Lower()+1) );
     gp_XY DPole (Vect(Vdeb), Vect(Vdeb+1));
     Grad(DebG-1) += (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)) * DPole;
     Grad(DebG) = MyLinearForm(0) * DPole;
     Vdeb += 2;
     DebG += 1;
  }
  if (MyWithAuxValue) { 
     Grad(FinG) = Vect( 2*MyPoles->Length()+1 );
     FinG -= 1;
  }  
  if (MyContrOrder2 >= 1) {
     gp_XY DPole (Vect(Vfin-1), Vect(Vfin));
     Grad(FinG) = MyLinearForm(1) * DPole;
     FinG -= 1;
  }
  if(MyContrOrder2 == 2) {
     Standard_Real Lambda1 = MyPoles->Value(MyPoles->Upper()) 
                            .Distance(MyPoles->Value(MyPoles->Upper()-1) );
     gp_XY DPole (Vect(Vfin-3), Vect(Vfin-2));
     Grad(FinG) =  Grad(FinG+1) +  (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)) * DPole;
     Grad(FinG+1) = MyLinearForm(1) * DPole;     
     FinG -= 1;
  }
// ... by recopy
   for (ii=DebG; ii<=FinG; ii++) {
     Grad(ii) = Vect(Vdeb);
     Vdeb += 1;
   }
}

//=======================================================================
Standard_Boolean FairCurve_Energy::Values(const math_Vector& X, 
					        Standard_Real& E, 
					        math_Vector& G)
//=======================================================================
{
   Standard_Boolean IsDone;

   ComputePoles(X);
   IsDone = Compute(1, MyGradient);
   if (IsDone) {
     E = MyGradient(0);
     Gradient1(MyGradient, G);
   }
   return IsDone;
}

//=======================================================================
Standard_Boolean FairCurve_Energy::Values(const math_Vector& X, 
					        Standard_Real& E, 
						math_Vector& G, 
						math_Matrix& H)
//=======================================================================
{
   Standard_Boolean IsDone;

   ComputePoles(X);
   IsDone = Compute(2, MyHessian);
   if (IsDone) {
     E = MyHessian(0);
     Gradient1(MyHessian, G);
     Hessian1 (MyHessian, H);
   }
   return IsDone;
}


//=======================================================================
void FairCurve_Energy::Hessian1(const math_Vector& Vect,
			              math_Matrix& H)
//=======================================================================
{

  Standard_Integer ii, jj, kk, Vk;
  Standard_Integer Vdeb = 3 + 2*MyContrOrder1, 
                   Vfin = 2*MyPoles->Length() - 2*(MyContrOrder2+1),
                   Vup  = 2*MyPoles->Length()+MyWithAuxValue;
  Standard_Integer DebH = 1+MyContrOrder1, 
                   FinH = MyNbVar - MyWithAuxValue - MyContrOrder2 ;
  Standard_Real Cos0 = pow(MyLinearForm(0).X(),2),
                Sin0 = pow(MyLinearForm(0).Y(),2),
                CosSin0 = 2 * MyLinearForm(0).X() * MyLinearForm(0).Y(),
                Cos1 = pow(MyLinearForm(1).X(),2),
                Sin1 = pow(MyLinearForm(1).Y(),2),
                CosSin1 =  2 * MyLinearForm(1).X() * MyLinearForm(1).Y() ;  
  Standard_Real Lambda0=0, Lambda1=0; 

  if (MyContrOrder1 >= 1) {
     Lambda0 = MyPoles->Value(MyPoles->Lower())
              .Distance( MyPoles->Value(MyPoles->Lower()+1) );}
      
  if (MyContrOrder2 >= 1) {
     Lambda1 = MyPoles->Value(MyPoles->Upper()) 
              .Distance(MyPoles->Value(MyPoles->Upper()-1) );}
 

  if (MyContrOrder1 >= 1) {

// calculate the left lambda column --------------------------------

     jj =  Vdeb-2*MyContrOrder1;
     kk=Indice(jj, jj); // X2X2
     ii=Indice(jj+1, jj); // X2Y2
     H(1, 1) = Cos0 * Vect(kk) + CosSin0*Vect(ii) + Sin0 * Vect(ii+1);

     if (MyContrOrder1 >= 2) {	             
       gp_XY Laux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)); 
       jj = Vdeb-2*(MyContrOrder1-1);
       kk=Indice(jj, jj-2); // X1X2
       ii=Indice(jj+1, jj-2);  //X1Y2
       gp_XY Aux(Vect(kk+2), Vect(ii+3));

       H (1, 1) += 2 * (
		   ( MyQuadForm(0).X() * Vect(5) + MyQuadForm(0).Y() * Vect(6) )
                 + ( Laux.X() * ( MyLinearForm(0).X()*Vect(kk) + MyLinearForm(0).Y()*Vect(kk+1))
                 +   Laux.Y() * ( MyLinearForm(0).X()*Vect(ii) + MyLinearForm(0).Y()*Vect(ii+1)) )
                 +   Laux.X() * Laux.Y() * Vect(ii+2) )
                 + ( Pow(Laux.X(),2) * Vect(kk+2) + Pow(Laux.Y(),2) * Vect(ii+3));
                 
       H(2,1) = (Cos0 * Vect(kk) + CosSin0*(Vect(ii)+Vect(kk+1))/2 + Sin0 * Vect(ii+1))
              +  Laux * MyLinearForm(0).Multiplied(Aux)
              + (Laux.X()*MyLinearForm(0).Y() + Laux.Y()*MyLinearForm(0).X()) * Vect(ii+2);
     }
       
      
     if (MyWithAuxValue) { 
        kk =Indice(Vup, Vdeb-2*MyContrOrder1); 
        H(MyNbVar, 1) = MyLinearForm(0).X() * Vect(kk)
                      + MyLinearForm(0).Y() * Vect(kk+1);
     }
  
     if (MyContrOrder2 >= 1) {    
        H(MyNbVar-MyWithAuxValue, 1) = 0; // correct if there are less than 3 nodes
        if (MyContrOrder2 == 2)  {H(MyNbVar-MyWithAuxValue-1, 1) = 0;}
     }


     Vk = Vdeb;
     kk = Indice(Vk, Vdeb-2*MyContrOrder1);
     for (ii=DebH; ii<=FinH; ii++) {
        H(ii, 1) = MyLinearForm(0).X() * Vect(kk)
	         + MyLinearForm(0).Y() * Vect(kk+1);
        kk += Vk;
        Vk++;
     }
   }

// calculate the left line mu ----------------------
  if (MyContrOrder1 >= 2) {
     jj = Vdeb-2*(MyContrOrder1-1);
     kk=Indice(jj, jj); // X3X3
     ii=Indice(jj+1, jj); // X3Y3
     H(2, 2) = Cos0 * Vect(kk) + CosSin0*Vect(ii) + Sin0 * Vect(ii+1);

     if (MyWithAuxValue) { 
        kk =Indice(Vup, Vdeb-2*(MyContrOrder1-1));
	gp_XY Pole (Vect(kk), Vect(kk+1));
        H(MyNbVar, 1) += (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)) * Pole;
        H(MyNbVar, 2) = MyLinearForm(0).X() * Vect(kk)
                      + MyLinearForm(0).Y() * Vect(kk+1);
     }
  
     if (MyContrOrder2 >= 1) {    
        H(MyNbVar-MyWithAuxValue, 2) = 0; // correct if there are less than 3 nodes
        if (MyContrOrder2 == 2)  {H(MyNbVar-MyWithAuxValue-1, 2) = 0;}
     }
     Vk = Vdeb;

     Standard_Real Xaux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)).X(),
                   Yaux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)).Y();

     kk = Indice(Vk, Vdeb-2*MyContrOrder1+2);
     for (ii=DebH; ii<=FinH; ii++) {
        H(ii, 2) = MyLinearForm(0).X() * Vect(kk)
	         + MyLinearForm(0).Y() * Vect(kk+1);
        H(ii, 1) +=  Xaux * Vect(kk) + Yaux*Vect(kk+1);
        kk += Vk;
        Vk++;
     }
   }
     
// calculate the right lambda line -----------------------
  if (MyContrOrder2 >= 1) {

     jj = FinH + 1;
     Vk = Vfin + 2*MyContrOrder2 - 1;
     kk = Indice(Vk, Vdeb);
     for (ii=DebH; ii<=FinH; ii++) {
       H(jj, ii) = MyLinearForm(1).X() * Vect(kk)
	         + MyLinearForm(1).Y() * Vect(kk+Vk);
       kk++;
     }

     kk = Indice(Vk, Vk);
     H(jj, jj) = Cos1 * Vect(kk) + CosSin1 * Vect(kk+Vk) + Sin1 * Vect(kk+Vk+1);

     if (MyContrOrder2 >= 2) {
     // H(jj,jj) +=
       gp_XY Laux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)); 
       jj = Vfin + 2*MyContrOrder2 - 3;
       kk=Indice(jj+2, jj); // Xn-1Xn-2
       ii=Indice(jj+3, jj);  //Yn-1Xn-2
       Standard_Integer ll = Indice(jj, jj);

       H (FinH+1, FinH+1) += 2 * (
		   ( MyQuadForm(1).X() * Vect(jj) + MyQuadForm(1).Y() * Vect(jj+1) )
                 + ( Laux.X() * ( MyLinearForm(1).X()*Vect(kk) + MyLinearForm(1).Y()*Vect(ii))
                 +   Laux.Y() * ( MyLinearForm(1).X()*Vect(kk+1) + MyLinearForm(1).Y()*Vect(ii+1)) )
                 +   Laux.X() * Laux.Y() * Vect(ll+jj) )
                 + ( Pow(Laux.X(),2) * Vect(ll) + Pow(Laux.Y(),2) * Vect(ll+jj+1));

       H(FinH+2, FinH+1) =  Cos1 * Vect(kk) + CosSin1*(Vect(ii)+Vect(kk+1))/2 + Sin1 * Vect(ii+1);
       gp_XY Aux(Vect(ll), Vect(ll+jj+1));
       H(FinH+2, FinH+1) += Laux * MyLinearForm(1).Multiplied(Aux)
                         + (Laux.X()*MyLinearForm(1).Y() + Laux.Y()*MyLinearForm(1).X()) 
			   * Vect(ll+jj);
//       H(FinH+2, FinH+1) = 0; // No better alternative. Bug in the previous expression
     }
   }

// calculate the right line mu  -----------------------
  if (MyContrOrder2 >= 2) {
     jj = FinH + 2;
     Vk = Vfin + 2*MyContrOrder2 - 3;
     kk = Indice(Vk, Vdeb);

     Standard_Real Xaux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)).X(),
                   Yaux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)).Y();
 
     for (ii=DebH; ii<=FinH; ii++) {
        H(jj, ii) = MyLinearForm(1).X() * Vect(kk)
	          + MyLinearForm(1).Y() * Vect(kk+Vk);
        // update the right line Lambda 
        H(jj-1,ii) += Xaux*Vect(kk) + Yaux*Vect(kk+Vk);
	kk++;
     }
     kk = Indice(Vk, Vk);
     ii = Indice(Vk+1, Vk);
     H(jj,jj) = Cos1*Vect(kk) + CosSin1*Vect(ii) + Sin1*Vect(ii+1);
   }

// calculate the Auxiliary Variable line -----------------------
   if (MyWithAuxValue) {

     kk = Indice(Vup, Vdeb);
     for (ii=DebH; ii<=FinH; ii++) {
        H(MyNbVar, ii) = Vect(kk);
	kk++;
     }
   
     if (MyContrOrder2 >= 1) {
       kk = Indice(Vup, Vfin+2*MyContrOrder2-1);
       H(MyNbVar, FinH+1) = 
               MyLinearForm(1).X() * Vect(kk) + MyLinearForm(1).Y() * Vect(kk+1);
     }
     if (MyContrOrder2 >= 2) {
       kk = Indice(Vup, Vfin+2*MyContrOrder2-3);
       gp_XY Pole( Vect(kk), Vect(kk+1));
       H(MyNbVar, FinH+1) +=  (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)) * Pole;
       H(MyNbVar, FinH+2) =  MyLinearForm(1) * Pole;
     }
       kk = Indice(Vup, Vup); 
       H(H.UpperRow(), H.UpperRow()) =  Vect(kk);
   }      

// recopy the internal block -----------------------------------

   kk = Indice(Vdeb, Vdeb);
   for (ii = DebH; ii <=FinH; ii++) {
     for (jj = DebH; jj<=ii; jj++) {
         H(ii,jj) = Vect(kk);
         kk++;
     }
     kk += Vdeb-1;
  }
// symmetry
   for (ii = H.LowerRow(); ii <= H.UpperRow(); ii++) 
     for (jj = ii+1; jj <= H.UpperRow(); jj++) H(ii,jj) = H(jj,ii);        
}

//=======================================================================
Standard_Boolean FairCurve_Energy::Variable(math_Vector& X) const
//======================================================================= 
{
  Standard_Integer ii,
                   IndexDeb1 = MyPoles->Lower()+1, 
                   IndexDeb2 = X.Lower(),
                   IndexFin1 = MyPoles->Upper()-1,
                   IndexFin2 = X.Upper() - MyWithAuxValue; //  decrease by 1 if the sliding is  
                                                           // free as the last value of X is reserved.
                   

// calculation of variables of constraints  
  if (MyContrOrder1 >= 1) {
     X(IndexDeb2) = MyPoles->Value(MyPoles->Lower())
                   .Distance( MyPoles->Value(MyPoles->Lower()+1) );
     IndexDeb1 += 1;
     IndexDeb2 += 1; 
  }
  if (MyContrOrder1 == 2) {
     gp_Vec2d b1b2( MyPoles->Value(MyPoles->Lower()+1),
		    MyPoles->Value(MyPoles->Lower()+2) );
     X(IndexDeb2) = b1b2.XY() * MyLinearForm(0);                   
     IndexDeb1 += 1;
     IndexDeb2 += 1; 
  }
  if (MyContrOrder2 == 2) {
     gp_Vec2d bn2bn1( MyPoles->Value(MyPoles->Upper()-2),
		      MyPoles->Value(MyPoles->Upper()-1));
     X(IndexFin2) = - bn2bn1.XY() * MyLinearForm(1);                   
     IndexFin1 -= 1;
     IndexFin2 -= 1; 
  }
  if (MyContrOrder2 >= 1) {
     X(IndexFin2) = MyPoles->Value(MyPoles->Upper()) 
                   .Distance(MyPoles->Value(MyPoles->Upper()-1) );
     IndexFin1 -= 1;
  }
 
//  Recopy of auxiliary variables is not done in the abstract class

// copy poles to variables
  for (ii=IndexDeb1; ii<=IndexFin1; ii++) {
     X(IndexDeb2)   =  MyPoles->Value(ii).X();
     X(IndexDeb2+1) =  MyPoles->Value(ii).Y();
     IndexDeb2 +=2;
  }   
  return Standard_True;      
}

//=======================================================================
void FairCurve_Energy::ComputePoles(const math_Vector& X)
//======================================================================= 
{
  Standard_Integer ii,
                   IndexDeb1 = MyPoles->Lower()+1, 
                   IndexDeb2 = X.Lower(),
                   IndexFin1 = MyPoles->Upper()-1,
                   IndexFin2 = X.Upper() - MyWithAuxValue; // decrease by 1 if the sliding is 
                                                           // is free as the last value of X is reserved.
// calculation of pole constraints
// for (ii=MyPoles->Lower();ii<=MyPoles->Upper();ii++) {
// std::cout << ii << " X = " <<  MyPoles->Value(ii).X() << 
//                " Y = " <<  MyPoles->Value(ii).Y() << std::endl;}
  
  if (MyContrOrder1 >= 1) {
     IndexDeb1 += 1;
     IndexDeb2 += 1;
     ComputePolesG1( 0, X(1), MyPoles->Value(MyPoles->Lower()), 
                              MyPoles->ChangeValue(MyPoles->Lower()+1) );
  }
  if (MyContrOrder1 == 2) {
     IndexDeb1 += 1;
     IndexDeb2 += 1;
     ComputePolesG2( 0, X(1), X(2), MyPoles->Value(MyPoles->Lower()), 
		                    MyPoles->ChangeValue(MyPoles->Lower()+2) );
  }
  if (MyContrOrder2 == 2) {
     IndexFin1 -= 1;
     IndexFin2 -= 1;
     ComputePolesG2( 1, X(IndexFin2),  X(IndexFin2+1), 
		     MyPoles->Value(MyPoles->Upper()), 
		     MyPoles->ChangeValue(MyPoles->Upper()-2) );
  } 
  if (MyContrOrder2 >= 1) {
     IndexFin1 -= 1;
     ComputePolesG1( 1, X(IndexFin2), MyPoles->Value(MyPoles->Upper()), 
                               MyPoles->ChangeValue(MyPoles->Upper()-1) );
  }

//  if (MyWithAuxValue) { MyLengthSliding = X(X.Upper()); }
// recopy others
  for (ii=IndexDeb1; ii<=IndexFin1; ii++) {
     MyPoles -> ChangeValue(ii).SetX( X(IndexDeb2) );
     MyPoles -> ChangeValue(ii).SetY( X(IndexDeb2+1) );
     IndexDeb2 += 2;
  }    
}

//=======================================================================
void FairCurve_Energy::ComputePolesG1(const Standard_Integer Side, 
				      const Standard_Real Lambda, 
				      const gp_Pnt2d& P1, 
				            gp_Pnt2d& P2) const
//======================================================================= 
{   P2.SetXY ( P1.XY() + MyLinearForm(Side) * Lambda );  }  

//=======================================================================
void FairCurve_Energy::ComputePolesG2(const Standard_Integer Side, 
				      const Standard_Real Lambda, 
				      const Standard_Real Rho,
				      const gp_Pnt2d& P1, 
				            gp_Pnt2d& P2) const
//======================================================================= 
{   P2.SetXY ( P1.XY() 
  + MyLinearForm(Side) * (Lambda + Rho ) 
  + MyQuadForm(Side)   * (Lambda * Lambda) ) ;  }