utils/helpers.py

"""
Utility Functions
=================
Helper functions for the Traffic Accident Reconstruction System.
"""

import numpy as np
from typing import List, Tuple, Dict, Any
from datetime import datetime
import math


def calculate_distance(point1: List[float], point2: List[float]) -> float:
    """
    Calculate the Haversine distance between two geographic points.
    
    Args:
        point1: [latitude, longitude]
        point2: [latitude, longitude]
    
    Returns:
        Distance in meters
    """
    R = 6371000  # Earth's radius in meters
    
    lat1, lon1 = math.radians(point1[0]), math.radians(point1[1])
    lat2, lon2 = math.radians(point2[0]), math.radians(point2[1])
    
    dlat = lat2 - lat1
    dlon = lon2 - lon1
    
    a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2
    c = 2 * math.asin(math.sqrt(a))
    
    return R * c


def calculate_bearing(point1: List[float], point2: List[float]) -> float:
    """
    Calculate the bearing between two points.
    
    Args:
        point1: [latitude, longitude]
        point2: [latitude, longitude]
    
    Returns:
        Bearing in degrees (0-360)
    """
    lat1, lon1 = math.radians(point1[0]), math.radians(point1[1])
    lat2, lon2 = math.radians(point2[0]), math.radians(point2[1])
    
    dlon = lon2 - lon1
    
    x = math.sin(dlon) * math.cos(lat2)
    y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
    
    bearing = math.atan2(x, y)
    bearing = math.degrees(bearing)
    bearing = (bearing + 360) % 360
    
    return bearing


def interpolate_path(path: List[List[float]], num_points: int = 100) -> List[List[float]]:
    """
    Interpolate a path to have more points for smoother simulation.
    
    Args:
        path: List of [lat, lng] coordinates
        num_points: Number of points in the interpolated path
    
    Returns:
        Interpolated path
    """
    if len(path) < 2:
        return path
    
    path_array = np.array(path)
    
    # Calculate cumulative distances
    distances = [0]
    for i in range(1, len(path)):
        d = calculate_distance(path[i-1], path[i])
        distances.append(distances[-1] + d)
    
    total_distance = distances[-1]
    if total_distance == 0:
        return path
    
    # Interpolate
    target_distances = np.linspace(0, total_distance, num_points)
    interpolated = []
    
    for target in target_distances:
        # Find the segment
        for i in range(len(distances) - 1):
            if distances[i] <= target <= distances[i+1]:
                if distances[i+1] - distances[i] == 0:
                    t = 0
                else:
                    t = (target - distances[i]) / (distances[i+1] - distances[i])
                
                lat = path[i][0] + t * (path[i+1][0] - path[i][0])
                lng = path[i][1] + t * (path[i+1][1] - path[i][1])
                interpolated.append([lat, lng])
                break
    
    return interpolated


def estimate_speed_at_point(
    path: List[List[float]],
    initial_speed: float,
    point_index: int,
    is_braking: bool = False,
    is_accelerating: bool = False
) -> float:
    """
    Estimate speed at a specific point along the path.
    
    Args:
        path: Vehicle path
        initial_speed: Initial speed in km/h
        point_index: Index of the point
        is_braking: Whether vehicle is braking
        is_accelerating: Whether vehicle is accelerating
    
    Returns:
        Estimated speed at the point in km/h
    """
    if not path or point_index >= len(path):
        return initial_speed
    
    progress = point_index / len(path)
    
    if is_braking:
        # Assume linear deceleration
        return initial_speed * (1 - progress * 0.5)
    elif is_accelerating:
        # Assume slight acceleration
        return initial_speed * (1 + progress * 0.2)
    else:
        return initial_speed


def find_intersection_point(
    path1: List[List[float]],
    path2: List[List[float]]
) -> Tuple[List[float], int, int]:
    """
    Find the intersection point between two paths.
    
    Args:
        path1: First vehicle path
        path2: Second vehicle path
    
    Returns:
        Tuple of (intersection point, index in path1, index in path2)
    """
    if not path1 or not path2:
        return None, -1, -1
    
    min_distance = float('inf')
    intersection = None
    idx1, idx2 = -1, -1
    
    for i, p1 in enumerate(path1):
        for j, p2 in enumerate(path2):
            dist = calculate_distance(p1, p2)
            if dist < min_distance:
                min_distance = dist
                intersection = [(p1[0] + p2[0]) / 2, (p1[1] + p2[1]) / 2]
                idx1, idx2 = i, j
    
    # Only consider as intersection if distance is small enough
    if min_distance < 50:  # 50 meters threshold
        return intersection, idx1, idx2
    
    return None, -1, -1


def calculate_impact_angle(
    direction1: str,
    direction2: str
) -> float:
    """
    Calculate the angle of impact between two vehicles.
    
    Args:
        direction1: Direction of vehicle 1
        direction2: Direction of vehicle 2
    
    Returns:
        Impact angle in degrees
    """
    direction_angles = {
        'north': 0, 'northeast': 45, 'east': 90, 'southeast': 135,
        'south': 180, 'southwest': 225, 'west': 270, 'northwest': 315
    }
    
    angle1 = direction_angles.get(direction1, 0)
    angle2 = direction_angles.get(direction2, 90)
    
    diff = abs(angle1 - angle2)
    if diff > 180:
        diff = 360 - diff
    
    return diff


def estimate_stopping_distance(speed_kmh: float, road_condition: str = 'dry') -> float:
    """
    Estimate the stopping distance for a vehicle.
    
    Args:
        speed_kmh: Speed in km/h
        road_condition: 'dry', 'wet', 'sandy', 'oily'
    
    Returns:
        Stopping distance in meters
    """
    speed_ms = speed_kmh / 3.6
    
    # Friction coefficients
    friction = {
        'dry': 0.8,
        'wet': 0.5,
        'sandy': 0.4,
        'oily': 0.25
    }
    
    mu = friction.get(road_condition, 0.8)
    g = 9.81  # Gravity
    
    # Stopping distance = v² / (2 * μ * g)
    stopping_distance = (speed_ms ** 2) / (2 * mu * g)
    
    # Add reaction distance (assuming 1.5 second reaction time)
    reaction_distance = speed_ms * 1.5
    
    return stopping_distance + reaction_distance


def format_duration(seconds: float) -> str:
    """Format duration in seconds to human-readable string."""
    if seconds < 1:
        return f"{seconds*1000:.0f} ms"
    elif seconds < 60:
        return f"{seconds:.1f} s"
    else:
        minutes = int(seconds // 60)
        secs = seconds % 60
        return f"{minutes} min {secs:.0f} s"


def format_speed(speed_kmh: float) -> str:
    """Format speed to human-readable string."""
    return f"{speed_kmh:.0f} km/h"


def format_distance(meters: float) -> str:
    """Format distance to human-readable string."""
    if meters < 1000:
        return f"{meters:.1f} m"
    else:
        return f"{meters/1000:.2f} km"


def validate_coordinates(lat: float, lng: float) -> bool:
    """Validate geographic coordinates."""
    return -90 <= lat <= 90 and -180 <= lng <= 180


def validate_speed(speed: float) -> bool:
    """Validate vehicle speed."""
    return 0 <= speed <= 300  # Max 300 km/h


def generate_unique_id() -> str:
    """Generate a unique identifier."""
    import uuid
    return str(uuid.uuid4())[:8]


def get_timestamp() -> str:
    """Get current timestamp string."""
    return datetime.now().strftime("%Y-%m-%d %H:%M:%S")