// Created on: 1998-03-27
// Created by: # Andre LIEUTIER
// 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 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 <math_Matrix.hxx>
#include <math_Vector.hxx>
#include <math_Gauss.hxx>

#include <Plate_FreeGtoCConstraint.ixx>

#include <gp_Pnt.hxx>
#include <gp_Dir.hxx>
#include <gp_Trsf.hxx>
#include <gp_Ax1.hxx>


static const Standard_Real NORMIN = 1.e-10;
static const Standard_Real COSMIN = 1.e-2;


// G1 Constraints  

Plate_FreeGtoCConstraint::Plate_FreeGtoCConstraint(const gp_XY& point2d,const Plate_D1& D1S,const Plate_D1& D1T,
						   const Standard_Real IncrementalLoad, const Standard_Integer orientation)
{
  pnt2d = point2d;
  nb_PPConstraints = 0;
  nb_LSConstraints = 0;

  gp_XYZ normale = D1T.Du^D1T.Dv;
  if(normale.Modulus() < NORMIN) return;
  normale.Normalize();

  if(IncrementalLoad!=1.)
    {
      gp_XYZ N0 = D1S.Du^D1S.Dv;
      if(N0.Modulus()< NORMIN) return;
      N0.Normalize();
      gp_XYZ N1 = normale;      
      if(orientation!=0) N1 *= orientation;      
      Standard_Real c = N0*N1;
      if(orientation==0)
	{
	  if (c <0.)
	    {
	      c *= -1.;
	      N1 *= -1.;
	    }
	}
      
      Standard_Real s =  N0.CrossMagnitude(N1);
      if((s < 1.e-2)&&(c<0.)) return;
      Standard_Real angle = atan2(c,s);
      //if (angle < 0.) angle += M_PI;
      
      gp_XYZ d = N0^N1;
      d.Normalize();
      gp_Dir dir = gp_Dir(d);
      gp_Trsf rota;
      gp_Ax1 Axe(gp_Pnt(0,0,0), dir);
      rota.SetRotation(Axe,angle*(IncrementalLoad-1.));
//      gp_Trsf rota = gce_MakeRotation(gp_Pnt(0,0,0), dir, angle*(IncrementalLoad-1.));
      rota.Transforms(normale);
    }
  
  gp_XYZ du = D1S.Du*(-1.);
  gp_XYZ dv = D1S.Dv*(-1.);

  myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale);
  myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale);
  nb_LSConstraints = 2;

}

// G1 + G2 Constraints  

Plate_FreeGtoCConstraint::Plate_FreeGtoCConstraint(const gp_XY& point2d,const Plate_D1& D1S,const Plate_D1& D1T0,
						   const Plate_D2& D2S,const Plate_D2& D2T0,
						   const Standard_Real IncrementalLoad, const Standard_Integer orientation)
{
  pnt2d = point2d;
  nb_PPConstraints = 0;
  nb_LSConstraints = 0;
  Plate_D1 D1T = D1T0;
  Plate_D2 D2T = D2T0;

  gp_XYZ normale = D1T.Du^D1T.Dv;
  if(normale.Modulus() < NORMIN) return;
  normale.Normalize();

// G1 Constraints
  gp_XYZ normaleS = D1S.Du^D1S.Dv;
  if(normaleS.Modulus() < NORMIN)
    {
      if(IncrementalLoad!=1.) return;
      gp_XYZ du = D1S.Du*(-1.);
      gp_XYZ dv = D1S.Dv*(-1.);

      myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale);
      myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale);
      nb_LSConstraints = 2;
      return;
    }
  normaleS.Normalize();


  if(IncrementalLoad!=1.)
    {
      gp_XYZ N0 = normaleS;
      gp_XYZ N1 = normale;      
      if(orientation!=0) N1 *= orientation;      
      Standard_Real c = N0*N1;
      if(orientation==0)
	{
	  if (c <0.)
	    {
	      c *= -1.;
	      N1 *= -1.;
	    }
	}
      
      Standard_Real s =  N0.CrossMagnitude(N1);
      if((s < 1.e-2)&&(c<0.)) return;
      Standard_Real angle = atan2(c,s);
      
      gp_XYZ d = N0^N1;
      d.Normalize();
      gp_Dir dir = gp_Dir(d);
      gp_Trsf rota;
      gp_Ax1 Axe(gp_Pnt(0,0,0), dir);
      rota.SetRotation(Axe,angle*(IncrementalLoad-1.));
//      gp_Trsf rota = gce_MakeRotation(gp_Pnt(0,0,0), dir, angle*(IncrementalLoad-1.));
      rota.Transforms(normale);
      rota.Transforms(D1T.Du);
      rota.Transforms(D1T.Dv);
      rota.Transforms(D2T.Duu);
      rota.Transforms(D2T.Duv);
      rota.Transforms(D2T.Dvv);
    }
  

  Standard_Real cos_normales = normale*normaleS;
  if( fabs(cos_normales)<COSMIN)
    {
      gp_XYZ du = D1S.Du*(-1.);
      gp_XYZ dv = D1S.Dv*(-1.);

      myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale);
      myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale);
      nb_LSConstraints = 2;
      return;
    }

  Standard_Real invcos = 1./cos_normales;

  gp_XYZ du = normaleS* -(normale*D1S.Du)*invcos;
  gp_XYZ dv = normaleS* -(normale*D1S.Dv)*invcos;

  myPPC[0] = Plate_PinpointConstraint(pnt2d, du,1,0);
  myPPC[1] = Plate_PinpointConstraint(pnt2d, dv,0,1);
  nb_PPConstraints = 2;

