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

package device_sys

import (
	"fmt"
	"log/slog"
	"strings"

	"joylink.club/ecs"
	"joylink.club/ecs/filter"
	"joylink.club/rtsssimulation/component"
	"joylink.club/rtsssimulation/entity"
	"joylink.club/rtsssimulation/repository"
	"joylink.club/rtsssimulation/repository/model/proto"
)

// SectionDetectSystem 区段检测系统
type SectionDetectSystem struct {
	trainQuery       *ecs.Query
	axleSectionQuery *ecs.Query
}

func NewSectionDetectSystem() *SectionDetectSystem {
	return &SectionDetectSystem{trainQuery: ecs.NewQuery(filter.Contains(component.UidType, component.TrainPositionInfoType)),
		axleSectionQuery: ecs.NewQuery(filter.Contains(component.UidType, component.AxlePhysicalSectionType))}
}
func (s *SectionDetectSystem) Update(w ecs.World) {
	//key-sectionId,统计区段上有车的情况
	sectionTrainMap := make(map[string]*trainCount)
	//所有列车
	s.trainQuery.Each(w, func(entry *ecs.Entry) {
		tp := component.TrainPositionInfoType.Get(entry)
		//fmt.Println("============>>>>>>列车位置信息：", tp.ToString())
		trainSectionIds := s.doSearchTrainOccupiedSections(w, tp)
		//fmt.Println("============>>>>>>列车所在物理区段：", trainSectionIds)
		for _, sectionId := range trainSectionIds { //车所在区段
			tc, find := sectionTrainMap[sectionId]
			if !find {
				tc = newTrainCount()
				sectionTrainMap[sectionId] = tc
			}
			tc.add()
		}
	})
	//计轴区段
	s.axleSectionQuery.Each(w, func(entry *ecs.Entry) {
		axleSectionId := component.UidType.Get(entry).Id
		axleSection := component.AxlePhysicalSectionType.Get(entry)
		tc, find := sectionTrainMap[axleSectionId]
		if find {
			axleSection.UpdateCount(int(tc.count))
		} else {
			axleSection.UpdateCount(0)
		}
	})
}

type trainCount struct {
	count int8
}

func newTrainCount() *trainCount {
	return &trainCount{count: 0}
}
func (c *trainCount) add() {
	c.count++
}
func (s *SectionDetectSystem) doSearchTrainOccupiedSections(w ecs.World, tp *component.TrainPositionInfo) []string {
	wd := entity.GetWorldData(w)
	curLink := wd.Repo.FindLink(tp.HeadLink)
	stp := &stpContext{w: w, trainLen: tp.Len, curLink: curLink, curOffset: tp.HeadLinkOffset, searchDirection: !tp.Up, acLen: 0}
	for stp.needAccumulate() {
		stp.accumulateLen()
		if stp.needAccumulate() {
			stp.nextLink()
		} else {
			break
		}
	}
	//
	return stp.trainSections()
}

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

type stpContext struct {
	w               ecs.World
	trainLen        int64            //列车长度
	curLink         *repository.Link //当前迭代的link
	curOffset       int64            //当前迭代的link,长度累加起点
	searchDirection bool             //在curLink上长度累加的方向，true:a->b,false:b->a,a->b偏移量变大
	acLen           int64            //当前累加长度
	acRanges        []*stpLinkRange  //轨道range列表
}

func (s *stpContext) printStpLinkRanges() {
	var sb strings.Builder
	for _, r := range s.acRanges {
		sb.WriteString(fmt.Sprintf("[linkId=%s start=%d end=%d  linkLen=%d]->", r.link.Id(), r.start, r.end, r.link.Length()))
	}
	fmt.Println("===========>>>>列车所在link range 链：", sb.String())
}

