OCCT

// Copyright (c) 1995-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 <AppParCurves_MultiBSpCurve.hxx>
#include <AppParCurves_MultiPoint.hxx>
#include <NCollection_Array1.hxx>
#include <Standard_Integer.hxx>
#include <NCollection_HArray1.hxx>
#include <gp_Pnt.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_Vec.hxx>
#include <gp_Vec2d.hxx>
#include <math_Vector.hxx>
#include <AppParCurves_ConstraintCouple.hxx>

AppParCurves_BSpFunction::AppParCurves_BSpFunction(
  const MultiLine&                                                       SSP,
  const int                                                              FirstPoint,
  const int                                                              LastPoint,
  const occ::handle<NCollection_HArray1<AppParCurves_ConstraintCouple>>& TheConstraints,
  const math_Vector&                                                     Parameters,
  const NCollection_Array1<double>&                                      Knots,
  const NCollection_Array1<int>&                                         Mults,
  const int                                                              NbPol)
    : MyMultiLine(SSP),
      MyMultiBSpCurve(NbPol),
      myParameters(Parameters.Lower(), Parameters.Upper()),
      ValGrad_F(FirstPoint, LastPoint),
      MyF(FirstPoint, LastPoint, 1, ToolLine::NbP3d(SSP) + ToolLine::NbP2d(SSP), 0.0),
      PTLX(FirstPoint, LastPoint, 1, ToolLine::NbP3d(SSP) + ToolLine::NbP2d(SSP), 0.0),
      PTLY(FirstPoint, LastPoint, 1, ToolLine::NbP3d(SSP) + ToolLine::NbP2d(SSP), 0.0),
      PTLZ(FirstPoint, LastPoint, 1, ToolLine::NbP3d(SSP) + ToolLine::NbP2d(SSP), 0.0),
      A(FirstPoint, LastPoint, 1, NbPol),
      DA(FirstPoint, LastPoint, 1, NbPol),
      MyLeastSquare(SSP,
                    Knots,
                    Mults,
                    FirstPoint,
                    LastPoint,
                    FirstConstraint(TheConstraints, FirstPoint),
                    LastConstraint(TheConstraints, LastPoint),
                    NbPol)
{
  int i;
  for (i = Parameters.Lower(); i <= Parameters.Upper(); i++)
    myParameters(i) = Parameters(i);
  FirstP        = FirstPoint;
  LastP         = LastPoint;
  myConstraints = TheConstraints;
  NbP           = LastP - FirstP + 1;
  Adeb          = FirstP;
  Afin          = LastP;
  nbpoles       = NbPol;
  MyMultiBSpCurve.SetKnots(Knots);
  MyMultiBSpCurve.SetMultiplicities(Mults);
  Contraintes = false;
  int                           myindex;
  int                           low = TheConstraints->Lower(), upp = TheConstraints->Upper();
  AppParCurves_ConstraintCouple mycouple;
  AppParCurves_Constraint       Cons;

  for (i = low; i <= upp; i++)
  {
    mycouple = TheConstraints->Value(i);
    Cons     = mycouple.Constraint();
    myindex  = mycouple.Index();
    if (myindex == FirstP)
    {
      if (Cons >= 1)
        Adeb = Adeb + 1;
    }
    else if (myindex == LastP)
    {
      if (Cons >= 1)
        Afin = Afin - 1;
    }
    else
    {
      if (Cons >= 1)
        Contraintes = true;
    }
  }

  int nb3d   = ToolLine::NbP3d(SSP);
  int nb2d   = ToolLine::NbP2d(SSP);
  int mynb3d = nb3d, mynb2d = nb2d;
  if (nb3d == 0)
    mynb3d = 1;
  if (nb2d == 0)
    mynb2d = 1;

  NbCu   = nb3d + nb2d;
  tabdim = new NCollection_HArray1<int>(0, NbCu - 1);

  if (Contraintes)
  {
    for (i = 1; i <= NbCu; i++)
    {
      if (i <= nb3d)
        tabdim->SetValue(i - 1, 3);
      else
        tabdim->SetValue(i - 1, 2);
    }

    NCollection_Array1<gp_Pnt>   TabP(1, mynb3d);
    NCollection_Array1<gp_Pnt2d> TabP2d(1, mynb2d);

    for (i = FirstP; i <= LastP; i++)
    {
      if (nb3d != 0 && nb2d != 0)
        ToolLine::Value(SSP, i, TabP, TabP2d);
      else if (nb3d != 0)
        ToolLine::Value(SSP, i, TabP);
      else
        ToolLine::Value(SSP, i, TabP2d);
      for (int j = 1; j <= NbCu; j++)
      {
        if (tabdim->Value(j - 1) == 3)
        {
          TabP(j).Coord(PTLX(i, j), PTLY(i, j), PTLZ(i, j));
        }
        else
        {
          TabP2d(j).Coord(PTLX(i, j), PTLY(i, j));
        }
      }
    }
  }
}

