rts-sim-testing-service/message_server/sfp_ms.go

package message_server

import (
	"fmt"
	"joylink.club/bj-rtsts-server/dto/state_proto"
	"joylink.club/rtsssimulation/repository/model/proto"
	"strings"
	"time"

	"joylink.club/bj-rtsts-server/dto/data_proto"
	"joylink.club/bj-rtsts-server/message_server/ms_api"
	"joylink.club/bj-rtsts-server/mqtt"
	"joylink.club/bj-rtsts-server/ts/simulation/wayside/memory"
	"joylink.club/ecs"
	"joylink.club/rtsssimulation/component"
	"joylink.club/rtsssimulation/entity"
)

// 信号平面布置图消息服务

func NewSfpMs(vs *memory.VerifySimulation, mapId int32) ms_api.MsgTask {
	return ms_api.NewScheduleTask(fmt.Sprintf("信号平面布置图[%d]状态", mapId), func() error {
		turnoutStates, err := collectTurnoutStates(vs, mapId)
		if err != nil {
			return err
		}
		signalStates, err := collectSignalStates(vs.World, mapId)
		if err != nil {
			return err
		}
		buttonStates, err := collectStationButtonStates(vs.World, mapId)
		if err != nil {
			return err
		}
		psdStates, err := collectPsdStates(vs.World, mapId)
		if err != nil {
			return err
		}
		sectionStates, err := collectSectionStates(vs.World, mapId)
		if err != nil {
			return err
		}
		platformStates, err := collectPlatformStates(vs.World, mapId)
		if err != nil {
			return err
		}
		trainState, err := collectTrainStates(vs)
		if err != nil {
			return err
		}
		baliseStates, err := collectBaliseStates(vs.World, mapId)
		if err != nil {
			return err
		}
		ststes := &state_proto.PushedDevicesStatus{
			All: true,
			AllStatus: &state_proto.AllDevicesStatus{
				TrainState:    trainState,
				SwitchState:   turnoutStates,
				SignalState:   signalStates,
				ButtonState:   buttonStates,
				PsdState:      psdStates,
				SectionState:  sectionStates,
				PlatformState: platformStates,
				BaliseState:   baliseStates,
			},
		}
		mqtt.GetMsgClient().PubSfpState(vs.SimulationId, mapId, ststes)
		return nil
	}, 200*time.Millisecond)
}

// 收集应答器状态
func collectBaliseStates(world ecs.World, mapId int32) ([]*state_proto.BaliseState, error) {
	uidStructure := memory.QueryUidStructure[*memory.StationUidStructure](mapId)
	var transponderStates []*state_proto.BaliseState
	for id, structure := range uidStructure.TransponderIds {
		entry, ok := entity.GetEntityByUid(world, structure.Uid)
		if ok {
			baliseState := &state_proto.BaliseState{
				Id:       id,
				Telegram: component.BaliseStateType.Get(entry).ValidTelegram,
			}
			transponderStates = append(transponderStates, baliseState)
			km := component.KmType.Get(entry)
			baliseState.Km = &data_proto.KilometerSystem{
				Kilometer:        km.Value,
				CoordinateSystem: km.CoordinateSystem,
			}
			if km.Direction == proto.Direction_LEFT {
				baliseState.Km.Direction = data_proto.KilometerSystem_LEFT
			} else if km.Direction == proto.Direction_RIGHT {
				baliseState.Km.Direction = data_proto.KilometerSystem_RIGHT
			}
		}
	}
	return transponderStates, nil
}

// 收集屏蔽门状态
func collectPsdStates(world ecs.World, mapId int32) ([]*state_proto.PsdState, error) {
	uidStructure := memory.QueryUidStructure[*memory.StationUidStructure](mapId)
	data := memory.QueryGiData[*data_proto.RtssGraphicStorage](mapId)
	var psdStateArr []*state_proto.PsdState
	for _, door := range data.ScreenDoors {
		did := memory.GetMapElementId(door.Common)
		uid := uidStructure.PsdIds[did].Uid
		psdEntry, ok := entity.GetEntityByUid(world, uid)
		if ok {
			list := component.AsdListType.Get(psdEntry).List
			asdStates := make([]*state_proto.AsdState, len(list))
			for i, asdEntry := range list {
				asdState := component.AsdStateType.Get(asdEntry)
				asdStates[i] = &state_proto.AsdState{
					Code: int32(i + 1),
					Kmdw: asdState.Kmdw,
					Gmdw: asdState.Gmdw,
					Mgj:  asdState.Mgj,
				}
			}
			psdStateArr = append(psdStateArr, &state_proto.PsdState{
				Id:        did,
				AsdStates: asdStates,
				Mgj:       component.PsdStateType.Get(psdEntry).Close,
			})
		}
	}
	return psdStateArr, nil
}

