<?php

class OptimizedFlightPathGenerator
{
    private $startPoint;
    private $points;
    private $noFlyZone;
    private $highVoltage;
    
    const EARTH_RADIUS = 6371000;
    
    public function __construct($flightInput)
    {
        $this->startPoint = $flightInput['start_point'];
        $this->points = $flightInput['points'];
        $this->noFlyZone = $flightInput['no_fly_zone'];
        $this->highVoltage = $flightInput['high_voltage'];
    }
    
    /**
     * 生成优化的航线路径
     */
    public function generateOptimizedFlightPath()
    {
        // 使用凸包算法找到外围点，然后填充内部点
        $optimizedPath = $this->generateEfficientPath();
        
        // 检查并处理禁飞区相交 - 使用简化的避让算法
        $pathAfterNoFly = $this->handleNoFlyZoneIntersectionsSimple($optimizedPath);
        
        // 检查并处理高压线相交
        $finalPath = $this->handleHighVoltageIntersections($pathAfterNoFly);
        
        return $finalPath;
    }
    
    /**
     * 简化的禁飞区相交处理方法 - 减少避让点数量
     */
    private function handleNoFlyZoneIntersectionsSimple($path)
    {
        $maxIterations = 10;
        $iteration = 0;
        $simplifiedPath = $path;
        
        do {
            $hasIntersection = false;
            $newPath = [];
            
            for ($i = 0; $i < count($simplifiedPath) - 1; $i++) {
                $currentPoint = $simplifiedPath[$i];
                $nextPoint = $simplifiedPath[$i + 1];
                
                // 检查当前线段是否与禁飞区相交
                $intersections = $this->checkLineCircleIntersection(
                    $currentPoint, 
                    $nextPoint, 
                    $this->noFlyZone['center'], 
                    $this->noFlyZone['radius']
                );
                
                if (!empty($intersections)) {
                    // 发现相交，采用简化避让策略
                    $hasIntersection = true;
                    
                    // 只在真正需要的地方添加一个避让点
                    $detourPoint = $this->generateSingleDetourPoint($currentPoint, $nextPoint);
                    
                    $newPath[] = $currentPoint;
                    $newPath[] = $detourPoint;
                    // 不添加nextPoint，它会在下一次循环中处理
                } else {
                    // 没有相交，保持原路径点
                    $newPath[] = $currentPoint;
                    
                    // 如果是最后一个点，也要添加
                    if ($i == count($simplifiedPath) - 2) {
                        $newPath[] = $nextPoint;
                    }
                }
            }
            
            $simplifiedPath = $newPath;
            $iteration++;
            
        } while ($hasIntersection && $iteration < $maxIterations);
        
        // 对最终路径进行简化，移除不必要的点
        return $this->simplifyPath($simplifiedPath);
    }
    
    /**
     * 生成单个避让点（简化版本）
     */
    private function generateSingleDetourPoint($pointA, $pointB)
    {
        $center = $this->noFlyZone['center'];
        $radius = $this->noFlyZone['radius'];
        $safeDistance = 100;
        
        // 计算线段中点
        $midPoint = [
            'lng' => ($pointA['lng'] + $pointB['lng']) / 2,
            'lat' => ($pointA['lat'] + $pointB['lat']) / 2
        ];
        
        // 计算从禁飞区中心指向中点的方向
        $dx = $midPoint['lng'] - $center['lng'];
        $dy = $midPoint['lat'] - $center['lat'];
        
        $distanceToCenter = $this->calculateDistance($midPoint, $center);
        
        if ($distanceToCenter <= $radius + $safeDistance) {
            // 需要向外移动
            $moveDistance = $radius + $safeDistance - $distanceToCenter;
            $vectorLength = sqrt($dx * $dx + $dy * $dy);
            
            if ($vectorLength > 0) {
                $dx /= $vectorLength;
                $dy /= $vectorLength;
                
                return [
                    'lng' => $midPoint['lng'] + $dx * $moveDistance * 0.00001,
                    'lat' => $midPoint['lat'] + $dy * $moveDistance * 0.00001,
                    'name' => '禁飞区避让点',
                    'type' => 'no_fly_zone_avoidance'
                ];
            }
        }
        
        // 如果已经在安全距离外，返回中点
        return [
            'lng' => $midPoint['lng'],
            'lat' => $midPoint['lat'],
            'name' => '路径中点',
            'type' => 'midpoint'
        ];
    }
    
