/******************************************************************************
 *
 * Project:  PROJ
 * Purpose:  Generic method to compute inverse projection from forward method
 * Author:   Even Rouault <even dot rouault at spatialys dot com>
 *
 ******************************************************************************
 * Copyright (c) 2018, Even Rouault <even dot rouault at spatialys dot com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 ****************************************************************************/

#include "proj_internal.h"

#include <algorithm>
#include <cmath>

/** Compute (lam, phi) corresponding to input (xy.x, xy.y) for projection P.
 *
 * 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 P->fwd method evaluated at close points.
 *
 * Note: thresholds used have been verified to work with adams_ws2 and wink2
 *
 * Starts with initial guess provided by user in lpInitial
 */
PJ_LP pj_generic_inverse_2d(PJ_XY xy, PJ *P, PJ_LP lpInitial) {
    PJ_LP lp = lpInitial;
    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++) {
        PJ_XY xyApprox = P->fwd(lp, P);
        const double deltaX = xyApprox.x - xy.x;
        const double deltaY = xyApprox.y - xy.y;
        if (fabs(deltaX) < 1e-10 && fabs(deltaY) < 1e-10) {
            return lp;
        }

        if (i == 0 || fabs(deltaX) > 1e-6 || fabs(deltaY) > 1e-6) {
            // Compute Jacobian matrix (only if we aren't close to the final
            // result to speed things a bit)
            PJ_LP lp2;
            PJ_XY xy2;
            const double dLam = lp.lam > 0 ? -1e-6 : 1e-6;
            lp2.lam = lp.lam + dLam;
            lp2.phi = lp.phi;
            xy2 = P->fwd(lp2, P);
            const double deriv_X_lam = (xy2.x - xyApprox.x) / dLam;
            const double deriv_Y_lam = (xy2.y - xyApprox.y) / dLam;

            const double dPhi = lp.phi > 0 ? -1e-6 : 1e-6;
            lp2.lam = lp.lam;
            lp2.phi = lp.phi + dPhi;
            xy2 = P->fwd(lp2, P);
            const double deriv_X_phi = (xy2.x - xyApprox.x) / dPhi;
            const double deriv_Y_phi = (xy2.y - xyApprox.y) / dPhi;

            // 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;
            }
        }

        if (xy.x != 0) {
            // Limit the amplitude of correction to avoid overshoots due to
            // bad initial guess
            const double delta_lam = std::max(
                std::min(deltaX * deriv_lam_X + deltaY * deriv_lam_Y, 0.3),
                -0.3);
            lp.lam -= delta_lam;
            if (lp.lam < -M_PI)
                lp.lam = -M_PI;
            else if (lp.lam > M_PI)
                lp.lam = M_PI;
        }

        if (xy.y != 0) {
            const double delta_phi = std::max(
                std::min(deltaX * deriv_phi_X + deltaY * deriv_phi_Y, 0.3),
                -0.3);
            lp.phi -= delta_phi;
            if (lp.phi < -M_HALFPI)
                lp.phi = -M_HALFPI;
            else if (lp.phi > M_HALFPI)
                lp.phi = M_HALFPI;
        }
    }
    proj_context_errno_set(P->ctx,
                           PROJ_ERR_COORD_TRANSFM_OUTSIDE_PROJECTION_DOMAIN);
    return lp;
}