diff --git a/lib/polar.py b/lib/polar.py
index d604b9f..abdbfb6 100755
--- a/lib/polar.py
+++ b/lib/polar.py
@@ -1,51 +1,51 @@
 #!/usr/bin/env python3
 
 import math, time
 import csv
 
 class Polar :
 	def __init__(self, filename) :
 		reader = csv.reader(open(filename, 'r'))
 		self.dict = {}
 		for row in reader:
 			l = row[0].split(';')
 			key = l[0]
 			value = l[1:]
 			self.dict[key] = value
 
 	def _getI(self, tws) :
 		i = 0
 		prev_speed = 0
 		for speed in self.dict['TWA\\TWS'] :
 			#print (speed)
 			if tws < float(speed) :
 				#print ('Column for TWS {0} ={1}'.format(prev_speed, i-1))
 				return i-1
 			prev_speed = speed
 			i += 1
 		return -1
 
 	def _getJ(self, twa) :
-		twa = int(twa)
+		twa = int(twa/5) * 5 # reduce twa to a 5-multiple
 		if twa > 180 :
 			twa = twa - 2*(twa - 180)
 		twa = str(twa)
 		#print ('Row for TWA {0} : {1}'.format(twa, self.dict[twa]))
 		return self.dict[twa]
 
 	def boatSpeed(self, twa, tws) :
 		twa = math.fabs(twa)
 		i = self._getI(tws)
 		row = self._getJ(twa)
 		return float(row[i])
 
 #--------------------
 #       Code for testing classes
 #--------------------
 
 if __name__ == '__main__':
 
 	p = Polar('boat_figaro2.csv')
 	print ('Boat speed : {0}'.format(p.boatSpeed(45.2, 20.2)))
 
 
diff --git a/lib/router_v1.py b/lib/router_v1.py
index 63e518e..ddde74d 100755
--- a/lib/router_v1.py
+++ b/lib/router_v1.py
@@ -1,170 +1,157 @@
 #!/usr/bin/env python3
 
 import math, time
 from geographiclib.geodesic import Geodesic
 from polar import Polar
 
 """
 --------------------
 This is a very simple router algorithm
  - very simple polar
  - don't handle derive
  - don't handle streams
  - don't converge quickly
 --------------------
 """
 
 
-class xxPolar :
-	def boatSpeed(self, twa, tws) :
-		twa = math.fabs(twa)
-		if twa < 20 :
-			return 0.0
-		elif twa < 35 :
-			return 2.0
-		elif twa < 60 :
-			return 6.0
-		elif twa < 90 :
-			return 7.0
-		elif twa < 120 :
-			return 8.0
-		elif twa < 140 :
-			return 9.0
-		else :
-			return 6.0
-
 class Wind :
 	def __init__(self, dir, speed) :
 		self.dir = dir
 		self.speed = speed
 
 class Boat :
 	def __init__(self, lat, lon, hdg, speed) :
 		# constants
 		self.Time = 10			# iteration time, in seconds
 		self.Precision = 5		# best vmg calculation, in degrees
-		self.Circle = 0.02		# cercle d'arrivée au WP, in Nm
+		self.GotoDirect = 0.05		# minimum distance to WP to go directly, in Nm
+		self.Circle = 0.01		# arrival circle, in Nm
 		self.Hysteresis = 10		# évite les virements de bord trop rapides
+		self.ReduceHysteresis = 0.1	# distance to WP when hysteresis is divided by 2
 		# variables
 		self.lat = lat
 		self.lon = lon
 		self.hdg = hdg
 		self.speed = speed
 		self.twa = 0
 		self.vmg = 0
 		self.wp = None
 		self.wind = None
 		self.polar = None
 		self._iter = 0
 		self.wp_reached = False
 		self.hyst_cnt = 0
 
 	def iter(self) :
 		# calculate heading and distance from WP
 		geo = Geodesic.WGS84.Inverse(self.lat, self.lon, self.wp.lat, self.wp.lon)
 		dist = geo['s12']/1852
 
 		print('Iteration #{0:3d} Time : {1:3d} sec'.format(self._iter, self._iter*self.Time))
 		print('  WP Azimut:{0:.3f} Dist:{1:.3f} Nm'.format(geo['azi1'], dist))
 
 		if dist <= self.Circle :
 			self.wp_reached = True
 			return
 
 		vmg, twa, hdg, speed = bestvmg(self, self.wind, geo['azi1'])
