#!/usr/bin/env python

import rospy

import smach
import importlib
import actionlib

from task_manager_common.printer import Printer
from task_manager_common.status_publisher import SkillStatusPublisher
from task_manager_common.ros_interface import ROSInterface

from pprint import pprint
import ast

class SkillState(smach.State):
    def __init__(self, outcomes=None, io_keys=None):
        _outcomes = outcomes if outcomes is not None else ["preempted", "aborted", "succeeded"]
        _io_keys = io_keys if io_keys is not None else ["skillId", "actionName", "actionGoal",
                                                        "actionType", "actionResult"]
        smach.State.__init__(self, outcomes=_outcomes, io_keys=_io_keys)

        self.ud = None

        robot_id = ROSInterface.get_ROS_param('task_manager/robot_configuration/robot_id')

        self.printer = Printer(robot_id, 'TaskManager')
        self.status_publisher = SkillStatusPublisher()

    def check_preemption(self):
        if self.preempt_requested():
            self.service_preempt()
            self.status_publisher.preempted()
            self.printer.logwarn("Skill Preempted!")
            return True
        else:
            return False

    def request_preempt(self):
        """Overload the preempt request method just to spew an error."""
        smach.State.request_preempt(self)
        self.status_publisher.preempted()

    def set_status_publisher_keys(self, ud):
        self.status_publisher.set_skill_id(str(ud.skillId))
        if ud.actionName.startswith('/'):
            self.status_publisher.set_skill_label(str(ud.actionName)[1:])
            self.status_publisher.set_skill_type(str(ud.actionName)[1:])
        else:
            self.status_publisher.set_skill_label(str(ud.actionName))
            self.status_publisher.set_skill_type(str(ud.actionName))

    @staticmethod
    def get_task_id():
        try:
            task_id = rospy.get_param('task_manager/current_task')
        except:
            task_id = None
        return task_id


class SkillSetup(SkillState):
    def __init__(self, outcomes=None, io_keys=None):
        SkillState.__init__(self)
        #self.goal = None

    @staticmethod
    def load_pkg_module(action_type):
        pkg = action_type[:action_type.find('/', 0, len(action_type))]  # -> wait_skill_msgs
        module_ = action_type[action_type.find('/', 0, len(action_type)) + 1:-6]  # -> WaitSkill
        return pkg, module_

    def load_type(self, action_type):
        pkg, module_ = self.load_pkg_module(action_type)

        try:
            module_ref = importlib.import_module('.msg', pkg)  # -> module 'wait_skill_msgs.msg'
        except Exception as e:
            rospy.logerr(str(e))
            return None
        return module_ref

    @staticmethod
    def load_action(module_, module_ref):
        type_instance = module_ref.__getattribute__(module_ + "Action")
        return type_instance

    @staticmethod
    def load_goal(module_, module_ref):
        goal_instance = module_ref.__getattribute__(module_ + "Goal")()  # -> waitTime: 0
        return goal_instance

    @staticmethod
    def load_result(module_, module_ref):
        result_instance = module_ref.__getattribute__(module_+"Result")()  # ->  percentage: 0 skillStatus: ''
        return result_instance

    def action_name_constructor(self, goal, ud):
        return ud.actionName

    @staticmethod
    def goal_parser(ud):
        for data, value in ud.actionGoal.iteritems():
            print data, value

        return ud.actionGoal

    def action_goal_constructor(self, goal, ud):
        return goal

    def get_goal_parameter(self, param):
        pass
        print 'GOAL GOAL GOAL'
        print type(self.goal)
        print self.goal
        #for item, value in self.goal.iteritems():
        #    print item, value
        pprint(dir(self.goal))
        if hasattr(self.goal, param):
            param_value = self.goal[param]
            return param_value
        else:
            raise AttributeError('Skill Goal does not have %s value defined' % param)


    def action_type_constructor(self, action_type, ud):
        return action_type

    def action_result_constructor(self, result, ud):
        return result

    def execute(self, ud):
        self.ud = ud
        self.set_status_publisher_keys(ud)
        pkg, module_ = self.load_pkg_module(ud.actionType)
        module_ref = self.load_type(ud.actionType)

        self.printer.tag_by_index(2, self.get_task_id())
        self.printer.tag_by_index(3, module_)

        self.printer.loginfo('Started Setup')
        if self.check_preemption():
            return 'preempted'
        try:
            print ud.actionGoal
            print type(ud.actionGoal)
            action_type = self.load_action(module_, module_ref)
            goal = self.load_goal(module_, module_ref)
            goals_dict = self.goal_parser(ud)
            result = self.load_result(module_, module_ref)
            ud.actionName = self.action_name_constructor(goals_dict, ud)
            ud.actionGoal = self.action_goal_constructor(goal, ud)
            ud.actionType = self.action_type_constructor(action_type, ud)
            ud.actionResult = self.action_result_constructor(result, ud)


        except Exception as e:
            self.printer.logerr(str(e))
            self.status_publisher.aborted()
            self.printer.logerr('Skill Aborted on Setup')
            return 'aborted'
        return "succeeded"


