// Created on: 1996-02-26
// 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 <BSplCLib.hxx>
#include <FairCurve_EnergyOfMVC.hxx>
#include <FairCurve_MinimalVariation.hxx>
#include <FairCurve_Newton.hxx>
#include <Geom2d_BSplineCurve.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_Vec2d.hxx>
#include <math_Matrix.hxx>
#include <PLib.hxx>
#include <Standard_NullValue.hxx>
#include <TColgp_HArray1OfPnt2d.hxx>
#include <TColStd_HArray1OfInteger.hxx>
#include <TColStd_HArray1OfReal.hxx>

//======================================================================================
FairCurve_MinimalVariation::FairCurve_MinimalVariation(const gp_Pnt2d&     P1,
                                                       const gp_Pnt2d&     P2,
                                                       const Standard_Real Heigth,
                                                       const Standard_Real Slope,
                                                       const Standard_Real PhysicalRatio)
    //======================================================================================
    : FairCurve_Batten(P1, P2, Heigth, Slope),
      OldCurvature1(0),
      OldCurvature2(0),
      OldPhysicalRatio(PhysicalRatio),
      NewCurvature1(0),
      NewCurvature2(0),
      NewPhysicalRatio(PhysicalRatio)
{
}

//======================================================================================
Standard_Boolean FairCurve_MinimalVariation::Compute(FairCurve_AnalysisCode& ACode,
                                                     const Standard_Integer  NbIterations,
                                                     const Standard_Real     Tolerance)
//======================================================================================
{
  Standard_Boolean Ok = Standard_True, End = Standard_False;
  // clang-format off
  Standard_Real AngleMax = 0.7;      // parameter regulating the function of increment ( 40 degrees )
  // clang-format on
  Standard_Real AngleMin = 2 * M_PI / 100; // parameter regulating the function of increment
                                           // full passage should not contain more than 100 steps.
  Standard_Real DAngle1, DAngle2, DRho1, DRho2, Ratio, Fraction, Toler;
  Standard_Real OldDist, NewDist;

  //  Loop of Homotopy : calculation of the step and optimisation

  while (Ok && !End)
  {
    DAngle1 = NewAngle1 - OldAngle1;
    DAngle2 = NewAngle2 - OldAngle2;
    DRho1   = NewCurvature1 - OldCurvature1;
    DRho2   = NewCurvature2 - OldCurvature2;
    Ratio   = 1;

    if (NewConstraintOrder1 > 0)
    {
      Fraction = Abs(DAngle1) / (AngleMax * Exp(-Abs(OldAngle1) / AngleMax) + AngleMin);
      if (Fraction > 1)
        Ratio = 1 / Fraction;
    }
    if (NewConstraintOrder2 > 0)
    {
      Fraction = Abs(DAngle2) / (AngleMax * Exp(-Abs(OldAngle2) / AngleMax) + AngleMin);
      if (Fraction > 1)
        Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction);
    }

    OldDist  = OldP1.Distance(OldP2);
    NewDist  = NewP1.Distance(NewP2);
    Fraction = Abs(OldDist - NewDist) / (OldDist / 3);
    if (Fraction > 1)
      Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction);

    if (NewConstraintOrder1 > 1)
    {
      Fraction = Abs(DRho1) * OldDist / (2 + Abs(OldAngle1) + Abs(OldAngle2));
      if (Fraction > 1)
        Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction);
    }

    if (NewConstraintOrder2 > 1)
    {
      Fraction = Abs(DRho2) * OldDist / (2 + Abs(OldAngle1) + Abs(OldAngle2));
      if (Fraction > 1)
        Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction);
    }

    gp_Vec2d DeltaP1(OldP1, NewP1), DeltaP2(OldP2, NewP2);
    if (Ratio == 1)
    {
      End   = Standard_True;
      Toler = Tolerance;
    }
    else
    {
      DeltaP1 *= Ratio;
      DeltaP2 *= Ratio;
      DAngle1 *= Ratio;
      DAngle2 *= Ratio;
      DRho1 *= Ratio;
      DRho2 *= Ratio;
      Toler = 10 * Tolerance;
    }

    Ok = Compute(DeltaP1, DeltaP2, DAngle1, DAngle2, DRho1, DRho2, ACode, NbIterations, Toler);

    if (ACode != FairCurve_OK)
      End = Standard_True;
    if (NewFreeSliding)
      NewSlidingFactor = OldSlidingFactor;
    if (NewConstraintOrder1 == 0)
      NewAngle1 = OldAngle1;
    if (NewConstraintOrder1 < 2)
      NewCurvature1 = OldCurvature1;
    if (NewConstraintOrder2 == 0)
      NewAngle2 = OldAngle2;
    if (NewConstraintOrder2 < 2)
      NewCurvature2 = OldCurvature2;
  }
  myCode = ACode;
  return Ok;
}

