#include <u.h>
#include <libc.h>

#include "project.h"

/* meridinal distance for ellipsoid and inverse
**	8th degree - accurate to < 1e-5 meters when used in conjuction
**		with typical major axis values.
**	Inverse determines phi to EPS (1e-11) radians, about 1e-6 seconds.
*/

#define C00 1.
#define C02 .25
#define C04 .046875
#define C06 .01953125
#define C08 .01068115234375
#define C22 .75
#define C44 .46875
#define C46 .01302083333333333333
#define C48 .00712076822916666666
#define C66 .36458333333333333333
#define C68 .00569661458333333333
#define C88 .3076171875
#define EPS 1e-11
#define MAX_ITER 10
#define EN_SIZE 5

double *
ell_en_init(struct elliptic *ell) {
	double e⁲, *en;

	e⁲ = ell->e⁲;
	if (en = (double *)malloc(EN_SIZE * sizeof(double))) {
		en[0] = C00 - e⁲ * (C02 + e⁲ * (C04 + e⁲ * (C06 + e⁲ * C08)));
		en[1] = (e⁲) * (C22 - e⁲ * (C04 + e⁲ * (C06 + e⁲ * C08)));
		en[2] = (e⁲ * e⁲) * (C44 - e⁲ * (C46 + e⁲ * C48));
		en[3] = (e⁲ * e⁲ * e⁲) * (C66 - e⁲ * C68);
		en[4] = (e⁲ * e⁲ * e⁲ * e⁲) * C88;
	}
	ell->en = en;
	return en;
}

double
lenMarc(double φ, double Sφ, double Cφ, struct elliptic *ell) {
	double *en = ell->en;
	Cφ *= Sφ;
	Sφ *= Sφ;
	return(en[0] * φ - Cφ * (en[1] + Sφ*(en[2] + Sφ*(en[3] + Sφ*en[4]))));
}

/* is there a non-iterative way? */
double
arcMlen(double y, struct elliptic *ell) {
	double s, t, phi, k = 1./(1.-ell->e⁲);
	int i;

	phi = y;
	for (i = 10; i ; --i) { /* rarely goes over 2 iterations */
		s = sin(phi);
		t = 1. - ell->e⁲ * s * s;
		t = (lenMarc(phi, s, cos(phi), ell) - y) * (t * sqrt(t)) * k;
		phi -= t;
		if (fabs(t) < EPS)
			return phi;
	}
	return phi;
}