// G2 Constraints  
  gp_XYZ Su = D1S.Du+du;
  gp_XYZ Sv = D1S.Dv+dv;

  math_Matrix mat(0,1,0,1);
  mat(0,0) = Su*D1T.Du;
  mat(0,1) = Su*D1T.Dv;
  mat(1,0) = Sv*D1T.Du;
  mat(1,1) = Sv*D1T.Dv;
  math_Gauss gauss(mat);
  if(!gauss.IsDone()) return;

  math_Vector vec(0,1);
  vec(0) = Su*Su;
  vec(1) = Su*Sv;
  math_Vector sol(0,1);

  gauss.Solve(vec,sol);
  Standard_Real a = sol(0);
  Standard_Real b = sol(1);

  vec(0) = Sv*Su;
  vec(1) = Sv*Sv;

  gauss.Solve(vec,sol);
  Standard_Real c = sol(0);
  Standard_Real d = sol(1);

  gp_XYZ Suu = D2T.Duu*(a*a) + D2T.Duv*(2*a*b) + D2T.Dvv*(b*b);
  gp_XYZ Suv = D2T.Duu*(a*c) + D2T.Duv*(a*d+b*c) + D2T.Dvv*(b*d);
  gp_XYZ Svv = D2T.Duu*(c*c) + D2T.Duv*(2*c*d) + D2T.Dvv*(d*d);  

  gp_XYZ duu = Suu-D2S.Duu;
  gp_XYZ duv = Suv-D2S.Duv;
  gp_XYZ dvv = Svv-D2S.Dvv;
  duu *= IncrementalLoad;
  duv *= IncrementalLoad;
  dvv *= IncrementalLoad;


  myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duu,2,0), normale);
  myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duv,1,1), normale);
  myLSC[2] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dvv,0,2), normale);
  nb_LSConstraints = 3;
}

// G1 + G2 + G3 Constraints  