//======================================================================================
Standard_Boolean FairCurve_MinimalVariation::Compute(const gp_Vec2d&         DeltaP1,
                                                     const gp_Vec2d&         DeltaP2,
                                                     const Standard_Real     DeltaAngle1,
                                                     const Standard_Real     DeltaAngle2,
                                                     const Standard_Real     DeltaCurvature1,
                                                     const Standard_Real     DeltaCurvature2,
                                                     FairCurve_AnalysisCode& ACode,
                                                     const Standard_Integer  NbIterations,
                                                     const Standard_Real     Tolerance)
//======================================================================================
{
  Standard_Boolean Ok, OkCompute = Standard_True;
  ACode = FairCurve_OK;

  // Deformation of the curve by adding a polynom of interpolation
  Standard_Integer L = 2 + NewConstraintOrder1 + NewConstraintOrder2, kk, ii;
//                    NbP1 = Poles->Length()-1, kk, ii;
#ifdef OCCT_DEBUG
  Standard_Integer NbP1 =
#endif
    Poles->Length();
#ifdef OCCT_DEBUG
  NbP1 = NbP1 - 1;
#endif
  TColStd_Array1OfReal knots(1, 2);
  knots(1) = 0;
  knots(2) = 1;
  TColStd_Array1OfInteger       mults(1, 2);
  TColgp_Array1OfPnt2d          HermitePoles(1, L);
  TColgp_Array1OfPnt2d          Interpolation(1, L);
  Handle(TColgp_HArray1OfPnt2d) NPoles = new TColgp_HArray1OfPnt2d(1, Poles->Length());

  // Polynomes of Hermite
  math_Matrix HermiteCoef(1, L, 1, L);
  Ok = PLib::HermiteCoefficients(0, 1, NewConstraintOrder1, NewConstraintOrder2, HermiteCoef);
  if (!Ok)
    return Standard_False;

  // Definition of constraints of interpolation
  TColgp_Array1OfXY ADelta(1, L);
  gp_Vec2d VOld(OldP1, OldP2), VNew(-(OldP1.XY() + DeltaP1.XY()) + (OldP2.XY() + DeltaP2.XY()));
  Standard_Real DAngleRef = VNew.Angle(VOld);
  Standard_Real DAngle1   = DeltaAngle1 - DAngleRef,
                // clang-format off
                 DAngle2 = DAngleRef   - DeltaAngle2; // Correction of Delta by the Delta induced by the points.
  // clang-format on

  ADelta(1) = DeltaP1.XY();
  kk        = 2;
  if (NewConstraintOrder1 > 0)
  {
    // rotation of the derivative premiereDeltaAngle1
    gp_Vec2d OldDerive(Poles->Value(Poles->Lower()), Poles->Value(Poles->Lower() + 1));
    OldDerive *= Degree / (Knots->Value(Knots->Lower() + 1) - Knots->Value(Knots->Lower()));
    ADelta(kk) = (OldDerive.Rotated(DAngle1) - OldDerive).XY();
    kk += 1;

    if (NewConstraintOrder1 > 1)
    {
      // rotation of the second derivative + adding
      gp_Vec2d OldSeconde(Poles->Value(Poles->Lower()).XY() + Poles->Value(Poles->Lower() + 2).XY()
                          - 2 * Poles->Value(Poles->Lower() + 1).XY());
      OldSeconde *= Degree * (Degree - 1)
                    / pow(Knots->Value(Knots->Lower() + 1) - Knots->Value(Knots->Lower()), 2);
      Standard_Real CPrim = OldDerive.Magnitude();
      ADelta(kk)          = (OldSeconde.Rotated(DAngle1) - OldSeconde
                    + DeltaCurvature1 * CPrim * OldDerive.Rotated(M_PI / 2 + DAngle1))
                     .XY();
      kk += 1;
    }
  }
  ADelta(kk) = DeltaP2.XY();
  kk += 1;
  if (NewConstraintOrder2 > 0)
  {
    gp_Vec2d OldDerive(Poles->Value(Poles->Upper() - 1), Poles->Value(Poles->Upper()));
    OldDerive *= Degree / (Knots->Value(Knots->Upper()) - Knots->Value(Knots->Upper() - 1));
    ADelta(kk) = (OldDerive.Rotated(DAngle2) - OldDerive).XY();
    kk += 1;
    if (NewConstraintOrder2 > 1)
    {
      // rotation of the second derivative + adding
      gp_Vec2d OldSeconde(Poles->Value(Poles->Upper()).XY() + Poles->Value(Poles->Upper() - 2).XY()
                          - 2 * Poles->Value(Poles->Upper() - 1).XY());
      OldSeconde *= Degree * (Degree - 1)
                    / pow(Knots->Value(Knots->Upper()) - Knots->Value(Knots->Upper() - 1), 2);
      Standard_Real CPrim = OldDerive.Magnitude();
      ADelta(kk)          = (OldSeconde.Rotated(DAngle2) - OldSeconde
                    + DeltaCurvature2 * CPrim * OldDerive.Rotated(M_PI / 2 + DAngle2))
                     .XY();
      kk += 1;
    }
  }

  // Interpolation
  gp_XY AuxXY(0, 0);
  for (ii = 1; ii <= L; ii++)
  {
    AuxXY.SetCoord(0.0, 0);
    for (kk = 1; kk <= L; kk++)
    {
      AuxXY += HermiteCoef(kk, ii) * ADelta(kk);
    }
    Interpolation(ii).SetXY(AuxXY);
  }
  // Conversion into BSpline of the same structure as the current batten.
  PLib::CoefficientsPoles(Interpolation, PLib::NoWeights(), HermitePoles, PLib::NoWeights());

  mults.Init(L);

  Handle(Geom2d_BSplineCurve) DeltaCurve =
    new Geom2d_BSplineCurve(HermitePoles, knots, mults, L - 1);

  DeltaCurve->IncreaseDegree(Degree);
  if (Mults->Length() > 2)
  {
    DeltaCurve->InsertKnots(Knots->Array1(), Mults->Array1(), 1.e-10);
  }

  // Summing
  DeltaCurve->Poles(NPoles->ChangeArray1());
  for (kk = NPoles->Lower(); kk <= NPoles->Upper(); kk++)
  {
    NPoles->ChangeValue(kk).ChangeCoord() += Poles->Value(kk).Coord();
  }

  // Intermediaires

  Standard_Real Angle1, Angle2, SlidingLength,
    Alph1 = OldAngle1 + DeltaAngle1, Alph2 = OldAngle2 + DeltaAngle2,
    Rho1 = OldCurvature1 + DeltaCurvature1, Rho2 = OldCurvature2 + DeltaCurvature2,
    Dist       = NPoles->Value(NPoles->Upper()).Distance(NPoles->Value(NPoles->Lower())),
    LReference = SlidingOfReference(Dist, Alph1, Alph2);
  gp_Vec2d Ox(1, 0),
    P1P2(NPoles->Value(NPoles->Upper()).Coord() - NPoles->Value(NPoles->Lower()).Coord());

  // Angles corresponding to axis ox

  Angle1 = Ox.Angle(P1P2) + Alph1;
  Angle2 = -Ox.Angle(P1P2) + Alph2;

  // Calculation of the length of sliding (imposed or initial);

  if (!NewFreeSliding)
  {
    SlidingLength = NewSlidingFactor * LReference;
  }
  else
  {
    if (OldFreeSliding)
    {
      SlidingLength = OldSlidingFactor * LReference;
    }
    else
    {
      SlidingLength = SlidingOfReference(Dist, Alph1, Alph2);
    }
  }

  // Energy and vectors of initialization
  FairCurve_BattenLaw   LBatten(NewHeight, NewSlope, SlidingLength);
  FairCurve_EnergyOfMVC EMVC(Degree + 1,
                             Flatknots,
                             NPoles,
                             NewConstraintOrder1,
                             NewConstraintOrder2,
                             LBatten,
                             NewPhysicalRatio,
                             SlidingLength,
                             NewFreeSliding,
                             Angle1,
                             Angle2,
                             Rho1,
                             Rho2);
  math_Vector           VInit(1, EMVC.NbVariables());

  // The value below gives an idea about the smallest value of the criterion of flexion.
  Standard_Real VConvex = 0.01 * pow(NewHeight / SlidingLength, 3);
  if (VConvex < 1.e-12)
  {
    VConvex = 1.e-12;
  }

  Ok = EMVC.Variable(VInit);

  // Minimisation
  FairCurve_Newton Newton(EMVC,
                          Tolerance * (P1P2.Magnitude() / 10),
                          Tolerance,
                          NbIterations,
                          VConvex);
  Newton.Perform(EMVC, VInit);
  Ok = Newton.IsDone();

  if (Ok)
  {
    gp_Vec2d Tangente, PseudoNormale;
    Poles = NPoles;
    Newton.Location(VInit);

    if (NewFreeSliding)
    {
      OldSlidingFactor = VInit(VInit.Upper()) / LReference;
    }
    else
    {
      OldSlidingFactor = NewSlidingFactor;
    }

    if (NewConstraintOrder1 < 2)
    {
      Tangente.SetXY(Poles->Value(Poles->Lower() + 1).XY() - Poles->Value(Poles->Lower()).XY());

      if (NewConstraintOrder1 == 0)
      {
        OldAngle1 = P1P2.Angle(Tangente);
      }
      else
      {
        OldAngle1 = Alph1;
      }

      PseudoNormale.SetXY(Poles->Value(Poles->Lower()).XY()
                          - 2 * Poles->Value(Poles->Lower() + 1).XY()
                          + Poles->Value(Poles->Lower() + 2).XY());
      OldCurvature1 = (((double)Degree - 1) / Degree) * (Tangente.Normalized() ^ PseudoNormale)
                      / Tangente.SquareMagnitude();
    }
    else
    {
      OldCurvature1 = Rho1;
      OldAngle1     = Alph1;
    }

    if (NewConstraintOrder2 < 2)
    {
      Tangente.SetXY(Poles->Value(Poles->Upper()).XY() - Poles->Value(Poles->Upper() - 1).XY());
      if (NewConstraintOrder2 == 0)
        OldAngle2 = (-Tangente).Angle(-P1P2);
      else
      {
        OldAngle2 = Alph2;
      }
      PseudoNormale.SetXY(Poles->Value(Poles->Upper()).XY()
                          - 2 * Poles->Value(Poles->Upper() - 1).XY()
                          + Poles->Value(Poles->Upper() - 2).XY());
      OldCurvature2 = (((double)Degree - 1) / Degree) * (Tangente.Normalized() ^ PseudoNormale)
                      / Tangente.SquareMagnitude();
    }
    else
    {
      OldAngle2     = Alph2;
      OldCurvature2 = Rho2;
    }

    OldP1               = Poles->Value(Poles->Lower());
    OldP2               = Poles->Value(Poles->Upper());
    OldConstraintOrder1 = NewConstraintOrder1;
    OldConstraintOrder2 = NewConstraintOrder2;
    OldFreeSliding      = NewFreeSliding;
    OldSlope            = NewSlope;
    OldHeight           = NewHeight;
    OldPhysicalRatio    = NewPhysicalRatio;
  }
  else
  {
    Standard_Real V;
    ACode = EMVC.Status();
    if (!LBatten.Value(0, V) || !LBatten.Value(1, V))
    {
      ACode = FairCurve_NullHeight;
    }
    else
    {
      OkCompute = Standard_False;
    }
    return OkCompute;
  }

  Ok = EMVC.Variable(VInit);

  // Processing of non convergence
  if (!Newton.IsConverged())
  {
    ACode = FairCurve_NotConverged;
  }

  // Prevention of infinite sliding
  if (NewFreeSliding && VInit(VInit.Upper()) > 2 * LReference)
    ACode = FairCurve_InfiniteSliding;

  // Eventual insertion of Nodes
  Standard_Boolean NewKnots  = Standard_False;
  Standard_Integer NbKnots   = Knots->Length();
  Standard_Real    ValAngles = (Abs(OldAngle1) + Abs(OldAngle2) + 2 * Abs(OldAngle2 - OldAngle1));
  while (ValAngles > (2 * (NbKnots - 2) + 1) * (1 + 2 * NbKnots))
  {
    NewKnots = Standard_True;
    NbKnots += NbKnots - 1;
  }

  if (NewKnots)
  {
    Handle(Geom2d_BSplineCurve) NewBS =
      new Geom2d_BSplineCurve(NPoles->Array1(), Knots->Array1(), Mults->Array1(), Degree);

    Handle(TColStd_HArray1OfInteger) NMults = new TColStd_HArray1OfInteger(1, NbKnots);
    NMults->Init(Degree - 3);

    Handle(TColStd_HArray1OfReal) NKnots = new TColStd_HArray1OfReal(1, NbKnots);
    for (ii = 1; ii <= NbKnots; ii++)
    {
      NKnots->ChangeValue(ii) = (double)(ii - 1) / (NbKnots - 1);
    }

    NewBS->InsertKnots(NKnots->Array1(), NMults->Array1(), 1.e-10);
    Handle(TColgp_HArray1OfPnt2d) NewNPoles = new TColgp_HArray1OfPnt2d(1, NewBS->NbPoles());
    NewBS->Poles(NewNPoles->ChangeArray1());
    NewBS->Multiplicities(NMults->ChangeArray1());
    NewBS->Knots(NKnots->ChangeArray1());
    Handle(TColStd_HArray1OfReal) FKnots =
      new TColStd_HArray1OfReal(1, NewBS->NbPoles() + Degree + 1);
    NewBS->KnotSequence(FKnots->ChangeArray1());

    Poles     = NewNPoles;
    Mults     = NMults;
    Knots     = NKnots;
    Flatknots = FKnots;
  }

  // For eventual debug Newton.Dump(std::cout);

  return OkCompute;
}

