/******************************************************************************
 * Project:  PROJ
 * Purpose:  Convert between geocentric coordinates and topocentric (ENU) coordinates
 *
 ******************************************************************************
 * Copyright (c) 2020, 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 "proj_internal.h"
#include <errno.h>
#include <math.h>

PROJ_HEAD(topocentric, "Geocentric/Topocentric conversion");

// Notations and formulas taken from IOGP Publication 373-7-2 -
// Geomatics Guidance Note number 7, part 2 - October 2020

namespace { // anonymous namespace
struct pj_opaque {
    double X0;
    double Y0;
    double Z0;
    double sinphi0;
    double cosphi0;
    double sinlam0;
    double coslam0;
};
} // anonymous namespace

// Convert from geocentric to topocentric
static PJ_COORD topocentric_fwd(PJ_COORD in, PJ * P)
{
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
    PJ_COORD out;
    const double dX = in.xyz.x - Q->X0;
    const double dY = in.xyz.y - Q->Y0;
    const double dZ = in.xyz.z - Q->Z0;
    out.xyz.x = -dX * Q->sinlam0              + dY * Q->coslam0;
    out.xyz.y = -dX * Q->sinphi0 * Q->coslam0 - dY * Q->sinphi0 * Q->sinlam0 + dZ * Q->cosphi0;
    out.xyz.z =  dX * Q->cosphi0 * Q->coslam0 + dY * Q->cosphi0 * Q->sinlam0 + dZ * Q->sinphi0;
    return out;
}

// Convert from topocentric to geocentric
static PJ_COORD topocentric_inv(PJ_COORD in, PJ * P)
{
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
    PJ_COORD out;
    out.xyz.x = Q->X0 - in.xyz.x * Q->sinlam0 - in.xyz.y * Q->sinphi0 * Q->coslam0 + in.xyz.z * Q->cosphi0 * Q->coslam0;
    out.xyz.y = Q->Y0 + in.xyz.x * Q->coslam0 - in.xyz.y * Q->sinphi0 * Q->sinlam0 + in.xyz.z * Q->cosphi0 * Q->sinlam0;
    out.xyz.z = Q->Z0                         + in.xyz.y * Q->cosphi0              + in.xyz.z * Q->sinphi0;
    return out;
}


/*********************************************************************/
PJ *CONVERSION(topocentric,1) {
/*********************************************************************/
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(calloc (1, sizeof (struct pj_opaque)));
    if (nullptr==Q)
        return pj_default_destructor (P, PROJ_ERR_OTHER /*ENOMEM*/);
    P->opaque = static_cast<void *>(Q);

    // The topocentric origin can be specified either in geocentric coordinates
    // (X_0,Y_0,Z_0) or as geographic coordinates (lon_0,lat_0,h_0)
    // Checks:
    // - X_0 or lon_0 must be specified
    // - If X_0 is specified, the Y_0 and Z_0 must also be
    // - If lon_0 is specified, then lat_0 must also be
    // - If any of X_0, Y_0, Z_0 is specified, then any of lon_0,lat_0,h_0 must
    //   not be, and vice versa.
    const auto hasX0 = pj_param_exists(P->params, "X_0");
    const auto hasY0 = pj_param_exists(P->params, "Y_0");
    const auto hasZ0 = pj_param_exists(P->params, "Z_0");
    const auto hasLon0 = pj_param_exists(P->params, "lon_0");
    const auto hasLat0 = pj_param_exists(P->params, "lat_0");
    const auto hash0 = pj_param_exists(P->params, "h_0");
    if( !hasX0 && !hasLon0 )
    {
        proj_log_error(P, _("missing X_0 or lon_0"));
        return pj_default_destructor(P, PROJ_ERR_INVALID_OP_MISSING_ARG);
    }
    if ( (hasX0 || hasY0 || hasZ0) &&
         (hasLon0 || hasLat0 || hash0) )
    {
        proj_log_error(P, _("(X_0,Y_0,Z_0) and (lon_0,lat_0,h_0) are mutually exclusive"));
        return pj_default_destructor(P, PROJ_ERR_INVALID_OP_MUTUALLY_EXCLUSIVE_ARGS);
    }
    if( hasX0 && (!hasY0 || !hasZ0) )
    {
        proj_log_error(P, _("missing Y_0 and/or Z_0"));
        return pj_default_destructor(P, PROJ_ERR_INVALID_OP_MISSING_ARG);
    }
    if( hasLon0 && !hasLat0 ) // allow missing h_0
    {
        proj_log_error(P, _("missing lat_0"));
        return pj_default_destructor(P, PROJ_ERR_INVALID_OP_MISSING_ARG);
    }

    // Pass a dummy ellipsoid definition that will be overridden just afterwards
    PJ* cart = proj_create(P->ctx, "+proj=cart +a=1");
    if (cart == nullptr)
        return pj_default_destructor(P, PROJ_ERR_OTHER /*ENOMEM*/);
    /* inherit ellipsoid definition from P to cart */
    pj_inherit_ellipsoid_def (P, cart);

    if( hasX0 )
    {
        Q->X0 = pj_param(P->ctx, P->params, "dX_0").f;
        Q->Y0 = pj_param(P->ctx, P->params, "dY_0").f;
        Q->Z0 = pj_param(P->ctx, P->params, "dZ_0").f;

        // Compute lam0, phi0 from X0,Y0,Z0
        PJ_XYZ xyz;
        xyz.x = Q->X0;
        xyz.y = Q->Y0;
        xyz.z = Q->Z0;
        const auto lpz = pj_inv3d(xyz, cart);
        Q->sinphi0 = sin(lpz.phi);
        Q->cosphi0 = cos(lpz.phi);
        Q->sinlam0 = sin(lpz.lam);
        Q->coslam0 = cos(lpz.lam);
    }
    else
    {
        // Compute X0,Y0,Z0 from lam0, phi0, h0
        PJ_LPZ lpz;
        lpz.lam = P->lam0;
        lpz.phi = P->phi0;
        lpz.z = pj_param(P->ctx, P->params, "dh_0").f;
        const auto xyz = pj_fwd3d(lpz, cart);
        Q->X0 = xyz.x;
        Q->Y0 = xyz.y;
        Q->Z0 = xyz.z;

        Q->sinphi0 = sin(P->phi0);
        Q->cosphi0 = cos(P->phi0);
        Q->sinlam0 = sin(P->lam0);
        Q->coslam0 = cos(P->lam0);
    }

    proj_destroy(cart);

    P->fwd4d  =  topocentric_fwd;
    P->inv4d  =  topocentric_inv;
    P->left   =  PJ_IO_UNITS_CARTESIAN;
    P->right  =  PJ_IO_UNITS_CARTESIAN;
    return P;
}