Plate_FreeGtoCConstraint::Plate_FreeGtoCConstraint(const gp_XY& point2d,const Plate_D1& D1S,const Plate_D1& D1T0,
						   const Plate_D2& D2S,const Plate_D2& D2T0,
						   const Plate_D3& D3S,const Plate_D3& D3T0,
						   const Standard_Real IncrementalLoad, const Standard_Integer orientation)
{
  pnt2d = point2d;
  nb_PPConstraints = 0;
  nb_LSConstraints = 0;
  Plate_D1 D1T = D1T0;
  Plate_D2 D2T = D2T0;
  Plate_D3 D3T = D3T0;

  gp_XYZ normale = D1T.Du^D1T.Dv;
  if(normale.Modulus() < NORMIN) return;
  normale.Normalize();

// G1 Constraints
  gp_XYZ normaleS = D1S.Du^D1S.Dv;
  if(normaleS.Modulus() < NORMIN)
    {
      if(IncrementalLoad!=1.) return;
      gp_XYZ du = D1S.Du*(-1.);
      gp_XYZ dv = D1S.Dv*(-1.);

      myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale);
      myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale);
      nb_LSConstraints = 2;
      return;
    }
  normaleS.Normalize();

  if(IncrementalLoad!=1.)
    {
      gp_XYZ N0 = normaleS;
      gp_XYZ N1 = normale;      
      if(orientation!=0) N1 *= orientation;      
      Standard_Real c = N0*N1;
      if(orientation==0)
	{
	  if (c <0.)
	    {
	      c *= -1.;
	      N1 *= -1.;
	    }
	}
      Standard_Real s =  N0.CrossMagnitude(N1);
      if((s < 1.e-2)&&(c<0.)) return;
      Standard_Real angle = atan2(c,s);
      
      gp_XYZ d = N0^N1;
      d.Normalize();
      gp_Dir dir = gp_Dir(d);
      gp_Trsf rota;
      gp_Ax1 Axe(gp_Pnt(0,0,0), dir);
      rota.SetRotation(Axe,angle*(IncrementalLoad-1.));
//      gp_Trsf rota = gce_MakeRotation(gp_Pnt(0,0,0), dir, angle*(IncrementalLoad-1.));
      rota.Transforms(normale);
      rota.Transforms(D1T.Du);
      rota.Transforms(D1T.Dv);
      rota.Transforms(D2T.Duu);
      rota.Transforms(D2T.Duv);
      rota.Transforms(D2T.Dvv);
      rota.Transforms(D3T.Duuu);
      rota.Transforms(D3T.Duuv);
      rota.Transforms(D3T.Duvv);
      rota.Transforms(D3T.Dvvv);
    }

  Standard_Real cos_normales = normale*normaleS;
  if( fabs(cos_normales)<COSMIN)
    {
      gp_XYZ du = D1S.Du*(-1.);
      gp_XYZ dv = D1S.Dv*(-1.);

      myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale);
      myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale);
      nb_LSConstraints = 2;
      return;
    }

  Standard_Real invcos = 1./cos_normales;

  gp_XYZ du = normaleS* -(normale*D1S.Du)*invcos;
  gp_XYZ dv = normaleS* -(normale*D1S.Dv)*invcos;

  myPPC[0] = Plate_PinpointConstraint(pnt2d, du,1,0);
  myPPC[1] = Plate_PinpointConstraint(pnt2d, dv,0,1);
  nb_PPConstraints = 2;

// G2 Constraints  
  gp_XYZ Su = D1S.Du+du;
  gp_XYZ Sv = D1S.Dv+dv;

  math_Matrix mat(0,1,0,1);
  mat(0,0) = Su*D1T.Du;
  mat(0,1) = Su*D1T.Dv;
  mat(1,0) = Sv*D1T.Du;
  mat(1,1) = Sv*D1T.Dv;
  math_Gauss gauss(mat);
  if(!gauss.IsDone()) return;

  math_Vector vec(0,1);
  vec(0) = Su*Su;
  vec(1) = Su*Sv;
  math_Vector sol(0,1);

  gauss.Solve(vec,sol);
  Standard_Real a = sol(0);
  Standard_Real b = sol(1);

  vec(0) = Sv*Su;
  vec(1) = Sv*Sv;

  gauss.Solve(vec,sol);
  Standard_Real c = sol(0);
  Standard_Real d = sol(1);

  gp_XYZ Suu = D2T.Duu*(a*a) + D2T.Duv*(2*a*b) + D2T.Dvv*(b*b);
  gp_XYZ Suv = D2T.Duu*(a*c) + D2T.Duv*(a*d+b*c) + D2T.Dvv*(b*d);
  gp_XYZ Svv = D2T.Duu*(c*c) + D2T.Duv*(2*c*d) + D2T.Dvv*(d*d);  

  gp_XYZ duu = normaleS * (normale*(Suu-D2S.Duu))*invcos;
  gp_XYZ duv = normaleS * (normale*(Suv-D2S.Duv))*invcos;
  gp_XYZ dvv = normaleS * (normale*(Svv-D2S.Dvv))*invcos;

  myPPC[2] = Plate_PinpointConstraint(pnt2d, duu,2,0);
  myPPC[3] = Plate_PinpointConstraint(pnt2d, duv,1,1);
  myPPC[4] = Plate_PinpointConstraint(pnt2d, dvv,0,2);
  nb_PPConstraints = 5;

