/******************************************************************************
 * Project:  PROJ
 * Purpose:  Geocentric translation using a grid
 * Author:   Even Rouault, <even.rouault at spatialys.com>
 *
 ******************************************************************************
 * Copyright (c) 2019, Even Rouault, <even.rouault at spatialys.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.
 *****************************************************************************/

#define PJ_LIB__

#include <errno.h>
#include <string.h>
#include <stddef.h>
#include <time.h>

#include "proj_internal.h"
#include "grids.hpp"

#include <algorithm>

PROJ_HEAD(xyzgridshift, "Geocentric grid shift");

using namespace NS_PROJ;

namespace { // anonymous namespace
struct xyzgridshiftData {
    PJ *cart = nullptr;
    bool grid_ref_is_input = true;
    ListOfGenericGrids grids{};
    bool defer_grid_opening = false;
    double multiplier = 1.0;
};
} // anonymous namespace

// ---------------------------------------------------------------------------

static bool get_grid_values(PJ* P,
                            xyzgridshiftData* Q,
                            const PJ_LP& lp,
                            double& dx,
                            double& dy,
                            double& dz)
{
    if ( Q->defer_grid_opening ) {
        Q->defer_grid_opening = false;
        Q->grids = pj_generic_grid_init(P, "grids");
        if ( proj_errno(P) ) {
            return false;
        }
    }

    GenericShiftGridSet* gridset = nullptr;
    auto grid = pj_find_generic_grid(Q->grids, lp, gridset);
    if( !grid ) {
        return false;
    }
    if( grid->isNullGrid() ) {
        dx = 0;
        dy = 0;
        dz = 0;
        return true;
    }
    const auto samplesPerPixel = grid->samplesPerPixel();
    if( samplesPerPixel < 3 ) {
        proj_log_error(P, "xyzgridshift: grid has not enough samples");
        return false;
    }
    int sampleX = 0;
    int sampleY = 1;
    int sampleZ = 2;
    for( int i = 0; i < samplesPerPixel; i++ )
    {
        const auto desc = grid->description(i);
        if( desc == "x_translation") {
            sampleX = i;
        } else if( desc == "y_translation") {
            sampleY = i;
        } else if( desc == "z_translation") {
            sampleZ = i;
        }
    }
    const auto unit = grid->unit(sampleX);
    if( !unit.empty() && unit != "metre" ) {
        proj_log_error(P, "xyzgridshift: Only unit=metre currently handled");
        return false;
    }

    bool must_retry = false;
    if( !pj_bilinear_interpolation_three_samples(P->ctx, grid, lp,
                                                 sampleX, sampleY, sampleZ,
                                                 dx, dy, dz,
                                                 must_retry) )
    {
        if( must_retry )
            return get_grid_values( P, Q, lp, dx, dy, dz);
        return false;
    }

    dx *= Q->multiplier;
    dy *= Q->multiplier;
    dz *= Q->multiplier;
    return true;
}

// ---------------------------------------------------------------------------

#define SQUARE(x) ((x)*(x))

// ---------------------------------------------------------------------------

static PJ_COORD iterative_adjustment(PJ* P,
                                     xyzgridshiftData* Q,
                                     const PJ_COORD& pointInit,
                                     double factor)
{
    PJ_COORD point = pointInit;
    for(int i = 0; i < 10; i++) {
        PJ_COORD geodetic;
        geodetic.lpz = pj_inv3d(point.xyz, Q->cart);

        double dx, dy, dz;
        if( !get_grid_values(P, Q, geodetic.lp, dx, dy, dz) ) {
            return proj_coord_error();
        }

        dx *= factor;
        dy *= factor;
        dz *= factor;

        const double err = SQUARE((point.xyz.x - pointInit.xyz.x) - dx) +
                            SQUARE((point.xyz.y - pointInit.xyz.y) - dy) +
                            SQUARE((point.xyz.z - pointInit.xyz.z) - dz);

        point.xyz.x = pointInit.xyz.x + dx;
        point.xyz.y = pointInit.xyz.y + dy;
        point.xyz.z = pointInit.xyz.z + dz;
        if( err < 1e-10 ) {
            break;
        }
    }
    return point;
}

// ---------------------------------------------------------------------------