// 收集区段状态
func collectSectionStates(world ecs.World, mapId int32) ([]*state_proto.SectionState, error) {
	uidMap := memory.QueryMapUidMapByType(mapId, &data_proto.Section{})
	var sectionArr []*state_proto.SectionState
	for _, u := range uidMap {
		s := handlerSectionState(world, u.Uid)
		if s == nil {
			continue
		}
		s.Id = u.CommonId
		sectionArr = append(sectionArr, s)
	}
	return sectionArr, nil
}

func handlerSectionState(w ecs.World, uid string) *state_proto.SectionState {
	entry, ok := entity.GetEntityByUid(w, uid)
	if !ok {
		//fmt.Printf("id=%s的信号机不存在", uid)
		return nil
	}
	if entry.HasComponent(component.PhysicalSectionStateType) { //计轴区段
		sectionState := &state_proto.SectionState{}
		axleState := component.PhysicalSectionStateType.Get(entry)
		sectionState.Occupied = axleState.Occ
		sectionState.AxleFault = entry.HasComponent(component.AxleSectionFaultTag)
		return sectionState
	}
	return nil
}

// 收集车站按钮状态
func collectStationButtonStates(world ecs.World, mapId int32) ([]*state_proto.ButtonState, error) {
	// 获取地图上的按钮状态
	uidMap := memory.QueryMapUidMapByType(mapId, &data_proto.EsbButton{})
	var btnStateArr []*state_proto.ButtonState
	for _, u := range uidMap {
		entry, ok := entity.GetEntityByUid(world, u.Uid)
		if !ok {
			return nil, fmt.Errorf("ESB按钮实体不存在: World id=%d, uid=%s", world.Id(), u.Uid)
		}
		if entry.HasComponent(component.ButtonTag) { // 按钮
			bit := component.BitStateType.Get(entry)
			btnStateArr = append(btnStateArr, &state_proto.ButtonState{Id: u.CommonId, Down: bit.Val})
		}
	}
	return btnStateArr, nil
}

// 收集信号机状态
func collectSignalStates(world ecs.World, mapId int32) ([]*state_proto.SignalState, error) {
	uidMap := memory.QueryMapUidMapByType(mapId, &data_proto.Signal{})
	var signalArr []*state_proto.SignalState
	for _, u := range uidMap {
		s, err := handlerSignalState(world, u.Uid)
		if err != nil {
			return nil, err
		}
		s.Id = u.CommonId
		signalArr = append(signalArr, s)
	}
	return signalArr, nil
}

func handlerSignalState(w ecs.World, uid string) (*state_proto.SignalState, error) {
	entry, ok := entity.GetEntityByUid(w, uid)
	if !ok {
		return nil, fmt.Errorf("信号机不存在: World id=%d, 信号机id=%s", w.Id(), uid)
	}
	if !entry.HasComponent(component.SignalLightsType) { //信号机灯列表
		return nil, fmt.Errorf("信号机没有SignalLights组件: World id=%d, 信号机id=%s", w.Id(), uid)
	}
	signalState := &state_proto.SignalState{}
	lights := component.SignalLightsType.Get(entry)
	isL := false
	isH := false
	isU := false
	isA := false
	isB := false
	for _, light := range lights.Lights {
		switch {
		case light.HasComponent(component.LdTag):
			isL = component.BitStateType.Get(light).Val
		case light.HasComponent(component.HdTag):
			isH = component.BitStateType.Get(light).Val
		case light.HasComponent(component.UdTag):
			isU = component.BitStateType.Get(light).Val
		case light.HasComponent(component.BdTag):
			isB = component.BitStateType.Get(light).Val
		case light.HasComponent(component.AdTag):
			isA = component.BitStateType.Get(light).Val
		}
	}
	if isH && isU {
		signalState.Aspect = state_proto.Signal_HU
	} else {
		switch {
		case isL:
			signalState.Aspect = state_proto.Signal_L
		case isH:
			signalState.Aspect = state_proto.Signal_H
		case isU:
			signalState.Aspect = state_proto.Signal_U
		case isB:
			signalState.Aspect = state_proto.Signal_B
		case isA:
			signalState.Aspect = state_proto.Signal_A
		}
	}
	return signalState, nil
}

