Merge remote-tracking branch 'origin/mapbox' into mapbox

2024-07-30 18:01:30 +02:00 · 2024-07-30 18:01:30 +02:00 · 73719bbedf
commit 73719bbedf
parent 3e3f5b2a4b 9a553cf563
15 changed files with 961 additions and 268 deletions
--- a/.gitattributes
+++ b/.gitattributes
@ -1 +1,2 @@
 * text eol=lf
+*.bat text eol=crlf
--- a/.gitignore
+++ b/.gitignore
@ -8,3 +8,4 @@
 .externalNativeBuild
 .cxx
 local.properties
+__pycache__/
--- a/app/build.gradle
+++ b/app/build.gradle
@ -47,6 +47,7 @@ dependencies {
    implementation 'com.google.android.material:material:1.5.0'
    implementation 'androidx.constraintlayout:constraintlayout:2.1.3'
    testImplementation 'org.junit.jupiter:junit-jupiter:5.8.1'
+    testImplementation 'org.assertj:assertj-core:3.24.2'
    androidTestImplementation 'androidx.test.ext:junit:1.1.3'
    androidTestImplementation 'androidx.test.espresso:espresso-core:3.4.0'

--- a/app/src/main/java/eu/ztsh/garmin/Garmin.kt
+++ b/app/src/main/java/eu/ztsh/garmin/Garmin.kt
@ -5,7 +5,12 @@ import android.bluetooth.BluetoothAdapter
 import android.bluetooth.BluetoothDevice
 import android.bluetooth.BluetoothSocket
 import android.util.Log
+import com.mapbox.navigation.core.trip.session.LocationMatcherResult
+import com.mapbox.navigation.core.trip.session.NavigationSessionState
 import com.mapbox.navigation.ui.maneuver.model.Maneuver
+import eu.ztsh.garmin.data.DataCache
+import eu.ztsh.garmin.data.GarminMapper
+import eu.ztsh.garmin.data.MapboxMapper
 import java.io.IOException
 import java.util.*
 import java.util.concurrent.SynchronousQueue
@ -19,7 +24,7 @@ class Garmin(

    private lateinit var connection: ConnectThread
    private lateinit var processing: ProcessingThread
-    private var stateCache: State = State()
+    private val cache = DataCache()

    fun start() {
        connection = ConnectThread()
@ -31,58 +36,55 @@ class Garmin(
    }

    fun process(maneuver: Maneuver) {
-        processing = ProcessingThread(maneuver)
+        processing = ManeuverProcessingThread(maneuver)
        processing.start()
        processing.join()
    }

+    fun process(location: LocationMatcherResult) {
+        processing = LocationProcessingThread(location)
+        processing.start()
+        processing.join()
+    }

+    fun process(navigationSessionState: NavigationSessionState) {
+        cache.update(navigationSessionState)
+    }
+
+    private inner class ManeuverProcessingThread(val maneuver: Maneuver) : ProcessingThread() {

-    private inner class ProcessingThread(val maneuver: Maneuver) : Thread() {
        override fun run() {
-            send(ManeuverMapper.apply(maneuver))
+            if (cache.hasChanged(maneuver)) {
+                cache.update(maneuver)
+                send(MapboxMapper.apply(maneuver))
+            }
        }

-        fun send(incoming: eu.ztsh.garmin.State) {
-            if (stateCache.distance != incoming.distance) {
-                setDistance(incoming)
+    }
+
+    private inner class LocationProcessingThread(val location: LocationMatcherResult) : ProcessingThread() {
+
+        override fun run() {
+            if (cache.hasChanged(location)) {
+                cache.update(location)
+                send(MapboxMapper.apply(location))
            }
-            if (stateCache.direction != incoming.direction) {
-                setDirection(stateCache.direction)
-            }
-            stateCache = incoming
        }

-        private fun setDistance(state: eu.ztsh.garmin.State) {
-            connection.enqueue(intArrayOf(
-                0x03, asDigit(state.distance / 1000), asDigit(state.distance / 100), asDigit(state.distance / 10),
-                0x00, asDigit(state.distance), state.unit.data
-            ))
+    }
+
+    private open inner class ProcessingThread : Thread() {
+
+        fun send(incoming: eu.ztsh.garmin.data.State) {
+            if (cache.hasChanged(incoming.distance)) {
+                connection.enqueue(GarminMapper.setDistance(incoming))
+            }
+            if (cache.hasChanged(incoming.direction)) {
+                connection.enqueue(GarminMapper.setDirection(incoming))
+            }
+            cache.update(incoming)
        }

-        private fun setDirection(direction: Direction) {
-            val param1 = when (direction.outAngle) {
-                OutAngle.LeftDown -> 0x10
-                OutAngle.RightDown -> 0x20
-                else -> direction.outType.data
-            }
-            val param2: Int = if (direction.outType == OutType.RightRoundabout
-                || direction.outType == OutType.LeftRoundabout) {
-                if (direction.roundabout == OutAngle.AsDirection) direction.outAngle.data else direction.roundabout.data
-            } else {
-                0x00
-            }
-            val param3: Int = if (direction.outAngle == OutAngle.LeftDown || direction.outAngle == OutAngle.RightDown) 0x00 else direction.outAngle.data
-            connection.enqueue(intArrayOf(0x01, param1, param2, param3))
-        }
-
-        private fun asDigit(n: Int): Int {
-            if (n == 0) {
-                return 0
-            }
-            val m = n % 10
-            return if (m == 0) 10 else m
-        }
    }

    private inner class ConnectThread : Thread() {
@ -105,10 +107,13 @@ class Garmin(
                sleep(3000)
                readAll()
                while (true) {
-                    val newCurrent = Optional.ofNullable(queue.poll()).orElse(current)
-                    current = newCurrent
+                    val newCurrent = queue.poll()
+                    if (newCurrent == null) {
+                        sleep(500)
+                    } else {
+                        current = newCurrent
+                    }
                    send(current)
-                    sleep(900)
                }
            } catch (e: IOException) {
                Log.d(TAG, "Not connected", e)
--- a/app/src/main/java/eu/ztsh/garmin/MainActivity.kt
+++ b/app/src/main/java/eu/ztsh/garmin/MainActivity.kt
@ -33,71 +33,31 @@ class MainActivity : AppCompatActivity() {
    lateinit var garmin: Garmin

    private lateinit var binding: ActivityMainBinding
-    private val mapboxObserver = MapboxObserver()
-
-    init {
-        lifecycle.addObserver(object : DefaultLifecycleObserver {
-            override fun onResume(owner: LifecycleOwner) {
-                MapboxNavigationApp.attach(owner)
-                MapboxNavigationApp.registerObserver(mapboxObserver)
-            }
-
-            override fun onPause(owner: LifecycleOwner) {
-                MapboxNavigationApp.detach(owner)
-                MapboxNavigationApp.unregisterObserver(mapboxObserver)
-            }
-        })
-    }
+    private val mapboxToolbox = MapboxToolbox(lifecycle, this)

    override fun onCreate(savedInstanceState: Bundle?) {
        super.onCreate(savedInstanceState)
        binding = ActivityMainBinding.inflate(layoutInflater)
        setContentView(binding.root)
-        if (!MapboxNavigationApp.isSetup()) {
-            MapboxNavigationApp.setup {
-                NavigationOptions.Builder(applicationContext)
-                    .accessToken(BuildConfig.MAPBOX_DOWNLOADS_TOKEN)
-                    .build()
-            }
-        }
-        MapboxNavigationApp.current()?.startTripSession()
+        mapboxToolbox.onCreate()
        bluetoothInit()
    }

    override fun onStart() {
        super.onStart()
-        MapboxNavigationApp.current()?.registerRouteProgressObserver(routeProgressObserver)
+        mapboxToolbox.onStart()
    }

    override fun onStop() {
        super.onStop()
-        MapboxNavigationApp.current()?.unregisterRouteProgressObserver(routeProgressObserver)
+        mapboxToolbox.onStop()
    }

    override fun onDestroy() {
        super.onDestroy()
-        MapboxNavigationApp.current()?.stopTripSession()
-        maneuverApi.cancel()
+        mapboxToolbox.onDestroy()
    }

-    // Define distance formatter options
-    private val distanceFormatter: DistanceFormatterOptions by lazy {
-        DistanceFormatterOptions.Builder(this).build()
-    }
-    // Create an instance of the Maneuver API
-    private val maneuverApi: MapboxManeuverApi by lazy {
-        MapboxManeuverApi(MapboxDistanceFormatter(distanceFormatter))
-    }
-
-    private val routeProgressObserver =
-        RouteProgressObserver { routeProgress ->
-            maneuverApi.getManeuvers(routeProgress).value?.apply {
-                garmin.process(
-                    this[0]
-                )
-            }
-        }
-
    private fun bluetoothInit() {
        val bluetoothManager: BluetoothManager = getSystemService(BluetoothManager::class.java)
        val bluetoothAdapter: BluetoothAdapter = bluetoothManager.adapter
--- a/app/src/main/java/eu/ztsh/garmin/MapboxToolbox.kt
+++ b/app/src/main/java/eu/ztsh/garmin/MapboxToolbox.kt
@ -0,0 +1,96 @@
+package eu.ztsh.garmin
+
+import android.annotation.SuppressLint
+import android.location.Location
+import android.util.Log
+import androidx.lifecycle.DefaultLifecycleObserver
+import androidx.lifecycle.Lifecycle
+import androidx.lifecycle.LifecycleOwner
+import com.mapbox.navigation.base.formatter.DistanceFormatterOptions
+import com.mapbox.navigation.base.options.NavigationOptions
+import com.mapbox.navigation.core.formatter.MapboxDistanceFormatter
+import com.mapbox.navigation.core.lifecycle.MapboxNavigationApp
+import com.mapbox.navigation.core.trip.session.LocationMatcherResult
+import com.mapbox.navigation.core.trip.session.LocationObserver
+import com.mapbox.navigation.core.trip.session.NavigationSessionState
+import com.mapbox.navigation.core.trip.session.NavigationSessionStateObserver
+import com.mapbox.navigation.core.trip.session.RouteProgressObserver
+import com.mapbox.navigation.ui.maneuver.api.MapboxManeuverApi
+
+@SuppressLint("MissingPermission")
+class MapboxToolbox(lifecycle: Lifecycle, private val context: MainActivity) {
+
+    private val mapboxObserver = MapboxObserver()
+
+    init {
+        lifecycle.addObserver(object : DefaultLifecycleObserver {
+            override fun onResume(owner: LifecycleOwner) {
+                MapboxNavigationApp.attach(owner)
+                MapboxNavigationApp.registerObserver(mapboxObserver)
+            }
+
+            override fun onPause(owner: LifecycleOwner) {
+                MapboxNavigationApp.detach(owner)
+                MapboxNavigationApp.unregisterObserver(mapboxObserver)
+            }
+        })
+    }
+
+    fun onCreate() {
+        if (!MapboxNavigationApp.isSetup()) {
+            MapboxNavigationApp.setup {
+                NavigationOptions.Builder(context)
+                    .accessToken(BuildConfig.MAPBOX_DOWNLOADS_TOKEN)
+                    .build()
+            }
+        }
+        MapboxNavigationApp.current()?.startTripSession()
+    }
+
+    fun onStart() {
+        MapboxNavigationApp.current()?.registerRouteProgressObserver(routeProgressObserver)
+        MapboxNavigationApp.current()?.registerLocationObserver(locationObserver)
+        MapboxNavigationApp.current()?.registerNavigationSessionStateObserver(navigationStateObserver)
+    }
+
+    fun onStop() {
+        MapboxNavigationApp.current()?.unregisterRouteProgressObserver(routeProgressObserver)
+        MapboxNavigationApp.current()?.unregisterLocationObserver(locationObserver)
+        MapboxNavigationApp.current()?.unregisterNavigationSessionStateObserver(navigationStateObserver)
+    }
+
+    fun onDestroy() {
+        MapboxNavigationApp.current()?.stopTripSession()
+        maneuverApi.cancel()
+    }
+
+    // Define distance formatter options
+    private val distanceFormatter: DistanceFormatterOptions by lazy {
+        DistanceFormatterOptions.Builder(context).build()
+    }
+    // Create an instance of the Maneuver API
+    private val maneuverApi: MapboxManeuverApi by lazy {
+        MapboxManeuverApi(MapboxDistanceFormatter(distanceFormatter))
+    }
+
+    private val routeProgressObserver =
+        RouteProgressObserver { routeProgress ->
+            maneuverApi.getManeuvers(routeProgress).value?.apply {
+                context.garmin.process(
+                    this[0]
+                )
+            }
+        }
+
+    private val locationObserver = object : LocationObserver {
+        override fun onNewLocationMatcherResult(locationMatcherResult: LocationMatcherResult) {
+            context.garmin.process(locationMatcherResult)
+        }
+
+        override fun onNewRawLocation(rawLocation: Location) {
+        }
+    }
+
+    private val navigationStateObserver = NavigationSessionStateObserver { context.garmin.process(it) }
+
+}
--- a/app/src/main/java/eu/ztsh/garmin/Model.kt
+++ b/app/src/main/java/eu/ztsh/garmin/Model.kt
@ -1,96 +0,0 @@
-package eu.ztsh.garmin
-
-enum class OutType(val data: Int) {
-
-    Off(0x00),
-    Lane(0x01),
-    LongerLane(0x02),
-    LeftRoundabout(0x04),
-    RightRoundabout(0x08),
-    Flag(0x40),
-    ArrowOnly(0x80);
-
-}
-
-
-enum class OutAngle(val data: Int) {
-
-    Down(0x01),
-    SharpRight(0x02),
-    Right(0x04),
-    EasyRight(0x08),
-    Straight(0x10),
-    EasyLeft(0x20),
-    Left(0x40),
-    SharpLeft(0x80),
-    LeftDown(0x81),
-    RightDown(0x82),
-    AsDirection(0x00)
-
-}
-
-enum class Unit(val data: Int) {
-
-    Any(0),
-    Metres(1),
-    Kilometres(3),
-    Miles(5),
-    Foot(8)
-
-}
-
-enum class Lane(val data: Int) {
-
-    DotsRight(0x01),
-    OuterRight(0x02),
-    MiddleRight(0x04),
-    InnerRight(0x08),
-    InnerLeft(0x10),
-    MiddleLeft(0x20),
-    OuterLeft(0x40),
-    DotsLeft(0x80)
-
-}
-
-class State {
-
-    var lineArrows: Int = 0
-    var lineOutlines: Int = 0
-    var direction = Direction()
-    var distance: Int = 0
-    var unit: Unit = Unit.Any
-    var speed: Int = 0
-    var limit: Int = 0
-    var hours: Int = 0
-    var minutes: Int = 0
-    var traffic: Boolean = false
-    var flag: Boolean = false
-    var control: Boolean = false
-
-}
-
-class Direction {
-    var outAngle: OutAngle = OutAngle.AsDirection
-    var outType: OutType = OutType.Lane
-    var roundabout: OutAngle = OutAngle.AsDirection
-    override fun equals(other: Any?): Boolean {
-        if (this === other) return true
-        if (javaClass != other?.javaClass) return false
-
-        other as Direction
-
-        if (outAngle != other.outAngle) return false
-        if (outType != other.outType) return false
-        if (roundabout != other.roundabout) return false
-
-        return true
-    }
-
-    override fun hashCode(): Int {
-        var result = outAngle.hashCode()
-        result = 31 * result + outType.hashCode()
-        result = 31 * result + roundabout.hashCode()
-        return result
-    }
-
-}
--- a/app/src/main/java/eu/ztsh/garmin/data/DataCache.kt
+++ b/app/src/main/java/eu/ztsh/garmin/data/DataCache.kt
@ -0,0 +1,88 @@
+package eu.ztsh.garmin.data
+
+import com.mapbox.navigation.core.trip.session.LocationMatcherResult
+import com.mapbox.navigation.core.trip.session.NavigationSessionState
+import com.mapbox.navigation.ui.maneuver.model.Maneuver
+
+class DataCache {
+
+    private val stateCache: State = State()
+    private var maneuverCache: Maneuver? = null
+    private var locationCache: LocationMatcherResult? = null
+    private var session: NavigationSessionState? = null
+
+    // state
+    fun hasChanged(lanes: Lanes?): Boolean {
+        return stateCache.lineArrows == null || stateCache.lineArrows != lanes
+    }
+
+    fun hasChanged(outlines: Outlines?): Boolean {
+        return stateCache.lineOutlines == null || stateCache.lineOutlines != outlines
+    }
+
+    fun hasChanged(distance: Distance?): Boolean {
+        return stateCache.distance == null || stateCache.distance != distance
+    }
+
+    fun hasChanged(direction: Direction?): Boolean {
+        return stateCache.direction == null || stateCache.direction != direction
+    }
+
+    fun hasChanged(speed: Speed?): Boolean {
+        return stateCache.speed == null || stateCache.speed != speed
+    }
+
+    fun hasChanged(arrival: Arrival?): Boolean {
+        return stateCache.arrival == null || stateCache.arrival != arrival
+    }
+
+    // Merge states
+    fun update(state: State) {
+        if (state.lineArrows != null) {
+            stateCache.lineArrows = state.lineArrows
+        }
+        if (state.lineOutlines != null) {
+            state.lineOutlines = state.lineOutlines
+        }
+        if (state.direction != null) {
+            stateCache.direction = state.direction
+        }
+        if (state.distance != null) {
+            stateCache.distance = state.distance
+        }
+        if (state.speed != null) {
+            stateCache.speed = state.speed
+        }
+        if (state.arrival != null) {
+            stateCache.arrival = state.arrival
+        }
+    }
+
+    // maneuver
+    fun hasChanged(maneuver: Maneuver): Boolean {
+        return maneuverCache == null || maneuverCache!! != maneuver
+    }
+
+    fun update(maneuver: Maneuver) {
+        maneuverCache = maneuver
+    }
+
+    // location
+    fun hasChanged(locationMatcherResult: LocationMatcherResult): Boolean {
+        return locationCache == null || locationCache!! != locationMatcherResult
+    }
+
+    fun update(locationMatcherResult: LocationMatcherResult) {
+        locationCache = locationMatcherResult
+    }
+
+    // session
+    fun isActive(): Boolean {
+        return session != null && session is NavigationSessionState.ActiveGuidance
+    }
+
+    fun update(sessionState: NavigationSessionState) {
+        session = sessionState
+    }
+
+}
--- a/app/src/main/java/eu/ztsh/garmin/data/GarminMapper.kt
+++ b/app/src/main/java/eu/ztsh/garmin/data/GarminMapper.kt
@ -0,0 +1,151 @@
+package eu.ztsh.garmin.data
+
+class GarminMapper {
+
+    companion object {
+
+        fun setLines(state: State): IntArray {
+            return intArrayOf(0x02, state.lineOutlines.sumOf { it.value }, state.lineArrows.sumOf { it.value })
+        }
+
+        fun setDirection(state: State): IntArray {
+            return setDirection(state.direction.angle, state.direction.out, state.direction.roundabout)
+        }
+
+        fun setDistance(state: State): IntArray {
+            return setDistance(state.distance, state.unit)
+        }
+
+        fun setSpeed(state: State): IntArray {
+            return setSpeed(state.speed, state.limit, state.speed > state.limit)
+        }
+
+        fun setSpeedFreeRide(state: State): Pair<IntArray, IntArray> {
+            return Pair(setDistance(state.speed), setSpeed(state.limit, limitWarning = state.speed > state.limit))
+        }
+
+        fun setTime(hours: Int, minutes: Int, traffic: Boolean = false, flag: Boolean = false): IntArray {
+            val trafficChar = asChar(traffic)
+            val flagChar = asChar(flag)
+            return if (hours > 99) {
+                intArrayOf(
+                    0x05,
+                    trafficChar,
+                    asDigit(hours / 1000),
+                    asDigit(hours / 100),
+                    0x00,
+                    asDigit(hours / 10),
+                    asDigit(hours),
+                    0xff,
+                    flagChar
+                )
+            } else {
+                intArrayOf(
+                    0x05,
+                    trafficChar,
+                    asDigit(hours / 10),
+                    asDigit(hours),
+                    0xff,
+                    asDigit(minutes / 10),
+                    asDigit(minutes),
+                    0x00,
+                    flagChar
+                )
+            }
+        }
+
+        fun setSpeedControl(state: State): IntArray {
+            return intArrayOf(0x04, if (state.control) 0x01 else 0x02)
+        }
+
+        fun setCompass(state: State): IntArray {
+            // TODO: Implement
+            return setDirection(OutAngle.Straight, OutType.ArrowOnly)
+        }
+
+        fun cleanDistance(): IntArray {
+            return intArrayOf(0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00)
+        }
+
+        private fun setDirection(
+            angle: OutAngle,
+            out: OutType = OutType.Lane,
+            roundabout: OutAngle = OutAngle.AsDirection
+        ): IntArray {
+            val param1: Int = when (angle) {
+                OutAngle.LeftDown -> 0x10
+                OutAngle.RightDown -> 0x20
+                else -> out.value
+            }
+            val param2: Int = if (out == OutType.RightRoundabout
+                || out == OutType.LeftRoundabout
+            ) {
+                if (roundabout == OutAngle.AsDirection) angle.value else roundabout.value
+            } else {
+                0x00
+            }
+            val param3: Int =
+                if (angle == OutAngle.LeftDown || angle == OutAngle.RightDown) 0x00 else angle.value
+            return intArrayOf(0x01, param1, param2, param3)
+        }
+
+        private fun setDistance(distance: Int, unit: Unit = Unit.Any): IntArray {
+            return intArrayOf(
+                0x03, asDigit(distance / 1000), asDigit(distance / 100), asDigit(distance / 10),
+                0x00, asDigit(distance), unit.value
+            )
+        }
+
+        private fun setSpeed(
+            speed: Int,
+            limit: Int = 0,
+            limitWarning: Boolean = false,
+            acc: Boolean = false
+        ): IntArray {
+            // TODO: car connection
+            val accChar = asChar(acc)
+            val limitWarningChar = asChar(limitWarning)
+            return if (limit > 0) {
+                intArrayOf(
+                    0x06,
+                    asDigit(speed / 100),
+                    asDigit(speed / 10),
+                    asDigit(speed),
+                    0xff,
+                    asDigit(limit / 100),
+                    asDigit(limit / 10),
+                    asDigit(limit),
+                    limitWarningChar,
+                    accChar
+                )
+            } else {
+                intArrayOf(
+                    0x06,
+                    0x00,
+                    0x00,
+                    0x00,
+                    0x00,
+                    asDigit(speed / 100),
+                    asDigit(speed / 10),
+                    asDigit(speed),
+                    limitWarningChar,
+                    accChar
+                )
+            }
+        }
+
+        private fun asDigit(n: Int): Int {
+            if (n == 0) {
+                return 0
+            }
+            val m = n % 10
+            return if (m == 0) 10 else m
+        }
+
+        private fun asChar(boolean: Boolean): Int {
+            return if (boolean) 0xff else 0x00
+        }
+
+    }
+
+}
--- a/app/src/main/java/eu/ztsh/garmin/data/MapboxMapper.kt
+++ b/app/src/main/java/eu/ztsh/garmin/data/MapboxMapper.kt
@ -1,8 +1,9 @@
-package eu.ztsh.garmin
+package eu.ztsh.garmin.data

+import com.mapbox.navigation.core.trip.session.LocationMatcherResult
 import com.mapbox.navigation.ui.maneuver.model.Maneuver

-class ManeuverMapper {
+class MapboxMapper {

    companion object {

@ -12,13 +13,8 @@ class ManeuverMapper {
                this.primary.apply {
                    state.direction = Direction()
                    when (this.type) {
-                        "turn" -> {
-                            state.direction.outType = OutType.Lane
-                            state.direction.roundabout = OutAngle.AsDirection
-                        }
-
                        "roundabout" -> {
-                            state.direction.outType = OutType.RightRoundabout
+                            state.direction!!.out = OutType.RightRoundabout
                        }

                        "arrive" -> {
@ -28,11 +24,11 @@ class ManeuverMapper {
                    when (this.modifier) {
                        "right" -> {
                            when (this.type) {
-                                "turn" -> state.direction.outAngle = OutAngle.Right
+                                "turn" -> state.direction!!.angle = OutAngle.Right
                                "roundabout" -> {
                                    when (this.degrees) {
-                                        137.0 -> state.direction.outAngle = OutAngle.EasyRight
-                                        180.0 -> state.direction.outAngle = OutAngle.Straight
+                                        137.0 -> state.direction!!.angle = OutAngle.EasyRight
+                                        180.0 -> state.direction!!.angle = OutAngle.Straight
                                    }
                                }
                            }
@ -40,7 +36,7 @@ class ManeuverMapper {

                        "left" -> {
                            when (this.type) {
-                                "turn" -> state.direction.outAngle = OutAngle.Left
+                                "turn" -> state.direction!!.angle = OutAngle.Left
                            }
                        }
                    }
@ -48,15 +44,14 @@ class ManeuverMapper {
                this.stepDistance.apply {
                    this.distanceRemaining?.apply {
                        distanceFormatter.formatDistance(distanceRemaining!!).split(" ").apply {
-                            // TODO: Send double
-                            state.distance = this[0].replace(',', '.').toDouble().toInt()
-                            state.unit = when (this[1]) {
-                                "m" -> Unit.Metres
-                                "km" -> Unit.Kilometres
-                                else -> {
-                                    Unit.Any
+                            state.distance = Distance(
+                                this[0].replace(',', '.').toDouble().toInt(),
+                                when (this[1]) {
+                                    "m" -> Unit.Metres
+                                    "km" -> Unit.Kilometres
+                                    else -> Unit.Any
                                }
-                            }
+                            )

                        }
                    }
@ -70,6 +65,12 @@ class ManeuverMapper {
            return state
        }

+        fun apply(locationMatcherResult: LocationMatcherResult): State {
+            val state = State()
+            // TODO: speed, limit, location?, bearing
+            return state
+        }
+
    }

 }
--- a/app/src/main/java/eu/ztsh/garmin/data/Model.kt
+++ b/app/src/main/java/eu/ztsh/garmin/data/Model.kt
@ -0,0 +1,141 @@
+package eu.ztsh.garmin.data
+
+enum class OutType(val value: Int) {
+
+    Off(0x00),
+    Lane(0x01),
+    LongerLane(0x02),
+    LeftRoundabout(0x04),
+    RightRoundabout(0x08),
+    Flag(0x40),
+    ArrowOnly(0x80);
+
+}
+
+
+enum class OutAngle(val value: Int) {
+
+    Down(0x01),
+    SharpRight(0x02),
+    Right(0x04),
+    EasyRight(0x08),
+    Straight(0x10),
+    EasyLeft(0x20),
+    Left(0x40),
+    SharpLeft(0x80),
+    LeftDown(0x81),
+    RightDown(0x82),
+    AsDirection(0x00)
+
+}
+
+enum class Unit(val value: Int) {
+
+    Any(0),
+    Metres(1),
+    Kilometres(3),
+    Miles(5),
+    Foot(8)
+
+}
+
+enum class Lane(val value: Int) {
+
+    DotsRight(0x01),
+    OuterRight(0x02),
+    MiddleRight(0x04),
+    InnerRight(0x08),
+    InnerLeft(0x10),
+    MiddleLeft(0x20),
+    OuterLeft(0x40),
+    DotsLeft(0x80)
+
+}
+
+open class Arrows(val lanes: List<Lane>) {
+
+    override fun equals(other: Any?): Boolean {
+        if (this === other) return true
+        if (javaClass != other?.javaClass) return false
+
+        other as Arrows
+
+        return lanes == other.lanes
+    }
+
+    override fun hashCode(): Int {
+        return lanes.hashCode()
+    }
+}
+
+class Lanes(lanes: List<Lane>) : Arrows(lanes)
+
+class Outlines(lanes: List<Lane>) : Arrows(lanes)
+
+class Distance(val distance: Int, val unit: Unit) {
+
+    override fun equals(other: Any?): Boolean {
+        if (this === other) return true
+        if (javaClass != other?.javaClass) return false
+
+        other as Distance
+
+        if (distance != other.distance) return false
+        if (unit != other.unit) return false
+
+        return true
+    }
+
+    override fun hashCode(): Int {
+        var result = distance
+        result = 31 * result + unit.hashCode()
+        return result
+    }
+
+}
+
+class Speed(val speed: Int, val limit: Int)
+
+class Arrival(val hours: Int, val minutes: Int)
+
+class State {
+
+    var lineArrows: Lanes? = null
+    var lineOutlines: Outlines? = null
+    var direction : Direction? = null
+    var distance: Distance? = null
+    var speed: Speed? = null
+    var arrival: Arrival? = null
+    // TODO: Bearing
+    // TODO: support
+    var traffic: Boolean? = null
+    var flag: Boolean? = null
+    var control: Boolean? = null
+
+}
+
+class Direction {
+    var angle: OutAngle = OutAngle.AsDirection
+    var out: OutType = OutType.Lane
+    var roundabout: OutAngle = OutAngle.AsDirection
+    override fun equals(other: Any?): Boolean {
+        if (this === other) return true
+        if (javaClass != other?.javaClass) return false
+
+        other as Direction
+
+        if (angle != other.angle) return false
+        if (out != other.out) return false
+        if (roundabout != other.roundabout) return false
+
+        return true
+    }
+
+    override fun hashCode(): Int {
+        var result = angle.hashCode()
+        result = 31 * result + out.hashCode()
+        result = 31 * result + roundabout.hashCode()
+        return result
+    }
+
+}
--- a/app/src/test/java/eu/ztsh/garmin/data/GarminMapperTest.kt
+++ b/app/src/test/java/eu/ztsh/garmin/data/GarminMapperTest.kt
@ -0,0 +1,279 @@
+package eu.ztsh.garmin.data
+
+import eu.ztsh.garmin.Garmin
+import org.assertj.core.api.Assertions.assertThat
+import org.junit.jupiter.api.Test
+
+class GarminMapperTest {
+
+    @Test
+    fun linesTest() {
+        linesTest(
+            listOf(Lane.DotsLeft),
+            listOf(),
+            intArrayOf(2, 128, 0),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 128, 0, 141, 16, 3)
+        )
+        linesTest(
+            Lane.OuterRight,
+            Lane.OuterLeft,
+            intArrayOf(2, 2, 64),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 2, 64, 203, 16, 3)
+        )
+        linesTest(
+            Lane.MiddleRight,
+            Lane.MiddleLeft,
+            intArrayOf(2, 4, 32),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 4, 32, 233, 16, 3)
+        )
+        linesTest(
+            Lane.InnerRight,
+            Lane.InnerLeft,
+            intArrayOf(2, 8, 16),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 8, 16, 16, 245, 16, 3)
+        )
+        linesTest(
+            Lane.InnerLeft,
+            Lane.InnerRight,
+            intArrayOf(2, 16, 8),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 16, 16, 8, 245, 16, 3)
+        )
+        linesTest(
+            Lane.MiddleLeft,
+            Lane.MiddleRight,
+            intArrayOf(2, 32, 4),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 32, 4, 233, 16, 3)
+        )
+        linesTest(
+            Lane.OuterLeft,
+            Lane.OuterRight,
+            intArrayOf(2, 64, 2),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 64, 2, 203, 16, 3)
+        )
+        linesTest(
+            listOf(Lane.DotsRight),
+            listOf(Lane.OuterRight, Lane.MiddleRight, Lane.InnerRight, Lane.InnerLeft, Lane.MiddleLeft, Lane.OuterLeft),
+            intArrayOf(2, 1, 126),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 1, 126, 142, 16, 3)
+        )
+        linesTest(
+            listOf(),
+            listOf(),
+            intArrayOf(2, 0, 0),
+            intArrayOf(16, 123, 9, 3, 0, 0, 0, 85, 21, 2, 0, 0, 13, 16, 3)
+        )
+    }
+
+    @Test
+    fun directionTest() {
+        directionTest(
+            OutAngle.RightDown,
+            intArrayOf(1, 32, 0, 0),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 32, 0, 0, 236, 16, 3)
+        )
+        directionTest(
+            OutAngle.SharpRight,
+            intArrayOf(1, 1, 0, 2),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 1, 0, 2, 9, 16, 3)
+        )
+        directionTest(
+            OutAngle.Right,
+            intArrayOf(1, 1, 0, 4),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 1, 0, 4, 7, 16, 3)
+        )
+        directionTest(
+            OutAngle.EasyRight,
+            intArrayOf(1, 1, 0, 8),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 1, 0, 8, 3, 16, 3)
+        )
+        directionTest(
+            OutAngle.Straight,
+            intArrayOf(1, 1, 0, 16),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 1, 0, 16, 16, 251, 16, 3)
+        )
+        directionTest(
+            OutAngle.EasyLeft,
+            intArrayOf(1, 1, 0, 32),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 1, 0, 32, 235, 16, 3)
+        )
+        directionTest(
+            OutAngle.Left,
+            intArrayOf(1, 1, 0, 64),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 1, 0, 64, 203, 16, 3)
+        )
+        directionTest(
+            OutAngle.SharpLeft,
+            intArrayOf(1, 1, 0, 128),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 1, 0, 128, 139, 16, 3)
+        )
+        directionTest(
+            OutAngle.LeftDown,
+            intArrayOf(1, 16, 0, 0),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 16, 16, 0, 0, 252, 16, 3)
+        )
+        directionTest(
+            OutAngle.Down,
+            intArrayOf(1, 1, 0, 1),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 1, 0, 1, 10, 16, 3)
+        )
+        directionTest(
+            OutAngle.SharpRight, OutType.LongerLane,
+            intArrayOf(1, 2, 0, 2),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 2, 0, 2, 8, 16, 3)
+        )
+        directionTest(
+            OutAngle.Right, OutType.LongerLane,
+            intArrayOf(1, 2, 0, 4),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 2, 0, 4, 6, 16, 3)
+        )
+        directionTest(
+            OutAngle.EasyRight, OutType.LongerLane,
+            intArrayOf(1, 2, 0, 8),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 2, 0, 8, 2, 16, 3)
+        )
+        directionTest(
+            OutAngle.Straight, OutType.LongerLane,
+            intArrayOf(1, 2, 0, 16),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 2, 0, 16, 16, 250, 16, 3)
+        )
+        directionTest(
+            OutAngle.EasyLeft, OutType.LongerLane,
+            intArrayOf(1, 2, 0, 32),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 2, 0, 32, 234, 16, 3)
+        )
+        directionTest(
+            OutAngle.Left, OutType.LongerLane,
+            intArrayOf(1, 2, 0, 64),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 2, 0, 64, 202, 16, 3)
+        )
+        directionTest(
+            OutAngle.SharpLeft, OutType.LongerLane,
+            intArrayOf(1, 2, 0, 128),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 2, 0, 128, 138, 16, 3)
+        )
+        directionTest(
+            OutAngle.SharpRight, OutType.RightRoundabout,
+            intArrayOf(1, 8, 2, 2),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 8, 2, 2, 0, 16, 3)
+        )
+        directionTest(
+            OutAngle.Right, OutType.RightRoundabout,
+            intArrayOf(1, 8, 4, 4),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 8, 4, 4, 252, 16, 3)
+        )
+        directionTest(
+            OutAngle.EasyRight, OutType.RightRoundabout,
+            intArrayOf(1, 8, 8, 8),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 8, 8, 8, 244, 16, 3)
+        )
+        directionTest(
+            OutAngle.Straight, OutType.RightRoundabout,
+            intArrayOf(1, 8, 16, 16),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 8, 16, 16, 16, 16, 228, 16, 3)
+        )
+        directionTest(
+            OutAngle.EasyLeft, OutType.RightRoundabout,
+            intArrayOf(1, 8, 32, 32),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 8, 32, 32, 196, 16, 3)
+        )
+        directionTest(
+            OutAngle.Left, OutType.RightRoundabout,
+            intArrayOf(1, 8, 64, 64),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 8, 64, 64, 132, 16, 3)
+        )
+        directionTest(
+            OutAngle.SharpLeft, OutType.RightRoundabout,
+            intArrayOf(1, 8, 128, 128),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 8, 128, 128, 4, 16, 3)
+        )
+        directionTest(
+            OutAngle.Left, OutType.Flag,
+            intArrayOf(1, 64, 0, 64),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 64, 0, 64, 140, 16, 3)
+        )
+        directionTest(
+            OutAngle.Right, OutType.Flag,
+            intArrayOf(1, 64, 0, 4),
+            intArrayOf(16, 123, 10, 4, 0, 0, 0, 85, 21, 1, 64, 0, 4, 200, 16, 3)
+        )
+    }
+
+    @Test
+    fun distanceTest() {
+        // TODO
+        intArrayOf(3, 0, 9, 9, 0, 9, 0)
+        intArrayOf(16, 123, 13, 7, 0, 0, 0, 85, 21, 3, 0, 9, 9, 0, 9, 0, 233, 16, 3)
+        intArrayOf(3, 0, 9, 9, 0, 9, 3)
+        intArrayOf(16, 123, 13, 7, 0, 0, 0, 85, 21, 3, 0, 9, 9, 0, 9, 3, 230, 16, 3)
+        intArrayOf(3, 0, 9, 9, 0, 9, 1)
+        intArrayOf(16, 123, 13, 7, 0, 0, 0, 85, 21, 3, 0, 9, 9, 0, 9, 1, 232, 16, 3)
+        intArrayOf(3, 0, 9, 9, 0, 9, 8)
+        intArrayOf(16, 123, 13, 7, 0, 0, 0, 85, 21, 3, 0, 9, 9, 0, 9, 8, 225, 16, 3)
+        intArrayOf(3, 0, 9, 9, 0, 9, 5)
+        intArrayOf(16, 123, 13, 7, 0, 0, 0, 85, 21, 3, 0, 9, 9, 0, 9, 5, 228, 16, 3)
+    }
+
+    @Test
+    fun speedTest() {
+        // TODO
+        intArrayOf(7, 1)
+        intArrayOf(16, 123, 8, 2, 0, 0, 0, 85, 21, 7, 1, 9, 16, 3)
+        intArrayOf(6, 0, 0, 0, 0, 0, 5, 10, 0, 0)
+        intArrayOf(16, 123, 16, 16, 10, 0, 0, 0, 85, 21, 6, 0, 0, 0, 0, 0, 5, 10, 0, 0, 236, 16, 3)
+        intArrayOf(6, 0, 5, 10, 255, 1, 10, 10, 0, 0)
+        intArrayOf(16, 123, 16, 16, 10, 0, 0, 0, 85, 21, 6, 0, 5, 10, 255, 1, 10, 10, 0, 0, 216, 16, 3)
+        intArrayOf(6, 1, 5, 10, 255, 1, 10, 10, 255, 0)
+        intArrayOf(16, 123, 16, 16, 10, 0, 0, 0, 85, 21, 6, 1, 5, 10, 255, 1, 10, 10, 255, 0, 216, 16, 3)
+        intArrayOf(6, 0, 5, 10, 255, 1, 10, 10, 0, 255)
+        intArrayOf(16, 123, 16, 16, 10, 0, 0, 0, 85, 21, 6, 0, 5, 10, 255, 1, 10, 10, 0, 255, 217, 16, 3)
+    }
+
+    @Test
+    fun timeTest() {
+        // TODO
+        intArrayOf(5, 0, 2, 2, 255, 2, 2, 0, 0)
+        intArrayOf(16, 123, 15, 9, 0, 0, 0, 85, 21, 5, 0, 2, 2, 255, 2, 2, 0, 0, 247, 16, 3)
+        intArrayOf(5, 255, 2, 2, 255, 2, 2, 0, 0)
+        intArrayOf(16, 123, 15, 9, 0, 0, 0, 85, 21, 5, 255, 2, 2, 255, 2, 2, 0, 0, 248, 16, 3)
+        intArrayOf(5, 0, 2, 2, 255, 2, 2, 0, 255)
+        intArrayOf(16, 123, 15, 9, 0, 0, 0, 85, 21, 5, 0, 2, 2, 255, 2, 2, 0, 255, 248, 16, 3)
+    }
+
+    @Test
+    fun controlTest() {
+        // TODO
+        intArrayOf(4, 1)
+        intArrayOf(16, 123, 8, 2, 0, 0, 0, 85, 21, 4, 1, 12, 16, 3)
+        intArrayOf(4, 0)
+        intArrayOf(16, 123, 8, 2, 0, 0, 0, 85, 21, 4, 0, 13, 16, 3)
+    }
+
+    private fun linesTest(outlines: Lane, arrows: Lane, expectedRaw: IntArray, expectedBoxed: IntArray) {
+        linesTest(listOf(outlines), listOf(arrows), expectedRaw, expectedBoxed)
+    }
+
+    private fun linesTest(outlines: List<Lane>, arrows: List<Lane>, expectedRaw: IntArray, expectedBoxed: IntArray) {
+        val state = State()
+        state.lineOutlines = outlines
+        state.lineArrows = arrows
+        makeAssertions(GarminMapper.setLines(state), expectedRaw, expectedBoxed)
+    }
+
+    private fun directionTest(outAngle: OutAngle, expectedRaw: IntArray, expectedBoxed: IntArray) {
+        directionTest(outAngle, OutType.Lane, expectedRaw, expectedBoxed)
+    }
+
+    private fun directionTest(outAngle: OutAngle, outType: OutType, expectedRaw: IntArray, expectedBoxed: IntArray) {
+        val state = State()
+        state.direction.angle = outAngle
+        state.direction.out = outType
+        makeAssertions(GarminMapper.setDirection(state), expectedRaw, expectedBoxed)
+    }
+
+    private fun makeAssertions(resultRaw: IntArray, expectedRaw: IntArray, expectedBoxed: IntArray) {
+        assertThat(resultRaw).containsExactly(expectedRaw.toTypedArray())
+        val resultBoxed = Garmin.prepareData(resultRaw)
+        assertThat(resultBoxed).containsExactly(expectedBoxed.toTypedArray())
+    }
+
+}
--- a/app/test.py
+++ b/app/test.py
@ -1,31 +0,0 @@
-from numpy import uint8
-
-
-def send_hud(buf: list):
-    n = len(buf)
-    chars = []
-    crc = uint8(0xeb + n + n)
-    chars.append(0x10)
-    chars.append(0x7b)
-    chars.append(n + 6)
-    if n == 0xa:
-        chars.append(0x10)
-    chars.append(n)
-    chars.append(0x00)
-    chars.append(0x00)
-    chars.append(0x00)
-    chars.append(0x55)
-    chars.append(0x15)
-    for char in buf:
-        crc = uint8(crc + char)
-        chars.append(char)
-        if char == 0x10:
-            chars.append(0x10)
-    chars.append((-crc) & 0xff)
-    chars.append(0x10)
-    chars.append(0x03)
-    print(chars)
-    print([bytes(chr(char), 'raw_unicode_escape') for char in chars])
-
-
-send_hud([0x04, 0x01])
--- a/python/main.py
+++ b/python/main.py
@ -52,11 +52,13 @@ class Lane(Enum):

 class Controller:

