// diff.go
package diff

import (
	"fmt"
	"math"
	"strconv"
	"strings"
)

type KalmanVarDiff struct {
	Kf *KalmanFilter

	StartDiff float64
	MinDiff   float64
	MaxDiff   float64

	TargetInterval float64

	MhsEst  float64
	DiffEst float64
}

type KalmanFilter struct {
	X float64 // 估计的状态
	P float64 // 状态协方差
	F float64 // 状态转移矩阵
	H float64 // 观测矩阵
	Q float64 // 过程噪声协方差
	R float64 // 观测噪声协方差
	K float64 // 卡尔曼增益
}

func NewKalmanFilter(init_mhs float64, init_state_p float64) *KalmanFilter {
	x := init_mhs
	p := init_state_p
	f := 1.0
	h := 1.0
	q := 0.01
	r := init_mhs * 3
	//r := 1.0
	return &KalmanFilter{
		X: x,
		P: p,
		F: f,
		H: h,
		Q: q,
		R: r,
	}
	/*
		return &KalmanFilter{
			X: 1.0,
			P: 1.0,
			F: 1.0,
			H: 1.0,
			Q: 0.1,
			R: 1.0,
		}*/
}

func (kf *KalmanFilter) Update(measurement float64) (float64, float64) {
	kf.R = measurement * 2

	// 预测
	p := kf.X*kf.Q + kf.P

	// 计算卡尔曼增益
	kf.K = p / (p + kf.R + 1)

	// 更新状态估计
	if measurement >= kf.X {
		kf.X = kf.X + kf.K*(measurement-kf.X)
	} else {
		kf.X = kf.X - kf.K*(kf.X-measurement)
	}

	// 更新协方差矩阵
	kf.P = (1 - kf.K) * p

	// 自适应调整过程噪声和观测噪声
	//kf.adapt()

	return kf.X, kf.P
}

func (kf *KalmanFilter) adapt() {
	// 自适应调整参数
	if kf.P > 10.0 {
		kf.Q *= 1.1 // 增加过程噪声
	} else {
		kf.Q *= 0.9 // 减少过程噪声
	}
	if kf.K > 0.5 {
		kf.R *= 1.1 // 增加观测噪声
	} else {
		kf.R *= 0.9 // 减少观测噪声
	}
}

func (kd *KalmanVarDiff) Init(startDiff float64, minDiff float64, maxDiff float64, targetTime float64) {
	kd.StartDiff = startDiff
	kd.MinDiff = minDiff
	kd.MaxDiff = maxDiff
	kd.TargetInterval = targetTime
	kd.DiffEst = startDiff
	kd.MhsEst = startDiff / targetTime
	kd.Kf = NewKalmanFilter(startDiff/targetTime, 1.0)
}

func (kd *KalmanVarDiff) DeInit() {

}

// 提取科学计数法有效数字（整数部分和一位小数）及指数，并合并为新的浮点数
func extractAndCombine(num float64) float64 {
	// 将浮点数格式化为科学计数法
	scientificStr := fmt.Sprintf("%.10e", num)

	// 分离小数部分和指数部分
	parts := strings.Split(scientificStr, "e")
	if len(parts) != 2 {
		fmt.Println("Error: unexpected scientific notation format")
		return 0
	}

	// 处理小数部分
	decimalPart := parts[0]
	exponentPart := parts[1]

	// 去除小数部分前的 "0."
	decimalPart = strings.TrimPrefix(decimalPart, "0.")

	// 提取整数部分和一位小数
	decimalParts := strings.Split(decimalPart, ".")
	if len(decimalParts) < 2 {
		decimalPart = decimalParts[0] + ".0" // 没有小数部分时，添加 ".0"
	} else {
		decimalPart = decimalParts[0] + "." + decimalParts[1][:1] // 只取一位小数
		//decimalPart = decimalParts[0]
	}

	// 将指数部分转换为整数
	exponent, err := strconv.Atoi(exponentPart)
	if err != nil {
		fmt.Println("Error parsing exponent:", err)
		return 0
	}

	// 计算新的浮点数
	newNumber := (func() float64 {
		digit, err := strconv.ParseFloat(decimalPart, 64)
		if err != nil {
			fmt.Println("Error parsing decimal part:", err)
			return 0
		}
		return digit * math.Pow(10, float64(exponent))
	})()

	return newNumber
}

func (kd *KalmanVarDiff) Handler(diff float64, interval float64) (float64, float64) {
	//newx, newp := kd.Kf.Update(kd.DiffEst / interval)
	newx, newp := kd.Kf.Update(diff / interval)
	kd.MhsEst = newx
	newdiff := newx * kd.TargetInterval
	kd.DiffEst = newdiff
	newdiff2 := extractAndCombine(newdiff)
	return newdiff2, newp
}