// 收集列车状态
func collectTrainStates(vs *memory.VerifySimulation) ([]*state_proto.TrainMapState, error) {
	allTrainMap := &vs.Memory.Status.TrainStateMap
	var trainArr []*state_proto.TrainMapState
	allTrainMap.Range(func(_, v any) bool {
		trainArr = append(trainArr, convertTrainState(v.(*state_proto.TrainState)))
		return true
	})
	return trainArr, nil
}

func convertTrainState(v *state_proto.TrainState) *state_proto.TrainMapState {
	t := &state_proto.TrainMapState{
		Id:               v.Id,
		Up:               v.Up,
		InitialSpeed:     v.Speed,
		TrainLength:      v.TrainLength,
		Show:             v.Show,
		HeadDeviceId:     v.HeadDeviceId,
		HeadOffset:       v.HeadOffset,
		DevicePort:       v.DevicePort,
		PointTo:          v.PointTo,
		RunDirection:     v.RunDirection,
		HeadDirection:    v.HeadDirection,
		TrainKilometer:   v.TrainKilometer,
		ControlDelayTime: v.ControlDelayTime,
		WheelDiameter:    v.WheelDiameter,
		// 动力学
		DynamicHeartbeat:     v.DynamicState.Heartbeat,
		HeadLinkId:           v.DynamicState.HeadLinkId,
		HeadLinkOffset:       v.DynamicState.HeadLinkOffset,
		TailLinkId:           v.DynamicState.TailLinkId,
		TailLinkOffset:       v.DynamicState.TailLinkOffset,
		OccupiedLinkIndex:    v.DynamicState.OccupiedLinkIndex,
		Slope:                v.DynamicState.Slope,
		Upslope:              v.DynamicState.Upslope,
		RunningUp:            v.DynamicState.RunningUp,
		RunningResistanceSum: v.DynamicState.RunningResistanceSum,
		AirResistance:        v.DynamicState.AirResistance,
		RampResistance:       v.DynamicState.RampResistance,
		CurveResistance:      v.DynamicState.CurveResistance,
		Speed:                v.DynamicState.Speed,
		HeadSensorSpeed1:     v.DynamicState.HeadSensorSpeed1,
		HeadSensorSpeed2:     v.DynamicState.HeadSensorSpeed2,
		TailSensorSpeed1:     v.DynamicState.TailSensorSpeed1,
		TailSensorSpeed2:     v.DynamicState.TailSensorSpeed2,
		HeadRadarSpeed:       v.DynamicState.HeadRadarSpeed,
		TailRadarSpeed:       v.DynamicState.TailRadarSpeed,
		DynamicInterruption:  v.DynamicState.UdpInterruption,
		Acceleration:         v.DynamicState.Acceleration,
		// 半实物
		VobcLifeSignal:         v.VobcState.LifeSignal,
		Tc1Active:              v.VobcState.Tc1Active,
		Tc2Active:              v.VobcState.Tc2Active,
		DirectionForward:       v.VobcState.DirectionForward,
		DirectionBackward:      v.VobcState.DirectionBackward,
		TractionStatus:         v.VobcState.TractionStatus,
		BrakingStatus:          v.VobcState.BrakingStatus,
		EmergencyBrakingStatus: v.VobcState.EmergencyBrakingStatus,
		TurnbackStatus:         v.VobcState.TurnbackStatus,
		JumpStatus:             v.VobcState.JumpStatus,
		Ato:                    v.VobcState.Ato,
		Fam:                    v.VobcState.Fam,
		Cam:                    v.VobcState.Cam,
		TractionSafetyCircuit:  v.VobcState.TractionSafetyCircuit,
		ParkingBrakeStatus:     v.VobcState.ParkingBrakeStatus,
		MaintainBrakeStatus:    v.VobcState.MaintainBrakeStatus,
		TractionForce:          v.VobcState.TractionForce,
		BrakeForce:             v.VobcState.BrakeForce,
		TrainLoad:              v.VobcState.TrainLoad,
		LeftDoorOpenCommand:    v.VobcState.LeftDoorOpenCommand,
		RightDoorOpenCommand:   v.VobcState.RightDoorOpenCommand,
		LeftDoorCloseCommand:   v.VobcState.LeftDoorCloseCommand,
		RightDoorCloseCommand:  v.VobcState.RightDoorCloseCommand,
		AllDoorClose:           v.VobcState.AllDoorClose,
		VobcInterruption:       v.VobcState.UdpInterruption,
	}
	return t
}

