package commons.gis

import kotlinx.coroutines.Dispatchers
import kotlinx.coroutines.withContext
import java.math.RoundingMode
import java.util.Collections
import kotlin.math.PI
import kotlin.math.abs
import kotlin.math.cos
import kotlin.math.pow

/*弯道分类标准（基于高速公路设计规范）
直道：半径 > 2000米（曲率 < 0.0005）
缓弯：半径 500-2000米
中弯：半径 200-500米
急弯：半径 < 200米*/

/**
 * 地理坐标点类
 * @param longitude 经度
 * @param latitude 纬度
 */
data class CurvePoint(
    val longitude: Double,
    val latitude: Double
)

/**
 * 弯道分析结果
 * @param curvature 曲率 (1/m)
 * @param radius 曲率半径 (m)，如果是直线则为无穷大
 * @param curveDirection 弯道方向：-1=左弯，0=直道，1=右弯
 * @param curveMagnitude 弯道大小等级：0=直道，1=小弯，2=中弯，3=大弯
 * @param angle 转向角度 (度)
 */
data class CurveAnalysis(
    val curvature: Double,
    val radius: Double,
    var curveDirection: Int,
    val curveMagnitude: Int,
    val angle: Double
)

/**
 * 三点分析结果
 *  @param avgDistance 3点2线段的平均距离
 * @param angle 转向角度 (度)
 */
data class ThreePointAnalysis(
    val avgDistance: Double,
    val angle: Double
)

/**
 * 地球相关常数
 */
object EarthConstants {
    // 地球半径 (米)
    const val EARTH_RADIUS = 6371000.0

    // 弧度转角度
    const val RAD_TO_DEG = 180.0 / PI

    // 弯道大小阈值 (米)
    const val SMALL_CURVE_THRESHOLD = 2000.0
    const val MEDIUM_CURVE_THRESHOLD = 500.0
    const val LARGE_CURVE_THRESHOLD = 200.0
}

/**
 * 弯道分析器
 */
object CurveAnalyzer {

    /**
     * 计算两点间的地理距离 (Haversine公式)
     */
    private fun calculateDistance(point1: CurvePoint, point2: CurvePoint): Double {
        val dis=BasicTools.calculateDistance(point1.longitude,point1.latitude,point2.longitude,point2.latitude)
        return dis.toDouble()

/*        val lat1Rad = Math.toRadians(point1.latitude)
        val lat2Rad = Math.toRadians(point2.latitude)
        val deltaLatRad = Math.toRadians(point2.latitude - point1.latitude)
        val deltaLonRad = Math.toRadians(point2.longitude - point1.longitude)

        val a = sin(deltaLatRad / 2).pow(2) +
                cos(lat1Rad) * cos(lat2Rad) *
                sin(deltaLonRad / 2).pow(2)

        val c = 2 * atan2(sqrt(a), sqrt(1 - a))

        return EarthConstants.EARTH_RADIUS * c*/
    }

/*    private fun calculateDistance(point1: GeoPoint, point2: GeoPoint): Double {
        val lat1Rad = Math.toRadians(point1.latitude)
        val lat2Rad = Math.toRadians(point2.latitude)
        val deltaLatRad = Math.toRadians(point2.latitude - point1.latitude)
        val deltaLonRad = Math.toRadians(point2.longitude - point1.longitude)

        val a = sin(deltaLatRad / 2).pow(2) +
                cos(lat1Rad) * cos(lat2Rad) *
                sin(deltaLonRad / 2).pow(2)

        val c = 2 * atan2(sqrt(a), sqrt(1 - a))

        return EarthConstants.EARTH_RADIUS * c
    }*/

    /**
     * 计算两点间的方位角 (bearing)
     */
    private fun calculateBearing(point1: CurvePoint, point2: CurvePoint): Double {
        val pair=BasicTools.calculateDistancePair(point1.latitude,point1.longitude,point2.latitude,point2.longitude)
        return pair.second.toDouble()


/*        val lat1Rad = Math.toRadians(point1.latitude)
        val lat2Rad = Math.toRadians(point2.latitude)
        val lon1Rad = Math.toRadians(point1.longitude)
        val lon2Rad = Math.toRadians(point2.longitude)

        val y = sin(lon2Rad - lon1Rad) * cos(lat2Rad)
        val x = cos(lat1Rad) * sin(lat2Rad) -
                sin(lat1Rad) * cos(lat2Rad) * cos(lon2Rad - lon1Rad)

        var bearing = atan2(y, x)
        bearing = (bearing * EarthConstants.RAD_TO_DEG + 360) % 360

        return bearing*/
    }

