#define PJ_LIB__

#include <errno.h>
#include <math.h>

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

PROJ_HEAD(poly, "Polyconic (American)")
    "\n\tConic, Sph&Ell";

namespace { // anonymous namespace
struct pj_opaque {
    double ml0; \
    double *en;
};
} // anonymous namespace

#define TOL 1e-10
#define CONV    1e-10
#define N_ITER  10
#define I_ITER 20
#define ITOL 1.e-12


static PJ_XY poly_e_forward (PJ_LP lp, PJ *P) {          /* Ellipsoidal, forward */
    PJ_XY xy = {0.0,0.0};
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
    double  ms, sp, cp;

    if (fabs(lp.phi) <= TOL) {
        xy.x = lp.lam;
        xy.y = -Q->ml0;
    } else {
        sp = sin(lp.phi);
        cp = cos(lp.phi);
        ms = fabs(cp) > TOL ? pj_msfn(sp, cp, P->es) / sp : 0.;
        lp.lam *= sp;
        xy.x = ms * sin(lp.lam);
        xy.y = (pj_mlfn(lp.phi, sp, cp, Q->en) - Q->ml0) + ms * (1. - cos(lp.lam));
    }

    return xy;
}


static PJ_XY poly_s_forward (PJ_LP lp, PJ *P) {           /* Spheroidal, forward */
    PJ_XY xy = {0.0,0.0};
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);

    if (fabs(lp.phi) <= TOL) {
        xy.x = lp.lam;
        xy.y = Q->ml0;
    } else {
        const double cot = 1. / tan(lp.phi);
        const double E = lp.lam * sin(lp.phi);
        xy.x = sin(E) * cot;
        xy.y = lp.phi - P->phi0 + cot * (1. - cos(E));
    }

    return xy;
}


static PJ_LP poly_e_inverse (PJ_XY xy, PJ *P) {          /* Ellipsoidal, inverse */
    PJ_LP lp = {0.0,0.0};
    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);

    xy.y += Q->ml0;
    if (fabs(xy.y) <= TOL) {
        lp.lam = xy.x;
        lp.phi = 0.;
    } else {
        int i;

        const double r = xy.y * xy.y + xy.x * xy.x;
        lp.phi = xy.y;
        for (i = I_ITER; i ; --i) {
            const double sp = sin(lp.phi);
            const double cp = cos(lp.phi);
            const double s2ph = sp * cp;
            if (fabs(cp) < ITOL) {
                proj_errno_set(P, PJD_ERR_TOLERANCE_CONDITION);
                return lp;
            }
            double mlp = sqrt(1. - P->es * sp * sp);
            const double c = sp * mlp / cp;
            const double ml = pj_mlfn(lp.phi, sp, cp, Q->en);
            const double mlb = ml * ml + r;
            mlp = P->one_es / (mlp * mlp * mlp);
            const double dPhi =
                ( ml + ml + c * mlb - 2. * xy.y * (c * ml + 1.) ) / (
                P->es * s2ph * (mlb - 2. * xy.y * ml) / c +
                2.* (xy.y - ml) * (c * mlp - 1. / s2ph) - mlp - mlp );
            lp.phi += dPhi;
            if (fabs(dPhi) <= ITOL)
                break;
        }
        if (!i) {
            proj_errno_set(P, PJD_ERR_TOLERANCE_CONDITION);
            return lp;
        }
        const double c = sin(lp.phi);
        lp.lam = asin(xy.x * tan(lp.phi) * sqrt(1. - P->es * c * c)) / sin(lp.phi);
    }

    return lp;
}


static PJ_LP poly_s_inverse (PJ_XY xy, PJ *P) {           /* Spheroidal, inverse */
    PJ_LP lp = {0.0,0.0};

    if (fabs(xy.y = P->phi0 + xy.y) <= TOL) {
        lp.lam = xy.x;
        lp.phi = 0.;
    } else {
        lp.phi = xy.y;
        const double B = xy.x * xy.x + xy.y * xy.y;
        int i = N_ITER;
        while(true) {
            const double tp = tan(lp.phi);
            const double dphi = (xy.y * (lp.phi * tp + 1.) - lp.phi -
                                    .5 * ( lp.phi * lp.phi + B) * tp) /
                                    ((lp.phi - xy.y) / tp - 1.);
            lp.phi -= dphi;
            if( !(fabs(dphi) > CONV) )
                break;
            --i;
            if( i == 0 ) {
                proj_errno_set(P, PJD_ERR_TOLERANCE_CONDITION);
                return lp;
            }
        }
        lp.lam = asin(xy.x * tan(lp.phi)) / sin(lp.phi);
    }

    return lp;
}


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

    if (nullptr==P->opaque)
        return pj_default_destructor (P, errlev);

    if (static_cast<struct pj_opaque*>(P->opaque)->en)
        pj_dealloc (static_cast<struct pj_opaque*>(P->opaque)->en);

    return pj_default_destructor(P, errlev);
}


PJ *PROJECTION(poly) {
    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;
    P->destructor = destructor;

    if (P->es != 0.0) {
        if (!(Q->en = pj_enfn(P->es)))
            return pj_default_destructor (P, ENOMEM);
        Q->ml0 = pj_mlfn(P->phi0, sin(P->phi0), cos(P->phi0), Q->en);
        P->inv = poly_e_inverse;
        P->fwd = poly_e_forward;
    } else {
        Q->ml0 = -P->phi0;
        P->inv = poly_s_inverse;
        P->fwd = poly_s_forward;
    }

    return P;
}