// 获取列车所占的物理区段
func (s *stpContext) trainSections() []string {
	secs := make(map[string]string)
	for _, acR := range s.acRanges {
		ids := acR.sectionIds()
		for _, id := range ids {
			secs[id] = id
		}
	}
	//
	//s.printStpLinkRanges()
	//
	var sectionIds []string
	for _, id := range secs {
		sectionIds = append(sectionIds, id)
	}
	return sectionIds
}
func (s *stpContext) needAccumulate() bool {
	return s.curLink != nil && s.acLen < s.trainLen
}
func (s *stpContext) nextLink() {
	var nextLinkPort *repository.LinkPort
	if s.searchDirection {
		bDc := s.curLink.BRelation()
		bDcDw := s.turnoutPosition(bDc.Turnout().Id())
		switch bDc.Port() {
		case proto.Port_A: //link-b连接turnout-A
			if bDcDw {
				nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_B)
			} else {
				nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_C)
			}
		case proto.Port_B: //link-b连接turnout-B
			fallthrough
		case proto.Port_C: //link-b连接turnout-C
			nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_A)
		}
	} else {
		aDc := s.curLink.ARelation()
		aDcDw := s.turnoutPosition(aDc.Turnout().Id())
		switch aDc.Port() {
		case proto.Port_A: //link-a连接turnout-A
			if aDcDw {
				nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_B)
			} else {
				nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_C)
			}
		case proto.Port_B: //link-a连接turnout-B
			fallthrough
		case proto.Port_C: //link-a连接turnout-C
			nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_A)
		}
	}
	//
	if nextLinkPort != nil {
		s.curLink = nextLinkPort.Device().(*repository.Link)
		fromA := nextLinkPort.IsPortA()
		s.searchDirection = fromA
		if fromA {
			s.curOffset = 0
		} else {
			s.curOffset = s.curLink.Length()
		}
	} else {
		slog.Warn("区段检测时，获取列车下的link失败")
	}
}

// 获取道岔实际位置
func (s *stpContext) turnoutPosition(id string) bool {
	wd := entity.GetWorldData(s.w)
	entry, ok := wd.EntityMap[id]
	if ok {
		return component.TurnoutPositionType.Get(entry).Dw
	} else {
		panic(fmt.Sprintf("道岔[%s]的实体不存在", id))
	}
}

// 累加
func (s *stpContext) accumulateLen() {
	if s.searchDirection {
		dl := s.curLink.Length() - s.curOffset
		if s.acLen+dl < s.trainLen {
			s.addLinkRange(&stpLinkRange{link: s.curLink, start: s.curOffset, end: s.curLink.Length()})
			s.acLen += dl
		} else {
			tl := s.trainLen - s.acLen
			s.addLinkRange(&stpLinkRange{link: s.curLink, start: s.curOffset, end: s.curOffset + tl})
			s.acLen += tl
		}
	} else {
		dl := s.curOffset
		if s.acLen+dl < s.trainLen {
			s.addLinkRange(&stpLinkRange{link: s.curLink, start: 0, end: s.curOffset})
			s.acLen += dl
		} else {
			tl := s.trainLen - s.acLen
			s.addLinkRange(&stpLinkRange{link: s.curLink, start: s.curOffset - tl, end: s.curOffset})
			s.acLen += tl
		}
	}
}
func (s *stpContext) addLinkRange(r *stpLinkRange) {
	s.acRanges = append(s.acRanges, r)
}

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

type stpLinkRange struct {
	link  *repository.Link
	start int64
	end   int64
}

// 获取link range 上的物理区段的id
func (s *stpLinkRange) sectionIds() []string {
	var ids []string
	ps := s.link.GetAllPhysicalSection()
	for _, p := range ps {
		for _, pr := range p.LinkRanges() {
			if isRangeOverlap(s.link.Id(), s.start, s.end, pr.Link().Id(), pr.Start(), pr.End()) {
				ids = append(ids, p.Id())
			}
		}
	}
	return ids
}
func isRangeOverlap(r1LinkId string, r1Start int64, r1End int64, r2LinkId string, r2Start int64, r2End int64) bool {
	if r1LinkId != r2LinkId {
		return false
	}
	r1Start, r1End = formatRange(r1Start, r1End)
	r2Start, r2End = formatRange(r2Start, r2End)
	if r2Start > r1Start && r2Start < r1End {
		return true
	}
	if r2End > r1Start && r2End < r1End {
		return true
	}
	if r1Start > r2Start && r1Start < r2End {
		return true
	}
	if r1End > r2Start && r1End < r2End {
		return true
	}
	//
	return false
}
func formatRange(start int64, end int64) (int64, int64) {
	if start < end {
		return start, end
	} else {
		return end, start
	}
}
func printPhSectionRange(w ecs.World, id string) string {
	wd := entity.GetWorldData(w)
	var sb strings.Builder
	sb.WriteString(fmt.Sprintf("物理区段[%s]range列表：", id))
	for _, r := range wd.Repo.FindPhysicalSection(id).LinkRanges() {
		sb.WriteString(fmt.Sprintf("[linkId=%s start=%d end=%d],", r.Link().Id(), r.Start(), r.End()))
	}
	return sb.String()
}