package auxilary

import (
	"fmt"
	"math"
	"os"
)

type Attitudes struct {
	Atts []*Attitude
}

func (atts Attitudes) Save(attFile string) error {
	f, err := os.Create(attFile)
	if err != nil {
		return err
	}
	defer f.Close()

	for _, att := range atts.Atts {
		content := fmt.Sprintf("%.6f %.8f %.8f %.8f %.8f\n", att.UTCTimestampSec, att.Q0, att.Q1, att.Q2, att.Q3)
		f.WriteString(content)
	}

	return nil
}

// 四元数球面线性插值
func (atts Attitudes) Slerp(t float64) *Attitude {
	// 取 t 前后的四元数
	var q0, q1 *Attitude

	for i, att := range atts.Atts {
		if att.UTCTimestampSec >= t {
			if i == 0 {
				q0 = att
				q1 = atts.Atts[i+1]
			} else {
				q0 = atts.Atts[i-1]
				q1 = att
			}
			break
		}
	}

	if q0 == nil || q1 == nil {
		return atts.Atts[len(atts.Atts)-1]
	}

	var w0, x0, y0, z0, w1, x1, y1, z1 float64
	w0, x0, y0, z0 = q0.Q0, q0.Q1, q0.Q2, q0.Q3
	w1, x1, y1, z1 = q1.Q0, q1.Q1, q1.Q2, q1.Q3

	// 用点乘计算两个四元数夹角的cos值
	cosOmega := w0*w1 + x0*x1 + y0*y1 + z0*z1

	// 如果点乘为负，则反转一个四元数以取得短的4D弧
	if cosOmega < 0.0 {
		w1 = -w1
		x1 = -x1
		y1 = -y1
		z1 = -z1
		cosOmega = -cosOmega
	}

	var k0, k1 float64
	t0, t1 := q0.UTCTimestampSec, q1.UTCTimestampSec
	if cosOmega > 0.99999999 { // cos=1的时候就是夹角为0，重合
		k0 = (t1 - t) / (t1 - t0)
		k1 = (t - t0) / (t1 - t0)
	} else {
		// 用三角公式sin^2+cos^2=1计算sin值
		sinOmega := math.Sqrt(1.0 - cosOmega*cosOmega)
		// 通过sin和cos计算角度
		omega := math.Atan2(sinOmega, cosOmega) // 计算点(cosOmega,sinOmega)与x轴正向的夹角
		// 计算分母的倒数，这样就只需要一次除法
		oneOverSinOmega := 1.0 / sinOmega
		//计算插值变量
		k0 = math.Sin((t1-t)/(t1-t0)*omega) * oneOverSinOmega
		k1 = math.Sin((t-t0)/(t1-t0)*omega) * oneOverSinOmega
	}
	// 插值
	w := w0*k0 + w1*k1
	x := x0*k0 + x1*k1
	y := y0*k0 + y1*k1
	z := z0*k0 + z1*k1

	return &Attitude{
		UTCTimestampSec: t,
		Q0:              w,
		Q1:              x,
		Q2:              y,
		Q3:              z,
	}
}

type Attitude struct {
	UTCTimestampSec float64
	Q0, Q1, Q2, Q3  float64
}

func StoreAtt(aps []*AuxPlatform, attFile string) (*Attitudes, error) {
	atts := ExtractAttitude(aps)
	return atts, atts.Save(attFile)
}

func ExtractAttitude(aps []*AuxPlatform) *Attitudes {
	var atts Attitudes
	var sec, microsec uint32
	for _, ap := range aps {
		if ap.UTCTimeSec != sec || ap.Microsecond != microsec {
			sec, microsec = ap.UTCTimeSec, ap.Microsecond
			att := Attitude{
				UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) +
					float64(transfromGPSandAttMicrosec(microsec))/1e6,
				Q0: ap.QuatAttstarQ0,
				Q1: ap.QuatAttstarQ1,
				Q2: ap.QuatAttstarQ2,
				Q3: ap.QuatAttstarQ3,
			}
			atts.Atts = append(atts.Atts, &att)
		}
	}

	return &atts
}