// G3 Constraints  

  vec(0) = (D2S.Duu + duu - Suu)*Su;
  vec(1) = (D2S.Duu + duu - Suu)*Sv;
  gauss.Solve(vec,sol);
  Standard_Real B0uu = sol(0);
  Standard_Real B1uu = sol(1);

  vec(0) = (D2S.Duv + duv - Suv)*Su;
  vec(1) = (D2S.Duv + duv - Suv)*Sv;
  gauss.Solve(vec,sol);
  Standard_Real B0uv = sol(0);
  Standard_Real B1uv = sol(1);

  vec(0) = (D2S.Dvv + dvv - Svv)*Su;
  vec(1) = (D2S.Dvv + dvv - Svv)*Sv;
  gauss.Solve(vec,sol);
  Standard_Real B0vv = sol(0);
  Standard_Real B1vv = sol(1);

  gp_XYZ Suuu = D3T.Duuu*(a*a*a) + D3T.Duuv*(3*a*a*b)       + D3T.Duvv*(3*a*b*b)       + D3T.Dvvv*(b*b*b);
  gp_XYZ Suuv = D3T.Duuu*(a*a*c) + D3T.Duuv*(a*a*d+2*a*b*c) + D3T.Duvv*(b*b*c+2*a*b*d) + D3T.Dvvv*(b*b*d);
  gp_XYZ Suvv = D3T.Duuu*(a*c*c) + D3T.Duuv*(b*c*c+2*a*c*d) + D3T.Duvv*(a*d*d+2*b*c*d) + D3T.Dvvv*(b*d*d);
  gp_XYZ Svvv = D3T.Duuu*(c*c*c) + D3T.Duuv*(3*c*c*d)       + D3T.Duvv*(3*c*d*d)       + D3T.Dvvv*(d*d*d);

  Standard_Real &A0u = a;
  Standard_Real &A1u = b;
  Standard_Real &A0v = c;
  Standard_Real &A1v = d;
  Suuu += D2T.Duu*(3*A0u*B0uu)          + D2T.Duv*(3*(A0u*B1uu+A1u*B0uu))                     + D2T.Dvv*(3*A1u*B1uu);
  Suuv += D2T.Duu*(2*A0u*B0uv+A0v*B0uu) + D2T.Duv*(2*(A0u*B1uv+A1u*B0uv)+A0v*B1uu+A1v*B0uu)   + D2T.Dvv*(2*A1u*B1uv+A1v*B1uu);
  Suvv += D2T.Duu*(A0u*B0vv+2*A0v*B0uv) + D2T.Duv*(2*(A0v*B1uv+A1v*B0uv)+A0u*B1vv+A1u*B0vv)   + D2T.Dvv*(2*A1v*B1uv+A1u*B1vv);
  Svvv += D2T.Duu*(3*A0v*B0vv)          + D2T.Duv*(3*(A0v*B1vv+A1v*B0vv))                     + D2T.Dvv*(3*A1v*B1vv);



  gp_XYZ duuu = Suuu-D3S.Duuu;
  gp_XYZ duuv = Suuv-D3S.Duuv;
  gp_XYZ duvv = Suvv-D3S.Duvv;
  gp_XYZ dvvv = Svvv-D3S.Dvvv;
  duuu *= IncrementalLoad;
  duuv *= IncrementalLoad;
  duvv *= IncrementalLoad;
  dvvv *= IncrementalLoad;


  myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duuu,3,0), normale);
  myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duuv,2,1), normale);
  myLSC[2] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duvv,1,2), normale);
  myLSC[3] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dvvv,0,3), normale);
  nb_LSConstraints = 4;
}