/******************************************************************************
 * Project:  SCH Coordinate system
 * Purpose:  Implementation of SCH Coordinate system
 * References :
 *      1. Hensley. Scott. SCH Coordinates and various transformations. June 15, 2000.
 *      2. Buckley, Sean Monroe. Radar interferometry measurement of land subsidence. 2000..
 *         PhD Thesis. UT Austin. (Appendix)
 *      3. Hensley, Scott, Elaine Chapin, and T. Michel. "Improved processing of AIRSAR
 *         data based on the GeoSAR processor." Airsar earth science and applications
 *         workshop, March. 2002. (http://airsar.jpl.nasa.gov/documents/workshop2002/papers/T3.pdf)
 *
 * Author:   Piyush Agram (piyush.agram@jpl.nasa.gov)
 * Copyright (c) 2015 California Institute of Technology.
 * Government sponsorship acknowledged.
 *
 * NOTE:  The SCH coordinate system is a sensor aligned coordinate system
 * developed at JPL for radar mapping missions. Details pertaining to the
 * coordinate system have been release in the public domain (see references above).
 * This code is an independent implementation of the SCH coordinate system
 * that conforms to the PROJ.4 conventions and uses the details presented in these
 * publicly released documents. All credit for the development of the coordinate
 * system and its use should be directed towards the original developers at JPL.
 ******************************************************************************
 * 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 <math.h>

#include "proj.h"
#include "proj_internal.h"
#include "geocent.h"

namespace { // anonymous namespace
struct pj_opaque {
    double plat; /*Peg Latitude */
    double plon; /*Peg Longitude*/
    double phdg; /*Peg heading  */
    double h0;   /*Average altitude */
    double transMat[9];
    double xyzoff[3];
    double rcurv;
    GeocentricInfo sph;
    GeocentricInfo elp_0;
};
} // anonymous namespace

PROJ_HEAD(sch, "Spherical Cross-track Height") "\n\tMisc\n\tplat_0= plon_0= phdg_0= [h_0=]";

static PJ_LPZ sch_inverse3d(PJ_XYZ xyz, PJ *P) {
    PJ_LPZ lpz = {0.0, 0.0, 0.0};
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
    double temp[3];
    /* Local lat,lon using radius */
    double pxyz[] = {
        xyz.y * P->a / Q->rcurv,
        xyz.x * P->a / Q->rcurv,
        xyz.z
    };

    if( pj_Convert_Geodetic_To_Geocentric( &(Q->sph), pxyz[0], pxyz[1], pxyz[2], temp, temp+1, temp+2) != 0) {
            proj_errno_set(P, PJD_ERR_TOLERANCE_CONDITION);
            return lpz;
    }

    /* Apply rotation */
    pxyz[0] = Q->transMat[0] * temp[0] + Q->transMat[1] * temp[1] + Q->transMat[2] * temp[2];
    pxyz[1] = Q->transMat[3] * temp[0] + Q->transMat[4] * temp[1] + Q->transMat[5] * temp[2];
    pxyz[2] = Q->transMat[6] * temp[0] + Q->transMat[7] * temp[1] + Q->transMat[8] * temp[2];

    /* Apply offset */
    pxyz[0] += Q->xyzoff[0];
    pxyz[1] += Q->xyzoff[1];
    pxyz[2] += Q->xyzoff[2];

    /* Convert geocentric coordinates to lat lon */
    pj_Convert_Geocentric_To_Geodetic( &(Q->elp_0), pxyz[0], pxyz[1], pxyz[2],
            temp, temp+1, temp+2);


    lpz.lam = temp[1] ;
    lpz.phi = temp[0] ;
    lpz.z = temp[2];

    return lpz;
}

static PJ_XYZ sch_forward3d(PJ_LPZ lpz, PJ *P) {
    PJ_XYZ xyz = {0.0, 0.0, 0.0};
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
    double temp[3];

    /* Convert lat lon to geocentric coordinates */
    if( pj_Convert_Geodetic_To_Geocentric( &(Q->elp_0), lpz.phi, lpz.lam, lpz.z, temp, temp+1, temp+2 ) != 0 ) {
        proj_errno_set(P, PJD_ERR_TOLERANCE_CONDITION);
        return xyz;
    }


    /* Adjust for offset */
    temp[0] -= Q->xyzoff[0];
    temp[1] -= Q->xyzoff[1];
    temp[2] -= Q->xyzoff[2];


    /* Apply rotation */
    double pxyz[] = {
        Q->transMat[0] * temp[0] + Q->transMat[3] * temp[1] + Q->transMat[6] * temp[2],
        Q->transMat[1] * temp[0] + Q->transMat[4] * temp[1] + Q->transMat[7] * temp[2],
        Q->transMat[2] * temp[0] + Q->transMat[5] * temp[1] + Q->transMat[8] * temp[2]
    };

    /* Convert to local lat,lon */
    pj_Convert_Geocentric_To_Geodetic( &(Q->sph), pxyz[0], pxyz[1], pxyz[2],
            temp, temp+1, temp+2);


    /* Scale by radius */
    xyz.x = temp[1] * Q->rcurv / P->a;
    xyz.y = temp[0] * Q->rcurv / P->a;
    xyz.z = temp[2];

    return xyz;
}


