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

package memory

import (
	"fmt"
	"sort"
	"strings"
	"sync"

	"joylink.club/rtsssimulation/component"
	"joylink.club/rtsssimulation/entity"
	"joylink.club/rtsssimulation/repository"
	proto2 "joylink.club/rtsssimulation/repository/model/proto"

	"google.golang.org/protobuf/proto"
	"joylink.club/bj-rtsts-server/dto"
	"joylink.club/bj-rtsts-server/dto/data_proto"
	"joylink.club/bj-rtsts-server/service"
	"joylink.club/bj-rtsts-server/sys_error"
)

var giPublishData sync.Map

type giStoreData struct {
	name    string
	giType  data_proto.PictureType
	message proto.Message
	version int32
}

// 将发布的地图数据放入内存中
func PublishMapVerifyStructure(graphic *dto.PublishedDto) {
	var message proto.Message
	switch data_proto.PictureType(graphic.Type) {
	case data_proto.PictureType_StationLayout:
		message = &data_proto.RtssGraphicStorage{}
	case data_proto.PictureType_RelayCabinetLayout:
		message = &data_proto.RelayCabinetGraphicStorage{}
	case data_proto.PictureType_Psl:
		message = &data_proto.PslGraphicStorage{}
	case data_proto.PictureType_IBP:
		message = &data_proto.IBPGraphicStorage{}
	case data_proto.PictureType_TrainControlCab:
		message = &data_proto.TccGraphicStorage{}
	}

	err := proto.Unmarshal(graphic.Proto, message)
	if err != nil {
		panic(&dto.ErrorDto{Code: dto.LogicError, Message: fmt.Sprintf("[id:%d]proto数据反序列化失败:%s", graphic.ID, err)})
	}
	// 缓存数据
	giPublishData.Store(graphic.ID, &giStoreData{
		name:    graphic.Name,
		giType:  data_proto.PictureType(graphic.Type),
		message: message,
		version: graphic.Version,
	})
	// 初始化地图结构
	switch data_proto.PictureType(graphic.Type) {
	case data_proto.PictureType_StationLayout:
		graphicStorage := message.(*data_proto.RtssGraphicStorage)
		filterOtherLineDevice(graphicStorage) // 处理掉其他线路的设备
		giUidMap.Store(graphic.ID, initStationUid(graphicStorage))
	case data_proto.PictureType_RelayCabinetLayout:
		graphicStorage := message.(*data_proto.RelayCabinetGraphicStorage)
		giUidMap.Store(graphic.ID, initRelayCabinetUid(graphicStorage))
	case data_proto.PictureType_IBP:
		graphicStorage := message.(*data_proto.IBPGraphicStorage)
		giUidMap.Store(graphic.ID, initIBPUid(graphicStorage))
		/*	case data_proto.PictureType_TrainControlCab:
			graphicStorage := message.(*data_proto.TccGraphicStorage)
			giUidMap.Store(graphic.ID, initTccUid(graphicStorage))*/
	}

}

func GenerateElementUid(city, lineId string, stationIndexList []string, code string) string {
	sort.Strings(stationIndexList)
	var idArr []string
	idArr = append(idArr, city, lineId)
	idArr = append(idArr, stationIndexList...)
	idArr = append(idArr, code)
	return BuildUid(idArr...)
}

// GenerateElementUid 构建uid。将args用'_'连接
func BuildUid(args ...string) string {
	return strings.Join(args, "_")
}

// 移除内存中的地图信息
func DeleteMapVerifyStructure(mapId int32) {
	giPublishData.Delete(mapId)
	giUidMap.Delete(mapId)
}

// 获取地图类型
func QueryGiType(mapId int32) data_proto.PictureType {
	value, ok := giPublishData.Load(mapId)
	if !ok {
		graphic := service.GetPublishedById(mapId)
		PublishMapVerifyStructure(graphic)
		return data_proto.PictureType(graphic.Type)
	}
	d, ok := value.(*giStoreData)
	if !ok {
		panic(sys_error.New(fmt.Sprintf("地图【%d】缓存数据结构错误", mapId)))
	}
	return d.giType
}

// 获取地图版本
func QueryGiVersion(mapId int32) int32 {
	value, ok := giPublishData.Load(mapId)
	if !ok {
		panic(sys_error.New(fmt.Sprintf("地图【%d】缓存数据丢失", mapId)))
	}
	d, ok := value.(*giStoreData)
	if !ok {
		panic(sys_error.New(fmt.Sprintf("地图【%d】缓存数据结构错误", mapId)))
	}
	return d.version
}

