rts-sim-testing-service/ats/verify/simulation/simulation_manage.go

package simulation

import (
	"encoding/binary"
	"encoding/hex"
	"joylink.club/rtsssimulation/repository"
	"joylink.club/rtsssimulation/repository/model/proto"
	"math"
	"time"

	"fmt"
	"net/http"
	"strconv"
	"sync"

	"go.uber.org/zap"
	"joylink.club/bj-rtsts-server/ats/verify/protos/state"
	"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/memory"
	"joylink.club/bj-rtsts-server/config"
	"joylink.club/bj-rtsts-server/dynamics"
	"joylink.club/bj-rtsts-server/vobc"

	"joylink.club/bj-rtsts-server/dto"
)

func init() {
	// vobc 发来的列车信息
	vobc.RegisterTrainInfoHandler(func(info []byte) {
		zap.S().Info("接收到vobc发送的列车消息", hex.EncodeToString(info))
		memory.UdpUpdateTime.VobcTime = time.Now().UnixMilli()
		for _, simulation := range GetSimulationArr() {
			simulation.Memory.Status.TrainStateMap.Range(func(_, value any) bool {
				train := value.(*state.TrainState)
				if !train.Show { // 下线列车
					return false
				}
				// 拼接列车ID
				trainId, err := strconv.Atoi(train.Id)
				if err != nil {
					panic(dto.ErrorDto{Code: dto.ArgumentParseError, Message: err.Error()})
				}
				trainInfo := decoderVobcTrainState(info)
				zap.S().Info("解析后vobc列车消息", trainInfo)
				d := append(info, uint8(trainId))
				// 发送给动力学
				dynamics.SendDynamicsTrainMsg(d)
				zap.S().Info("发送后：", hex.EncodeToString(d))
				// 存放至列车中
				train.VobcState = trainInfo
				return true
			})
		}
	})
	dynamics.RegisterTrainInfoHandler(func(info *dynamics.TrainInfo) {
		zap.S().Info("发送到vobc的列车编号为", info.Number)
		memory.UdpUpdateTime.DynamicsTime = time.Now().UnixMilli()
		for _, simulation := range GetSimulationArr() {
			sta, ok := simulation.Memory.Status.TrainStateMap.Load(strconv.Itoa(int(info.Number)))
			if !ok {
				continue
			}
			trainState := sta.(*state.TrainState)
			// 给半实物仿真发送速度
			zap.S().Infof("发送到vobc发送的速度%d, %d \n", info.Speed, math.Abs(float64(info.Speed*36)))
			vobc.SendTrainSpeedTask(math.Abs(float64(info.Speed * 36)))
			// 更新列车状态
			memory.UpdateTrainState(simulation, convert(info, trainState, simulation))
		}
	})
}

// 仿真存储集合
var simulationMap sync.Map

// 创建前检查
func IsExistSimulation() bool {
	i := 0
	simulationMap.Range(func(_, _ any) bool {
		i++
		return true
	})
	return i > 0
}

// 创建仿真对象
func CreateSimulation(projectId int32, mapIds []int32) string {
	simulationId := createSimulationId(projectId)
	_, e := simulationMap.Load(simulationId)
	if !e && IsExistSimulation() {
		panic(dto.ErrorDto{Code: dto.DataAlreadyExist, Message: "已有仿真在运行"})
	}
	if !e {
		verifySimulation, err := memory.CreateSimulation(projectId, mapIds)
		if err != nil {
			panic(fmt.Sprintf("创建仿真失败:%s", err.Error()))
		}
		verifySimulation.SimulationId = simulationId
		//通知动力学
		lineBaseInfo := buildLineBaseInfo(verifySimulation.Repo)
		httpCode, _, err := dynamics.SendSimulationStartReq(lineBaseInfo)
		if httpCode != http.StatusOK || err != nil {
			panic(dto.ErrorDto{Code: dto.DynamicsError, Message: fmt.Sprintf("动力学接口调用失败：[%d][%s]", httpCode, err)})
		}

		simulationMap.Store(simulationId, verifySimulation)
		dynamicsRun(verifySimulation)
	}
	return simulationId

	//simulationId := createSimulationId(projectId)
	//_, e := simulationMap.Load(simulationId)
	//if !e && IsExistSimulation() {
	//	panic(dto.ErrorDto{Code: dto.DataAlreadyExist, Message: "已有仿真在运行"})
	//}
	//if !e {
	//	verifySimulation := memory.CreateSimulation(projectId, mapIds)
	//	verifySimulation.SimulationId = simulationId
	//	lineDataInfo := &dynamics.LineBaseInfo{}
	//	for _, mapId := range mapIds {
	//		b := buildLineBaseInfo(memory.QueryMapVerifyStructure(mapId))
	//		lineDataInfo.CurveList = append(lineDataInfo.CurveList, b.CurveList...)
	//		lineDataInfo.LinkList = append(lineDataInfo.LinkList, b.LinkList...)
	//		lineDataInfo.SlopeList = append(lineDataInfo.SlopeList, b.SlopeList...)
	//	}
	//	//通知动力学
	//	httpCode, _, err := dynamics.SendSimulationStartReq(lineDataInfo)
	//	if httpCode != http.StatusOK || err != nil {
	//		panic(dto.ErrorDto{Code: dto.DynamicsError, Message: fmt.Sprintf("动力学接口调用失败：[%d][%s]", httpCode, err)})
	//	}
	//	simulationMap.Store(simulationId, verifySimulation)
	//	dynamicsRun(verifySimulation)
	//}
	//return simulationId
}

