#!/usr/bin/env python3

import rospy
import actionlib
import geometry_msgs
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
import tf.transformations
import tf

from actionlib_msgs.msg import GoalStatus

from std_msgs.msg import ByteMultiArray

from path_creator.srv import way_points_srv, way_points_srvRequest
from path_creator.srv import conturs_by_point_srv, conturs_by_point_srvRequest
import threading

import os
import sys
sys.path.append(os.path.join(sys.path[0], '../../libraries'))
from marker_lib import *
from path_helper import *

class Goal_move():

    def __init__(self):
        rospy.init_node('path_creator_goal_mover')
        rospy.on_shutdown(self.shutdown)
        
        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        self.cmd_vel_pub = rospy.Publisher('/ppp/cmd_vel', Twist, queue_size = 50)

        self.funAndBrushes_pub = rospy.Publisher("/funAndBrushes", ByteMultiArray, queue_size = 1)

        
        rospy.loginfo('Waiting for server...')
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        #self.client.wait_for_server(rospy.Duration(5))
        rospy.wait_for_service('contur_creator/get_by_xy')
        rospy.wait_for_service('path_creator/get_by_id')

    def shutdown(self):
        self.clean(False)
        rospy.sleep(1)

        rospy.loginfo("Stopping the robot...")
        self.client.cancel_goal()
        rospy.sleep(2)
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)

    def clean(self, is_turn_on):
        bma = ByteMultiArray()
        if is_turn_on:
            bma.data = [1,1,1]
        else:
            bma.data = [0,0,0]
        self.funAndBrushes_pub.publish(bma)

    def process(self, goals):
        self.clean(True)

        i=0
        for g in goals:

            pose = geometry_msgs.msg.Pose()
            pose.position.x = g.x
            pose.position.y = g.y
            # pose.position.x = self.goal_list[self.current_goal_index][0]
            # pose.position.y = self.goal_list[self.current_goal_index][1]
            pose.position.z = 0.0

            q = tf.transformations.quaternion_from_euler(0, 0, g.z)
            # q = tf.transformations.quaternion_from_euler(0, 0, self.goal_list[self.current_goal_index][2])
            pose.orientation = geometry_msgs.msg.Quaternion(*q)


            goal = MoveBaseGoal()
            goal.target_pose.pose = pose
            goal.target_pose.header.frame_id = 'map'
            goal.target_pose.header.stamp = rospy.Time.now()

            self.markers = Marker_lib('viz_current_goal_markers', [[g.x,g.y,g.z]])

            rospy.loginfo('Sending goal to action server: %s' % goal)
            self.client.send_goal(goal)

    # PENDING = 0
    # ACTIVE = 1
    # DONE = 2


            done_condition = threading.Condition()

            #-----------------------------------------
            finished_within_time = False
            timeout = rospy.Duration(10)
            timeout_time = rospy.get_rostime() + timeout
            loop_period = rospy.Duration(0.1)
            lost_count = 0
            with done_condition:
                while not rospy.is_shutdown():
                    time_left = timeout_time - rospy.get_rostime()
                    if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0):
                        break
                    
                    #--------------------------------------------------
                    # client_state = self.client.get_state()
                    # # PENDING = 0, ACTIVE = 1, PREEMPTED = 2, SUCCEEDED = 3, ABORTED = 4
                    # # REJECTED = 5, PREEMPTING = 6, RECALLING = 7, RECALLED = 8, LOST = 9
                    # if client_state==2 or client_state==4 or client_state==5 or \
                    #     client_state==6 or client_state==8:
                    #     break
                    # if client_state == 9 and lost_count < 5:
                    #     timeout_time = rospy.get_rostime() + timeout
                    #     lost_count += 1
                    # if lost_count>=5:
                    #     break
                    #--------------------------------------------------

                    if self.client.simple_state == 2:
                        break

                    if time_left > loop_period or timeout == rospy.Duration():
                        time_left = loop_period

                    self.markers.publish()

                    done_condition.wait(time_left.to_sec())
            finished_within_time = self.client.simple_state == 2
            #-----------------------------------------
            
            if not finished_within_time:
                self.client.cancel_goal()
                rospy.loginfo("Timed out achieving goal {}. x ={} y={}".format(i, g.x, g.y))
                rospy.loginfo('Result received. Action state is %s' % self.client.get_state())
                rospy.loginfo('Goal status message is: {}'.format(self.client.get_goal_status_text()))
            else:
                state = self.client.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")
                    rospy.loginfo('Result received. Action state is %s' % self.client.get_state())
                    rospy.loginfo('Goal status message is %s' % self.client.get_goal_status_text())
            i+=1

if __name__ == '__main__':
    try:
        g = Goal_move()

        way_points = get_way_points()
        for w in way_points:
            g.process(w.points)
            g.clean(False)

        rospy.spin()

    except rospy.ROSInterruptException:
        rospy.loginfo("program interrupted before completion")