-		if self.hyst_cnt == 0 :
+		if self.hyst_cnt <= 0 :
 			self.vmg = vmg
 			self.twa = twa
 			self.hdg = hdg
 			self.speed = speed
 			self.hyst_cnt = self.Hysteresis
+			if dist <= self.ReduceHysteresis :
+				self.hyst_cnt = self.Hysteresis / 2
 		else :
 			self.hyst_cnt -= 1
 
+		if dist <= self.GotoDirect :
+			print ('*** go direct to WP')
+			self.hdg = geo['azi1']
+			self.vmg,self.twa,self.speed = Vmg(hdg, self.wind, geo['azi1'])
+
 		print('  Heading:{0:.3f}'.format(self.hdg))
 		print('  TWA:{0:.3f}'.format(self.twa))
 		print('  TWS:{0:.3f}'.format(self.wind.speed))
 		print('  boat speed:{0:.3f} Kts'.format(self.speed))
 		print('  VMG:{0:.3f} Kts'.format(self.vmg))
 		print('  ETA:{0:.3f} sec = {1:.3f} h'.format(3600*dist/self.vmg, dist/self.vmg))
 
 		# calculate next boat position
 		step = self.Time*(self.speed*1852)/3600
 		geo = Geodesic.WGS84.Direct(self.lat, self.lon, self.hdg, step)
 		self.lat = geo['lat2']
 		self.lon = geo['lon2']
 		print('  New position : {0:.5f} {1:.5f}'.format(self.lat, self.lon))
 		self._iter += 1
 
 class WP :
 	def __init__(self, lat, lon) :
 		self.lat = lat
 		self.lon = lon
 
 def angleDiff(a1, a2) :
 	d = a1 - a2
 	if d > 180 :
 		d -= 360
 	elif d < -180 :
 		d += 360
 	#print('diff '+str(d))
 	return d
 
 #	sinus degrees
 def sind(deg) :
 	return math.sin(math.radians(deg))
 
 #	cosinus degrees
 def cosd(deg) :
 	return math.cos(math.radians(deg))
 
+def Vmg(hdg, w, azi) :
+	twa = angleDiff(hdg, w.dir)
+	twa = math.fabs(twa)
+	x = angleDiff(hdg, azi)
+	x = math.fabs(x)
+	speed = b.polar.boatSpeed(twa, w.speed)
+	vmg = speed * cosd(x)
+	return vmg,twa,speed
+
 def bestvmg(b, w, azi) :
 	best_vmg = 0
 	best_twa = 0
 	best_hdg = 0
 	best_speed = 0
 	hdg = b.hdg
 	while True :
-		twa = angleDiff(hdg, w.dir)
-		twa = math.fabs(twa)
-		x = angleDiff(hdg, azi)
-		x = math.fabs(x)
-		speed = b.polar.boatSpeed(twa, w.speed)
-		vmg = speed * cosd(x)
+		vmg,twa,speed = Vmg(hdg, w, azi)
 		if twa > 180 :
 			break
 		if vmg > best_vmg :
 			best_vmg = vmg
 			best_twa = twa
 			best_hdg = hdg
 			best_speed = speed
-			"""
-			print('   hdg   '+str(hdg))
-			print('   twa   '+str(twa))
-			print('   angle '+str(x))
-			print('   speed '+str(speed))
-			print('   vmg   '+str(vmg))
-			print('    ****** BEST *****')
-			"""
 		hdg += b.Precision
 
 	if best_hdg > 360 :
 		best_hdg -= 360
 
 	return best_vmg, best_twa, best_hdg, best_speed
 
 
 #--------------------
 #       Code for testing classes
 #--------------------
 
 if __name__ == '__main__':
 
 	b = Boat(43.0, 6.0, 90.0, 5.0)
 	b.polar = Polar('boat_figaro2.csv')
 	b.wind = Wind(90.0, 15.0)
 	b.wp = WP(43.1, 6.1)
 
 	while b.wp_reached == False :
 		b.iter()