/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : navigation.cc                                                 */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <signal.h>
#include <sys/sched.h>
#include <errno.h>
#include "PeriodicTask.h"
#include "Navigation.h"
#include "NavigationIF.h"
#include "Syslog.h"
#include "Exception.h"
#include "System.h"

class GNC : public PeriodicTask {

public:

  GNC(int millisec)
    : PeriodicTask("GNC")
  {
    // Needed for real-time missions and sims
    addPeriodicCallback(millisec, (CallbackMethod )GNC::callback);
  }

  virtual ~GNC(){}

  ///////////////////////////////////////////////////////////////////
  // Periodic callback
  void callback()
  {
    nav.process();
    triggerEvent(NavigationIF::NewOutput);
  }

protected:

  Navigation nav;

};


int main(int argc, char **argv)
{
  // Set task priority
  if (System::setTaskPriority(NavigationPriority) == -1)
    return 1;

  GNC *gnc = 0;

  try {
    gnc = new GNC(200);

    // Real-time missions and sims
    gnc->run();

    // FastSims
    // while (1) gnc->callback();
  }
  catch (Exception e) {
    Syslog::write("navigation - caught exception: %s\n", e.msg);
  }
  catch (...) {
    Syslog::write("navigation - caught exception!\n");
  }

  return 0;
}