// Created on: 1998-07-09
// Created by: Stephanie HUMEAU
// 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 <GeomFill_FunctionGuide.hxx>

#include <Geom_BSplineCurve.hxx>
#include <Geom_Curve.hxx>
#include <Geom_SurfaceOfRevolution.hxx>
#include <Geom_TrimmedCurve.hxx>
#include <GeomAdaptor_Curve.hxx>
#include <GeomFill_SectionLaw.hxx>
#include <gp_Ax1.hxx>
#include <gp_Ax3.hxx>
#include <gp_Dir.hxx>
#include <gp_Pnt.hxx>
#include <gp_Trsf.hxx>
#include <gp_Vec.hxx>
#include <gp_XYZ.hxx>
#include <math_Matrix.hxx>
#include <Precision.hxx>

// #include <Standard_NotImplemented.hxx>
//==============================================
//    Calcul de la valeur de la fonction :
//                       G(w) - S(teta,v) = 0
//  ou G : guide   et   S : surface de revolution
//==============================================
//==============================================
//  Function : FunctionGuide
//  Purpose : Initialisation de la section et de la surface d'arret
//==============================================
GeomFill_FunctionGuide::GeomFill_FunctionGuide(const Handle(GeomFill_SectionLaw)& S,
                                               const Handle(Adaptor3d_Curve)&     C,
                                               const Standard_Real                Param)
    : TheGuide(C),
      TheLaw(S),
      isconst(Standard_False),
      First(0.0),
      Last(0.0),
      TheUonS(Param)
{
  Standard_Real Tol = Precision::Confusion();
  if (TheLaw->IsConstant(Tol))
  {
    isconst  = Standard_True;
    TheConst = TheLaw->ConstantSection();
    First    = TheConst->FirstParameter();
    Last     = TheConst->LastParameter();
  }
  else
  {
    isconst = Standard_False;
    TheConst.Nullify();
  }
  TheCurve.Nullify();
}

//==============================================
// Function : SetParam
// Purpose : Initialisation de la surface de revolution
//==============================================
// void GeomFill_FunctionGuide::SetParam(const Standard_Real Param,
void GeomFill_FunctionGuide::SetParam(const Standard_Real,
                                      const gp_Pnt& C,
                                      const gp_XYZ& D,
                                      const gp_XYZ& DX)
{
  Centre = C.XYZ();
  Dir    = D;

  // repere fixe
  gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX());

  // calculer transfo entre triedre et Oxyz
  gp_Dir  B2 = DX;
  gp_Ax3  RepTriedre(C, D, B2);
  gp_Trsf Transfo;
  Transfo.SetTransformation(RepTriedre, Rep);

  if (isconst)
  {
    TheCurve = new (Geom_TrimmedCurve)(Handle(Geom_Curve)::DownCast(TheConst->Copy()), First, Last);
  }
  else
  {
    Standard_Integer NbPoles, NbKnots, Deg;
    TheLaw->SectionShape(NbPoles, NbKnots, Deg);
    TColStd_Array1OfInteger Mult(1, NbKnots);
    TheLaw->Mults(Mult);
    TColStd_Array1OfReal Knots(1, NbKnots);
    TheLaw->Knots(Knots);
    TColgp_Array1OfPnt   Poles(1, NbPoles);
    TColStd_Array1OfReal Weights(1, NbPoles);
    TheLaw->D0(TheUonS, Poles, Weights);
    if (TheLaw->IsRational())
      TheCurve = new (Geom_BSplineCurve)(Poles, Weights, Knots, Mult, Deg, TheLaw->IsUPeriodic());
    else
      TheCurve = new (Geom_BSplineCurve)(Poles, Knots, Mult, Deg, TheLaw->IsUPeriodic());
  }

  gp_Ax1 Axe(C, Dir);
  TheCurve->Transform(Transfo);
  TheSurface = new (Geom_SurfaceOfRevolution)(TheCurve, Axe);
}

//==============================================
// Function : NbVariables (w, u, v)
// Purpose :
//==============================================
Standard_Integer GeomFill_FunctionGuide::NbVariables() const
{
  return 3;
}

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

