<commit_before>
import logging
from multiprocessing.managers import DictProxy
from typing import Dict, List, Union, Any, Iterable
import numpy as np

from Engine.Communication.robot_state import RobotState
from Engine.filters.ball_kalman_filter import BallFilter
from Engine.filters.robot_kalman_filter import RobotFilter

from Util.geometry import wrap_to_pi
from Util import Pose, Position
from Util.team_color_service import TeamColorService


class Tracker:

    MAX_ROBOT_ID = 12
    MAX_BALL_ON_FIELD = 1
    MAX_UNDETECTED_DELAY = 3
    NUMBER_OF_CAMERA = 4

    def __init__(self, vision_state: DictProxy):

        logging.basicConfig(format='%(levelname)s: %(name)s: %(message)s', level=logging.DEBUG)
        self.logger = logging.getLogger('Tracker')

        self.on_negative_side = False

        self.vision_state = vision_state

        self._blue_team = [RobotFilter(robot_id) for robot_id in range(Tracker.MAX_ROBOT_ID)]
        self._yellow_team = [RobotFilter(robot_id) for robot_id in range(Tracker.MAX_ROBOT_ID)]
        self._ball = BallFilter()

        self._camera_frame_number = [-1 for _ in range(Tracker.NUMBER_OF_CAMERA)]
        self._current_timestamp = 0

    def update(self) -> Dict[str, List[Dict[str, Any]]]:

        camera_frames = [frame for frame in self.vision_state
                         if frame and frame['frame_number'] > self._camera_frame_number[frame['camera_id']]]

        if any(camera_frames):

            for frame in camera_frames:

                # if self.on_negative_side:
                #     vision_frame = Tracker.change_reference(frame)

                self._camera_frame_number[frame['camera_id']] = frame['frame_number']
                self._update(frame, frame['t_capture'])

            self._current_timestamp = max(frame['t_capture'] for frame in camera_frames)
            self.remove_undetected()

        return self.game_state

    def _update(self, detection_frame: Dict[str, List[Dict[str, Any]]], timestamp: int):

        new_robots = {'blue': set(), 'yellow': set()}

        for robot_obs in detection_frame.get('robots_blue', ()):
            if not self._blue_team[robot_obs['robot_id']].is_active:
                new_robots['blue'].add(robot_obs['robot_id'])
            obs = np.array([robot_obs['x'], robot_obs['y'], robot_obs['orientation']])
            self._blue_team[robot_obs['robot_id']].update(obs, timestamp)

        for robot_obs in detection_frame.get('robots_yellow', ()):
            if not self._yellow_team[robot_obs['robot_id']].is_active:
                new_robots['yellow'].add(robot_obs['robot_id'])
            obs = np.array([robot_obs['x'], robot_obs['y'], robot_obs['orientation']])
            self._yellow_team[robot_obs['robot_id']].update(obs, timestamp)

        for ball_obs in detection_frame.get('balls', ()):
            obs = np.array([ball_obs['x'], ball_obs['y']])
            self._ball.update(obs, timestamp)

        if new_robots['blue']:
            self.logger.debug('New blue robot(s) detected: %r', new_robots['blue'])
        if new_robots['yellow']:
            self.logger.debug('New yellow robot(s) detected: %r', new_robots['yellow'])

    def predict(self, robot_state: RobotState):

        input_commands = [None for _ in range(12)]
        for packet in robot_state.packet:
            input_commands[packet.robot_id] = packet.command.to_array()

        for robot, input_cmd in zip(self._our_team, input_commands):
            robot.predict(input_cmd)
        for robot in self._their_team:
            robot.predict()

        self._ball.predict()

    def remove_undetected(self):
        active_robots = (robot for robot in self._yellow_team + self._blue_team if robot.is_active)
        for robot in active_robots:
            if self._current_timestamp - robot.last_capture_time > Tracker.MAX_UNDETECTED_DELAY:
                robot.reset()
                self.logger.info('Robot %d was undetected for more than %d seconds.',
                                 robot.id,
                                 Tracker.MAX_UNDETECTED_DELAY)

        if self._ball.is_active:
            if self._current_timestamp - self._ball.last_capture_time > Tracker.MAX_UNDETECTED_DELAY:
                self._ball.reset()
                self.logger.info('The ball was undetected for more than %d seconds.',
                                 Tracker.MAX_UNDETECTED_DELAY)

    @staticmethod
    def change_reference(detection_frame: Dict[str, List[Dict[str, Any]]]) -> Dict[str, List[Dict[str, Any]]]:

        teams = detection_frame.get('robots_blue', ())\
                + detection_frame.get('robots_yellow', ())

        for robot_obs in teams:
            robot_obs['x'] *= -1
            robot_obs['y'] *= -1
            robot_obs['orientation'] = wrap_to_pi(robot_obs['orientation'] + np.pi)

        for ball_obs in detection_frame.get('balls', ()):
            ball_obs.position *= -1<commit_msg>modify info to debug<commit_after>
import logging
from multiprocessing.managers import DictProxy
from typing import Dict, List, Union, Any, Iterable
import numpy as np

