Garmin/app/src/main/java/eu/ztsh/garmin/Garmin.kt

package eu.ztsh.garmin

import android.annotation.SuppressLint
import android.bluetooth.BluetoothAdapter
import android.bluetooth.BluetoothDevice
import android.bluetooth.BluetoothSocket
import android.util.Log
import com.mapbox.navigation.tripdata.maneuver.model.Maneuver
import java.io.IOException
import java.util.*
import java.util.concurrent.SynchronousQueue

@SuppressLint("MissingPermission")
class Garmin(
    val context: MainActivity,
    val device: BluetoothDevice,
    val adapter: BluetoothAdapter
) {

    private lateinit var connection: ConnectThread
    private lateinit var processing: ProcessingThread
    private var stateCache: State = State()

    fun start() {
        connection = ConnectThread()
        connection.start()
    }

    fun close() {
        connection.close()
    }

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



    private inner class ProcessingThread(val maneuver: Maneuver) : Thread() {
        override fun run() {
            send(ManeuverMapper.apply(maneuver))
        }

        fun send(incoming: eu.ztsh.garmin.State) {
            if (stateCache.distance != incoming.distance) {
                setDistance(incoming)
            }
            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 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() {

        private val queue: SynchronousQueue<IntArray> = SynchronousQueue()
        private var current: IntArray = intArrayOf()

        private val socket: BluetoothSocket? by lazy(LazyThreadSafetyMode.NONE) {
            context.checkBt()
            device.createRfcommSocketToServiceRecord(UUID.fromString("00001101-0000-1000-8000-00805F9B34FB"))
        }

        override fun run() {
            // Cancel discovery because it otherwise slows down the connection.
            context.checkBt()
            adapter.cancelDiscovery()
            try {
                socket?.connect()
                context.setConnectionStatus(true)
                sleep(3000)
                readAll()
                while (true) {
                    val newCurrent = Optional.ofNullable(queue.poll()).orElse(current)
                    current = newCurrent
                    send(current)
                    sleep(900)
                }
            } 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)
                }
            }
        }

        // Closes the client socket and causes the thread to finish.
        fun close() {
            try {
                socket?.close()
            } catch (e: IOException) {
                Log.e(TAG, "Could not close the client socket", e)
            }
        }

        fun enqueue(data: IntArray) {
            queue.put(data)
        }

        private fun send(data: IntArray) {
            sendRaw(prepareData(data))
        }

        private fun readAll() {
            val buffer = ByteArray(64)
            val istr = socket!!.inputStream
            istr!!.let {
                if (it.available() > 0) {
                    it.read(buffer)
                }
            }
        }

        private fun sendRaw(buff: IntArray) {
            buff.forEach { socket!!.outputStream.write(it) }
            socket!!.outputStream.flush()
            sleep(2000)
            readAll()
        }

    }

    companion object {
        fun prepareData(input: IntArray): IntArray {
            val n = input.size
            var crc = (0xeb + n + n).toUInt()
            val chars = ArrayList<Int>()
            chars.add(0x10)
            chars.add(0x7b)
            chars.add((n + 0x06))
            if (n == 0xa)
                chars.add(0x10)
            chars.add(n)
            chars.add(0x00)
            chars.add(0x00)
            chars.add(0x00)
            chars.add(0x55)
            chars.add(0x15)
            for (char in input) {
                crc = (crc + char.toUInt())
                chars.add(char)
                if (char == 0x10)
                    chars.add(0x10)
            }
            chars.add((-(crc.toInt()) and 0xff))
            chars.add(0x10)
            chars.add(0x03)
            return chars.toIntArray()
        }

        private const val TAG = "GARMIN"
    }
}