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

package device_sys

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

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

func NewBaliseDetectSystem() *BaliseDetectSystem {
	return &BaliseDetectSystem{
		trainQuery: ecs.NewQuery(filter.Contains(component.UidType, component.TrainPositionInfoType, component.TrainBtmType)),
	}
}

var (
	lastHeadLink   string
	lastHeadOffset int64
)

func (s *BaliseDetectSystem) Update(w ecs.World) {
	//所有列车
	//列车速度80KM/H时，222mm/10ms
	s.trainQuery.Each(w, func(entry *ecs.Entry) {
		s.DetectBalise(w, entry)
	})
}
func NewBaliseDetection() *BaliseDetectSystem {
	return &BaliseDetectSystem{}
}

// DetectBalise 列车应答器天线探测应答器
func (s *BaliseDetectSystem) DetectBalise(w ecs.World, trainEntry *ecs.Entry) {
	wd := entity.GetWorldData(w)
	btm := component.TrainBtmType.Get(trainEntry)
	if btm.PowerAmplifierSwitch { //车载应答器天线功率放大器开启
		tp := component.TrainPositionInfoType.Get(trainEntry)
		//测试用
		if tp.HeadLink == lastHeadLink {
			dm := math.Abs(float64(tp.HeadLinkOffset-lastHeadOffset) / 1000)
			if dm >= 2 {
				slog.Debug(fmt.Sprintf("车头移动当前帧与上一帧移动的差值 : %f (m)", dm))
			}
		}
		balises := wd.Repo.ResponderListByLink(tp.HeadLink)
		detectedBalise := s.detect(wd, tp, balises)
		//列车应答器天线扫描到应答器,获取应答器报文发送给BTM
		tbt := s.findBaliseTelegram(wd, detectedBalise)
		var detectedBaliseId string
		if detectedBalise != nil {
			detectedBaliseId = detectedBalise.Id()
		}
		btm.Scanning(detectedBalise != nil, detectedBaliseId, tbt)
		//测试用
		lastHeadLink = tp.HeadLink
		lastHeadOffset = tp.HeadLinkOffset
	}
}

// 获取应答器激活的有效报文
func (s *BaliseDetectSystem) findBaliseTelegram(wd *component.WorldData, 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(detectedBalise.Id(), baliseState.ValidTelegram)
		}
	} else {
		slog.Warn(fmt.Sprintf("应答器[%s]实体不存在", detectedBalise.Id()))
	}
	//
	return nil
}
func (s *BaliseDetectSystem) detect(wd *component.WorldData, tp *component.TrainPositionInfo, balises []*repository.Transponder) *repository.Transponder {
	scanRange := s.calculateScanRange(wd, tp)
	if tp.Up {
		sort.SliceStable(balises, func(i, j int) bool {
			return balises[j].LinkPosition().Offset() < balises[i].LinkPosition().Offset()
		})
	} else {
		sort.SliceStable(balises, func(i, j int) bool {
			return balises[i].LinkPosition().Offset() < balises[j].LinkPosition().Offset()
		})
	}
	for _, balise := range balises {
		if scanRange.contains(balise.LinkPosition()) {
			return balise
		}
	}
	return nil
}

const scanWidth = int64(1000) //1000mm
// 计算车载应答器天线扫描范围，如果应答器在此范围内则天线可以接收到应答器报文
func (s *BaliseDetectSystem) calculateScanRange(wd *component.WorldData, tp *component.TrainPositionInfo) *scanRange {
	headLink := wd.Repo.FindLink(tp.HeadLink)
	if tp.Up { //列车运行方向a->b
		if tp.HeadLinkOffset >= scanWidth {
			return newScanRange(headLink.Id(), tp.HeadLinkOffset-scanWidth, tp.HeadLinkOffset)
		} else {
			return newScanRange(headLink.Id(), 0, tp.HeadLinkOffset)
		}
	} else { //列车运行方向b->a
		if headLink.Length()-tp.HeadLinkOffset >= scanWidth {
			return newScanRange(headLink.Id(), tp.HeadLinkOffset, tp.HeadLinkOffset+scanWidth)
		} else {
			return newScanRange(headLink.Id(), tp.HeadLinkOffset, headLink.Length())
		}
	}
}

type scanRange struct {
	linkId      string
	startOffset int64
	endOffset   int64
}

func newScanRange(linkId string, start int64, end int64) *scanRange {
	return &scanRange{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
}