#!/usr/bin/env python
import rospy
# import functools
from ackermann_msgs.msg import AckermannDriveStamped
from sensor_msgs.msg import Range
from std_msgs.msg import Float32,Bool

STEERING_ANGLE = Float32()
STEERING_ANGLE.data = 0 # radians
STEERING_ANGLE_VELOCITY = 0
SPEED = 0.5 # m/s
ACCELERATION = 1 # m/s^2
JERK = 0.1 # m/s^3
sonar_sensor = Float32()
flag_Emergency = bool()
flag_Overtaking = bool()
flag_Overtaking_itHappens = bool()
sonar_sensor.data = 3.75
flag_Stop = Bool()
flag_Stop.data = False


def sonar_callback(sonar_data):
    sonar_sensor.data = sonar_data.range
    # rospy.loginfo("Sonar range: %lf" % sonar_sensor.data)
    print("am intrat in callback sonar")

def steering_callback(stering_angle_data):
    print("am intrat in callback steering")
    STEERING_ANGLE.data = stering_angle_data.data
    # rospy.loginfo("DATA CALLBACK_stering: %lf" % STEERING_ANGLE.data)


def stop_line_callback(stop_line_data):
    flag_Stop.data = stop_line_data.data
    # rospy.loginfo("callback stopLine: %lf" % flag_Stop.data)
    print("am intrat in callback stop line")
    print(flag_Stop.data)

def listener_talker():
    overaking_cnt = 0
    publisher_obj = rospy.Publisher('/ackermann_vehicle/ackermann_cmd', AckermannDriveStamped, queue_size=10)
    rospy.Subscriber('/steering_angle', Float32,steering_callback)
    rospy.Subscriber('ackermann_vehicle/sonar', Range, sonar_callback)
    rospy.Subscriber('/Stop_pub_topic',Bool,stop_line_callback)
    rate = rospy.Rate(100) # 10hz


    # !!! In Gazebo the scale is x2.5 to Real Simulation (ex. 1meter(Real Simulation) = 2.5m(Gazebo))

    while not rospy.is_shutdown():
        cmd_to_publish = AckermannDriveStamped() # publisher object for ackermann_cmd
        cmd_to_publish.drive.steering_angle = STEERING_ANGLE.data
        cmd_to_publish.drive.steering_angle_velocity = STEERING_ANGLE_VELOCITY
        cmd_to_publish.drive.speed = SPEED
        cmd_to_publish.drive.acceleration = ACCELERATION
        cmd_to_publish.drive.jerk = JERK

        publisher_obj.publish(cmd_to_publish) # publish Ackermann messagec

        rospy.loginfo("Soanar data: %lf " %sonar_sensor.data)
        print(flag_Stop)

        # if the obtacle is detected between 0.8-1.2m -> perform a overtaking manuver
        if sonar_sensor.data <= 3:
            rospy.loginfo("flag_Overtaking")
            flag_Overtaking = True
        else:
            flag_Overtaking = False
        rospy.loginfo("Overtaking: %lf " %flag_Overtaking)
        rospy.loginfo("Soanr data: %lf " %sonar_sensor.data)


        if sonar_sensor.data < 1.5 and flag_Overtaking == True:

            flag_Overtaking_itHappens = True
            # cmd_to_publish.drive.speed = 0.3 #incetinesc la overtaking?

            start_time = rospy.get_rostime()
            timeout1 = rospy.Duration(3)  # Timeout of 2 seconds
            while(rospy.get_rostime() - start_time < timeout1):
                cmd_to_publish.drive.steering_angle = -0.21 #dreapta
                publisher_obj.publish(cmd_to_publish)

            start_time2 = rospy.get_rostime()
            timeout2 = rospy.Duration(4)
            while(rospy.get_rostime() - start_time2 < timeout2):
                cmd_to_publish.drive.steering_angle = 0.4 #stanga
                publisher_obj.publish(cmd_to_publish)

            start_time3 = rospy.get_rostime()
            timeout3 =rospy.Duration(1)
            while(rospy.get_rostime() - start_time3 < timeout3):
                cmd_to_publish.drive.steering_angle = -0.2
                publisher_obj.publish(cmd_to_publish)

            flag_Overtaking_itHappens = False

        # if the obtacle is detected at least 0.8m -> stop at 0.3m
        if sonar_sensor.data <= 2 and flag_Overtaking == False:
            flag_Emergency = True
        else:
            flag_Emergency = False
        if sonar_sensor.data < 1.25 and flag_Emergency == True:  # stop at 0.5m
            cmd_to_publish.drive.speed = 0
            publisher_obj.publish(cmd_to_publish) # publish Ackermann message

        if flag_Stop.data == True:

            start_time_stop = rospy.get_rostime()
            timeout_stop = rospy.Duration(2)
            while(rospy.get_rostime() - start_time_stop < timeout_stop):
                cmd_to_publish.drive.speed = 0
                publisher_obj.publish(cmd_to_publish)



        rate.sleep()

if __name__ == '__main__':


    try:
        rospy.init_node('publisher_node', anonymous=True)

        listener_talker()
    except rospy.ROSInterruptException:
        pass