#! /usr/bin/python
"""
Node to report diagnostic info for PTP.
"""
from __future__ import division, print_function

# ROS imports
import rospy
from diagnostic_msgs.msg import DiagnosticStatus

# Workspace imports
from ptp_health.system_checks import (
    checkHardware
    )


class ptpHealthROS(object):
    """
    The ROS implementation of a diagnostic publisher for PTP info

    """

    def __init__(self, pps_device, nmea_device, ros_ins_device, ptp_device,
                 gps_service, time_service, ptp_service):
        # initialize items
        self.hardwareChecker = checkHardware()
        # get arguments
        self.pps_device = pps_device
        self.nmea_device = nmea_device
        self.ros_ins_device = ros_ins_device
        self.ptp_device = ptp_device
        self.gps_service = gps_service
        self.time_service = time_service
        self.ptp_service = ptp_service

    def sysCheck(self):
        # check to see if they are using the PPS device
        if self.pps_device is not None:
            self.hardwareChecker.checkPPS(self.pps_device, self.PPSpub)
        # check nmea
        if self.nmea_device is not None:
            self.hardwareChecker.checkNMEA(self.nmea_device, self.NMEApub,
                                           self.gps_service)
        # check ros ins
        if self.ros_ins_device is not None:
            self.hardwareChecker.checkINS(self.ros_ins_device, self.INSpub,
                                          self.time_service)
        # check ptp
        if self.ptp_device is not None:
            self.hardwareChecker.checkPTP(self.ptp_device, self.PTPpub,
                                          self.ptp_service)

    def talker(self):
        """
        Runs in a loop and executes the checks

        """
        # setup publishers
        self.PPSpub = rospy.Publisher(
            '/ptp_health/pps', DiagnosticStatus, queue_size=10)
        self.NMEApub = rospy.Publisher(
            '/ptp_health/nmea', DiagnosticStatus, queue_size=10)
        self.INSpub = rospy.Publisher(
            '/ptp_health/ins', DiagnosticStatus, queue_size=10)
        self.PTPpub = rospy.Publisher(
            '/ptp_health/ptp', DiagnosticStatus, queue_size=10)

        rate = rospy.Rate(1)  # 1hz
        rospy.loginfo('checking system for required devices...')
        rospy.loginfo('    review rostopic /ptp_health/* for more info')
        # loop until stopped
        while not rospy.is_shutdown():
            self.sysCheck()
            rate.sleep()


def main():
    # Launch the node.
    node = 'ptp_health'
    rospy.init_node(node, anonymous=False)
    node_name = rospy.get_name()

    # -------------------------- Read Parameters -----------------------------
    #pps_device: /dev/pps0
    #nmea_device: /dev/gps
    #ros_ims_device: /dev/spatial
    #ptp_device: /dev/ptp0
    pps_device = rospy.get_param('%s/pps_device' % node_name, None)
    nmea_device = rospy.get_param('%s/nmea_device' % node_name, None)
    ros_ins_device = rospy.get_param('%s/ros_ins_device' % node_name, None)
    ptp_device = rospy.get_param('%s/ptp_device' % node_name, None)

    gps_service = rospy.get_param('%s/gps_service' % node_name, None)
    time_service = rospy.get_param('%s/time_service' % node_name, None)
    ptp_service = rospy.get_param('%s/ptp_service' % node_name, None)

    rospy.loginfo("starting ptp health diagnostics")
    # ------------------------------------------------------------------------
    ptpH = ptpHealthROS(pps_device, nmea_device,
                        ros_ins_device, ptp_device,
                        gps_service, time_service, ptp_service)
    ptpH.talker()

    # Blocks until ROS node is shutdown. Yields activity to other threads.
    # rospy.spin()


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
