from distutils.log import error
from matplotlib.pyplot import twinx
import rclpy
from rclpy.node import Node
import math

from turtlesim.srv import Spawn
from std_srvs.srv import Empty
from turtlesim.srv import TeleportAbsolute

from aruco_interfaces.msg import ArucoMarkerPose
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

class TurtleFollowerPy(Node):

    def __init__(self):
        super().__init__('turtle_follower_node')

        
        #Create client for reset service
        self.cli_turtlesim_reset = self.create_client(srv_type = Empty, srv_name = "/reset")
        while not self.cli_turtlesim_reset.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('/reset service not available, waiting again...')
        self.req_turtlesim_reset = Empty.Request()

        #Create client for spawn service
        self.cli_turtlesim_spawn = self.create_client(srv_type = Spawn, srv_name = "/spawn")
        while not self.cli_turtlesim_spawn.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('/spawn service not available, waiting again...')
        self.req_turtlesim_spawn = Spawn.Request()

        #Create client for clear service
        self.cli_turtlesim_clear = self.create_client(srv_type = Empty, srv_name = "/clear")
        while not self.cli_turtlesim_clear.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('/clear service not available, waiting again...')
        self.req_turtlesim_clear = Empty.Request()

        #Create subscriber to get the current positions
        self.sub_marker_pose = self.create_subscription(ArucoMarkerPose, "/aruco/aruco_poses", self.cbk_aruco_poses, 10)
        self.sub_turtle1_pose = self.create_subscription(Pose, "/turtle1/pose", self.cbk_turtle1_pose ,10)
        self.sub_turtle2_pose = self.create_subscription(Pose, "/turtle2/pose", self.cbk_turtle2_pose ,10)

        #Create publisher to control turtle2 (follower)
        self.pub_turtle2_motion = self.create_publisher(Twist, "/cmd_vel", 10)

        #reset turtlesim and spawn second turtle
        self.get_logger().info('resetting turtlesim ...')
        self.reset_turtlesim()
        self.target_pose = Pose()
        self.current_pose = Pose()
        self.target_posex = 1.0
        self.target_posey = 1.0
        self.target_pose.theta = math.pi
        self.current_pose.x = 1.0
        self.current_pose.y = 1.0
        self.current_pose.theta = math.pi
        self.spawn_turtle("turtle2", self.target_pose.x, self.target_pose.y, self.target_pose.theta) 
        self.spawn_turtle("turtle2", self.current_pose.x, self.current_pose.y, self.current_pose.theta)

        #Start timer for the follower controler
        self.timer = self.create_timer(0.1, self.turtle2_follower_controler)

    def spawn_turtle(self, name, x, y, theta):
        self.req_turtlesim_spawn.x = x
        self.req_turtlesim_spawn.y = y
        self.req_turtlesim_spawn.theta = theta
        self.req_turtlesim_spawn.name = name
        future = self.cli_turtlesim_spawn.call_async(self.req_turtlesim_spawn)

    def reset_turtlesim(self):
        future = self.cli_turtlesim_reset.call_async(self.req_turtlesim_reset)

    def cbk_aruco_poses(self, msg_pose):

        #Create client for clear service
        self.cli_turlesim_teleport = self.create_client(srv_type = TeleportAbsolute, srv_name = "turtle2/teleport_absolute")

        #get the current coordinates and transform it to Turtlesim coordination system
        self.req_turtlesim_teleport = TeleportAbsolute.Request()
        self.req_turtlesim_teleport.x = (msg_pose.poses[0].position.x * 5) + 5.5
        self.req_turtlesim_teleport.y = ((msg_pose.poses[0].position.y * -5) + 5.5)
        self.req_turtlesim_teleport.theta = (math.asin(msg_pose.poses[0].orientation.z)*(-2) + math.pi/2)    # calculation of angle theta by asin, because of cosinus axis symmetry   
        future = self.cli_turlesim_teleport.call_async(self.req_turtlesim_teleport)

    def cbk_turtle1_pose(self, msg_pose):
        self.target_pose.x = msg_pose.x
        self.target_pose.y = msg_pose.y
        self.target_pose.theta = msg_pose.theta


    def cbk_turtle2_pose(self, msg_pose):
        self.current_pose.x = msg_pose.x
        self.current_pose.y = msg_pose.y
        self.current_pose.theta = msg_pose.theta

    def turtle2_follower_controler(self):
        tolerance = 0.5
        dist = math.sqrt(math.pow((self.target_pose.x - self.current_pose.x),2) + math.pow((self.target_pose.y - self.current_pose.y),2))
        target_theta = math.atan2(self.target_pose.y - self.current_pose.y, self.target_pose.x - self.current_pose.x)
        angle = target_theta - self.current_pose.theta

        Ta = 0.1
        kp_lin = 1.6/25
        ki_lin = 1.1/25
        kp_ang = 4*2/3
        esum = 0
        esum = esum + dist

        #AntiWindup
        esum_max = 2.0
        if (esum > esum_max): esum = esum_max

        #faster direction of rotation
        if (angle > math.pi): angle = angle - (2*math.pi)
        if (angle < -math.pi): angle = angle + (2*math.pi)

        msg = Twist()
        msg.linear.x = kp_lin * dist + ki_lin * Ta * esum;#dist
        msg.linear.y = 0.0
        msg.linear.z = 0.0
        msg.angular.x = 0.0
        msg.angular.y = 0.0
        msg.angular.z = kp_ang*angle

        if (abs(self.target_pose.x - self.current_pose.x) < tolerance and abs(self.target_pose.y - self.current_pose.y) < tolerance):
            msg.linear.x = 0.0
            msg.angular.z = 2 * (self.target_pose.theta - self.current_pose.theta)

        if (abs(self.target_pose.x - self.current_pose.x) < tolerance and abs(self.target_pose.y - self.current_pose.y) < tolerance and abs(self.target_pose.theta - self.current_pose.theta)):
            msg.linear.x = 0.0
            msg.angular.z = 0.0
            esum = 0.0  

        self.pub_turtle2_motion.publish(msg)
        self.get_logger().info("publishing")

def main(args=None):
    rclpy.init(args=args)

    turtle_follower = TurtleFollowerPy()

    rclpy.spin(turtle_follower)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    turtle_follower.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()