OCCT

// Copyright (c) 2025 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 GeomBndLib_OptimizationHelpers_HeaderFile
#define GeomBndLib_OptimizationHelpers_HeaderFile

#include <Adaptor3d_Curve.hxx>
#include <Adaptor2d_Curve2d.hxx>
#include <Adaptor3d_Surface.hxx>

#include <type_traits>
#include <Precision.hxx>
#include <MathOpt_Brent.hxx>
#include <MathOpt_PSO.hxx>
#include <MathOpt_Powell.hxx>
#include <math_Vector.hxx>
#include <math_Matrix.hxx>

//! @file GeomBndLib_OptimizationHelpers.pxx
//! PSO+Brent and PSO+Powell optimization wrappers for precise bounding box computation.

namespace GeomBndLib_OptimizationHelpers
{

// ============================================================
// Internal function objects for optimization
// ============================================================

//! 1D function object for coordinate minimization/maximization along a 3D curve.
class CurveCoordFunc
{
public:
  CurveCoordFunc(const Adaptor3d_Curve& theCurve,
                 const double           theUMin,
                 const double           theUMax,
                 const int              theCoordIdx,
                 const double           theSign)
      : myCurve(theCurve),
        myUMin(theUMin),
        myUMax(theUMax),
        myCoordIdx(theCoordIdx),
        mySign(theSign)
  {
  }

  bool Value(const double theX, double& theF)
  {
    if (theX < myUMin || theX > myUMax)
    {
      return false;
    }
    gp_Pnt aP = myCurve.Value(theX);
    theF      = mySign * aP.Coord(myCoordIdx);
    return true;
  }

private:
  CurveCoordFunc& operator=(const CurveCoordFunc&) = delete;

  const Adaptor3d_Curve& myCurve;
  double                 myUMin;
  double                 myUMax;
  int                    myCoordIdx;
  double                 mySign;
};

//! Multi-variable function for PSO on a 3D curve.
class CurveCoordMVarFunc
{
public:
  CurveCoordMVarFunc(const Adaptor3d_Curve& theCurve,
                     const double           theUMin,
                     const double           theUMax,
                     const int              theCoordIdx,
                     const double           theSign)
      : myCurve(theCurve),
        myUMin(theUMin),
        myUMax(theUMax),
        myCoordIdx(theCoordIdx),
        mySign(theSign)
  {
  }

  bool Value(const math_Vector& theX, double& theF)
  {
    if (theX.At(0) < myUMin || theX.At(0) > myUMax)
    {
      return false;
    }
    gp_Pnt aP = myCurve.Value(theX.At(0));
    theF      = mySign * aP.Coord(myCoordIdx);
    return true;
  }

  int NbVariables() const { return 1; }

private:
  CurveCoordMVarFunc& operator=(const CurveCoordMVarFunc&) = delete;

  const Adaptor3d_Curve& myCurve;
  double                 myUMin;
  double                 myUMax;
  int                    myCoordIdx;
  double                 mySign;
};

//! 1D function object for coordinate minimization/maximization along a 2D curve.
class Curve2dCoordFunc
{
public:
  Curve2dCoordFunc(const Adaptor2d_Curve2d& theCurve,
                   const double             theUMin,
                   const double             theUMax,
                   const int                theCoordIdx,
                   const double             theSign)
      : myCurve(theCurve),
        myUMin(theUMin),
        myUMax(theUMax),
        myCoordIdx(theCoordIdx),
        mySign(theSign)
  {
  }

  bool Value(const double theX, double& theF)
  {
    if (theX < myUMin || theX > myUMax)
    {
      return false;
    }
    const gp_Pnt2d aP = myCurve.Value(theX);
    theF              = mySign * aP.Coord(myCoordIdx);
    return true;
  }

private:
  Curve2dCoordFunc& operator=(const Curve2dCoordFunc&) = delete;

  const Adaptor2d_Curve2d& myCurve;
  double                   myUMin;
  double                   myUMax;
  int                      myCoordIdx;
  double                   mySign;
};

//! Multi-variable function for PSO on a 2D curve.
class Curve2dCoordMVarFunc
{
public:
  Curve2dCoordMVarFunc(const Adaptor2d_Curve2d& theCurve,
                       const double             theUMin,
                       const double             theUMax,
                       const int                theCoordIdx,
                       const double             theSign)
      : myCurve(theCurve),
        myUMin(theUMin),
        myUMax(theUMax),
        myCoordIdx(theCoordIdx),
        mySign(theSign)
  {
  }

