#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_poly_data {
    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_poly_data *Q = static_cast<struct pj_poly_data *>(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_poly_data *Q = static_cast<struct pj_poly_data *>(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_poly_data *Q = static_cast<struct pj_poly_data *>(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, PROJ_ERR_COORD_TRANSFM_OUTSIDE_PROJECTION_DOMAIN);
                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, PROJ_ERR_COORD_TRANSFM_OUTSIDE_PROJECTION_DOMAIN);
            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, PROJ_ERR_COORD_TRANSFM_OUTSIDE_PROJECTION_DOMAIN);
                return lp;
            }
        }
        lp.lam = asin(xy.x * tan(lp.phi)) / sin(lp.phi);
    }

    return lp;
}

static PJ *pj_poly_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_poly_data *>(P->opaque)->en)
        free(static_cast<struct pj_poly_data *>(P->opaque)->en);

    return pj_default_destructor(P, errlev);
}

PJ *PJ_PROJECTION(poly) {
    struct pj_poly_data *Q = static_cast<struct pj_poly_data *>(
        calloc(1, sizeof(struct pj_poly_data)));
    if (nullptr == Q)
        return pj_default_destructor(P, PROJ_ERR_OTHER /*ENOMEM*/);

    P->opaque = Q;
    P->destructor = pj_poly_destructor;

    if (P->es != 0.0) {
        if (!(Q->en = pj_enfn(P->n)))
            return pj_default_destructor(P, PROJ_ERR_OTHER /*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;
}

#undef TOL
#undef CONV
#undef N_ITER
#undef I_ITER
#undef ITOL