sofa.py

import numpy as np
from dataclasses import dataclass, field
from typing import List, Tuple, Optional
from scipy.spatial import ConvexHull
import matplotlib.pyplot as plt
from shapely.geometry import Polygon, LineString, Point, MultiPoint
import random
from datetime import datetime
from matplotlib.animation import FuncAnimation
from matplotlib.collections import LineCollection


@dataclass
class RayHistory:
    """Track the history of each ray's min and max distances"""
    min_distances: np.ndarray  # Minimum distance for each ray angle
    max_distances: np.ndarray  # Maximum distance for each ray angle
    current_points: np.ndarray  # Current valid points
    angles: np.ndarray  # Ray angles
    valid: np.ndarray  # Valid ray mask

@dataclass
class RaycastResult:
    angles: np.ndarray  # Angles of rays
    min_distances: np.ndarray  # Minimum distances for each ray
    max_distances: np.ndarray  # Maximum distances for each ray
    points: np.ndarray  # (x, y) coordinates for each ray endpoint
    valid: np.ndarray  # Boolean mask for valid rays

@dataclass
class PathParameters:
    x0: float = -2.5
    y0: float = 0
    s1_length: float = field(default_factory=lambda: random.uniform(1.7, 3.5))
    control_offset: float = field(default_factory=lambda: random.uniform(-0.5, 0.6))
    num_points: int = 300
    num_rays: int = 300  # Number of rays to cast
    corridor_bounds_outer: List[Tuple[float, float]] = field(default_factory=lambda: [
        (-5000, 1), (1, 1), (1, -5000)
    ])
    corridor_bounds_inner: List[Tuple[float, float]] = field(default_factory=lambda: [
        (-0.0001, -5000), (0.0001, -0.0001), (-5000, -0.0001)
    ])

@dataclass
class PathResult:
    points: np.ndarray
    control_points: List[Tuple[float, float]]
    metrics: dict
    raycast_results: List[RaycastResult] = field(default_factory=list)
    sofa_areas: List[float] = field(default_factory=list)

class OverallResult:
    def __init__(self):
        self.best_path: Optional[PathResult] = None
        self.best_score: float = 0

