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

# ROS imports
import rospy
import rosnode
from std_srvs.srv import Trigger, TriggerResponse
import subprocess
import os


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

    """

    def __init__(self, record_folder, topics):
        # initialize items
        self.start_recording_service = rospy.Service(
            '/recorder/start_recording', Trigger, self.startRecord)
        self.stop_recording_service = rospy.Service(
            '/recorder/stop_recording', Trigger, self.stopRecord)
        self.reboot_service = rospy.Service(
            '/recorder/reboot', Trigger, self.reboot)
        self.shutdown_service = rospy.Service(
            '/recorder/shutdown', Trigger, self.shutdown)
        self.process = None
        self.recording = False
        # get arguments
        self.record_folder = record_folder + '/'
        self.topics = topics

        # setup the command
        self.command = ['rosrun', 'rosbag', 'record', '-e'] + \
            self.topics + ['__name:=recorder_rosbag']

    def startRecord(self, req):
        if self.recording:
            rospy.logerr('tried to start when already recording')
            return TriggerResponse(False, 'already recording')

        rospy.loginfo("starting recording")
        self.process = subprocess.Popen(
            self.command, cwd=self.record_folder)
        self.recording = True
        rospy.loginfo('Started recorder, process ID: ' + str(self.process.pid))
        return TriggerResponse(True, 'Started recorder, PID: ' +
                               str(self.process.pid))

    def stopRecord(self, req):
        if not self.recording:
            rospy.logerr('sent stop when not recording')
            return TriggerResponse(False, 'not recording')

        rospy.loginfo("stopping recording")
        rosnode.kill_nodes(['/recorder_rosbag'])

        self.process = None
        self.recording = False

        rospy.loginfo('Stopped Recording')
        return TriggerResponse(True, 'Stopped Recording')

    def reboot(self, req):
        rospy.loginfo("command to reboot recvd")
        os.system('reboot')

    def shutdown(self, req):
        rospy.loginfo("command to shutdown recvd")
        os.system('shutdown -h now')


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

    # -------------------------- Read Parameters -----------------------------
    record_folder = rospy.get_param('%s/record_folder' % node_name, None)
    topics = rospy.get_param('%s/topics' % node_name, [])
    if not topics:
        rospy.logerr('No Topics Specified.')

    rospy.loginfo("starting recorder node")
    # ------------------------------------------------------------------------
    rosRecorder(record_folder, topics)
    rospy.spin()


if __name__ == '__main__':
    main()