// 删除仿真对象
func DestroySimulation(simulationId string) {
	simulationMap.Delete(simulationId)
	//移除道岔状态发送
	dynamics.Stop()
	//通知动力学
	httpCode, _, err := dynamics.SendSimulationEndReq()
	if httpCode != http.StatusOK || err != nil {
		panic(dto.ErrorDto{Code: dto.DynamicsError, Message: fmt.Sprintf("动力学接口调用失败：[%d][%s]", httpCode, err)})
	}
}

func createSimulationId(projectId int32) string {
	// 当前服务器IP + 端口 + 项目
	return config.SimulationId_prefix + "_" + strconv.Itoa(config.Config.Server.Port) + "_" + strconv.Itoa(int(projectId))
}

// 获取仿真列表
func ListAllSimulations() []*dto.SimulationInfoRspDto {
	var simArr []*dto.SimulationInfoRspDto
	simulationMap.Range(func(_, v any) bool {
		s := v.(*memory.VerifySimulation)
		simArr = append(simArr, &dto.SimulationInfoRspDto{
			SimulationId: s.SimulationId,
			MapId:        s.MapIds[0],
			MapIds:       s.MapIds,
			ProjectId:    s.ProjectId,
		})
		return true
	})
	return simArr
}

// 根据仿真id查找仿真实例
func FindSimulation(simulationId string) *memory.VerifySimulation {
	m, e := simulationMap.Load(simulationId)
	if e {
		return m.(*memory.VerifySimulation)
	}
	return nil
}

// 获取普通仿真数组
func GetSimulationArr() []*memory.VerifySimulation {
	var result []*memory.VerifySimulation
	simulationMap.Range(func(_, v any) bool {
		result = append(result, v.(*memory.VerifySimulation))
		return true
	})
	return result
}

func convert(info *dynamics.TrainInfo, sta *state.TrainState, simulation *memory.VerifySimulation) *state.TrainState {
	//var vs *memory.VerifyStructure
	//linkId := int32(info.Link)
	//for _, mapId := range simulation.MapIds {
	//	vm := memory.QueryMapVerifyStructure(mapId)
	//	if vm.LinkModelMap[linkId] == nil {
	//		continue
	//	}
	//	vs = vm
	//	break
	//}
	//zap.S().Debugf("原始消息：[%d-%d-%d]", info.Number, info.Link, info.LinkOffset)
	//id, port, offset, runDirection, pointTo, kilometer := memory.QueryDeviceByCalcLink(vs, linkId, int64(info.LinkOffset), info.Up, sta.RunDirection)
	//zap.S().Debugf("转换后的消息：[%d-车头:%s-偏移:%d-上行:%v-是否ab:%v]", info.Number, id, offset, runDirection, pointTo)
	//sta.HeadDeviceId = id
	//sta.DevicePort = port
	//sta.HeadOffset = offset
	//sta.PointTo = pointTo
	//sta.TrainKilometer = kilometer
	//sta.RunDirection = runDirection
	////判定车头方向
	//sta.HeadDirection = runDirection
	//if sta.VobcState != nil {
	//	if sta.VobcState.DirectionForward {
	//		sta.HeadDirection = runDirection
	//	} else if sta.VobcState.DirectionBackward {
	//		sta.HeadDirection = !runDirection
	//	}
	//}
	//if info.Speed < 0 {
	//	sta.RunDirection = !sta.RunDirection
	//}
	//// 赋值动力学信息
	//sta.DynamicState.Heartbeat = int32(info.LifeSignal)
	//sta.DynamicState.HeadLinkId = strconv.Itoa(int(info.Link))
	//sta.DynamicState.HeadLinkOffset = int64(info.LinkOffset)
	//sta.DynamicState.Slope = int32(info.Slope)
	//sta.DynamicState.Upslope = info.UpSlope
	//sta.DynamicState.RunningUp = info.Up
	//sta.DynamicState.RunningResistanceSum = float32(info.TotalResistance) / 1000
	//sta.DynamicState.AirResistance = float32(info.AirResistance) / 1000
	//sta.DynamicState.RampResistance = float32(info.SlopeResistance) / 1000
	//sta.DynamicState.CurveResistance = float32(info.CurveResistance) / 1000
	//sta.DynamicState.Speed = speedParse(info.Speed)
	//sta.DynamicState.HeadSensorSpeed1 = speedParse(info.HeadSpeed1)
	//sta.DynamicState.HeadSensorSpeed2 = speedParse(info.HeadSpeed2)
	//sta.DynamicState.TailSensorSpeed1 = speedParse(info.TailSpeed1)
	//sta.DynamicState.TailSensorSpeed2 = speedParse(info.TailSpeed2)
	//sta.DynamicState.HeadRadarSpeed = speedParse(info.HeadRadarSpeed)
	//sta.DynamicState.TailRadarSpeed = speedParse(info.TailRadarSpeed)
	return sta
}

