#!/usr/bin/env python3
# Correction du TP 4 (partie 3, ROS), etape 3 -- un carre mesure par
# l'odometrie.
# Usage : python3 carre.py [N] [COTE]   (robot N, 1 par defaut ; cote en m)
import math
import sys

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

V_MAX = 0.15        # m/s, sous le plafond du limiteur (0,18)
W_MAX = 0.8         # rad/s
TOL_D = 0.01        # m
TOL_A = math.radians(1.0)


def yaw_of(q):
    return math.atan2(2.0 * (q.w * q.z + q.x * q.y),
                      1.0 - 2.0 * (q.y * q.y + q.z * q.z))


def wrap(a):
    """Ramene un angle dans ]-pi, pi] : gere le passage de +180 a -180."""
    return math.atan2(math.sin(a), math.cos(a))


class Carre(Node):
    def __init__(self, robot, side):
        super().__init__('ratbot_lab_carre')
        ns = f'/ratbot_{robot}'
        self.side = side
        self.pub = self.create_publisher(Twist, f'{ns}/base/cmd_vel', 10)
        self.create_subscription(Odometry, f'{ns}/odometry/filtered', self.on_odom, 10)
        self.pose = None            # (x, y, cap)
        self.state = 'ATTENTE'
        self.sides_done = 0
        self.start = None           # pose au debut du cote ou du virage
        self.origin = None
        self.timer = self.create_timer(0.05, self.tick)

    def on_odom(self, msg):
        p = msg.pose.pose
        self.pose = (p.position.x, p.position.y, yaw_of(p.orientation))

    def send(self, v, w):
        cmd = Twist()
        cmd.linear.x = v
        cmd.angular.z = w
        self.pub.publish(cmd)

    def tick(self):
        if self.pose is None:
            return
        x, y, a = self.pose
        if self.state == 'ATTENTE':
            self.origin = self.pose
            self.start = self.pose
            self.state = 'AVANCER'
        if self.state == 'AVANCER':
            d = math.hypot(x - self.start[0], y - self.start[1])
            reste = self.side - d
            if reste <= TOL_D:
                self.start = self.pose
                self.state = 'TOURNER'
                self.send(0.0, 0.0)
                return
            # ralentir a l'approche : proportionnel, borne
            self.send(max(0.03, min(V_MAX, 1.0 * reste)), 0.0)
        elif self.state == 'TOURNER':
            # but absolu, compte depuis le cap d'origine : les petites erreurs
            # de chaque virage ne s'additionnent pas
            but = wrap(self.origin[2] + (self.sides_done + 1) * math.pi / 2)
            reste = wrap(but - a)
            if abs(reste) <= TOL_A:
                self.sides_done += 1
                self.start = self.pose
                self.state = 'FINI' if self.sides_done == 4 else 'AVANCER'
                self.send(0.0, 0.0)
                if self.state == 'FINI':
                    ox, oy, oa = self.origin
                    self.get_logger().info(
                        f'fini : ecart {100 * math.hypot(x - ox, y - oy):.1f} cm, '
                        f'{math.degrees(wrap(a - oa)):.1f} deg (odometrie)')
                    self.timer.cancel()
                return
            w = max(0.15, min(W_MAX, 2.0 * abs(reste)))
            self.send(0.0, math.copysign(w, reste))


def main():
    robot = int(sys.argv[1]) if len(sys.argv) > 1 else 1
    side = float(sys.argv[2]) if len(sys.argv) > 2 else 0.5
    rclpy.init()
    node = Carre(robot, side)
    try:
        while rclpy.ok() and node.state != 'FINI':
            rclpy.spin_once(node, timeout_sec=0.1)
    finally:
        node.send(0.0, 0.0)
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

# Reponses attendues aux questions :
#  1. Le noeud de teleoperation (clavier) publie ; twist_mux s'y abonne,
#     puis le limiteur, le garde-barriere, et la base (simulee) en bout de
#     chaine.
#  2. Lidar ~10 Hz ; odometrie de la base ~50 Hz, odometry/filtered selon le
#     filtre (voir ros2 topic hz).
#  3. map (si cartographie), odom, base_footprint, base_link, les roues, le
#     lidar... base_link : le corps du robot ; odom : le repere fixe ou
#     l'odometrie accumule les deplacements depuis le depart.
#  4. Il s'arrete une demi-seconde apres le dernier message : twist_mux
#     considere la source inactive, et la carte coupe les moteurs sans
#     commande.
#  5. Le clavier : cmd_vel_teleop (priorite 100) passe devant cmd_vel (50).
#  6. 0,18 m/s : le limiteur plafonne la vitesse.
#  7. Le temps ne mesure rien : acceleration, glissement, latence, limiteur
#     changent la distance reelle. L'odometrie mesure ce qui s'est passe.
#  8. On ramene la difference d'angles dans ]-pi, pi] (atan2(sin, cos)).
#  9. Pas exactement : tolerances d'arret, inertie, et surtout derive de
#     l'odometrie (faible en simulation). Sur le vrai robot l'ecart est
#     plus grand (glissement, rayons de roue, entraxe) : c'est ce que mesure
#     la calibration UMBmark.