package calculator

import "math"

func WGS84XYZtoLatLngH(X, Y, Z float64) (float64, float64, float64) {
	return ECEFGeocentricToGeodetic(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
}

// Converts degrees to radians.
func degreesToRadians(degrees float64) float64 {
	return degrees * math.Pi / 180
}

// Converts radians to degrees.
func radiansToDegrees(radians float64) float64 {
	return radians * 180 / math.Pi
}

// Calculate destination point given a start point (lat1, lng1), distance dx, dy (in meters).
func CalculateDestination(lat1, lng1, dx, dy float64) (float64, float64) {
	// Convert latitude and longitude from degrees to radians.
	lat1Rad := degreesToRadians(lat1)
	lng1Rad := degreesToRadians(lng1)

	// Radius of the Earth at the given latitude.
	radius := EarthRadius * math.Cos(lat1Rad)

	// Calculate the new latitude in radians.
	newLatRad := lat1Rad + dy/EarthRadius

	// Calculate the new longitude in radians.
	newLngRad := lng1Rad + dx/radius

	// Convert the new latitude and longitude back to degrees.
	newLat := radiansToDegrees(newLatRad)
	newLng := radiansToDegrees(newLngRad)

	return newLat, newLng
}

const (
	MetersPerDegreeLatitude = 111320.0 // 1度纬度约等于111,320米
)

// Converts a distance in meters to degrees longitude at a specific latitude
func MetersToDegreesLongitude(meters float64, latitude float64) float64 {
	return meters / (111320.0 * cosLatitude(latitude))
}

// Approximates the cosine of the latitude in radians
func cosLatitude(latitude float64) float64 {
	return 1.0 / (1.0 + (latitude/90.0)*(latitude/90.0))
}