rts-sim-module/sys/device_sys/balise_detection.go

package device_sys

import (
	"fmt"
	"joylink.club/ecs"
	"joylink.club/rtsssimulation/component"
	"joylink.club/rtsssimulation/entity"
	"joylink.club/rtsssimulation/repository"
	"log/slog"
	"sort"
	"strings"
)

// BaliseDetectSystem 列车应答器天线探测轨旁应答器
// 一条应答器报文长830bits;单个应答器对应多条报文；
// 固定应答器对应1条报文；默认报文对应1条；
type BaliseDetectSystem struct {
}

/////////////////////////////////////////////////////////////////

func NewBaliseDetection() *BaliseDetectSystem {
	return &BaliseDetectSystem{}
}

// DetectBalise 列车应答器天线探测应答器
func (s *BaliseDetectSystem) DetectBalise(w ecs.World, trainEntry *ecs.Entry, last *TrainHeadPosition, cur *TrainHeadPosition) {
	wd := entity.GetWorldData(w)
	btm := component.TrainBtmType.Get(trainEntry)
	if btm.PowerAmplifierSwitch { //车载应答器天线功率放大器开启
		ranges := s.headMoveRanges(wd, last, cur)
		var tbts []*component.TrainBaliseTelegram
		aboveBalise := false
		for _, r := range ranges {
			tt := s.findBalisesInRange(wd, r)
			if !aboveBalise {
				aboveBalise = len(tt) > 0
			}
			for _, detectedBalise := range tt {
				//列车应答器天线扫描到应答器,获取应答器报文发送给BTM
				tbt := s.findBaliseTelegram(wd, r.up, detectedBalise)
				if tbt != nil {
					tbts = append(tbts, tbt)
				}
			}
		}
		if len(tbts) <= 0 {
			btm.Scanning(aboveBalise, nil)
		} else {
			for _, tbt := range tbts {
				btm.Scanning(true, tbt)
			}
		}
	}
}

// 获取车头移动范围内的应答器
func (s *BaliseDetectSystem) findBalisesInRange(wd *component.WorldData, r *scanRange) []*repository.Transponder {
	var tt []*repository.Transponder
	balises := wd.Repo.ResponderListByLink(r.linkId)
	if r.up {
		sort.SliceStable(balises, func(i, j int) bool {
			return balises[i].LinkPosition().Offset() < balises[j].LinkPosition().Offset()
		})
	} else {
		sort.SliceStable(balises, func(i, j int) bool {
			return balises[j].LinkPosition().Offset() < balises[i].LinkPosition().Offset()
		})
	}
	for _, b := range balises {
		if r.contains(b.LinkPosition()) {
			tt = append(tt, b)
		}
	}
	return tt
}

// 获取列车车头移动范围
func (s *BaliseDetectSystem) headMoveRanges(wd *component.WorldData, last *TrainHeadPosition, cur *TrainHeadPosition) []*scanRange {
	lastLink := wd.Repo.FindLink(last.HeadLink)
	curLink := wd.Repo.FindLink(cur.HeadLink)
	var rr []*scanRange
	if last.HeadLink == cur.HeadLink {
		r := newScanRange(cur.Up, curLink.Id(), last.HeadLinkOffset, cur.HeadLinkOffset)
		r.format()
		rr = append(rr, r)
	} else {
		if len(strings.TrimSpace(last.HeadLink)) > 0 { //last 与 cur 不同的道岔连接的轨道
			if last.Up {
				r := newScanRange(last.Up, lastLink.Id(), last.HeadLinkOffset, lastLink.Length())
				rr = append(rr, r)
			} else {
				r := newScanRange(last.Up, lastLink.Id(), 0, last.HeadLinkOffset)
				rr = append(rr, r)
			}
			if cur.Up {
				r := newScanRange(cur.Up, curLink.Id(), 0, cur.HeadLinkOffset)
				rr = append(rr, r)
			} else {
				r := newScanRange(cur.Up, curLink.Id(), cur.HeadLinkOffset, curLink.Length())
				rr = append(rr, r)
			}
		} else { //last不存在
			if cur.Up { //列车运行方向a->b
				if cur.HeadLinkOffset >= scanWidth {
					return []*scanRange{newScanRange(cur.Up, curLink.Id(), cur.HeadLinkOffset-scanWidth, cur.HeadLinkOffset)}
				} else {
					return []*scanRange{newScanRange(cur.Up, curLink.Id(), 0, cur.HeadLinkOffset)}
				}
			} else { //列车运行方向b->a
				if curLink.Length()-cur.HeadLinkOffset >= scanWidth {
					return []*scanRange{newScanRange(cur.Up, curLink.Id(), cur.HeadLinkOffset, cur.HeadLinkOffset+scanWidth)}
				} else {
					return []*scanRange{newScanRange(cur.Up, curLink.Id(), cur.HeadLinkOffset, curLink.Length())}
				}
			}
		}
	}
	//
	return rr
}

// 获取应答器激活的有效报文
func (s *BaliseDetectSystem) findBaliseTelegram(wd *component.WorldData, up bool, detectedBalise *repository.Transponder) *component.TrainBaliseTelegram {
	if detectedBalise == nil {
		return nil
	}
	//
	baliseEntry := wd.EntityMap[detectedBalise.Id()]
	if baliseEntry != nil {
		baliseState := component.BaliseStateType.Get(baliseEntry)
		//测试,设置应答器默认报文
		baliseState.ValidTelegram = []byte{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}
		//
		if len(baliseState.ValidTelegram) > 0 {
			return component.NewTrainBaliseTelegram(up, detectedBalise.Id(), baliseState.ValidTelegram)
		}
	} else {
		slog.Warn(fmt.Sprintf("应答器[%s]实体不存在", detectedBalise.Id()))
	}
	//
	return nil
}

const scanWidth = int64(1000) //1000mm

type scanRange struct {
	up          bool //该范围内车的运行方向
	linkId      string
	startOffset int64
	endOffset   int64
}

func newScanRange(up bool, linkId string, start int64, end int64) *scanRange {
	return &scanRange{up: up, linkId: linkId, startOffset: start, endOffset: end}
}
func (s *scanRange) format() {
	if s.startOffset > s.endOffset {
		s.startOffset, s.endOffset = s.endOffset, s.startOffset
	}
}
func (s *scanRange) str() string {
	sb := strings.Builder{}
	sb.WriteString("LinkRange[")
	sb.WriteString("linkId:")
	sb.WriteString(s.linkId)
	sb.WriteString(" start:")
	sb.WriteString(fmt.Sprintf("%d", s.startOffset))
	sb.WriteString(" end:")
	sb.WriteString(fmt.Sprintf("%d", s.endOffset))
	sb.WriteString("]")
	return sb.String()
}

// true-应答器balisePosition在该scanRange内
func (s *scanRange) contains(balisePosition *repository.LinkPosition) bool {
	if s.linkId != balisePosition.Link().Id() {
		return false
	}
	s.format()
	return balisePosition.Offset() >= s.startOffset && balisePosition.Offset() <= s.endOffset
}

type TrainHeadPosition struct {
	// 列车运行方向
	Up bool
	//列车所在轨道link
	HeadLink string
	//列车所在link偏移量（mm）
	HeadLinkOffset int64
}