static PJ *setup(PJ *P) { /* general initialization */
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
    double reast, rnorth;
    double chdg, shdg;
    double clt, slt;
    double clo, slo;
    double temp;
    double pxyz[3];

    temp = P->a * sqrt(1.0 - P->es);

    /* Setup original geocentric system */
    if ( pj_Set_Geocentric_Parameters(&(Q->elp_0), P->a, temp) != 0)
        return pj_default_destructor(P, PJD_ERR_FAILED_TO_FIND_PROJ);

    clt = cos(Q->plat);
    slt = sin(Q->plat);
    clo = cos(Q->plon);
    slo = sin(Q->plon);

    /* Estimate the radius of curvature for given peg */
    temp = sqrt(1.0 - (P->es) * slt * slt);
    reast = (P->a)/temp;
    rnorth = (P->a) * (1.0 - (P->es))/pow(temp,3);

    chdg = cos(Q->phdg);
    shdg = sin(Q->phdg);

    Q->rcurv = Q->h0 + (reast*rnorth)/(reast * chdg * chdg + rnorth * shdg * shdg);

    /* Set up local sphere at the given peg point */
    if ( pj_Set_Geocentric_Parameters(&(Q->sph), Q->rcurv, Q->rcurv) != 0)
            return pj_default_destructor(P, PJD_ERR_FAILED_TO_FIND_PROJ);

    /* Set up the transformation matrices */
    Q->transMat[0] = clt * clo;
    Q->transMat[1] = -shdg*slo - slt*clo * chdg;
    Q->transMat[2] =  slo*chdg - slt*clo*shdg;
    Q->transMat[3] =  clt*slo;
    Q->transMat[4] =  clo*shdg - slt*slo*chdg;
    Q->transMat[5] = -clo*chdg - slt*slo*shdg;
    Q->transMat[6] =  slt;
    Q->transMat[7] =  clt*chdg;
    Q->transMat[8] =  clt*shdg;


    if( pj_Convert_Geodetic_To_Geocentric( &(Q->elp_0), Q->plat, Q->plon, Q->h0,
                                           pxyz, pxyz+1, pxyz+2 ) != 0 )
        return pj_default_destructor(P, PJD_ERR_LAT_OR_LON_EXCEED_LIMIT);


    Q->xyzoff[0] = pxyz[0] - (Q->rcurv) * clt * clo;
    Q->xyzoff[1] = pxyz[1] - (Q->rcurv) * clt * slo;
    Q->xyzoff[2] = pxyz[2] - (Q->rcurv) * slt;

    P->fwd3d = sch_forward3d;
    P->inv3d = sch_inverse3d;
    return P;
}


PJ *PROJECTION(sch) {
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(pj_calloc (1, sizeof (struct pj_opaque)));
    if (nullptr==Q)
        return pj_default_destructor(P, ENOMEM);
    P->opaque = Q;

    Q->h0 = 0.0;

    /* Check if peg latitude was defined */
    if (pj_param(P->ctx, P->params, "tplat_0").i)
        Q->plat = pj_param(P->ctx, P->params, "rplat_0").f;
    else {
        return pj_default_destructor(P, PJD_ERR_FAILED_TO_FIND_PROJ);
    }

    /* Check if peg longitude was defined */
    if (pj_param(P->ctx, P->params, "tplon_0").i)
        Q->plon = pj_param(P->ctx, P->params, "rplon_0").f;
    else {
        return pj_default_destructor(P, PJD_ERR_FAILED_TO_FIND_PROJ);
    }

    /* Check if peg heading is defined */
    if (pj_param(P->ctx, P->params, "tphdg_0").i)
        Q->phdg = pj_param(P->ctx, P->params, "rphdg_0").f;
    else {
        return pj_default_destructor(P, PJD_ERR_FAILED_TO_FIND_PROJ);
    }


    /* Check if average height was defined - If so read it in */
    if (pj_param(P->ctx, P->params, "th_0").i)
        Q->h0 = pj_param(P->ctx, P->params, "dh_0").f;


    return setup(P);
}
