package calculator

import (
	"fmt"
	"math"
)

// Quaternion represents a quaternion with scalar (w) and vector (x, y, z) parts
type Quaternion struct {
	w, x, y, z float64
}

// Quaternion multiplication
func (q1 Quaternion) Mul(q2 Quaternion) Quaternion {
	return Quaternion{
		w: q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
		x: q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y,
		y: q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x,
		z: q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w,
	}
}

// Quaternion conjugate
func (q Quaternion) Conjugate() Quaternion {
	return Quaternion{w: q.w, x: -q.x, y: -q.y, z: -q.z}
}

// Rotate vector by quaternion
func (q Quaternion) Rotate(v [3]float64) [3]float64 {
	qv := Quaternion{w: 0, x: v[0], y: v[1], z: v[2]}
	qConj := q.Conjugate()
	qvRotated := q.Mul(qv).Mul(qConj)
	return [3]float64{qvRotated.x, qvRotated.y, qvRotated.z}
}

func main() {
	// 示例数据
	qBI := Quaternion{w: 1, x: 0, y: 0, z: 0} // 本体相对惯性系四元数
	posJ2000 := [3]float64{7000, 0, 0}        // J2000位置
	// velJ2000 := [3]float64{0, 7.5, 0}         // J2000速度

	// 相机参数
	const numPixels = 9520
	const fov = 10.0 * math.Pi / 180 // 假设视场角为10度

	// 逐像素计算地面交点
	for i := 0; i < numPixels; i++ {
		// 计算像素点相对光轴的偏角
		pixelOffset := (float64(i) - float64(numPixels)/2) / float64(numPixels)
		angle := pixelOffset * fov

		// 假设光轴在本体坐标系中指向-z方向，计算视线方向
		dBody := [3]float64{-math.Sin(angle), 0, -math.Cos(angle)}

		// 转换到惯性系
		dInertial := qBI.Rotate(dBody)

		// 计算地面交点（假设dInertial已经标准化）
		k := -posJ2000[2] / dInertial[2] // 简化的交点计算
		groundPoint := [3]float64{
			posJ2000[0] + k*dInertial[0],
			posJ2000[1] + k*dInertial[1],
			posJ2000[2] + k*dInertial[2],
		}

		// 转换到地理坐标
		lat, lon, _ := ECEFToGeodetic(groundPoint[0], groundPoint[1], groundPoint[2])
		fmt.Printf("Pixel %d: Latitude: %f, Longitude: %f\n", i, lat, lon)
	}
}