#!/usr/bin/env python

from __future__ import print_function

import shlex
import subprocess

import colorama

import rostopic
import rosgraph


def main():
    master = rosgraph.Master('/ros_host_sanity')
    state = master.getSystemState()
    pubs, subs, _ = state

    host_pub_topics, host_sub_topics = rostopic._rostopic_list_group_by_host(
        master, pubs, subs)

    hostnames = set(host_pub_topics.keys() + host_sub_topics.keys())
    for hostname in hostnames:
        cmd = 'ping -c 1 -W 1 {}'.format(hostname)
        ret = subprocess.call(shlex.split(cmd),
                              stdout=subprocess.PIPE, stderr=subprocess.PIPE)
        if ret == 0:
            result = colorama.Fore.GREEN + 'Connected' + colorama.Fore.RESET
        else:
            result = colorama.Fore.RED + 'Disconnected' + colorama.Fore.RESET
        print('{}: {}'.format(hostname, result))


if __name__ == '__main__':
    main()
