package commons.gis

import kotlin.math.PI
import kotlin.math.acos
import kotlin.math.asin
import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.max
import kotlin.math.min
import kotlin.math.pow
import kotlin.math.sin
import kotlin.math.sqrt

/**
 * 带时间戳的地理坐标+航向角模型
 * @property lat 纬度
 * @property lng 经度
 * @property heading 航向角（0-360°，正北为0°，顺时针）
 * @property timestamp 时间戳（毫秒）
 */
data class TimedGeoPoint(
    val lat: Double,
    val lng: Double,
    val heading: Float,
    val timestamp: Long
)

/**
 * 三维笛卡尔坐标数据类
 * @param x x轴坐标
 * @param y y轴坐标
 * @param z z轴坐标
 */
data class CartesianPoint(val x: Double, val y: Double, val z: Double)

/**
 * 基于时间的地理坐标插值工具类
 *
 * 核心功能：根据起始点和结束点的时间戳、经纬度、航向角，计算任意时间点的插值结果
 * 适用于：轨迹回放、导航预测、运动轨迹模拟等场景
 *
 * 支持两种插值模式：
 * - 线性插值：适用于短距离（<10km）场景，计算效率高
 * - 球面插值：适用于长距离（>10km）场景，考虑地球曲率，精度更高
 */
object TimeBasedGeoInterpolator {
    // 地球半径（米）
    private const val EARTH_RADIUS = 6371000.0

    /**
     * 角度转弧度
     */
    private fun Double.toRadians(): Double = this * PI / 180.0

    /**
     * 弧度转角度
     */
    private fun Double.toDegrees(): Double = this * 180.0 / PI

    /**
     * 根据目标时间，插值计算对应时刻的地理坐标和航向角
     *
     * @param start 起始点（包含经纬度、航向角、时间戳）
     * @param end 结束点（包含经纬度、航向角、时间戳）
     * @param targetTime 目标时间戳（毫秒）
     * @param useSpherical 是否使用球面插值（true：球面插值，false：线性插值）
     * @return 目标时间对应的插值点（包含计算出的经纬度、航向角和目标时间戳）
     *
     * @throws IllegalArgumentException 当起止时间戳无效时（end.timestamp <= start.timestamp）
     * @sample
     * ```kotlin
     * val startTime = System.currentTimeMillis()
     * val endTime = startTime + 5000 // 5秒后
     * val startPoint = TimedGeoPoint(39.908823, 116.397470, 90.0f, startTime)
     * val endPoint = TimedGeoPoint(39.916404, 116.397096, 100.0f, endTime)
     *
     * // 计算2秒后的位置
     * val interpolatedPoint = TimeBasedGeoInterpolator.interpolateAtTime(
     *     start = startPoint,
     *     end = endPoint,
     *     targetTime = startTime + 2000,
     *     useSpherical = false
     * )
     * ```
     */
    fun interpolateAtTime(
        start: TimedGeoPoint,
        end: TimedGeoPoint,
        targetTime: Long,
        useSpherical: Boolean = false
    ): TimedGeoPoint {
        // 边界处理：目标时间超出范围，直接返回起止点
        if (targetTime <= start.timestamp) return start.copy(timestamp = targetTime)
        if (targetTime >= end.timestamp) return end.copy(timestamp = targetTime)

        // 1. 计算时间比例（0~1）
        val totalDuration = end.timestamp - start.timestamp
        val timeRatio = (targetTime - start.timestamp).toDouble() / totalDuration

        // 2. 经纬度插值
        val (interpolatedLat, interpolatedLng) = if (useSpherical) {
//            interpolateSphericalLatLng(start, end, timeRatio)
            slerp(start, end, timeRatio)
        } else {
            interpolateLinearLatLng(start, end, timeRatio)
        }

        // 3. 航向角插值（处理360°环绕）
        val interpolatedHeading = interpolateHeading(start.heading, end.heading, timeRatio)

        return TimedGeoPoint(
            lat = interpolatedLat,
            lng = interpolatedLng,
            heading = interpolatedHeading,
            timestamp = targetTime
        )
    }

