#!/usr/bin/env python3
# Correction du TP 5 (partie 3, ROS), etape 3 -- une patrouille par Nav2.
# Usage : python3 patrouille.py [N] [TOURS]   (robot N, 1 par defaut)
# Points choisis libres dans la carte du decor « rooms » (repere map).
import math
import sys

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from action_msgs.msg import GoalStatus

POINTS = [(1.5, 0.0, 0.0), (2.0, 1.5, 90.0), (-1.5, 1.5, 180.0), (0.0, 0.0, 0.0)]


def pose(node, x, y, yaw_deg):
    p = PoseStamped()
    p.header.frame_id = 'map'
    p.header.stamp = node.get_clock().now().to_msg()
    p.pose.position.x = x
    p.pose.position.y = y
    # rotation autour de z seulement : quaternion (0, 0, sin(a/2), cos(a/2))
    a = math.radians(yaw_deg)
    p.pose.orientation.z = math.sin(a / 2.0)
    p.pose.orientation.w = math.cos(a / 2.0)
    return p


class Patrouille(Node):
    def __init__(self, robot):
        super().__init__('ratbot_lab_patrouille')
        self.client = ActionClient(self, NavigateToPose, f'/ratbot_{robot}/navigate_to_pose')
        self.last_print = 0.0

    def on_feedback(self, fb):
        # le retour arrive plusieurs fois par seconde : un affichage par 2 s
        now = self.get_clock().now().nanoseconds / 1e9
        if now - self.last_print >= 2.0:
            self.last_print = now
            self.get_logger().info(
                f'  reste {fb.feedback.distance_remaining:.2f} m, '
                f'{fb.feedback.number_of_recoveries} recuperation(s)')

    def go(self, x, y, yaw):
        goal = NavigateToPose.Goal()
        goal.pose = pose(self, x, y, yaw)
        send = self.client.send_goal_async(goal, feedback_callback=self.on_feedback)
        rclpy.spin_until_future_complete(self, send)
        handle = send.result()
        if not handle.accepted:
            self.get_logger().warn('but refuse')
            return False
        res = handle.get_result_async()
        rclpy.spin_until_future_complete(self, res)
        return res.result().status == GoalStatus.STATUS_SUCCEEDED


def main():
    robot = int(sys.argv[1]) if len(sys.argv) > 1 else 1
    tours = int(sys.argv[2]) if len(sys.argv) > 2 else 1
    rclpy.init()
    node = Patrouille(robot)
    if not node.client.wait_for_server(timeout_sec=30.0):
        node.get_logger().error('pas de navigation : ratbot nav lance ?')
        return
    reussis = 0
    for t in range(tours):
        for (x, y, yaw) in POINTS:
            node.get_logger().info(f'tour {t + 1} : vers ({x}, {y}, {yaw} deg)')
            ok = node.go(x, y, yaw)
            reussis += ok
            node.get_logger().info('  atteint' if ok else '  ECHEC, point suivant')
    node.get_logger().info(f'{reussis}/{tours * len(POINTS)} buts atteints')
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

# Reponses attendues aux questions :
#  1. La simulation publie sinon une transformation exacte vers odom ; la
#     cartographie publie map -> odom ; deux sources pour la meme chaine, la
#     localisation echoue.
#  2. Le lidar et l'odometrie se decalent : murs dedoubles, carte tordue.
#  3. NOM.yaml (resolution, origine, seuils) et NOM.pgm. Blanc : libre ;
#     noir : occupe ; gris : inconnu.
#  4. Globale (toute la carte, pour planifier le chemin) et locale (autour
#     du robot, a jour avec les capteurs, pour suivre le chemin et eviter).
#  5. Il replanifie autour de l'obstacle, vu par le lidar. Passage bouche :
#     comportements de recuperation (recul, rotation, attente), puis echec
#     du but.
#  6. Le clavier : cmd_vel_teleop passe devant cmd_vel_nav ; la navigation
#     reprend quand on lache le clavier.
#  7. Un echec (STATUS_ABORTED) : aucun chemin n'existe vers ce point.
#  8. Nav2 n'execute qu'un but a la fois : le nouveau remplace le premier,
#     qui est interrompu (le premier client recoit un statut annule ou
#     abandonne).
#  9. Oui, a l'identique : memes topics, meme action. Il faut une carte du
#     vrai lieu, des points libres dans cette carte, et le bon domaine (77).