rts-sim-testing-service/third_party/can_btm/balise_detection.go

package can_btm

import (
	"fmt"
	"joylink.club/bj-rtsts-server/const/balise_const"
	"joylink.club/bj-rtsts-server/dto/state_proto"
	"joylink.club/bj-rtsts-server/third_party/btm_vobc"
	"joylink.club/rtsssimulation/component"
	"joylink.club/rtsssimulation/fi"
	"joylink.club/rtsssimulation/repository"
	"joylink.club/rtsssimulation/repository/model/proto"
	"log/slog"
	"math"
	"sort"
	"strings"
	"sync"
	"time"
)

//通过提前预测来实现BTM天线扫描应答器的实时性
//根据当前列车运行信息预测出列车前方应答器被扫描到的时刻

// BtmAntennaRunningInfo 车载BTM天线中心点位置运行信息
// BTM天线一般在第一车轴后某个位置
type BtmAntennaRunningInfo struct {
	Up           bool    //车载BTM天线中心点运行方向
	LinkId       string  //车载BTM天线中心点所在轨道的id
	LinkOffset   int64   //车载BTM天线中心点所在轨道上的偏移，mm
	Speed        float32 //列车运行速度（m/s）
	Acceleration float32 //加速度(m/s^2)
}

const (
	//BtmAntennaOffsetHead = int64(1000) //车载BTM天线距车头端点的距离，mm
	BtmAntennaOffsetHead = int64(-1000) //车载BTM天线距车头端点的距离，mm
)

// TrainHeadPositionInfo 列车车头运行位置信息
type TrainHeadPositionInfo = fi.TrainHeadPositionInfo
type BtmAntennaToBaliseInfo struct {
	Distance   int64                  //BTM天线中心到应答器的距离，mm
	BaliseId   string                 //应答器id
	BaliseType proto.Transponder_Type //应答器类型
}
type BtmAntennaScanningBaliseInfo struct {
	BaliseId    string                 //应答器id
	Time        time.Time              //应答器预计被BTM天线激活的时刻
	active      bool                   //true-激活过，即列车扫过
	telegram    []byte                 //应答器用户报文
	telegram128 []byte                 //应答器报文
	Distance    int64                  //BTM天线中心到应答器的距离，mm
	BaliseType  proto.Transponder_Type //应答器类型
	IsSend      bool
}

// BaliseDetector 车载BTM天线，应答器探测器
type BaliseDetector struct {
	eq     [3]*BtmAntennaScanningBaliseInfo //预测将被BTM天线扫描的应答器队列，左边为头
	eqLock sync.Mutex
	//车载应答器天线功率放大器开关，true-开，false-关
	powerAmplifierSwitch bool
	//天线此时是否在应答器上方
	aboveBalise bool
	//应答器计数（每过一个应答器加一，在同一个应答器内不变）
	baliseCounter int
	//报文计数器（每解出一个应答器报文加一,应答器报文长度830bits）
	messageCounter int
	//BTM所在列车id
	trianId string
}

// 由于同一时间只能有一辆列车与CAN BTM绑定
// 当检测到重新绑定列车时，重置相关数据
func (t *BaliseDetector) tryRebind(th *TrainHeadPositionInfo) {
	if th.TrainId != t.trianId {
		t.trianId = th.TrainId
		t.clearExpectedBalise()
		t.baliseCounter = 0
		t.messageCounter = 0
		//slog.Debug(fmt.Sprintf("列车[%s]与CAN-BTM绑定", t.trianId))
	}
}
func (t *BaliseDetector) newDetect(wd *component.WorldData, repo *repository.Repository, th, th2 *TrainHeadPositionInfo, btmCache *state_proto.TrainBtmCache) {
	//BTM天线中心点运行信息
	curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th)   //目前车头
	curAntennaRi2 := t.createBtmAntennaRunningInfo(wd, repo, th2) //上次车头
	var startBalises []*repository.Transponder

	//startBalises = t.searchBalisesFromBetweenLinkPosition(repo, th.Up, curAntennaRi2.LinkId, th2.LinkOffset, th.LinkOffset)

	startBalises = t.searchBalisesFromBetweenLinkPosition(repo, th.Up, curAntennaRi.LinkId, curAntennaRi2.LinkOffset, curAntennaRi.LinkOffset)

	balises := make([]*repository.Transponder, 0)
	for _, balise := range startBalises {
		//slog.Info(fmt.Sprintf("id:%v , %v,linkeId:%v ,headoffset:%v,tailOffset:%v", balise.Id(), th.Up, curAntennaRi.LinkId, curAntennaRi.LinkOffset, curAntennaRi2.LinkOffset))
		find := false
		for _, transponder := range balises {
			if transponder.Id() == balise.Id() {
				find = true
				break
			}
		}
		if !find {
			balises = append(balises, balise)
		}
	}
	if len(balises) > 0 {
		balise := balises[0]
		telegram, utel := t.rcvTelegram(wd, balise.Id())
		t.addNewExpectedBalise(balise, btmCache, telegram, utel)
	}

}

