#!/usr/bin/env python

import os
import rospy
import rospkg

from qt_gui.plugin import Plugin

from qt_wrapper import QtWrapper
from ros_wrapper import ROSWrapper
from user_widget import UserWidget
from user_info import UserInfo

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

class UserPlugin(Plugin):

    def __init__(self,context):
        super(UserPlugin, self).__init__(context)
        self.setObjectName('User Plugin - L1.5 safety system')

        self._widget = UserWidget(context)
        self._qt_wrapper = QtWrapper(self._widget)
        self._ros_wrapper = ROSWrapper()
        self._user_info = UserInfo()

        self._user_info.user_stop_state = SS.STOPPED
        self._user_info.master_stop_state = SS.UNKNOWN
        self._user_info.clutch_state = CS.UNKNOWN

        # self.register_callback_functions()

        # At the end!
        # self._qt_wrapper.connect_signals()
        # self._ros_wrapper.setup_node()

    def shutdown_plugin(self):
        # TODO unregister all publishers here
        self._ros_wrapper.unregister_node()

    def register_callback_functions(self):
        raise NotImplementedError
        # self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)


    # ROS callback functions
    def handle_robots_list_update(self,robots_id_list):
        self._user_info.update_robots_id_list(robots_id_list)
        self._qt_wrapper.update_robots_list_gui(robots_id_list)

    def handle_selected_robot_info_update(self,robot_info):
        self._qt_wrapper.update_selected_robot_info(self,robot_info)

    def handle_master_stop_update(self,master_stop_state):
        self._user_info.master_stop_state = master_stop_state
        if master_stop_state == SS.STOPPED:
            self.master_stopped()
        elif master_stop_state == SS.RUNNING:
            self.master_started()
        else:
            # TODO - Wrong masterstop value exception
            raise NotImplementedError             

    def handle_restrictions_update(self,restrictions):
        raise NotImplementedError


    # Qt callback functions
    def handle_robot_selection(self,robot_id):
        if self._user_info.selected_robot == None:
            self.select_robot(robot_id)
        else:
            # TODO - raise user already selected robot exception
            raise NotImplementedError

    def handle_user_stop_state_updated(self):
        if self._user_info.master_stop_state == CS.STOPPED:
            self._qt_wrapper.master_is_stopped_notify()
        elif self._user_info.master_stop_state == CS.RUNNING:
            if self._user_info.user_stop_state == CS.RUNNING:
                self.user_stopped()
            elif self._user_info.user_stop_state == CS.STOPPED:
                self.user_started()
            else:
                # TODO - Wrong userstop value exception
                raise NotImplementedError             
        else:
            # TODO - raise switching unknown masterstop value exception
            raise NotImplementedError

    def handle_clutch_switched(self):
        if self._user_info.clutch_state == CS.ENGAGED:
            self.disengage_clutch()
        elif self._user_info.clutch_state == CS.DISENGAGED:
            self.engage_clutch()
        else:
            # TODO - raise switching unknown clutch value exception
            raise NotImplementedError

    def master_stopped(self):
        self.user_stopped()
        self._qt_wrapper.master_stopped()
        self._user_info.master_stop_state = CS.STOPPED

    def master_started(self):
        self._qt_wrapper.master_started()
        self._user_info.master_stop_state = CS.RUNNING

    def user_stopped(self):
        self._user_info.user_stop_state = CS.STOPPED
        self._ros_wrapper.user_stopped()
        self._qt_wrapper.user_stopped()

    def user_started(self):
        self._user_info.user_stop_state = CS.RUNNING
        self._ros_wrapper.user_started()
        self._qt_wrapper.user_started()

    def engage_clutch(self):
        self._user_info.clutch_state = CS.ENGAGED
        self._ros_wrapper.engage_clutch()

    def disengage_clutch(self):
        self._user_info.clutch_state = CS.DISENGAGED
        self._ros_wrapper.disengage_clutch()

    def select_robot(self):
        if self._ros_wrapper.select_robot(robot_id) == True:
            self._qt_wrapper.select_robot(robot_id)
            self._user_info.select_robot(robot_id)
        else:
            # TODO - raise ROS subscriptions problem exception
            raise NotImplementedError