//dprintf("trigger<Navigation, event> - _eventTriggers ptr = x%x\n", gnc);
/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Gnc.cc                                                        */
/* Author   : K. Headley                                                    */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/25/2016                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "Gnc.h"
#include "GncParentIF.h"
#include "GncInput.h"
#include "GncOutput.h"

#include "Navigation.h"
//include "LayeredControl.h"

#include "Behavior.h"
#include "DynamicControl.h"

#include "FastTime.h"
#include "Syslog.h"
#include <stdio.h>
#include <stdlib.h>
#include <process.h>
#include <signal.h>
#include <errno.h>
#include <string.h>
#include <sys/sched.h>
#include <sys/stat.h>
#include <dirent.h>
#include <time.h>
#include <unistd.h>

///////////////////////////////////////////////////////////////////
// No-arg Constructor
Gnc::Gnc()
: PeriodicTask("gnc")
{
    Boolean debug = True;
    dprintf("Gnc default ctor");
    _navigation=NULL;
    _laycon=NULL;
    _dyncon=NULL;
    _simulatorIF=NULL;
    addPeriodicCallback(3, (CallbackMethod )Gnc::callback);
    _quitFlag=False;
}

// periodic task Constructor
Gnc::Gnc(int millisec,
         Navigation *nav,
         LayeredControl *laycon,
         DynamicControl *dyncon,
         Boolean enSim)
: PeriodicTask("gnc")
{
    Boolean debug = True;
    dprintf("Gnc init ctor");
    _quitFlag=False;
    _navigation=nav;
    _laycon=laycon;
    _dyncon=dyncon;
    _enSim=enSim;
    _simulatorIF=NULL;
    
    try {
        _output = new GncOutput(SharedData::ReadWrite);
    }
    catch(...) {
        Syslog::write("Gnc::Gnc Error - Couldn't open output mem");
    }

    if (!_enSim) {
        addPeriodicCallback(millisec, (CallbackMethod)Gnc::callback);
    }else{
    	_simulatorIF = new SimulatorIF("simulator");
    }
    if (_navigation) {
        _navigation->setParent((GncParentIF<Navigation> *)this);
        _navigation->_firstCallback = True;
    }
    
    try
    {
        _input = new GncInput(MessageQueue::ReadWrite);
    }
    catch(...)
    {
        Syslog::write("Gnc::Gnc Error - Couldn't open message queue.");
    }
    

}

///////////////////////////////////////////////////////////////////
// Default Destructor
Gnc::~Gnc()
{
    Boolean debug = True;
    dprintf("Gnc default dtor");
}


void Gnc::triggerEvent(EventCode event)
{
//    Boolean debug =True;
//    dprintf("triggerEvent - triggering event [%d]\n",event);
    Task::triggerEvent(event);
}


///////////////////////////////////////////////////////////////////
// implementation of GncIF trigger method
// [template class pure virtual method]
void Gnc::trigger(Navigation *nav, EventCode event)
{
    Boolean debug =False;
    if (nav) {
        dprintf("trigger<Navigation> - getting gncif self[%x]\n",this);
        Gnc *gnc = (Gnc *)nav->getParent();
        if (gnc) {
            dprintf("trigger<Navigation> - triggering !!! gncif[%x]\n", gnc);
            gnc->triggerEvent(event);
            dprintf("trigger<Navigation> - Trigger OK\n");
        }else{
            dprintf("trigger<Navigation> - gncif is NULL\n");
        }
    }else{
        dprintf("trigger<Navigation, event> - nav is NULL\n");
    }
}


void Gnc::configure(Navigation *nav, LayeredControl *lcon, DynamicControl *dcon)
{
    Boolean debug = True;
    dprintf("Gnc configure - in: n[%p] l[%p] d[%p]",nav,lcon,dcon);
    // replace references (if not NULL)
    // !!! NOTE:  does NOT destroy existing references (left to owner)
    if (nav) {
        _navigation=nav;

    }else{
        dprintf("Navigation reference n/c");
    }
    if (lcon) {
        _laycon=lcon;
    }else{
        dprintf("LayeredControl reference n/c");
    }
    if(dcon){
	    _dyncon=dcon;
    }else{
        dprintf("DynamicControl reference n/c");
    }
    dprintf("Gnc configure - out: n[%p] l[%p] d[%p]",nav,lcon,dcon);

}
void Gnc::startSampling()
{
    Boolean debug = True;
#ifdef FASTTIME
    dprintf("Gnc - startSampling: FASTTIME");
    // check whether FastTime enabled
    // [could cache the value, instead of checking environment each time]
    char *fast_time_en=getenv(FAST_TIME_ENV);
    if (fast_time_en && strcmp(fast_time_en,FAST_TIME_EN)==0)
    {
        Syslog::write("\t\t\t\t============== Gnc giving simulation a push start in 2...");
        sleep(2);
        callback();
    }
    // [klh] removed GNC
      else
#endif
          dprintf("Gnc - calling startSampling");
    // [klh] removed GNC
       PeriodicTask::startSampling();
}