    /**
     * 按固定时间间隔生成等分的插值点列表
     *
     * @param start 起始点（包含经纬度、航向角、时间戳）
     * @param end 结束点（包含经纬度、航向角、时间戳）
     * @param intervalMs 时间间隔（毫秒），例如：1000表示每秒生成一个点
     * @param useSpherical 是否使用球面插值
     * @return 按时间升序排列的插值点列表，包含起始点和结束点
     *
     * @throws IllegalArgumentException 当时间间隔小于等于0或起止时间无效时
     * @sample
     * ```kotlin
     * val startTime = System.currentTimeMillis()
     * val endTime = startTime + 10000 // 10秒后
     * val startPoint = TimedGeoPoint(39.908823, 116.397470, 90.0f, startTime)
     * val endPoint = TimedGeoPoint(39.916404, 116.397096, 100.0f, endTime)
     *
     * // 每秒生成一个插值点
     * val points = TimeBasedGeoInterpolator.interpolateByTimeInterval(
     *     start = startPoint,
     *     end = endPoint,
     *     intervalMs = 1000,
     *     useSpherical = false
     * )
     * // points.size 将是 11（包含起始点和结束点）
     * ```
     */
    fun interpolateByTimeInterval(
        start: TimedGeoPoint,
        end: TimedGeoPoint,
        intervalMs: Long,
        useSpherical: Boolean = false
    ): List<TimedGeoPoint> {
        if (intervalMs <= 0 || end.timestamp <= start.timestamp) return listOf(start, end)

        val interpolatedPoints = mutableListOf<TimedGeoPoint>()
        interpolatedPoints.add(start)

        var currentTime = start.timestamp + intervalMs
        while (currentTime < end.timestamp) {
            val interpolatedPoint = interpolateAtTime(start, end, currentTime, useSpherical)
            interpolatedPoints.add(interpolatedPoint)
            currentTime += intervalMs
        }

        // 确保最后一个点是结束点（避免时间误差）
        if (interpolatedPoints.last().timestamp != end.timestamp) {
            interpolatedPoints.add(end)
        }

        return interpolatedPoints
    }

    /**
     * 线性插值经纬度
     *
     * 适用场景：短距离（<10km），地球曲率影响可忽略
     * 计算方式：直接按时间比例线性计算中间点坐标
     *
     * @param start 起始点
     * @param end 结束点
     * @param timeRatio 时间比例（0~1）
     * @return 插值后的（纬度，经度）对
     * @see interpolateSphericalLatLng 球面插值方法（长距离场景）
     */
    private fun interpolateLinearLatLng(
        start: TimedGeoPoint,
        end: TimedGeoPoint,
        timeRatio: Double
    ): Pair<Double, Double> {
        val lat = start.lat + (end.lat - start.lat) * timeRatio
        val lng = start.lng + (end.lng - start.lng) * timeRatio
        return Pair(lat, lng)
    }


    /**
     * 球面插值（SLERP）
     * @param start 起始经纬度点
     * @param end 结束经纬度点
     * @param t 插值因子（0~1，0=起始点，1=结束点）
     * @param radius 球半径（默认地球半径）
     * @return 插值后的经纬度点
     */
    fun slerp(
        start: TimedGeoPoint,
        end: TimedGeoPoint,
        t: Double,
        radius: Double = EARTH_RADIUS
    ): Pair<Double, Double> {
        // 1. 转换为笛卡尔坐标
        val startCart = sphericalToCartesian(start, radius)
        val endCart = sphericalToCartesian(end, radius)

        // 2. 计算向量夹角
        val theta = vectorAngle(startCart, endCart)

        // 处理夹角为0的情况（两点重合）
        if (theta < 1e-6) {
            return Pair(start.lat, start.lng)
        }

        // 3. SLERP核心计算
        val sinTheta = sin(theta)
        val sinTTheta = sin(t * theta)
        val sin1TTheta = sin((1 - t) * theta)

        val x = (sin1TTheta / sinTheta) * startCart.x + (sinTTheta / sinTheta) * endCart.x
        val y = (sin1TTheta / sinTheta) * startCart.y + (sinTTheta / sinTheta) * endCart.y
        val z = (sin1TTheta / sinTheta) * startCart.z + (sinTTheta / sinTheta) * endCart.z

        // 4. 转回球面坐标
        return cartesianToSpherical(CartesianPoint(x, y, z), radius)
    }