static PJ_COORD direct_adjustment(PJ* P,
                                  xyzgridshiftData* Q,
                                  PJ_COORD point,
                                  double factor)
{
    PJ_COORD geodetic;
    geodetic.lpz = pj_inv3d(point.xyz, Q->cart);

    double dx, dy, dz;
    if( !get_grid_values(P, Q, geodetic.lp, dx, dy, dz) ) {
        return proj_coord_error();
    }
    point.xyz.x += factor * dx;
    point.xyz.y += factor * dy;
    point.xyz.z += factor * dz;
    return point;
}

// ---------------------------------------------------------------------------

static PJ_XYZ forward_3d(PJ_LPZ lpz, PJ *P) {
    auto Q = static_cast<xyzgridshiftData*>(P->opaque);
    PJ_COORD point = {{0,0,0,0}};
    point.lpz = lpz;

    if( Q->grid_ref_is_input ) {
        point = direct_adjustment(P, Q, point, 1.0);
    }
    else {
        point = iterative_adjustment(P, Q, point, 1.0);
    }

    return point.xyz;
}


static PJ_LPZ reverse_3d(PJ_XYZ xyz, PJ *P) {
    auto Q = static_cast<xyzgridshiftData*>(P->opaque);
    PJ_COORD point = {{0,0,0,0}};
    point.xyz = xyz;

    if( Q->grid_ref_is_input ) {
        point = iterative_adjustment(P, Q, point, -1.0);
    }
    else {
        point = direct_adjustment(P, Q, point, -1.0);
    }

    return point.lpz;
}

static PJ *destructor (PJ *P, int errlev) {
    if (nullptr==P)
        return nullptr;

    auto Q = static_cast<struct xyzgridshiftData*>(P->opaque);
    if( Q )
    {
        if (Q->cart)
            Q->cart->destructor (Q->cart, errlev);
        delete Q;
    }
    P->opaque = nullptr;

    return pj_default_destructor(P, errlev);
}

static void reassign_context( PJ* P, PJ_CONTEXT* ctx )
{
    auto Q = (struct xyzgridshiftData *) P->opaque;
    for( auto& grid: Q->grids ) {
        grid->reassign_context(ctx);
    }
}


PJ *TRANSFORMATION(xyzgridshift,0) {
    auto Q = new xyzgridshiftData;
    P->opaque = (void *) Q;
    P->destructor = destructor;
    P->reassign_context = reassign_context;

    P->fwd4d  = nullptr;
    P->inv4d  = nullptr;
    P->fwd3d  = forward_3d;
    P->inv3d  = reverse_3d;
    P->fwd    = nullptr;
    P->inv    = nullptr;

    P->left  = PJ_IO_UNITS_CARTESIAN;
    P->right = PJ_IO_UNITS_CARTESIAN;

    // Pass a dummy ellipsoid definition that will be overridden just afterwards
    Q->cart = proj_create(P->ctx, "+proj=cart +a=1");
    if (Q->cart == nullptr)
        return destructor(P, ENOMEM);

    /* inherit ellipsoid definition from P to Q->cart */
    pj_inherit_ellipsoid_def (P, Q->cart);

    const char* grid_ref = pj_param (P->ctx, P->params, "sgrid_ref").s;
    if( grid_ref ) {
        if (strcmp(grid_ref, "input_crs") == 0 ) {
            // default
        } else if (strcmp(grid_ref, "output_crs") == 0 ) {
            // Convention use for example for NTF->RGF93 grid that contains
            // delta x,y,z from NTF to RGF93, but the grid itself is referenced
            // in RGF93
            Q->grid_ref_is_input = false;
        } else {
            proj_log_error(P, "xyzgridshift: unusupported value for grid_ref");
            return destructor (P, PJD_ERR_NO_ARGS);
        }
    }

    if (0==pj_param(P->ctx, P->params, "tgrids").i) {
        proj_log_error(P, "xyzgridshift: +grids parameter missing.");
        return destructor (P, PJD_ERR_NO_ARGS);
    }

    /* multiplier for delta x,y,z */
    if (pj_param(P->ctx, P->params, "tmultiplier").i) {
        Q->multiplier = pj_param(P->ctx, P->params, "dmultiplier").f;
    }

    if( P->ctx->defer_grid_opening ) {
        Q->defer_grid_opening = true;
    }
    else {
        Q->grids = pj_generic_grid_init(P, "grids");
        /* Was gridlist compiled properly? */
        if ( proj_errno(P) ) {
            proj_log_error(P, "xyzgridshift: could not find required grid(s).");
            return destructor(P, PJD_ERR_FAILED_TO_LOAD_GRID);
        }
    }

    return P;
}