int Gnc::initialize()
{
    Boolean debug = True;
    dprintf("Gnc initialize");

	// notify control that mission started
    // [triggers initialization]
    if (_dyncon) {
        _dyncon->notifyEvent(LayeredControlIF::MissionStarted);
    }else{
        dprintf("Gnc::initialize - dynamicControl is NULL\n");
    }

    // notify control that mission started
    // [triggers initialization]
    if (_simulatorIF) {
        _simulatorIF->notifyEvent(LayeredControlIF::MissionStarted);
    }else{
        dprintf("Gnc::initialize - simulator is NULL\n");
    }
    
// NOTE: sim fails (propSpeed,depth don't update) when DynamicControlIF::Ready
// event fired after MissionStarted sent to DynamicControl and Simulator
// Q: No one responds to DynamicControlIF::Ready, so why would it matter?
// A: Ready and NewOutput have the same value, so it's kicking off layeredControl
//    out of order.
    
//    if (_laycon) {
//        _laycon->notifyEvent(DynamicControlIF::Ready);
//    }else{
//        dprintf("Gnc::initialize - layeredControl is NULL\n");
//    }
    

    return -1;
}

///////////////////////////////////////////////////////////////////
// Periodic callback
void Gnc::callback()
{
    Boolean debug = False;
    //    static loop_count=-1;
    //    loop_count++;
    //
    if (_navigation && _navigation->abortFlag()) {
        debug=True;
        dprintf("Gnc::callback - navigation abort flag set\n");
        _quitFlag=True;
    }else if (_laycon && _laycon->abortFlag()) {
        debug=True;
        dprintf("Gnc::callback - layered control abort flag set\n");
        _quitFlag=True;
        dprintf("Gnc::callback - triggering LayeredControlIF::MissionStatusChange\n");
        triggerEvent(LayeredControlIF::MissionStatusChange);
    }else{
        if (_navigation) {
            dprintf("Gnc callback - calling nav callback this [%x]\n",this);
            _navigation->notifyEvent(SimulatorIF::NewOutput);
        }

        if (_laycon) {
            dprintf("Gnc callback - calling lcon callback");
            _laycon->notifyEvent(NavigationIF::NewOutput);
        }
	
        if (_dyncon) {
            dprintf("Gnc callback - calling dcon callback");
            _dyncon->notifyEvent(LayeredControlIF::OutputGenerated);
        }
        // simulator should handle itself
//        debug=True;
        if (_dyncon) {
            dprintf("Gnc::callback - writing state: lwt[%.2f] lwb[%.2f]\n",_dyncon->_time,_dyncon->_wallBearing);
            _state.dc.lastWallTime=_dyncon->_time;
            _state.dc.lastWallBearing=_dyncon->_wallBearing;
            _output->write(&_state);
       }else{
            dprintf("Gnc::callback - writing state: dyncon NULL\n");
        }
    }
    // do one control cycle
}

void Gnc::run()
{
    Boolean debug = True;
    dprintf("Gnc run");
    if (_enSim) {
        while (!_quitFlag) {
//            dprintf("Gnc run - sim loop");
            callback();
        }
        dprintf("Gnc::run - aborting !!!\n");
    }else{
        PeriodicTask::run();
    }
}


////////////////////////////////////////////////////////////////////
// Signal handler
static void Gnc::signalHandler(int signalNo)
{}

//void Gnc::trigger(EventCode event){
//    Boolean debug = False;
//    dprintf("Task::triggerEvent() - _eventTriggers ptr = x%x\n", gnc->_eventTriggers);
//    gnc->_eventTriggers->notify(event);
//}