    /**
     * 路径简化算法 - 移除不必要的中间点
     */
    private function simplifyPath($path, $tolerance = 0.0001)
    {
        if (count($path) <= 3) {
            return $path;
        }
        
        $simplified = [];
        $simplified[] = $path[0]; // 总是保留起点
        
        for ($i = 1; $i < count($path) - 1; $i++) {
            $current = $path[$i];
            $prev = $path[$i - 1];
            $next = $path[$i + 1];
            
            // 如果是关键点（避让点、起点、终点等），总是保留
            if (isset($current['type']) && 
                ($current['type'] === 'no_fly_zone_avoidance' || 
                 $current['type'] === 'high_voltage_intersection' ||
                 $current['type'] === 'no_fly_zone_detour')) {
                $simplified[] = $current;
                continue;
            }
            
            // 检查当前点是否可以被移除（与前后点形成的角度是否足够大）
            if ($this->isPointNecessary($prev, $current, $next, $tolerance)) {
                $simplified[] = $current;
            }
        }
        
        $simplified[] = $path[count($path) - 1]; // 总是保留终点
        
        return $simplified;
    }
    
    /**
     * 检查点是否必要（基于角度变化）
     */
    private function isPointNecessary($prev, $current, $next, $tolerance)
    {
        // 计算向量
        $v1 = [
            'lng' => $current['lng'] - $prev['lng'],
            'lat' => $current['lat'] - $prev['lat']
        ];
        
        $v2 = [
            'lng' => $next['lng'] - $current['lng'],
            'lat' => $next['lat'] - $current['lat']
        ];
        
        // 计算向量长度
        $len1 = sqrt($v1['lng'] * $v1['lng'] + $v1['lat'] * $v1['lat']);
        $len2 = sqrt($v2['lng'] * $v2['lng'] + $v2['lat'] * $v2['lat']);
        
        if ($len1 < 0.00001 || $len2 < 0.00001) {
            return true; // 避免除零错误
        }
        
        // 计算点积和角度
        $dotProduct = $v1['lng'] * $v2['lng'] + $v1['lat'] * $v2['lat'];
        $cosAngle = $dotProduct / ($len1 * $len2);
        
        // 如果角度接近180度（直线），可以移除该点
        // cos(180°) = -1，我们使用容差值来判断
        return ($cosAngle < 0.95); // 如果角度变化大于约18度，保留该点
    }
    
    /**
     * 改进的路径生成 - 减少不必要的航点
     */
    private function generateEfficientPath()
    {
        if (count($this->points) <= 2) {
            return $this->nearestNeighborPath();
        }
        
        // 使用改进的最近邻算法，减少路径复杂度
        $path = $this->optimizedNearestNeighbor();
        
        $fullPath = [$this->startPoint];
        foreach ($path as $point) {
            $fullPath[] = $point;
        }
        $fullPath[] = $this->startPoint;
        
        return $fullPath;
    }
    
    /**
     * 优化的最近邻算法
     */
    private function optimizedNearestNeighbor()
    {
        $unvisited = $this->points;
        $currentPoint = $this->startPoint;
        $path = [];
        
        while (!empty($unvisited)) {
            $nearestIndex = $this->findNearestPoint($currentPoint, $unvisited);
            $nearestPoint = $unvisited[$nearestIndex];
            
            $path[] = $nearestPoint;
            $currentPoint = $nearestPoint;
            
            array_splice($unvisited, $nearestIndex, 1);
        }
        
        return $path;
    }
    
    // 以下方法保持不变，只做必要的最小修改...
    private function findNearestPoint($currentPoint, $points)
    {
        $minDistance = PHP_FLOAT_MAX;
        $nearestIndex = 0;
        
        foreach ($points as $index => $point) {
            $distance = $this->calculateDistance($currentPoint, $point);
            if ($distance < $minDistance) {
                $minDistance = $distance;
                $nearestIndex = $index;
            }
        }
        
        return $nearestIndex;
    }
    