// 根据地图ID获取地图数据
func QueryGiData[T proto.Message](mapId int32) T {
	value, ok := giPublishData.Load(mapId)
	if !ok {
		panic(sys_error.New(fmt.Sprintf("地图【%d】缓存数据丢失", mapId)))
	}
	d, ok := value.(*giStoreData)
	if !ok {
		panic(sys_error.New(fmt.Sprintf("地图【%d】缓存数据结构错误", mapId)))
	}
	return d.message.(T)
}

// 根据地图名称获取地图数据
func QueryGiDataByName[T proto.Message](name string) (int32, T) {
	var (
		id     int32
		giData T
	)
	giPublishData.Range(func(key, value any) bool {
		d, _ := value.(*giStoreData)
		if d.name == name {
			id = key.(int32)
			giData = d.message.(T)
			return false
		}
		return true
	})
	return id, giData
}

// 转换成统一坐标公里标
func convertRepoBaseKm(r *repository.Repository, km *proto2.Kilometer) *proto2.Kilometer {
	k, err := r.ConvertKilometer(km, r.GetCoordinateInfo().Coordinate)
	if err != nil {
		panic(sys_error.New(fmt.Sprintf("公里标转换【%s->%s】错误", km.CoordinateSystem, r.GetCoordinateInfo().Coordinate)))
	}
	return k
}

// 根据传入link、偏移、link运行方向，查找所在设备信息
// 入参：仿真、linkId、link偏移量、link运行方向
// 输出：linkId、设备Id、设备端口、link偏移量、设备上偏移量、偏移量对应的公里标
func CalcInitializeLink(sim *VerifySimulation, linkId string, offset int64, up bool) (
	outLinkId, deviceId, port string, outLinkOffset, deviceOffset int64, km *proto2.Kilometer, err error) {
	link := sim.Repo.FindLink(linkId)
	if link == nil {
		err = sys_error.New(fmt.Sprintf("未找到link【%s】", linkId))
	}
	// 获取计算link与所在偏移
	outLinkId, outLinkOffset, err = findCalcLinkIdAndOffset(sim, link, offset)
	if err != nil {
		return
	}
	calcLink := sim.Repo.FindLink(outLinkId)
	// 判断是否在道岔上
	onTurnout, isA := isOnLinkTurnout(calcLink, outLinkOffset)
	if onTurnout {
		deviceId, port, deviceOffset, km = calcTurnoutOffset(sim.Repo, calcLink, isA, outLinkOffset, up)
	} else {
		deviceId, port, deviceOffset, km = calcSectionOffset(sim.Repo, calcLink, outLinkOffset, up)
	}
	// 输出当前所在设备、端口、设备偏移
	return
}

// 列车是否在link的道岔上
func isOnLinkTurnout(link *repository.Link, offset int64) (bool, bool) {
	aTp := link.ARelation()
	var turnoutOffset int64
	if aTp != nil {
		turnoutOffset = aTp.Turnout().FindLinkPositionByPort(aTp.Port()).Offset()
	}
	if offset <= turnoutOffset {
		return true, true
	}
	bTp := link.BRelation()
	if bTp != nil {
		turnoutOffset = bTp.Turnout().FindLinkPositionByPort(bTp.Port()).Offset()
		return offset >= turnoutOffset, false
	}
	return false, false
}

// 查找最终的linkId与link偏移
// 入参：仿真、link、link偏移量
// 输出：linkId、link偏移量
func findCalcLinkIdAndOffset(sim *VerifySimulation, link *repository.Link, offset int64) (outId string, outOffset int64, err error) {
	if 0 <= offset && offset <= link.Length() {
		outId, outOffset, err = link.Id(), offset, nil
		return
	}
	// 超出端、超出长度
	tp, length := link.ARelation(), 0-offset
	if offset > 0 {
		tp, length = link.BRelation(), offset-link.Length()
	}
	if tp == nil {
		err = sys_error.New("列车偏移超出link位置")
		return
	}
	nextPort, er1 := getTurnoutNextPort(sim, tp.Device().Id(), tp.Port().String())
	if er1 != nil {
		err = er1
		return
	}
	var nextLink *repository.Link
	var isB bool
	// 寻找匹配到的link
	for _, l := range sim.Repo.LinkList() {
		if l.ARelation() != nil && l.ARelation().Port() == nextPort && l.ARelation().Device().Id() == tp.Device().Id() {
			nextLink = l
			break
		}
		if l.BRelation() != nil && l.BRelation().Port() == nextPort && l.BRelation().Device().Id() == tp.Device().Id() {
			nextLink = l
			isB = true
			break
		}
	}
	// 没有找到连接信息，说明已经到尽头找不到位置
	if nextLink == nil {
		err = sys_error.New(fmt.Sprintf("未找到对应的link信息, linkId=%s, offset=%d", link.Id(), offset))
		return
	}
	// 下个link偏移
	nextOffset := length
	if isB {
		nextOffset = nextLink.Length() - length
	}
	outId, outOffset, err = findCalcLinkIdAndOffset(sim, nextLink, nextOffset)
	return
}

