#include <math.h>
#include <grass/gis.h>
#include "pi.h"

/* distance from point to point along a geodesic
 * code from
 *   Paul D. Thomas
 *   "Spheroidal Geodesics, Reference Systems, and Local Geometry"
 *   U.S. Naval Oceanographic Office, p. 162
 *   Engineering Library 526.3 T36s
 */

/* a corruption of geodist.c library module of GRASS, used to enhance
   execution efficiency of interpolation routine */

static double boa;
static double f;
static double ff64;
static double al;

static double t1r, t2r;

#define DIST_PARAMS struct dist_params
DIST_PARAMS
{
    short targetrow; /* interpolation row for which params apply */
    double t1, t2, t3, t4;
};

static DIST_PARAMS *lat_params, *nextcalc;

/* must be called once to establish the ellipsoid */
int G_begin_geodesic_distance_l(short nrows, double a, double e2)
{
    int i;

    al = a;
    boa = sqrt(1 - e2);
    f = 1 - boa;
    ff64 = f * f / 64;

    /* initialize lat_params array and indicate no prior data storage */
    lat_params = (DIST_PARAMS *)G_calloc(nrows, sizeof(DIST_PARAMS));
    for (i = 0, nextcalc = lat_params; i < nrows; i++, nextcalc++)
        nextcalc->targetrow = -1;

    return 0;
}

double LL_set_geodesic_distance_lat(double lat)
{
    return (atan(boa * tan(Radians(lat))));
}

double set_sdlmr(double lon_diff)
{
    return (sin(Radians(lon_diff) / 2));
}

/* must be called first */
int LL_set_geodesic_distance(
    double *rowlook,  /* preprocessed latitude data by row */
    int unk, int data /* row (y) of interpolation target, data value */
)
{
    double stm, ctm, sdtm, cdtm;
    double tm, dtm;
    double temp;

    t1r = *(rowlook + unk);
    t2r = *(rowlook + data);

    tm = (t1r + t2r) / 2;
    dtm = (t2r - t1r) / 2;

    stm = sin(tm);
    ctm = cos(tm);
    sdtm = sin(dtm);
    cdtm = cos(dtm);

    nextcalc = lat_params + data;
    if (nextcalc->targetrow != unk) { /* reset latitude offset parameters */
        temp = stm * cdtm;
        nextcalc->t1 = temp * temp * 2;

        temp = sdtm * ctm;
        nextcalc->t2 = temp * temp * 2;

        nextcalc->t3 = sdtm * sdtm;
        nextcalc->t4 = cdtm * cdtm - stm * stm;

        nextcalc->targetrow = unk; /* parameterization tagged to row */
    }

    return 0;
}

double LL_geodesic_distance(double sdlmr)
{
    double a, cd, d, e, q, sd, t, u, v, x, y;

    /* special case - shapiro */
    if (sdlmr == 0.0 && t1r == t2r)
        return 0.0;

    q = nextcalc->t3 + sdlmr * sdlmr * nextcalc->t4;
    /* special case - shapiro */
    if (q == 1.0)
        return PI * al;

    cd = 1 - 2 * q; /* ill-conditioned subtraction for small q */
    /* mod starts here */
    sd = 2 * sqrt(q - q * q);  /* sd^2 = 1 - cd^2 */
    if (q != 0.0 && cd == 1.0) /* test for small q */
        t = 1.0;
    else if (sd == 0.0)
        t = 1.0;
    else
        t = acos(cd) / sd; /* don't know how to fix acos(1-2*q) yet */
    /* mod ends here */

    u = nextcalc->t1 / (1 - q);
    v = nextcalc->t2 / q;
    d = 4 * t * t;
    x = u + v;
    e = -2 * cd;
    y = u - v;
    a = -d * e;

    return (al * sd *
            (t - f / 4 * (t * x - y) +
             ff64 * (x * (a + (t - (a + e) / 2) * x) + y * (-2 * d + e * y) +
                     d * x * y)));
}

int free_dist_params(void)
{
    G_free(lat_params);

    return 0;
}