    private function handleHighVoltageIntersections($path)
    {
        $finalPath = [];
        
        for ($i = 0; $i < count($path) - 1; $i++) {
            $currentPoint = $path[$i];
            $nextPoint = $path[$i + 1];
            
            $finalPath[] = $currentPoint;
            
            $intersections = $this->checkHighVoltageIntersections(
                $currentPoint, 
                $nextPoint
            );
            
            if (!empty($intersections)) {
                // 只添加第一个相交点，避免过多点
                $intersection = $intersections[0];
                $finalPath[] = [
                    'name' => '高压线避让点',
                    'lng' => $intersection['lng'],
                    'lat' => $intersection['lat'],
                    'type' => 'high_voltage_avoidance'
                ];
            }
        }
        
        $finalPath[] = $path[count($path) - 1];
        
        return $finalPath;
    }
    
    private function checkHighVoltageIntersections($pointA, $pointB)
    {
        $intersections = [];
        $towers = $this->highVoltage['towers'];
        
        for ($i = 0; $i < count($towers) - 1; $i++) {
            $tower1 = $towers[$i];
            $tower2 = $towers[$i + 1];
            
            $intersection = $this->checkLineSegmentIntersection(
                $pointA, $pointB, $tower1, $tower2
            );
            
            if ($intersection !== null) {
                $intersections[] = [
                    'lng' => $intersection['lng'],
                    'lat' => $intersection['lat'],
                    'tower1' => $tower1,
                    'tower2' => $tower2
                ];
                break; // 只找一个相交点
            }
        }
        
        return $intersections;
    }
    
    private function checkLineSegmentIntersection($line1A, $line1B, $line2A, $line2B)
    {
        $p1 = $this->latLngToMeters($line1A['lat'], $line1A['lng']);
        $p2 = $this->latLngToMeters($line1B['lat'], $line1B['lng']);
        $p3 = $this->latLngToMeters($line2A['lat'], $line2A['lng']);
        $p4 = $this->latLngToMeters($line2B['lat'], $line2B['lng']);
        
        $denominator = (($p4[1] - $p3[1]) * ($p2[0] - $p1[0]) - ($p4[0] - $p3[0]) * ($p2[1] - $p1[1]));
        
        if ($denominator == 0) {
            return null;
        }
        
        $ua = (($p4[0] - $p3[0]) * ($p1[1] - $p3[1]) - ($p4[1] - $p3[1]) * ($p1[0] - $p3[0])) / $denominator;
        $ub = (($p2[0] - $p1[0]) * ($p1[1] - $p3[1]) - ($p2[1] - $p1[1]) * ($p1[0] - $p3[0])) / $denominator;
        
        if ($ua >= 0 && $ua <= 1 && $ub >= 0 && $ub <= 1) {
            $x = $p1[0] + $ua * ($p2[0] - $p1[0]);
            $y = $p1[1] + $ua * ($p2[1] - $p1[1]);
            
            $latLng = $this->metersToLatLng($x, $y);
            
            return [
                'lng' => $latLng['lng'],
                'lat' => $latLng['lat']
            ];
        }
        
        return null;
    }
    
    private function checkLineCircleIntersection($pointA, $pointB, $circleCenter, $radius)
    {
        $A = $this->latLngToMeters($pointA['lat'], $pointA['lng']);
        $B = $this->latLngToMeters($pointB['lat'], $pointB['lng']);
        $C = $this->latLngToMeters($circleCenter['lat'], $circleCenter['lng']);
        
        $AB = [$B[0] - $A[0], $B[1] - $A[1]];
        $AC = [$C[0] - $A[0], $C[1] - $A[1]];
        
        $ab2 = $AB[0] * $AB[0] + $AB[1] * $AB[1];
        $t = ($AC[0] * $AB[0] + $AC[1] * $AB[1]) / $ab2;
        
        $closestPoint = [
            $A[0] + $t * $AB[0],
            $A[1] + $t * $AB[1]
        ];
        
        $distance = sqrt(
            pow($closestPoint[0] - $C[0], 2) + 
            pow($closestPoint[1] - $C[1], 2)
        );
        
        $intersections = [];
        
        if ($distance <= $radius) {
            $dt = sqrt($radius * $radius - $distance * $distance) / sqrt($ab2);
            
            $t1 = $t - $dt;
            $t2 = $t + $dt;
            
            if ($t1 >= 0 && $t1 <= 1) {
                $intersection1 = [
                    $A[0] + $t1 * $AB[0],
                    $A[1] + $t1 * $AB[1]
                ];
                $intersectionLatLng1 = $this->metersToLatLng($intersection1[0], $intersection1[1]);
                $intersections[] = [
                    'lng' => $intersectionLatLng1['lng'],
                    'lat' => $intersectionLatLng1['lat']
                ];
            }
            
            if ($t2 >= 0 && $t2 <= 1) {
                $intersection2 = [
                    $A[0] + $t2 * $AB[0],
                    $A[1] + $t2 * $AB[1]
                ];
                $intersectionLatLng2 = $this->metersToLatLng($intersection2[0], $intersection2[1]);
                $intersections[] = [
                    'lng' => $intersectionLatLng2['lng'],
                    'lat' => $intersectionLatLng2['lat']
                ];
            }
        }
        
        return $intersections;
    }
    
