package info.benjaminhill.klatbot.piconzero

import android.content.Context
import android.hardware.Sensor
import android.hardware.SensorEvent
import android.hardware.SensorEventListener
import android.hardware.SensorManager
import com.google.android.things.pio.Gpio
import com.google.android.things.pio.GpioCallback
import com.google.android.things.pio.PeripheralManagerService
import com.google.android.things.userdriver.UserDriverManager
import com.google.android.things.userdriver.UserSensor
import com.google.android.things.userdriver.UserSensorDriver
import com.google.android.things.userdriver.UserSensorReading
import java.util.concurrent.*
import java.util.concurrent.atomic.AtomicBoolean
import java.util.concurrent.atomic.AtomicLong
import java.util.logging.Logger

/** Callback for when the distance changes "enough to care" */
interface SignificantDistanceChangeListener {
    fun onDistanceChanged(distanceCm: Float)
}

/**
 * User Sensor - Ultrasonic range finder
 */
class HCSR04(context: Context, val sdcl: SignificantDistanceChangeListener) : UserSensorDriver(), SensorEventListener, AutoCloseable {
    private val LOG = Logger.getLogger(this.javaClass.name)
    private val gpio = PeripheralManagerService().openGpio("BCM23")
    private val distanceReading: BlockingQueue<Float> = ArrayBlockingQueue(1)
    // Choreography of each ping
    private val scheduler: ScheduledExecutorService = Executors.newScheduledThreadPool(1)
    private val userSensor: UserSensor

    init {
        userSensor = UserSensor.Builder()
                .setName("HC-SR04 Ultrasonic Distance Sensor")
                .setVersion(1)
                .setType(Sensor.TYPE_PROXIMITY) // Could this be something more linear like TYPE_LIGHT
                .setDriver(this)
                .build()
        UserDriverManager.getManager().registerSensor(userSensor)

        val sensorManager = context.getSystemService(Context.SENSOR_SERVICE) as SensorManager
        LOG.info("ALL Sensors: ${sensorManager.getSensorList(Sensor.TYPE_ALL)}")

        sensorManager.registerDynamicSensorCallback(object : SensorManager.DynamicSensorCallback() {
            override fun onDynamicSensorConnected(sensor: Sensor) {
                LOG.info("onDynamicSensorConnected")
                if (sensor.type == Sensor.TYPE_PROXIMITY) {
                    sensorManager.registerListener(
                            this@HCSR04,
                            sensor,
                            SensorManager.SENSOR_DELAY_NORMAL
                    )
                }
            }
        })

    }

    val gpioEdgeCallback = object : GpioCallback() {
        // Track the reply rise/fall
        private val startMs = AtomicLong()
        private val startValid = AtomicBoolean(false)

        private fun calculate() {
            val elapsed = (System.nanoTime() / 1000) - startMs.get()
            if (startValid.get() && elapsed > 0) {
                distanceReading.put(elapsed * 34000 / 2f)
            } else {
                LOG.warning("Discarding edge callback ${startMs.get()} ${startValid.get()} $elapsed")
            }
            startValid.set(false)
        }

        override fun onGpioEdge(gpio: Gpio?): Boolean {
            if (gpio != null) {
                if (gpio.value) {
                    startMs.set(System.nanoTime() / 1000)
                    startValid.set(true)
                } else {
                    calculate()
                }
                LOG.finer("GPIO input edge: ${System.nanoTime() / 1000} ${gpio.value}")
            }
            return true
        }

        override fun onGpioError(gpio: Gpio?, error: Int) = LOG.severe("$gpio Error event $error")
    }

    /** Launch a new thread to get the distance, then block until we have a result */
    override fun read(): UserSensorReading {
        distanceReading.clear()

        gpio.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW)
        gpio.setActiveType(Gpio.ACTIVE_HIGH)
        gpio.value = false

        scheduler.schedule({ gpio.value = true }, 1, TimeUnit.MICROSECONDS)
        scheduler.schedule({ gpio.value = false }, 11, TimeUnit.MICROSECONDS)
        scheduler.schedule({
            gpio.setDirection(Gpio.DIRECTION_IN)
            gpio.setActiveType(Gpio.ACTIVE_HIGH) // redundant?
            gpio.setEdgeTriggerType(Gpio.EDGE_BOTH)
            gpio.registerGpioCallback(gpioEdgeCallback)
        }, 12, TimeUnit.MICROSECONDS)

        val distanceCm = distanceReading.take()
        gpio.unregisterGpioCallback(gpioEdgeCallback)
        LOG.info("New distance reading: $distanceCm")
        return UserSensorReading(floatArrayOf(distanceCm))
    }

    /** from @SensorEventListener */
    override fun onAccuracyChanged(sensor: Sensor, accuracy: Int) = LOG.info("$sensor accuracy change: $accuracy")

    /**
     * from @SensorEventListener
     */
    override fun onSensorChanged(event: SensorEvent) = sdcl.onDistanceChanged(event.values[0])

    /** from @AutoCloseable */
    override fun close() {
        LOG.warning("Closing Sensor HCSR04")
        UserDriverManager.getManager().unregisterSensor(userSensor)
        gpio.close()
        scheduler.shutdownNow()
    }
}