package auxilary

import (
	"encoding/binary"
	"fmt"
	"os"
	"time"

	"github.com/k0kubun/pp/v3"
	"starwiz.cn/sjy01/image-proc/pkg/calculator"

	log "github.com/sirupsen/logrus"
)

const AuxPlatformFrmSize = 512 // 512 bytes

// 中心机辅助数据 512 字节 (每 16 行原始图像数据为一组)
type AuxPlatform struct {
	// 结构中标注的字节序号从 1 开始计数
	UTCTimeSec     uint32    // [1-4] 卫星 UTC 时间戳（秒）
	Waveway        uint8     // [5] 波道 0x00：波道1（241.0~311.0）0x01：波道2（241.1~311.1）
	Microsecond    uint32    // [6-8]卫星秒小数（微秒）
	QuatAttstarQ0  float64   // [9-12]定姿四元数(J2000) 的 Q0 值，量纲 1/0x40000000
	QuatAttstarQ1  float64   // [13-16]Q1 值
	QuatAttstarQ2  float64   // [17-20]Q2 值
	QuatAttstarQ3  float64   // [21-24]Q3 值
	QuatOrbitQ1    float64   // [25-28]本体相对轨道四元数矢部 的 Q1 值，量纲 1/0x40000000
	QuatOrbitQ2    float64   // [29-32]Q2 值
	QuatOrbitQ3    float64   // [33-36]Q3 值Q3 值
	QuatOrbJQ1     float64   // [37-40]轨道相对惯性系四元数矢部 的 Q1 值，量纲 1/0x40000000
	QuatOrbJQ2     float64   // [41-44]Q2 值
	QuatOrbJQ3     float64   // [45-48]Q3 值
	Eular1         float64   // [49-52]本体相对轨道姿态角，[X]Roll 测摆角，量纲：1/1000000 单位：rad
	Eular2         float64   // [53-56] Pitch [Y] Pitch
	Eular3         float64   // [57-60] Roll [Z] YAW
	DotEular1      float64   // [61-62]本体相对轨道角速度，量纲：1/100000 单位：rad
	DotEular2      float64   // [63-64]
	DotEular3      float64   // [65-66]
	ModTime        uint32    // [67-70]模式运行时间 秒
	DTime          float64   // [71-72]姿控调用周期，量纲：1/1000 单位：秒
	AutoState      [3]uint8  // [73-75]姿轨控部件使用标志
	ProTrack       [16]uint8 // [76-91]姿轨控算法执行标记
	QeQ1           float64   // [92-95]偏差四元数 double 量纲：1/0x40000000 /bit
	QeQ2           float64   // [96-99]
	QeQ3           float64   // [100-103]
	We1            float64   // [104-105]偏差角速度 量纲：1/0x100000 单位：rad
	We2            float64   // [106-107]
	We3            float64   // [108-109]
	WTFX           float64   // [110-111]X飞轮估计摩擦力矩 量纲：1/0x400000
	WTFY           float64   // [112-113]
	WTFZ           float64   // [114-115]
	FbdriftX       float64   // [116-117]三轴陀螺X轴角速度漂移估计 1/0x4000000 rad/s
	FbdriftY       float64   // [118-119]
	FbdriftZ       float64   // [120-121]
	KalbX          float64   // [122-123]三轴角速度卡尔曼漂移X 量纲：1/0x4000000 单位：rad/s
	KalbY          float64   // [124-125]
	KalbZ          float64   // [126-127]
	HTDX           float64   // [128-129]X轴估计环境干扰力矩 量纲：1/0x200000
	HTDY           float64   // [130-131]
	HTDZ           float64   // [132-133]
	CommandWheelX  float64   // [134-135]X轴计算飞轮控制力矩 量纲：1/0x40000
	CommandWheelY  float64   // [136-137]
	CommandWheelZ  float64   // [138-139]
	QuatG1         float64   // [140-143]期望四元数矢部1 量纲：1/0x40000000 /bit
	QuatG2         float64   // [144-147]
	QuatG3         float64   // [148-151]
	WG1            float64   // [152-153]期望角速度1 量纲：1/0x40000
	WG2            float64   // [154-155]
	WG3            float64   // [156-157]
	J2000PosX      float64   // [158-161]计算当前J2000位置X 1/0x100 单位：m
	J2000PosY      float64   // [162-165]
	J2000PosZ      float64   // [166-169]
	J2000VelX      float64   // [170-173]计算当前J2000速度X 量纲：1/0x100 单位：m/s
	J2000VelY      float64   // [174-177]
	J2000VelZ      float64   // [178-181]
	W84PosX        float64   // [182-185]计算当前WGS 84位置X 1/0x100 单位：m
	W84PosY        float64   // [186-189]
	W84PosZ        float64   // [190-193]
	W84VelX        float64   // [194-197]计算当前WGS 84速度X 量纲：1/0x100 单位：m/s
	W84VelY        float64   // [198-201]
	W84VelZ        float64   // [202-205]
	AngleDraft     float64   // [206-209]偏流角 量纲：1/10000000 单位：rad
	DataTransLong  float64   // [210-211]数传点经度 量纲：1/1000
	DataTransLat   float64   // [212-213]数传点纬度 量纲：1/1000
	DataTransH     float64   // [214-215]数传点地程高 量纲：1
	StarPrio       [3]byte   // [216-218] 星敏1定姿方式标志1 0x00：星敏1定姿；0xA0:星敏1滤波;0x01星敏2定姿；0xA1:星敏2滤波;0xff:未使用星敏定姿
	WheelspeedcalX float64   // [219-222]X飞轮期望转速 量纲：1/10000 单位：rpm
	WheelspeedcalY float64   // [223-226]
	WheelspeedcalZ float64   // [227-230]
	Reserved0      [2]byte   // 预留字节
	Reserved1      [4]byte   // 预留字节
	Reserved2      [2]byte   // 预留字节
	Reserved3      [2]byte   // 预留字节
	// 241.0 - 311.0 波道1参数
	// 241.1 - 311.1 波道2参数
	WGS84PosX          float64 // [312-315]计算当前WGS 84位置X 单位：0.01米
	WGS84PosY          float64 // [316-319]
	WGS84PosZ          float64 // [320-323]
	WGS84VelX          float64 // [324-327]计算当前WGS 84速度X 单位：0.01米/秒
	WGS84VelY          float64 // [328-331]
	WGS84VelZ          float64 // [332-335]
	J2000Pos_X         float64 // [336-339]计算当前J2000位置X 单位：0.01米
	J2000Pos_Y         float64 // [340-343]
	J2000Pos_Z         float64 // [344-347]
	J2000Vel_X         float64 // [348-351]计算当前J2000速度X 单位：0.01米/秒
	J2000Vel_Y         float64 // [352-355]
	J2000Vel_Z         float64 // [356-359]
	FOGyroXAV          float64 // [360-362]三轴光纤陀螺X轴角速度 陀螺速度共3字节单位°/s量纲：1/72359
	FOGyroYAV          float64 // [363-365]
	FOGyroZAV          float64 // [366-368]
	FOGyroXAV1         float64 // [369-371]
	FOGyroYAV1         float64 // [372-374]
	FOGyroZAV1         float64 // [375-377]
	SS1_UTCTime        uint32  // [378-381] 星敏1 UTC时间
	SS1_UTCTimeFrac    float32 // [382-384] 星敏1 UTC秒小数 单位为40.96 us，高字节在前
	SS1_Q1             float64 // [385-388] 星敏1四元数q1 当量：1/2147483647 （星敏坐标系相对于J2000惯性坐标系）
	SS1_Q2             float64 // [389-392] 星敏1四元数q2
	SS1_Q3             float64 // [393-396] 星敏1四元数q3
	SS1_Q4             float64 // [397-400] 星敏1四元数q4
	SS1_ExposureTime   uint8   // [401] 星敏1曝光时间 无符号数，单位ms
	SS1_Threshold      uint8   // [402] 星敏1阈值 无符号数
	SS1_BackgroudV     uint8   // [403] 星敏1背景值 无符号数
	SS1_Flags          byte    // [404] 星敏1标志位 0x01：曝光时间有效；0x02：阈值有效；0x04：背景值有效
	SS1_WorkMode       uint8   // [405.(7~6)] 星敏1工作模式: 0正常工作模式；1保留；2固定阈值模式；3测试模式（保留）
	SS1_ExtractStars   uint8   // [405.(5~0)]
	SS1_NavStars       uint8   // [406.(7~5)] 星敏1导航星数
	SS1_Gain           uint8   // [406.(4~0)] 星敏1增益
	SS1_RegonizedStars uint8   // [407.(7~2)] 星敏1识别星数
	SS1_ExtenalImage   bool    // [407.(1)] 星敏1外部图像标志位 0x01：外部图像；0x00：关闭
	SS1_AttitudeActive bool    // [407.(0)] 星敏1姿态有效标志位 0x01：激活；0x00：关闭
	SS1_ImgFrmNo       uint32  // [411-413] 星敏1图像帧号
	SS1_XAV            float64 // [417-418] 星敏1X方向角速度 单位：2e-11 °/s
	SS1_YAV            float64 // [419-420]
	SS1_ZAV            float64 // [421-422]
	SS2_UTCTime        uint32  // [424-427] 星敏2 UTC时间
	SS2_UTCTimeFrac    float32 // [428-430] 星敏2 UTC秒小数 单位为40.96 us，高字节在前
	SS2_Q1             float64 // [431-434] 星敏2四元数q1 当量：1/2147483647 （星敏坐标系相对于J2000惯性坐标系）
	SS2_Q2             float64 // [435-438] 星敏2四元数q2
	SS2_Q3             float64 // [439-442] 星敏2四元数q3
	SS2_Q4             float64 // [443-446] 星敏2四元数q4
	SS2_ExposureTime   uint8   // [447] 星敏2曝光时间 无符号数，单位ms
	SS2_Threshold      uint8   // [448] 星敏2阈值 无符号数
	SS2_BackgroudV     uint8   // [449] 星敏2背景值 无符号数
	SS2_Flags          byte    // [450] 星敏2标志位 0x01：曝光时间有效；0x02：阈值有效；0x04：背景值有效
	SS2_WorkMode       uint8   // [451.(7~6)] 星敏2工作模式: 0正常工作模式；1保留；2固定阈值模式；3测试模式（保留）
	SS2_ExtractStars   uint8   // [451.(5~0)]
	SS2_NavStars       uint8   // [452.(7~5)] 星敏2导航星数
	SS2_Gain           uint8   // [452.(4~0)] 星敏2增益
	SS2_RegonizedStars uint8   // [453.(7~2)] 星敏2识别星数
	SS2_ExtenalImage   bool    // [453.(1)] 星敏2外部图像标志位 0x01：外部图像；0x00：关闭
	SS2_AttitudeActive bool    // [453.(0)] 星敏2姿态有效标志位 0x01：激活；0x00：关闭
	SS2_ImgFrmNo       uint32  // [457-459] 星敏2图像帧号
	FlyWheel1_Vel      float64 // [464-466] 飞轮1速度 单位：0.1rpm
	FlyWheel1_Amps     float64 // [467] 飞轮1电流 单位：0.009A
	FlyWheel2_Vel      float64 // [468-470] 飞轮2速度 单位：0.1rpm
	FlyWheel2_Amps     float64 // [471] 飞轮2电流 单位：0.009A
	FlyWheel3_Vel      float64 // [472-474] 飞轮3速度 单位：0.1rpm
	FlyWheel3_Amps     float64 // [475] 飞轮3电流 单位：0.009A
	FlyWheel4_Vel      float64 // [476-478] 飞轮4速度 单位：0.1rpm
	FlyWheel4_Amps     float64 // [479] 飞轮4电流 单位：0.009A
	MemsGyroXAV        float64 // [480-483] MEMS陀螺X轴角速度 0.000001°/s
	MemsGyroYAV        float64 // [484-487] MEMS陀螺Y轴角速度 0.000001°/s
	MemsGyroZAV        float64 // [488-491] MEMS陀螺Z轴角速度 0.000001°/s
	MagnetictrengthX   float64 // [492-495] 磁场强度X 量纲19200,单位uT
	MagnetictrengthY   float64 // [496-499] 磁场强度Y 量纲19200,单位uT
	MagnetictrengthZ   float64 // [500-503] 磁场强度Z 量纲19200,单位uT
	ASSTimeInt         uint32  // [504-507] 输入姿轨控数据UTC时间秒
	ASSTimeDec         uint32  // [508-510] 输入姿轨控数据UTC时间秒小数 单位为μs
	Reserved4          byte    // [511] 保留字节
	CheckSum           byte    // [512] 校验和
}