// 根据当前link端，寻找下一个link连接端端口
// 入参：仿真、道岔端口
// 输出：道岔端口
func getTurnoutNextPort(sim *VerifySimulation, turnoutId string, port string) (nextPort proto2.Port, err error) {
	entry, ok := entity.GetEntityByUid(sim.World, turnoutId)
	if !ok {
		err = sys_error.New(fmt.Sprintf("道岔不存在: World id=%d,道岔id=%s", sim.World.Id(), turnoutId))
		return
	}
	if !entry.HasComponent(component.TurnoutPositionType) {
		err = sys_error.New(fmt.Sprintf("道岔没有TurnoutPosition组件: World id=%d,道岔id=%s", sim.World.Id(), turnoutId))
		return
	}
	// 获取定反数据
	pos := component.TurnoutPositionType.Get(entry)
	switch port {
	case "A":
		if pos.Dw {
			nextPort = proto2.Port_B
		} else {
			nextPort = proto2.Port_C
		}
		return
	case "B":
		if pos.Dw {
			nextPort = proto2.Port_A
		} else {
			nextPort = proto2.Port_None
		}
		return
	case "C":
		if pos.Dw {
			nextPort = proto2.Port_None
		} else {
			nextPort = proto2.Port_A
		}
		return
	}
	err = sys_error.New(fmt.Sprintf("非法端口:端口=%s", port))
	return
}

// 计算link offset 在道岔上的位置
// 入参：仿真Repository、link、是否从A端开始、link偏移量、link运行方向
// 输出：设备Id、设备所在端口、设备偏移量、公里标信息（地图主坐标系）
func calcTurnoutOffset(repo *repository.Repository, link *repository.Link, isA bool, offset int64, up bool) (
	deviceId, port string, deviceOffset int64, km *proto2.Kilometer) {
	tp := link.ARelation()
	if !isA {
		tp = link.BRelation()
	}
	// 设备ID
	deviceId = tp.Turnout().Id()
	// 端口信息
	port = tp.Port().String()
	// 设备上的偏移量
	if isA {
		deviceOffset = offset
	} else {
		deviceOffset = link.Length() - offset
	}
	// 公里标计算
	crossKmInfo := convertRepoBaseKm(repo, tp.Turnout().GetTurnoutKm(proto2.Port_None))
	portKmInfo := convertRepoBaseKm(repo, tp.Turnout().GetTurnoutKm(tp.Port()))
	km = &proto2.Kilometer{CoordinateSystem: crossKmInfo.CoordinateSystem}
	if crossKmInfo.Value > portKmInfo.Value {
		km.Value = crossKmInfo.Value - deviceOffset
	} else {
		km.Value = crossKmInfo.Value + deviceOffset
	}
	return
}

// 计算link offset 在区段上的位置
// 入参：仿真Repository、link、link偏移量、link运行方向
// 输出：设备Id、设备所在端口、设备偏移量、公里标信息（地图主坐标系）
func calcSectionOffset(repo *repository.Repository, link *repository.Link, offset int64, up bool) (
	deviceId, port string, deviceOffset int64, km *proto2.Kilometer) {
	var section *repository.PhysicalSection
	for _, s := range link.PhysicalSections() {
		ao, bo := s.ALinkPosition().Offset(), s.BLinkPosition().Offset()
		if (ao < offset && offset <= bo) || (bo < offset && offset <= ao) {
			section = s
			break
		}
	}
	if section == nil {
		panic(sys_error.New(fmt.Sprintf("未找到link设备偏移【%d】", offset)))
	}
	// 元素ID
	deviceId = section.Id()
	// a点偏移 大于 b点偏移
	ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset()
	if ao > bo {
		deviceOffset = ao - offset
	} else {
		deviceOffset = offset - ao
	}
	// a点公里标 大于 b点公里标
	ak, bk := convertRepoBaseKm(repo, section.AKilometer()), convertRepoBaseKm(repo, section.BKilometer())
	km = &proto2.Kilometer{CoordinateSystem: ak.CoordinateSystem}
	if ak.Value > bk.Value {
		km.Value = ak.Value - deviceOffset
	} else {
		km.Value = ak.Value + deviceOffset
	}
	return
}

