#!/usr/bin/env python

from enums.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS

class RobotInfo:

    def __init__(self,robot_id):
        self.robot_id = robot_id
        self.battery_voltage = None
        self.linear_velocity = None
        self.angular_velocity = None
        self.state = None
        self.clutch = None
        self.obstacle_detected = None

    def update_robot_info(self,robot_info_msg):
        self.robot_id = robot_info_msg.robot_id.data
        self.battery_voltage = robot_info_msg.battery_voltage.data
        self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z]
        self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z]
        self.obstacle_detected = robot_info_msg.obstacle_detected.data

        if robot_info_msg.state.data == True:
            self.state = SS.RUNNING
        else:
            self.state = SS.STOPPED

        if robot_info_msg.clutch.data == True:
            self.clutch = CS.ENGAGED
        else:
            self.clutch = CS.DISENGAGED