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 _MathSys_Newton4D_HeaderFile
#define _MathSys_Newton4D_HeaderFile

#include <MathSys_NewtonTypes.hxx>

#include <gp_Pnt.hxx>
#include <gp_Vec.hxx>

#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <optional>

//! @file MathSys_Newton4D.hxx
//! @brief Optimized 4D Newton-Raphson solver for systems of 4 equations in 4 unknowns.
//!
//! This solver is specifically optimized for 4D problems like:
//! - Surface-surface extrema (find u1,v1,u2,v2 where distance is extremal)
//! - Curve-surface intersection with tangent constraints
//! - Two-curve closest point with additional constraints
//!
//! Optimizations compared to general Newton:
//! - No math_Vector/math_Matrix allocation overhead
//! - Gaussian elimination with partial pivoting for 4x4 system
//! - Overflow-safe norm comparisons
//! - Step limiting to prevent wild oscillations
//! - Gradient descent fallback for singular Jacobian

namespace MathSys
{
namespace Utils
{

//! Check that NewtonBoundsN<4> has valid (min <= max) ranges.
inline bool IsBoundsValid4D(const NewtonBoundsN<4>& theBounds)
{
  if (!theBounds.HasBounds)
  {
    return true;
  }

  return IsFiniteArray(theBounds.Min) && IsFiniteArray(theBounds.Max)
         && theBounds.Min[0] <= theBounds.Max[0] && theBounds.Min[1] <= theBounds.Max[1]
         && theBounds.Min[2] <= theBounds.Max[2] && theBounds.Min[3] <= theBounds.Max[3];
}

//! Check that NewtonOptions fields are positive and valid.
inline bool IsOptionsValid4D(const NewtonOptions& theOptions)
{
  return std::isfinite(theOptions.FTolerance) && theOptions.FTolerance > 0.0
         && std::isfinite(theOptions.XTolerance) && theOptions.XTolerance > 0.0
         && theOptions.MaxIterations > 0 && std::isfinite(theOptions.MaxStepRatio)
         && theOptions.MaxStepRatio > 0.0 && std::isfinite(theOptions.SoftBoundsExtension)
         && theOptions.SoftBoundsExtension >= 0.0;
}

//! Return the step limit for a bounded problem; no value means no step limit.
inline std::optional<double> MaxStep4D(const NewtonBoundsN<4>& theBounds, double theMaxStepRatio)
{
  if (!theBounds.HasBounds)
  {
    return std::nullopt;
  }

  const double aD0 = theBounds.Max[0] - theBounds.Min[0];
  const double aD1 = theBounds.Max[1] - theBounds.Min[1];
  const double aD2 = theBounds.Max[2] - theBounds.Min[2];
  const double aD3 = theBounds.Max[3] - theBounds.Min[3];
  return theMaxStepRatio * std::max(1.0, std::max(aD0, std::max(aD1, std::max(aD2, aD3))));
}

//! Clamp solution array to bounds, optionally extending by soft-bounds ratio.
inline void Clamp4D(std::array<double, 4>&  theX,
                    const NewtonBoundsN<4>& theBounds,
                    bool                    theUseSoftBounds,
                    double                  theSoftExtRatio)
{
  if (!theBounds.HasBounds)
  {
    return;
  }

  for (size_t i = 0; i < 4; ++i)
  {
    const double aExt =
      theUseSoftBounds ? (theBounds.Max[i] - theBounds.Min[i]) * theSoftExtRatio : 0.0;
    theX[i] = std::clamp(theX[i], theBounds.Min[i] - aExt, theBounds.Max[i] + aExt);
  }
}

//! Solve 4x4 linear system using Gaussian elimination with partial pivoting.
//! @param[in] theJ 4x4 Jacobian matrix
//! @param[in] theF 4-element right-hand side
//! @param[out] theDelta 4-element solution vector
//! @return true if system was solved successfully
inline bool Solve4x4(const double theJ[4][4], const double theF[4], double theDelta[4])
{
  // Augmented matrix [J | -F]
  double A[4][5];
  for (size_t i = 0; i < 4; ++i)
  {
    const double aRowScale = std::max(
      {std::abs(theJ[i][0]), std::abs(theJ[i][1]), std::abs(theJ[i][2]), std::abs(theJ[i][3])});
    if (!(aRowScale > 0.0))
    {
      return false;
    }
    for (size_t j = 0; j < 4; ++j)
    {
      A[i][j] = theJ[i][j] / aRowScale;
    }
    A[i][4] = -theF[i] / aRowScale;
    if (!std::isfinite(A[i][4]))
    {
      return false;
    }
  }

  // Forward elimination with partial pivoting
  for (size_t k = 0; k < 4; ++k)
  {
    // Find pivot
    size_t aMaxRow = k;
    double aMaxVal = std::abs(A[k][k]);
    for (size_t i = k + 1; i < 4; ++i)
    {
      const double aVal = std::abs(A[i][k]);
      if (aVal > aMaxVal)
      {
        aMaxVal = aVal;
        aMaxRow = i;
      }
    }

    // Check for singularity
    if (aMaxVal <= THE_NEWTON_PIVOT_TOL)
    {
      return false;
    }

    // Swap rows if needed
    if (aMaxRow != k)
    {
      for (size_t j = k; j <= 4; ++j)
      {
        std::swap(A[k][j], A[aMaxRow][j]);
      }
    }

    // Eliminate column
    const double aInvPivot = 1.0 / A[k][k];
    for (size_t i = k + 1; i < 4; ++i)
    {
      const double aFactor = A[i][k] * aInvPivot;
      for (size_t j = k + 1; j <= 4; ++j)
      {
        A[i][j] -= aFactor * A[k][j];
      }
      A[i][k] = 0.0;
    }
  }

  // Back substitution
  for (size_t i = 4; i-- > 0;)
  {
    double aSum = A[i][4];
    for (size_t j = i + 1; j < 4; ++j)
    {
      aSum -= A[i][j] * theDelta[j];
    }
    theDelta[i] = aSum / A[i][i];
  }

  return std::isfinite(theDelta[0]) && std::isfinite(theDelta[1]) && std::isfinite(theDelta[2])
         && std::isfinite(theDelta[3]);
}

} // namespace Utils

//! Solve a 4x4 nonlinear system by Newton iteration with bounds.
//!
//! Solves the system [F1, F2, F3, F4] = [0, 0, 0, 0] using Newton-Raphson iteration
//! with Gaussian elimination for the 4x4 linear system at each step.
//!
//! The function type must be callable with signature:
//! @code
//!   bool operator()(double theX1, double theX2, double theX3, double theX4,
//!                   double theF[4], double theJ[4][4]) const;
//! @endcode
//! where theF is the function values and theJ is the 4x4 Jacobian matrix.
//!
//! @tparam Function callable type (functor, lambda, or function pointer)
//! @param[in] theFunc function to solve (provides F and Jacobian)
//! @param[in] theX0 initial guess {x1, x2, x3, x4}
//! @param[in] theBounds box bounds for each variable
//! @param[in] theOptions solver options (tolerances, max iterations, etc.)
//! @return NewtonResultN<4> containing solution, status, and diagnostics
template <typename Function>
NewtonResultN<4> Solve4D(const Function&              theFunc,
                         const std::array<double, 4>& theX0,
                         const NewtonBoundsN<4>&      theBounds,
                         const NewtonOptions&         theOptions = NewtonOptions())
{
  NewtonResultN<4> aRes;
  aRes.X = theX0;

  if (!Utils::IsOptionsValid4D(theOptions) || !Utils::IsBoundsValid4D(theBounds)
      || !Utils::IsFiniteArray(theX0))
  {
    aRes.Status = MathUtils::Status::InvalidInput;
    return aRes;
  }

  Utils::Clamp4D(aRes.X, theBounds, theOptions.AllowSoftBounds, theOptions.SoftBoundsExtension);

  const std::optional<double> aMaxStep = Utils::MaxStep4D(theBounds, theOptions.MaxStepRatio);

  for (uint32_t anIter = 0; anIter < theOptions.MaxIterations; ++anIter)
  {
    aRes.NbIterations = anIter + 1;

    double aF[4];
    double aJ[4][4];
    if (!theFunc(aRes.X[0], aRes.X[1], aRes.X[2], aRes.X[3], aF, aJ)
        || !Utils::IsFiniteSystemN(aF, aJ))
    {
      aRes.Status = MathUtils::Status::NumericalError;
      return aRes;
    }

    aRes.ResidualNorm = Utils::SafeNormN(aF);
    if (aRes.ResidualNorm <= theOptions.FTolerance)
    {
      aRes.Status = MathUtils::Status::OK;
      return aRes;
    }

    // Solve 4x4 linear system: J * delta = -F
    double aDelta[4];
    if (!Utils::Solve4x4(aJ, aF, aDelta))
    {
      // Singular Jacobian - try steepest descent direction: -J^T * F
      double aGrad[4] = {0.0, 0.0, 0.0, 0.0};
      for (size_t c = 0; c < 4; ++c)
      {
        for (size_t r = 0; r < 4; ++r)
        {
          aGrad[c] += aJ[r][c] * aF[r];
        }
      }

      const double aGradNorm = Utils::SafeNormN(aGrad);
      if (aGradNorm < 1.0e-30)
      {
        aRes.Status = MathUtils::Status::Singular;
        return aRes;
      }

      const double aAlpha = std::min(1.0, aRes.ResidualNorm / aGradNorm * 0.1);
      for (size_t i = 0; i < 4; ++i)
      {
        aDelta[i] = -aAlpha * aGrad[i];
      }
    }

    // Limit step size to prevent wild oscillations
    const double aStepNorm = Utils::SafeNormN(aDelta);
    if (aMaxStep && aStepNorm > *aMaxStep)
    {
      const double aScale = *aMaxStep / aStepNorm;
      for (size_t i = 0; i < 4; ++i)
      {
        aDelta[i] *= aScale;
      }
    }

    std::array<double, 4> aNewX = aRes.X;
    if (theOptions.EnableLineSearch)
    {
      bool isAccepted = false;
      for (size_t aLineIter = 0; aLineIter < Utils::THE_LINE_SEARCH_MAX; ++aLineIter)
      {
        const double anAlpha = std::ldexp(1.0, -static_cast<int>(aLineIter));
        for (size_t anIndex = 0; anIndex < 4; ++anIndex)
        {
          aNewX[anIndex] = aRes.X[anIndex] + anAlpha * aDelta[anIndex];
        }
        Utils::Clamp4D(aNewX,
                       theBounds,
                       theOptions.AllowSoftBounds,
                       theOptions.SoftBoundsExtension);
        const std::array<double, 4> aProjectedStep = {aNewX[0] - aRes.X[0],
                                                      aNewX[1] - aRes.X[1],
                                                      aNewX[2] - aRes.X[2],
                                                      aNewX[3] - aRes.X[3]};
        const long double aDerivative = Utils::MeritDirectionalDerivative(aF, aJ, aProjectedStep);
        double            aTrialF[4];
        double            aTrialJ[4][4];
        if (theFunc(aNewX[0], aNewX[1], aNewX[2], aNewX[3], aTrialF, aTrialJ)
            && Utils::IsFiniteSystemN(aTrialF, aTrialJ)
            && Utils::IsArmijoAccepted(aRes.ResidualNorm, Utils::SafeNormN(aTrialF), aDerivative))
        {
          isAccepted = true;
          break;
        }
      }
      if (!isAccepted)
      {
        aRes.Status = MathUtils::Status::NonDescentDirection;
        return aRes;
      }
    }
    else
    {
      aNewX = {aRes.X[0] + aDelta[0],
               aRes.X[1] + aDelta[1],
               aRes.X[2] + aDelta[2],
               aRes.X[3] + aDelta[3]};
      Utils::Clamp4D(aNewX, theBounds, theOptions.AllowSoftBounds, theOptions.SoftBoundsExtension);
    }

    const bool isStepWithinTolerance =
      Utils::IsStepWithinTolerance(aNewX, aRes.X, theOptions.XTolerance);
    aRes.StepNorm = Utils::SafeDistanceN(aNewX, aRes.X);
    aRes.X        = aNewX;

    if (isStepWithinTolerance)
    {
      double aCheckF[4];
      double aCheckJ[4][4];
      if (!theFunc(aRes.X[0], aRes.X[1], aRes.X[2], aRes.X[3], aCheckF, aCheckJ)
          || !Utils::IsFiniteSystemN(aCheckF, aCheckJ))
      {
        aRes.Status = MathUtils::Status::NumericalError;
        return aRes;
      }

      aRes.ResidualNorm = Utils::SafeNormN(aCheckF);
      aRes.Status       = (aRes.ResidualNorm <= theOptions.FTolerance) ? MathUtils::Status::OK
                                                                       : MathUtils::Status::MaxIterations;
      return aRes;
    }
  }

  // Final convergence check after max iterations
  double aF[4];
  double aJ[4][4];
  if (!theFunc(aRes.X[0], aRes.X[1], aRes.X[2], aRes.X[3], aF, aJ)
      || !Utils::IsFiniteSystemN(aF, aJ))
  {
    aRes.Status = MathUtils::Status::NumericalError;
    return aRes;
  }

  aRes.ResidualNorm = Utils::SafeNormN(aF);
  aRes.Status       = (aRes.ResidualNorm <= theOptions.FTolerance) ? MathUtils::Status::OK
                                                                   : MathUtils::Status::MaxIterations;
  return aRes;
}

//! Optimized 4D Newton solver for surface-surface extrema.
//!
//! Specialized version for finding extrema between two surfaces S1(u1,v1) and S2(u2,v2).
//! The function values are the gradient components of the squared distance:
//! - F1 = (S1-S2) . dS1/dU1
//! - F2 = (S1-S2) . dS1/dV1
//! - F3 = (S2-S1) . dS2/dU2
//! - F4 = (S2-S1) . dS2/dV2
//!
//! The Jacobian has a special block structure with 2x2 blocks.
//!
//! @tparam SurfaceEvaluator1 type providing D2 evaluation for first surface
//! @tparam SurfaceEvaluator2 type providing D2 evaluation for second surface
//! @param[in] theSurf1 first surface evaluator
//! @param[in] theSurf2 second surface evaluator
//! @param[in] theX0 initial guess {u1, v1, u2, v2}
//! @param[in] theBounds box bounds for {u1, v1, u2, v2}
//! @param[in] theOptions solver options
//! @return NewtonResultN<4> with u1, v1, u2, v2 stored in X[0..3]
template <typename SurfaceEvaluator1, typename SurfaceEvaluator2>
NewtonResultN<4> SolveSurfaceSurfaceExtrema4D(const SurfaceEvaluator1&     theSurf1,
                                              const SurfaceEvaluator2&     theSurf2,
                                              const std::array<double, 4>& theX0,
                                              const NewtonBoundsN<4>&      theBounds,
                                              const NewtonOptions& theOptions = NewtonOptions())
{
  auto aFunc = [&theSurf1, &theSurf2](double theU1,
                                      double theV1,
                                      double theU2,
                                      double theV2,
                                      double theF[4],
                                      double theJ[4][4]) -> bool {
    gp_Pnt aPt1, aPt2;
    gp_Vec aD1U1, aD1V1, aD2UU1, aD2VV1, aD2UV1;
    gp_Vec aD1U2, aD1V2, aD2UU2, aD2VV2, aD2UV2;

    theSurf1.D2(theU1, theV1, aPt1, aD1U1, aD1V1, aD2UU1, aD2VV1, aD2UV1);
    theSurf2.D2(theU2, theV2, aPt2, aD1U2, aD1V2, aD2UU2, aD2VV2, aD2UV2);

    // D = P1 - P2
    const gp_Vec aD(aPt2, aPt1);

    // Function values: gradient of ||S1 - S2||^2
    theF[0] = aD.Dot(aD1U1);
    theF[1] = aD.Dot(aD1V1);
    theF[2] = -aD.Dot(aD1U2);
    theF[3] = -aD.Dot(aD1V2);

    // Jacobian: Hessian of ||S1 - S2||^2
    // Block [0:2, 0:2]: derivatives w.r.t. (u1, v1)
    theJ[0][0] = aD1U1.Dot(aD1U1) + aD.Dot(aD2UU1);
    theJ[0][1] = aD1V1.Dot(aD1U1) + aD.Dot(aD2UV1);
    theJ[1][0] = theJ[0][1]; // Symmetric
    theJ[1][1] = aD1V1.Dot(aD1V1) + aD.Dot(aD2VV1);

    // Block [0:2, 2:4]: cross derivatives
    theJ[0][2] = -aD1U2.Dot(aD1U1);
    theJ[0][3] = -aD1V2.Dot(aD1U1);
    theJ[1][2] = -aD1U2.Dot(aD1V1);
    theJ[1][3] = -aD1V2.Dot(aD1V1);

    // Block [2:4, 0:2]: cross derivatives (symmetric to above)
    theJ[2][0] = theJ[0][2];
    theJ[2][1] = theJ[1][2];
    theJ[3][0] = theJ[0][3];
    theJ[3][1] = theJ[1][3];

    // Block [2:4, 2:4]: derivatives w.r.t. (u2, v2)
    theJ[2][2] = aD1U2.Dot(aD1U2) - aD.Dot(aD2UU2);
    theJ[2][3] = aD1V2.Dot(aD1U2) - aD.Dot(aD2UV2);
    theJ[3][2] = theJ[2][3]; // Symmetric
    theJ[3][3] = aD1V2.Dot(aD1V2) - aD.Dot(aD2VV2);
    return true;
  };

  return Solve4D(aFunc, theX0, theBounds, theOptions);
}

} // namespace MathSys

#endif // _MathSys_Newton4D_HeaderFile