package main

import (
	"cicv-data-closedloop/pjisuv_msgs"
	"fmt"
	"math"
	"sync"
)

var (
	Framenum             int64 = 0
	YawofCicvLocation    any
	ObjDicOfTpperception any
	objDic               = make(map[uint32][][]float32)
	yaw                  float64
	objId                uint32
	obj                  [][]float32
	ok1                  bool
	ok2                  bool
	ObjectSlice          = make(map[uint32][][]float32)
)

func isRetrograde(ObjectList [][]float32) bool {
	for i := 0; i < len(ObjectList[0]); i++ {
		xi := ObjectList[0][i]
		yi := math.Abs(float64(ObjectList[1][i]))
		diff_hi := ObjectList[7][i]
		Type := ObjectList[6][0]

		if xi >= 20 && yi <= 1.2 && diff_hi > 150 && Type != 1.0 {
			for j := 0; j < len(ObjectList[0])-i-1; j++ {
				xj := ObjectList[0][j]
				yj := math.Abs(float64(ObjectList[1][j]))
				diff_hj := ObjectList[7][j]

				if xj <= 8 && yj <= 1.2 && diff_hj > 150 {
					return true
				}
			}
		}
	}
	return false
}

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

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

func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	YawofCicvLocation, ok1 = shareVars.Load("YawOfCicvLocation")
	if !ok1 {
		return ""
	}
	yaw = YawofCicvLocation.(float64)
	Framenum += 1
	ObjDicOfTpperception, ok2 = shareVars.Load("ObjDicOfTpperception")
	if !ok2 {
		return ""
	}
	objDic = ObjDicOfTpperception.(map[uint32][][]float32)
	for _, objValue := range objDic {
		if len(objValue[0]) <= 10 || !isRetrograde(objValue) {
			continue
		}
		return Label()
	}
	return ""
}