Reaktor-Orbital-Challenge/src/main/kotlin/name/lamperi/orbital/main.kt

package name.lamperi.orbital

import java.io.File
import java.util.*


// Latitude = South - North (-90...90)
// Longitude = East West (-180...180)

val EARTH_RADIUS = 6371.0; // KM


data class Problem (
        val seed : Double, val satellites : Set<Satellite>, val route : Route)

data class Satellite (
        val id : String, val lat : Double, val lon : Double, val height : Double)

data class Route (
        val startLat : Double, val startLon : Double, val endLat : Double, val endLon : Double)

data class Point3D (
        val x : Double, val y : Double, val z : Double)

data class Vec3 (
        val x : Double, val y : Double, val z : Double
) {
    fun len() : Double {
        return Math.sqrt(x * x + y * y + z * z)
    }

    fun unit() : Vec3 {
        val l = len()
        return Vec3(x/l, y/l, z/l)
    }

    fun dot(o: Vec3) : Double {
        return x * o.x + y * o.y + z * o.z
    }
}

fun toVec(point : Point3D) : Vec3 {
    return Vec3(point.x, point.y, point.z)
}

fun createVec(start : Point3D, end : Point3D) : Vec3 {
    return Vec3(end.x - start.x, end.y - start.y, end.z - start.z)
}

data class SphericalCoordinate (
        val radius : Double, val theta : Double, val phi : Double)

data class NamedPoint3D (
        val name : String, val point : Point3D)

data class Graph (
        val nodes : Map<String,Node>)

data class Node (
        val info : NamedPoint3D, val neighbors : Set<String>)

// Coordinate system
// x = + when lon = 0°, - when lon = 180°
// y = + when lon = 90°, - when lon = -90°
// z = + when lat = 90°, - when lat = -90°

fun rad(degrees : Double) = degrees * Math.PI / 180.0

// let theta = rad(lon) in (-pi, pi)
// let phi = rad(-lat + 90°) in (0, pi)
fun toSpherical(lat : Double, lon : Double, radius : Double = EARTH_RADIUS)
        =  SphericalCoordinate(radius = radius, theta = rad(lon), phi = rad(-lat + 90.0))

// Cartesian coordinates can be calculated using
// x = r * sin(theta) * cos(phi)
// y = r * sin(theta) * sin(phi)
// z = r * cos(theta)
fun toCartesian(c : SphericalCoordinate)
        = Point3D(
            x = c.radius * Math.sin(c.theta) * Math.cos(c.phi),
            y = c.radius * Math.sin(c.theta) * Math.sin(c.phi),
            z = c.radius * Math.cos(c.theta))

fun toCartesian(lat : Double, lon : Double)
        = toCartesian(toSpherical(lat, lon))

fun toCartesian(lat : Double, lon : Double, height : Double) : Point3D
        = toCartesian(toSpherical(lat, lon, EARTH_RADIUS + height))

fun named(name : String, point : Point3D) = NamedPoint3D(name = name, point = point)

fun parseFile(file : java.io.File) : Problem {
    val contents : String = file.readText()
    val seed : Double = contents.splitToSequence('\n').first().splitToSequence(':').last().toDouble()
    val satellites : Set<Satellite> =  contents.splitToSequence('\n')
            .filter { it.startsWith("SAT") }
            .map {
                val parts = it.split(',')
                Satellite(id = parts[0], lat = parts[1].toDouble(),
                    lon = parts[2].toDouble(), height = parts[3].toDouble())
            }.toSet()
    val route : Route = contents.splitToSequence('\n')
            .filter { it.startsWith("ROUTE") }
            .map {
                val parts = it.split(',')
                Route(startLat = parts[1].toDouble(), startLon = parts[2].toDouble(),
                        endLat =  parts[3].toDouble(), endLon = parts[4].toDouble())
            }.first()
    return Problem(seed = seed, satellites = satellites, route = route)
}

// https://en.wikipedia.org/wiki/Line–sphere_intersection
// Solutions are of quadratic form
// l = unit vector from point 0 to point 1
// o = point 0
// c = [0,0,0]
// r = EARTH_RADIUS
// Calculate (l · o)^2 - (o · o)  + r^2
// If it is non-zero, then the route cannot be made
fun canRoute(p0 : Point3D, p1 : Point3D) : Boolean {
    val l = createVec(p0, p1).unit()
    val o = toVec(p0)

    val l_dot_o = l.dot(o)
    val o_dot_o = o.dot(o)
    val result = l_dot_o * l_dot_o - o_dot_o + EARTH_RADIUS * EARTH_RADIUS
    if (result <= 0) {
        println("$p0 $p1 => $result")
    }
    return result > 0
}

fun <E> concat(set : Collection<E>, vararg more : E) : Set<E> {
    val mutable = set.toMutableSet()
    for (e in more) mutable.add(e)
    return mutable.toSet()
}

fun buildGraph(problem : Problem) : Graph {
    val start = named("Start", toCartesian(problem.route.startLat, problem.route.startLon))
    val end = named("End", toCartesian(problem.route.endLat, problem.route.endLon))

    val satellites = problem.satellites.map { sat ->
        named(sat.id, toCartesian(sat.lat, sat.lon, sat.height))
    }.toList()

    val startNode = Node(info = start, neighbors = satellites.filter {
        canRoute(start.point, it.point)
    }.map { it.name }.toSet())

    val allPoints = concat(satellites, start, end)

    val otherNodes = satellites.map { sat ->
        Node(info = sat, neighbors = allPoints.filter { other ->
            !sat.name.equals(other.name) && canRoute(sat.point, other.point)
        }.map { it.name }.toSet())
    }

    val endNode = Node(info = end, neighbors = satellites.filter {
        canRoute(it.point, end.point)
    }.map { it.name }.toSet())

    val nodeMap = concat(otherNodes, startNode, endNode).associateBy({ it.info.name }, { it })
    return Graph(nodeMap)
}

data class Routing (val node : String, var parent : String?, var distance : Int = Integer.MAX_VALUE)

fun solveGraph(start : String, end : String, graph : Graph) {
    val routes : Map<String,Routing> = graph.nodes.map { Routing(node = it.key, parent = null) }.associateBy { it.node }

    val queue = LinkedList<String>()

    val route = routes[start]!!
    route.distance = 0

    queue.add(start)

    while (!queue.isEmpty()) {
        val current = queue.remove()
        val currentRoute = routes[current]!!
        val route = routes[current]!!

        val node = graph.nodes[current]!!
        for (neighbor in node.neighbors) {
            val neighborRoute = routes[neighbor]!!
            if (neighborRoute.distance == Integer.MAX_VALUE) {
                neighborRoute.distance = currentRoute.distance + 1
                neighborRoute.parent = current
                queue.add(neighbor)
            }
        }
    }

    val endRoute = routes[end]
    if (endRoute != null) {
        println("Distance was ${endRoute.distance}")
    }
}

fun main(args : Array<String>) {
    val file = File("src/main/resources/satellites.txt")
    val problem = parseFile(file)
    println(problem)

    val graph = buildGraph(problem)
    println(graph)

    solveGraph("Start", "End", graph)

    /*
    graph.nodes.values.forEach {
        println("[\"${it.info.name}\", ${it.info.point.x}, ${it.info.point.y}, ${it.info.point.z}]")
    }
    */



}