package main

import (
	pjibotGuideConfig "cicv-data-closedloop/aarch64/pjibot_guide/master/package/config"
	pjisuvConfig "cicv-data-closedloop/aarch64/pjisuv/master/config"
	"cicv-data-closedloop/common/util"
	"cicv-data-closedloop/kinglong_msgs"
	"cicv-data-closedloop/pjibot_guide_msgs"
	"cicv-data-closedloop/pjisuv_msgs"
	"fmt"
	"github.com/bluenviron/goroslib/v2"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/diagnostic_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs"
	"os"
)

// main 命令格式 topic-echo.exe pjisuv /cicv_location
func main() {
	rosNode, _ := goroslib.NewNode(goroslib.NodeConf{
		Name:          "cicvDataClosedloopTopicEcho",
		MasterAddress: "127.0.0.1:11311",
	})
	eType := util.ToString(os.Args[1])
	topic := util.ToString(os.Args[2])

	if eType == "pjibot_guide" {
		//1 一致
		if topic == pjibotGuideConfig.TopicOfDiagnostics {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *diagnostic_msgs.DiagnosticArray) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//2 一致
		if topic == pjibotGuideConfig.TopicOfImu {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.Imu) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//3 一致
		if topic == pjibotGuideConfig.TopicOfLocateInfo {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjibot_guide_msgs.LocateInfo) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//4 一致
		if topic == pjibotGuideConfig.TopicOfObstacleDetection {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *std_msgs.UInt8) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//5 一致
		if topic == pjibotGuideConfig.TopicOfOdom {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *nav_msgs.Odometry) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//6 一致
		if topic == pjibotGuideConfig.TopicOfSysInfo {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjibot_guide_msgs.SysInfo) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
	}

	if eType == "pjisuv" {
		//1 一致
		if topic == pjisuvConfig.TopicOfAmrPose {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *visualization_msgs.MarkerArray) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//2 一致
		if topic == pjisuvConfig.TopicOfBoundingBoxesFast {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.BoundingBoxArray) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//3 一致
		if topic == pjisuvConfig.TopicOfCameraFault {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.FaultVec) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//4 一致
		// history 消息不一致
		if topic == pjisuvConfig.TopicOfCanData {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.Frame) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//5 一致
		if topic == pjisuvConfig.TopicOfCh128x1LslidarPointCloud {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.PointCloud2) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//6 一致
		if topic == pjisuvConfig.TopicOfCh64wLLslidarPointCloud {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.PointCloud2) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//7 一致
		if topic == pjisuvConfig.TopicOfCh64wLScan {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.LaserScan) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//8 一致
		if topic == pjisuvConfig.TopicOfCh64wRLslidarPointCloud {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.PointCloud2) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//9 一致
		if topic == pjisuvConfig.TopicOfCh64wRScan {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.LaserScan) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//10 一致
		if topic == pjisuvConfig.TopicOfCicvLidarclusterMovingObjects {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.PerceptionCicvMovingObjects) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//11 一致
		// history 没有数据发出（未开启自动驾驶）
		if topic == pjisuvConfig.TopicOfCicvAmrTrajectory {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.Trajectory) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//12 一致
		if topic == pjisuvConfig.TopicOfCicvLocation {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
					fmt.Println("收到话题", topic, "的数据", data)
					fmt.Println("当前 poxitionX 为", data.PositionX, "，positionY 为", data.PositionY)
				}})
		}
		//13 一致
		// history 消息不一致
		if topic == pjisuvConfig.TopicOfCloudClusters {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.AutowareCloudClusterArray) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//14 一致
		if topic == pjisuvConfig.TopicOfHeartbeatInfo {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.HeartBeatInfo) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//15 一致
		if topic == pjisuvConfig.TopicOfLidarPretreatmentCost {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *geometry_msgs.Vector3Stamped) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//16 一致
		if topic == pjisuvConfig.TopicOfLidarPretreatmentOdometry {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *nav_msgs.Odometry) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//17 一致
		if topic == pjisuvConfig.TopicOfLidarRoi {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *geometry_msgs.PolygonStamped) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//18 一致
		if topic == pjisuvConfig.TopicOfLine1 {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *nav_msgs.Path) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//19 一致
		if topic == pjisuvConfig.TopicOfLine2 {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *nav_msgs.Path) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//20 一致
		if topic == pjisuvConfig.TopicOfMapPolygon {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.PolygonStamped) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//21 一致
		if topic == pjisuvConfig.TopicOfObstacleDisplay {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *visualization_msgs.MarkerArray) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//22 一致
		if topic == pjisuvConfig.TopicOfPjControlPub {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.CommonVehicleCmd) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//23 一致
		if topic == pjisuvConfig.TopicOfPointsCluster {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.PointCloud2) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//24 一致
		if topic == pjisuvConfig.TopicOfPointsConcat {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.PointCloud2) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//25 一致
		// history 没有数据发出（未开启自动驾驶）
		if topic == pjisuvConfig.TopicOfReferenceDisplay {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *nav_msgs.Path) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//26 一致
		// history 没有数据发出（未开启自动驾驶）
		if topic == pjisuvConfig.TopicOfReferenceTrajectory {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.Trajectory) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//27 一致
		// history 没有数据发出（未开启自动驾驶）
		if topic == pjisuvConfig.TopicOfRoiPoints {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.PointCloud2) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//28 一致
		if topic == pjisuvConfig.TopicOfRoiPolygon {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *nav_msgs.Path) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//29 一致
		if topic == pjisuvConfig.TopicOfTf {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *tf2_msgs.TFMessage) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//30 一致
		if topic == pjisuvConfig.TopicOfTpperception {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.PerceptionObjects) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//31 一致
		// history 没有数据发出（未开启自动驾驶）
		if topic == pjisuvConfig.TopicOfTpperceptionVis {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *visualization_msgs.MarkerArray) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//32 一致
		if topic == pjisuvConfig.TopicOfTprouteplan {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.RoutePlan) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//33 一致
		// history 没有数据发出（未开启自动驾驶）
		if topic == pjisuvConfig.TopicOfTrajectoryDisplay {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *nav_msgs.Path) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//34 一致
		if topic == pjisuvConfig.TopicOfUngroundCloudpoints {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.PointCloud2) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//35 一致
		if topic == pjisuvConfig.TopicOfCameraImage {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *sensor_msgs.Image) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//36 仿真平台部孙亚伦开发的底盘数据读取程序
		if topic == pjisuvConfig.TopicOfDataRead {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.Retrieval) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//37
		if topic == pjisuvConfig.TopicOfPjiGps {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		//37
		// 自动驾驶状态 0 人工 1 自动
		if topic == pjisuvConfig.TopicOfPjVehicleFdbPub {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.VehicleFdb) {
					fmt.Println("自动驾驶状态为", data.Automode)
				}})
		}
		//37
		if topic == pjisuvConfig.TopicOfFaultInfo {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.FaultVec) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
	}

	TopicOfNodeFaultInfo := "/nodefault_info"
	TopicOfCicvLocation := "/cicv_location"
	TopicOfTpperception := "/tpperception"
	TopicOfFaultInfo := "/fault_info"
	TopicOfDataRead := "/data_read"

	if eType == "kinglong" {
		if topic == TopicOfNodeFaultInfo {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: TopicOfNodeFaultInfo,
				Callback: func(data *kinglong_msgs.FaultInfo) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		if topic == TopicOfCicvLocation {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: TopicOfCicvLocation,
				Callback: func(data *kinglong_msgs.PerceptionLocalization) {
					fmt.Println("收到话题", topic, "的数据", data)
					fmt.Println("X坐标为：", data.PositionX)
					fmt.Println("Y坐标为：", data.PositionY)
				}})
		}
		if topic == TopicOfTpperception {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: TopicOfTpperception,
				Callback: func(data *kinglong_msgs.PerceptionObjects) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		if topic == TopicOfFaultInfo {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: TopicOfFaultInfo,
				Callback: func(data *kinglong_msgs.FaultVec) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
		if topic == TopicOfDataRead {
			_, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  rosNode,
				Topic: TopicOfDataRead,
				Callback: func(data *kinglong_msgs.Retrieval) {
					fmt.Println("收到话题", topic, "的数据", data)
				}})
		}
	}

	select {}

}