class PathComputer:
    def _compute_base_path(self, params: PathParameters) -> PathResult:
        """Compute the base path with selected curve type"""
        # Select curve type based on parameter (default to original if not specified)
        curve_type = getattr(params, 'curve_type', 'asymmetric')
        
        if curve_type == 'unpredictable':
            return self._compute_unpredictable_path(params)
        elif curve_type == 'asymmetric':
            return self._compute_asymmetric_path(params)
        else:  # Default to original
            return self._compute_original_path(params)
    
    def _compute_original_path(self, params: PathParameters) -> PathResult:
        """Original path computation as in the original code"""
        # Calculate key points
        start = np.array([params.x0, params.y0])
        s1_end = np.array([params.x0 + params.s1_length, params.y0])
        s3_start = np.array([0, params.s1_length + params.x0])
        
        # Control points for Bézier curve
        p0 = s1_end
        p3 = s3_start
        p1 = p0 + np.array([params.control_offset, params.control_offset])
        p2 = p3 + np.array([params.control_offset, params.control_offset])
        
        control_points = [(p0[0], p0[1]), (p1[0], p1[1]), 
                         (p2[0], p2[1]), (p3[0], p3[1])]
        
        # Generate path points
        points_per_segment = params.num_points // 3
        
        # First straight segment
        t1 = np.linspace(0, 1, points_per_segment)
        straight1 = np.column_stack((
            np.linspace(params.x0, s1_end[0], points_per_segment),
            np.zeros_like(t1),
            np.zeros_like(t1)
        ))
        
        # Curved segment
        t2 = np.linspace(0, 1, points_per_segment)
        curve_points = np.array([self._cubic_bezier(t, p0, p1, p2, p3) for t in t2])
        rotation = self._compute_rotation(t2)
        curved = np.column_stack((curve_points[:, 0], curve_points[:, 1], rotation))
        
        # Final straight segment
        t3 = np.linspace(0, 1, points_per_segment)
        straight3 = np.column_stack((
            np.zeros_like(t3),
            np.linspace(s3_start[1], params.x0, points_per_segment),
            np.full_like(t3, -np.pi/2)
        ))
        
        # Combine all segments
        points = np.vstack((straight1, curved, straight3))
        
        # Compute basic metrics
        metrics = {
            'path_length': self._compute_path_length(points),
            'max_curvature': self._compute_max_curvature(points),
            'smoothness': self._compute_smoothness(points)
        }
        
        return PathResult(
            points=points,
            control_points=control_points,
            metrics=metrics,
            raycast_results=[],
            sofa_areas=[]
        )
    
    def _compute_asymmetric_path(self, params: PathParameters) -> PathResult:
        """Compute path with straight first and last segments of varying lengths while maintaining similar start/end locations"""
        # Start point is the same as original
        start = np.array([params.x0, params.y0])
        
        # First segment - straight but with varying length
        s1_length_mod = params.s1_length * np.random.uniform(0.95, 1.05)
        s1_end = np.array([params.x0 + s1_length_mod, params.y0])
        
        # Final segment end point - ensure x is between 0 and 1
        end_x = np.random.uniform(0.0, 1.0)  # Small positive x value
        end_y = params.x0
        end_point = np.array([end_x, end_y])
        
        # Last segment - start point calculation (similar to original [0, params.s1_length + params.x0])
        s3_start_x = end_x  # Same x as end point for vertical segment
        s3_start_y = params.s1_length * np.random.uniform(0.95, 1.05) + params.x0  # Vary slightly from original
        s3_start = np.array([s3_start_x, s3_start_y])
        
        # Control points for Bézier curve connecting the straight segments
        p0 = s1_end
        p3 = s3_start
        
        # Control points that ensure a smooth transition between segments
        # Similar to original but with slight variation
        p1 = p0 + np.array([
            params.control_offset * np.random.uniform(0.8, 1.2),
            params.control_offset * np.random.uniform(0.8, 1.2)
        ])
        
        p2 = p3 + np.array([
            params.control_offset * np.random.uniform(0.8, 1.2),
            params.control_offset * np.random.uniform(0.8, 1.2)
        ])
        
        control_points = [(p0[0], p0[1]), (p1[0], p1[1]), 
                        (p2[0], p2[1]), (p3[0], p3[1])]
        
        # Generate path points
        points_per_segment = params.num_points // 3
        
        # First straight segment - perfectly straight, pointing right (0 radians)
        t1 = np.linspace(0, 1, points_per_segment)
        straight1 = np.column_stack((
            np.linspace(params.x0, s1_end[0], points_per_segment),
            np.full_like(t1, params.y0),  # Constant y-value
            np.zeros_like(t1)  # Zero rotation for horizontal segment
        ))
        
        # Curved segment
        t2 = np.linspace(0, 1, points_per_segment)
        curve_points = np.array([self._cubic_bezier(t, p0, p1, p2, p3) for t in t2])
        rotation = self._compute_rotation(t2)  # Use the original rotation computation method
        curved = np.column_stack((curve_points[:, 0], curve_points[:, 1], rotation))
        
        # Final straight segment - perfectly straight, vertical
        t3 = np.linspace(0, 1, points_per_segment)
        straight3 = np.column_stack((
            np.full_like(t3, s3_start[0]),  # Constant x-value
            np.linspace(s3_start[1], end_point[1], points_per_segment),  # Decreasing y
            np.full_like(t3, -np.pi/2)  # 90 degree rotation (pointing up)
        ))
        
        # Combine all segments
        points = np.vstack((straight1, curved, straight3))
        
        # Apply smoothing only at segment transitions for rotation
        smoothing_window = 3
        transition1 = points_per_segment
        transition2 = 2 * points_per_segment
        
        # First transition (straight1 to curved)
        for i in range(transition1 - smoothing_window, transition1 + smoothing_window):
            if 0 <= i < len(points):
                # Linear interpolation weight
                weight = 0.5 - abs(i - transition1) / (2 * smoothing_window)
                if i < transition1:
                    # Blend straight1 rotation with first curved rotation
                    points[i, 2] = (1 - weight) * straight1[i - transition1 + points_per_segment - 1, 2] + weight * curved[0, 2]
                else:
                    # Blend curved rotation with last straight1 rotation
                    points[i, 2] = (1 - weight) * curved[i - transition1, 2] + weight * straight1[-1, 2]
        
        # Second transition (curved to straight3)
        for i in range(transition2 - smoothing_window, transition2 + smoothing_window):
            if 0 <= i < len(points):
                weight = 0.5 - abs(i - transition2) / (2 * smoothing_window)
                if i < transition2:
                    # Blend curved rotation with first straight3 rotation
                    points[i, 2] = (1 - weight) * curved[i - transition1, 2] + weight * straight3[0, 2]
                else:
                    # Blend straight3 rotation with last curved rotation
                    points[i, 2] = (1 - weight) * straight3[i - transition2, 2] + weight * curved[-1, 2]
        
        # Compute metrics
        metrics = {
            'path_length': self._compute_path_length(points),
            'max_curvature': self._compute_max_curvature(points),
            'smoothness': self._compute_smoothness(points)
        }
        
        return PathResult(
            points=points,
            control_points=control_points,
            metrics=metrics,
            raycast_results=[],
            sofa_areas=[]
        )
    
    def _compute_unpredictable_path(self, params: PathParameters) -> PathResult:
        """Compute highly unpredictable and asymmetric path"""
        # Calculate key points with significant randomization
        start = np.array([params.x0, params.y0])
        
        # Create a more complex first segment with multiple bends
        angle1 = np.radians(25 * np.random.uniform(0.6, 1.4))  # Wider angle variation
        s1_length_mod = params.s1_length * np.random.uniform(0.85, 1.2)
        s1_end = np.array([
            params.x0 + s1_length_mod * np.cos(angle1),
            params.y0 + s1_length_mod * np.sin(angle1)
        ])
        
        # Make the third segment start at a much more varied position
        s3_start = np.array([
            params.x0 * np.random.uniform(-0.6, 0.3),  # Asymmetric horizontal offset
            params.s1_length * np.random.uniform(0.8, 1.3) + params.x0
        ])
        
        # Control points with highly variable offsets for less predictable curve
        p0 = s1_end
        p3 = s3_start
        
        # Set up basis for perlin-like noise (using weighted random values)
        noise_scale = params.control_offset * 1.5
        noise_weights = np.random.normal(0, 1, 4)  # 4 weighted random values
        noise_weights /= np.sum(np.abs(noise_weights))  # Normalize
        
        # Control points with different influences and noise based offsets
        # This creates a much more unpredictable curve shape
        p1 = p0 + np.array([
            params.control_offset * np.random.uniform(0.5, 2.0) + noise_scale * noise_weights[0],
            params.control_offset * np.random.uniform(0.4, 1.6) + noise_scale * noise_weights[1]
        ])
        
        p2 = p3 + np.array([
            params.control_offset * np.random.uniform(0.3, 1.8) + noise_scale * noise_weights[2],
            -params.control_offset * np.random.uniform(0.5, 1.4) + noise_scale * noise_weights[3]
        ])
        
        # Add small inflection in control points for more complex curves
        if np.random.random() > 0.5:
            midpoint = (p0 + p3) / 2
            p1 = p1 * 0.8 + midpoint * 0.2
            p2 = p2 * 0.8 + midpoint * 0.2
        
        control_points = [(p0[0], p0[1]), (p1[0], p1[1]), 
                         (p2[0], p2[1]), (p3[0], p3[1])]
        
        # Generate path points with varied sampling
        points_per_segment = params.num_points // 3
        
        # First segment with complex curve instead of simple line
        t1 = np.linspace(0, 1, points_per_segment)
        # Use composite sine waves for more chaotic first segment
        phase = np.random.uniform(0, 2*np.pi)
        freq1 = np.random.uniform(0.5, 2.5)
        freq2 = np.random.uniform(3.0, 5.0)
        amp1 = params.control_offset * np.random.uniform(0.05, 0.2)
        amp2 = params.control_offset * np.random.uniform(0.02, 0.08)
        
        first_segment_y = params.y0 + amp1 * np.sin(t1 * np.pi * freq1 + phase) + amp2 * np.sin(t1 * np.pi * freq2)
        first_segment_x = np.linspace(params.x0, s1_end[0], points_per_segment)
        
        # Calculate tangent for proper orientation along the curve
        dx1 = np.gradient(first_segment_x)
        dy1 = np.gradient(first_segment_y)
        first_segment_theta = np.arctan2(dy1, dx1)
        
        straight1 = np.column_stack((
            first_segment_x,
            first_segment_y,
            first_segment_theta
        ))
        
        # Curved segment with non-uniform parameterization and warping
        # Create a non-uniform sampling for the curve parameter
        curve_exponent = np.random.uniform(1.2, 1.8)  # More extreme non-linearity
        t2_raw = np.linspace(0, 1, points_per_segment)
        
        # Apply a warping function to create uneven sampling along the curve
        if np.random.random() > 0.5:
            # Create a region of denser sampling (like slowing down in a region)
            focus_point = np.random.uniform(0.3, 0.7)
            focus_width = np.random.uniform(0.1, 0.3)
            warping = 1.0 + np.exp(-(t2_raw - focus_point)**2 / (2 * focus_width**2))
            t2_raw = np.cumsum(warping)
            t2_raw = t2_raw / t2_raw[-1]  # Normalize to [0,1]
        
        t2 = t2_raw**curve_exponent  # Non-linear parameterization
        curve_points = np.array([self._cubic_bezier(t, p0, p1, p2, p3) for t in t2])
        
        # Compute tangent vectors for more accurate rotation
        tangents = np.zeros((len(t2), 2))
        for i, t in enumerate(t2):
            tangents[i] = self._bezier_tangent(t, p0, p1, p2, p3)
        
        # Calculate base rotation from tangent vectors
        rotation = np.arctan2(tangents[:, 1], tangents[:, 0])
        
        # Add complex oscillations for more unpredictable rotation
        # This simulates the effect of a traveler making small adjustments
        osc_amp = np.random.uniform(0.05, 0.2)
        osc_freq1 = np.random.uniform(3, 8)
        osc_freq2 = np.random.uniform(10, 15)
        osc_phase = np.random.uniform(0, 2*np.pi)
        
        rotation_osc = (osc_amp * np.sin(t2 * osc_freq1 * np.pi + osc_phase) +
                        osc_amp * 0.3 * np.sin(t2 * osc_freq2 * np.pi))
        rotation += rotation_osc
        
        curved = np.column_stack((curve_points[:, 0], curve_points[:, 1], rotation))
        
        # Final segment with complex non-linear path
        t3 = np.linspace(0, 1, points_per_segment)
        
        # Create a meandering final path
        # Composite function with varying frequency & amplitude
        amp_x = params.control_offset * np.random.uniform(0.1, 0.25)
        freq_x1 = np.random.uniform(1.5, 3.0) 
        freq_x2 = np.random.uniform(4.0, 7.0)
        phase_x = np.random.uniform(0, 2*np.pi)
        
        final_segment_x = s3_start[0] + amp_x * np.sin(t3 * np.pi * freq_x1 + phase_x) + amp_x * 0.4 * np.sin(t3 * np.pi * freq_x2)
        final_segment_y = np.linspace(s3_start[1], params.x0, points_per_segment)
        
        # Calculate tangent for proper orientation
        dx3 = np.gradient(final_segment_x)
        dy3 = np.gradient(final_segment_y)
        final_segment_theta = np.arctan2(dy3, dx3)
        
        straight3 = np.column_stack((
            final_segment_x,
            final_segment_y,
            final_segment_theta
        ))
        
        # Combine all segments
        points = np.vstack((straight1, curved, straight3))
        
        # Apply a final smoothing to ensure continuity between segments
        # Smooth orientation angles at segment transitions
        smoothing_window = 5
        for i in range(points_per_segment-smoothing_window, points_per_segment+smoothing_window):
            if i >= 0 and i < len(points):
                # Apply weighted averaging at transition points
                weight = 0.5 - abs(i - points_per_segment) / (2 * smoothing_window)
                if i < points_per_segment:
                    points[i, 2] = (1-weight) * points[i, 2] + weight * curved[0, 2]
                else:
                    points[i, 2] = (1-weight) * points[i, 2] + weight * straight1[-1, 2]
                    
        # Apply similar smoothing at the second transition
        for i in range(2*points_per_segment-smoothing_window, 2*points_per_segment+smoothing_window):
            if i >= 0 and i < len(points):
                weight = 0.5 - abs(i - 2*points_per_segment) / (2 * smoothing_window)
                if i < 2*points_per_segment:
                    points[i, 2] = (1-weight) * points[i, 2] + weight * straight3[0, 2]
                else:
                    points[i, 2] = (1-weight) * points[i, 2] + weight * curved[-1, 2]
        
        # Compute basic metrics
        metrics = {
            'path_length': self._compute_path_length(points),
            'max_curvature': self._compute_max_curvature(points),
            'smoothness': self._compute_smoothness(points)
        }
        
        return PathResult(
            points=points,
            control_points=control_points,
            metrics=metrics,
            raycast_results=[],
            sofa_areas=[]
        )
    
    def _cubic_bezier(self, t: float, p0: np.ndarray, p1: np.ndarray, 
                     p2: np.ndarray, p3: np.ndarray) -> np.ndarray:
        """Compute point on cubic Bézier curve"""
        return (1-t)**3 * p0 + 3*(1-t)**2 * t * p1 + \
               3*(1-t) * t**2 * p2 + t**3 * p3
    
    def _compute_rotation(self, t: np.ndarray) -> np.ndarray:
        """Compute rotation angle along the curve (for original curve)"""
        return -np.pi/2 * t
    
    def _bezier_tangent(self, t: float, p0: np.ndarray, p1: np.ndarray, 
                       p2: np.ndarray, p3: np.ndarray) -> np.ndarray:
        """Compute tangent vector at point t on cubic Bézier curve"""
        return 3*(1-t)**2 * (p1 - p0) + \
               6*(1-t)*t * (p2 - p1) + \
               3*t**2 * (p3 - p2)
    
    def _compute_path_length(self, points: np.ndarray) -> float:
        """Compute the total path length"""
        return np.sum(np.sqrt(np.sum(np.diff(points[:, :2], axis=0)**2, axis=1)))
    
    def _compute_max_curvature(self, points: np.ndarray) -> float:
        """Estimate maximum curvature along the path"""
        # Simple estimation using finite differences
        dx = np.gradient(points[:, 0])
        dy = np.gradient(points[:, 1])
        d2x = np.gradient(dx)
        d2y = np.gradient(dy)
        curvature = np.abs(d2x*dy - dx*d2y) / (dx**2 + dy**2)**1.5
        return np.nanmax(curvature)
    
    def _compute_smoothness(self, points: np.ndarray) -> float:
        """Compute path smoothness metric"""
        # Measure angular changes between segments
        vectors = np.diff(points[:, :2], axis=0)
        angles = np.arctan2(vectors[:, 1], vectors[:, 0])
        angle_diffs = np.abs(np.diff(angles))
        # Wrap angle differences to [-pi, pi]
        angle_diffs = (angle_diffs + np.pi) % (2 * np.pi) - np.pi
        # Smoothness is inverse of mean angular change
        return 1.0 / (np.mean(angle_diffs) + 1e-6)
        
    def compute_path(self, params: PathParameters) -> PathResult:
        # Compute base path
        path_result = self._compute_base_path(params)
        
        # Initialize RayCaster and ray history
        raycaster = RayCaster(params.corridor_bounds_outer, params.corridor_bounds_inner)
        ray_history = raycaster.initialize_ray_history(params.num_rays)
        
        # For each point in the path, perform raycasting
        raycast_results = []
        sofa_areas = []
        
        for i, point in enumerate(path_result.points):
            # Cast rays and update history
            ray_history = raycaster.cast_rays_with_history(
                origin=(point[0], point[1]),
                theta=point[2],
                ray_history=ray_history
            )
            
            # Create current raycast result for visualization
            current_raycast = RaycastResult(
                angles=ray_history.angles + point[2],
                min_distances=ray_history.min_distances.copy(),
                max_distances=ray_history.max_distances.copy(),
                points=ray_history.current_points.copy(),
                valid=ray_history.valid.copy()
            )
            
            raycast_results.append(current_raycast)
            
        # Calculate final sofa shape using ray history
        final_area = self._calculate_final_sofa_area(ray_history)
        
        # Update metrics
        path_result.raycast_results = raycast_results
        path_result.metrics.update({
            'final_sofa_area': final_area
        })
        
        return path_result
        
    # Rest of the methods (calculate_final_sofa_area, etc.) remain the same
    def _calculate_final_sofa_area(self, ray_history: RayHistory) -> float:
        """Calculate final sofa area with robust error handling"""
        # Initial validation
        if not self._is_valid_ray_data(ray_history):
            return 0.0
        
        # Get valid ray data
        valid_rays = ray_history.valid
        angles = ray_history.angles[valid_rays]
        max_distances = ray_history.max_distances[valid_rays]
        min_distances = ray_history.min_distances[valid_rays]
        
        # Validate distances
        if not self._are_distances_valid(max_distances, min_distances):
            return 0.0
        
        try:
            # Create boundary points
            outer_points = self._create_boundary_points(angles, max_distances)
            inner_points = self._create_boundary_points(angles, min_distances)
            all_points = self._combine_and_perturb_points(outer_points, inner_points)
            
            # Validate combined points
            if self._are_points_too_close(all_points):
                return 0.0
                
            # Calculate hull and create polygon
            polygon = self._create_hull_polygon(all_points)
            
            # Handle minimum distance constraints if necessary
            if np.any(min_distances > 0):
                polygon = self._apply_min_distance_constraints(polygon, angles, min_distances)
            
            return polygon.area
            
        except Exception as e:
            return 0.0

    def _is_valid_ray_data(self, ray_history: RayHistory) -> bool:
        """Validate initial ray data"""
        valid_rays = ray_history.valid
        return len(ray_history.angles[valid_rays]) >= 3

    def _are_distances_valid(self, max_distances: np.ndarray, min_distances: np.ndarray) -> bool:
        """Check if max distances are greater than min distances"""
        return np.all(max_distances >= min_distances)

    def _create_boundary_points(self, angles: np.ndarray, distances: np.ndarray) -> np.ndarray:
        """Create boundary points from angles and distances"""
        return np.column_stack((
            distances * np.cos(angles),
            distances * np.sin(angles)
        ))

    def _combine_and_perturb_points(self, outer_points: np.ndarray, inner_points: np.ndarray) -> np.ndarray:
        """Combine and add small perturbation to points"""
        all_points = np.vstack((outer_points, inner_points))
        # Increase perturbation magnitude to ensure uniqueness
        perturbation = np.random.normal(0, 1e-8, all_points.shape)
        return all_points + perturbation

    def _are_points_too_close(self, points: np.ndarray, threshold: float = 1e-10) -> bool:
        """Check if any points are too close together"""
        return np.any(np.linalg.norm(np.diff(points, axis=0), axis=1) < threshold)

    def _create_hull_polygon(self, points: np.ndarray) -> Polygon:
        """Create a polygon from the convex hull of points"""
        hull = ConvexHull(points, qhull_options='Qt')
        hull_points = points[hull.vertices]
        return Polygon(hull_points)

    def _apply_min_distance_constraints(self, polygon: Polygon, angles: np.ndarray, 
                                    min_distances: np.ndarray) -> Polygon:
        """Apply minimum distance constraints to the polygon"""
        circle_points = []
        
        for angle, min_dist in zip(angles, min_distances):
            if min_dist > 0:
                for t in [-0.01, 0, 0.01]:  # Small angle variation
                    x = min_dist * np.cos(angle + t)
                    y = min_dist * np.sin(angle + t)
                    circle_points.append((x, y))
        
        if circle_points:
            min_polygon = Polygon(circle_points).convex_hull
            return polygon.difference(min_polygon)
        
        return polygon
