#!/usr/bin/env python
import rospy
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from pi_trees_ros.pi_trees_ros import *

class Patrol_tree:
    def __init__(self):
        rospy.init_node("patrol_tree")
        rospy.on_shutdown(self.shutdown)
        self.nodo_raiz = Sequence("ARBOL")

        nodo_healthy = Selector("STAY_HEALTHY")
        nodo_battery_OK = MonitorTask("BATTERY_OK?", "battery_level", Int32, self.checkBattery )

        nodo_recharge = SimpleActionTask("RECHARGE_AT_HOME", "move_base", MoveBaseAction,
                                             self.createGoal(2,2), reset_after=True)
        nodo_healthy.add_child(nodo_battery_OK)
        nodo_healthy.add_child(nodo_recharge)

        nodo_patrol = Sequence("PATROL")
        nodo_wp1 = SimpleActionTask("WP_1", "move_base", MoveBaseAction, self.createGoal(2,7))
        nodo_wp2 = SimpleActionTask("WP_2", "move_base", MoveBaseAction, self.createGoal(9,2))
        nodo_patrol.add_child(nodo_wp1)
        nodo_patrol.add_child(nodo_wp2)

        self.nodo_raiz.add_child(nodo_healthy)
        self.nodo_raiz.add_child(nodo_patrol)

        print_tree(self.nodo_raiz)

    def checkBattery(self, msg):
        if msg.data is None:
            return TaskStatus.RUNNING
        else:
            if msg.data < 30:
                rospy.loginfo("BATERIA BAJA!: " + str(int(msg.data)))
                return TaskStatus.FAILURE
            else:
                rospy.loginfo("BATERIA OK!: " + str(int(msg.data)))
                return TaskStatus.SUCCESS

    def createGoal(self, pos_x, pos_y):
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = 'map'
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = pos_x
        goal.target_pose.pose.position.y = pos_y
        goal.target_pose.pose.orientation.w = 1
        return goal

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        rospy.sleep(1)

    def start(self):
        while not rospy.is_shutdown():
            self.nodo_raiz.run()
            rospy.sleep(0.1)


if __name__ == '__main__':
    tree = Patrol_tree()
    tree.start()