rts-sim-testing-service/ts/simulation/wayside/memory/wayside_memory_train.go

package memory

import (
	"fmt"
	"log/slog"
	"math"
	"strconv"
	"time"

	"joylink.club/bj-rtsts-server/dto"
	"joylink.club/bj-rtsts-server/sys_error"
	"joylink.club/bj-rtsts-server/third_party/dynamics"
	"joylink.club/bj-rtsts-server/third_party/message"
	"joylink.club/rtsssimulation/fi"

	"joylink.club/bj-rtsts-server/ts/protos/graphicData"
	"joylink.club/bj-rtsts-server/ts/protos/state"
)

// 增加列车状态
func AddTrainState(vs *VerifySimulation, status *state.TrainState, mapId int32) {
	allTrainMap := &vs.Memory.Status.TrainStateMap
	_, ok := allTrainMap.Load(status.Id)
	if ok {
		panic(fmt.Sprintf("列车【%s】已存在", status.Id))
	}
	// 显示状态
	status.Show = true
	//向动力学发送初始化请求
	trainIndex, _ := strconv.ParseUint(status.Id, 10, 16)
	slog.Debug("添加列车", "trainIndex", trainIndex, "HeadDeviceId", status.HeadDeviceId, "HeadOffset", status.HeadOffset)
	// 映射link、偏移量、运行方向
	var uid string
	if status.DevicePort == "" {
		uid = QueryUidByMidAndComId(mapId, status.HeadDeviceId, &graphicData.Section{})
	} else {
		uid = QueryUidByMidAndComId(mapId, status.HeadDeviceId, &graphicData.Turnout{})
	}
	// 车头所在link、link上的偏移
	linkId, loffset := QueryLinkAndOffsetByDevice(vs.Repo, uid, status.DevicePort, status.HeadOffset)
	// link上的运行方向、设备上的运行方向
	up, pointTo := QueryUpAndABByDevice(vs.Repo, uid, status.DevicePort, status.RunDirection)
	// 车头所在公里标
	kilometer := CalcTrainKilometer(vs.Repo, uid, status.DevicePort, status.RunDirection, status.HeadOffset)
	// 车尾相对车头link的偏移量
	calctailOffset := calcTrailTailOffset(loffset, status.TrainLength, up)
	// 车尾位置
	tailLink, _, _, tailLOffset, _, _, e1 := CalcInitializeLink(vs, linkId, calctailOffset, up)
	if e1 != nil {
		panic(sys_error.New("添加列车失败,列车车尾占用位置计算出错", e1))
	}
	status.Up = up
	status.PointTo = pointTo
	status.TrainKilometer = kilometer.Value
	status.DynamicState = &state.TrainDynamicState{
		HeadLinkId:     linkId,
		HeadLinkOffset: loffset,
		TailLinkId:     tailLink,
		TailLinkOffset: tailLOffset,
		RunningUp:      up,
	}
	status.VobcState = &state.TrainVobcState{}
	slog.Debug("列车初始化", "trainIndex", trainIndex, "linkId", linkId, "loffset", loffset)
	linkIdInt, _ := strconv.Atoi(linkId)
	err := dynamics.Default().RequestAddTrain(&message.InitTrainInfo{
		TrainIndex:  uint16(trainIndex),
		LinkIndex:   uint16(linkIdInt),
		LinkOffset:  uint32(loffset),
		Speed:       uint16(math.Round(float64(status.Speed * 10))),
		Up:          status.Up,
		TrainLength: uint32(status.TrainLength),
	})
	if err != nil {
		panic(dto.ErrorDto{Code: dto.DynamicsError, Message: err.Error()})
	}
	// world中加车
	fi.AddTrainToWorld(vs.World, status.Id)
	fi.UpdateTrainPositionFromDynamics(vs.World, fi.TrainPositionInfo{
		TrainId:        status.Id,
		Up:             status.Up,
		Len:            uint32(status.TrainLength),
		HeadLink:       linkId,
		HeadLinkOffset: uint32(loffset),
		TailLink:       tailLink,
		TailLinkOffset: uint32(tailLOffset),
	})
	// 将信息合并到当前设备状态中
	allTrainMap.Store(status.Id, status)
}

