import numpy as np
import math
from . import config

class DistanceCalculator:
    def __init__(self):
        self.focal_length = config.FOCAL_LENGTH
        self.known_height = config.KNOWN_PERSON_HEIGHT
        self.reference_distance = config.REFERENCE_DISTANCE
        self.reference_height_pixels = config.REFERENCE_HEIGHT_PIXELS
        
    def calculate_distance_by_height(self, bbox_height):
        """
        根据人体框高度计算距离
        使用相似三角形原理：距离 = (已知高度 × 焦距) / 像素高度
        """
        if bbox_height <= 0:
            return 0
        
        # 使用参考距离和参考像素高度来校准
        distance = (self.reference_distance * self.reference_height_pixels) / bbox_height
        return max(distance, 30)  # 最小距离限制为30cm
    
    def calculate_distance_by_focal_length(self, bbox_height):
        """
        使用焦距公式计算距离
        距离 = (真实高度 × 焦距) / 像素高度
        """
        if bbox_height <= 0:
            return 0
        
        distance = (self.known_height * self.focal_length) / bbox_height
        return max(distance, 30)  # 最小距离限制为30cm
    
    def calibrate_focal_length(self, known_distance, measured_height_pixels):
        """
        标定焦距
        焦距 = (像素高度 × 真实距离) / 真实高度
        """
        self.focal_length = (measured_height_pixels * known_distance) / self.known_height
        print(f"焦距已标定为: {self.focal_length:.2f}")
    
    def get_distance(self, bbox):
        """
        根据边界框计算距离
        bbox: [x1, y1, x2, y2]
        """
        x1, y1, x2, y2 = bbox
        bbox_height = y2 - y1
        bbox_width = x2 - x1
        
        # 使用高度计算距离（更准确）
        distance = self.calculate_distance_by_height(bbox_height)
        
        return distance
    
    def format_distance(self, distance):
        """
        格式化距离显示
        """
        if distance < 100:
            return f"{distance:.1f}cm"
        else:
            return f"{distance/100:.1f}m"
    
    def calculate_person_gps_position(self, camera_lat, camera_lng, camera_heading, 
                                    bbox, distance_meters, frame_width, frame_height, 
                                    camera_fov=60):
        """
        🎯 核心算法：根据摄像头GPS位置、朝向、人体检测框计算人员真实GPS坐标
        
        Args:
            camera_lat: 摄像头纬度
            camera_lng: 摄像头经度  
            camera_heading: 摄像头朝向角度 (0=正北, 90=正东)
            bbox: 人体检测框 [x1, y1, x2, y2]
            distance_meters: 人员距离摄像头的距离(米)
            frame_width: 画面宽度(像素)
            frame_height: 画面高度(像素)
            camera_fov: 摄像头水平视场角(度)
        
        Returns:
            (person_lat, person_lng): 人员GPS坐标
        """
        x1, y1, x2, y2 = bbox
        
        # 计算人体检测框中心点
        person_center_x = (x1 + x2) / 2
        person_center_y = (y1 + y2) / 2
        
        # 计算人员相对于画面中心的偏移角度
        frame_center_x = frame_width / 2
        horizontal_offset_pixels = person_center_x - frame_center_x
        
        # 将像素偏移转换为角度偏移
        horizontal_angle_per_pixel = camera_fov / frame_width
        horizontal_offset_degrees = horizontal_offset_pixels * horizontal_angle_per_pixel
        
        # 计算人员相对于正北的实际方位角
        person_bearing = (camera_heading + horizontal_offset_degrees) % 360
        
        # 使用球面几何计算人员GPS坐标
        person_lat, person_lng = self._calculate_destination_point(
            camera_lat, camera_lng, distance_meters, person_bearing
        )
        
        return person_lat, person_lng
    
    def _calculate_destination_point(self, lat, lng, distance, bearing):
        """
        🌍 球面几何计算：根据起点坐标、距离和方位角计算目标点坐标
        
        Args:
            lat: 起点纬度
            lng: 起点经度
            distance: 距离(米)
            bearing: 方位角(度，0=正北)
        
        Returns:
            (target_lat, target_lng): 目标点坐标
        """
        # 地球半径(米)
        R = 6371000
        
        # 转换为弧度
        lat1 = math.radians(lat)
        lng1 = math.radians(lng)
        bearing_rad = math.radians(bearing)
        
        # 球面几何计算目标点坐标
        lat2 = math.asin(
            math.sin(lat1) * math.cos(distance / R) +
            math.cos(lat1) * math.sin(distance / R) * math.cos(bearing_rad)
        )
        
        lng2 = lng1 + math.atan2(
            math.sin(bearing_rad) * math.sin(distance / R) * math.cos(lat1),
            math.cos(distance / R) - math.sin(lat1) * math.sin(lat2)
        )
        
        return math.degrees(lat2), math.degrees(lng2)
    
    def is_person_in_camera_fov(self, camera_lat, camera_lng, camera_heading,
                              person_lat, person_lng, camera_fov=60, max_distance=100):
        """
        🔍 检查人员是否在摄像头视野范围内
        
        Args:
            camera_lat: 摄像头纬度
            camera_lng: 摄像头经度
            camera_heading: 摄像头朝向角度
            person_lat: 人员纬度  
            person_lng: 人员经度
            camera_fov: 摄像头视场角(度)
            max_distance: 最大检测距离(米)
        
        Returns:
            bool: 是否在视野内
        """
        # 计算人员相对于摄像头的距离和方位角
        distance, bearing = self._calculate_distance_and_bearing(
            camera_lat, camera_lng, person_lat, person_lng
        )
        
        # 检查距离是否在范围内
        if distance > max_distance:
            return False
        
        # 计算人员方位角与摄像头朝向的角度差
        angle_diff = abs(bearing - camera_heading)
        if angle_diff > 180:
            angle_diff = 360 - angle_diff
        
        # 检查是否在视场角范围内
        return angle_diff <= camera_fov / 2
    
    def _calculate_distance_and_bearing(self, lat1, lng1, lat2, lng2):
        """
        🧭 计算两点间距离和方位角
        
        Returns:
            (distance_meters, bearing_degrees): 距离(米)和方位角(度)
        """
        # 转换为弧度
        lat1_rad = math.radians(lat1)
        lng1_rad = math.radians(lng1)
        lat2_rad = math.radians(lat2)
        lng2_rad = math.radians(lng2)
        
        # 计算距离 (Haversine公式)
        dlat = lat2_rad - lat1_rad
        dlng = lng2_rad - lng1_rad
        
        a = (math.sin(dlat/2)**2 + 
             math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(dlng/2)**2)
        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
        distance = 6371000 * c  # 地球半径6371km
        
        # 计算方位角
        y = math.sin(dlng) * math.cos(lat2_rad)
        x = (math.cos(lat1_rad) * math.sin(lat2_rad) - 
             math.sin(lat1_rad) * math.cos(lat2_rad) * math.cos(dlng))
        bearing = math.atan2(y, x)
        bearing_degrees = (math.degrees(bearing) + 360) % 360
        
        return distance, bearing_degrees 