package space.nstart.pcp.complan.apl

import space.nstart.pcp.pcp_types_lib.dto.ballistics.OrbPointDTO
import kotlin.math.PI
import kotlin.math.atan
import kotlin.math.sqrt

class CaptureAngleCalculator {

    fun calculateCaptureAngle(routeWidthKm: Double, points: List<OrbPointDTO>): Double? {
        if (routeWidthKm <= 0.0 || points.isEmpty()) {
            return null
        }

        val averageHeightMeters = points
            .map { it.orbitRadiusMeters() - EARTH_RADIUS_METERS }
            .filter { it > 0.0 }
            .average()

        if (averageHeightMeters.isNaN() || averageHeightMeters <= 0.0) {
            return null
        }

        val halfWidthMeters = routeWidthKm * METERS_IN_KILOMETER / 2.0
        return atan(halfWidthMeters / averageHeightMeters).toDegrees()
    }

    private fun OrbPointDTO.orbitRadiusMeters(): Double = sqrt(x * x + y * y + z * z)

    private fun Double.toDegrees(): Double = this * 180.0 / PI

    private companion object {
        const val EARTH_RADIUS_METERS = 6_378_136.0
        const val METERS_IN_KILOMETER = 1_000.0
    }
}