//======================================================================================
void FairCurve_MinimalVariation::Dump(Standard_OStream& o) const
//======================================================================================
{

  o << "  MVCurve      |";
  o.width(7);
  o << "Old  |   New" << std::endl;
  o << "  P1    X      |";
  o.width(7);
  o << OldP1.X() << " | " << NewP1.X() << std::endl;
  o << "        Y      |";
  o.width(7);
  o << OldP1.Y() << " | " << NewP1.Y() << std::endl;
  o << "  P2    X      |";
  o.width(7);
  o << OldP2.X() << " | " << NewP2.X() << std::endl;
  o << "        Y      |";
  o.width(7);
  o << OldP2.Y() << " | " << NewP2.Y() << std::endl;
  o << "      Angle1   |";
  o.width(7);
  o << OldAngle1 << " | " << NewAngle1 << std::endl;
  o << "      Angle2   |";
  o.width(7);
  o << OldAngle2 << " | " << NewAngle2 << std::endl;
  o << " Curvature1    |";
  o.width(7);
  o << OldCurvature1 << " | " << NewCurvature1 << std::endl;
  o << " Curvature2    |";
  o.width(7);
  o << OldCurvature2 << " | " << NewCurvature2 << std::endl;
  o << "      Height   |";
  o.width(7);
  o << OldHeight << " | " << NewHeight << std::endl;
  o << "      Slope    |";
  o.width(7);
  o << OldSlope << " | " << NewSlope << std::endl;
  o << " PhysicalRatio |";
  o.width(7);
  o << OldPhysicalRatio << " | " << NewPhysicalRatio << std::endl;
  o << " SlidingFactor |";
  o.width(7);
  o << OldSlidingFactor << " | " << NewSlidingFactor << std::endl;
  o << " FreeSliding   |";
  o.width(7);
  o << OldFreeSliding << " | " << NewFreeSliding << std::endl;
  o << " ConstrOrder1  |";
  o.width(7);
  o << OldConstraintOrder1 << " | " << NewConstraintOrder1 << std::endl;
  o << " ConstrOrder2  |";
  o.width(7);
  o << OldConstraintOrder2 << " | " << NewConstraintOrder2 << std::endl;
  switch (myCode)
  {
    case FairCurve_OK:
      o << "AnalysisCode : Ok" << std::endl;
      break;
    case FairCurve_NotConverged:
      o << "AnalysisCode : NotConverged" << std::endl;
      break;
    case FairCurve_InfiniteSliding:
      o << "AnalysisCode : InfiniteSliding" << std::endl;
      break;
    case FairCurve_NullHeight:
      o << "AnalysisCode : NullHeight" << std::endl;
      break;
  }
}