// 收集道岔状态
func collectTurnoutStates(sim *memory.VerifySimulation, mapId int32) ([]*state_proto.SwitchState, error) {
	uidMap := memory.QueryMapUidMapByType(mapId, &data_proto.Turnout{})
	wd := entity.GetWorldData(sim.World)
	var switchArr []*state_proto.SwitchState
	for _, u := range uidMap {
		entry, ok := entity.GetEntityByUid(sim.World, u.Uid)
		if !ok {
			return nil, fmt.Errorf("道岔不存在: World id=%d,道岔id=%s", sim.World.Id(), u.Uid)
		}
		if !entry.HasComponent(component.TurnoutPositionType) {
			return nil, fmt.Errorf("道岔没有TurnoutPosition组件: World id=%d,道岔id=%s", sim.World.Id(), u.Uid)
		}
		pos := component.TurnoutPositionType.Get(entry)
		s := &state_proto.SwitchState{
			Id:      u.CommonId,
			Normal:  pos.Db,
			Reverse: pos.Fb,
			Dw:      pos.Dw,
			Fw:      pos.Fw,
		}
		// 强制(联锁驱动无效)
		s.Force = entry.HasComponent(component.TurnoutFaultCiqdType)
		// 失表
		s.Sb = entry.HasComponent(component.TurnoutFaultSbType)
		// 定位失表
		s.Dwsb = entry.HasComponent(component.TurnoutFaultDwsbType)
		// 反位失表
		s.Fwsb = entry.HasComponent(component.TurnoutFaultFwsbType)
		jc := false
		zzj := component.TurnoutZzjType.Get(entry)
		for _, e := range zzj.ZzjList {
			if e.HasComponent(component.TurnoutFaultJcType) {
				jc = true
				break
			}
		}
		// 挤岔
		s.Jc = jc
		if entry.HasComponent(component.Zdj9TwoElectronicType) {
			elec := component.Zdj9TwoElectronicType.Get(entry)
			dcj := component.BitStateType.Get(elec.TDC_DCJ)
			s.Dc = dcj.Val
			qdc, err := wd.QueryQdBit(component.UidType.Get(elec.TDC_DCJ).Id)
			if err == nil {
				s.Qdc = qdc
			}
			fcj := component.BitStateType.Get(elec.TDC_FCJ)
			s.Fc = fcj.Val
			qfc, err := wd.QueryQdBit(component.UidType.Get(elec.TDC_FCJ).Id)
			if err == nil {
				s.Qfc = qfc
			}
			ycj := component.BitStateType.Get(elec.TDC_YCJ)
			s.Yc = ycj.Val
			qyc, err := wd.QueryQdBit(component.UidType.Get(elec.TDC_YCJ).Id)
			if err == nil {
				s.Qyc = qyc
			}
		}
		// 查看道岔是否被占用
		turnout := sim.Repo.FindTurnout(u.Uid)
		if turnout == nil {
			return nil, fmt.Errorf("道岔不存在: World id=%d,道岔id=%s", sim.World.Id(), u.Uid)
		}
		if turnout.GetPhysicalSection() == nil {
			return nil, fmt.Errorf("道岔关联的物理区段不存在: World id=%d,道岔id=%s", sim.World.Id(), u.Uid)
		}
		sectionState := handlerSectionState(sim.World, turnout.GetPhysicalSection().Id())
		s.Occupied = sectionState.Occupied
		switchArr = append(switchArr, s)
	}
	return switchArr, nil
}