class RayCaster:
    def __init__(self, corridor_bounds_outer: List[Tuple[float, float]], 
                 corridor_bounds_inner: List[Tuple[float, float]], 
                 max_distance: float = 10.0):
        self.corridor_lines_outer = [
            LineString([corridor_bounds_outer[i], corridor_bounds_outer[i+1]])
            for i in range(len(corridor_bounds_outer)-1)
        ]
        self.corridor_lines_inner = [
            LineString([corridor_bounds_inner[i], corridor_bounds_inner[i+1]])
            for i in range(len(corridor_bounds_inner)-1)
        ]
        self.max_distance = max_distance
    
    def initialize_ray_history(self, num_rays: int) -> RayHistory:
        """Initialize ray history with default bounds"""
        angles = np.linspace(-np.pi/2, np.pi/2, num_rays, endpoint=False)
        return RayHistory(
            min_distances=np.zeros(num_rays),
            max_distances=np.full(num_rays, self.max_distance),
            current_points=np.zeros((num_rays, 2)),
            angles=angles,
            valid=np.ones(num_rays, dtype=bool)
        )
    
    def _get_intersections(self, ray: LineString, boundary_lines: List[LineString]) -> List[Tuple[float, float]]:
        """Helper method to get all valid intersection points with a set of boundary lines"""
        valid_intersections = []
        
        for line in boundary_lines:
            if ray.intersects(line):
                intersection = ray.intersection(line)
                
                if isinstance(intersection, Point):
                    valid_intersections.append((intersection.x, intersection.y))
                elif isinstance(intersection, MultiPoint):
                    for point in intersection:
                        valid_intersections.append((point.x, point.y))
                elif isinstance(intersection, LineString):
                    coords = list(intersection.coords)
                    for coord in coords:
                        valid_intersections.append(coord)
                        
        return valid_intersections

    def _calculate_distances(self, intersections: List[Tuple[float, float]], 
                           origin: np.ndarray, ray_dir: np.ndarray, 
                           to_local: np.ndarray) -> List[Tuple[float, np.ndarray, np.ndarray]]:
        """Helper method to calculate distances for intersection points"""
        distances_and_points = []
        for x, y in intersections:
            global_vec = np.array([x - origin[0], y - origin[1]])
            local_vec = to_local @ global_vec
            dist = np.sqrt(local_vec[0]**2 + local_vec[1]**2)
            if np.dot(global_vec, ray_dir) > 0:  # Point is in front
                distances_and_points.append((dist, local_vec, global_vec))
        return distances_and_points

    def cast_rays_with_history(self, origin: Tuple[float, float], theta: float, 
                             ray_history: RayHistory) -> RayHistory:
        """Cast rays and update history with inner and outer corridor bounds"""
        # Setup coordinate transformation
        local_frame_theta = theta + np.pi/2
        cos_theta = np.cos(-local_frame_theta)
        sin_theta = np.sin(-local_frame_theta)
        to_local = np.array([[cos_theta, -sin_theta], [sin_theta, cos_theta]])
        to_global = np.array([[cos_theta, sin_theta], [-sin_theta, cos_theta]])
        
        origin_point = np.array(origin)
        current_points = np.zeros((len(ray_history.angles), 2))
        current_valid = np.ones_like(ray_history.valid)
        
        for i, base_angle in enumerate(ray_history.angles):
            global_angle = base_angle + local_frame_theta
            ray_dir = np.array([np.cos(global_angle), np.sin(global_angle)])
            
            # Create ray with maximum possible length
            ray_end = origin_point + self.max_distance * ray_dir
            ray = LineString([origin_point, ray_end])
            
            # Get intersections with inner and outer boundaries
            inner_intersections = self._get_intersections(ray, self.corridor_lines_inner)
            outer_intersections = self._get_intersections(ray, self.corridor_lines_outer)
            
            # Calculate distances for both sets of intersections
            inner_distances = self._calculate_distances(inner_intersections, origin_point, 
                                                      ray_dir, to_local)
            outer_distances = self._calculate_distances(outer_intersections, origin_point, 
                                                      ray_dir, to_local)
            
            if inner_distances or outer_distances:
                # Update min_distances based on inner corridor
                min_dist = 0.0
                if inner_distances:
                    inner_distances.sort(key=lambda x: x[0])
                    min_dist = inner_distances[0][0]
                
                # Update max_distances based on outer corridor
                max_dist = self.max_distance
                if outer_distances:
                    outer_distances.sort(key=lambda x: x[0])
                    max_dist = min(outer_distances[0][0], self.max_distance)
                
                if ray_history.valid[i]:
                    ray_history.min_distances[i] = max(ray_history.min_distances[i], min_dist)
                    ray_history.max_distances[i] = min(ray_history.max_distances[i], max_dist)
                else:
                    ray_history.min_distances[i] = min_dist
                    ray_history.max_distances[i] = max_dist
                
                # Store current points in global frame
                max_local = np.array([
                    max_dist * np.cos(base_angle),
                    max_dist * np.sin(base_angle)
                ])
                max_global = to_global @ max_local + origin_point
                current_points[i] = max_global
                current_valid[i] = True
            else:
                current_valid[i] = False
            
            if not current_valid[i]:
                # ray_history.valid[i] = False
                m = 100
        
        ray_history.current_points = current_points
        return ray_history

