#!/usr/bin/env python3
# Correction du TP 6 (partie 3, ROS), etape 1 -- tableau de la flotte.
# Reponses aux questions du TP 6 : en fin de suiveur.py.
# Decouvre les robots par leurs topics (/ratbot_N/...), s'abonne a la
# batterie et a l'odometrie de chacun, et affiche un tableau toutes les 2 s.
import re

import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from nav_msgs.msg import Odometry
from sensor_msgs.msg import BatteryState

PATTERN = re.compile(r'^/(ratbot_\d+)/')


class Flotte(Node):
    def __init__(self):
        super().__init__('ratbot_lab_flotte')
        self.robots = {}            # nom -> {'v': tension, 'x':, 'y':, 'speed':}
        self.subs = []
        self.create_timer(2.0, self.tick)

    def discover(self):
        for name, _types in self.get_topic_names_and_types():
            m = PATTERN.match(name)
            if m and m.group(1) not in self.robots:
                self.add(m.group(1))

    def add(self, robot):
        state = {'v': float('nan'), 'x': float('nan'), 'y': float('nan'), 'speed': 0.0}
        self.robots[robot] = state

        def on_batt(msg, s=state):
            s['v'] = msg.voltage

        def on_odom(msg, s=state):
            s['x'] = msg.pose.pose.position.x
            s['y'] = msg.pose.pose.position.y
            s['speed'] = msg.twist.twist.linear.x

        self.subs.append(self.create_subscription(
            BatteryState, f'/{robot}/sensors/battery', on_batt, 10))
        self.subs.append(self.create_subscription(
            Odometry, f'/{robot}/odometry/filtered', on_odom, 10))
        self.get_logger().info(f'nouveau robot : {robot}')

    def tick(self):
        self.discover()
        lines = ['robot       batterie   x (odom)  y (odom)  vitesse']
        for robot in sorted(self.robots, key=lambda r: int(r.split('_')[1])):
            s = self.robots[robot]
            lines.append(f"{robot:<11} {s['v']:6.2f} V  {s['x']:8.2f}  {s['y']:8.2f}"
                         f"  {s['speed']:5.2f} m/s")
        self.get_logger().info('\n' + '\n'.join(lines))


def main():
    rclpy.init()
    node = Flotte()
    try:
        rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == '__main__':
    main()