package extract

import (
	"fmt"
	"math"
)

// 定义常数
const (
	GM = 3.986004418e14 // 地球引力常数（m^3/s^2）
	Re = 6378137.0      // 地球半径（m）
)

// 四元数
type Quaternion struct {
	w, x, y, z float64
}

// 矢量
type Vector3 struct {
	x, y, z float64
}

// 计算轨道参数
func calculateOrbitalParameters(position, velocity Vector3) (float64, float64, float64, float64, float64) {
	r := math.Sqrt(position.x*position.x + position.y*position.y + position.z*position.z)
	v := math.Sqrt(velocity.x*velocity.x + velocity.y*velocity.y + velocity.z*velocity.z)

	// 计算轨道参数
	energy := 0.5*v*v - GM/r // 比能
	a := -GM / (2 * energy)  // 轨道半长轴
	e := math.Sqrt(1 - (math.Pow(math.Sqrt(math.Pow(position.x*velocity.y-position.y*velocity.x, 2)+
		math.Pow(position.y*velocity.z-position.z*velocity.y, 2)+
		math.Pow(position.z*velocity.x-position.x*velocity.z, 2)), 2) / (GM * a)))
	i := math.Acos(position.z / r)                                                                                // 轨道倾角
	Omega := math.Atan2(position.x, -position.y)                                                                  // 升交点赤经
	omega := math.Atan2(position.z*velocity.x-position.x*velocity.z, position.x*velocity.y-position.y*velocity.x) // 升交点赤纬
	return a, e, i * 180 / math.Pi, Omega * 180 / math.Pi, omega * 180 / math.Pi
}

// 四元数到旋转矩阵
func quaternionToRotationMatrix(q Quaternion) [3][3]float64 {
	w, x, y, z := q.w, q.x, q.y, q.z
	return [3][3]float64{
		{1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w},
		{2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w},
		{2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y},
	}
}

// 向量加法
func add(v1, v2 Vector3) Vector3 {
	return Vector3{v1.x + v2.x, v1.y + v2.y, v1.z + v2.z}
}

// 向量数乘
func scale(v Vector3, c float64) Vector3 {
	return Vector3{v.x * c, v.y * c, v.z * c}
}

// 矩阵乘法
func matrixMult(m [3][3]float64, v Vector3) Vector3 {
	return Vector3{
		m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z,
		m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z,
		m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z,
	}
}

// 计算图像位置
func calculateImagePosition(roll, pitch, yaw float64, quat Quaternion, satellitePos, satelliteVel Vector3) (float64, float64) {
	// 将角度转换为弧度
	roll = roll * math.Pi / 180
	pitch = pitch * math.Pi / 180
	yaw = yaw * math.Pi / 180

	// 构造旋转矩阵
	R := quaternionToRotationMatrix(quat)
	// 传感器坐标系中的Z轴向量
	sensorZ := Vector3{0, 0, 1}
	// 将Z轴向量通过旋转矩阵转换到卫星坐标系中
	lineOfSight := matrixMult(R, sensorZ)

	// 计算交点经纬度
	lat := math.Asin(lineOfSight.z) * 180 / math.Pi
	lon := math.Atan2(lineOfSight.y, lineOfSight.x) * 180 / math.Pi

	return lat, lon
}

func Calculate(satellitePos, satelliteVel Vector3, quat Quaternion) {
	// 假设一些已知数据
	// satellitePos := Vector3{x: 7000e3, y: 0, z: 0}       // 假设一个简单的地心坐标位置
	// satelliteVel := Vector3{x: 0, y: 7.5e3, z: 0}        // 假设一个简单的速度
	// quat := Quaternion{w: 0.7071, x: 0.7071, y: 0, z: 0} // 假设一个简单的四元数
	roll := 0.0
	pitch := 0.0
	yaw := 0.0

	// 计算轨道参数
	a, e, i, Omega, omega := calculateOrbitalParameters(satellitePos, satelliteVel)

	// 打印轨道参数
	fmt.Println("轨道参数：")
	fmt.Printf("轨道半长轴 (a): %.2f m\n", a)
	fmt.Printf("偏心率 (e): %.6f\n", e)
	fmt.Printf("轨道倾角 (i): %.2f 度\n", i)
	fmt.Printf("升交点赤经 (Ω): %.2f 度\n", Omega)
	fmt.Printf("近地点幅角 (ω): %.2f 度\n", omega)

	// 计算图像位置
	lat, lon := calculateImagePosition(roll, pitch, yaw, quat, satellitePos, satelliteVel)
	fmt.Printf("\n图像位置：\n纬度: %.6f\n经度: %.6f\n", lat, lon)
}