ghostrider/app/src/main/java/de/chst/ghostrider/presentation/GeolocationInfo.kt

package de.chst.ghostrider.presentation

import android.util.Log
import org.w3c.dom.Element
import java.io.InputStream
import java.lang.IllegalArgumentException
import java.time.Duration
import java.time.Instant
import java.time.LocalTime
import javax.xml.parsers.DocumentBuilderFactory

data class Route(val timeMilliseconds: Array<Long>, val lat: Array<Float>, val lon: Array<Float>) {
    fun size(): Int {
        return timeMilliseconds.size
    }

    fun lengthSeconds(): Long {
        assert(timeMilliseconds.isNotEmpty())
        return timeMilliseconds[timeMilliseconds.size - 1] / 1000
    }
}

fun load_gpx(input: InputStream): Route {
    val factory = DocumentBuilderFactory.newInstance()
    val builder = factory.newDocumentBuilder()

    val document = builder.parse(input)


    val trkpts = document.getElementsByTagName("trkpt")
    var lats: Array<Float> = Array(trkpts.length) { 0f }
    var longs: Array<Float> = Array(trkpts.length) { 0f }
    var timeMilliseconds: Array<Long> = Array(trkpts.length) { 0L }

    var startTime: Instant? = null

    for (i in 0 until trkpts.length) {
        val trkpt = trkpts.item(i) as Element

        lats[i] = trkpt.getAttribute("lat").toFloat()
        longs[i] = trkpt.getAttribute("lon").toFloat()

        val timeTags = trkpt.getElementsByTagName("time")
        if(timeTags.length == 0) {
            throw IllegalArgumentException("GPX does not contain time informationm")
        }

        val time = Instant.parse((timeTags.item(0) as Element).textContent)
        if (i == 0) {
            timeMilliseconds[i] = 0
            startTime = time
        } else {
            val dur = Duration.between(startTime, time)
            timeMilliseconds[i] = dur.toMillis()
        }

    }

    return Route(timeMilliseconds, lats, longs)
}

fun deg2rad(degrees: Float): Double {
    return degrees * Math.PI / 180f
}
fun geoDistance(lat1: Float, lon1: Float, lat2: Float, lon2: Float): Float {
    // adapted from https://stackoverflow.com/a/27943

    val R = 6371; // Radius of the earth in km
    val dLat = deg2rad(lat2-lat1);  // deg2rad below
    val dLon = deg2rad(lon2-lon1);
    val a =
        Math.sin(dLat/2) * Math.sin(dLat/2) +
                Math.cos(deg2rad(lat1)) * Math.cos(deg2rad(lat2)) *
                Math.sin(dLon/2) * Math.sin(dLon/2)
    ;
    val c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
    val d = R * c * 1000; // Distance in m
    return d.toFloat();
}

fun find_closest_point(route: Route, lat: Float, lon: Float): Pair<Int, Float> {
    var minIndex: Int = 0
    var distance: Float = Float.POSITIVE_INFINITY
    for (i in 0 until route.timeMilliseconds.size) {
        val newDistance = geoDistance(lat, lon, route.lat[i], route.lon[i])

        if (newDistance < distance) {
            minIndex = i
            distance = newDistance
        }
    }

    return Pair(minIndex, distance)
}

/**
 * Given a route, the milliseconds since planned start of transit
 * and the current position, calculate
 * the relative time to the scheduled route in milliseconds.
 * If we are ahead, returns a negative number, if we are behind, returns a positive number.
 *
 * If the position is not close enough, returns null.
 */
fun calculate_relative_time(route: Route,
                            routeTime: Int,
                            timeInTransit: Long,
                            lat: Float, lon: Float): Long? {
    val (closestTrackpointIndex, distance) = find_closest_point(route, lat, lon)

    val routeTimeFactor = routeTime.toFloat() / route.lengthSeconds().toFloat()
    val plannedDeltaTime = route.timeMilliseconds[closestTrackpointIndex] * routeTimeFactor

    val delay = if (distance > 60) {
        null // if not close enough we can not determine the delay reliably.
    } else {
        plannedDeltaTime - timeInTransit
    }

    Log.d("de.chst.GhostRider", "Distance to closest point: ${distance}m, plannedTime: $plannedDeltaTime, delay: $delay")


    return delay?.toLong()
}