#!/usr/bin/env python3

import rospy
# from std_msgs.msg import String
from hinge_car_nav_msgs.msg import HingeCarNavActionGoal
from hinge_car_nav_msgs.msg import hinge_car_nav_msgs
import geometry_msgs.msg 
# import numpy as np
# import sys
# Brings in the SimpleActionClient
import yaml
import actionlib

# Brings in the messages used by the fibonacci action, including the
# goal message and the result message.
# from tf.transformations import quaternion_from_euler


def callback(data):
    # rospy.loginfo(rospy.get_caller_id() + 'I heard %s', )
    rospy.loginfo('received move base action goal')
    # rospy.loginfo(data.goal_id)


def done_cb(data, data1):
    print("action done cb:%d,%s" % (data, data1))


def active_cb():
    print("active_cb")


def feedback_cb(data):
    pass

    # print("feedback_cb x:%f,y%f", data.pose.position.x,data.pose.position.y)
    # print("feedback_cb ", data.base_position.pose.position.x )


if __name__ == '__main__':
    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('action_client', anonymous=True)

    # rospy.Subscriber('/perception/traffic_light_recognition/output/rois', String, callback)
    # rospy.Subscriber('/move_base/goal',MoveBaseActionGoal, callback)
    # goal_pub = rospy.Publisher('/move_base/goal',MoveBaseActionGoal)

    # (FibonacciAction) to the constructor.
    client = actionlib.SimpleActionClient(
        'hinge_car_nav', hinge_car_nav_msgs.msg.HingeCarNavAction)

    # Waits until the action server has started up and started
    # listening for goals.
    p0=geometry_msgs.msg.Point() 
    p00=geometry_msgs.msg.Point() 
    p1=geometry_msgs.msg.Point() 
    p2=geometry_msgs.msg.Point() 
    p3=geometry_msgs.msg.Point() 
    p4=geometry_msgs.msg.Point() 


    #in map
    # p1.x = 0.0 
    # p1.y = 0.0
    # p2.x = 162.0
    # p2.y = 2.5
    # p3.x = 162.0
    # p3.y = 30.5  #28
    # p4.x = 0.0
    # p4.y = 28


   #in world
    #0 base_link in world:181.809424,200.828833,2.987182
    #1 base_link in world:15.517689,203.215752,3.130124
    #2 base_link in world:15.794460,176.277180,-0.312712
    #3 base_link in world:182.272948,171.620984,0.011221
    #0 base_link in world:181.809424,200.828833,2.987182
	#154.616427,196.909903
    p0.x = 173.45 #182.0 qidian
    p0.y = 196.3180  #200.0
	
    p00.x = p0.x+2.0 #182.0 zhongdian
    p00.y = p0.y  #200.0	
	#184.933232,195.708944
    p1.x = 184.93 #182.0
    p1.y = 195.7 #
	
    p2.x = 14.45 #15.5 维修
    p2.y = 199.64 #203.0
	
    p3.x = 14.81 #15.5
    p3.y = 172.2 #175.0  #28
	
    p4.x = 185.08 #182.0   
    p4.y = 168.3 #172.0  #28


    print("before client connected")
    client.wait_for_server()
    print("client connected")
    goal = hinge_car_nav_msgs.msg.HingeCarNavGoal()
    path = hinge_car_nav_msgs.msg.HingeCarPath()
    tp=[]
    goal.goal_type = goal.BACKWARD
  
    # s = hinge_car_nav_msgs.msg.Segment()
    # s.segment_type = s.LINE
    # start_point = geometry_msgs.msg.Point() 
    # start_point.x = 0.0
    # start_point.y = 0.0
    # s.start_point = start_point

    # end_point = geometry_msgs.msg.Point() 
    # end_point.x = 162
    # end_point.y = 2.5
    # s.end_point = end_point
    # s.max_vel = 0.45
    # tp.append(s)
    # path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.LINE
    # start_point = geometry_msgs.msg.Point() 
    # start_point.x = -1.5
    # start_point.y = -1.2
    s.start_point=p0
    s.end_point =p2
    s.max_vel = -1.0
    path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.CIRCLE
    s.start_point=p2
    s.end_point =p3
    s.center_point.x=0.5*(p2.x+p3.x)
    s.center_point.y=0.5*(p2.y+p3.y)
    s.max_vel = -0.45
    path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.LINE
    s.start_point=p3
    s.end_point =p4
    s.max_vel = -1.0
    path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.CIRCLE
    s.start_point=p4
    s.end_point =p1
    s.center_point.x=0.5*(p1.x+p4.x)
    s.center_point.y=0.5*(p1.y+p4.y)
    s.max_vel = -0.45
    path.target_path.append(s)
	
	
    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.LINE
    s.start_point=p1
    s.end_point =p00
    s.max_vel = -1.0
    path.target_path.append(s)


    # path.target_path=tp
  
    # s = hinge_car_nav_msgs.msg.Segment
    # s.segment_type = s.LINE
    # s.start_point.x = 1.0
    # s.start_point.y = 1.0
    # s.end_point.x = 2.0
    # s.end_point.y = 2.0
    # s.switch_location.swtich_action = s.switch_location.RTK2LIDAR
    # s.switch_location.need_swtich_location = True
  
    # q = quaternion_from_euler(0.0, 0, 0.1)


    goal.path = path
    client.send_goal(goal, done_cb, active_cb, feedback_cb)
    # Sends the goal to the action server.
    # Waits for the server to finish performing the action.
    
    client.wait_for_result()
    client.get_result()
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        result = client.get_result()
    except rospy.ROSInterruptException:
        print("program interrupted before completion")
    rospy.sleep(5.0)