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

import org.nstart.dep265.tletools.zeptomoby.core.Globals
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.coord.Geo
import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.sqrt

open class Eci {

    constructor(pos: Vector, vel: Vector) {
        position = pos
        velocity = vel
    }

    constructor(geo: Geo, date: Julian) {
        val lat: Double = geo.latitudeRad
        val lon: Double = geo.longitudeRad
        val alt: Double = geo.altitudeKm

        val theta: Double = date.toLMST(lon)
        val c: Double = 1.0 / sqrt(1.0 + org.nstart.dep265.tletools.zeptomoby.core.Globals.wgs.f * (org.nstart.dep265.tletools.zeptomoby.core.Globals.wgs.f - 2.0) * sqr(sin(lat)))
        val s: Double = sqr(1.0 - org.nstart.dep265.tletools.zeptomoby.core.Globals.wgs.f) * c
        val achcp: Double = (org.nstart.dep265.tletools.zeptomoby.core.Globals.wgs.xkmPer * c + alt) * cos(lat)

        val x = achcp * cos(theta) // km
        val y = achcp * sin(theta) // km
        val z = (org.nstart.dep265.tletools.zeptomoby.core.Globals.wgs.xkmPer * s + alt) * sin(lat) // km

        position = Vector(x, y, z,
            sqrt(
                sqr(x) + sqr(y) + sqr(z) // range, km
            )
        )

        val mFactor: Double = org.nstart.dep265.tletools.zeptomoby.core.Globals.twoPi * (org.nstart.dep265.tletools.zeptomoby.core.Globals.omegaE / org.nstart.dep265.tletools.zeptomoby.core.Globals.secPerDay)

        val vx = -mFactor * position.y  // km / sec
        val vy = mFactor * position.x   // km / sec
        val vz = 0.0                    // km / sec

        velocity = Vector(vx, vy, vz,
            sqrt(
                sqr(vx) + sqr(vy) // range rate km/sec^2
            )
        )
    }

    val position: Vector
    val velocity: Vector

    fun scalePosVector(f: Double) = position.mul(f)
    fun scaleVelVector(f: Double) = velocity.mul(f)
}