diff --git a/mapd.go b/mapd.go
index 008d63a..ae9f4b5 100644
--- a/mapd.go
+++ b/mapd.go
@@ -3,6 +3,7 @@ package main
 import (
 	"encoding/json"
 	"flag"
+	"math"
 	"os"
 	"time"
 
@@ -14,10 +15,17 @@ import (
 )
 
 type State struct {
-	Data       []uint8
-	CurrentWay CurrentWay
-	NextWays   []NextWayResult
-	Position   Position
+	Data                   []uint8
+	CurrentWay             CurrentWay
+	NextWays               []NextWayResult
+	Position               Position
+	LastPosition           Position
+	StableWayCounter       int
+	LastGPSUpdate          time.Time
+	LastWayChange          time.Time
+	LastSpeedLimitDistance float64
+	LastSpeedLimitValue    float64
+	LastSpeedLimitWayName  string
 }
 
 type Position struct {
@@ -30,6 +38,7 @@ type NextSpeedLimit struct {
 	Latitude   float64 `json:"latitude"`
 	Longitude  float64 `json:"longitude"`
 	Speedlimit float64 `json:"speedlimit"`
+	Distance   float64 `json:"distance"`
 }
 
 type AdvisoryLimit struct {
@@ -90,6 +99,115 @@ func readPosition(persistent bool) (Position, error) {
 	return pos, errors.Wrap(err, "could not unmarshal coordinates")
 }
 
+func shouldUpdateRoadInfo(state *State) bool {
+	return true
+}
+
+func hasRoadInfoChanged(oldWay, newWay Way) bool {
+	if !oldWay.HasNodes() && !newWay.HasNodes() {
+		return false
+	}
+	if !oldWay.HasNodes() || !newWay.HasNodes() {
+		return true
+	}
+
+	return !(oldWay.MinLat() == newWay.MinLat() &&
+		oldWay.MaxLat() == newWay.MaxLat() &&
+		oldWay.MinLon() == newWay.MinLon() &&
+		oldWay.MaxLon() == newWay.MaxLon())
+}
+
+func calculateNextSpeedLimit(state *State, currentMaxSpeed float64) NextSpeedLimit {
+	if len(state.NextWays) == 0 {
+		return NextSpeedLimit{}
+	}
+
+	// Find the next speed limit change
+	cumulativeDistance := 0.0
+	currentPos := state.Position
+
+	if state.CurrentWay.Way.HasNodes() {
+		distToEnd, err := DistanceToEndOfWay(currentPos, state.CurrentWay.Way, state.CurrentWay.OnWay.IsForward)
+		if err == nil && distToEnd > 0 {
+			cumulativeDistance = distToEnd
+		}
+	}
+
+	// Look through next ways for speed limit change
+	for _, nextWay := range state.NextWays {
+		nextMaxSpeed := nextWay.Way.MaxSpeed()
+		if nextWay.IsForward && nextWay.Way.MaxSpeedForward() > 0 {
+			nextMaxSpeed = nextWay.Way.MaxSpeedForward()
+		} else if !nextWay.IsForward && nextWay.Way.MaxSpeedBackward() > 0 {
+			nextMaxSpeed = nextWay.Way.MaxSpeedBackward()
+		}
+
+		if nextMaxSpeed != currentMaxSpeed && nextMaxSpeed > 0 {
+			result := NextSpeedLimit{
+				Latitude:   nextWay.StartPosition.Latitude(),
+				Longitude:  nextWay.StartPosition.Longitude(),
+				Speedlimit: nextMaxSpeed,
+				Distance:   cumulativeDistance,
+			}
+
+			wayName := RoadName(nextWay.Way)
+			if nextMaxSpeed == state.LastSpeedLimitValue && wayName == state.LastSpeedLimitWayName {
+				smoothedDistance := state.LastSpeedLimitDistance*0.8 + cumulativeDistance*0.2
+
+				if math.Abs(smoothedDistance-state.LastSpeedLimitDistance) < 50 {
+					result.Distance = smoothedDistance
+				}
+
+				log.Debug().
+					Float64("raw_distance", cumulativeDistance).
+					Float64("smoothed_distance", result.Distance).
+					Float64("last_distance", state.LastSpeedLimitDistance).
+					Str("way", wayName).
+					Msg("Smoothed speed limit distance")
+			}
+			state.LastSpeedLimitDistance = result.Distance
+			state.LastSpeedLimitValue = nextMaxSpeed
+			state.LastSpeedLimitWayName = wayName
+
+			return result
+		}
+		if nextWay.Way.HasNodes() {
+			wayDistance, err := calculateWayDistance(nextWay.Way)
+			if err == nil {
+				cumulativeDistance += wayDistance
+			}
+		}
+	}
+
+	return NextSpeedLimit{}
+}
+
+func calculateWayDistance(way Way) (float64, error) {
+	nodes, err := way.Nodes()
+	if err != nil {
+		return 0, err
+	}
+
+	if nodes.Len() < 2 {
+		return 0, nil
+	}
+
+	totalDistance := 0.0
+	for i := 0; i < nodes.Len()-1; i++ {
+		nodeStart := nodes.At(i)
+		nodeEnd := nodes.At(i + 1)
+		distance := DistanceToPoint(
+			nodeStart.Latitude()*TO_RADIANS,
+			nodeStart.Longitude()*TO_RADIANS,
+			nodeEnd.Latitude()*TO_RADIANS,
+			nodeEnd.Longitude()*TO_RADIANS,
+		)
+		totalDistance += distance
+	}
+
+	return totalDistance, nil
+}
+
 func loop(state *State) {
 	defer func() {
 		if err := recover(); err != nil {
@@ -100,6 +218,8 @@ func loop(state *State) {
 			state.NextWays = []NextWayResult{}
 			state.CurrentWay = CurrentWay{}
 			state.Position = Position{}
+			state.LastPosition = Position{}
+			state.StableWayCounter = 0
 		}
 	}()
 
@@ -133,11 +253,17 @@ func loop(state *State) {
 	time.Sleep(1 * time.Second)
 	DownloadIfTriggered()
 
+	state.LastPosition = state.Position
+
 	pos, err := readPosition(false)
 	if err != nil {
 		logwe(errors.Wrap(err, "could not read current position"))
 		return
 	}
+
+	state.Position = pos
+	state.LastGPSUpdate = time.Now()
+
 	offline := readOffline(state.Data)
 
 	// ------------- Find current and next ways ------------
@@ -145,11 +271,32 @@ func loop(state *State) {
 	if !PointInBox(pos.Latitude, pos.Longitude, offline.MinLat(), offline.MinLon(), offline.MaxLat(), offline.MaxLon()) {
 		state.Data, err = FindWaysAroundLocation(pos.Latitude, pos.Longitude)
 		logde(errors.Wrap(err, "could not find ways around current location"))
+		state.CurrentWay.ConfidenceCounter = 0
 	}
 
-	state.CurrentWay, err = GetCurrentWay(state.CurrentWay, state.NextWays, offline, pos)
+	oldWay := state.CurrentWay.Way
+
+	// get GPS accuracy estimate
+	gpsAccuracy := 5.0 // Default 5m accuracy
+
+	state.CurrentWay, err = GetCurrentWay(state.CurrentWay, state.NextWays, offline, pos, state.LastPosition, gpsAccuracy)
 	logde(errors.Wrap(err, "could not get current way"))
 
+	if hasRoadInfoChanged(oldWay, state.CurrentWay.Way) {
+		state.LastWayChange = time.Now()
+		state.StableWayCounter = 0
+		state.LastSpeedLimitDistance = 0
+		state.LastSpeedLimitValue = 0
+		state.LastSpeedLimitWayName = ""
+		log.Info().
+			Str("new_road", RoadName(state.CurrentWay.Way)).
+			Float64("distance", state.CurrentWay.Distance.Distance).
+			Int("confidence", state.CurrentWay.ConfidenceCounter).
+			Msg("Way changed")
+	} else {
+		state.StableWayCounter++
+	}
+
 	state.NextWays, err = NextWays(pos, state.CurrentWay, offline, state.CurrentWay.OnWay.IsForward)
 	logde(errors.Wrap(err, "could not get next way"))
 
@@ -170,124 +317,120 @@ func loop(state *State) {
 	err = PutParam(MAP_TARGET_VELOCITIES, data)
 	logwe(errors.Wrap(err, "could not write curvatures"))
 
-	// ----------------- Current Data --------------------
-	err = PutParam(ROAD_NAME, []byte(RoadName(state.CurrentWay.Way)))
-	logwe(errors.Wrap(err, "could not write road name"))
-
-	data, err = json.Marshal(state.CurrentWay.Way.AdvisorySpeed())
-	logde(errors.Wrap(err, "could not marshal advisory speed limit"))
-	err = PutParam(MAP_ADVISORY_LIMIT, data)
-	logwe(errors.Wrap(err, "could not write advisory speed limit"))
-
-	hazard, err := state.CurrentWay.Way.Hazard()
-	logde(errors.Wrap(err, "could not read current way hazard"))
-	data, err = json.Marshal(Hazard{
-		StartLatitude:  state.CurrentWay.StartPosition.Latitude(),
-		StartLongitude: state.CurrentWay.StartPosition.Longitude(),
-		EndLatitude:    state.CurrentWay.EndPosition.Latitude(),
-		EndLongitude:   state.CurrentWay.EndPosition.Longitude(),
-		Hazard:         hazard,
-	})
-	logde(errors.Wrap(err, "could not marshal hazard"))
-	err = PutParam(MAP_HAZARD, data)
-	logwe(errors.Wrap(err, "could not write hazard"))
-
-	data, err = json.Marshal(AdvisoryLimit{
-		StartLatitude:  state.CurrentWay.StartPosition.Latitude(),
-		StartLongitude: state.CurrentWay.StartPosition.Longitude(),
-		EndLatitude:    state.CurrentWay.EndPosition.Latitude(),
-		EndLongitude:   state.CurrentWay.EndPosition.Longitude(),
-		Speedlimit:     state.CurrentWay.Way.AdvisorySpeed(),
-	})
-	logde(errors.Wrap(err, "could not marshal advisory speed limit"))
-	err = PutParam(MAP_ADVISORY_LIMIT, data)
-	logwe(errors.Wrap(err, "could not write advisory speed limit"))
-
-	// ---------------- Next Data ---------------------
-
-	if len(state.NextWays) > 0 {
-		hazard, err = state.NextWays[0].Way.Hazard()
-		logde(errors.Wrap(err, "could not read next hazard"))
+	if shouldUpdateRoadInfo(state) {
+		// ----------------- Current Data --------------------
+		err = PutParam(ROAD_NAME, []byte(RoadName(state.CurrentWay.Way)))
+		logwe(errors.Wrap(err, "could not write road name"))
+
+		data, err = json.Marshal(state.CurrentWay.Way.AdvisorySpeed())
+		logde(errors.Wrap(err, "could not marshal advisory speed limit"))
+		err = PutParam(MAP_ADVISORY_LIMIT, data)
+		logwe(errors.Wrap(err, "could not write advisory speed limit"))
+
+		hazard, err := state.CurrentWay.Way.Hazard()
+		logde(errors.Wrap(err, "could not read current way hazard"))
 		data, err = json.Marshal(Hazard{
-			StartLatitude:  state.NextWays[0].StartPosition.Latitude(),
-			StartLongitude: state.NextWays[0].StartPosition.Longitude(),
-			EndLatitude:    state.NextWays[0].EndPosition.Latitude(),
-			EndLongitude:   state.NextWays[0].EndPosition.Longitude(),
+			StartLatitude:  state.CurrentWay.StartPosition.Latitude(),
+			StartLongitude: state.CurrentWay.StartPosition.Longitude(),
+			EndLatitude:    state.CurrentWay.EndPosition.Latitude(),
+			EndLongitude:   state.CurrentWay.EndPosition.Longitude(),
 			Hazard:         hazard,
 		})
-		logde(errors.Wrap(err, "could not marshal next hazard"))
-		err = PutParam(NEXT_MAP_HAZARD, data)
-		logwe(errors.Wrap(err, "could not write next hazard"))
-	}
+		logde(errors.Wrap(err, "could not marshal hazard"))
+		err = PutParam(MAP_HAZARD, data)
+		logwe(errors.Wrap(err, "could not write hazard"))
 
-	if len(state.NextWays) > 0 {
-		currentMaxSpeed := state.CurrentWay.Way.MaxSpeed()
-		if state.CurrentWay.OnWay.IsForward && state.CurrentWay.Way.MaxSpeedForward() > 0 {
-			currentMaxSpeed = state.CurrentWay.Way.MaxSpeedForward()
-		} else if !state.CurrentWay.OnWay.IsForward && state.CurrentWay.Way.MaxSpeedBackward() > 0 {
-			currentMaxSpeed = state.CurrentWay.Way.MaxSpeedBackward()
+		data, err = json.Marshal(AdvisoryLimit{
+			StartLatitude:  state.CurrentWay.StartPosition.Latitude(),
+			StartLongitude: state.CurrentWay.StartPosition.Longitude(),
+			EndLatitude:    state.CurrentWay.EndPosition.Latitude(),
+			EndLongitude:   state.CurrentWay.EndPosition.Longitude(),
+			Speedlimit:     state.CurrentWay.Way.AdvisorySpeed(),
+		})
+		logde(errors.Wrap(err, "could not marshal advisory speed limit"))
+		err = PutParam(MAP_ADVISORY_LIMIT, data)
+		logwe(errors.Wrap(err, "could not write advisory speed limit"))
+
+		// ---------------- Next Data ---------------------
+
+		if len(state.NextWays) > 0 {
+			hazard, err = state.NextWays[0].Way.Hazard()
+			logde(errors.Wrap(err, "could not read next hazard"))
+			data, err = json.Marshal(Hazard{
+				StartLatitude:  state.NextWays[0].StartPosition.Latitude(),
+				StartLongitude: state.NextWays[0].StartPosition.Longitude(),
+				EndLatitude:    state.NextWays[0].EndPosition.Latitude(),
+				EndLongitude:   state.NextWays[0].EndPosition.Longitude(),
+				Hazard:         hazard,
+			})
+			logde(errors.Wrap(err, "could not marshal next hazard"))
+			err = PutParam(NEXT_MAP_HAZARD, data)
+			logwe(errors.Wrap(err, "could not write next hazard"))
 		}
 
-		data, err = json.Marshal(currentMaxSpeed)
-		logde(errors.Wrap(err, "could not marshal speed limit"))
-		err = PutParam(MAP_SPEED_LIMIT, data)
-		logwe(errors.Wrap(err, "could not write speed limit"))
-
-		nextMaxSpeed := currentMaxSpeed
-		nextSpeedWay := state.NextWays[0]
-		for _, nextWay := range state.NextWays {
-			nextMaxSpeed = nextWay.Way.MaxSpeed()
-			if nextWay.IsForward && nextWay.Way.MaxSpeedForward() > 0 {
-				nextMaxSpeed = nextWay.Way.MaxSpeedForward()
-			} else if !nextWay.IsForward && nextWay.Way.MaxSpeedBackward() > 0 {
-				nextMaxSpeed = nextWay.Way.MaxSpeedBackward()
-			}
-			if nextMaxSpeed != currentMaxSpeed {
-				nextSpeedWay = nextWay
-				break
+		if len(state.NextWays) > 0 || state.CurrentWay.Way.HasNodes() {
+			currentMaxSpeed := state.CurrentWay.Way.MaxSpeed()
+			if state.CurrentWay.OnWay.IsForward && state.CurrentWay.Way.MaxSpeedForward() > 0 {
+				currentMaxSpeed = state.CurrentWay.Way.MaxSpeedForward()
+			} else if !state.CurrentWay.OnWay.IsForward && state.CurrentWay.Way.MaxSpeedBackward() > 0 {
+				currentMaxSpeed = state.CurrentWay.Way.MaxSpeedBackward()
 			}
+
+			data, err = json.Marshal(currentMaxSpeed)
+			logde(errors.Wrap(err, "could not marshal speed limit"))
+			err = PutParam(MAP_SPEED_LIMIT, data)
+			logwe(errors.Wrap(err, "could not write speed limit"))
+
+			nextSpeedLimit := calculateNextSpeedLimit(state, currentMaxSpeed)
+			data, err = json.Marshal(nextSpeedLimit)
+			logde(errors.Wrap(err, "could not marshal next speed limit"))
+			err = PutParam(NEXT_MAP_SPEED_LIMIT, data)
+			logwe(errors.Wrap(err, "could not write next speed limit"))
+		} else {
+			data, err = json.Marshal(NextSpeedLimit{})
+			logde(errors.Wrap(err, "could not marshal next speed limit"))
+			err = PutParam(NEXT_MAP_SPEED_LIMIT, data)
+			logwe(errors.Wrap(err, "could not write next speed limit"))
 		}
-		data, err = json.Marshal(NextSpeedLimit{
-			Latitude:   nextSpeedWay.StartPosition.Latitude(),
-			Longitude:  nextSpeedWay.StartPosition.Longitude(),
-			Speedlimit: nextSpeedWay.Way.MaxSpeed(),
-		})
-		logde(errors.Wrap(err, "could not marshal next speed limit"))
-		err = PutParam(NEXT_MAP_SPEED_LIMIT, data)
-		logwe(errors.Wrap(err, "could not write next speed limit"))
-	} else {
-		data, err = json.Marshal(NextSpeedLimit{})
-		logde(errors.Wrap(err, "could not marshal next speed limit"))
-		err = PutParam(NEXT_MAP_SPEED_LIMIT, data)
-		logwe(errors.Wrap(err, "could not write next speed limit"))
-	}
 
-	if len(state.NextWays) > 0 {
-		currentAdvisorySpeed := state.CurrentWay.Way.AdvisorySpeed()
-		nextAdvisorySpeed := currentAdvisorySpeed
-		nextAdvisoryWay := state.NextWays[0]
+		if len(state.NextWays) > 0 {
+			currentAdvisorySpeed := state.CurrentWay.Way.AdvisorySpeed()
+			nextAdvisorySpeed := currentAdvisorySpeed
+			nextAdvisoryWay := state.NextWays[0]
 
-		for _, nextWay := range state.NextWays {
-			if nextAdvisorySpeed == currentAdvisorySpeed {
-				nextAdvisoryWay = nextWay
-				nextAdvisorySpeed = nextWay.Way.AdvisorySpeed()
+			for _, nextWay := range state.NextWays {
+				if nextAdvisorySpeed == currentAdvisorySpeed {
+					nextAdvisoryWay = nextWay
+					nextAdvisorySpeed = nextWay.Way.AdvisorySpeed()
+				}
 			}
+			data, err = json.Marshal(AdvisoryLimit{
+				StartLatitude:  nextAdvisoryWay.StartPosition.Latitude(),
+				StartLongitude: nextAdvisoryWay.StartPosition.Longitude(),
+				EndLatitude:    nextAdvisoryWay.EndPosition.Latitude(),
+				EndLongitude:   nextAdvisoryWay.EndPosition.Longitude(),
+				Speedlimit:     nextAdvisoryWay.Way.AdvisorySpeed(),
+			})
+			logde(errors.Wrap(err, "could not marshal next advisory speed limit"))
+			err = PutParam(NEXT_MAP_ADVISORY_LIMIT, data)
+			logwe(errors.Wrap(err, "could not write next advisory speed limit"))
+		} else {
+			data, err = json.Marshal(AdvisoryLimit{})
+			logde(errors.Wrap(err, "could not marshal next advisory speed limit"))
+			err = PutParam(NEXT_MAP_ADVISORY_LIMIT, data)
+			logwe(errors.Wrap(err, "could not write next advisory speed limit"))
 		}
-		data, err = json.Marshal(AdvisoryLimit{
-			StartLatitude:  nextAdvisoryWay.StartPosition.Latitude(),
-			StartLongitude: nextAdvisoryWay.StartPosition.Longitude(),
-			EndLatitude:    nextAdvisoryWay.EndPosition.Latitude(),
-			EndLongitude:   nextAdvisoryWay.EndPosition.Longitude(),
-			Speedlimit:     nextAdvisoryWay.Way.AdvisorySpeed(),
-		})
-		logde(errors.Wrap(err, "could not marshal next advisory speed limit"))
-		err = PutParam(NEXT_MAP_ADVISORY_LIMIT, data)
-		logwe(errors.Wrap(err, "could not write next advisory speed limit"))
-	} else {
-		data, err = json.Marshal(AdvisoryLimit{})
-		logde(errors.Wrap(err, "could not marshal next advisory speed limit"))
-		err = PutParam(NEXT_MAP_ADVISORY_LIMIT, data)
-		logwe(errors.Wrap(err, "could not write next advisory speed limit"))
+	}
+
+	// Log current status for debugging
+	if state.CurrentWay.Way.HasNodes() {
+		log.Debug().
+			Str("road", RoadName(state.CurrentWay.Way)).
+			Float64("distance", state.CurrentWay.Distance.Distance).
+			Int("confidence", state.CurrentWay.ConfidenceCounter).
+			Int("stable_counter", state.StableWayCounter).
+			Float64("stable_distance", state.CurrentWay.StableDistance).
+			Msg("Current way status")
 	}
 }
 
@@ -321,11 +464,16 @@ func main() {
 	}
 	EnsureParamDirectories()
 	ResetParams()
-	state := State{}
+
+	state := State{
+		LastGPSUpdate: time.Now(),
+		LastWayChange: time.Now(),
+	}
 
 	pos, err := readPosition(true)
 	logde(err)
 	if err == nil {
+		state.Position = pos
 		state.Data, err = FindWaysAroundLocation(pos.Latitude, pos.Longitude)
 		logde(errors.Wrap(err, "could not find ways around initial location"))
 	}
@@ -340,6 +488,7 @@ func main() {
 		}
 	}
 
+	log.Info().Msg("Starting main loop with stability improvements and distance smoothing")
 	for {
 		loop(&state)
 	}
diff --git a/way.go b/way.go
index 285881f..230968d 100644
--- a/way.go
+++ b/way.go
@@ -3,6 +3,7 @@ package main
 import (
 	"math"
 	"strings"
+	"time"
 
 	"github.com/pkg/errors"
 )
@@ -29,12 +30,66 @@ var LANE_COUNT_PRIORITY = map[uint8]int{
 	0: 30, // Unknown
 }
 
+// Highway hierarchy ranking
+var HIGHWAY_RANK = map[string]int{
+	"motorway":       0,
+	"motorway_link":  1,
+	"trunk":          10,
+	"trunk_link":     11,
+	"primary":        20,
+	"primary_link":   21,
+	"secondary":      30,
+	"secondary_link": 31,
+	"tertiary":       40,
+	"tertiary_link":  41,
+	"unclassified":   50,
+	"residential":    60,
+	"living_street":  61,
+}
+
+// Bearing alignment thresholds
+const ACCEPTABLE_BEARING_DELTA_SIN = 0.7071067811865475 // sin(45°) - max acceptable bearing mismatch
+
 type OnWayResult struct {
 	OnWay     bool
 	Distance  DistanceResult
 	IsForward bool
 }
 
+type WayCandidate struct {
+	Way              Way
+	OnWayResult      OnWayResult
+	BearingAlignment float64 // sin(bearing_delta) - lower is better
+	DistanceToWay    float64
+	HierarchyRank    int
+	Context          RoadContext
+}
+
+type DistanceResult struct {
+	LineStart Coordinates
+	LineEnd   Coordinates
+	Distance  float64
+}
+
+// Updated CurrentWay struct with stability fields
+type CurrentWay struct {
+	Way               Way
+	Distance          DistanceResult
+	OnWay             OnWayResult
+	StartPosition     Coordinates
+	EndPosition       Coordinates
+	ConfidenceCounter int
+	LastChangeTime    time.Time
+	StableDistance    float64
+}
+
+type NextWayResult struct {
+	Way           Way
+	IsForward     bool
+	StartPosition Coordinates
+	EndPosition   Coordinates
+}
+
 func OnWay(way Way, pos Position, extended bool) (OnWayResult, error) {
 	res := OnWayResult{}
 	if pos.Latitude < way.MaxLat()+PADDING && pos.Latitude > way.MinLat()-PADDING && pos.Longitude < way.MaxLon()+PADDING && pos.Longitude > way.MinLon()-PADDING {
@@ -120,6 +175,117 @@ func isFreeway(way Way) bool {
 	return false
 }
 
+// Get highway hierarchy rank for a way
+func getHighwayRank(way Way) int {
+	name, _ := way.Name()
+	ref, _ := way.Ref()
+	lanes := way.Lanes()
+
+	// Infer highway type from characteristics
+	if isFreeway(way) {
+		if lanes >= 6 {
+			return HIGHWAY_RANK["motorway"]
+		}
+		return HIGHWAY_RANK["trunk"]
+	}
+
+	nameUpper := strings.ToUpper(name)
+	refUpper := strings.ToUpper(ref)
+
+	// Primary roads (usually have ref numbers)
+	if len(ref) > 0 && !strings.Contains(nameUpper, "STREET") {
+		if strings.HasPrefix(refUpper, "US-") || strings.HasPrefix(refUpper, "SR-") {
+			return HIGHWAY_RANK["primary"]
+		}
+		return HIGHWAY_RANK["secondary"]
+	}
+
+	// Local roads
+	if strings.Contains(nameUpper, "STREET") ||
+		strings.Contains(nameUpper, "AVENUE") ||
+		strings.Contains(nameUpper, "ROAD") {
+		return HIGHWAY_RANK["residential"]
+	}
+
+	// Default to unclassified
+	return HIGHWAY_RANK["unclassified"]
+}
+
+func calculateBearingAlignment(way Way, pos Position) (float64, error) {
+	d, err := DistanceToWay(pos, way)
+	if err != nil {
+		return 1.0, err
+	}
+
+	startLat := d.LineStart.Latitude()
+	startLon := d.LineStart.Longitude()
+	endLat := d.LineEnd.Latitude()
+	endLon := d.LineEnd.Longitude()
+
+	wayBearing := Bearing(startLat, startLon, endLat, endLon)
+
+	// Calculate bearing delta
+	delta := math.Abs(pos.Bearing*TO_RADIANS - wayBearing)
+
+	// Normalize to 0-π range
+	if delta > math.Pi {
+		delta = 2*math.Pi - delta
+	}
+	return math.Sin(delta), nil
+}
+
+func selectBestWayAdvanced(possibleWays []Way, pos Position, currentWay Way, context RoadContext, gpsAccuracy float64) Way {
+	if len(possibleWays) == 0 {
+		return Way{}
+	}
+	if len(possibleWays) == 1 {
+		return possibleWays[0]
+	}
+
+	bestWay := possibleWays[0]
+	bestScore := float64(-1000)
+
+	for _, way := range possibleWays {
+		onWay, err := OnWay(way, pos, false)
+		if err != nil || !onWay.OnWay {
+			continue
+		}
+
+		score := float64(0)
+
+		hierarchyRank := getHighwayRank(way)
+		score += float64(100 - hierarchyRank)
+
+		bearingAlignment, err := calculateBearingAlignment(way, pos)
+		if err == nil {
+			score += (1.0 - bearingAlignment) * 50
+		}
+
+		score -= onWay.Distance.Distance * 0.1
+
+		if currentWay.HasNodes() {
+			currentName, _ := currentWay.Name()
+			currentRef, _ := currentWay.Ref()
+			wayName, _ := way.Name()
+			wayRef, _ := way.Ref()
+
+			if len(currentName) > 0 && currentName == wayName {
+				score += 30.0
+			}
+			if len(currentRef) > 0 && currentRef == wayRef {
+				score += 25.0
+			}
+		}
+
+		if score > bestScore {
+			bestScore = score
+			bestWay = way
+		}
+	}
+
+	return bestWay
+}
+
 func getRoadPriority(way Way, context RoadContext) int {
 	lanes := way.Lanes()
 	name, _ := way.Name()
@@ -143,12 +309,18 @@ func getRoadPriority(way Way, context RoadContext) int {
 
 	case CONTEXT_CITY:
 		if isFreeway(way) {
-			priority += 15
+			priority += 10
 		}
 		nameUpper := strings.ToUpper(name)
 		if strings.Contains(nameUpper, "SERVICE") {
 			priority -= 5
 		}
+		// Boost local street names in city context
+		if strings.Contains(nameUpper, "STREET") ||
+			strings.Contains(nameUpper, "AVENUE") ||
+			strings.Contains(nameUpper, "ROAD") {
+			priority += 5
+		}
 
 	case CONTEXT_UNKNOWN:
 		if isFreeway(way) {
@@ -163,12 +335,6 @@ func getRoadPriority(way Way, context RoadContext) int {
 	return priority
 }
 
-type DistanceResult struct {
-	LineStart Coordinates
-	LineEnd   Coordinates
-	Distance  float64
-}
-
 func DistanceToWay(pos Position, way Way) (DistanceResult, error) {
 	res := DistanceResult{}
 	var minNodeStart Coordinates
@@ -201,14 +367,6 @@ func DistanceToWay(pos Position, way Way) (DistanceResult, error) {
 	return res, nil
 }
 
-type CurrentWay struct {
-	Way           Way
-	Distance      DistanceResult
-	OnWay         OnWayResult
-	StartPosition Coordinates
-	EndPosition   Coordinates
-}
-
 func GetWayStartEnd(way Way, isForward bool) (Coordinates, Coordinates) {
 	if !way.HasNodes() {
 		return Coordinates{}, Coordinates{}
@@ -234,7 +392,7 @@ func GetWayStartEnd(way Way, isForward bool) (Coordinates, Coordinates) {
 	return nodes.At(nodes.Len() - 1), nodes.At(0)
 }
 
-func GetCurrentWay(currentWay CurrentWay, nextWays []NextWayResult, offline Offline, pos Position) (CurrentWay, error) {
+func GetCurrentWay(currentWay CurrentWay, nextWays []NextWayResult, offline Offline, pos Position, lastPos Position, gpsAccuracy float64) (CurrentWay, error) {
 	currentContext := CONTEXT_UNKNOWN
 	if currentWay.Way.HasNodes() {
 		currentContext = determineRoadContext(currentWay.Way, pos)
@@ -242,83 +400,83 @@ func GetCurrentWay(currentWay CurrentWay, nextWays []NextWayResult, offline Offl
 
 	if currentWay.Way.HasNodes() {
 		onWay, err := OnWay(currentWay.Way, pos, false)
-		logde(errors.Wrap(err, "could not check if on current way"))
-		if onWay.OnWay {
+		if err == nil && onWay.OnWay {
 			stickThreshold := 15.0
 			if currentContext == CONTEXT_FREEWAY {
-				stickThreshold = 25.0
+				stickThreshold = 20.0
 			} else if currentContext == CONTEXT_CITY {
 				stickThreshold = 10.0
 			}
 
-			possibleWays, err := getPossibleWays(offline, pos)
-			if err == nil && len(possibleWays) > 1 {
-				if onWay.Distance.Distance < stickThreshold {
-					start, end := GetWayStartEnd(currentWay.Way, onWay.IsForward)
-					return CurrentWay{
-						Way:           currentWay.Way,
-						Distance:      onWay.Distance,
-						OnWay:         onWay,
-						StartPosition: start,
-						EndPosition:   end,
-					}, nil
-				}
-			} else {
+			if onWay.Distance.Distance < stickThreshold {
+				newStableDistance := onWay.Distance.Distance
+
 				start, end := GetWayStartEnd(currentWay.Way, onWay.IsForward)
 				return CurrentWay{
-					Way:           currentWay.Way,
-					Distance:      onWay.Distance,
-					OnWay:         onWay,
-					StartPosition: start,
-					EndPosition:   end,
+					Way:               currentWay.Way,
+					Distance:          onWay.Distance,
+					OnWay:             onWay,
+					StartPosition:     start,
+					EndPosition:       end,
+					ConfidenceCounter: currentWay.ConfidenceCounter + 1,
+					LastChangeTime:    currentWay.LastChangeTime,
+					StableDistance:    newStableDistance,
 				}, nil
 			}
 		}
 	}
 
 	for _, nextWay := range nextWays {
-		onWay, err := OnWay(nextWay.Way, pos, true)
-		logde(errors.Wrap(err, "could not check if on next way"))
-		if onWay.OnWay {
+		onWay, err := OnWay(nextWay.Way, pos, false)
+		if err == nil && onWay.OnWay {
 			start, end := GetWayStartEnd(nextWay.Way, onWay.IsForward)
 			return CurrentWay{
-				Way:           nextWay.Way,
-				Distance:      onWay.Distance,
-				OnWay:         onWay,
-				StartPosition: start,
-				EndPosition:   end,
+				Way:               nextWay.Way,
+				Distance:          onWay.Distance,
+				OnWay:             onWay,
+				StartPosition:     start,
+				EndPosition:       end,
+				ConfidenceCounter: 1,
+				LastChangeTime:    time.Now(),
+				StableDistance:    onWay.Distance.Distance,
 			}, nil
 		}
 	}
 
 	possibleWays, err := getPossibleWays(offline, pos)
-	logde(errors.Wrap(err, "Failed to get possible ways"))
-	if len(possibleWays) > 0 {
-		selectedWay := selectBestWay(possibleWays, pos, currentWay.Way, currentContext)
-		selectedOnWay, err := OnWay(selectedWay, pos, false)
-		logde(errors.Wrap(err, "Could not check if on way"))
-
-		start, end := GetWayStartEnd(selectedWay, selectedOnWay.IsForward)
-		return CurrentWay{
-			Way:           selectedWay,
-			Distance:      selectedOnWay.Distance,
-			OnWay:         selectedOnWay,
-			StartPosition: start,
-			EndPosition:   end,
-		}, nil
+	if err == nil && len(possibleWays) > 0 {
+		selectedWay := selectBestWayAdvanced(possibleWays, pos, currentWay.Way, currentContext, gpsAccuracy)
+		if selectedWay.HasNodes() {
+			selectedOnWay, err := OnWay(selectedWay, pos, false)
+			if err == nil && selectedOnWay.OnWay {
+				start, end := GetWayStartEnd(selectedWay, selectedOnWay.IsForward)
+				return CurrentWay{
+					Way:               selectedWay,
+					Distance:          selectedOnWay.Distance,
+					OnWay:             selectedOnWay,
+					StartPosition:     start,
+					EndPosition:       end,
+					ConfidenceCounter: 1,
+					LastChangeTime:    time.Now(),
+					StableDistance:    selectedOnWay.Distance.Distance,
+				}, nil
+			}
+		}
 	}
 
 	if currentWay.Way.HasNodes() {
 		onWay, err := OnWay(currentWay.Way, pos, true)
-		logde(errors.Wrap(err, "could not extended check if on current way"))
-		if onWay.OnWay {
+		if err == nil && onWay.OnWay {
 			start, end := GetWayStartEnd(currentWay.Way, onWay.IsForward)
 			return CurrentWay{
-				Way:           currentWay.Way,
-				Distance:      onWay.Distance,
-				OnWay:         onWay,
-				StartPosition: start,
-				EndPosition:   end,
+				Way:               currentWay.Way,
+				Distance:          onWay.Distance,
+				OnWay:             onWay,
+				StartPosition:     start,
+				EndPosition:       end,
+				ConfidenceCounter: currentWay.ConfidenceCounter,
+				LastChangeTime:    currentWay.LastChangeTime,
+				StableDistance:    currentWay.StableDistance,
 			}, nil
 		}
 	}
@@ -326,92 +484,6 @@ func GetCurrentWay(currentWay CurrentWay, nextWays []NextWayResult, offline Offl
 	return CurrentWay{}, errors.New("could not find a current way")
 }
 
-func selectBestWay(possibleWays []Way, pos Position, currentWay Way, context RoadContext) Way {
-	if len(possibleWays) == 1 {
-		return possibleWays[0]
-	}
-
-	bestWay := possibleWays[0]
-	bestScore := float64(-1000)
-
-	for _, way := range possibleWays {
-		score := calculateWayScore(way, pos, currentWay, context)
-		if score > bestScore {
-			bestScore = score
-			bestWay = way
-		}
-	}
-
-	return bestWay
-}
-
-func calculateWayScore(way Way, pos Position, currentWay Way, context RoadContext) float64 {
-	score := float64(0)
-
-	priority := getRoadPriority(way, context)
-	score += float64(priority)
-
-	onWay, err := OnWay(way, pos, false)
-	if err == nil {
-		distanceWeight := -2.0
-		if context == CONTEXT_CITY {
-			distanceWeight = -3.0
-		} else if context == CONTEXT_FREEWAY {
-			distanceWeight = -1.5
-		}
-		score += onWay.Distance.Distance * distanceWeight
-	}
-
-	lanes := way.Lanes()
-	if lanes > 0 {
-		laneWeight := 3.0
-		if context == CONTEXT_FREEWAY {
-			laneWeight = 5.0
-		} else if context == CONTEXT_CITY {
-			laneWeight = 2.0
-		}
-		score += float64(lanes) * laneWeight
-	}
-
-	if currentWay.HasNodes() {
-		currentName, _ := currentWay.Name()
-		currentRef, _ := currentWay.Ref()
-		wayName, _ := way.Name()
-		wayRef, _ := way.Ref()
-
-		continuityWeight := 25.0
-		if context == CONTEXT_FREEWAY {
-			continuityWeight = 40.0
-		} else if context == CONTEXT_CITY {
-			continuityWeight = 15.0
-		}
-
-		if len(currentName) > 0 && currentName == wayName {
-			score += continuityWeight
-		}
-		if len(currentRef) > 0 && currentRef == wayRef {
-			score += continuityWeight * 0.8
-		}
-	}
-
-	name, _ := way.Name()
-	nameUpper := strings.ToUpper(name)
-
-	if context == CONTEXT_FREEWAY {
-		if strings.Contains(nameUpper, "SERVICE") ||
-			strings.Contains(nameUpper, "ACCESS") ||
-			strings.Contains(nameUpper, "RAMP") {
-			score -= 30.0
-		}
-	} else if context == CONTEXT_CITY {
-		if strings.Contains(nameUpper, "SERVICE") {
-			score -= 5.0
-		}
-	}
-
-	return score
-}
-
 func getPossibleWays(offline Offline, pos Position) ([]Way, error) {
 	possibleWays := []Way{}
 	ways, err := offline.Ways()
@@ -476,13 +548,6 @@ func MatchingWays(currentWay Way, offline Offline, matchNode Coordinates) ([]Way
 	return matchingWays, nil
 }
 
-type NextWayResult struct {
-	Way           Way
-	IsForward     bool
-	StartPosition Coordinates
-	EndPosition   Coordinates
-}
-
 func NextIsForward(nextWay Way, matchNode Coordinates) bool {
 	if !nextWay.HasNodes() {
 		return true