func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo, vobcBtm *state_proto.VobcBtmState) {
	t.tryRebind(th)
	//if !t.powerAmplifierSwitch { //天线功率放大器未开启，不进行探测
	//	return
	//}
	curTime := time.Now()
	//BTM天线中心点运行信息
	curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th)
	//预测BTM天线到最近一个应答器的时刻
	curExpect := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi)

	if curExpect != nil && curExpect.Time.UnixMilli()-curTime.UnixMilli() < 20 { //20ms
		//if curExpect != nil && curExpect.Distance < 80 { //20ms
		//slog.Debug("将要激活应答器", "BaliseId", curExpect.BaliseId, "ActiveTime", dt)
		//slog.Info(fmt.Sprintf("baliseId:%v,Distance:%v,up:%v", curExpect.BaliseId, curExpect.Distance, curAntennaRi.Up))
		telegram, utel := t.rcvTelegram(wd, curExpect.BaliseId)

		if curExpect.Distance <= 50 {
			btm_vobc.Default().AppendBaliseMsgForTrain(vobcBtm, curExpect.BaliseId, telegram, curTime.UnixMilli())
		}
		//记录即将经过的应答器
		if t.addExpectedBalise(curExpect) {
			t.baliseCounterAdd1() //应答器计数器
			if len(telegram) > 0 {
				curExpect.telegram = utel
				curExpect.telegram128 = telegram
				t.baliseMessageCounterAdd1() //报文计数器
			}
		}
		//BTM天线即将经过应答器
		t.aboveBalise = true
	} else {
		t.aboveBalise = false
	}

	curAntennaRi2 := t.createBtmAntennaRunningInfo(wd, repo, &TrainHeadPositionInfo{TrainId: th.TrainId,
		Up:           !th.Up,
		Link:         th.Link,
		LinkOffset:   th.LinkOffset,
		Speed:        th.Speed,
		Acceleration: th.Acceleration})
	curExpect2 := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi2)
	if curExpect2 != nil && curExpect2.Distance > 20 {
		btm_vobc.Default().UpdateTrainLeave(vobcBtm, curExpect2.BaliseId, curTime.UnixMilli())
	}
}

// 应答器计数器加1，[0,255]
func (t *BaliseDetector) baliseCounterAdd1() {
	t.baliseCounter++
	if t.baliseCounter > 255 {
		t.baliseCounter = 0
	}
}

// 报文计数器加1，[0,255]
func (t *BaliseDetector) baliseMessageCounterAdd1() {
	t.messageCounter++
	if t.messageCounter > 255 {
		t.messageCounter = 0
	}
}

func BaliseCounterAdd(counter uint32) byte {
	c := byte(counter)
	c++
	if c > 255 {
		c = 0
	}
	return c
}