  bool Value(const math_Vector& theX, double& theF)
  {
    if (theX.At(0) < myUMin || theX.At(0) > myUMax)
    {
      return false;
    }
    const gp_Pnt2d aP = myCurve.Value(theX.At(0));
    theF              = mySign * aP.Coord(myCoordIdx);
    return true;
  }

  int NbVariables() const { return 1; }

private:
  Curve2dCoordMVarFunc& operator=(const Curve2dCoordMVarFunc&) = delete;

  const Adaptor2d_Curve2d& myCurve;
  double                   myUMin;
  double                   myUMax;
  int                      myCoordIdx;
  double                   mySign;
};

//! Multi-variable function for PSO+Powell on a surface.
class SurfCoordFunc
{
public:
  SurfCoordFunc(const Adaptor3d_Surface& theSurf,
                const double             theUMin,
                const double             theUMax,
                const double             theVMin,
                const double             theVMax,
                const int                theCoordIdx,
                const double             theSign)
      : mySurf(theSurf),
        myUMin(theUMin),
        myUMax(theUMax),
        myVMin(theVMin),
        myVMax(theVMax),
        myCoordIdx(theCoordIdx),
        mySign(theSign),
        myPenalty(0.)
  {
    math_Vector aX(size_t{2});
    aX.ChangeAt(0) = theUMin;
    aX.ChangeAt(1) = (theVMin + theVMax) / 2.;
    double aF1, aF2;
    Value(aX, aF1);
    aX.ChangeAt(0) = theUMax;
    Value(aX, aF2);
    double aDU     = std::abs((aF2 - aF1) / (theUMax - theUMin));
    aX.ChangeAt(0) = (theUMin + theUMax) / 2.;
    aX.ChangeAt(1) = theVMin;
    Value(aX, aF1);
    aX.ChangeAt(1) = theVMax;
    Value(aX, aF2);
    double aDV = std::abs((aF2 - aF1) / (theVMax - theVMin));
    myPenalty  = 10. * std::max(aDU, aDV);
    myPenalty  = std::max(myPenalty, 1.);
  }

  bool Value(const math_Vector& theX, double& theF)
  {
    if (theX.At(0) >= myUMin && theX.At(0) <= myUMax && theX.At(1) >= myVMin
        && theX.At(1) <= myVMax)
    {
      gp_Pnt aP = mySurf.Value(theX.At(0), theX.At(1));
      theF      = mySign * aP.Coord(myCoordIdx);
    }
    else
    {
      double aUPen = 0., aVPen = 0., u0, v0;
      if (theX.At(0) < myUMin)
      {
        aUPen = myPenalty * (myUMin - theX.At(0));
        u0    = myUMin;
      }
      else if (theX.At(0) > myUMax)
      {
        aUPen = myPenalty * (theX.At(0) - myUMax);
        u0    = myUMax;
      }
      else
      {
        u0 = theX.At(0);
      }
      if (theX.At(1) < myVMin)
      {
        aVPen = myPenalty * (myVMin - theX.At(1));
        v0    = myVMin;
      }
      else if (theX.At(1) > myVMax)
      {
        aVPen = myPenalty * (theX.At(1) - myVMax);
        v0    = myVMax;
      }
      else
      {
        v0 = theX.At(1);
      }
      gp_Pnt aP = mySurf.Value(u0, v0);
      theF      = mySign * aP.Coord(myCoordIdx) + aUPen + aVPen;
    }
    return true;
  }

  int NbVariables() const { return 2; }

private:
  SurfCoordFunc& operator=(const SurfCoordFunc&) = delete;

