#!/usr/bin/env python

#ROS driver for the Chemspeed FLEX robot
#Utilises ROS Topics to facilitate communication with robot
#Made by Hatem Fakhruldeen 23/03/2022

import rospy
import os
import subprocess
from chemspeed_flex_msgs.msg import CSFlexCommand, CSFlexStatus
from chemspeed_flex_driver.chemspeed_flex_driver import ChemSpeedFlexDriver
        
class ChemSpeedFlexROS:
    
    def __init__(self, app_path: str):
        self._cs_driver = ChemSpeedFlexDriver(app_path)
        self._cs_driver.initialse()
        self._state = CSFlexStatus.DOOR_CLOSED
        self._terminate_driver = False
        # init the pub and subscriber
        rospy.Subscriber("ChemSpeed_Flex_commands", CSFlexCommand, self.command_callback)
        self.pub = rospy.Publisher("ChemSpeed_Flex_status", CSFlexStatus, queue_size=10)
        rate = rospy.Rate(1) #Initialize rate object for consistent timed looping
        rospy.loginfo("ChemSpeed FLEX driver started")
        try:
            while not rospy.is_shutdown(): #Whenever driver is running, loop each second polling all values and publishing them to topic
                if (self._state == CSFlexStatus.RUNNING_JOB and self._cs_driver.is_job_complete()):
                    self._state = CSFlexStatus.JOB_COMPLETE
                self.pub.publish(CSFlexStatus(self._state))
                if (self._terminate_driver):
                    break
                rate.sleep()
            self._cs_driver.terminate()
        except KeyboardInterrupt:
            self._cs_driver.terminate()
        
    def command_callback(self, msg): #Callback for subscriber. Calls correct function depending on command received
        if msg.cs_flex_command == CSFlexCommand.OPEN_DOOR:
            self._cs_driver.open_door()
            self._state = CSFlexStatus.DOOR_OPEN
        elif(msg.cs_flex_command == CSFlexCommand.CLOSE_DOOR):
            self._cs_driver.close_door()
            self._state = CSFlexStatus.DOOR_CLOSED 
        elif (msg.cs_flex_command == CSFlexCommand.RUN_APP):
            if self._cs_driver.start_job():
                self._state = CSFlexStatus.RUNNING_JOB
        elif (msg.cs_flex_command == CSFlexCommand.TERMINATE):
            self._terminate_driver = True

