package main

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

/*
def callback_cicv_location(data):
    global angular_velocity_z
    global Ego_position_x
    global Ego_position_y
    global Ego_yaw


    Ego_position_x=data.position_x
    Ego_position_y=data.position_y
    Ego_yaw=data.yaw
    #print(Ego_yaw)
    angular_velocity_z=data.angular_velocity_z
    #print(angular_velocity_z)
    if abs(angular_velocity_z)>=27:
        event_label='overswing' #横摆角速度过大
        print(event_label)

*/

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

func Label() string {
	return "ExcessiveSpeedWhenUphill"
}

var (
	count     int = 0
	threshold     = 0.05
)

func QuaternionToEuler(x, y, z, w float64) float64 {
	// 归一化四元数
	length := math.Sqrt(x*x + y*y + z*z + w*w)
	x /= length
	y /= length
	z /= length
	w /= length
	// 计算欧拉角
	pitch := math.Asin(2 * (w*y - z*x))
	return pitch
}
func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	if count%10 == 0 {

		OutsideWorkshopFlag, ok1 := shareVars.Load("OutsideWorkshopFlag")
		if ok1 && OutsideWorkshopFlag.(bool) {
			if math.Abs(data.AngularVelocityZ) >= 17.0 && QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw) >= threshold {
				count = 1
				return Label()
			}
		}
	}
	count++
	return ""
}