// 根据设备查找link
// 入参：设备ID、端口、设备偏移
// 输出：linkId、link偏移
func QueryLinkAndOffsetByDevice(repo *repository.Repository, uid, port string, offset int64) (linkId string, linkOffset int64) {
	if port == "" {
		linkId, linkOffset = sectionToLink(repo, uid, offset)
	} else {
		linkId, linkOffset = turnoutToLink(repo, uid, port, offset)
	}
	return
}

// 根据区段设备查找link
// 入参：仿真Repository、设备ID、设备偏移
// 输出：linkId、link偏移、公里标
func sectionToLink(repo *repository.Repository, uid string, offset int64) (string, int64) {
	section := repo.FindPhysicalSection(uid)
	if section == nil {
		panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", uid)))
	}
	ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset()
	if ao < bo {
		return section.ALinkPosition().Link().Id(), ao + offset
	} else {
		return section.ALinkPosition().Link().Id(), ao - offset
	}
}

// 根据道岔设备查找link
// 入参：仿真Repository、道岔设备ID、道岔端口、设备偏移
// 输出：linkId、link偏移
func turnoutToLink(repo *repository.Repository, uid, port string, offset int64) (string, int64) {
	turnout := repo.FindTurnout(uid)
	if turnout == nil {
		panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", uid)))
	}
	var portPosition *repository.LinkPosition
	switch port {
	case "A":
		portPosition = turnout.FindLinkPositionByPort(proto2.Port_A)
	case "B":
		portPosition = turnout.FindLinkPositionByPort(proto2.Port_B)
	case "C":
		portPosition = turnout.FindLinkPositionByPort(proto2.Port_C)
	default:
		panic(sys_error.New(fmt.Sprintf("无效端口【%s】偏移量", port)))
	}
	// 关联link
	link := portPosition.Link()
	isStart := link.ARelation().Device().Id() == uid
	if isStart {
		return link.Id(), offset
	} else {
		return link.Id(), link.Length() - offset
	}
}

// 根据设备信息、上下行，获取link运行方向，ab指向
// 入参：仿真Repository、uid、端口、上下行
// 输出：link运行方向、设备上的指向
func QueryUpAndABByDevice(repo *repository.Repository, uid, port string, runDirection bool) (up, ab bool) {
	if port == "" {
		up, ab = runDirectionSectionToUpAndAB(repo, uid, runDirection)
	} else {
		up, ab = runDirectionTurnoutToUpAndAB(repo, uid, port, runDirection)
	}
	return
}

// 根据区段及上下行，获取link运行方向，ab指向
// 入参：仿真Repository、区段UID、上下行
// 输出：link运行方向、设备上的指向
func runDirectionSectionToUpAndAB(repo *repository.Repository, id string, runDirection bool) (up, ab bool) {
	section := repo.FindPhysicalSection(id)
	if section == nil {
		panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", id)))
	}
	ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset()
	// 是否从A到B，统一坐标
	ak, bk := convertRepoBaseKm(repo, section.AKilometer()), convertRepoBaseKm(repo, section.BKilometer())
	ab = (runDirection == (ak.Value <= bk.Value))
	up = (ab == (ao < bo))
	return
}

// 根据区段及上下行，获取link运行方向，ab指向
// 入参：仿真Repository、道岔UID、道岔端口、上下行
// 输出：link运行方向，设备上的指向
func runDirectionTurnoutToUpAndAB(repo *repository.Repository, id, port string, runDirection bool) (up, ab bool) {
	turnout := repo.FindTurnout(id)
	if turnout == nil {
		panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", id)))
	}
	var portKm *proto2.Kilometer
	var portPosition *repository.LinkPosition
	switch port {
	case "A":
		portKm = convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_A))
		portPosition = turnout.FindLinkPositionByPort(proto2.Port_A)
	case "B":
		portKm = convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_B))
		portPosition = turnout.FindLinkPositionByPort(proto2.Port_B)
	case "C":
		portKm = convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_C))
		portPosition = turnout.FindLinkPositionByPort(proto2.Port_C)
	default:
		panic(sys_error.New(fmt.Sprintf("无效端口【%s】偏移量", port)))
	}
	// 岔心公里标
	crossKm := convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_None))
	// 是否是link的起始断点
	isStart := (portPosition.Link().ARelation() != nil && portPosition.Link().ARelation().Device().Id() == id)
	up = runDirection
	if (portKm.Value > crossKm.Value) != isStart {
		up = !runDirection
	}
	ab = (portKm.Value > crossKm.Value) == runDirection
	return
}