from Engine.Communication.robot_state import RobotState
from Engine.filters.ball_kalman_filter import BallFilter
from Engine.filters.robot_kalman_filter import RobotFilter

from Util.geometry import wrap_to_pi
from Util import Pose, Position
from Util.team_color_service import TeamColorService


class Tracker:

    MAX_ROBOT_ID = 12
    MAX_BALL_ON_FIELD = 1
    MAX_UNDETECTED_DELAY = 3
    NUMBER_OF_CAMERA = 4

    def __init__(self, vision_state: DictProxy):

        logging.basicConfig(format='%(levelname)s: %(name)s: %(message)s', level=logging.DEBUG)
        self.logger = logging.getLogger('Tracker')

        self.on_negative_side = False

        self.vision_state = vision_state

        self._blue_team = [RobotFilter(robot_id) for robot_id in range(Tracker.MAX_ROBOT_ID)]
        self._yellow_team = [RobotFilter(robot_id) for robot_id in range(Tracker.MAX_ROBOT_ID)]
        self._ball = BallFilter()

        self._camera_frame_number = [-1 for _ in range(Tracker.NUMBER_OF_CAMERA)]
        self._current_timestamp = 0

    def update(self) -> Dict[str, List[Dict[str, Any]]]:

        camera_frames = [frame for frame in self.vision_state
                         if frame and frame['frame_number'] > self._camera_frame_number[frame['camera_id']]]

        if any(camera_frames):

            for frame in camera_frames:

                # if self.on_negative_side:
                #     vision_frame = Tracker.change_reference(frame)

                self._camera_frame_number[frame['camera_id']] = frame['frame_number']
                self._update(frame, frame['t_capture'])

            self._current_timestamp = max(frame['t_capture'] for frame in camera_frames)
            self.remove_undetected()

        return self.game_state

    def _update(self, detection_frame: Dict[str, List[Dict[str, Any]]], timestamp: int):

        new_robots = {'blue': set(), 'yellow': set()}

        for robot_obs in detection_frame.get('robots_blue', ()):
            if not self._blue_team[robot_obs['robot_id']].is_active:
                new_robots['blue'].add(robot_obs['robot_id'])
            obs = np.array([robot_obs['x'], robot_obs['y'], robot_obs['orientation']])
            self._blue_team[robot_obs['robot_id']].update(obs, timestamp)

        for robot_obs in detection_frame.get('robots_yellow', ()):
            if not self._yellow_team[robot_obs['robot_id']].is_active:
                new_robots['yellow'].add(robot_obs['robot_id'])
            obs = np.array([robot_obs['x'], robot_obs['y'], robot_obs['orientation']])
            self._yellow_team[robot_obs['robot_id']].update(obs, timestamp)

        for ball_obs in detection_frame.get('balls', ()):
            obs = np.array([ball_obs['x'], ball_obs['y']])
            self._ball.update(obs, timestamp)

        if new_robots['blue']:
            self.logger.debug('New blue robot(s) detected: %r', new_robots['blue'])
        if new_robots['yellow']:
            self.logger.debug('New yellow robot(s) detected: %r', new_robots['yellow'])

    def predict(self, robot_state: RobotState):

        input_commands = [None for _ in range(12)]
        for packet in robot_state.packet:
            input_commands[packet.robot_id] = packet.command.to_array()

        for robot, input_cmd in zip(self._our_team, input_commands):
            robot.predict(input_cmd)
        for robot in self._their_team:
            robot.predict()

        self._ball.predict()

    def remove_undetected(self):
        active_robots = (robot for robot in self._yellow_team + self._blue_team if robot.is_active)
        for robot in active_robots:
            if self._current_timestamp - robot.last_capture_time > Tracker.MAX_UNDETECTED_DELAY:
                robot.reset()
                self.logger.debug('Robot %d was undetected for more than %d seconds.',
                                 robot.id,
                                 Tracker.MAX_UNDETECTED_DELAY)

        if self._ball.is_active:
            if self._current_timestamp - self._ball.last_capture_time > Tracker.MAX_UNDETECTED_DELAY:
                self._ball.reset()
                self.logger.debug('The ball was undetected for more than %d seconds.',
                                 Tracker.MAX_UNDETECTED_DELAY)

    @staticmethod
    def change_reference(detection_frame: Dict[str, List[Dict[str, Any]]]) -> Dict[str, List[Dict[str, Any]]]:

        teams = detection_frame.get('robots_blue', ())\
                + detection_frame.get('robots_yellow', ())

        for robot_obs in teams:
            robot_obs['x'] *= -1
            robot_obs['y'] *= -1
            robot_obs['orientation'] = wrap_to_pi(robot_obs['orientation'] + np.pi)

        for ball_obs in detection_frame.get('balls', ()):
            ball_obs.position *= -1
{'fraction_non_alphanumeric': 0.11305490949539893, 'fraction_numerical': 0.002426939023157043, 'mean_word_length': 2.7208427389014296, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 13, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1153128', 'n_tokens_mistral': 3133, 'n_tokens_neox': 2976, 'n_words': 600}