    private var bufferSize = 0
    private var windowSize = 3

    //缓存轨迹
    private var bufList = Collections.synchronizedList(mutableListOf<CurvePoint>())

    /**
     *更新点位
     */
    suspend fun upTrajectory(geoPoint: CurvePoint, bufferSize: Int = 9): CurveAnalysis {
        return withContext(Dispatchers.Default){
            CurveAnalyzer.bufferSize = bufferSize
            if (bufList.count() > bufferSize) {
                val iterator = bufList.iterator()
                if (iterator.hasNext()) {
                    iterator.next()  // 移动到第一个元素
                    iterator.remove()  // 删除当前元素（第一个）
                }
            }
            bufList.add(geoPoint)
            val curveAnalysis = analyzeTrajectory(bufList)
            return@withContext curveAnalysis
        }
    }


    /**
     * 滑动窗口分析轨迹
     * @param points 轨迹点列表
     * @param windowSize 滑动窗口大小，默认为3
     * @return 每个中间点的分析结果列表
     */
    /*    private fun analyzeTrajectory(points: List<GeoPoint>, windowSize: Int = 3): List<CurveAnalysis> {
            if (points.count() < windowSize) {
                return emptyList()
            }
            val results = mutableListOf<CurveAnalysis>()

            for (i in 0..points.size - windowSize) {
                val windowPoints = points.subList(i, i + windowSize)
                val analysis = calculateThreePoints(windowPoints)
                results.add(analysis)
            }

            return results
        }*/
    /**
     * 滑动窗口分析轨迹
     * @param points 轨迹点列表
     * @return 基于一整段点的分析结果
     */
    private fun analyzeTrajectory(points: List<CurvePoint>): CurveAnalysis {
        if (points.count() < windowSize) {
            return CurveAnalysis(
                curvature = 1.0,
                radius = Double.POSITIVE_INFINITY,
                curveDirection = 0,
                curveMagnitude = 0,
                angle = 0.0
            )
        }
        //收集三点计算结果
        val threeResult = mutableListOf<ThreePointAnalysis>()
        for (i in 0..points.count() - windowSize) {
            val windowPoints = points.subList(i, i + windowSize)
            val analysis = calculateThreePoints(windowPoints)
            threeResult.add(analysis)
        }
        if (points.count() < bufferSize - windowSize) {
            return CurveAnalysis(
                curvature = 1.0,
                radius = 0.0,
                curveDirection = 0,
                curveMagnitude = 0,
                angle = 0.0
            )
        }
        //分析三点计算结果
        return analyzeThreePoints(threeResult)
    }

    /**收集三点计算结果*/
    private fun calculateThreePoints(points: List<CurvePoint>): ThreePointAnalysis {
        require(points.count() == 3) { "需要3个点进行计算" }

        val p0 = points[0]
        val p1 = points[1]
        val p2 = points[2]

        // 计算各段距离
        val d1 = calculateDistance(p0, p1)
        val d2 = calculateDistance(p1, p2)

        // 计算各段方位角
        val bearing1 = calculateBearing(p0, p1)
        val bearing2 = calculateBearing(p1, p2)

        // 计算转向角度（角度差）
//        var deltaAngle = bearing2 - bearing1
        val diffAngle = bearing2.toBigDecimal().subtract(bearing1.toBigDecimal()).setScale(
            5,
            RoundingMode.HALF_UP
        ).toDouble()
        // 规范化角度到 [-180, 180] 范围
        val deltaAngle = when {
            diffAngle > 180 -> diffAngle - 360
            diffAngle < -180 -> diffAngle + 360
            else -> diffAngle
        }

        // 计算曲率半径
        val avgDistance = (d1 + d2) / 2

        return ThreePointAnalysis(
            avgDistance = avgDistance,
            angle = deltaAngle
        )
    }

