diff --git a/app/src/main/java/eu/ztsh/garmin/Garmin.kt b/app/src/main/java/eu/ztsh/garmin/Garmin.kt
index 27f9b03..16966cb 100644
--- a/app/src/main/java/eu/ztsh/garmin/Garmin.kt
+++ b/app/src/main/java/eu/ztsh/garmin/Garmin.kt
@@ -17,6 +17,7 @@ import java.io.IOException
 import java.util.*
 import java.util.concurrent.Executors
 import java.util.concurrent.SynchronousQueue
+import java.util.concurrent.atomic.AtomicBoolean
 
 @SuppressLint("MissingPermission")
 class Garmin(
@@ -185,6 +186,7 @@ class Garmin(
     private inner class ConnectThread : Thread() {
 
         private val queue: SynchronousQueue<IntArray> = SynchronousQueue()
+        private val state = AtomicBoolean(false)
         private var current: IntArray = intArrayOf()
 
         private val socket: BluetoothSocket? by lazy(LazyThreadSafetyMode.NONE) {
@@ -192,18 +194,31 @@ class Garmin(
             device.createRfcommSocketToServiceRecord(UUID.fromString("00001101-0000-1000-8000-00805F9B34FB"))
         }
 
-        override fun run() {
-            // Cancel discovery because it otherwise slows down the connection.
+        fun tryConnect() {
             context.checkBt()
+            // Cancel discovery because it otherwise slows down the connection.
             adapter.cancelDiscovery()
             try {
                 socket?.connect()
-                context.setConnectionStatus(true)
+                context.toastConnectionStatus(true)
                 sleep(3000)
                 readAll()
                 send(intArrayOf(0x07, 0x01)) // Set GPS to true
                 send(intArrayOf(0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00)) // Clear screen
-                while (true) {
+                state.set(true)
+            } catch (e: IOException) {
+                Log.d(TAG, "Not connected", e)
+                context.toastConnectionStatus(false)
+                state.set(false)
+            }
+            queue.clear()
+            current = intArrayOf()
+        }
+
+        override fun run() {
+            tryConnect()
+            while (true) {
+                if (state.get()) {
                     val newCurrent = queue.poll()
                     if (newCurrent == null) {
                         Log.d(TAG, "run (${currentThread().name}): Sleep...")
@@ -212,15 +227,9 @@ class Garmin(
                         current = newCurrent
                     }
                     send(current)
-                }
-            } catch (e: IOException) {
-                Log.d(TAG, "Not connected", e)
-                context.setConnectionStatus(false)
-                while (true) {
-                    // Just dequeue
-                    // TODO: Add option to reconnect
-                    queue.poll()
-                    sleep(900)
+                } else {
+                    queue.clear()
+                    sleep(2000)
                 }
             }
         }
diff --git a/app/src/main/java/eu/ztsh/garmin/MainActivity.kt b/app/src/main/java/eu/ztsh/garmin/MainActivity.kt
index c2f5e9d..c40f1d4 100644
--- a/app/src/main/java/eu/ztsh/garmin/MainActivity.kt
+++ b/app/src/main/java/eu/ztsh/garmin/MainActivity.kt
@@ -124,7 +124,7 @@ class MainActivity : AppCompatActivity() {
         return true
     }
 
-    fun setConnectionStatus(success: Boolean) {
+    fun toastConnectionStatus(success: Boolean) {
         this.runOnUiThread {
             if (success) {
                 Toast.makeText(this, "Garmin connected", Toast.LENGTH_LONG).show()