class SkillExecution(SkillState):
    def __init__(self, outcomes=None, io_keys=None):
        SkillState.__init__(self)
        self.client = None
        self.action_name = None
        self.action_type = None
        self.action_goal = None

    def action_client_setup(self, action_name, action_type):
        try:
            self.client = actionlib.SimpleActionClient(action_name, action_type)
            self.status_publisher.setup()
        except Exception as e:
            self.printer.logerr('Error on Action Client Setup: ' + str(e))
            raise Exception(e)

    def wait_for_server(self, wait_for_server_timeout=10):
        self.printer.loginfo('Waiting for server...')
        if not self.client.wait_for_server(rospy.Duration(wait_for_server_timeout)):
            self.printer.logerr("Skill Action Client Error: Timed out (%i's) while trying to find Action Server"
                                % wait_for_server_timeout)
            self.status_publisher.aborted()
            return False
        return True

    def send_goal(self, goal):
        try:
            self.printer.loginfo("Sending goal to '%s' Server" % self.action_name)
            self.client.send_goal(goal=goal,  feedback_cb=self.action_feedback_callback,
                                  done_cb=self.action_done_callback, active_cb=self.action_active_callback)
        except Exception as e:
            self.printer.logerr('Error on goal sending: ' + str(e))
            self.status_publisher.aborted()
            raise Exception(e)

    def wait_for_result(self, time_out=300):
        self.printer.loginfo("Waiting response from '%s' Server" % self.action_name)
        if not self.client.wait_for_result(rospy.Duration(time_out)):
            self.printer.logerr("Skill Action Client Error: Timed out (%i's) while waiting for %s Action Server Result"
                                % (time_out, self.action_name))
            return False
        return True

    def state_handler(self):
        if self.client.get_state() == 2:
            self.printer.logwarn('Skill Preempted')
            self.service_preempt()
            return 'preempted'
        elif self.client.get_state() == 4:
            self.printer.logerr('Skill Aborted')
            self.status_publisher.aborted()
            return 'aborted'
        elif self.client.get_state() == 3:
            #self.printer.loginfo('Skill succeeded')
            return 'succeeded'

    # NOTE: action_feedback_cb can be overloaded on the derived class
    def action_feedback_callback(self, feedback):
        self.printer.loginfo("Feedback Received: %i%% Executed. Status: %s."
                             % (int(feedback.percentage), feedback.skillStatus))

    # NOTE: action_done_cb can be overloaded on the derived class
    def action_done_callback(self, status, result):
        self.printer.loginfo('Skill Execution Completed.')

    # NOTE: action_active_cb can be overloaded on the derived class
    def action_active_callback(self):
        self.printer.loginfo('Goal sent to the %s Server.' % self.action_name)

    def execute(self, ud):
        self.action_name = ud.actionName[1:] if ud.actionName.startswith('/') else ud.actionName
        self.action_type = ud.actionType
        self.action_goal = ud.actionGoal
        self.ud = ud
        self.set_status_publisher_keys(ud)

        self.printer.tag_by_index(2, self.get_task_id())
        self.printer.tag_by_index(3, self.action_name)
        self.printer.loginfo('Started Execution')

        if self.check_preemption():
            return 'preempted'
        try:
            self.action_client_setup(ud.actionName, ud.actionType)
            self.status_publisher.execution()
            if not self.wait_for_server(wait_for_server_timeout=10):  # TODO: get param from task_manager_configurations
                return 'aborted'
            self.send_goal(ud.actionGoal)
            if not self.wait_for_result(time_out=300):  # TODO: get param from task_manager_configurations
                return 'aborted'
            state_handle = self.state_handler()
            if state_handle != 'succeeded':
                return state_handle
            result = self.client.get_result()
            ud.actionResult = result
        except Exception as e:
            self.printer.logerr('Error on Action Client Setup: ' + str(e))
            raise Exception(e)
        return "succeeded"

    def request_preempt(self):

        """Overload the preempt request method just to spew an error."""

        self.client.cancel_all_goals()
        smach.State.request_preempt(self)
        self.status_publisher.preempted()
        self.printer.logwarn("Skill Execution Preempted!")


class SkillAnalysis(SkillState):
    def __init__(self, outcomes=None, io_keys=None):
        _outcomes = self.set_outcomes()

        SkillState.__init__(self, _outcomes)
        self.action_name = None

    def result_analysis(self, action_result):
        try:
            if action_result.percentage == 100:
                self.printer.loginfo('Skill Succeeded')
                return 'succeeded'
            else:
                self.printer.logerr('Skill aborted on Analysis')
                return 'aborted'
        except Exception as e:
            self.printer.logerr('Error on Action Client Analysis: ' + str(e))
            raise Exception(e)

    def set_outcomes(self):
        return ["preempted", "aborted", "succeeded"]

    def execute(self, ud):
        self.action_name = ud.actionName[1:] if ud.actionName.startswith('/') else ud.actionName
        self.ud = ud
        self.printer.tag_by_index(2, self.get_task_id())
        self.printer.tag_by_index(3, self.action_name)
        self.set_status_publisher_keys(ud)
        if self.check_preemption():
            return 'preempted'
        self.status_publisher.succeeded()
        return self.result_analysis(action_result=ud.actionResult)

    def request_preempt(self):

        """Overload the preempt request method just to spew an error."""

        smach.State.request_preempt(self)
        self.status_publisher.preempted()
        self.printer.logwarn("Skill Analysis Preempted!")