    /**
     * 使用三点法计算曲率和转向信息
     * @param points 三点计算结果
     * @return 弯道分析结果
     */
    private fun analyzeThreePoints(points: List<ThreePointAnalysis>): CurveAnalysis {
        // 计算转向角度（累计角度差）
        var totalDeltaAngle = points.sumOf { it.angle }
        // 计算平均移动距离
        val totalAvgDistance = points.map { it.avgDistance }.average()
        // 计算曲率半径
        val deltaAngleRad = Math.toRadians(totalDeltaAngle)
        // 防止除以零
        val radius = if (abs(deltaAngleRad) > 1e-10) {
            totalAvgDistance / abs(deltaAngleRad)
        } else {
            Double.POSITIVE_INFINITY
        }

        // 计算曲率
        val curvature = if (radius.isFinite()) {
            1.0 / radius
        } else {
            0.0
        }
        // 判断弯道方向
        val curveDirection = when {
            totalDeltaAngle > 2 -> 1    // 右转
            totalDeltaAngle < -2 -> -1  // 左转
            else -> 0              // 直行
        }
        // 判断弯道大小
        val curveMagnitude = when {
            !radius.isFinite() || radius > EarthConstants.SMALL_CURVE_THRESHOLD -> 0 // 直道
            radius > EarthConstants.MEDIUM_CURVE_THRESHOLD -> 1 // 缓弯
            radius > EarthConstants.LARGE_CURVE_THRESHOLD -> 2 // 中弯
            else -> 3 // 急弯
        }

        return CurveAnalysis(
            curvature = curvature,
            radius = radius,
            curveDirection = curveDirection,
            curveMagnitude = curveMagnitude,
            angle = totalDeltaAngle
        )
    }


    /**
     * 使用三点法计算曲率和转向信息
     * @param points 三个连续的点 [p0, p1, p2]
     * @return 弯道分析结果
     */
    /*    private fun analyzeThreePoints(points: List<GeoPoint>): CurveAnalysis {
            require(points.count() == 3) { "需要3个点进行计算" }

            val p0 = points[0]
            val p1 = points[1]
            val p2 = points[2]

            // 计算各段距离
            val d1 = calculateDistance(p0, p1)
            val d2 = calculateDistance(p1, p2)

            // 计算各段方位角
            val bearing1 = calculateBearing(p0, p1)
            val bearing2 = calculateBearing(p1, p2)

            // 计算转向角度（角度差）
            var deltaAngle = bearing2 - bearing1

            // 规范化角度到 [-180, 180] 范围
            deltaAngle = when {
                deltaAngle > 180 -> deltaAngle - 360
                deltaAngle < -180 -> deltaAngle + 360
                else -> deltaAngle
            }

            // 计算曲率半径
            val avgDistance = (d1 + d2) / 2
            val deltaAngleRad = Math.toRadians(deltaAngle)

            // 防止除以零
            val radius = if (abs(deltaAngleRad) > 1e-10) {
                avgDistance / abs(deltaAngleRad)
            } else {
                Double.POSITIVE_INFINITY
            }

            // 计算曲率
            val curvature = if (radius.isFinite()) {
                1.0 / radius
            } else {
                0.0
            }

            // 判断弯道方向（高速）
            val curveDirection = when {
                deltaAngle > 2 -> 1    // 右转
                deltaAngle < -2 -> -1  // 左转
                else -> 0              // 直行
            }

            // 判断弯道大小
            val curveMagnitude = when {
                !radius.isFinite() || radius > EarthConstants.SMALL_CURVE_THRESHOLD -> 0 // 直道
                radius > EarthConstants.MEDIUM_CURVE_THRESHOLD -> 1 // 小弯
                radius > EarthConstants.LARGE_CURVE_THRESHOLD -> 2 // 中弯
                else -> 3 // 大弯
            }

            return CurveAnalysis(
                curvature = curvature,
                radius = radius,
                curveDirection = curveDirection,
                curveMagnitude = curveMagnitude,
                angle = deltaAngle
            )
        }*/


    /**
     * 使用最小二乘法拟合曲线计算曲率（更精确的方法）
     * @param points 多个点（至少3个）
     * @return 拟合的曲率
     */
    fun calculateCurvatureByLeastSquares(points: List<CurvePoint>): Double {
        if (points.size < 3) return 0.0

        // 将经纬度转换为平面坐标（使用UTM简化投影）
        val xyPoints = points.map { point ->
            // 简化的投影转换：使用经纬度的线性近似（适用于小范围）
            // 在实际应用中，应考虑使用UTM或其他投影
            val x =
                point.longitude * EarthConstants.EARTH_RADIUS * cos(Math.toRadians(point.latitude))
            val y = point.latitude * EarthConstants.EARTH_RADIUS
            Pair(x, y)
        }

        // 计算二阶导数来估算曲率
        val n = xyPoints.size
        val curvatures = mutableListOf<Double>()

        for (i in 1 until n - 1) {
            val pPrev = xyPoints[i - 1]
            val pCurr = xyPoints[i]
            val pNext = xyPoints[i + 1]

            // 计算一阶导数（中心差分）
            val dxPrev = pCurr.first - pPrev.first
            val dyPrev = pCurr.second - pPrev.second
            val dxNext = pNext.first - pCurr.first
            val dyNext = pNext.second - pCurr.second

            val dx = (dxPrev + dxNext) / 2
            val dy = (dyPrev + dyNext) / 2

            // 计算二阶导数
            val d2x = dxNext - dxPrev
            val d2y = dyNext - dyPrev

            // 计算曲率
            val numerator = abs(dx * d2y - dy * d2x)
            val denominator = (dx * dx + dy * dy).pow(1.5)

            val curvature = if (denominator > 1e-10) {
                numerator / denominator
            } else {
                0.0
            }

            curvatures.add(curvature)
        }

        // 返回平均曲率
        return curvatures.average()
    }