func dynamicsRun(verifySimulation *memory.VerifySimulation) {
	_ = dynamics.Run(func() []*dynamics.TurnoutInfo {
		stateSlice := memory.GetAllTurnoutState(verifySimulation)
		var turnoutInfoSlice []*dynamics.TurnoutInfo
		for _, sta := range stateSlice {
			code64, err := strconv.ParseUint(sta.Id, 10, 16)
			if err != nil {
				zap.S().Error("id转uint16报错", err)
			}
			info := dynamics.TurnoutInfo{
				Code:      uint16(code64),
				NPosition: sta.Normal,
				RPosition: sta.Reverse,
			}
			turnoutInfoSlice = append(turnoutInfoSlice, &info)
		}
		return turnoutInfoSlice
	})
}

func buildLineBaseInfo(repo *repository.Repository) *dynamics.LineBaseInfo {
	info := &dynamics.LineBaseInfo{}
	for _, model := range repo.LinkList() {
		id, _ := strconv.Atoi(model.Id())
		link := &dynamics.Link{
			ID:  int32(id),
			Len: int32(model.Length()),
		}
		info.LinkList = append(info.LinkList, link)
		if model.ARelation() != nil {
			turnoutId, _ := strconv.Atoi(model.ARelation().Device().Id())
			link.ARelTurnoutId = int32(turnoutId)
			switch model.ARelation().Port() {
			case proto.Port_A:
				link.ARelTurnoutPoint = "A"
			case proto.Port_B:
				link.ARelTurnoutPoint = "B"
			case proto.Port_C:
				link.ARelTurnoutPoint = "C"
			}
		}
		if model.BRelation() != nil {
			turnoutId, _ := strconv.Atoi(model.BRelation().Device().Id())
			link.BRelTurnoutId = int32(turnoutId)
			switch model.BRelation().Port() {
			case proto.Port_A:
				link.BRelTurnoutPoint = "A"
			case proto.Port_B:
				link.BRelTurnoutPoint = "B"
			case proto.Port_C:
				link.BRelTurnoutPoint = "C"
			}
		}
	}
	for _, model := range repo.SlopeList() {
		id, _ := strconv.Atoi(model.Id())
		slope := &dynamics.Slope{
			ID:              int32(id),
			StartLinkOffset: int32(model.StartLinkPosition().Offset()),
			EndLinkOffset:   int32(model.EndLinkPosition().Offset()),
			DegreeTrig:      model.Degree(),
		}
		info.SlopeList = append(info.SlopeList, slope)
		startLinkId, _ := strconv.Atoi(model.StartLinkPosition().Link().Id())
		slope.StartLinkId = int32(startLinkId)
		endLinkId, _ := strconv.Atoi(model.EndLinkPosition().Link().Id())
		slope.EndLinkId = int32(endLinkId)
	}
	for _, model := range repo.SectionalCurvatureList() {
		id, _ := strconv.Atoi(model.Id())
		curve := &dynamics.Curve{
			ID:              int32(id),
			StartLinkOffset: int32(model.StartLinkPosition().Offset()),
			EndLinkOffset:   int32(model.EndLinkPosition().Offset()),
			Curvature:       model.Radius(),
		}
		info.CurveList = append(info.CurveList, curve)
		startLinkId, _ := strconv.Atoi(model.StartLinkPosition().Link().Id())
		curve.StartLinkId = int32(startLinkId)
		endLinkId, _ := strconv.Atoi(model.EndLinkPosition().Link().Id())
		curve.EndLinkId = int32(endLinkId)
	}
	return info
}

