rts-sim-testing-service/ats/calculate/calculate.go

package calculate

import (
	"errors"
	"fmt"
	"math"
	"strconv"

	"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/face"
	"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/memory"
	"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/model/device"
	"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/model/ref"
)

// CalculateLinkPositionAndDirection 根据起始link位置、偏移量、偏移方向，计算偏移后的位置及方向。偏移方向上的剩余距离不够，会返回error
func CalculateLinkPositionAndDirection(simulation *memory.VerifySimulation, linkIndex int32, linkOffset int32, up bool,
	offset int32) (int32, int32, bool, error) {
	vs := memory.QueryMapVerifyStructure(simulation.MapId)

	direction := "下"
	if up {
		direction = "上"
	}

	var tempOffset int32
	if up {
		tempOffset = linkOffset + offset
		linkModel := vs.LinkModelMap[linkIndex]
		if tempOffset <= linkModel.Length {
			return linkIndex, tempOffset, up, nil
		}
		if linkModel.BRelatedSwitchRef == nil {
			return linkIndex, 0, up, errors.New(fmt.Sprintf("link位置[%d-%d]向[%s]偏移[%d]超出极限", linkIndex, linkOffset, direction, offset))
		}
		turnoutState := memory.GetTurnoutState(simulation, linkModel.ARelatedSwitchRef.SwitchDevice.GetIndex())
		turnoutModel := linkModel.ARelatedSwitchRef.SwitchDevice.(*device.SwitchDeviceModel)
		var linkPort *ref.DevicePort[*device.LinkModel]
		if turnoutState.Normal {
			switch linkModel.BRelatedSwitchRef.Port {
			case face.A:
				linkPort = turnoutModel.BLinkPort
			case face.B:
				linkPort = turnoutModel.ALinkPort
			}
		} else if turnoutState.Reverse {
			switch linkModel.BRelatedSwitchRef.Port {
			case face.A:
				linkPort = turnoutModel.CLinkPort
			case face.C:
				linkPort = turnoutModel.ALinkPort
			}
		}
		if linkPort != nil {
			var newOffset int32
			var newUp bool
			i, _ := strconv.Atoi(linkPort.Device.Index)
			switch linkPort.Port {
			case face.A:
				newOffset = 0
				newUp = true
			case face.B:
				newOffset = linkPort.Device.Length
				newUp = false
			}
			return CalculateLinkPositionAndDirection(simulation, int32(i), newOffset, newUp, tempOffset-linkModel.Length)
		} else {
			return linkIndex, 0, up, errors.New(fmt.Sprintf("link位置[%d-%d]向[%s]偏移[%d]超出极限", linkIndex, linkOffset, direction, offset))
		}
	} else {
		tempOffset = linkOffset - offset
		if tempOffset >= 0 {
			return linkIndex, tempOffset, up, nil
		}
		linkModel := vs.LinkModelMap[linkIndex]
		if linkModel.ARelatedSwitchRef == nil {
			return linkIndex, 0, up, errors.New(fmt.Sprintf("link位置[%d-%d]向[%s]偏移[%d]超出极限", linkIndex, linkOffset, direction, offset))
		}
		turnoutState := memory.GetTurnoutState(simulation, linkModel.ARelatedSwitchRef.SwitchDevice.GetIndex())
		turnoutModel := linkModel.ARelatedSwitchRef.SwitchDevice.(*device.SwitchDeviceModel)
		var linkPort *ref.DevicePort[*device.LinkModel]
		if turnoutState.Normal {
			switch linkModel.ARelatedSwitchRef.Port {
			case face.A:
				linkPort = turnoutModel.BLinkPort
			case face.B:
				linkPort = turnoutModel.ALinkPort
			}
		} else if turnoutState.Reverse {
			switch linkModel.ARelatedSwitchRef.Port {
			case face.A:
				linkPort = turnoutModel.CLinkPort
			case face.C:
				linkPort = turnoutModel.ALinkPort
			}
		}
		if linkPort != nil {
			var newOffset int32
			var newUp bool
			i, _ := strconv.Atoi(linkPort.Device.Index)
			switch linkPort.Port {
			case face.A:
				newOffset = 0
				newUp = true
			case face.B:
				newOffset = linkPort.Device.Length
				newUp = false
			}
			return CalculateLinkPositionAndDirection(simulation, int32(i), newOffset, newUp, int32(math.Abs(float64(tempOffset))))
		} else {
			return linkIndex, 0, up, errors.New(fmt.Sprintf("link位置[%d-%d]向[%s]偏移[%d]超出极限", linkIndex, linkOffset, direction, offset))
		}
	}
}

// CalculateLen 计算两个link位置的间距
// @param up 优先向这个方向查找终点
func CalculateLen(start *ref.DevicePosition[*device.LinkModel], end *ref.DevicePosition[*device.LinkModel], up bool) (int, error) {
	return calculateLen(start, end, up, 0)
}

//// CalculateDirection 计算以{link}为基准的dp方向
//// @param up 优先向这个方向查找link
//func CalculateDirection(dp *ref.DevicePort[*device.LinkModel], link *device.LinkModel, up bool) *ref.DevicePort[*device.LinkModel] {
//	if link == dp.Device {
//		return dp
//	}
//
//}

func calculateLen(start *ref.DevicePosition[*device.LinkModel], end *ref.DevicePosition[*device.LinkModel],
	up bool, l int) (int, error) {
	if start.Device == end.Device {
		return l + int(math.Abs(float64(start.Offset-end.Offset))), nil
	}

	var turnoutPort *ref.SwitchRef
	if up {
		l += int(start.Device.Length - start.Offset)
		turnoutPort = start.Device.BRelatedSwitchRef
	} else {
		l += int(start.Offset)
		turnoutPort = start.Device.ARelatedSwitchRef
	}
	var lps []*ref.DevicePosition[*device.LinkModel]
	if turnoutPort != nil {
		turnoutModel := start.Device.BRelatedSwitchRef.SwitchDevice.(*device.SwitchDeviceModel)
		device.ConvertFromDevicePort(turnoutModel.BLinkPort)
		switch turnoutPort.Port {
		case face.A:
			lps = append(lps, device.ConvertFromDevicePort(turnoutModel.BLinkPort))
			lps = append(lps, device.ConvertFromDevicePort(turnoutModel.CLinkPort))
		case face.B | face.C:
			lps = append(lps, device.ConvertFromDevicePort(turnoutModel.ALinkPort))
		}
	}
	for _, lp := range lps {
		i, err := calculateLen(lp, end, lp.Offset == 0, l)
		if err == nil {
			return i, nil
		}
	}
	return 0, errors.New(fmt.Sprintf("从[%s]到[%s]的间距无法计算", start.String(), end.String()))
}