package calculator

import (
	"math"
	"time"
)

// Function to convert current J2000 position to WGS84
func J2000ToWGS84(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
	itrsX, itrsY, itrsZ := ECItoECEF(j2000X, j2000Y, j2000Z, utc)
	// Convert ITRS to geodetic coordinates (WGS84)
	latitude, longitude, height := ECEFToGeodetic(itrsX, itrsY, itrsZ)
	return latitude, longitude, height
}

func ECItoECEF(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
	julianDate := UTCToJulianDate(utc)
	gast := CalculateGAST(julianDate, utc)

	// Calculate rotation matrix from J2000 to ITRS
	rotationMatrix := CreateRotationMatrix(gast)

	// Rotate J2000 position to ITRS
	itrsX := rotationMatrix[0][0]*j2000X + rotationMatrix[0][1]*j2000Y + rotationMatrix[0][2]*j2000Z
	itrsY := rotationMatrix[1][0]*j2000X + rotationMatrix[1][1]*j2000Y + rotationMatrix[1][2]*j2000Z
	itrsZ := rotationMatrix[2][0]*j2000X + rotationMatrix[2][1]*j2000Y + rotationMatrix[2][2]*j2000Z

	return itrsX, itrsY, itrsZ
}

// Function to convert UTC to Julian Date
func UTCToJulianDate(t time.Time) float64 {
	year, month, day := t.Date()
	hour, minute, second := t.Clock()
	fracOfDay := float64(hour)/24 + float64(minute)/(24*60) + float64(second)/(24*60*60)

	if month <= 2 {
		year -= 1
		month += 12
	}

	A := year / 100
	B := 2 - A + A/4
	julianDate := float64(int(365.25*float64(year))) + float64(int(30.6001*float64(month+1))) + float64(day) + 1720994.5 + fracOfDay + float64(B)
	return julianDate
}

// Function to calculate GAST (Greenwich Apparent Sidereal Time)
func CalculateGAST(julianDate float64, utc time.Time) float64 {
	// Calculate Julian centuries since J2000.0
	T := (julianDate - J2000Epoch) / 36525.0

	// Calculate Greenwich Mean Sidereal Time (GMST) in degrees
	GMST := 280.46061837 + 360.98564736629*(julianDate-J2000Epoch) + 0.000387933*T*T - T*T*T/38710000
	GMST = math.Mod(GMST, 360.0)
	if GMST < 0 {
		GMST += 360.0
	}

	// Calculate the equation of the equinoxes (simplified)
	omega := 125.04 - 0.052954*T
	L := 280.47 + 0.98565*T
	epsilon := 23.4393 - 0.0000004*T
	deltaPsi := -0.000319*math.Sin(math.Pi/180.0*omega) - 0.000024*math.Sin(math.Pi/180.0*2*L)

	// Greenwich Apparent Sidereal Time (GAST)
	GAST := GMST + deltaPsi*math.Cos(math.Pi/180.0*epsilon)
	GAST = math.Mod(GAST, 360.0)
	if GAST < 0 {
		GAST += 360.0
	}

	return GAST * math.Pi / 180.0 // Convert to radians
}

// Function to create the rotation matrix for the given GAST
func CreateRotationMatrix(gast float64) [3][3]float64 {
	return [3][3]float64{
		{math.Cos(gast), math.Sin(gast), 0},
		{-math.Sin(gast), math.Cos(gast), 0},
		{0, 0, 1},
	}
}