//func buildLineBaseInfo(vs *memory.VerifyStructure) *dynamics.LineBaseInfo {
//	var links []*dynamics.Link
//	var slopes []*dynamics.Slope
//	var curves []*dynamics.Curve
//	for _, link := range vs.LinkModelMap {
//		id, _ := strconv.Atoi(link.Index)
//		var aTurnoutId int
//		var aPort string
//		if link.ARelatedSwitchRef != nil && link.ARelatedSwitchRef.SwitchDevice != nil {
//			aTurnoutId, _ = strconv.Atoi(link.ARelatedSwitchRef.SwitchDevice.GetIndex())
//			aPort = link.ARelatedSwitchRef.Port.Name()
//		}
//		var bTurnoutId int
//		var bPort string
//		if link.BRelatedSwitchRef != nil && link.BRelatedSwitchRef.SwitchDevice != nil {
//			bTurnoutId, _ = strconv.Atoi(link.BRelatedSwitchRef.SwitchDevice.GetIndex())
//			bPort = link.BRelatedSwitchRef.Port.Name()
//		}
//		links = append(links, &dynamics.Link{
//			ID:               int32(id),
//			Len:              link.Length,
//			ARelTurnoutId:    int32(aTurnoutId),
//			ARelTurnoutPoint: aPort,
//			BRelTurnoutId:    int32(bTurnoutId),
//			BRelTurnoutPoint: bPort,
//		})
//	}
//	for _, slope := range vs.SlopeModelMap {
//		id, _ := strconv.Atoi(slope.Index)
//		slopes = append(slopes, &dynamics.Slope{
//			ID:              int32(id),
//			StartLinkId:     slope.StartLinkIndex,
//			StartLinkOffset: slope.StartLinkOffset,
//			EndLinkId:       slope.EndLinkIndex,
//			EndLinkOffset:   slope.EndLinkOffset,
//			DegreeTrig:      slope.DegreeTrig,
//		})
//	}
//	for _, curve := range vs.CurveModelMap {
//		id, _ := strconv.Atoi(curve.Index)
//		curves = append(curves, &dynamics.Curve{
//			ID:              int32(id),
//			StartLinkId:     curve.StartLinkIndex,
//			StartLinkOffset: curve.StartLinkOffset,
//			EndLinkId:       curve.EndLinkIndex,
//			EndLinkOffset:   curve.EndLinkOffset,
//			Curvature:       curve.Curvature,
//		})
//	}
//	return &dynamics.LineBaseInfo{
//		LinkList:  links,
//		SlopeList: slopes,
//		CurveList: curves,
//	}
//}

// 解析VOBC列车信息
func decoderVobcTrainState(buf []byte) *state.TrainVobcState {
	trainVobcInfo := &state.TrainVobcState{}
	trainVobcInfo.LifeSignal = int32(binary.BigEndian.Uint16(buf[0:2]))
	b2 := buf[2]
	trainVobcInfo.Tc1Active = (b2 & 1) != 0
	trainVobcInfo.Tc2Active = (b2 & (1 << 1)) != 0
	trainVobcInfo.DirectionForward = (b2 & (1 << 2)) != 0
	trainVobcInfo.DirectionBackward = (b2 & (1 << 3)) != 0
	trainVobcInfo.TractionStatus = (b2 & (1 << 4)) != 0
	trainVobcInfo.BrakingStatus = (b2 & (1 << 5)) != 0
	trainVobcInfo.EmergencyBrakingStatus = (b2 & (1 << 6)) != 0
	trainVobcInfo.TurnbackStatus = (b2 & 7) != 0
	b3 := buf[3]
	trainVobcInfo.JumpStatus = (b3 & 1) != 0
	trainVobcInfo.Ato = (b3 & (1 << 1)) != 0
	trainVobcInfo.Fam = (b3 & (1 << 2)) != 0
	trainVobcInfo.Cam = (b3 & (1 << 3)) != 0
	trainVobcInfo.TractionSafetyCircuit = (b3 & (1 << 4)) != 0
	trainVobcInfo.ParkingBrakeStatus = (b3 & (1 << 5)) != 0
	trainVobcInfo.MaintainBrakeStatus = (b3 & (1 << 6)) != 0
	trainVobcInfo.TractionForce = int64(binary.BigEndian.Uint16(buf[4:6]))
	trainVobcInfo.BrakeForce = int64(binary.BigEndian.Uint16(buf[6:8]))
	trainVobcInfo.TrainLoad = int64(binary.BigEndian.Uint16(buf[8:10]))
	b4 := buf[15]
	trainVobcInfo.LeftDoorOpenCommand = (b4 & 1) != 0
	trainVobcInfo.RightDoorOpenCommand = (b4 & (1 << 1)) != 0
	trainVobcInfo.LeftDoorCloseCommand = (b4 & (1 << 2)) != 0
	trainVobcInfo.RightDoorCloseCommand = (b4 & (1 << 3)) != 0
	trainVobcInfo.AllDoorClose = (b4 & (1 << 4)) != 0
	return trainVobcInfo
}

// 发送给前端的速度格式化
func speedParse(speed float32) int32 {
	return int32(math.Abs(float64(speed * 3.6 * 100)))
}