content: module Ros.Node.RunNode (runNode) where
  import Control.Concurrent (readMVar,forkIO, killThread)
  import qualified Control.Concurrent.SSem as Sem
  import qualified Control.Exception as E
  import Control.Monad.IO.Class
  import System.Posix.Signals (installHandler, Handler(..), sigINT)
  import Ros.Internal.RosTypes
  import Ros.Internal.Util.AppConfig (Config, debug)
  import Ros.Graph.Master
  import Ros.Graph.Slave
  
  -- Inform the master that we are publishing a particular topic.
  registerPublication :: RosSlave n => 
                         String -> n -> String -> String -> 
                         (TopicName, TopicType, a) -> Config ()
  registerPublication name _n master uri (tname, ttype, _) = 
      do debug $ "Registering publication of "++ttype++" on topic "++
                 tname++" on master "++master
         _subscribers <- liftIO $ registerPublisher master name tname ttype uri
         return ()
  
  -- Inform the master that we are subscribing to a particular topic.
  registerSubscription :: RosSlave n =>
                          String -> n -> String -> String -> 
                          (TopicName, TopicType, a) -> Config ()
  registerSubscription name n master uri (tname, ttype, _) = 
      do debug $ "Registring subscription to "++tname++" for "++ttype
         (r,_,publishers) <- liftIO $ registerSubscriber master name tname ttype uri
         if r == 1 
           then liftIO $ publisherUpdate n tname publishers
           else error "Failed to register subscriber with master"
         return ()
  
  registerNode :: RosSlave s => String -> s -> Config ()
  registerNode name n = 
      do uri <- liftIO $ readMVar (getNodeURI n)
         let master = getMaster n
         debug $ "Starting node "++name++" at " ++ uri
         liftIO (getPublications n) >>= 
           mapM_ (registerPublication name n master uri)
         liftIO (getSubscriptions n) >>= 
           mapM_ (registerSubscription name n master uri)
  
  -- |Run a ROS Node with the given name. Returns when the Node has
  -- shutdown either by receiving an interrupt signal (e.g. Ctrl-C) or
  -- because the master told it to stop.
  runNode :: RosSlave s => String -> s -> Config ()
  runNode name s = do (wait, _port) <- liftIO $ runSlave s
                      registerNode name s
                      debug "Spinning"
                      allDone <- liftIO $ Sem.new 0
                      let ignoreEx :: E.SomeException -> IO ()
                          ignoreEx _ = return ()
                          shutdown = do putStrLn "Shutting down"
                                        cleanupNode s `E.catch` ignoreEx
                                        Sem.signal allDone
                      liftIO $ setShutdownAction s shutdown
                      _ <- liftIO $ 
                           installHandler sigINT (CatchOnce shutdown) Nothing
                      t <- liftIO . forkIO $ wait >> Sem.signal allDone
                      liftIO $ Sem.wait allDone
                      liftIO $ killThread t `E.catch` ignoreEx
  