package calculator

import (
	"fmt"
	"math"
	"time"

	"github.com/sirupsen/logrus"
	"gonum.org/v1/gonum/mat"
)

type IntersectionPoint struct {
	X84, Y84, Z84 float64
	Lat           float64
	Lon           float64
	H             float64
}

func Camera2GroundPoint(q Quaternion, satPos84 []float64, satTime time.Time, ucam int, groundH int) (IntersectionPoint, error) {
	// alpha := FOV * math.Pi / 180.0
	// alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
	// direction := []float64{0, math.Tan(alpha), -1.3}
	direction := CameraDirectionVec(0, float64(ucam))

	// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
	Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, CameraYaw*math.Pi/180.0)
	var dCam mat.VecDense
	dCam.MulVec(Rcam, mat.NewVecDense(3, direction))

	// -------- 转到ECI坐标系 --------
	Ratt := q.ToRotationMatrix()
	var result mat.VecDense
	result.MulVec(Ratt, &dCam)
	dECI := result.RawVector().Data

	// -------- 转到ECEF坐标系 --------
	x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime)
	dECEF := []float64{x, y, z}

	// -------- 计算与地球表面的交点 --------
	intersection, err := SolveEllipticEquation(satPos84, dECEF, groundH)
	if err != nil {
		return IntersectionPoint{}, err
	}

	lat, lon, h := ECEFGeocentricToGeodetic(intersection[0], intersection[1], intersection[2])
	return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
}

func IntersectionECI(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
	alpha := FOV * math.Pi / 180.0
	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
	direction := []float64{0, math.Tan(alpha), -1.3} // 卫星（相机）坐标系下CCD成像方向向量

	// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
	Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, 0)
	var dCam mat.VecDense
	dCam.MulVec(Rcam, mat.NewVecDense(3, direction))

	// -------- 转到轨道坐标系 --------
	Rsat2orbit := Qsat2orbit.ToRotationMatrix()
	var r0 mat.VecDense
	r0.MulVec(Rsat2orbit, &dCam)
	dOrbit := r0.RawVector().Data

	// -------- 转到ECI坐标系 --------
	Rorbit2eci := Qorbit2eci.ToRotationMatrix()
	var r1 mat.VecDense
	r1.MulVec(Rorbit2eci, mat.NewVecDense(3, dOrbit))
	dECI := r1.RawVector().Data

	// -------- 转到ECEF坐标系 --------
	x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime)
	dECEF := []float64{x, y, z}

	// -------- 计算交点 --------}
	intersection, err := SolveEllipticEquation(satPos84, dECEF, 0)
	if err != nil {
		return IntersectionPoint{}, err
	}

	lat, lon, h := ECEFGeocentricToGeodetic(intersection[0], intersection[1], intersection[2])

	return IntersectionPoint{
		X84: intersection[0], Y84: intersection[1], Z84: intersection[2],
		Lat: lat, Lon: lon, H: h,
	}, nil
}

// 计算与椭球表面的交点
func SolveEllipticEquation(p0, d []float64, groundH int) ([]float64, error) {
	a2 := (a + float64(groundH)) * (a + float64(groundH))
	b2 := (b + float64(groundH)) * (b + float64(groundH))

	A := d[0]*d[0]/a2 + d[1]*d[1]/a2 + d[2]*d[2]/b2
	B := 2 * (p0[0]*d[0]/a2 + p0[1]*d[1]/a2 + p0[2]*d[2]/b2)
	C := p0[0]*p0[0]/a2 + p0[1]*p0[1]/a2 + p0[2]*p0[2]/b2 - 1

	delta := B*B - 4*A*C
	if delta < 0 {
		logrus.Error("line of sight: no intersection with ellipsoid")
		return nil, fmt.Errorf("line of sight: no intersection with ellipsoid") // No intersection
	}
	t1 := (-B + math.Sqrt(delta)) / (2 * A)
	t2 := (-B - math.Sqrt(delta)) / (2 * A)
	t := math.Max(t1, t2)
	return []float64{
		p0[0] + t*d[0],
		p0[1] + t*d[1],
		p0[2] + t*d[2],
	}, nil
}