-    def __init__(self):
+    def __init__(self, device: str = None):
        self.gps = False
-        self.ser = None#serial.Serial('COM9', 9600)
-        time.sleep(2)
-        # print(self.ser.read_all())
+        self.debug = device is None
+        if not self.debug:
+            self.ser = serial.Serial(device, 9600)
+            time.sleep(2)
+            print(self.ser.read_all())

    def clear(self):
        self.send_hud([0x03, 0, 0, 0, 0x00, 0, 0])
@ -123,8 +125,8 @@ class Controller:
            self.send_hud([0x07, 0x01])
        self.gps = state

-    def set_speed_control(self):
-        self.send_hud([0x04, 0x01])
+    def set_speed_control(self, on: bool = True):
+        self.send_hud([0x04, 0x01 if on else 0x00])

    def set_compass(self, direction: float):
        if direction > 337.5 or direction <= 22.5:
@ -172,35 +174,18 @@ class Controller:
        self.send_packet(chars)

    def send_packet(self, buff):
-        print("raw", buff)
        encoded = [bytes(chr(char), 'raw_unicode_escape') for char in buff]
-        print("enc", encoded)
-        # for char in buff:
-        #     self.ser.write(bytes(chr(char), 'raw_unicode_escape'))
-        # time.sleep(0.2)
-        # print(self.ser.read_all())
-
-
-def test():
-    c.set_direction(OutAngle.Straight, OutType.Lane)
-    target = 100
-    for i in range(10):
-        c.set_distance(target, Unit.Kilometres)
-        time.sleep(0.5)
-        target -= 1
-    c.set_speed_control()
-    time.sleep(0.5)
-    for i in range(10):
-        c.set_distance(target, Unit.Kilometres)
-        time.sleep(0.5)
-        target -= 1
-    c.set_direction(OutAngle.Right, OutType.LongerLane)
-    for i in range(10):
-        c.set_distance(target, Unit.Kilometres)
-        time.sleep(0.5)
-        target -= 1
+        if self.debug:
+            print("raw", buff)
+            print("enc", encoded)
+        else:
+            for char in buff:
+                self.ser.write(bytes(chr(char), 'raw_unicode_escape'))
+            time.sleep(0.2)
+            print(self.ser.read_all())


 if __name__ == '__main__':
