import copy
import time

class RobotController(object):

    def __init__(self, fire_ext_max_capacity, collision_damage = 1, k_sof = 1, k_cas = 1, k_asset = 1):

        # STATE VARIABLES
        self._fire_ext_max_capacity = fire_ext_max_capacity
        self._fire_ext_capacity = self._fire_ext_max_capacity

        self._hit_points = 100
        self._collision_damage = collision_damage
        self._charge_points = 100

        self._collisions = []
        self._exted_sofs = []
        self._found_casualty = []
        self._found_assets = []

        self._k_sof = k_sof
        self._k_cas = k_cas
        self._k_asset = k_asset


    ''' CONTROL '''

    # speeds in [-1, 1] extra stuff will be cut
    def send_twist_cmd(self, v, w):
        v = min(max(-1, v), 1)
        w = min(max(-1, w), 1)
        return v, w

    def send_fire_ext_burst_cmd(self):
        if self._fire_ext_capacity > 0:
            self._fire_ext_capacity -= 1
            return True
        return False

    # poses in [-1, 1] extra stuff will be cut
    def send_fire_ext_pose_cmd(self, horisontal_pose, vertical_pose):
        horisontal_pose = min(max(-1, horisontal_pose), 1)
        vertical_pose = min(max(-1, vertical_pose), 1)
        return horisontal_pose, vertical_pose

    def send_pick_asset_cmd(self):
        raise NotImplemented("")

    def get_score(self, time_stamp):
        score = self._k_sof * len(self._exted_sofs) + self._k_cas * len(self._found_casualty) + self.k_assest * len(self._found_assets)
        return score

    ''' EVENTS '''

    def _register_collision(self, time_stamp):
        self._collisions.append(time_stamp)
        self._hit_points = max(0, self._hit_points - self._collision_damage)

    def _register_exted_sof(self, time_stamp, sof_params = {}):
        self._exted_sofs.append((time_stamp,
                                 copy.deepcopy(sof_params)))

    def _register_found_casualty(self, time_stamp, casualty_params = {}):
        self._found_casualty.append((time_stamp,
                                     copy.deepcopy(casualty_params)))

    def _register_found_assest(self, time_stamp, asset_params = {}):
        self._found_assets.append((time_stamp,
                                   copy.deepcopy(asset_params)))