package space.nstart.pcp.angularmotion

import ballistics.Ballistics
import ballistics.orbitalPoints.timeStepper.AbstractStepper
import ballistics.types.BallisticsError
import ballistics.types.EarthType
import ballistics.types.InitialConditions
import ballistics.types.OrbitalPoint
import ballistics.utils.fromDateTime
import ballistics.utils.math.Vector3D
import ballistics.utils.toDateTime
import java.time.LocalDateTime
import java.time.format.DateTimeFormatter
import java.util.Locale
import kotlin.math.PI
import kotlin.math.atan
import kotlin.math.cos
import kotlin.math.sin
import kotlin.test.Test
import kotlin.test.assertEquals
import kotlin.test.assertTrue

class AngularMotionCalculatorSmokeTest {
    private val outputTimeFormatter = DateTimeFormatter.ofPattern("yyyy-MM-dd'T'HH:mm:ss.SSS")

    @Test
    fun `const orientation returns points`() {
        val result = ConstOrientPUUD(CircularStepper()).calculate(
            SurveyId(
                t = 1000.0,
                b = 0.1,
                l = 0.2,
                h = 0.0,
                duration = 1.0,
            )
        )

        assertEquals(AngularMotionMode.CONST_ORIENT, result.mode)
        assertTrue(result.points.isNotEmpty())
    }

    @Test
    fun `factory creates smooth sdi calculator`() {
        val calculator = AngularMotionCalculatorFactory.create(
            mode = AngularMotionMode.SMOOTH_SDI,
            stepper = CircularStepper(),
        )

        val result = calculator.calculate(
            SurveyId(
                t = 1000.0,
                b = 0.1,
                l = 0.2,
                h = 0.0,
                duration = 1.0,
                sdi = listOf(100.0),
                azimuth = 0.3,
            )
        )

        assertEquals(AngularMotionMode.SMOOTH_SDI, result.mode)
        assertTrue(result.points.isNotEmpty())
    }

    @Test
    fun `survey contour calculator returns closed wkt polygon`() {
        val result = ConstOrientPUUD(CircularStepper()).calculate(
            SurveyId(
                t = 1000.0,
                b = 0.1,
                l = 0.2,
                h = 0.0,
                duration = 1.0,
            )
        )

        val wkt = SurveyContourCalculator().calculate(result, 0.0, AngularMotionConfig().focus * atan(1.5 * PI / 180),
            AngularMotionConfig().focus)
        val coordinates = wkt
            .removePrefix("POLYGON ((")
            .removeSuffix("))")
            .split(", ")

        assertTrue(wkt.startsWith("POLYGON (("))
        assertTrue(coordinates.size >= result.points.size * 2 + 1)
        assertEquals(coordinates.first(), coordinates.last())
    }

    private class CircularStepper : AbstractStepper {
        private val radius = 7_000_000.0
        private val omega = 0.001

        override fun calculate(t: Double): OrbitalPoint = point(t)

        override fun calculate(t: Double, p: OrbitalPoint): OrbitalPoint = point(t)

        override fun clear() = Unit

        private fun point(t: Double): OrbitalPoint {
            val a = omega * t
            return OrbitalPoint(
                t = t,
                vit = 0,
                r = Vector3D(radius * cos(a), radius * sin(a), radius * 0.01 * sin(a * 0.5)),
                v = Vector3D(-radius * omega * sin(a), radius * omega * cos(a), radius * 0.005 * omega * cos(a * 0.5)),
            )
        }
    }