-    c = Controller()
-    test()
+    import sys
+    device = sys.argv[1] if len(sys.argv) > 1 else None
+    c = Controller(device)
--- a/python/test.py
+++ b/python/test.py
@ -0,0 +1,111 @@
+from main import *
+from time import sleep
+
+interval = 0.2
+
+
+def suite(controller: Controller):
+    lines(controller)
+    direction(controller)
+    distance(controller)
+    speed(controller)
+    time(controller)
+    control(controller)
+    compass(controller)
+
+
+def lines(controller: Controller):
+    print("Lines")
+    controller.set_lines([Lane.DotsLeft], [])
+    controller.set_lines([Lane.OuterRight], [Lane.OuterLeft])
+    controller.set_lines([Lane.MiddleRight], [Lane.MiddleLeft])
+    controller.set_lines([Lane.InnerRight], [Lane.InnerLeft])
+    controller.set_lines([Lane.InnerLeft], [Lane.InnerRight])
+    controller.set_lines([Lane.MiddleLeft], [Lane.MiddleRight])
+    controller.set_lines([Lane.OuterLeft], [Lane.OuterRight])
+    controller.set_lines(
+        [Lane.DotsRight],
+        [Lane.OuterRight, Lane.MiddleRight, Lane.InnerRight, Lane.InnerLeft, Lane.MiddleLeft, Lane.OuterLeft]
+    )
+    controller.set_lines([], [])
+
+
+def direction(controller: Controller):
+    print("Direction")
+    controller.set_direction(OutAngle.RightDown)
+    controller.set_direction(OutAngle.SharpRight)
+    controller.set_direction(OutAngle.Right)
+    controller.set_direction(OutAngle.EasyRight)
+    controller.set_direction(OutAngle.Straight)
+    controller.set_direction(OutAngle.EasyLeft)
+    controller.set_direction(OutAngle.Left)
+    controller.set_direction(OutAngle.SharpLeft)
+    controller.set_direction(OutAngle.LeftDown)
+    controller.set_direction(OutAngle.Down)
+    controller.set_direction(OutAngle.SharpRight, OutType.LongerLane)
+    controller.set_direction(OutAngle.Right, OutType.LongerLane)
+    controller.set_direction(OutAngle.EasyRight, OutType.LongerLane)
+    controller.set_direction(OutAngle.Straight, OutType.LongerLane)
+    controller.set_direction(OutAngle.EasyLeft, OutType.LongerLane)
+    controller.set_direction(OutAngle.Left, OutType.LongerLane)
+    controller.set_direction(OutAngle.SharpLeft, OutType.LongerLane)
+    controller.set_direction(OutAngle.SharpRight, OutType.RightRoundabout)
+    controller.set_direction(OutAngle.Right, OutType.RightRoundabout)
+    controller.set_direction(OutAngle.EasyRight, OutType.RightRoundabout)
+    controller.set_direction(OutAngle.Straight, OutType.RightRoundabout)
+    controller.set_direction(OutAngle.EasyLeft, OutType.RightRoundabout)
+    controller.set_direction(OutAngle.Left, OutType.RightRoundabout)
+    controller.set_direction(OutAngle.SharpLeft, OutType.RightRoundabout)
+    controller.set_direction(OutAngle.Left, OutType.Flag)
+    controller.set_direction(OutAngle.Right, OutType.Flag)
+
+
+def distance(controller: Controller):
+    print("Distance")
+    controller.set_distance(999)
+    controller.set_distance(999, Unit.Kilometres)
+    controller.set_distance(999, Unit.Metres)
+    controller.set_distance(999, Unit.Foot)
+    controller.set_distance(999, Unit.Miles)
+    pass
+
+
+def speed(controller: Controller):
+    print("Speed")
+    controller.set_gps(True)
+    controller.set_speed(50)
+    controller.set_speed(50, 100)
+    controller.set_speed(150, 100)
+    controller.set_speed(50, 100, True)
+    pass
+
+
+def time(controller: Controller):
+    print("Time")
+    controller.set_time(22, 22)
+    controller.set_time(22, 22, traffic=True)
+    controller.set_time(22, 22, flag=True)
+    pass
+
+
+def control(controller: Controller):
+    print("Speed Control")
+    controller.set_speed_control()
+    controller.set_speed_control(False)
+
+
+def compass(controller: Controller):
+    print("Compass")
+    controller.set_compass(22.5)
+    controller.set_compass(67.5)
+    controller.set_compass(112.5)
+    controller.set_compass(157.5)
+    controller.set_compass(202.5)
+    controller.set_compass(247.5)
+    controller.set_compass(292.5)
+    controller.set_compass(337.5)
+    pass
+
+
+if __name__ == '__main__':
+    instance = Controller('/dev/rfcomm0')