update

cmd/planner/planner.go

@@ -94,6 +94,20 @@ func (tp *TripPlanner) Preprocess() error {
 	return nil
 }
 
+type History struct {
+	*types.StopTime
+	DistanceToEnd float64
+	TravelTime    types.SecondsAfterMidnight
+}
+
+func NewHistory(st *types.StopTime, distanceToEnd float64, travelTime types.SecondsAfterMidnight) *History {
+	return &History{
+		StopTime:      st,
+		DistanceToEnd: distanceToEnd,
+		TravelTime:    travelTime,
+	}
+}
+
 // FindRoutes finds the best routes (up to num) between two stops starting at the given time
 func (tp *TripPlanner) FindRoute(from, to string, when time.Time) (*Route, error) {
 
@@ -105,19 +119,20 @@ func (tp *TripPlanner) FindRoute(from, to string, when time.Time) (*Route, error
 	}
 
 	possibleNextStops := make([]*types.StopTime, 0)
-
+	var startTime *types.StopTime
 	for start, stop := range fromStop.GetStopsAfter(when) {
 		if stop.StopId == toStop.StopId {
 			return &Route{
 				Legs: []Leg{NewLeg(start, stop)},
 			}, nil
-		} else {
-			possibleNextStops = append(possibleNextStops, start)
+		} else if from != stop.StopId {
+			startTime = start
+			possibleNextStops = append(possibleNextStops, stop)
 		}
 	}
-	slices.SortFunc(possibleNextStops, byArrivalTime(*toStop))
+	slices.SortFunc(possibleNextStops, byDistanceTo(*toStop))
 	for _, nextStop := range possibleNextStops {
-		route, err := tp.findRoute(*nextStop, toStop, nextStop)
+		route, err := tp.findRoute(*nextStop, toStop, *NewHistory(startTime, startTime.Stop.HaversineDistance(toStop), types.AsSecondsAfterMidnight(when)), *NewHistory(nextStop, nextStop.Stop.HaversineDistance(toStop), nextStop.ArrivalTime-startTime.ArrivalTime))
 		if err == nil && route != nil {
 			return route, nil
 		}
@@ -126,17 +141,36 @@ func (tp *TripPlanner) FindRoute(from, to string, when time.Time) (*Route, error
 	return nil, fmt.Errorf("no route found")
 }
 
-func byArrivalTime(end types.Stop) func(a, b *types.StopTime) int {
+func byDistanceTo(end types.Stop) func(a, b *types.StopTime) int {
 	return func(a, b *types.StopTime) int {
-		distanceA := haversine(a.Stop.StopLat, a.Stop.StopLon, end.StopLat, end.StopLon) * 1000
-		distanceB := haversine(b.Stop.StopLat, b.Stop.StopLon, end.StopLat, end.StopLon) * 1000
-		return (int(distanceA) - int(distanceB)) + (int(b.ArrivalTime - a.ArrivalTime))
+		distanceA := haversine(a.Stop.StopLat, a.Stop.StopLon, end.StopLat, end.StopLon)
+		distanceB := haversine(b.Stop.StopLat, b.Stop.StopLon, end.StopLat, end.StopLon)
+		return (int(distanceA) - int(distanceB)) // + (int(b.ArrivalTime - a.ArrivalTime))
 	}
 }
-func (tp *TripPlanner) findRoute(start types.StopTime, end *types.Stop, changes ...*types.StopTime) (*Route, error) {
+
+func shouldTryStop(end *types.Stop, visited ...History) func(possible *types.StopTime) bool {
+	lastDistance := visited[len(visited)-1].Stop.HaversineDistance(end)
+	return func(possible *types.StopTime) bool {
+		distance := possible.Stop.HaversineDistance(end)
+		for _, v := range visited {
+			if v.DistanceToEnd <= distance*1.2 {
+				return false
+			}
+			if v.TripId == possible.TripId || v.StopId == possible.StopId {
+				return false
+			}
+		}
+
+		return distance <= lastDistance*1.2
+	}
+}
+
+func (tp *TripPlanner) findRoute(start types.StopTime, end *types.Stop, changes ...History) (*Route, error) {
 	if len(changes) >= maxTransfers {
 		return nil, fmt.Errorf("max transfers reached")
 	}
+	isOk := shouldTryStop(end, changes...)
 	possibleNextStops := make([]*types.StopTime, 0)
 	for stop := range start.Stop.GetUpcomingStops(&start) {
 		if stop.StopId == end.StopId {
@@ -144,16 +178,16 @@ func (tp *TripPlanner) findRoute(start types.StopTime, end *types.Stop, changes
 				Legs: CreateLegs(changes, stop),
 			}, nil
 		} else {
-			if !slices.ContainsFunc(changes, func(c *types.StopTime) bool { return c.StopId == stop.StopId }) {
+			if isOk(stop) {
 				possibleNextStops = append(possibleNextStops, stop)
 			}
 		}
 	}
-	slices.SortFunc(possibleNextStops, byArrivalTime(*end))
+	slices.SortFunc(possibleNextStops, byDistanceTo(*end))
 
 	tries := 15
 	for _, nextStop := range possibleNextStops {
-		route, err := tp.findRoute(*nextStop, end, append(changes, nextStop)...)
+		route, err := tp.findRoute(*nextStop, end, append(changes, *NewHistory(nextStop, nextStop.Stop.HaversineDistance(end), nextStop.ArrivalTime-start.ArrivalTime))...)
 		if err == nil && route != nil {
 			return route, nil
 		}
@@ -165,14 +199,14 @@ func (tp *TripPlanner) findRoute(start types.StopTime, end *types.Stop, changes
 	return nil, fmt.Errorf("no route found")
 }
 
-func CreateLegs(stops []*types.StopTime, finalStop *types.StopTime) []Leg {
+func CreateLegs(stops []History, finalStop *types.StopTime) []Leg {
 	legs := make([]Leg, 0, len(stops)+1)
 	var previousStop *types.StopTime
 	for _, stop := range stops {
 		if previousStop != nil {
-			legs = append(legs, NewLeg(previousStop, stop))
+			legs = append(legs, NewLeg(previousStop, stop.StopTime))
 		}
-		previousStop = stop
+		previousStop = stop.StopTime
 	}
 	legs = append(legs, NewLeg(previousStop, finalStop))
 	return legs