    @Test
    fun chekConstOrient(){
        val ballistics = Ballistics()
        val t = LocalDateTime.of(2023, 4, 12, 17, 41, 18, 0)
        val ic = InitialConditions(
            OrbitalPoint(
                fromDateTime(t),
                1,
                Vector3D(
                    -3101926.630,
                    6018678.8,
                    0.0,
                ),
                Vector3D(
                    1287.651,
                    663.634,
                    7612.951,
                ),
            ),
            0.005,
            160.0
        )
        val result = ballistics.calculateOrbPoints(
            ic,
            ic.point.t,
            ic.point.t + 86400 * 10
        )
        assertEquals(result, BallisticsError.OK)
        if (result != BallisticsError.OK)
            return
        val stepper = ballistics.getStepper()
        assert(stepper != null)
        if (stepper == null)
            return
        val calculator = ConstOrientPUUD(stepper, EarthType.PZ90d02)
        val id = SurveyId(
            oep = listOf(false, false, false, true),
            t = fromDateTime(LocalDateTime.of(2023, 4, 16, 3, 48, 22, 0)),
            b = 49.25824 * PI / 180,
            l = 153.65914 * PI / 180,
            h = 0.0,
            duration = 69.0,
            sdi = listOf(30.0),
            azimuth = -15.567 * PI / 180,
            uprAngle = 0.0,
            pointInCenter = false
        )
        val rc = calculator.calculate(id)

        println(rc.mode)
        println(rc.points.size)
        println(rc.startTime)

        println(ballistics.revolutions.first().hVuz)

        for (v in rc.points) {
            println(
                String.format(
                    Locale.US,
                    "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f , %.3f",
                    toDateTime(v.t).format(outputTimeFormatter),
                    v.orientation.tang * 180 / PI,
                    v.orientation.kren * 180 / PI,
                    v.orientation.risk * 180 / PI,
                    v.groundPoint?.let { String.format(Locale.US, "%.3f", it.lat * 180 / PI) },
                    v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) },
                    v.omega.x,
                    v.omega.y,
                    v.omega.z,
                    v.wd.x,
                    v.wd.y,
                    v.wd.z,
                    v.sdi
                )
            )
        }
    }


    @Test
    fun chekSDI(){
        val ballistics = Ballistics()
        val t = LocalDateTime.of(2023, 4, 12, 17, 41, 18, 0)
        val ic = InitialConditions(
            OrbitalPoint(
                fromDateTime(t),
                1,
                Vector3D(
                    -3101926.630,
                    6018678.8,
                    0.0,
                ),
                Vector3D(
                    1287.651,
                    663.634,
                    7612.951,
                ),
            ),
            0.005,
            160.0
        )
        val result = ballistics.calculateOrbPoints(
            ic,
            ic.point.t,
            ic.point.t + 86400 * 10
        )
        assertEquals(result, BallisticsError.OK)
        if (result != BallisticsError.OK)
            return
        val stepper = ballistics.getStepper()
        assert(stepper != null)
        if (stepper == null)
            return
        val calculator = SmoothSDIPUUD(stepper, EarthType.PZ90d02)
        val id = SurveyId(
            oep = listOf(false, false, false, true),
            t = fromDateTime(LocalDateTime.of(2023, 4, 16, 3, 48, 22, 0)),
            b = 49.25824 * PI / 180,
            l = 153.65914 * PI / 180,
            h = 0.0,
            duration = 69.0,
            sdi = listOf(30.0),
            azimuth = 25.567 * PI / 180,
            uprAngle = 6.0 * PI / 180,
            pointInCenter = false
        )
        val rc = calculator.calculate(id)

        println(rc.mode)
        println(rc.points.size)
        println(rc.startTime)

        for (v in rc.points) {
            println(
                String.format(
                    Locale.US,
                    "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f , %.3f",
                    toDateTime(v.t).format(outputTimeFormatter),
                    v.orientation.tang * 180 / PI,
                    v.orientation.kren * 180 / PI,
                    v.orientation.risk * 180 / PI,
                    v.groundPoint?.let { String.format(Locale.US, "%.3f", it.lat * 180 / PI) },
                    v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) },
                    v.omega.x,
                    v.omega.y,
                    v.omega.z,
                    v.wd.x,
                    v.wd.y,
                    v.wd.z,
                    v.sdi
                )
            )
        }
    }


    @Test
    fun chekAzimuth(){
        val ballistics = Ballistics()
        val t = LocalDateTime.of(2023, 4, 12, 17, 41, 18, 0)
        val ic = InitialConditions(
            OrbitalPoint(
                fromDateTime(t),
                1,
                Vector3D(
                    -3101926.630,
                    6018678.8,
                    0.0,
                ),
                Vector3D(
                    1287.651,
                    663.634,
                    7612.951,
                ),
            ),
            0.005,
            160.0
        )
        val result = ballistics.calculateOrbPoints(
            ic,
            ic.point.t,
            ic.point.t + 86400 * 10
        )
        assertEquals(result, BallisticsError.OK)
        if (result != BallisticsError.OK)
            return
        val stepper = ballistics.getStepper()
        assert(stepper != null)
        if (stepper == null)
            return
        val calculator = AzimuthPUUD(stepper, EarthType.PZ90d02)
        val id = SurveyId(
            oep = listOf(false, false, false, true),
            t = fromDateTime(LocalDateTime.of(2023, 4, 16, 3, 48, 22, 0)),
            b = 49.25824 * PI / 180,
            l = 153.65914 * PI / 180,
            h = 0.0,
            duration = 69.0,
            sdi = listOf(30.0),
            azimuth = 25.567 * PI / 180,
            uprAngle = 6.0 * PI / 180,
            pointInCenter = false
        )
        val rc = calculator.calculate(id)

        println(rc.mode)
        println(rc.points.size)
        println(rc.startTime)

        for (v in rc.points) {
            println(
                String.format(
                    Locale.US,
                    "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f , %.3f",
                    toDateTime(v.t).format(outputTimeFormatter),
                    v.orientation.tang * 180 / PI,
                    v.orientation.kren * 180 / PI,
                    v.orientation.risk * 180 / PI,
                    v.groundPoint?.let { String.format(Locale.US, "%.3f", it.lat * 180 / PI) },
                    v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) },
                    v.omega.x,
                    v.omega.y,
                    v.omega.z,
                    v.wd.x,
                    v.wd.y,
                    v.wd.z,
                    v.sdi
                )
            )
        }
    }
}
