"""
TeamDataCapsule
^^^^^^^^^^^^^^^
"""
import math
from collections import defaultdict

import rospy
from humanoid_league_msgs.msg import Strategy, TeamData
from geometry_msgs.msg import PointStamped


class TeamDataCapsule:
    def __init__(self):
        self.bot_id = rospy.get_param("bot_id", 1)
        self.strategy_sender = None  # type: rospy.Publisher
        self.time_to_ball_publisher = None  # type: rospy.Publisher
        # indexed with one to match robot ids
        self.team_data = {}
        for i in range(1, 7):
            self.team_data[i] = TeamData()
        self.team_strategy = dict()
        self.times_to_ball = dict()
        self.roles = {
            'striker': Strategy.ROLE_STRIKER,
            'offense': Strategy.ROLE_STRIKER,
            'supporter': Strategy.ROLE_SUPPORTER,
            'defender': Strategy.ROLE_DEFENDER,
            'defense': Strategy.ROLE_DEFENDER,
            'other': Strategy.ROLE_OTHER,
            'goalie': Strategy.ROLE_GOALIE,
            'idle': Strategy.ROLE_IDLING
        }
        self.own_time_to_ball = 9999.0
        self.strategy = Strategy()
        self.strategy.role = self.roles[rospy.get_param('role')]
        self.strategy_update = None
        self.action_update = None
        self.role_update = None