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, string
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 without raycasting"""
        # 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 _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"""
        return -np.pi/2 * t
    
    def _compute_path_length(self, points: np.ndarray) -> float:
        """Compute 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"""
        dx = np.gradient(points[:, 0])
        dy = np.gradient(points[:, 1])
        ddx = np.gradient(dx)
        ddy = np.gradient(dy)
        curvature = np.abs(dx * ddy - dy * ddx) / (dx * dx + dy * dy)**1.5
        return np.nanmax(curvature)
    
    def _compute_smoothness(self, points: np.ndarray) -> float:
        """Estimate path smoothness through angle changes"""
        vectors = np.diff(points[:, :2], axis=0)
        angles = np.arctan2(vectors[:, 1], vectors[:, 0])
        angle_changes = np.diff(angles)
        return np.std(angle_changes)
    
    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

    def _calculate_polygon_area_from_history(self, ray_history: RayHistory) -> float:
        """Calculate area using current valid points from history with robust error handling"""
        valid_points = ray_history.current_points[ray_history.valid]
        
        if len(valid_points) < 3:
            return 0.0
        
        try:
            # Add small random perturbation to break coplanarity
            perturbation = np.random.normal(0, 1e-10, valid_points.shape)
            perturbed_points = valid_points + perturbation
            
            # Check if points are too close together
            if np.any(np.linalg.norm(np.diff(perturbed_points, axis=0), axis=1) < 1e-10):
                return 0.0
            
            hull = ConvexHull(perturbed_points, qhull_options='Qt')  # Qt for triangulation
            return hull.area
        except Exception as e:
            print(f"Warning: Hull calculation failed: {e}")
            return 0.0

    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))
        perturbation = np.random.normal(0, 1e-10, 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, 
                 extra_args=['-vcodec', 'libx264', '-pix_fmt', 'yuv420p'],
                 savefig_kwargs={'pad_inches': 0})
        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:
        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 __name__ == "__main__":
    main()