using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace skyscraper8.Skyscraper.Math
{
	public static class Geodesy
	{
		const double a = 6378137.0;                 // WGS84 semi-major axis
		const double f = 1.0 / 298.257223563;       // WGS84 flattening
		const double e2 = 2 * f - f * f;            // WGS84eccentricity squared

		public static SpatialReferenceWithHeight EcefToLla(double X, double Y, double Z)
		{
			// Longitude is straightforward
			double lon = System.Math.Atan2(Y, X);

			// Distance from Z-axis
			double p = System.Math.Sqrt(X * X + Y * Y);

			// Initial latitude estimate
			double lat = System.Math.Atan2(Z, p * (1 - e2));

			// Iterate to improve latitude
			for (int i = 0; i < 10; i++)
			{
				double sinLat = System.Math.Sin(lat);
				double N = a / System.Math.Sqrt(1 - e2 * sinLat * sinLat);
				lat = System.Math.Atan2(Z + e2 * N * sinLat, p);
			}

			// Compute height
			double sinLatFinal = System.Math.Sin(lat);
			double Nfinal = a / System.Math.Sqrt(1 - e2 * sinLatFinal * sinLatFinal);
			double h = p / System.Math.Cos(lat) - Nfinal;

			// Convert radians → degrees
			double latDeg = lat * 180.0 / System.Math.PI;
			double lonDeg = lon * 180.0 / System.Math.PI;

			SpatialReferenceWithHeight spatialReferenceWithHeight = new SpatialReferenceWithHeight(latDeg, lonDeg, h);
			return spatialReferenceWithHeight;
		}
	}
}