AppParCurves_Constraint AppParCurves_BSpFunction::FirstConstraint(
  const occ::handle<NCollection_HArray1<AppParCurves_ConstraintCouple>>& TheConstraints,
  const int                                                              FirstPoint) const
{
  int                           i, myindex;
  int                           low = TheConstraints->Lower(), upp = TheConstraints->Upper();
  AppParCurves_ConstraintCouple mycouple;
  AppParCurves_Constraint       Cons = AppParCurves_NoConstraint;

  for (i = low; i <= upp; i++)
  {
    mycouple = TheConstraints->Value(i);
    Cons     = mycouple.Constraint();
    myindex  = mycouple.Index();
    if (myindex == FirstPoint)
    {
      break;
    }
  }
  return Cons;
}

AppParCurves_Constraint AppParCurves_BSpFunction::LastConstraint(
  const occ::handle<NCollection_HArray1<AppParCurves_ConstraintCouple>>& TheConstraints,
  const int                                                              LastPoint) const
{
  int                           i, myindex;
  int                           low = TheConstraints->Lower(), upp = TheConstraints->Upper();
  AppParCurves_ConstraintCouple mycouple;
  AppParCurves_Constraint       Cons = AppParCurves_NoConstraint;

  for (i = low; i <= upp; i++)
  {
    mycouple = TheConstraints->Value(i);
    Cons     = mycouple.Constraint();
    myindex  = mycouple.Index();
    if (myindex == LastPoint)
    {
      break;
    }
  }
  return Cons;
}

bool AppParCurves_BSpFunction::Value(const math_Vector& X, double& F)
{

  myParameters = X;

  // Resolution moindres carres:
  // ===========================
  MyLeastSquare.Perform(myParameters, mylambda1, mylambda2);
  if (!(MyLeastSquare.IsDone()))
  {
    Done = false;
    return false;
  }
  if (!Contraintes)
  {
    MyLeastSquare.Error(FVal, ERR3d, ERR2d);
    F = FVal;
  }

  // Resolution avec contraintes:
  // ============================
  else
  {
  }
  return true;
}

void AppParCurves_BSpFunction::Perform(const math_Vector& X)
{
  int j;

  myParameters = X;
  // Resolution moindres carres:
  // ===========================
  MyLeastSquare.Perform(myParameters, mylambda1, mylambda2);

  if (!(MyLeastSquare.IsDone()))
  {
    Done = false;
    return;
  }

  for (j = myParameters.Lower(); j <= myParameters.Upper(); j++)
  {
    ValGrad_F(j) = 0.0;
  }

  if (!Contraintes)
  {
    MyLeastSquare.ErrorGradient(ValGrad_F, FVal, ERR3d, ERR2d);
  }
  else
  {
  }
}

void AppParCurves_BSpFunction::SetFirstLambda(const double l1)
{
  mylambda1 = l1;
}

void AppParCurves_BSpFunction::SetLastLambda(const double l2)
{
  mylambda2 = l2;
}

int AppParCurves_BSpFunction::NbVariables() const
{
  return NbP;
}

bool AppParCurves_BSpFunction::Gradient(const math_Vector& X, math_Vector& G)
{

  Perform(X);
  G = ValGrad_F;

  return true;
}

bool AppParCurves_BSpFunction::Values(const math_Vector& X, double& F, math_Vector& G)
{

  Perform(X);
  F = FVal;
  G = ValGrad_F;

  /*
    math_Vector mygradient = G;
    math_Vector myx = X;
    double myf = FVal;
    double F2 = FVal;
    math_Vector G2 = ValGrad_F;
    for (int i = 1; i <= X.Length(); i++) {
      myx = X;
      myx(i) = X(i) + 1.0e-10;
      Value(myx, F2);
      mygradient(i) = (F2 - myf)/(1.0e-10);
    }

  cout << " Gradient calcule : " << G2 << endl;
  cout << " Gradient interpole : " <<  mygradient << endl;
  */
  return true;
}

AppParCurves_MultiBSpCurve AppParCurves_BSpFunction::CurveValue()
{
  if (!Contraintes)
    MyMultiBSpCurve = MyLeastSquare.BSplineValue();
  return MyMultiBSpCurve;
}

double AppParCurves_BSpFunction::Error(const int IPoint, const int CurveIndex)
{
  const math_Matrix& DD = MyLeastSquare.Distance();
  double             d  = DD.Value(IPoint, CurveIndex);
  if (!Contraintes)
    return d;
  else
    return std::sqrt(MyF(IPoint, CurveIndex));
}

double AppParCurves_BSpFunction::MaxError3d() const
{
  return ERR3d;
}

double AppParCurves_BSpFunction::MaxError2d() const
{
  return ERR2d;
}

const math_Vector& AppParCurves_BSpFunction::NewParameters() const
{
  return myParameters;
}

const math_Matrix& AppParCurves_BSpFunction::FunctionMatrix() const
{
  return MyLeastSquare.FunctionMatrix();
}

const math_Matrix& AppParCurves_BSpFunction::DerivativeFunctionMatrix() const
{
  return MyLeastSquare.DerivativeFunctionMatrix();
}

const math_IntegerVector& AppParCurves_BSpFunction::Index() const
{
  return MyLeastSquare.KIndex();
}