// 根据设备以及link运行方向，获取上下行、设备上的运行指向
// 入参：仿真Repository、道岔UID、道岔端口、上下行
// 输出：link运行方向，设备上的指向
func QueryDirectionAndABByDevice(repo *repository.Repository, uid, port string, up bool) (runDirection, ab bool) {
	if port == "" {
		runDirection, ab = upSectionToDirectionAndAB(repo, uid, up)
	} else {
		runDirection, ab = upTurnoutToDirectionAndAB(repo, uid, port, up)
	}
	return
}

// 根据区段以及link运行方向，获取上下行、设备上的运行指向
// 入参：仿真Repository、道岔UID、道岔端口、上下行
// 输出：link运行方向，设备上的指向
func upSectionToDirectionAndAB(repo *repository.Repository, uid string, up bool) (runDirection, ab bool) {
	section := repo.FindPhysicalSection(uid)
	if section == nil {
		panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", uid)))
	}
	// a点偏移、b点偏移
	ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset()
	// a点公里标、b点公里标
	ak, bk := convertRepoBaseKm(repo, section.AKilometer()), convertRepoBaseKm(repo, section.BKilometer())
	if up {
		runDirection = ((ao < bo) == (ak.Value < bk.Value))
	} else {
		runDirection = ((ao > bo) == (ak.Value < bk.Value))
	}
	ab = (runDirection == (ak.Value < bk.Value))
	return
}

// 根据道岔以及link运行方向，获取上下行、设备上的运行指向
// 入参：仿真Repository、道岔UID、道岔端口、上下行
// 输出：link运行方向，设备上的指向
func upTurnoutToDirectionAndAB(repo *repository.Repository, uid, port string, up bool) (runDirection, ab bool) {
	turnout := repo.FindTurnout(uid)
	if turnout == nil {
		panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", uid)))
	}
	var port_enum proto2.Port
	switch port {
	case "A":
		port_enum = proto2.Port_A
	case "B":
		port_enum = proto2.Port_B
	case "C":
		port_enum = proto2.Port_C
	default:
		panic(sys_error.New(fmt.Sprintf("无效端口【%s】偏移量", port)))
	}
	tpo := turnout.FindLinkPositionByPort(port_enum)
	isA := tpo.Link().ARelation() != nil && tpo.Link().ARelation().Device().Id() == uid
	crossKmInfo := convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_None))
	portKmInfo := convertRepoBaseKm(repo, turnout.GetTurnoutKm(port_enum))
	if isA {
		ab = !up
		runDirection = (up == (crossKmInfo.Value < portKmInfo.Value))
	} else {
		ab = up
		runDirection = (up == (crossKmInfo.Value > portKmInfo.Value))
	}
	return
}

// 根据设备获取列车公里标
// 入参：仿真Repository、UID、端口、上下行、设备偏移
// 输出：公里标信息
func CalcTrainKilometer(repo *repository.Repository, uid, port string, runDirection bool, offset int64) (
	km *proto2.Kilometer) {
	if port == "" {
		km = sectionOffsetToKilometer(repo, uid, runDirection, offset)
	} else {
		km = turnoutOffsetToKilometer(repo, uid, runDirection, offset)
	}
	return
}

// 获取区段的上公里标
// 入参：仿真Repository、UID、上下行、设备偏移
// 输出：公里标信息
func sectionOffsetToKilometer(repo *repository.Repository, uid string, runDirection bool, offset int64) (km *proto2.Kilometer) {
	section := repo.FindPhysicalSection(uid)
	if section == nil {
		panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", uid)))
	}
	km = convertRepoBaseKm(repo, section.AKilometer())
	if runDirection {
		km.Value = km.Value + offset
	} else {
		km.Value = km.Value - offset
	}
	return
}

// 获取道岔的公里标
// 入参：仿真Repository、UID、上下行、设备偏移
// 输出：公里标信息
func turnoutOffsetToKilometer(repo *repository.Repository, uid string, runDirection bool, offset int64) (km *proto2.Kilometer) {
	turnout := repo.FindTurnout(uid)
	if turnout == nil {
		panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", uid)))
	}
	// 岔心公里标
	km = convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_None))
	if runDirection {
		km.Value = km.Value + offset
	} else {
		km.Value = km.Value - offset
	}
	return
}