    /**
     * 格式化输出分析结果
     */
    fun formatAnalysis(analysis: CurveAnalysis): String {
        val directionStr = when (analysis.curveDirection) {
            -1 -> "左转"
            0 -> "直行"
            1 -> "右转"
            else -> "未知"
        }

        val magnitudeStr = when (analysis.curveMagnitude) {
            0 -> "直道"
            1 -> "小弯"
            2 -> "中弯"
            3 -> "大弯"
            else -> "未知"
        }

        return String.format(
            "曲率: %.6f 1/m, 半径: %.1f m, 方向: %s, 大小: %s, 角度: %.1f°",
            analysis.curvature,
            if (analysis.radius.isFinite()) analysis.radius else Double.POSITIVE_INFINITY,
            directionStr,
            magnitudeStr,
            analysis.angle
        )
    }
}

/**
 * 使用示例
 */
/*fun main() {
    val analyzer = CurveAnalyzer()

    // 示例轨迹点（经纬度）
    val trajectory = listOf(
        GeoPoint(116.397128, 39.916527),  // 北京天安门
        GeoPoint(116.397428, 39.916727),  // 向东向北移动
        GeoPoint(116.397828, 39.916527),  // 向东移动，形成右转
        GeoPoint(116.398128, 39.916327),  // 继续右转
        GeoPoint(116.398028, 39.916127)   // 轻微左转
    )

    println("=== 弯道分析示例 ===")
    println("轨迹点数量: ${trajectory.size}")
    println()

    // 方法1：滑动窗口分析
    println("方法1: 滑动窗口分析 (3点法)")
    val slidingResults = analyzer.analyzeTrajectory(trajectory)
    slidingResults.forEachIndexed { index, analysis ->
        println("窗口 ${index + 1}: ${analyzer.formatAnalysis(analysis)}")
    }

    println()

    // 方法2：最小二乘法分析整个轨迹
    println("方法2: 最小二乘法拟合曲率")
    val curvature = analyzer.calculateCurvatureByLeastSquares(trajectory)
    println("平均曲率: ${"%.6f".format(curvature)} 1/m")
    println("平均半径: ${"%.1f".format(1.0 / curvature)} m")

    println()

    // 计算单个弯道
    println("方法3: 三点法计算单个弯道")
    val threePoints = trajectory.take(3)
    val singleAnalysis = analyzer.analyzeThreePoints(threePoints)
    println("三点分析: ${analyzer.formatAnalysis(singleAnalysis)}")
}*/

/**
 * 扩展函数：计算两点间距离
 */
//fun GeoPoint.distanceTo(other: GeoPoint): Double {
//    return calculateDistance(this, other)
//}

/**
 * 扩展函数：计算到另一点的方位角
 */
//fun GeoPoint.bearingTo(other: GeoPoint): Double {
//    return CurveAnalyzer().calculateBearing(this, other)
//}

/**
 * 单位转换工具
 */
/*
object UnitConverter {
    */
/**
     * 弧度转角度
     *//*

    fun radiansToDegrees(radians: Double): Double {
        return radians * EarthConstants.RAD_TO_DEG
    }

    */
/**
     * 角度转弧度
     *//*

    fun degreesToRadians(degrees: Double): Double {
        return degrees / EarthConstants.RAD_TO_DEG
    }

    */
/**
     * 米转千米
     *//*

    fun metersToKilometers(meters: Double): Double {
        return meters / 1000.0
    }

    */
/**
     * 千米转米
     *//*

    fun kilometersToMeters(kilometers: Double): Double {
        return kilometers * 1000.0
    }


}*/