func (ap *AuxPlatform) Decode(data []byte) error {
	if len(data) < 512 {
		return fmt.Errorf("data length less than 512")
	}

	// 按需解析数据
	ap.UTCTimeSec = binary.BigEndian.Uint32(data[0:4])
	ap.Waveway = data[4]
	ap.Microsecond = uint32(data[5])<<16 | uint32(data[6])<<8 | uint32(data[7])
	ap.QuatAttstarQ0 = float64(bigEndianToInt32(data[8:12])) / 0x40000000
	ap.QuatAttstarQ1 = float64(bigEndianToInt32(data[12:16])) / 0x40000000
	ap.QuatAttstarQ2 = float64(bigEndianToInt32(data[16:20])) / 0x40000000
	ap.QuatAttstarQ3 = float64(bigEndianToInt32(data[20:24])) / 0x40000000
	ap.QuatOrbitQ1 = float64(bigEndianToInt32(data[24:28])) / 0x40000000
	ap.QuatOrbitQ2 = float64(bigEndianToInt32(data[28:32])) / 0x40000000
	ap.QuatOrbitQ3 = float64(bigEndianToInt32(data[32:36])) / 0x40000000
	ap.QuatOrbJQ1 = float64(bigEndianToInt32(data[36:40])) / 0x40000000
	ap.QuatOrbJQ2 = float64(bigEndianToInt32(data[40:44])) / 0x40000000
	ap.QuatOrbJQ3 = float64(bigEndianToInt32(data[44:48])) / 0x40000000
	ap.Eular1 = float64(bigEndianToInt32(data[48:52])) / 1000000
	ap.Eular2 = float64(bigEndianToInt32(data[52:56])) / 1000000
	ap.Eular3 = float64(bigEndianToInt32(data[56:60])) / 1000000
	ap.DotEular1 = float64(bigEndianToInt16(data[60:62])) / 100000
	ap.DotEular2 = float64(bigEndianToInt16(data[62:64])) / 100000
	ap.DotEular3 = float64(bigEndianToInt16(data[64:66])) / 100000
	ap.ModTime = binary.BigEndian.Uint32(data[66:70])
	ap.DTime = float64(bigEndianToInt16(data[70:72])) / 1000
	ap.AutoState[0] = uint8(data[72])
	ap.AutoState[1] = uint8(data[73])
	ap.AutoState[2] = uint8(data[74])
	for i := 0; i < 16; i++ {
		ap.ProTrack[i] = uint8(data[75+i])
	}
	ap.QeQ1 = float64(bigEndianToInt32(data[91:95])) / 0x40000000
	ap.QeQ2 = float64(bigEndianToInt32(data[95:99])) / 0x40000000
	ap.QeQ3 = float64(bigEndianToInt32(data[99:103])) / 0x40000000
	ap.We1 = float64(bigEndianToInt16(data[103:105])) / 0x100000
	ap.We2 = float64(bigEndianToInt16(data[105:107])) / 0x100000
	ap.We3 = float64(bigEndianToInt16(data[107:109])) / 0x100000
	ap.WTFX = float64(bigEndianToInt16(data[109:111])) / 0x400000
	ap.WTFY = float64(bigEndianToInt16(data[111:113])) / 0x400000
	ap.WTFZ = float64(bigEndianToInt16(data[113:115])) / 0x400000
	ap.FbdriftX = float64(bigEndianToInt16(data[115:117])) / 0x4000000
	ap.FbdriftY = float64(bigEndianToInt16(data[117:119])) / 0x4000000
	ap.FbdriftZ = float64(bigEndianToInt16(data[119:121])) / 0x4000000
	ap.KalbY = float64(bigEndianToInt16(data[121:123])) / 0x4000000
	ap.KalbZ = float64(bigEndianToInt16(data[123:125])) / 0x4000000
	ap.KalbX = float64(bigEndianToInt16(data[125:127])) / 0x4000000
	ap.HTDX = float64(bigEndianToInt16(data[127:129])) / 0x200000
	ap.HTDY = float64(bigEndianToInt16(data[129:131])) / 0x200000
	ap.HTDZ = float64(bigEndianToInt16(data[131:133])) / 0x200000
	ap.CommandWheelX = float64(bigEndianToInt16(data[133:135])) / 0x40000
	ap.CommandWheelY = float64(bigEndianToInt16(data[135:137])) / 0x40000
	ap.CommandWheelZ = float64(bigEndianToInt16(data[137:139])) / 0x40000
	ap.QuatG1 = float64(bigEndianToInt32(data[139:143])) / 0x40000000
	ap.QuatG2 = float64(bigEndianToInt32(data[143:147])) / 0x40000000
	ap.QuatG3 = float64(bigEndianToInt32(data[147:151])) / 0x40000000
	ap.WG1 = float64(bigEndianToInt16(data[151:153])) / 0x40000
	ap.WG2 = float64(bigEndianToInt16(data[153:155])) / 0x40000
	ap.WG3 = float64(bigEndianToInt16(data[155:157])) / 0x40000
	ap.J2000PosX = float64(bigEndianToInt32(data[157:161])) / 0x100
	ap.J2000PosY = float64(bigEndianToInt32(data[161:165])) / 0x100
	ap.J2000PosZ = float64(bigEndianToInt32(data[165:169])) / 0x100
	ap.J2000VelX = float64(bigEndianToInt32(data[169:173])) / 0x100
	ap.J2000VelY = float64(bigEndianToInt32(data[173:177])) / 0x100
	ap.J2000VelZ = float64(bigEndianToInt32(data[177:181])) / 0x100
	ap.W84PosX = float64(bigEndianToInt32(data[181:185])) / 0x100
	ap.W84PosY = float64(bigEndianToInt32(data[185:189])) / 0x100
	ap.W84PosZ = float64(bigEndianToInt32(data[189:193])) / 0x100
	ap.W84VelX = float64(bigEndianToInt32(data[193:197])) / 0x100
	ap.W84VelY = float64(bigEndianToInt32(data[197:201])) / 0x100
	ap.W84VelZ = float64(bigEndianToInt32(data[201:205])) / 0x100
	ap.AngleDraft = float64(bigEndianToInt32(data[205:209])) / 10000000
	ap.DataTransLong = float64(bigEndianToInt16(data[209:211])) / 1000
	ap.DataTransLat = float64(bigEndianToInt16(data[211:213])) / 1000
	ap.DataTransH = float64(bigEndianToInt16(data[213:215]))
	copy(ap.StarPrio[:], data[215:218])
	ap.WheelspeedcalX = float64(bigEndianToInt32(data[218:222])) / 10000
	ap.WheelspeedcalY = float64(bigEndianToInt32(data[222:226])) / 10000
	ap.WheelspeedcalZ = float64(bigEndianToInt32(data[226:230])) / 10000
	if ap.Waveway == 0x0 {
		// 241.0 - 311.0 波道1参数
	} else {
		// 241.1 - 311.1 波道2参数
	}
	ap.WGS84PosX = float64(bigEndianToInt32(data[311:315])) / 100
	ap.WGS84PosY = float64(bigEndianToInt32(data[315:319])) / 100
	ap.WGS84PosZ = float64(bigEndianToInt32(data[319:323])) / 100
	ap.WGS84VelX = float64(bigEndianToInt32(data[323:327])) / 100
	ap.WGS84VelY = float64(bigEndianToInt32(data[327:331])) / 100
	ap.WGS84VelZ = float64(bigEndianToInt32(data[331:335])) / 100
	ap.J2000Pos_X = float64(bigEndianToInt32(data[335:339])) / 100
	ap.J2000Pos_Y = float64(bigEndianToInt32(data[339:343])) / 100
	ap.J2000Pos_Z = float64(bigEndianToInt32(data[343:347])) / 100
	ap.J2000Vel_X = float64(bigEndianToInt32(data[347:351])) / 100
	ap.J2000Vel_Y = float64(bigEndianToInt32(data[351:355])) / 100
	ap.J2000Vel_Z = float64(bigEndianToInt32(data[355:359])) / 100
	ap.FOGyroXAV = float64(bigEndianToInt24(data[359:362])) / 72359
	ap.FOGyroYAV = float64(bigEndianToInt24(data[362:365])) / 72359
	ap.FOGyroZAV = float64(bigEndianToInt24(data[365:368])) / 72359
	ap.FOGyroXAV1 = float64(bigEndianToInt24(data[368:371])) / 72359
	ap.FOGyroYAV1 = float64(bigEndianToInt24(data[371:374])) / 72359
	ap.FOGyroZAV1 = float64(bigEndianToInt24(data[374:377])) / 72359
	ap.SS1_UTCTime = binary.BigEndian.Uint32(data[377:381])
	ap.SS1_UTCTimeFrac = float32(uint32(data[381])<<16|uint32(data[382])<<8|uint32(data[383])) * 40.96
	ap.SS1_Q1 = float64(bigEndianToInt32(data[384:388])) / 2147483647
	ap.SS1_Q2 = float64(bigEndianToInt32(data[388:392])) / 2147483647
	ap.SS1_Q3 = float64(bigEndianToInt32(data[392:396])) / 2147483647
	ap.SS1_Q4 = float64(bigEndianToInt32(data[396:400])) / 2147483647
	ap.SS1_ExposureTime = uint8(data[400])
	ap.SS1_Threshold = uint8(data[401])
	ap.SS1_BackgroudV = uint8(data[402])
	ap.SS1_Flags = data[403]
	ap.SS1_WorkMode = uint8(data[404] >> 6)
	ap.SS1_ExtractStars = uint8(data[404] & 0x3F)
	ap.SS1_NavStars = uint8(data[405] >> 5)
	ap.SS1_Gain = uint8(data[405] & 0x1F)
	ap.SS1_RegonizedStars = uint8(data[406] >> 2)
	ap.SS1_ExtenalImage = data[406]&0x02 == 0x02
	ap.SS1_AttitudeActive = data[406]&0x01 == 0x01
	ap.SS1_ImgFrmNo = uint32(data[410])<<16 | uint32(data[411])<<8 | uint32(data[412])
	ap.SS1_XAV = float64(bigEndianToInt16(data[416:418])) * 2e-11
	ap.SS1_YAV = float64(bigEndianToInt16(data[418:420])) * 2e-11
	ap.SS1_ZAV = float64(bigEndianToInt16(data[420:422])) * 2e-11
	ap.SS2_UTCTime = binary.BigEndian.Uint32(data[423:427])
	ap.SS2_UTCTimeFrac = float32(uint32(data[427])<<16|uint32(data[428])<<8|uint32(data[429])) * 40.96
	ap.SS2_Q1 = float64(bigEndianToInt32(data[430:434])) / 2147483647
	ap.SS2_Q2 = float64(bigEndianToInt32(data[434:438])) / 2147483647
	ap.SS2_Q3 = float64(bigEndianToInt32(data[438:442])) / 2147483647
	ap.SS2_Q4 = float64(bigEndianToInt32(data[442:446])) / 2147483647
	ap.SS2_ExposureTime = uint8(data[446])
	ap.SS2_Threshold = uint8(data[447])
	ap.SS2_BackgroudV = uint8(data[448])
	ap.SS2_Flags = data[449]
	ap.SS2_WorkMode = uint8(data[450] >> 6)
	ap.SS2_ExtractStars = uint8(data[450] & 0x3F)
	ap.SS2_NavStars = uint8(data[451] >> 5)
	ap.SS2_Gain = uint8(data[451] & 0x1F)
	ap.SS2_RegonizedStars = uint8(data[452] >> 2)
	ap.SS2_ExtenalImage = data[452]&0x02 == 0x02
	ap.SS2_AttitudeActive = data[452]&0x01 == 0x01
	ap.SS2_ImgFrmNo = uint32(data[456])<<16 | uint32(data[457])<<8 | uint32(data[458])
	ap.FlyWheel1_Vel = float64(bigEndianToInt16(data[463:466])) * 0.1
	ap.FlyWheel1_Amps = float64(uint8(data[466])) * 0.009
	ap.FlyWheel2_Vel = float64(bigEndianToInt16(data[467:470])) * 0.1
	ap.FlyWheel2_Amps = float64(uint8(data[470])) * 0.009
	ap.FlyWheel3_Vel = float64(bigEndianToInt16(data[471:474])) * 0.1
	ap.FlyWheel3_Amps = float64(uint8(data[474])) * 0.009
	ap.FlyWheel4_Vel = float64(bigEndianToInt16(data[475:478])) * 0.1
	ap.FlyWheel4_Amps = float64(uint8(data[478])) * 0.009
	ap.MemsGyroXAV = float64(bigEndianToInt32(data[479:483])) * 0.000001
	ap.MemsGyroYAV = float64(bigEndianToInt32(data[483:487])) * 0.000001
	ap.MemsGyroZAV = float64(bigEndianToInt32(data[487:491])) * 0.000001
	ap.MagnetictrengthX = float64(bigEndianToInt32(data[491:495])) / 19200
	ap.MagnetictrengthY = float64(bigEndianToInt32(data[495:499])) / 19200
	ap.MagnetictrengthZ = float64(bigEndianToInt32(data[499:503])) / 19200
	ap.ASSTimeInt = binary.BigEndian.Uint32(data[503:507])
	ap.ASSTimeDec = uint32(data[507])<<16 | uint32(data[508])<<8 | uint32(data[509])
	ap.CheckSum = data[511]

	return nil
}