    /**
     * 计算两个三维向量的夹角（弧度）
     * @param a 向量a
     * @param b 向量b
     * @return 夹角（0 ~ π）
     */
    private fun vectorAngle(a: CartesianPoint, b: CartesianPoint): Double {
        // 点积
        val dotProduct = a.x * b.x + a.y * b.y + a.z * b.z
        // 向量模长
        val magA = sqrt(a.x.pow(2) + a.y.pow(2) + a.z.pow(2))
        val magB = sqrt(b.x.pow(2) + b.y.pow(2) + b.z.pow(2))

        // 防止浮点误差导致acos参数超出[-1,1]
        val cosTheta = max(min(dotProduct / (magA * magB), 1.0), -1.0)
        return acos(cosTheta)
    }

    /**
     * 球面坐标（经纬度）转笛卡尔坐标
     * @param spherical 经纬度点
     * @param radius 球半径（默认地球半径）
     * @return 三维笛卡尔坐标
     */
    fun sphericalToCartesian(
        spherical: TimedGeoPoint,
        radius: Double = EARTH_RADIUS
    ): CartesianPoint {
        val lonRad = spherical.lng.toRadians()
        val latRad = spherical.lat.toRadians()

        val x = radius * cos(latRad) * cos(lonRad)
        val y = radius * cos(latRad) * sin(lonRad)
        val z = radius * sin(latRad)

        return CartesianPoint(x, y, z)
    }

    /**
     * 笛卡尔坐标转球面坐标（经纬度）
     * @param cartesian 三维笛卡尔坐标
     * @param radius 球半径（默认地球半径）
     * @return 经纬度点
     */
    fun cartesianToSpherical(
        cartesian: CartesianPoint,
        radius: Double = EARTH_RADIUS
    ): Pair<Double, Double> {
        // 计算纬度（弧度）：arcsin(z / 半径)
        val latRad = asin(cartesian.z / radius)
        // 计算经度（弧度）：arctan2(y, x)
        val lonRad = atan2(cartesian.y, cartesian.x)

        return Pair(latRad.toDegrees(),lonRad.toDegrees())
    }


    /**
     * 航向角时间插值
     *
     * 特殊处理：自动处理360°环绕问题
     * 例如：350°到10°的插值会按350°→360°/0°→10°的路径计算，而非直接350°→10°
     *
     * @param startHeading 起始航向角（0-360°）
     * @param endHeading 结束航向角（0-360°）
     * @param timeRatio 时间比例（0~1）
     * @return 插值后的航向角（0-360°）
     */
    private fun interpolateHeading(
        startHeading: Float,
        endHeading: Float,
        timeRatio: Double
    ): Float {
        var diff = endHeading - startHeading
        // 调整差值到[-180, 180]区间（避免350°→10°时差值为-340°）
        if (diff > 180) diff -= 360
        else if (diff < -180) diff += 360

        // 按时间比例计算插值
        var interpolated = startHeading + diff * timeRatio
        // 修正到0~360°范围
        interpolated %= 360
        if (interpolated < 0) interpolated += 360

        return interpolated.toFloat()
    }

