package calculator

import "math"

func WGS84XYZtoLatLngH(X, Y, Z float64) (float64, float64, float64) {
	return ECEFToGeodetic(X, Y, Z)
}

// Function to convert ECEF (ITRS) coordinates to geodetic coordinates (latitude, longitude, height)
func ECEFToGeodetic(X, Y, Z float64) (float64, float64, float64) {
	b := a * (1 - f)
	e2Prime := e2 * (a * a) / (b * b)
	p := math.Sqrt(X*X + Y*Y)
	theta := math.Atan2(Z*a, p*b)

	// Calculate Longitude
	longitude := math.Atan2(Y, X)

	// Calculate Latitude
	latitude := math.Atan2(Z+e2Prime*b*math.Pow(math.Sin(theta), 3), p-e2*a*math.Pow(math.Cos(theta), 3))

	// Calculate Height
	N := a / math.Sqrt(1-e2*math.Pow(math.Sin(latitude), 2))
	height := p/math.Cos(latitude) - N

	// Convert radians to degrees for latitude and longitude
	latitudeDeg := latitude * 180 / math.Pi
	longitudeDeg := longitude * 180 / math.Pi

	return latitudeDeg, longitudeDeg, height
}