// BTM天线接收应答器报文(线程不安全)
func (t *BaliseDetector) rcvTelegram(wd *component.WorldData, baliseId string) ([]byte, []byte) {

	if entry, ok := wd.EntityMap[baliseId]; ok {
		workedState := component.BaliseWorkStateType.Get(entry)
		fixBalise := component.BaliseFixedTelegramType.Get(entry)
		if entry.HasComponent(component.BaliseVariableTelegramType) {
			baliseVar := component.BaliseVariableTelegramType.Get(entry)
			if !workedState.Work {
				return fixBalise.Telegram, fixBalise.UserTelegram
			} else if baliseVar.UserTelegram == nil || len(baliseVar.UserTelegram) == 0 {
				slog.Warn(fmt.Sprintf("BTM天线未接受到应答器可变报文，即将使用对应的固定报文, baliseId: %v", baliseId))
				return fixBalise.Telegram, fixBalise.UserTelegram
			} else {
				return baliseVar.Telegram, baliseVar.UserTelegram
			}
		} else if workedState.Work {
			return fixBalise.Telegram, fixBalise.UserTelegram
		} else {
			slog.Warn(fmt.Sprintf("BTM天线未接受到应答器报文，应答器未工作 baliseId: %v", baliseId))
		}
	} else {
		slog.Warn(fmt.Sprintf("BTM天线接收应答器报文，未找到 baliseId: %v", baliseId))
	}
	return nil, nil
}
func (t *BaliseDetector) addNewExpectedBalise(balise *repository.Transponder, btmCache *state_proto.TrainBtmCache, telegram, userTelegram []byte) bool {
	t.eqLock.Lock()
	defer t.eqLock.Unlock()
	bl := btmCache.BaliseList
	for _, tt := range bl {
		if tt != nil && tt.BaliseId == balise.Id() {
			return false
		}
	}
	for i := 1; i < len(bl); i++ {
		bl[i-1] = bl[i]
	}
	unpack := false
	bc := BaliseCounterAdd(btmCache.BaliseCount)
	mc := btmCache.MessageCounter
	if userTelegram != nil && len(userTelegram) > 0 {
		mc = uint32(BaliseCounterAdd(mc))
		unpack = true
	}

	btmCache.BaliseCount = uint32(bc)
	btmCache.MessageCounter = mc
	btmS := &state_proto.BTMState{BaliseId: balise.Id(),
		Telegram:    fmt.Sprintf("%x", userTelegram),
		Telegram128: fmt.Sprintf("%X", telegram),
		Unpack:      unpack,
		BaliseType:  int32(balise.BaliseType().Number()),
		AboveBalise: true, HasData: true}

	if userTelegram == nil || len(userTelegram) == 0 {
		btmS.Telegram = strings.Repeat("00", balise_const.UserTelegramByteLen)
		btmS.Telegram128 = strings.Repeat("00", balise_const.TelegramByteLen)
		btmS.HasData = false
	}
	//存入队尾
	bl[len(bl)-1] = btmS
	return true
}

// true-新增；false-更新
func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) bool {
	//
	t.eqLock.Lock()
	defer t.eqLock.Unlock()
	////更新,去重
	//for i, e := range t.eq {
	//	if e != nil && e.BaliseId == curExpect.BaliseId {
	//		t.eq[i].Time = curExpect.Time
	//		return false
	//	}
	//}
	//检查是否已经记录过

	for _, tt := range t.eq {
		if tt != nil && tt.BaliseId == curExpect.BaliseId {
			return false
		}
	}

	/*	eq := t.eq[len(t.eq)-1]
		if eq != nil && eq.BaliseId == curExpect.BaliseId {
			return false
		}*/
	//左移
	for i := 1; i < len(t.eq); i++ {
		t.eq[i-1] = t.eq[i]
	}
	//存入队尾
	t.eq[len(t.eq)-1] = curExpect
	return true
}
func (t *BaliseDetector) clearExpectedBalise() {
	//
	t.eqLock.Lock()
	defer t.eqLock.Unlock()
	//
	for i := 0; i < len(t.eq); i++ {
		t.eq[i] = nil
	}
}
func (t *BaliseDetector) doScan() *BtmAntennaScanningBaliseInfo {
	//
	t.eqLock.Lock()
	defer t.eqLock.Unlock()
	//
	var rt *BtmAntennaScanningBaliseInfo
	for i := 0; i < len(t.eq); i++ {
		if t.eq[i] != nil && !t.eq[i].active {
			rt = t.eq[i]
			t.eq[i].active = true
			break
		}
	}
	return rt
}

