package calculator

import (
	"fmt"
	"math"
)

const (
	FocalLength = 1300.0 // 焦距, mm
	FOV         = 1.7    // 对角线视场角,degree
	FOVCALC     = 1.86   // 对角线视场角,degree
	PANPixels   = 9344.0
	PANCellSize = 3.2 // µm
	MSSPixels   = 2336.0
	MSSCellSize = 12.8 // µm
)

// 计算过程使用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(d/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 // y方向, mm
	directionVec[2] = -FocalLength                             // z方向, mm

	// 归一化
	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
}