package auxilary

import (
	"fmt"
	"math"
	"os"

	"starwiz.cn/sjy01/image-proc/pkg/utils"
)

type GPSs struct {
	GPSs []*GPS
}

func (g GPSs) Save(gpsFile string) error {
	f, err := os.Create(gpsFile)
	if err != nil {
		return err
	}
	defer f.Close()

	for _, gps := range g.GPSs {
		content := fmt.Sprintf("%.6f %.8f %.8f %.8f %.8f %.8f %.8f\n",
			gps.UTCTimestampSec,
			gps.X84, gps.Y84, gps.Z84,
			gps.Vx84, gps.Vy84, gps.Vz84)
		f.WriteString(content)
	}

	return nil
}

// 采用拉格朗日内插方法获得gps位置
// 输入：t为UTC时间戳，单位为秒
func (g GPSs) Lagrange(t float64) *GPS {
	// 找到扫描行成像时刻前后四个时间点的位置矢量
	var gps_sample []*GPS
	var idx int

	if t-g.GPSs[0].UTCTimestampSec < 1e-7 {
		return g.GPSs[0]
	} else if t-g.GPSs[len(g.GPSs)-1].UTCTimestampSec > -1e-7 {
		return g.GPSs[len(g.GPSs)-1]
	}

	for i, g := range g.GPSs {
		if g.UTCTimestampSec <= t {
			idx = i
		}
	}

	idx0 := int(math.Max(0, float64(idx-3)))
	for i := idx0; i <= idx; i++ {
		gps_sample = append(gps_sample, g.GPSs[i])
	}

	idx1 := int(math.Min(float64(len(g.GPSs))-1, float64(idx+1)))
	idx2 := int(math.Min(float64(len(g.GPSs))-1, float64(idx+4)))
	for i := idx1; i <= idx2; i++ {
		gps_sample = append(gps_sample, g.GPSs[i])
	}

	var x, y, z, tt []float64
	for i := 0; i < len(gps_sample); i++ {
		tt = append(tt, gps_sample[i].UTCTimestampSec)
		x = append(x, gps_sample[i].X84)
		y = append(y, gps_sample[i].Y84)
		z = append(z, gps_sample[i].Z84)
	}

	px := utils.InterpLagrange(tt, x, t)
	py := utils.InterpLagrange(tt, y, t)
	pz := utils.InterpLagrange(tt, z, t)

	return &GPS{
		UTCTimestampSec: t,
		X84:             px,
		Y84:             py,
		Z84:             pz,
		Vx84:            g.GPSs[idx].Vx84,
		Vy84:            g.GPSs[idx].Vy84,
		Vz84:            g.GPSs[idx].Vz84,
	}
}

type GPS struct {
	UTCTimestampSec  float64
	X84, Y84, Z84    float64
	Vx84, Vy84, Vz84 float64
}

func StoreGPS(aps []*AuxPlatform, gpsFile string) (*GPSs, error) {
	gpss := ExtractGPS(aps)
	var interGPS GPSs
	for _, gps := range gpss.GPSs {
		interGPS.GPSs = append(interGPS.GPSs, gps)
		t := gps.UTCTimestampSec + 0.098678123
		interGPS.GPSs = append(interGPS.GPSs, gpss.Lagrange(t))
	}

	return gpss, gpss.Save(gpsFile)
}

func ExtractGPS(aps []*AuxPlatform) *GPSs {
	var gpss GPSs
	var sec, microsec uint32
	for _, ap := range aps {
		if ap.UTCTimeSec != sec || ap.Microsecond != microsec {
			sec, microsec = ap.UTCTimeSec, ap.Microsecond
			gps := GPS{
				UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) +
					float64(transfromGPSandAttMicrosec(microsec))/1e6,
				X84:  ap.W84PosX,
				Y84:  ap.W84PosY,
				Z84:  ap.W84PosZ,
				Vx84: ap.W84VelX,
				Vy84: ap.W84VelY,
				Vz84: ap.W84VelZ,
			}

			gpss.GPSs = append(gpss.GPSs, &gps)
		}
	}

	return &gpss
}