// 计算列车在当前运行状态下，预测到最近一个应答器的时刻
func (t *BaliseDetector) timeScanNearestBalise(curTime time.Time, wd *component.WorldData, repo *repository.Repository, ba *BtmAntennaRunningInfo) *BtmAntennaScanningBaliseInfo {
	expectedBalise := t.findBaliseWillScanByBtmAntenna(wd, repo, ba)
	if expectedBalise != nil {
		curV := float64(ba.Speed)
		curAc := float64(ba.Acceleration)
		s := float64(expectedBalise.Distance) / 1000
		st, ok := t.calculateBtmAntennaScanNextBaliseTime(curTime, curV, curAc, s)

		if ok {

			return &BtmAntennaScanningBaliseInfo{BaliseId: expectedBalise.BaliseId, Time: st, Distance: expectedBalise.Distance, BaliseType: expectedBalise.BaliseType}
		}
	}
	return nil
}

// 预测BTM天线到运行方向的最近应答器的时刻
// curV-当前时刻BTM天线速度,m/s
// curAc-当前时刻BTM天线加速度,m/s^2
// s-BTM天线从当前时刻所处位置到运行方向最近一个应答器的位移,m
func (t *BaliseDetector) calculateBtmAntennaScanNextBaliseTime(curTime time.Time, curV float64, curAc float64, s float64) (time.Time, bool) {
	a := 0.5 * curAc
	b := curV
	c := -s
	//nt 单位秒
	nt, ok := t.calculateQuadratic(a, b, c)
	if ok {
		return curTime.Add(time.Millisecond * time.Duration(nt*1000)), true
	}
	return curTime, false
}

// 获取车载BTM天线中心点运行方向最近的1个应答器
func (t *BaliseDetector) findBaliseWillScanByBtmAntenna2(wd *component.WorldData, repo *repository.Repository, ba *BtmAntennaRunningInfo) *BtmAntennaToBaliseInfo {
	//BTM天线中心点所在轨道
	baLink := repo.FindLink(ba.LinkId)
	rs1 := t.searchBalisesFromLinkPosition(repo, ba.LinkId, ba.Up, ba.LinkOffset)
	if ba.Up {
		if len(rs1) > 0 {
			rs := rs1[0]
			return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: rs.LinkPosition().Offset() - ba.LinkOffset, BaliseType: rs.BaliseType()}
		} else {
			nextLinkPort := t.getNextLink(wd, repo, ba.LinkId, ba.Up)
			if nextLinkPort != nil {
				if nextLinkPort.IsPortA() {
					rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), true, 0)
					if len(rs2) > 0 {
						rs := rs2[0]

						return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: baLink.Length() - ba.LinkOffset + rs.LinkPosition().Offset(), BaliseType: rs.BaliseType()}
					}
				} else {
					rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), false, nextLinkPort.Link().Length())
					if len(rs2) > 0 {
						rs := rs2[0]
						return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: baLink.Length() - ba.LinkOffset + nextLinkPort.Link().Length() - rs.LinkPosition().Offset(), BaliseType: rs.BaliseType()}
					}
				}
			}
		}
	} else {
		if len(rs1) > 0 {
			return &BtmAntennaToBaliseInfo{BaliseId: rs1[0].Id(), Distance: ba.LinkOffset - rs1[0].LinkPosition().Offset()}
		} else {
			nextLinkPort := t.getNextLink(wd, repo, ba.LinkId, ba.Up)
			if nextLinkPort != nil {
				if nextLinkPort.IsPortA() {
					rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), true, 0)
					if len(rs2) > 0 {
						return &BtmAntennaToBaliseInfo{BaliseId: rs2[0].Id(), Distance: ba.LinkOffset + rs2[0].LinkPosition().Offset()}
					}
				} else {
					rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), false, nextLinkPort.Link().Length())
					if len(rs2) > 0 {
						return &BtmAntennaToBaliseInfo{BaliseId: rs2[0].Id(), Distance: ba.LinkOffset + nextLinkPort.Link().Length() - rs2[0].LinkPosition().Offset()}
					}
				}
			}
		}
	}
	return nil
}