func (ap AuxPlatform) Print() {
	pp.Println(ap)
}

// Extractor 辅助数据提取器
// auxfile 辅助数据文件路径
func ParseAuxPlatform(auxfile string) ([]*AuxPlatform, error) {
	data, err := os.ReadFile(auxfile)
	if err != nil {
		log.Println("read aux data from", auxfile, "error:", err.Error())
		return nil, err
	}

	var aps []*AuxPlatform
	for i := 0; i < len(data); i += 24 + 128 + 512 {
		ap := AuxPlatform{}
		if err := ap.Decode(data[i+24+128 : i+24+128+512]); err != nil {
			return nil, err
		}

		aps = append(aps, &ap)
		// ap.Print()
		// Calculate(
		// 	Vector3{ap.J2000Pos_X, ap.J2000Pos_Y, ap.J2000Pos_Y},
		// 	Vector3{ap.J2000Vel_X, ap.J2000Vel_Y, ap.J2000Vel_Z},
		// 	Quaternion{ap.SS1_Q1, ap.SS1_Q2, ap.SS1_Q3, ap.SS1_Q4},
		// )

		pp.Println(calculator.WGS84XYZtoLatLngH(ap.W84PosX, ap.W84PosY, ap.W84PosZ))
		pp.Println(calculator.WGS84XYZtoLatLngH(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ))

		pp.Println(calculator.J2000ToWGS84(
			ap.J2000PosX,
			ap.J2000PosY,
			ap.J2000PosZ,
			time.Unix(
				int64(ap.UTCTimeSec+uint32(ReferenceTime2000)),
				int64(ap.Microsecond)*1000).UTC()))

		pp.Println(calculator.J2000ToWGS84(
			ap.J2000Pos_X,
			ap.J2000Pos_Y,
			ap.J2000Pos_Z,
			time.Unix(
				int64(ap.UTCTimeSec+uint32(ReferenceTime2000)),
				int64(ap.Microsecond)*1000).UTC()))
		break
	}
	return aps, nil
}

func Time2000UTCSec() int64 {
	t, _ := time.ParseInLocation("2006-01-02 15:04:05", "2000-01-01 12:00:00", time.UTC)
	return t.Unix()
}