// 收集站台状态
func collectPlatformStates(world ecs.World, mapId int32) ([]*state_proto.PlatformState, error) {
	var states []*state_proto.PlatformState
	mapData := memory.QueryGiData[*data_proto.RtssGraphicStorage](mapId)
	uidsMap := memory.QueryUidStructure[*memory.StationUidStructure](mapId)
	platformScreenDoorMap := wrapScreenDoorToPlatform(mapData)
	for _, platform := range mapData.Platforms {
		pid := memory.GetMapElementId(platform.Common)
		stationCommonId := platform.RefStationId
		if stationCommonId == 0 {
			return nil, fmt.Errorf("站台没有绑定车站:id=%v", pid)
		}
		uidInfo := uidsMap.StationIds[stationCommonId]
		if uidInfo == nil {
			return nil, fmt.Errorf("车站实体不存在uid映射:id=%v", stationCommonId)
		}
		entry, ok := entity.GetEntityByUid(world, uidInfo.Uid)
		if !ok {
			return nil, fmt.Errorf("车站实体不存在: World id=%d, uid=%s", world.Id(), uidInfo.Uid)
		}
		sta := &state_proto.PlatformState{Id: pid}
		isX := strings.Contains(platform.Code, "下行站台")       //下行站台
		if entry.HasComponent(component.EmpElectronicType) { // 紧急停车继电器
			empElectronic := component.EmpElectronicType.Get(entry)
			deviceElectronic := empElectronic.EMPJMap[platform.GetRefEsbRelayCode()]
			if deviceElectronic != nil {
				sta.Empj = getRelayXqVal(deviceElectronic.EMPJ)
			}
		}
		if entry.HasComponent(component.SpkElectronicType) { // SPKS继电器
			spkElectronic := component.SpkElectronicType.Get(entry)
			if isX {
				sta.SpksState = append(sta.SpksState, &state_proto.ReplyState{Code: "SPKSX旁路", Xh: getRelayXqVal(spkElectronic.SPKSXPLAJ)})
				sta.SpksState = append(sta.SpksState, &state_proto.ReplyState{Code: "SPKS1", Xh: getRelayXqVal(spkElectronic.SPKSX1J)})
				sta.SpksState = append(sta.SpksState, &state_proto.ReplyState{Code: "SPKS3", Xh: getRelayXqVal(spkElectronic.SPKSX3J)})
			} else {
				sta.SpksState = append(sta.SpksState, &state_proto.ReplyState{Code: "SPKSS旁路", Xh: getRelayXqVal(spkElectronic.SPKSSPLAJ)})
				sta.SpksState = append(sta.SpksState, &state_proto.ReplyState{Code: "SPKS2", Xh: getRelayXqVal(spkElectronic.SPKSS2J)})
				sta.SpksState = append(sta.SpksState, &state_proto.ReplyState{Code: "SPKS4", Xh: getRelayXqVal(spkElectronic.SPKSS4J)})
			}
		}
		psdId := platformScreenDoorMap[pid]
		if psdId != 0 {
			psdUid, ok := uidsMap.PsdIds[psdId]
			if !ok {
				continue
			}
			psdEntry, ok := entity.GetEntityByUid(world, psdUid.Uid)
			if !ok {
				return nil, fmt.Errorf("屏蔽门实体不存在: World id=%d, uid=%s", world.Id(), psdUid.Uid)
			}
			if psdEntry.HasComponent(component.PlatformMkxCircuitType) {
				mkxCircuit := component.PlatformMkxCircuitType.Get(psdEntry)
				mkxj := &state_proto.MkxJState{Code: psdUid.Code}
				if mkxCircuit.PABJ != nil {
					mkxj.ReplyState = append(mkxj.ReplyState, &state_proto.ReplyState{Code: "站台确认继电器", Xh: getRelayXqVal(mkxCircuit.PABJ)})
				}
				if mkxCircuit.PCBJ != nil {
					mkxj.ReplyState = append(mkxj.ReplyState, &state_proto.ReplyState{Code: "站台关门继电器", Xh: getRelayXqVal(mkxCircuit.PCBJ)})
				}
				if mkxCircuit.POBJ != nil {
					mkxj.ReplyState = append(mkxj.ReplyState, &state_proto.ReplyState{Code: "站台开门继电器", Xh: getRelayXqVal(mkxCircuit.POBJ)})
				}
				sta.MkxJState = mkxj
			}

		}
		states = append(states, sta)
	}
	return states, nil
}

// 将屏蔽门关联到站台
func wrapScreenDoorToPlatform(mapData *data_proto.RtssGraphicStorage) map[uint32]uint32 {
	platformMap := make(map[uint32]uint32, len(mapData.Platforms))
	for _, s := range mapData.ScreenDoors {
		platformMap[s.RefPlatformId] = memory.GetMapElementId(s.Common)
	}
	return platformMap
}

// 获取继电器吸起状态
func getRelayXqVal(entry *ecs.Entry) bool {
	relay := component.BitStateType.Get(entry)
	return relay.Val
}