// 获取车载BTM天线中心点运行方向最近的1个应答器
func (t *BaliseDetector) findBaliseWillScanByBtmAntenna(wd *component.WorldData, repo *repository.Repository, ba *BtmAntennaRunningInfo) *BtmAntennaToBaliseInfo {
	//BTM天线中心点所在轨道
	baLink := repo.FindLink(ba.LinkId)
	rs1 := t.searchBalisesFromLinkPosition(repo, ba.LinkId, ba.Up, ba.LinkOffset)
	if ba.Up {
		if len(rs1) > 0 {
			rs := rs1[0]
			return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: rs.LinkPosition().Offset() - ba.LinkOffset, BaliseType: rs.BaliseType()}
		} else {
			nextLinkPort := t.getNextLink(wd, repo, ba.LinkId, ba.Up)
			if nextLinkPort != nil {
				if nextLinkPort.IsPortA() {
					rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), true, 0)
					if len(rs2) > 0 {
						rs := rs2[0]

						return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: baLink.Length() - ba.LinkOffset + rs.LinkPosition().Offset(), BaliseType: rs.BaliseType()}
					}
				} else {
					rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), false, nextLinkPort.Link().Length())
					if len(rs2) > 0 {
						rs := rs2[0]
						return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: baLink.Length() - ba.LinkOffset + nextLinkPort.Link().Length() - rs.LinkPosition().Offset(), BaliseType: rs.BaliseType()}
					}
				}
			}
		}
	} else {
		if len(rs1) > 0 {
			return &BtmAntennaToBaliseInfo{BaliseId: rs1[0].Id(), Distance: ba.LinkOffset - rs1[0].LinkPosition().Offset()}
		} else {
			nextLinkPort := t.getNextLink(wd, repo, ba.LinkId, ba.Up)
			if nextLinkPort != nil {
				if nextLinkPort.IsPortA() {
					rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), true, 0)
					if len(rs2) > 0 {
						return &BtmAntennaToBaliseInfo{BaliseId: rs2[0].Id(), Distance: ba.LinkOffset + rs2[0].LinkPosition().Offset()}
					}
				} else {
					rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), false, nextLinkPort.Link().Length())
					if len(rs2) > 0 {
						return &BtmAntennaToBaliseInfo{BaliseId: rs2[0].Id(), Distance: ba.LinkOffset + nextLinkPort.Link().Length() - rs2[0].LinkPosition().Offset()}
					}
				}
			}
		}
	}
	return nil
}

func (t *BaliseDetector) searchBalisesFromBetweenLinkPosition(repo *repository.Repository, up bool, linkId string, fromOffset int64, toOffset int64) []*repository.Transponder {
	rs := repo.ResponderListByLink(linkId)
	balises := make([]*repository.Transponder, 0)
	if up {
		sort.SliceStable(rs, func(i, j int) bool {
			return rs[i].LinkPosition().Offset() < rs[j].LinkPosition().Offset()
		})
		for _, r := range rs {

			if r.LinkPosition().Offset() >= fromOffset && r.LinkPosition().Offset() <= toOffset {
				//slog.Info(fmt.Sprintf("up  id:%v,offset:%v,from:%v,to:%v", r.Id(), r.LinkPosition().Offset(), fromOffset, toOffset))
				balises = append(balises, r)
			}
		}
	} else {
		sort.SliceStable(rs, func(i, j int) bool {
			return rs[j].LinkPosition().Offset() < rs[i].LinkPosition().Offset()
		})
		for _, r := range rs {
			//slog.Info(fmt.Sprintf("down  id:%v,offset:%v,from:%v,to:%v", r.Id(), r.LinkPosition().Offset(), fromOffset, toOffset))
			if r.LinkPosition().Offset() <= toOffset {
				cha := int64(math.Abs(float64(toOffset - fromOffset)))
				cha2 := int64(math.Abs(float64(toOffset - r.LinkPosition().Offset())))
				if cha2 <= cha {
					balises = append(balises, r)
				}

			}
		}
	}
	return balises
}

// up-在轨道上的搜索方向
func (t *BaliseDetector) searchBalisesFromLinkPosition(repo *repository.Repository, linkId string, up bool, fromOffset int64) []*repository.Transponder {
	rs := repo.ResponderListByLink(linkId)
	if up {
		sort.SliceStable(rs, func(i, j int) bool {
			return rs[i].LinkPosition().Offset() < rs[j].LinkPosition().Offset()
		})
		//
		for i, r := range rs {
			if r.LinkPosition().Offset() >= fromOffset {
				return rs[i:]
			}
		}
	} else {
		sort.SliceStable(rs, func(i, j int) bool {
			return rs[j].LinkPosition().Offset() < rs[i].LinkPosition().Offset()
		})
		for i, r := range rs {
			if r.LinkPosition().Offset() <= fromOffset {
				return rs[i:]
			}
		}
	}
	return nil
}