    private function latLngToMeters($lat, $lng)
    {
        $x = $lng * (M_PI / 180) * self::EARTH_RADIUS * cos($lat * M_PI / 180);
        $y = $lat * (M_PI / 180) * self::EARTH_RADIUS;
        return [$x, $y];
    }
    
    private function metersToLatLng($x, $y)
    {
        $lng = $x / (self::EARTH_RADIUS * cos($y / self::EARTH_RADIUS));
        $lat = $y / self::EARTH_RADIUS;
        
        return [
            'lng' => $lng * (180 / M_PI),
            'lat' => $lat * (180 / M_PI)
        ];
    }
    
    private function calculateDistance($point1, $point2)
    {
        $lat1 = $point1['lat'];
        $lng1 = $point1['lng'];
        $lat2 = $point2['lat'];
        $lng2 = $point2['lng'];
        
        $dLat = deg2rad($lat2 - $lat1);
        $dLng = deg2rad($lng2 - $lng1);
        
        $a = sin($dLat/2) * sin($dLat/2) + 
             cos(deg2rad($lat1)) * cos(deg2rad($lat2)) * 
             sin($dLng/2) * sin($dLng/2);
        $c = 2 * atan2(sqrt($a), sqrt(1-$a));
        
        return self::EARTH_RADIUS * $c;
    }
    
    public function printPathInfo($path)
    {
        echo "生成的优化航线路径:\n";
        echo "总点数: " . count($path) . "\n\n";
        
        $totalDistance = 0;
        
        for ($i = 0; $i < count($path); $i++) {
            $point = $path[$i];
            $name = isset($point['name']) ? $point['name'] : (isset($point['type']) ? $point['type'] : '航点');
            $type = isset($point['type']) ? " ({$point['type']})" : "";
            
            echo ($i + 1) . ". {$name}{$type}";
            echo "   经度: " . number_format($point['lng'], 9) . ", 纬度: " . number_format($point['lat'], 9) . "\n";
            
            if ($i > 0) {
                $segmentDistance = $this->calculateDistance($path[$i-1], $point);
                $totalDistance += $segmentDistance;
            }
        }
        
        echo "\n总路径长度: " . number_format($totalDistance, 2) . " 米\n";
    }
}

// 使用示例
$flightInput = [
    "start_point" => ["lng" => 119.728918245, "lat" => 29.158205373],
    "points" => [
        ["name" => "航点1", "lng" => 119.731844828, "lat" => 29.155170432],
        ["name" => "航点2", "lng" => 119.734061703, "lat" => 29.157567795],
        ["name" => "航点3", "lng" => 119.731450602, "lat" => 29.163489386],
        ["name" => "航点4", "lng" => 119.727492742, "lat" => 29.165032019],
        ["name" => "航点5", "lng" => 119.727926162, "lat" => 29.162911848]
    ],
    "no_fly_zone" => [
        "center" => ["lng" => 119.729029043, "lat" => 29.161302197],
        "radius" => 200
    ],
    "high_voltage" => [
        "towers" => [
            ["lng" => 119.728687943, "lat" => 29.156864388],
            ["lng" => 119.735076462, "lat" => 29.160592962]
        ]
    ]
];

// 创建航线生成器实例
$flightGenerator = new OptimizedFlightPathGenerator($flightInput);

// 生成优化航线
$flightPath = $flightGenerator->generateOptimizedFlightPath();
// 输出航线信息
$flightGenerator->printPathInfo($flightPath);
?>