Standard_Integer GeomFill_FunctionGuide::NbEquations() const
{
  return 3;
}

//==============================================
// Function : Value
// Purpose : calcul of the value of the function at <X>
//==============================================
Standard_Boolean GeomFill_FunctionGuide::Value(const math_Vector& X, math_Vector& F)
{
  gp_Pnt P, P1;

  TheGuide->D0(X(1), P);
  TheSurface->D0(X(2), X(3), P1);

  F(1) = P.Coord(1) - P1.Coord(1);
  F(2) = P.Coord(2) - P1.Coord(2);
  F(3) = P.Coord(3) - P1.Coord(3);

  return Standard_True;
}

//==============================================
// Function : Derivatives
// Purpose :calcul of the derivative of the function
//==============================================
Standard_Boolean GeomFill_FunctionGuide::Derivatives(const math_Vector& X, math_Matrix& D)
{
  gp_Pnt P, P1;
  gp_Vec DP, DP1U, DP1V;

  TheGuide->D1(X(1), P, DP);
  TheSurface->D1(X(2), X(3), P1, DP1U, DP1V);

  Standard_Integer i;
  for (i = 1; i <= 3; i++)
  {
    D(i, 1) = DP.Coord(i);
    D(i, 2) = -DP1U.Coord(i);
    D(i, 3) = -DP1V.Coord(i);
  } // for

  return Standard_True;
}

//==============================================
// Function : Values
// Purpose : calcul of the value and the derivative of the function
//==============================================
Standard_Boolean GeomFill_FunctionGuide::Values(const math_Vector& X,
                                                math_Vector&       F,
                                                math_Matrix&       D)
{
  gp_Pnt P, P1;
  gp_Vec DP, DP1U, DP1V;

  TheGuide->D1(X(1), P, DP);                  // derivee de la generatrice
  TheSurface->D1(X(2), X(3), P1, DP1U, DP1V); // derivee de la new surface

  Standard_Integer i;
  for (i = 1; i <= 3; i++)
  {
    F(i) = P.Coord(i) - P1.Coord(i);

    D(i, 1) = DP.Coord(i);
    D(i, 2) = -DP1U.Coord(i);
    D(i, 3) = -DP1V.Coord(i);
  } // for

  return Standard_True;
}

//==============================================
// Function : DerivT
// Purpose : calcul of the first derivative from t
//==============================================
Standard_Boolean GeomFill_FunctionGuide::DerivT(const math_Vector& X,
                                                const gp_XYZ&      DCentre,
                                                const gp_XYZ&      DDir,
                                                math_Vector&       F)
{
  gp_Pnt P;
  gp_Vec DS;
  DSDT(X(2), X(3), DCentre, DDir, DS);

  TheCurve->D0(X(1), P);

  F(1) = P.Coord(1) - DS.Coord(1);
  F(2) = P.Coord(2) - DS.Coord(2);
  F(3) = P.Coord(3) - DS.Coord(3);

  return Standard_True;
}

//=========================================================
// Function : DSDT
// Purpose : calcul de la derive de la surface /t en U, V
//=========================================================
void GeomFill_FunctionGuide::DSDT(const Standard_Real U,
                                  const Standard_Real V,
                                  const gp_XYZ&       DC,
                                  const gp_XYZ&       DDir,
                                  gp_Vec&             DS) const
{
  // C origine sur l'axe de revolution
  // Vdir vecteur unitaire definissant la direction de l'axe de revolution
  // Q(v) point de parametre V sur la courbe de revolution
  // OM (u,v) = OC + CQ * Cos(U) + (CQ.Vdir)(1-Cos(U)) * Vdir +
  //            (Vdir^CQ)* Sin(U)

  gp_Pnt Pc;
  TheCurve->D0(V, Pc); // Q(v)
  //   if (!isconst)

  gp_XYZ &Q = Pc.ChangeCoord(), DQ(0, 0, 0); // Q
  if (!isconst)
  {
    std::cout << "Not implemented" << std::endl;
  }

  Q.Subtract(Centre); // CQ
  DQ -= DC;

  gp_XYZ DVcrossCQ;
  DVcrossCQ.SetLinearForm(DDir.Crossed(Q),
                          Dir.Crossed(DQ)); // Vdir^CQ
  DVcrossCQ.Multiply(Sin(U));               //(Vdir^CQ)*Sin(U)

  Standard_Real CosU = Cos(U);
  gp_XYZ        DVdotCQ;
  DVdotCQ.SetLinearForm(DDir.Dot(Q) + Dir.Dot(DQ), Dir, Dir.Dot(Q), DDir); //(CQ.Vdir)(1-Cos(U))Vdir
  DVdotCQ.Add(DVcrossCQ); // addition des composantes

  DQ.Multiply(CosU);
  DQ.Add(DVdotCQ);
  DQ.Add(DC);
  DS.SetXYZ(DQ);
}