// 列车车头端点运行信息转换为车载BTM天线中心点运行信息
func (t *BaliseDetector) createBtmAntennaRunningInfo(wd *component.WorldData, repo *repository.Repository, head *TrainHeadPositionInfo) *BtmAntennaRunningInfo {
	headLink := repo.FindLink(head.Link)
	if head.Up {
		if head.LinkOffset >= BtmAntennaOffsetHead { //车头与BTM天线在同一个轨道上
			return &BtmAntennaRunningInfo{Up: head.Up, LinkId: head.Link, LinkOffset: head.LinkOffset - BtmAntennaOffsetHead, Speed: head.Speed, Acceleration: head.Acceleration}
		} else { //车头与BTM天线在同一个轨道上
			nextLinkPort := t.getNextLink(wd, repo, head.Link, !head.Up)
			nextLink := nextLinkPort.Link()
			if nextLinkPort.IsPortA() {
				return &BtmAntennaRunningInfo{Up: false, LinkId: nextLink.Id(), LinkOffset: BtmAntennaOffsetHead - head.LinkOffset, Speed: head.Speed, Acceleration: head.Acceleration}
			} else {
				return &BtmAntennaRunningInfo{Up: true, LinkId: nextLink.Id(), LinkOffset: nextLink.Length() - (BtmAntennaOffsetHead - head.LinkOffset), Speed: head.Speed, Acceleration: head.Acceleration}
			}
		}
	} else {
		if headLink.Length()-head.LinkOffset >= BtmAntennaOffsetHead { //车头与BTM天线在同一个轨道上
			return &BtmAntennaRunningInfo{Up: head.Up, LinkId: head.Link, LinkOffset: head.LinkOffset + BtmAntennaOffsetHead, Speed: head.Speed, Acceleration: head.Acceleration}
		} else {
			nextLinkPort := t.getNextLink(wd, repo, head.Link, !head.Up)
			nextLink := nextLinkPort.Link()
			if nextLinkPort.IsPortA() {
				return &BtmAntennaRunningInfo{Up: false, LinkId: nextLink.Id(), LinkOffset: BtmAntennaOffsetHead - headLink.Length() + head.LinkOffset, Speed: head.Speed, Acceleration: head.Acceleration}
			} else {
				return &BtmAntennaRunningInfo{Up: true, LinkId: nextLink.Id(), LinkOffset: nextLink.Length() - (BtmAntennaOffsetHead - headLink.Length() + head.LinkOffset), Speed: head.Speed, Acceleration: head.Acceleration}
			}
		}
	}
}
func (t *BaliseDetector) getNextLink(wd *component.WorldData, repo *repository.Repository, curLinkId string, searchDirection bool) *repository.LinkPort {
	var nextLinkPort *repository.LinkPort
	curLink := repo.FindLink(curLinkId)
	if searchDirection {
		bDc := curLink.BRelation()
		if bDc == nil || bDc.Turnout() == nil {
			return nil
		}
		//
		bDcDw := t.turnoutPosition(wd, 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 := curLink.ARelation()
		if aDc == nil || aDc.Turnout() == nil {
			return nil
		}
		//
		aDcDw := t.turnoutPosition(wd, 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)
		}
	}
	//
	return nextLinkPort
}

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

// 解 ax²+bx+c=0
func (t *BaliseDetector) calculateQuadratic(a, b, c float64) (float64, bool) {
	//D=b²-4ac
	d := b*b - 4*a*c
	if d < 0 {
		return 0, false
	} else if d == 0 { //若D=0，则计算它的重根：x=-b/2a
		return -(b / (2 * a)), true
	} else { //若D>0，则计算它的两个根：x1=(-b+√D)/2a，x2=(-b-√D)/2a
		sqrtD := math.Sqrt(d)
		ax2 := 2 * a
		//选择正直输出
		x1 := (-b + sqrtD) / ax2
		x2 := (-b - sqrtD) / ax2
		if x1 >= 0 && x2 >= 0 {
			//panic("x1 >= 0 && x2 >= 0")
			return 0, false
		}
		if x1 >= 0 {
			return x1, true
		}
		if x2 >= 0 {
			return x2, true
		}
	}
	return 0, false
}