// File:	AppParCurves_Variational_8.gxx
// Created:	Mon Feb 15 18:46:25 1999
// Author:	Igor FEOKTISTOV
//		<ifv@paradox.nnov.matra-dtv.fr>

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

void AppParCurves_Variational::AssemblingConstraints(const Handle(FEmTool_Curve)& Curve,
						     const TColStd_Array1OfReal& Parameters,
						     const Standard_Real CBLONG,
						     FEmTool_Assembly& A ) const
{

  Standard_Integer MxDeg = Curve->Base()->WorkDegree(),
                   NbElm = Curve->NbElements(),
                   NbDim = Curve->Dimension();

  TColStd_Array1OfReal G0(0, MxDeg), G1(0, MxDeg), G2(0, MxDeg);
  math_Vector V0((Standard_Real*)&G0(0), 0, MxDeg), 
              V1((Standard_Real*)&G1(0), 0, MxDeg), 
              V2((Standard_Real*)&G2(0), 0, MxDeg);

  Standard_Integer IndexOfConstraint, Ng3d, Ng2d, NBeg2d, NPass, NgPC1, 
                   NTang3d, NTang2d,  
                   Point, TypOfConstr, 
                   p0 = Parameters.Lower() - myFirstPoint,
                   curel = 1, el, i, ipnt, ityp, j, k, pnt, curdim,
                   jt, Ntheta = 6 * myNbP3d + 2 * myNbP2d;
  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  
//  Ng3d = 3 * NbConstr + 2 * myNbTangPoints + 5 * myNbCurvPoints;
//  Ng2d = 2 * NbConstr + 1 * myNbTangPoints + 3 * myNbCurvPoints;
  Ng3d = 3 * NbConstr + 3 * myNbTangPoints + 5 * myNbCurvPoints;
  Ng2d = 2 * NbConstr + 2 * myNbTangPoints + 3 * myNbCurvPoints;
  NBeg2d = Ng3d * myNbP3d;
//  NgPC1 = NbConstr + myNbCurvPoints;
  NgPC1 = NbConstr + myNbTangPoints + myNbCurvPoints;
  NPass = 0; 
  NTang3d = 3 * NgPC1;
  NTang2d = 2 * NgPC1;

  TColStd_Array1OfReal& Intervals = Curve->Knots();

  Standard_Real t, R1, R2;

  Handle(PLib_Base) myBase = Curve->Base();
  Handle(PLib_HermitJacobi) myHermitJacobi = (*((Handle(PLib_HermitJacobi)*)&myBase));
  Standard_Integer Order = myHermitJacobi->NivConstr() + 1;

  Standard_Real UFirst, ULast, coeff, c0, mfact, mfact1;

  A.NullifyConstraint();

  ipnt = -1;
  ityp = 0;
  for(i = 1; i <= NbConstr; i++) {

    ipnt += 2; ityp += 2;

    Point = myTypConstraints->Value(ipnt);
    TypOfConstr = myTypConstraints->Value(ityp);

    t = Parameters(p0 + Point);

    for(el = curel; el <= NbElm; ) {
      if( t <= Intervals(++el) ) {
	curel = el - 1;
	break;
      }
    }
    

    UFirst = Intervals(curel); ULast = Intervals(curel + 1);
    coeff = (ULast - UFirst)/2.; c0 = (ULast + UFirst)/2.;

    t = (t - c0) / coeff;
    
    if(TypOfConstr == 0) {
      myBase->D0(t, G0);
      for(k = 1; k < Order; k++) {
	mfact = Pow(coeff, k);
	G0(k) *= mfact;
	G0(k + Order) *= mfact;
      }
    }
    else if(TypOfConstr == 1) {
      myBase->D1(t, G0, G1);
      for(k = 1; k < Order; k++) {
	mfact = Pow(coeff, k);
	G0(k) *= mfact;
	G0(k + Order) *= mfact;
	G1(k) *= mfact;
	G1(k + Order) *= mfact;
      }
      mfact = 1./coeff;
      for(k = 0; k <= MxDeg; k++) {
	G1(k) *= mfact;
      }
    }
    else {
      myBase->D2(t, G0, G1, G2);
      for(k = 1; k < Order; k++) {
	mfact = Pow(coeff, k);
	G0(k) *= mfact;
	G0(k + Order) *= mfact;
	G1(k) *= mfact;
	G1(k + Order) *= mfact;
	G2(k) *= mfact;
	G2(k + Order) *= mfact;
      }
      mfact = 1. / coeff;
      mfact1 = mfact / coeff;
      for(k = 0; k <= MxDeg; k++) {
	G1(k) *= mfact;
	G2(k) *= mfact1;
      }
    }

    NPass++;

    j = NbDim * (Point - myFirstPoint);
    Standard_Integer n0 = NPass;
    curdim = 0;
    for(pnt = 1; pnt <= myNbP3d; pnt++) {
      IndexOfConstraint = n0;
      for(k = 1; k <= 3; k++) {
	curdim++;
	A.AddConstraint(IndexOfConstraint, curel, curdim, V0, myTabPoints->Value(j + k));
	IndexOfConstraint += NgPC1;
      }
      j +=3;
      n0 += Ng3d;
    }

    n0 = NPass + NBeg2d;
    for(pnt = 1; pnt <= myNbP2d; pnt++) {
      IndexOfConstraint = n0;
      for(k = 1; k <= 2; k++) {
	curdim++;
	A.AddConstraint(IndexOfConstraint, curel, curdim, V0, myTabPoints->Value(j + k));
	IndexOfConstraint += NgPC1;
      }
      j +=2;
      n0 += Ng2d;
    }

/*    if(TypOfConstr == 1) {

      IndexOfConstraint = NTang3d + 1;
      jt = Ntheta * (i - 1);
      for(pnt = 1; pnt <= myNbP3d; pnt++) {
	for(k = 1; k <= 3; k++) {
	  A.AddConstraint(IndexOfConstraint, curel, k, myTtheta->Value(jt + k) *  V1, 0.);
	  A.AddConstraint(IndexOfConstraint + 1, curel, k, myTtheta->Value(jt + 3 + k) *  V1, 0.);
	}
	IndexOfConstraint += Ng3d;
	jt += 6;
      }

      IndexOfConstraint = NBeg2d + NTang2d + 1;
      for(pnt = 1; pnt <= myNbP2d; pnt++) {
	for(k = 1; k <= 2; k++) {
	  A.AddConstraint(IndexOfConstraint, curel, k, myTtheta->Value(jt + k) * V1, 0.);
	}
	IndexOfConstraint += Ng2d;
	jt += 2;
      }

      NTang3d += 2;
      NTang2d += 1;
    } */
    if(TypOfConstr == 1) {

      NPass++;
      n0 = NPass;
      j = 2 * NbDim * (i - 1);
      curdim = 0;
      for(pnt = 1; pnt <= myNbP3d; pnt++) {
	IndexOfConstraint = n0;
	for(k = 1; k <= 3; k++) {
	  curdim++;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
	  IndexOfConstraint += NgPC1;
	}
	n0 += Ng3d;
	j += 6;
      }

      n0 = NPass + NBeg2d;
      for(pnt = 1; pnt <= myNbP2d; pnt++) {
	IndexOfConstraint = n0;
	for(k = 1; k <= 2; k++) {
	  curdim++;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
	  IndexOfConstraint += NgPC1;
	}
	n0 += Ng2d;
	j += 4;
      }
    }
    if(TypOfConstr == 2) {

      NPass++;
      n0 = NPass;
      j = 2 * NbDim * (i - 1);
      curdim = 0;
      for(pnt = 1; pnt <= myNbP3d; pnt++) {
	IndexOfConstraint = n0;
	for(k = 1; k <= 3; k++) {
	  curdim++;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
	  IndexOfConstraint += NgPC1;
	}
	n0 += Ng3d;
	j += 6;
      }

      n0 = NPass + NBeg2d;
      for(pnt = 1; pnt <= myNbP2d; pnt++) {
	IndexOfConstraint = n0;
	for(k = 1; k <= 2; k++) {
	  curdim++;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
	  IndexOfConstraint += NgPC1;
	}
	n0 += Ng2d;
	j += 4;
      }

      j = 2 * NbDim * (i - 1) + 3;
      jt = Ntheta * (i - 1);
      IndexOfConstraint = NTang3d + 1;
      curdim = 0;
      for(pnt = 1; pnt <= myNbP3d; pnt++) {
	R1 = 0.; R2 = 0.;
	for(k = 1; k <= 3; k++) { 
	  R1 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + k);
	  R2 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + 3 + k);
	}
	R1 *= CBLONG * CBLONG;
	R2 *= CBLONG * CBLONG;
	for(k = 1; k <= 3; k++) {
	  curdim++;
	  if(k > 1) R1 = R2 = 0.;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, myTfthet->Value(jt + k) * V2, R1);
	  A.AddConstraint(IndexOfConstraint + 1, curel, curdim, myTfthet->Value(jt + 3 + k) * V2, R2);
	}
	IndexOfConstraint += Ng3d;
	j += 6;
	jt += 6;
      }

      j--;
      IndexOfConstraint = NBeg2d + NTang2d + 1;
      for(pnt = 1; pnt <= myNbP2d; pnt++) {
	R1 = 0.;
	for(k = 1; k <= 2; k++) { 
	  R1 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + k);
	}
	R1 *= CBLONG * CBLONG;
	for(k = 1; k <= 2; k++) {
	  curdim++;
	  if(k > 1) R1 = 0.;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, myTfthet->Value(jt + k) * V2, R1);
	}
	IndexOfConstraint += Ng2d;
	j += 4;
	jt += 2;
      }

      NTang3d += 2;
      NTang2d += 1;
    }

  }

}