//=========================================================
// Function : Deriv2T
// Purpose : calcul of the second derivatice from t
//=========================================================

/* Standard_Boolean GeomFill_FunctionGuide::Deriv2T(const Standard_Real Param1,
                          const Standard_Real Param,
                          const Standard_Real Param0,
                          const math_Vector & R1,
                          const math_Vector & R,
                          const math_Vector & R0,
                          math_Vector& F)
{
  math_Vector F1(1,3,0);
  math_Vector F2(1,3,0);

  DerivT(Param1, Param, R1, R, F1);
  DerivT(Param, Param0, R, R0, F2);

  Standard_Real h1 = Param - Param1;
  Standard_Real h2 = Param0 - Param;

  Standard_Integer i;
  for (i=1;i<=3;i++)
    F(i) = (F2(i) - F1(i))  / ((h2 + h1)/2);

  return Standard_True;
}

//=========================================================
// Function : DerivTX
// Purpose : calcul of the second derivative from t and x
//=========================================================
 Standard_Boolean GeomFill_FunctionGuide::DerivTX(const Standard_Real Param,
                          const Standard_Real Param0,
                          const math_Vector & R,
                          const math_Vector & X0,
                          math_Matrix& D)
{
  gp_Pnt P1,P2;
  gp_Vec DP1,DP2,DP2U,DP2V,DP1U,DP1V;

  TheCurve->D1(R(1), P1, DP1); // guide
  TheCurve->D1(X0(1), P2, DP2);
  TheSurface->D1(R(2), R(3), P1, DP1U, DP1V); // surface
  TheSurface->D1(X0(2), X0(3), P2, DP2U, DP2V); //derivee de la new surface

  Standard_Real h = Param0 - Param;

  Standard_Integer i;
  for (i=1;i<=3;i++)
    {
      D(i,1) = (DP2.Coord(i) - DP1.Coord(i)) / h;
      //D(i,2) = - (DP2U.Coord(i) - DP1U.Coord(i)) / h;
      D(i,2) = - DP1U.Coord(i) * (X0(2)-R(2)) / h;
      //D(i,3) = - (DP2V.Coord(i) - DP1V.Coord(i)) / h;
      D(i,3) = - DP1V.Coord(i) * (X0(3)-R(3)) / h;
    }// for

  return Standard_True;
}

//=========================================================
// Function : Deriv2X
// Purpose : calcul of the second derivative from x
//=========================================================
 Standard_Boolean GeomFill_FunctionGuide::Deriv2X(const math_Vector & X,
                          GeomFill_Tensor& T)
{
  gp_Pnt P,P1;
  gp_Vec DP,D2P,DPU,DPV;
  gp_Vec D2PU, D2PV, D2PUV;

  TheCurve->D2(X(1), P1, DP, D2P);
  TheSurface->D2(X(2), X(3), P, DPU, DPV, D2PU, D2PV, D2PUV);

  T.Init(0.); // tenseur

  Standard_Integer i;
  for (i=1;i<=3;i++)
    {
      T(i,1,1) = D2P.Coord(i);
      T(i,2,2) = -D2PU.Coord(i);
      T(i,3,2) = T(i,2,3) = -D2PUV.Coord(i);
      T(i,3,3) = -D2PV.Coord(i);
    }// for

  return Standard_True;
}*/