// 修改列车信息
func UpdateTrainInfo(vs *VerifySimulation, status *state.TrainState) {
	allTrainMap := &vs.Memory.Status.TrainStateMap
	data, ok := allTrainMap.Load(status.Id)
	if !ok {
		panic(sys_error.New(fmt.Sprintf("列车【%s】不存在", status.Id)))
	}
	sta := data.(*state.TrainState)
	sta.TrainLength = status.TrainLength
	sta.WheelDiameter = status.WheelDiameter
}

// 根据动力学发来的信息修改列车状态
func UpdateTrainStateByDynamics(vs *VerifySimulation, trainId string, info *message.DynamicsTrainInfo) {
	data, ok := vs.Memory.Status.TrainStateMap.Load(trainId)
	if !ok {
		panic(sys_error.New(fmt.Sprintf("动力学传输数据:列车【%s】不存在", trainId)))
	}
	sta := data.(*state.TrainState)
	delayTime := time.Now().UnixMilli() - sta.VobcState.UpdateTime
	sta.ControlDelayTime = (int64(sta.VobcState.LifeSignal)-int64(info.VobcLifeSignal))*20 + delayTime
	slog.Debug("收到动力学原始消息", "Number", info.Number, "Link", info.Link, "LinkOffset", info.LinkOffset)
	inLinkId, inLinkOffset := strconv.Itoa(int(info.Link)), int64(info.LinkOffset)
	outLinkId, id, port, outLinkOffset, offset, kilometer, e1 := CalcInitializeLink(vs, inLinkId, inLinkOffset, info.Up)
	if e1 != nil {
		panic(sys_error.New("动力学传输数据:列车车头位置计算出错", e1))
	}
	runDirection, pointTo := QueryDirectionAndABByDevice(vs.Repo, id, port, info.Up)
	slog.Debug("处理动力学转换后的消息", "number", info.Number, "车头位置", id, "偏移", offset, "是否上行", runDirection, "是否ab", pointTo)
	// 车尾相对车头link的偏移量
	calctailOffset := calcTrailTailOffset(outLinkOffset, int64(info.Len), info.Up)
	tailLinkId, tailDeviceId, tailDevicePort, tailLinkOffset, tailDeviceOffset, _, e2 := CalcInitializeLink(vs, outLinkId, calctailOffset, info.Up)
	if e2 != nil {
		panic(sys_error.New("动力学传输数据:列车车尾位置计算出错", e2))
	}
	slog.Debug("车尾位置", tailDeviceId, "偏移", tailDeviceOffset, "所在设备端", tailDevicePort)
	// 修改world中的列车位置
	fi.UpdateTrainPositionFromDynamics(vs.World, fi.TrainPositionInfo{
		TrainId:        trainId,
		Up:             info.Up,
		Len:            info.Len,
		HeadLink:       outLinkId,
		HeadLinkOffset: uint32(outLinkOffset),
		TailLink:       tailLinkId,
		TailLinkOffset: uint32(tailLinkOffset),
	})
	sta.HeadDeviceId = vs.GetComIdByUid(id)
	sta.DevicePort = port
	sta.HeadOffset = offset
	sta.PointTo = pointTo
	sta.TrainKilometer = kilometer.Value
	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 = outLinkId
	sta.DynamicState.HeadLinkOffset = outLinkOffset
	sta.DynamicState.TailLinkId = tailLinkId
	sta.DynamicState.TailLinkOffset = tailLinkOffset
	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)
	sta.DynamicState.Acceleration = info.Acceleration
}

// 删除列车状态
func RemoveTrainState(vs *VerifySimulation, id string) {
	allTrainMap := &vs.Memory.Status.TrainStateMap
	d, ok := allTrainMap.Load(id)
	if ok {
		t := d.(*state.TrainState)
		trainIndex, _ := strconv.ParseUint(id, 10, 16)
		err := dynamics.Default().RequestRemoveTrain(&message.RemoveTrainReq{
			TrainIndex: uint16(trainIndex),
		})
		if err != nil {
			panic(dto.ErrorDto{Code: dto.DynamicsError, Message: err.Error()})
		}
		// 从仿真内存中移除列车
		t.Show = false
		// 移除车
		fi.RemoveTrainFromWorld(vs.World, id)
		allTrainMap.Store(id, t)
	} else {
		panic(fmt.Sprintf("列车【%s】不存在", id))
	}
}

func calcTrailTailOffset(headerOffset, length int64, up bool) (calctailOffset int64) {
	if up {
		calctailOffset = headerOffset - length
	} else {
		calctailOffset = headerOffset + length
	}
	return
}