class PathVisualizer:
    @staticmethod
    def plot_final_shape(path_result: PathResult):
        """Plot path and final sofa shape determined by all constraints"""
        plt.figure(figsize=(15, 15))
        
        # Plot corridor bounds
        corridor_points = [
            (-4, 0), (-4, 1), (1, 1), (1, -4), (0, -4), (0, 0), (-4, 0)
        ]
        corridor_x, corridor_y = zip(*corridor_points, corridor_points[0])
        plt.plot(corridor_x, corridor_y, 'k--', label='Corridor')
        
        # Plot path
        points = path_result.points
        plt.plot(points[:, 0], points[:, 1], 'b-', label='Path', alpha=0.5)
        
        # Get final raycast result
        final_raycast = path_result.raycast_results[-1]
        valid_rays = final_raycast.valid
        
        # Get the constraint history
        angles = final_raycast.angles[valid_rays]
        max_distances = final_raycast.max_distances[valid_rays]
        min_distances = final_raycast.min_distances[valid_rays]
        
        # Plot points at max distances
        outer_points = np.column_stack((
            max_distances * np.cos(angles + np.pi/2),  # Add 90 degree rotation
            max_distances * np.sin(angles + np.pi/2)
        ))

        # Plot points at min distances
        inner_points = np.column_stack((
            min_distances * np.cos(angles + np.pi/2),  # Add 90 degree rotation
            min_distances * np.sin(angles + np.pi/2)
        ))
        plt.plot(outer_points[:, 0], outer_points[:, 1], 
                'r.', markersize=2, label='Max Constraints')
        
        plt.plot(inner_points[:, 0], inner_points[:, 1], 
                'b.', markersize=2, label='Min Constraints')
        
        # Plot final shape
        try:
            all_points = np.vstack((outer_points, inner_points))
            hull = ConvexHull(all_points)
            hull_points = all_points[hull.vertices]
            hull_points = np.vstack((hull_points, hull_points[0]))
            plt.fill(hull_points[:, 0], hull_points[:, 1], 
                    'g', alpha=0.3, label='Final Sofa Shape')
        except Exception as e:
            print(f"Warning: Failed to plot final sofa shape: {e}")
        
        plt.grid(True, alpha=0.3)
        plt.axis('equal')
        plt.title('Final Sofa Shape with Path')
        
        # Add metrics
        plt.figtext(0.02, 0.02, 
                   f"Final Area: {path_result.metrics['final_sofa_area']:.2f}\n"
                   f"Min Distance: {np.min(min_distances):.2f}\n"
                   f"Max Distance: {np.max(max_distances):.2f}")
        
        plt.legend()
        plt.show()

    @staticmethod
    def visualize_frame_constraints(path_result: PathResult, frame_index: int = 0):
        """Visualize a single frame showing both min and max constraints"""
        plt.figure(figsize=(15, 15))
        
        # Plot corridor bounds
        corridor_points = [
            (-4, 0), (-4, 1), (1, 1), (1, -4), (0, -4), (0, 0), (-4, 0)
        ]
        corridor_x, corridor_y = zip(*corridor_points, corridor_points[0])
        plt.plot(corridor_x, corridor_y, 'k--', label='Corridor')
        
        # Get frame data
        raycast = path_result.raycast_results[frame_index]
        point = path_result.points[frame_index]
        point_2d = np.array([point[0], point[1]])
        valid_rays = raycast.valid
        
        # Plot the center point
        plt.plot(point[0], point[1], 'ro', markersize=10, label='Sofa Center')
        
        # Get valid rays data
        valid_angles = raycast.angles[valid_rays]
        valid_max_distances = raycast.max_distances[valid_rays]
        valid_min_distances = raycast.min_distances[valid_rays]
        
        # Plot rays and their constraints
        for i, (angle, max_dist, min_dist) in enumerate(zip(valid_angles, valid_max_distances, valid_min_distances)):
            # Calculate rotated ray direction (90 degrees from direction of travel)
            rotated_angle = angle + np.pi/2  # Rotate by 90 degrees
            ray_dir = np.array([np.cos(rotated_angle), np.sin(rotated_angle)])
            
            # Calculate points in rotated frame
            max_point = point_2d + max_dist * ray_dir
            min_point = point_2d + min_dist * ray_dir
            
            # Plot constraint points
            plt.plot(max_point[0], max_point[1], 'r.', markersize=5)
            plt.plot(min_point[0], min_point[1], 'b.', markersize=5)
            
            # Add distance labels for some rays
            if i % 20 == 0:
                mid_point = point_2d + (max_dist * 0.6) * ray_dir
                plt.annotate(f'max: {max_dist:.2f}\nmin: {min_dist:.2f}',
                        (mid_point[0], mid_point[1]),
                        fontsize=8)
        
        # Plot frame orientation arrows
        orientation_length = 0.5

        # Direction of travel
        plt.arrow(point[0], point[1], 
                orientation_length * np.cos(point[2]), 
                orientation_length * np.sin(point[2]),
                head_width=0.1, head_length=0.1, fc='r', ec='r',
                label='Direction')
        
        # Local frame direction (perpendicular)
        perp_theta = point[2] + np.pi/2
        plt.arrow(point[0], point[1], 
                orientation_length * np.cos(perp_theta), 
                orientation_length * np.sin(perp_theta),
                head_width=0.1, head_length=0.1, fc='b', ec='b',
                label='Local Frame')
        
        plt.grid(True, alpha=0.3)
        plt.axis('equal')
        plt.title(f'Frame {frame_index} Constraints\n' +
                 f'Position: ({point[0]:.2f}, {point[1]:.2f})\n' +
                 f'Orientation: {np.degrees(point[2]):.1f}°')
        
        # Add metrics
        metrics_text = (
            f"Number of valid rays: {np.sum(valid_rays)}\n"
            f"Max distance: {np.max(valid_max_distances):.2f}\n"
            f"Min distance: {np.min(valid_min_distances):.2f}\n"
        )
        plt.figtext(0.02, 0.02, metrics_text, fontsize=10)
        
        plt.legend()
        plt.show()
    
    def create_animation(self, path_result: PathResult, output_file: str = 'path_animation.mp4', fps: int = 30):
        """Create an optimized animation of the path constraints"""
        fig, ax = plt.subplots(figsize=(15, 15))
        
        # Pre-compute corridor bounds once
        corridor_points = [
            (-4, 0), (-4, 1), (1, 1), (1, -4), (0, -4), (0, 0), (-4, 0)
        ]
        corridor_x, corridor_y = zip(*corridor_points, corridor_points[0])
        
        # Pre-compute path data
        points = path_result.points
        ax.plot(points[:, 0], points[:, 1], 'b-', label='Path', alpha=0.5)
        
        # Create static elements that won't change
        ax.plot(corridor_x, corridor_y, 'k--', label='Corridor')
        
        # Pre-create updating elements
        center_point, = ax.plot([], [], 'ro', markersize=10, label='Sofa Center')
        direction_arrow = ax.arrow(0, 0, 0, 0, head_width=0.1, head_length=0.1, fc='r', ec='r', label='Direction')
        perp_arrow = ax.arrow(0, 0, 0, 0, head_width=0.1, head_length=0.1, fc='b', ec='b', label='Local Frame')
        
        # Create line collections for rays
        ray_lines = LineCollection([], colors='tab:pink', alpha=0.1)
        ax.add_collection(ray_lines)
        
        # Pre-create constraint points
        max_points, = ax.plot([], [], 'c.', markersize=5)
        min_points, = ax.plot([], [], 'b.', markersize=5)
        
        # Set static plot properties
        ax.grid(True, alpha=0.3)
        ax.set_aspect('equal')
        ax.set_xlim(-4.5, 1.5)
        ax.set_ylim(-4.5, 1.5)
        
        # Create text elements
        title = ax.set_title('')
        metrics_text = ax.text(0.02, 0.02, '', transform=ax.transAxes, fontsize=10)
        
        def update(frame_index):
            # Update only changing elements
            point = path_result.points[frame_index]
            point_2d = np.array([point[0], point[1]])
            raycast = path_result.raycast_results[frame_index]
            valid_rays = raycast.valid
            
            # Update center point
            center_point.set_data([point[0]], [point[1]])
            
            # Update rays and constraints
            valid_angles = raycast.angles[valid_rays]
            valid_max_distances = raycast.max_distances[valid_rays]
            valid_min_distances = raycast.min_distances[valid_rays]
            
            # Compute ray segments efficiently
            rotated_angles = valid_angles + np.pi/2
            ray_dirs = np.column_stack((np.cos(rotated_angles), np.sin(rotated_angles)))
            
            max_points_xy = point_2d + valid_max_distances[:, np.newaxis] * ray_dirs
            min_points_xy = point_2d + valid_min_distances[:, np.newaxis] * ray_dirs
            
            # Update ray lines using LineCollection
            segments = np.stack((min_points_xy, max_points_xy), axis=1)
            ray_lines.set_segments(segments)
            
            # Update constraint points
            max_points.set_data(max_points_xy.T)
            min_points.set_data(min_points_xy.T)
            
            # Update arrows
            orientation_length = 0.5
            for arrow, angle in [(direction_arrow, point[2]), (perp_arrow, point[2] + np.pi/2)]:
                dx = orientation_length * np.cos(angle)
                dy = orientation_length * np.sin(angle)
                arrow.set_data(x=point[0], y=point[1], dx=dx, dy=dy)
            
            # Update text elements
            title.set_text(f'Frame {frame_index} of {len(points)}\n' +
                         f'Position: ({point[0]:.2f}, {point[1]:.2f})\n' +
                         f'Orientation: {np.degrees(point[2]):.1f}°\n' +
                         f'Area: {path_result.metrics["final_sofa_area"]:.2f}')
            
            metrics_text.set_text(
                f"Valid rays: {np.sum(valid_rays)}\n"
                f"Max dist: {np.max(valid_max_distances):.2f}\n"
                f"Min dist: {np.min(valid_min_distances):.2f}\n"
                f"Area: {path_result.metrics['final_sofa_area']:.2f}"
            )
            
            return (center_point, ray_lines, max_points, min_points, 
                   direction_arrow, perp_arrow, title, metrics_text)
        
        anim = FuncAnimation(
            fig, 
            update,
            frames=len(path_result.points),
            interval=1000/fps,
            blit=True
        )
        
        # Save animation with optimized settings
        anim.save(output_file, fps=fps)
        plt.close()

def generate_filename(iteration=0, area=0, extension='mp4'):
    # Get today's date in MM_DD_YYYY format
    date_str = datetime.now().strftime('%H_%M_%m_%d_%Y')
        
    # Combine components
    filename = f"{date_str}_{iteration}_{area}.{extension}"
    
    return filename

def run_iteration(iteration: int = 0, overall_result: OverallResult = OverallResult()):
    params = PathParameters()
    computer = PathComputer()
    visualizer = PathVisualizer()
    
    # Compute path with raycasting
    path_result = computer.compute_path(params)
    
    # Show final shape
    # visualizer.plot_final_shape(path_result)

    area = path_result.metrics['final_sofa_area']
    area_legible = round(area,2)

    if area > overall_result.best_score:
    # if True:
        overall_result.best_score = area
        print(f"New best path found with area: {area_legible}")
        visualizer.create_animation(path_result, output_file=generate_filename(iteration=iteration, area=area_legible), fps=30)

def main():
    overall_result = OverallResult()

    i = 0
    while True:
        run_iteration(i, overall_result)
        i += 1
        if i % 10 == 0:
            print(f"Iteration {i}")

if __name__ == "__main__":
    main()