package calculator

import (
	"math"

	"gonum.org/v1/gonum/mat"
	"starwiz.cn/sjy01/image-proc/pkg/payload"
)

const (
	FocalLength = 1300.0 // 焦距, mm
	FOV         = 1.7    // 对角线视场角,degree
	FOVCALC     = 1.86   // 对角线视场角,degree
	PANPixels   = float64(payload.PAN_PIXEL_WIDTH)
	PANCellSize = 3.2 // µm
	MSSPixels   = float64(payload.MSS_PIXEL_WIDTH)
	MSSCellSize = 12.8   // µm
	CameraRoll  = -0.010 // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供
	CameraPitch = 0.150  // 相机与卫星本体Y轴的安装角度, degree
	CameraYaw   = 0.010  // 相机与卫星本体Z轴的安装角度, degree
)

// 计算过程使用PAN分辨率
func CameraDirectionVec(u, v float64) []float64 {
	// w := PANPixels * PANCellSize / 1000.0 // 像素宽度, mm
	// h := w
	// d := math.Sqrt(math.Pow(w, 2) + math.Pow(h, 2)) // 对角线长度, mm
	// fov := 2 * math.Atan2(w/2, FocalLength)         // 对角线视场角

	// fmt.Println("Camera Parameters:")
	// fmt.Printf("Focal Length: %.6f mm\n", FocalLength)
	// fmt.Printf("FOV (calculated): %.6f degree\n", fov*180/math.Pi)
	// fmt.Printf("Width: %.6f mm\n", w)
	// fmt.Printf("Height: %.6f mm\n", h)
	// fmt.Printf("Diagonal: %.6f mm\n", d)

	// 从给定FOV计算d
	// dCalcOfFOV := 2 * FocalLength * math.Tan(FOV/2*math.Pi/180)
	// fmt.Printf("Diagonal (calculated from FOV): %.6f mm\n", dCalcOfFOV)

	directionVec := []float64{0, 0, 0}
	directionVec[0] = 0                                                 // x方向, mm，线性CCD每次单行成像
	directionVec[1] = (v - PANPixels/2) * PANCellSize / 1000.0 / 1000.0 // y方向, mm
	directionVec[2] = -FocalLength / 1000.0                             // z方向, mm

	// deltaFOV := (FOV * math.Pi / 180) / PANPixels
	// directionVec[1] = math.Tan((v - PANPixels/2) * deltaFOV)
	// directionVec[2] = -1

	// 归一化
	// fmt.Printf("Direction Vector: (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])
	// directionVec = normalizeVec(directionVec)
	// fmt.Printf("Direction Vector (normalized): (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])

	return directionVec
}

func normalizeVec(vec []float64) []float64 {
	var norm float64
	for i := 0; i < len(vec); i++ {
		norm += vec[i] * vec[i]
	}

	for i := 0; i < len(vec); i++ {
		vec[i] /= math.Sqrt(norm)
	}
	return vec
}

// 假设相机相对卫星本体的安装矩阵只在Y轴方向旋转角度为θ
func CameraRotMatrix(phi, theta, psi float64) *mat.Dense {
	Rx := mat.NewDense(3, 3, []float64{
		1, 0, 0,
		0, math.Cos(phi), -math.Sin(phi),
		0, math.Sin(phi), math.Cos(phi),
	})

	Ry := mat.NewDense(3, 3, []float64{
		math.Cos(theta), 0, math.Sin(theta),
		0, 1, 0,
		-math.Sin(theta), 0, math.Cos(theta),
	})

	Rz := mat.NewDense(3, 3, []float64{
		math.Cos(psi), -math.Sin(psi), 0,
		math.Sin(psi), math.Cos(psi), 0,
		0, 0, 1,
	})

	// R = Rz * Ry * Rx
	RyRx := mat.NewDense(3, 3, nil)
	RyRx.Mul(Ry, Rx)
	Rzxy := mat.NewDense(3, 3, nil)
	Rzxy.Mul(Rz, RyRx)

	return Rzxy
}