package org.nstart.dep265.tletools.zeptomoby.core.coord

import org.nstart.dep265.tletools.zeptomoby.core.Globals
import org.nstart.dep265.tletools.zeptomoby.core.Globals.acTan
import org.nstart.dep265.tletools.zeptomoby.core.Globals.fmod
import org.nstart.dep265.tletools.zeptomoby.core.Globals.rad2deg
import org.nstart.dep265.tletools.zeptomoby.core.Globals.sqr
import org.nstart.dep265.tletools.zeptomoby.core.Julian
import org.nstart.dep265.tletools.zeptomoby.core.Vector
import org.nstart.dep265.tletools.zeptomoby.core.eci.Eci
import java.lang.StringBuilder
import kotlin.math.abs
import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.sqrt

open class Geo {

    constructor(eci: Eci, date: Julian) {
        val coords = construct(eci.position, fmod((acTan(eci.position.y, eci.position.x) - date.toGMST()), org.nstart.dep265.tletools.zeptomoby.core.Globals.twoPi))

        latitudeRad = coords[0]
        longitudeRad = coords[1]
        altitudeKm = coords[2]
    }

    constructor(latRad: Double, lonRad: Double, altKm: Double) {
        latitudeRad = latRad
        longitudeRad = lonRad
        altitudeKm = altKm
    }

    var altitudeKm: Double
    val latitudeRad: Double
    val longitudeRad: Double
    val latitudeDeg: Double
        get() = rad2deg(latitudeRad)
    val longitudeDeg: Double
        get() = rad2deg(longitudeRad)

    override fun toString(): String {
        val strBuilder = StringBuilder()
        val isNorth = latitudeRad >= 0.0
        val isEast  = longitudeRad >= 0.0

        strBuilder.append(String.format("%04.3f%c", latitudeDeg, if (isNorth) 'N' else 'S'))
        strBuilder.append(" ")
        strBuilder.append(String.format("%05.3f%c", latitudeDeg, if (isEast) 'E' else 'W'))
        strBuilder.append(" ")
        strBuilder.append(String.format("%.1fm", altitudeKm * 1000.0))

        return strBuilder.toString()
    }

    protected fun construct(posEcf: Vector, theta: Double): Array<Double> {
        var fTheta = fmod(theta, org.nstart.dep265.tletools.zeptomoby.core.Globals.twoPi)
        if (fTheta  < 0.0) fTheta += org.nstart.dep265.tletools.zeptomoby.core.Globals.twoPi

        val kmSemiMaj = org.nstart.dep265.tletools.zeptomoby.core.Globals.wgs.xkmPer

        val r = sqrt(sqr(posEcf.x) + sqr(posEcf.y))
        val e2 = org.nstart.dep265.tletools.zeptomoby.core.Globals.wgs.f * (2.0 - org.nstart.dep265.tletools.zeptomoby.core.Globals.wgs.f)
        var lat = acTan(posEcf.z, r)

        val delta = 1.0e-7
        var phi: Double
        var c: Double

        do {
            phi = lat
            c = 1.0 / sqrt(1.0 - e2 * sqr(sin(phi)))
            lat = acTan(posEcf.z + kmSemiMaj * c * e2 * sin(phi), r)
        }
        while (abs(lat - phi) > delta)

        return arrayOf(lat, fTheta, r / cos(lat) - kmSemiMaj * c)
    }
}