/*
 * Copyright (c) 2020 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * 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.
 */

#include <math.h>

#include "gnss-galileo.h"

#include <jeffpc/types.h>
#include <jeffpc/error.h>

void galileo_print_eph(const struct galileo_ephemeris *eph)
{
	printf("E%02u: %svalid iod %u "
	       "m0 %e delta_n %e e %e sqrt_a %e omega0 %e i0 %e "
	       "omega %e omegadot %e idot %e cuc %e cus %e "
	       "crc %e crs %e cic %e cis %e t0 %u (%u raw)\n",
	       eph->sv, eph->valid ? "" : "NOT ", eph->iod, eph->m0,
	       eph->delta_n, eph->e, eph->sqrt_a, eph->omega0, eph->i0,
	       eph->omega, eph->omegadot, eph->idot, eph->cuc, eph->cus,
	       eph->crc, eph->crs, eph->cic, eph->cis, eph->t0.gst,
	       eph->t0.raw);
}

void galileo_eph_ecef(const struct galileo_ephemeris *eph,
		      const uint32_t gst, struct ecef *ecef)
{
	int approx_iter;

	ASSERT(eph->valid);

	/* Based on Galileo ICD 5.1.1 */
	const double mu = 3.986004418e14; /* m3/s2 */
	const double omegaE = 7.2921151467e-5; /* rad/s */

	double A = eph->sqrt_a * eph->sqrt_a;
	double n0 = sqrt(mu / (A * A * A));
	double tk = ((double) gst) - ((double) eph->t0.gst);
	double n = n0 + eph->delta_n;
	double M = eph->m0 + n * tk;
	double E = M;

	for (approx_iter = 0; approx_iter < 1000; approx_iter++) {
		double prevE = E;
		E = M + eph->e * sin(prevE);

		if (fabs(prevE - E) < 0.00000001)
			break;
	}

	/*
	 * True anomaly from eccentric anomaly - according to the ICD.
	 *
	 * There are other ways to calculate the true anomaly, but let's
	 * stick with what's in the ICD.  For more information and the other
	 * algorithms, see https://en.wikipedia.org/wiki/True_anomaly.
	 */
	double nu = atan2(sqrt(1 - eph->e * eph->e) * sin(E) / (1 - eph->e * cos(E)),
			  (cos(E) - eph->e) / (1 - eph->e * cos(E)));

	double Phi = nu + eph->omega;
	double deltau = eph->cus * sin(2 * Phi) + eph->cuc * cos(2 * Phi);
	double deltar = eph->crs * sin(2 * Phi) + eph->crc * cos(2 * Phi);
	double deltai = eph->cis * sin(2 * Phi) + eph->cic * cos(2 * Phi);
	double u = Phi + deltau;
	double r = A * (1 - eph->e * cos(E)) + deltar;
	double i = eph->i0 + deltai + eph->idot * tk;
	double xprime = r * cos(u);
	double yprime = r * sin(u);
	double Omega = eph->omega0 + (eph->omegadot - omegaE) * tk - omegaE * eph->t0.raw;

	ecef->x = xprime * cos(Omega) - yprime * cos(i) * sin(Omega);
	ecef->y = xprime * sin(Omega) + yprime * cos(i) * cos(Omega);
	ecef->z = yprime * sin(i);
}