  const Adaptor3d_Surface& mySurf;
  double                   myUMin;
  double                   myUMax;
  double                   myVMin;
  double                   myVMax;
  int                      myCoordIdx;
  double                   mySign;
  double                   myPenalty;
};

// ============================================================
// Public helper functions
// ============================================================

//! Adjust coordinate extremum using PSO + Brent for 1D parameter curves.
//! @param[in] theCurve    the adaptor curve
//! @param[in] theUMin     lower parameter bound for search
//! @param[in] theUMax     upper parameter bound for search
//! @param[in] theExtr0    current extremum value
//! @param[in] theCoordIdx coordinate index (1=X, 2=Y, 3=Z)
//! @param[in] theTol      tolerance
//! @param[in] theIsMin    true if looking for minimum, false for maximum
//! @return adjusted extremum value
inline double AdjustExtrCurve(const Adaptor3d_Curve& theCurve,
                              const double           theUMin,
                              const double           theUMax,
                              const double           theExtr0,
                              const int              theCoordIdx,
                              const double           theTol,
                              const bool             theIsMin)
{
  double aSign = theIsMin ? 1. : -1.;
  double extr  = aSign * theExtr0;

  double uTol = std::max(theCurve.Resolution(theTol), Precision::PConfusion());
  double Du   = (theCurve.LastParameter() - theCurve.FirstParameter());

  double          reltol = uTol / std::max(std::abs(theUMin), std::abs(theUMax));
  MathOpt::Config aBrentCfg;
  aBrentCfg.XTolerance    = reltol;
  aBrentCfg.FTolerance    = uTol;
  aBrentCfg.MaxIterations = 100;

  if (theUMax - theUMin < 0.01 * Du)
  {
    CurveCoordFunc        aFunc(theCurve, theUMin, theUMax, theCoordIdx, aSign);
    MathOpt::ScalarResult anOptLoc = MathOpt::Brent(aFunc, theUMin, theUMax, aBrentCfg);
    if (anOptLoc.Status == MathOpt::Status::OK || anOptLoc.Status == MathOpt::Status::MaxIterations)
    {
      if (anOptLoc.Value.has_value())
      {
        extr = anOptLoc.Value.value();
      }
      return aSign * extr;
    }
  }

  int         aNbParticles = std::max(8, RealToInt(32 * (theUMax - theUMin) / Du));
  math_Vector aLowBorder(size_t{1}, theUMin);
  math_Vector aUppBorder(size_t{1}, theUMax);

  CurveCoordMVarFunc    aFunc(theCurve, theUMin, theUMax, theCoordIdx, aSign);
  MathOpt::PSOConfig    aPsoCfg(aNbParticles);
  MathOpt::VectorResult aPsoRes = MathOpt::PSO(aFunc, aLowBorder, aUppBorder, aPsoCfg);

  math_Vector aT(size_t{1});
  if (aPsoRes.Status == MathOpt::Status::OK || aPsoRes.Status == MathOpt::Status::MaxIterations)
  {
    if (aPsoRes.Solution.has_value())
    {
      aT.ChangeAt(0) = aPsoRes.Solution->At(0);
    }
    else
    {
      aT.ChangeAt(0) = (theUMin + theUMax) / 2.0;
    }
    if (aPsoRes.Value.has_value())
    {
      extr = aPsoRes.Value.value();
    }
  }
  else
  {
    aT.ChangeAt(0) = (theUMin + theUMax) / 2.0;
  }

  CurveCoordFunc        aFunc1(theCurve, theUMin, theUMax, theCoordIdx, aSign);
  double                aStep    = std::min(0.1 * Du, (theUMax - theUMin) / (aNbParticles + 1));
  MathOpt::ScalarResult anOptLoc = MathOpt::Brent(aFunc1,
                                                  std::max(aT.At(0) - aStep, theUMin),
                                                  std::min(aT.At(0) + aStep, theUMax),
                                                  aBrentCfg);

  if (anOptLoc.Status == MathOpt::Status::OK || anOptLoc.Status == MathOpt::Status::MaxIterations)
  {
    if (anOptLoc.Value.has_value())
    {
      extr = anOptLoc.Value.value();
      if (aPsoRes.Value.has_value() && aPsoRes.Solution.has_value()
          && aT.At(0) == aPsoRes.Solution->At(0))
      {
        extr = std::min(extr, aPsoRes.Value.value());
      }
      return aSign * extr;
    }
  }

  return aSign * extr;
}

//! Adjust coordinate extremum using PSO + Brent for 1D parameter 2D curves.
//! @param[in] theCurve    the adaptor curve
//! @param[in] theUMin     lower parameter bound for search
//! @param[in] theUMax     upper parameter bound for search
//! @param[in] theExtr0    current extremum value
//! @param[in] theCoordIdx coordinate index (1=X, 2=Y)
//! @param[in] theTol      tolerance
//! @param[in] theIsMin    true if looking for minimum, false for maximum
//! @return adjusted extremum value
inline double AdjustExtrCurve2d(const Adaptor2d_Curve2d& theCurve,
                                const double             theUMin,
                                const double             theUMax,
                                const double             theExtr0,
                                const int                theCoordIdx,
                                const double             theTol,
                                const bool               theIsMin)
{
  double aSign = theIsMin ? 1. : -1.;
  double extr  = aSign * theExtr0;

  const double uTol = std::max(theCurve.Resolution(theTol), Precision::PConfusion());
  const double Du   = (theCurve.LastParameter() - theCurve.FirstParameter());

  const double reltol = uTol / std::max(std::abs(theUMin), std::abs(theUMax));

  MathOpt::Config aBrentCfg;
  aBrentCfg.XTolerance    = reltol;
  aBrentCfg.FTolerance    = uTol;
  aBrentCfg.MaxIterations = 100;

  if (theUMax - theUMin < 0.01 * Du)
  {
    Curve2dCoordFunc      aFunc(theCurve, theUMin, theUMax, theCoordIdx, aSign);
    MathOpt::ScalarResult anOptLoc = MathOpt::Brent(aFunc, theUMin, theUMax, aBrentCfg);
    if (anOptLoc.Status == MathOpt::Status::OK || anOptLoc.Status == MathOpt::Status::MaxIterations)
    {
      if (anOptLoc.Value.has_value())
      {
        extr = anOptLoc.Value.value();
      }
      return aSign * extr;
    }
  }

  const int   aNbParticles = std::max(8, RealToInt(32 * (theUMax - theUMin) / Du));
  math_Vector aLowBorder(size_t{1}, theUMin);
  math_Vector aUppBorder(size_t{1}, theUMax);

  Curve2dCoordMVarFunc  aFunc(theCurve, theUMin, theUMax, theCoordIdx, aSign);
  MathOpt::PSOConfig    aPsoCfg(aNbParticles);
  MathOpt::VectorResult aPsoRes = MathOpt::PSO(aFunc, aLowBorder, aUppBorder, aPsoCfg);

  math_Vector aT(size_t{1});
  if (aPsoRes.Status == MathOpt::Status::OK || aPsoRes.Status == MathOpt::Status::MaxIterations)
  {
    if (aPsoRes.Solution.has_value())
    {
      aT.ChangeAt(0) = aPsoRes.Solution->At(0);
    }
    else
    {
      aT.ChangeAt(0) = (theUMin + theUMax) / 2.0;
    }
    if (aPsoRes.Value.has_value())
    {
      extr = aPsoRes.Value.value();
    }
  }
  else
  {
    aT.ChangeAt(0) = (theUMin + theUMax) / 2.0;
  }

  const double          aStep = std::min(0.1 * Du, (theUMax - theUMin) / (aNbParticles + 1));
  Curve2dCoordFunc      aFunc1(theCurve, theUMin, theUMax, theCoordIdx, aSign);
  MathOpt::ScalarResult anOptLoc = MathOpt::Brent(aFunc1,
                                                  std::max(aT.At(0) - aStep, theUMin),
                                                  std::min(aT.At(0) + aStep, theUMax),
                                                  aBrentCfg);

  if (anOptLoc.Status == MathOpt::Status::OK || anOptLoc.Status == MathOpt::Status::MaxIterations)
  {
    if (anOptLoc.Value.has_value())
    {
      extr = anOptLoc.Value.value();
      if (aPsoRes.Value.has_value() && aPsoRes.Solution.has_value()
          && aT.At(0) == aPsoRes.Solution->At(0))
      {
        extr = std::min(extr, aPsoRes.Value.value());
      }
      return aSign * extr;
    }
  }

  return aSign * extr;
}

//! Adjust coordinate extremum using PSO + Powell for 2D parameter surfaces.
//! @param[in] theSurf     the adaptor surface
//! @param[in] theUMin     lower U bound
//! @param[in] theUMax     upper U bound
//! @param[in] theVMin     lower V bound
//! @param[in] theVMax     upper V bound
//! @param[in] theExtr0    current extremum value
//! @param[in] theCoordIdx coordinate index (1=X, 2=Y, 3=Z)
//! @param[in] theTol      tolerance
//! @param[in] theIsMin    true if looking for minimum, false for maximum
//! @return adjusted extremum value
inline double AdjustExtrSurf(const Adaptor3d_Surface& theSurf,
                             const double             theUMin,
                             const double             theUMax,
                             const double             theVMin,
                             const double             theVMax,
                             const double             theExtr0,
                             const int                theCoordIdx,
                             const double             theTol,
                             const bool               theIsMin)
{
  double aSign  = theIsMin ? 1. : -1.;
  double extr   = aSign * theExtr0;
  double relTol = 2. * theTol;
  if (std::abs(extr) > theTol)
  {
    relTol /= std::abs(extr);
  }
  double Du = (theSurf.LastUParameter() - theSurf.FirstUParameter());
  double Dv = (theSurf.LastVParameter() - theSurf.FirstVParameter());

  math_Vector aLowBorder(size_t{2});
  math_Vector aUppBorder(size_t{2});
  aLowBorder.ChangeAt(0) = theUMin;
  aUppBorder.ChangeAt(0) = theUMax;
  aLowBorder.ChangeAt(1) = theVMin;
  aUppBorder.ChangeAt(1) = theVMax;

  int aNbU         = std::max(8, RealToInt(32 * (theUMax - theUMin) / Du));
  int aNbV         = std::max(8, RealToInt(32 * (theVMax - theVMin) / Dv));
  int aNbParticles = aNbU * aNbV;

  SurfCoordFunc         aFunc(theSurf, theUMin, theUMax, theVMin, theVMax, theCoordIdx, aSign);
  MathOpt::PSOConfig    aPsoCfg(aNbParticles);
  MathOpt::VectorResult aPsoRes = MathOpt::PSO(aFunc, aLowBorder, aUppBorder, aPsoCfg);

  math_Vector aT(size_t{2});
  if (aPsoRes.Status == MathOpt::Status::OK || aPsoRes.Status == MathOpt::Status::MaxIterations)
  {
    if (aPsoRes.Solution.has_value())
    {
      aT.ChangeAt(0) = aPsoRes.Solution->At(0);
      aT.ChangeAt(1) = aPsoRes.Solution->At(1);
    }
    else
    {
      aT.ChangeAt(0) = (theUMin + theUMax) / 2.0;
      aT.ChangeAt(1) = (theVMin + theVMax) / 2.0;
    }
    if (aPsoRes.Value.has_value())
    {
      extr = aPsoRes.Value.value();
    }
  }
  else
  {
    aT.ChangeAt(0) = (theUMin + theUMax) / 2.0;
    aT.ChangeAt(1) = (theVMin + theVMax) / 2.0;
  }

  math_Matrix aDir(size_t{2}, size_t{2}, 0.0);
  aDir.ChangeAt(0, 0) = 1.;
  aDir.ChangeAt(1, 1) = 1.;

  MathOpt::Config aPowellCfg;
  aPowellCfg.XTolerance    = relTol;
  aPowellCfg.FTolerance    = theTol;
  aPowellCfg.MaxIterations = 200;

  MathOpt::VectorResult powell = MathOpt::PowellWithDirections(aFunc, aT, aDir, aPowellCfg);

  if (powell.Status == MathOpt::Status::OK || powell.Status == MathOpt::Status::MaxIterations)
  {
    if (powell.Value.has_value())
    {
      extr = std::min(extr, powell.Value.value());
    }
  }

  return aSign * extr;
}

//! Dispatch to AdjustExtrCurve or AdjustExtrCurve2d based on adaptor type.
template <typename TAdaptor>
inline double AdjustExtrT(const TAdaptor& theCurve,
                          const double    theUMin,
                          const double    theUMax,
                          const double    theExtr0,
                          const int       theCoordIdx,
                          const double    theTol,
                          const bool      theIsMin)
{
  if constexpr (std::is_base_of_v<Adaptor3d_Curve, TAdaptor>)
    return AdjustExtrCurve(theCurve, theUMin, theUMax, theExtr0, theCoordIdx, theTol, theIsMin);
  else
    return AdjustExtrCurve2d(theCurve, theUMin, theUMax, theExtr0, theCoordIdx, theTol, theIsMin);
}

} // namespace GeomBndLib_OptimizationHelpers

#endif // GeomBndLib_OptimizationHelpers_HeaderFile