┌─────────────────────────────────────┐
│                 │                   │
│    ┌───────┐    │    ┌───────┐      │
│    │       │    │    │       │      │
│    │ Goal  │    │    │ Goal  │      │
│    │ Area  │    │    │ Area  │      │
│    └───────┘    │    └───────┘      │
│                 │                   │
│       ●─────────●─────────●         │
│                 │                   │
│    ┌───────┐    │    ┌───────┐      │
│    │Penalty│    │    │Penalty│      │
│    │ Area  │    │    │ Area  │      │
│    └───────┘    │    └───────┘      │
│                 │                   │
└─────────────────────────────────────┘

class BallDetector:
    def __init__(self):
        # Ball is white/black pattern
        self.ball_cascade = cv2.CascadeClassifier('ball_cascade.xml')
        # Or using color + shape
        self.min_radius = 10
        self.max_radius = 150

    def detect(self, frame):
        # Convert to HSV for better color segmentation
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        # White detection (for white ball)
        lower_white = np.array([0, 0, 200])
        upper_white = np.array([180, 30, 255])
        mask = cv2.inRange(hsv, lower_white, upper_white)

        # Find circles
        circles = cv2.HoughCircles(
            mask, cv2.HOUGH_GRADIENT, 1, 50,
            param1=50, param2=30,
            minRadius=self.min_radius,
            maxRadius=self.max_radius
        )

        return circles

def detect_field_lines(frame):
    """Detect white lines on green field."""
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    # Detect green field
    lower_green = np.array([35, 40, 40])
    upper_green = np.array([85, 255, 255])
    green_mask = cv2.inRange(hsv, lower_green, upper_green)

    # Detect white lines within green field
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    edges = cv2.Canny(gray, 50, 150)

    # Mask edges with field
    field_edges = cv2.bitwise_and(edges, edges, mask=green_mask)

    # Hough line detection
    lines = cv2.HoughLinesP(
        field_edges, 1, np.pi/180, 50,
        minLineLength=30, maxLineGap=10
    )

    return lines

class ParticleFilter:
    def __init__(self, num_particles=1000, field_size=(9, 6)):
        self.particles = self._init_particles(num_particles, field_size)
        self.weights = np.ones(num_particles) / num_particles

    def _init_particles(self, n, field_size):
        """Initialize particles uniformly on field."""
        particles = np.zeros((n, 3))  # x, y, theta
        particles[:, 0] = np.random.uniform(0, field_size[0], n)
        particles[:, 1] = np.random.uniform(0, field_size[1], n)
        particles[:, 2] = np.random.uniform(-np.pi, np.pi, n)
        return particles

    def update(self, observations, odometry):
        """Update particles based on observations."""
        # Motion update
        self._motion_update(odometry)

        # Measurement update
        self._measurement_update(observations)

        # Resample
        self._resample()

    def get_estimate(self):
        """Return weighted average position."""
        x = np.average(self.particles[:, 0], weights=self.weights)
        y = np.average(self.particles[:, 1], weights=self.weights)
        theta = np.arctan2(
            np.average(np.sin(self.particles[:, 2]), weights=self.weights),
            np.average(np.cos(self.particles[:, 2]), weights=self.weights)
        )
        return x, y, theta

from enum import Enum

class GameState(Enum):
    INITIAL = 0
    READY = 1
    SET = 2
    PLAYING = 3
    FINISHED = 4

class RobotBehavior(Enum):
    STAND = 0
    WALK_TO_BALL = 1
    KICK = 2
    SEARCH_BALL = 3
    DEFEND_GOAL = 4
    POSITION = 5

class BehaviorController:
    def __init__(self, role='striker'):
        self.game_state = GameState.INITIAL
        self.behavior = RobotBehavior.STAND
        self.role = role

    def update(self, world_model):
        """Update behavior based on world model."""
        if self.game_state != GameState.PLAYING:
            self.behavior = RobotBehavior.STAND
            return

        ball = world_model.ball

        if ball.visible:
            if ball.distance < 0.3:
                self.behavior = RobotBehavior.KICK
            else:
                self.behavior = RobotBehavior.WALK_TO_BALL
        else:
            self.behavior = RobotBehavior.SEARCH_BALL