    /**
     * 计算两点间的平均移动速度
     *
     * @param start 起始点
     * @param end 结束点
     * @return 平均速度（米/秒）
     *
     * @throws ArithmeticException 当起止时间相同时（除零错误）
     * @sample
     * ```kotlin
     * val startTime = System.currentTimeMillis()
     * val endTime = startTime + 10000 // 10秒
     * val startPoint = TimedGeoPoint(39.908823, 116.397470, 90.0f, startTime)
     * val endPoint = TimedGeoPoint(39.916404, 116.397096, 100.0f, endTime)
     *
     * val speed = TimeBasedGeoInterpolator.calculateSpeed(startPoint, endPoint)
     * // speed 约为 8.5 米/秒（假设两点距离约85米）
     * ```
     */
    fun calculateSpeed(start: TimedGeoPoint, end: TimedGeoPoint): Double {
        val distance = calculateDistance(start.lat, start.lng, end.lat, end.lng)
        val durationSec = (end.timestamp - start.timestamp) / 1000.0
        return if (durationSec == 0.0) 0.0 else distance / durationSec
    }

    /**
     * 使用哈辛公式（Haversine Formula）计算两点间地表距离
     *
     * 精度：适用于大多数应用场景，误差约为0.5%
     * 适用范围：全球范围内的两点距离计算
     *
     * @param lat1 第一点纬度（度）
     * @param lng1 第一点经度（度）
     * @param lat2 第二点纬度（度）
     * @param lng2 第二点经度（度）
     * @return 两点间距离（米）
     * @see <a href="https://en.wikipedia.org/wiki/Haversine_formula">Haversine formula on Wikipedia</a>
     */
    private fun calculateDistance(lat1: Double, lng1: Double, lat2: Double, lng2: Double): Double {
        val dLat = Math.toRadians(lat2 - lat1)
        val dLng = Math.toRadians(lng2 - lng1)
        val a = sin(dLat / 2) * sin(dLat / 2) +
                cos(Math.toRadians(lat1)) * cos(Math.toRadians(lat2)) *
                sin(dLng / 2) * sin(dLng / 2)
        val c = 2 * atan2(sqrt(a), sqrt(1 - a))
        return EARTH_RADIUS * c
    }
}

// ====================== 使用示例 ======================
fun main() {
    /* // 示例1：定义起止点（带时间戳）
     val startTime = System.currentTimeMillis() // 起始时间
     val endTime = startTime + 5000 // 结束时间（5秒后）
     val startPoint = TimedGeoPoint(
         lat = 39.908823,    // 北京天安门
         lng = 116.397470,
         heading = 90.0f,    // 航向角90°（正东）
         timestamp = startTime
     )
     val endPoint = TimedGeoPoint(
         lat = 39.916404,    // 北京故宫
         lng = 116.397096,
         heading = 100.0f,   // 航向角100°
         timestamp = endTime
     )

     // 示例2：插值计算「2秒后」的坐标+航向角
     val targetTime = startTime + 2000 // 目标时间：2秒后
     val interpolatedAt2s = TimeBasedGeoInterpolator.interpolateAtTime(
         start = startPoint,
         end = endPoint,
         targetTime = targetTime,
         useSpherical = false // 短距离用线性插值
     )
     println("2秒后插值结果：")
     println("纬度：${interpolatedAt2s.lat.format(6)}")
     println("经度：${interpolatedAt2s.lng.format(6)}")
     println("航向角：${interpolatedAt2s.heading}°")
     println("时间戳：${interpolatedAt2s.timestamp}")

     // 示例3：按1秒间隔生成5秒内的所有插值点
     val intervalPoints = TimeBasedGeoInterpolator.interpolateByTimeInterval(
         start = startPoint,
         end = endPoint,
         intervalMs = 1000, // 每秒一个点
         useSpherical = false
     )
     println("\n按1秒间隔插值结果：")
     intervalPoints.forEachIndexed { index, point ->
         val timeElapsed = (point.timestamp - startTime) / 1000.0
         println("第${timeElapsed}秒：纬度=${point.lat.format(6)}, 经度=${point.lng.format(6)}, 航向=${point.heading}°")
     }

     // 示例4：计算移动速度
     val speed = TimeBasedGeoInterpolator.calculateSpeed(startPoint, endPoint)
     println("\n移动速度：${speed.format(2)} 米/秒")*/
}

// 扩展函数：Double保留指定位数小数
fun Double.format(digits: Int): String = "%.${digits}f".format(this)