/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : _gnc.cc                                                       */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "Gnc.h"
#include "Navigation.h"
#include "LayeredControl.h"
#include "DynamicControl.h"
#include "Simulator.h"
#include "Syslog.h"
#include "System.h"

int main(int argc, char **argv)
{
    Boolean debug=True;
    char *planFileName = strdup("normalMplan.cfg");
    char *abortFileName = strdup("abortMplan.cfg");
    Boolean error=False;
    Boolean enSim=False;
    Boolean restart=False;
    int retval=1;
    
    // parse command line
    for (int i = 1; i < argc; i++) {
        dprintf("_gnc - processing arg[%d]:%s\n",i,argv[i]);
        if (!strcmp(argv[i], "-plan")) {
            if (i < argc - 1) {
                free((void *)planFileName);
                planFileName = strdup(argv[++i]);
            }
            else
                error = True;
        }else if (!strcmp(argv[i], "-abort")) {
            if (i < argc - 1) {
                free((void *)abortFileName);
                abortFileName = strdup(argv[++i]);
            }
            else
                error = True;
        }else if (!strcmp(argv[i], "-fastsim")) {
            enSim=True;
        }else if (!strcmp(argv[i], "-restart")) {
            Syslog::write("_gnc - restart set: ignoring, returning\n");
            return retval;
        }
    }

    // Set task priority
    if (System::setTaskPriority(GncPriority) == -1){
        dprintf("_gnc - setTaskPriority Failed");
        return retval;
    }
    
    // configure Navigation instance
    Navigation *nav = NULL;
    
    try{
        dprintf("_gnc - create Navigation");
        nav = new Navigation(0);
    }catch(Exception naverr){
        Syslog::write("_gnc - caught exception [nav]: %s\n", naverr.msg );
        error=True;
    }
    
    // configure LayeredControl instance
    LayeredControl *laycon = NULL;
    char normalPlanFile[256];
    char abortPlanFile[256];
    
    try{
        dprintf("_gnc - create LayeredControl");
        
        strcpy(normalPlanFile, System::missionPlanFile(planFileName));
        strcpy(abortPlanFile, System::missionPlanFile(abortFileName));
        
        System::copyToLogDir(normalPlanFile);
        System::copyToLogDir(abortPlanFile);

        laycon = new LayeredControl(normalPlanFile, abortPlanFile,0);
        dprintf("_gnc - initializing LayeredControl [%s %s]",normalPlanFile,abortPlanFile);
    }catch(Exception layerr){
        Syslog::write("_gnc - caught exception [laycon]: %s\n", layerr.msg );
        error=True;
    }

    // configure DynamicControl instance
    DynamicControl *dyncon = NULL;
    
    try{
        dprintf("_gnc - create DynamicControl");
        dyncon = new DynamicControl();
    }catch(Exception dconerr){
        Syslog::write("_gnc -- caught exception [dyncon]: %s\n", dconerr.msg );
        error=True;
    }

    // if no errors, configure and run GNC
    if (!error) {
        // initialize LayeredControl (requires DynamicControl reference)
        laycon->initialize(normalPlanFile,abortPlanFile, dyncon);
        
        Gnc *gnc = NULL;
        
        try {
            // configure and run GNC instance
            dprintf("_gnc - creating gnc sim[%s]",(enSim?"Y":"N"));
            gnc  = new Gnc(GncPeriodMillisec,nav,laycon,dyncon,enSim);
            dprintf("_gnc - calling initialize");
            gnc->initialize();
            dprintf("_gnc - calling run");
            gnc->run();
        }
        catch (Exception e) {
            Syslog::write("_gnc - caught exception [gnc]: %s\n", e.msg);
        }
        catch (...) {
            Syslog::write("_gnc - caught unknown exception [gnc]\n");
        }
        // clean up, set return value
        delete gnc;
        retval=0;
    }else{
        Syslog::write("_gnc - there were errors, exiting\n");
    }
    
    return retval;

}