/******************************************************************************
 *
 * Project:  GDAL
 * Purpose:  Generic method to compute inverse coordinate transformation from
 *           forward method
 * Author:   Even Rouault <even dot rouault at spatialys dot com>
 *
 ******************************************************************************
 * Copyright (c) 2023, Even Rouault <even dot rouault at spatialys dot com>
 *
 * SPDX-License-Identifier: MIT
 ****************************************************************************/

#include <algorithm>
#include <cmath>

#include "gdalgenericinverse.h"
#include <cstdio>

/** Compute (xOut, yOut) corresponding to input (xIn, yIn) using
 * the provided forward transformation to emulate the reverse direction.
 *
 * Uses Newton-Raphson method, extended to 2D variables, that is using
 * inversion of the Jacobian 2D matrix of partial derivatives. The derivatives
 * are estimated numerically from the forward method evaluated at close points.
 *
 * Starts with initial guess provided by user in (guessedXOut, guessedYOut)
 *
 * It iterates at most for 15 iterations or as soon as we get below the specified
 * tolerance (on input coordinates)
 */
bool GDALGenericInverse2D(double xIn, double yIn, double guessedXOut,
                          double guessedYOut,
                          GDALForwardCoordTransformer pfnForwardTranformer,
                          void *pfnForwardTranformerUserData, double &xOut,
                          double &yOut,
                          bool computeJacobianMatrixOnlyAtFirstIter,
                          double toleranceOnInputCoordinates,
                          double toleranceOnOutputCoordinates)
{
    const double dfAbsValOut = std::max(fabs(guessedXOut), fabs(guessedYOut));
    const double dfEps = dfAbsValOut > 0 ? dfAbsValOut * 1e-6 : 1e-6;
    if (toleranceOnInputCoordinates == 0)
    {
        const double dfAbsValIn = std::max(fabs(xIn), fabs(yIn));
        toleranceOnInputCoordinates =
            dfAbsValIn > 0 ? dfAbsValIn * 1e-12 : 1e-12;
    }
    xOut = guessedXOut;
    yOut = guessedYOut;
    double deriv_lam_X = 0;
    double deriv_lam_Y = 0;
    double deriv_phi_X = 0;
    double deriv_phi_Y = 0;
    for (int i = 0; i < 15; i++)
    {
        double xApprox;
        double yApprox;
        if (!pfnForwardTranformer(xOut, yOut, xApprox, yApprox,
                                  pfnForwardTranformerUserData))
            return false;
        const double deltaX = xApprox - xIn;
        const double deltaY = yApprox - yIn;
        if (fabs(deltaX) < toleranceOnInputCoordinates &&
            fabs(deltaY) < toleranceOnInputCoordinates)
        {
            return true;
        }

        if (i == 0 || !computeJacobianMatrixOnlyAtFirstIter)
        {
            // Compute Jacobian matrix
            double xTmp = xOut + dfEps;
            double yTmp = yOut;
            double xTmpOut;
            double yTmpOut;
            if (!pfnForwardTranformer(xTmp, yTmp, xTmpOut, yTmpOut,
                                      pfnForwardTranformerUserData))
                return false;
            const double deriv_X_lam = (xTmpOut - xApprox) / dfEps;
            const double deriv_Y_lam = (yTmpOut - yApprox) / dfEps;

            xTmp = xOut;
            yTmp = yOut + dfEps;
            if (!pfnForwardTranformer(xTmp, yTmp, xTmpOut, yTmpOut,
                                      pfnForwardTranformerUserData))
                return false;
            const double deriv_X_phi = (xTmpOut - xApprox) / dfEps;
            const double deriv_Y_phi = (yTmpOut - yApprox) / dfEps;

            // Inverse of Jacobian matrix
            const double det =
                deriv_X_lam * deriv_Y_phi - deriv_X_phi * deriv_Y_lam;
            if (det != 0)
            {
                deriv_lam_X = deriv_Y_phi / det;
                deriv_lam_Y = -deriv_X_phi / det;
                deriv_phi_X = -deriv_Y_lam / det;
                deriv_phi_Y = deriv_X_lam / det;
            }
            else
            {
                return false;
            }
        }

        const double xOutDelta = deltaX * deriv_lam_X + deltaY * deriv_lam_Y;
        const double yOutDelta = deltaX * deriv_phi_X + deltaY * deriv_phi_Y;
        xOut -= xOutDelta;
        yOut -= yOutDelta;
        if (toleranceOnOutputCoordinates > 0 &&
            fabs(xOutDelta) < toleranceOnOutputCoordinates &&
            fabs(yOutDelta) < toleranceOnOutputCoordinates)
        {
            return true;
        }
    }
    return false;
}