package main

import (
	"cicv-data-closedloop/common/entity"
	"math"
	"sort"
)

func Topic() string {
	return "/cicv_extend"
}

// Label todo 禁止存在下划线_
func Label() string {
	return "cutout_difference"
}

func Rule(param entity.PjisuvParam) string {
	for _, objValue := range param.ObjDicOfTpperception {
		if len(objValue) <= 5 || !isCuttingOut(objValue, param) || !isDifference(param) {
			continue
		}
		return "cutout_difference"
	}
	return ""
}

func isCuttingOut(objYList []float32, param entity.PjisuvParam) bool {
	for i, objY := range objYList {
		if math.Abs(float64(objY)) <= 0.6 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
			for j := 0; j < len(objYList)-i-1; j++ {
				if math.Abs(float64(objYList[1+i+j])) >= 1.2 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
					return true
				}
			}
		}
	}
	return false
}

// 利用曼惠特尼U检验的方式判断车端数据和驾驶员控制数据是否存在显著差异
func isDifference(param entity.PjisuvParam) bool {
	_, pValueSteering := mannWhitneyUTest(param.EgoSteeringRealOfDataRead, param.EgoSteeringCmdOfPjControlPub)
	_, pValueThrottle := mannWhitneyUTest(param.EgoThrottleRealOfDataRead, param.EgoThrottleCmdOfPjControlPub)
	if pValueSteering < 0.05 || pValueThrottle < 0.05 {
		return true
	}
	return false
}

func mannWhitneyUTest(group1, group2 []float64) (float64, float64) {
	// 统计样本大小
	n1 := float64(len(group1))
	n2 := float64(len(group2))

	// 合并样本并排序
	combined := append(group1, group2...)
	sort.Float64s(combined)

	// 计算秩和
	var rankSum1, rankSum2 float64
	for i := range combined {
		if i < len(group1) {
			rankSum1 += float64(i + 1)
		} else {
			rankSum2 += float64(i + 1)
		}
	}

	// 计算 U 统计量
	u1 := rankSum1 - (n1 * (n1 + 1) / 2)
	u2 := rankSum2 - (n2 * (n2 + 1) / 2)
	u := math.Min(u1, u2)

	// 计算 p 值
	meanU := (n1 * n2 / 2)
	stdDevU := math.Sqrt((n1 * n2 * (n1 + n2 + 1)) / 12)
	z := (u - meanU) / stdDevU
	p := 2 * (1 - normalCDF(z))

	return u, p
}

func normalCDF(x float64) float64 {
	const (
		b1 = 0.319381530
		b2 = -0.356563782
		b3 = 1.781477937
		b4 = -1.821255978
		b5 = 1.330274429
		p  = 0.2316419
		c  = 0.39894228
	)
	if x >= 0.0 {
		t := 1.0 / (1.0 + p*x)
		return 1.0 - c*t*(t*(t*(t*(t*b5+b4)+b3)+b2)+b1)
	}
	t := 1.0 / (1.0 - p*x)
	return c * t * (t*(t*(t*(t*b5+b4)+b3)+b2) + b1)
}
