import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

from turtlesim.srv import TeleportAbsolute
from turtlesim.srv import SetPen

delta_t = 0.1
v = 0.5
t = 0


def robot_angular_v(tt):
    return tt / 100


def timed_callback():
    global t, node, pub
    t += delta_t

    w = robot_angular_v(t)

    msg = Twist()
    msg.linear.x = v
    msg.angular.z = w
    pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    global node, pub

    node = Node('my_favourite_turtle')

    cli = node.create_client(SetPen, "/turtle1/set_pen")
    req = SetPen.Request()
    req.off = True
    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('1: set_pen service not available, waiting again...')
    future = cli.call_async(req)
    rclpy.spin_until_future_complete(node, future)

    cli = node.create_client(TeleportAbsolute, "/turtle1/teleport_absolute")
    req = TeleportAbsolute.Request()
    req.x = 0.0
    req.y = 0.0
    req.theta = 0.0
    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('2: teleport_absolute service not available, waiting again...')
    future = cli.call_async(req)
    rclpy.spin_until_future_complete(node, future)

    cli = node.create_client(SetPen, "/turtle1/set_pen")
    req = SetPen.Request()
    req.off = False
    req.r = 255
    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('3: set_pen service not available, waiting again...')
    future = cli.call_async(req)
    rclpy.spin_until_future_complete(node, future)

    pub = node.create_publisher(Twist, '/turtle1/cmd_vel', 10)
    node.create_timer(delta_t, timed_callback)

    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == '__main__':
    main()