/*********************************************************************************
 **
 ** class SmartSampler
 **
 *********************************************************************************/
#include <Attributes.h>
#include <AttributeParser.h>
#include <StringAttribute.h>
#include <Syslog.h>
#include "SmartSampler.hh"
#include "SmartSamplerExceptions.hh"
#include "System.h"
#include "dtConfigList.hh"

#define CFG_FNAME "smartSampler.cfg"

// Add a signal handler!

// structors :
SmartSampler::SmartSampler(int millisec)
  :PeriodicTask(SmartSamplerName),
   cmd_queue_rd(MessageQueue::Read),
   shared_mem_wr(SharedData::Write),
   ssi(),ssd(&ssi){
  GulperIF::gs10 gu_state;
  int i;
  dti_t di;

  Syslog::write("SmartSampler -- Main class created.");
  
  loadConfig(); // also instantiates the decisiontools

  // Just in case we already have commands queued up, take care of
  // them here.
  ssdbg(DBG_LOAD)("SmartSampler -- Calling checkMessages.");
  checkMessages();

  // Initialize the shared memory
  ssdbg(DBG_LOAD)("Initializing shared memory with gulper info:");
  for(i=0;i<NGULPERS;i++){
    ssdbg(DBG_LOAD)("Gulper%d state: %d, alloc: %d",i,gulps[i].state,gulps[i].dtid);
  }
  dumpOutput();

  ssd.finishInit(); // Tell SSD that we're done with initialization.

  addPeriodicCallback(millisec, (CallbackMethod)SmartSampler::callback);
}

SmartSampler::~SmartSampler() {
  Syslog::write("SmartSampler -- destroying the main class.");
  int i;
  dti_t di;
  for(di=dtinfo.begin();di!=dtinfo.end();di++){
    // Shouldn't be necessary, but I'll play it safe.
    if((*di).name != NULL)free((*di).name);
    if((*di).inst != NULL)delete((*di).inst);
  }
}

// Manipulators :

// For loading the config file, we use the Attributes class, and a
// derived ItemList class, but we keep them local here because we only
// use them to load.  The information picked up goes into structures
// which are simpler to manage.
void SmartSampler::loadConfig(){
  Attributes  cfg_att(SmartSamplerName);
  cfgItemList dtinst_att;
  cfgItem     *curItem;
  int         i,j,cfgid,nextID;
  dti_t       di;
  dt_info_t   dtTmp;
  char        at_name[10],*at_gulps[NGULPERS];
  const char  *cfgFileName = System::configurationFile( CFG_FNAME );
  double      timenow;

  ssdbg(DBG_LOAD)("SmartSampler -- Entered loadConfig.");
  timenow = sst.missionTime();

  System::copyToLogDir(cfgFileName);

  // We add a stub to be DT[0] for direct firing
  dtTmp.id = 0;
  dtTmp.model = DecisionTool::dt_model_none;
  dtTmp.name = strdup("Direct firing mechanism");
  dtTmp.suspend = false; // Direct firing should never be suspended!
  dtTmp.lasttime = 0;
  dtTmp.maxwait = 0;
  dtTmp.minwait = 0;
  dtTmp.inst = NULL;
  dtinfo.push_back(dtTmp);

  for(i=0;i<NGULPERS;i++){
    sprintf(at_name,"gulper%d",i);
    cfg_att.add(new StringAttribute(at_name,"Gulper allocation attribute",&at_gulps[i],"none"));
    gulps[i].dtid = -1;
    gulps[i].state = SmartSamplerIF::Free;
    gulps[i].time = timenow;
  }

  // pick up attributes defined at this level:
  AttributeParser::reset();
  try{
    AttributeParser::parse(cfgFileName, &cfg_att, &dtinst_att);
  }catch(...){
    throw LoadError("Failed to parse attributes.");
    exit(0);
  }

  nextID = 0;
  for(i=0;i<dtinst_att.count();i++){
    curItem = (cfgItem *)dtinst_att.findItem(i);
    if((curItem->model() < DecisionTool::dt_model_none) || 
       (curItem->model() >= DecisionTool::dt_model_outofrange)){ 
      Syslog::write("SmartSampler -- DecisionTool %d (\"%s\") has invalid model number %d!",
		    i,curItem->name(),curItem->model());
      throw LoadError("Unknown DecisionTool model in config file.");
      exit(0);
    }else if(curItem->model() == DecisionTool::dt_model_none){
      ssdbg(DBG_LOAD)("Found empty DT in attributes.  Do nothing.");
    }else{
      nextID = dtinfo.size();
      ssdbg(DBG_LOAD)("SmartSampler -- Initializing info for decision tool %d (\"%s\").",
		    nextID,curItem->name());
      curItem->setid(nextID);
      dtTmp.id = nextID;
      dtTmp.model = (DecisionTool::model_type)curItem->model();
      dtTmp.name = strdup(curItem->name());
      dtTmp.suspend = false;
      dtTmp.lasttime = 0;
      dtTmp.maxwait = curItem->maxwait();
      dtTmp.minwait = curItem->minwait();
      dtTmp.inst = DecisionTool::genDT(dtTmp.model,dtTmp.name);
      dtinfo.push_back(dtTmp);
    }
  }
  ssdbg(DBG_LOAD)("SmartSampler -- Found %d valid decision tools in config file.",nextID);

  for(i=0;i<NGULPERS;i++){
    if(!strcmp(at_gulps[i],"none")){
      Syslog::write("SmartSampler -- Gulper %d unassigned!",i);
    }else{
      curItem = (cfgItem *)dtinst_att.findItem(at_gulps[i]);
      if(curItem == NULL){
	Syslog::write("SmartSampler -- Gulper %d assigned to Unknown DecisionTool \"%s\"!  Ignoring!",
		      i,at_gulps[i]);
	// throw LoadError("Unknown DecisionTool in gulper assignment.");
	// exit(0);
      }else{
	cfgid = curItem->getid();
	ssdbg(DBG_LOAD)("SmartSampler -- Gulper %d assigned to DecisionTool %d (\"%s\").",
		      i,cfgid,at_gulps[i]);
	gulps[i].dtid = cfgid;
	gulps[i].state = SmartSamplerIF::Allocated;
      }
    }
    ssdbg(DBG_LOAD)("SmartSampler -- Freeing at_gulps[%d]=\"%s\".",i,at_gulps[i]);
    free((void *)at_gulps[i]);
  }

  // Here we load configuration info for each of the instantiated decision tools. 
  ssdbg(DBG_LOAD)("SmartSampler -- Loading dt configs.");
  for(di=dtinfo.begin()+1;di!=dtinfo.end();di++){
    curItem = (cfgItem *)dtinst_att.findItem((*di).name);
    ssdbg(DBG_LOAD)("SmartSampler -- Loading config file \"%s\" for \"%s\".",
	  curItem->dtconfig(),(*di).name);
    (*di).inst->loadConfig(curItem->dtconfig(),&ssd);
  }

  ssdbg(DBG_LOAD)("SmartSampler -- Loaded config file.");
}

void SmartSampler::callback() {
  GulperIF::gs10 gu_state;
  double      timenow,lasttime;
  int         gnum,dtnum,lastgulper;
  dti_t       di;
  bool        fireOne = false;

  timenow = sst.missionTime();
  ssdbg(DBG_CALL)("SmartSampler -- Callback at %.4f",timenow);

  ssdbg(DBG_CALL)("SmartSampler -- Updating gulper state.");
  if(!ssi.gulperState(gu_state)){
    Syslog::write("SmartSampler -- No gulper interface attached!  Exiting callback.");
    return;
  }

  ssdbg(DBG_CALL)("SmartSampler -- Updating data manager state.");
  ssd.update();

  // Handle any commands from server.
  checkMessages();

  // Pick up the official state of the gulpers (different from my internal state)
  ssdbg(DBG_CALL)("SmartSampler -- Picking up gulper state.");
  lasttime = 0;
  lastgulper = -1;
  for(gnum=0;gnum<NGULPERS;gnum++){
    if(gu_state[gnum] == GulperIF::Used){
      if(gulps[gnum].state != SmartSamplerIF::Fired){
	Syslog::write("SmartSampler -- Gulper number %d fired without me!",gnum);
	gulps[gnum].state = SmartSamplerIF::Fired;
	gulps[gnum].dtid = -1;
	gulps[gnum].time = timenow;
      }
      if(lasttime < gulps[gnum].time){
	lasttime = gulps[gnum].time;
	lastgulper = gnum;
      }
    }else if(gulps[gnum].dtid == 0){
      if(gulps[gnum].state == SmartSamplerIF::Fired){
	Syslog::write("SmartSampler -- Warning! Gulper number %d failed direct fire!",gnum);
	gulps[gnum].dtid = -1;
      }
      fireOne = true;
    }
  }

  // Loop through all the instantiated decision tools and update.
  // Each DT sends back a fire/don't fire boolean.  We then test
  // against suspension, and maxwait.  If all of this results in a
  // fire request, AND the system-wide lockout and minwait lockouts
  // are satisfied, AND there is a Ready gulper allocated to this guy,
  // then we fire.
  for(di=dtinfo.begin();di!=dtinfo.end();di++){
    //  for(dtnum=0;(dtnum<MAX_DTS) && (dtinfo[dtnum].model != DecisionTool::dt_model_outofrange);dtnum++){
    dtnum = (*di).id;
    if(dtnum > 0){
      fireOne = (*di).inst->update(); // Fire request from DT
    }
    if(!fireOne && ((*di).lasttime > 0) && ((*di).maxwait > 0) && 
       (timenow - (*di).lasttime > (*di).maxwait)){
      // Not sure how I feel about this...
      Syslog::write("SmartSampler -- Requesting gulp on behalf of DT \"%s\" due to maxwait at t=%.4fs",
		    (*di).name,timenow);
      fireOne = true;
    }
    if(fireOne){
      ssdbg(DBG_CALL)("SmartSampler -- Checking DT %d for next gulper number.",dtnum);
      gnum = nextGulper(dtnum); // gulper allocation check
      ssdbg(DBG_CALL)("SmartSampler -- Found gulper number %d.",gnum);
      if((gnum < 0) || (gnum >= NGULPERS)){
	Syslog::write("SmartSampler -- DT \"%s\" request denied due to lack of allocated gulpers at t=%.4fs",
		      (*di).name,timenow);
	fireOne = false;
      }
    }
    if(fireOne && (timenow - lasttime < SYSTEM_LOCKOUT_MIN)){
      Syslog::write("SmartSampler -- DT \"%s\" request denied due to system wide lockout at t=%.4fs",
		    (*di).name,timenow);
      // Treat a lockout as though we fired.  This ONLY affects debug messages.  Future lockout
      // is based on (*di).time, which is only set when fired by us or someone else.
      (*di).lasttime = timenow;
      fireOne = false;
    }
    if(fireOne && (timenow - lasttime < (*di).minwait)){
      Syslog::write("SmartSampler -- DT \"%s\" request denied due to DT specific lockout (minwait) at t=%.4fs",
		    (*di).name,timenow);
      (*di).lasttime = timenow;
      fireOne = false;
    }
    if(fireOne && (*di).suspend){
      Syslog::write("SmartSampler -- DT \"%s\" request denied due to suspension at t=%.4fs",
		    (*di).name,timenow);
      fireOne = false;
    }
    if(fireOne){
      Syslog::write("SmartSampler -- Firing gulper %d per DT \"%s\" at t=%.4fs",
		    gnum,(*di).name,timenow);
      (*di).lasttime = timenow;
      lasttime = timenow; // This will lock out any other guys from triggering a gulp this round.
if(ssi.fireGulper(gnum)){
	gulps[gnum].state = SmartSamplerIF::Fired;
	gulps[gnum].time = timenow;
      }
    }
  } // end DT loop

  // Update the shared memory
  dumpOutput();
}

// Here we handle any commands sent from the server.
void SmartSampler::checkMessages(){
  SmartSamplerInput::Command cmdObj;
  double          timenow;
  bool            argsok;
  short           gnum,dtnum,dtMaxID;

  dtMaxID = dtinfo.size()-1;
  ssdbg(DBG_CMD)("SmartSampler -- Entered checkMessages.");
  timenow = sst.missionTime();
  ssdbg(DBG_CMD)("SmartSampler -- checkMessages time = %.4fs.",timenow);
  ssdbg(DBG_CMD)("SmartSampler:: %d messages pending.", 
		cmd_queue_rd.msgsPending());

  while ((cmd_queue_rd.read(&cmdObj)) > 0){
    ssdbg(DBG_CMD)("Checking next message."); 
    // First we do a quick check to see if the args are ok.
    argsok = true;
    switch(cmdObj._cmd){
    case SmartSamplerInput::cmdSuspend:
    case SmartSamplerInput::cmdResume:
    case SmartSamplerInput::cmdReset:
      dtnum = cmdObj._target;
      if((dtnum < 0) || (dtnum > dtMaxID)){
	Syslog::write("SmartSampler -- Command ERROR! Bad DT index arg \"%d\"!",dtnum);
	argsok = false;
      }
      break;

    case SmartSamplerInput::cmdDumpDTState:
      dtnum = cmdObj._target;
      if((dtnum < -1) || (dtnum > dtMaxID)){
	Syslog::write("SmartSampler -- DT state dump requested for unknown DT index %d.",dtnum);
	argsok = false;
      }
      break;

    case SmartSamplerInput::cmdFire:
      gnum = cmdObj._target;
      if((gnum < 0) || (gnum >= NGULPERS)){
	Syslog::write("SmartSampler -- Command ERROR! Bad gulper index arg \"%d\"!",gnum);
	argsok = false;
      }
      break;

    case SmartSamplerInput::cmdAllocate:
      gnum = cmdObj._target;
      dtnum = cmdObj._arg;
      if((gnum < 0) || (gnum >= NGULPERS)){
	Syslog::write("SmartSampler -- Command ERROR! Bad gulper index arg \"%d\"!",gnum);
	argsok = false;
      }else if(dtnum == 0){
	Syslog::write("SmartSampler -- Allocating gulper %d to DT 0 will cause immediate gulper fire.",gnum);
      }else if((dtnum < 0) || (dtnum > dtMaxID)){
	Syslog::write("SmartSampler -- Warning! Bad DT id arg \"%d\" will cause gulper deallocation.",dtnum);
      }
      break;

    default:
      Syslog::write("SmartSampler -- Unrecognized command code at t=%.4fs",timenow);
      argsok = false;
      break;
    } // end cases (check args)

    // Only bother with this guy if the args are valid.
    if(argsok){
      switch (cmdObj._cmd) {
      case SmartSamplerInput::cmdNone:
	ssdbg(DBG_CMD)("SmartSampler -- Warning! Received empty command at t=%.4fs", 
		      timenow);
	break;

      case SmartSamplerInput::cmdSuspend:
	if(dtnum == 0)
	  Syslog::write("Can't suspend decision tool 0.  Command ignored.");
	else{
	  ssdbg(DBG_CMD)("SmartSampler -- Suspending DT number %d at t=%.4fs",
		dtnum,timenow);
	  dtinfo[dtnum].suspend = true;
	}
	break;

      case SmartSamplerInput::cmdResume:
	ssdbg(DBG_CMD)("SmartSampler -- Resuming DT number %d at t=%.4fs",
		      dtnum,timenow);
	dtinfo[dtnum].suspend = false;
	break;

      case SmartSamplerInput::cmdReset:
	ssdbg(DBG_CMD)("SmartSampler -- Resetting DT number %d at t=%.4fs",
		      dtnum,timenow);
	dtinfo[dtnum].inst->reset();
	break;

      case SmartSamplerInput::cmdDumpDTState:
	if(dtnum == -1){
	  Syslog::write("SmartSampler -- Dumping state for all DecisionTool's");
	  for(dtnum=1;dtnum<=dtMaxID;dtnum++){
	    dumpDTState(dtnum);
	  }
	}else{
	  dumpDTState(dtnum);
	}
	break;

      case SmartSamplerInput::cmdAllocate:
	switch (gulps[gnum].state) {
	case SmartSamplerIF::Free:
	  ssdbg(DBG_CMD)("SmartSampler -- Allocating gulper %d to DT %d at t=%.4fs",
			gnum, dtnum,timenow);
	  gulps[gnum].dtid = dtnum;
	  gulps[gnum].time = timenow;
	  gulps[gnum].state = SmartSamplerIF::Allocated;
	  break;
	case SmartSamplerIF::Allocated:
	  // Same as case above, but keep it separate in case we change something.
	  ssdbg(DBG_CMD)("SmartSampler -- Reallocating gulper %d from DT %d to DT %d at t=%.4fs",
			gnum, gulps[gnum].dtid, dtnum, timenow);
	  gulps[gnum].dtid = dtnum;
	  gulps[gnum].time = timenow;
	  break;
	case SmartSamplerIF::Fired:
	  ssdbg(DBG_CMD)("SmartSampler -- Cannot Reallocate spent gulper %d at t=%.4fs",
			gnum, gulps[gnum].dtid, timenow);	 
	  break;
	}// end case (gulper state)
	break; // End of allocate command.

      case SmartSamplerInput::cmdFire:
	switch (gulps[gnum].state) {
	case SmartSamplerIF::Free:
	  ssdbg(DBG_CMD)("SmartSampler -- Allocating gulper %d to direct firing mechanism at t=%.4fs",
			gnum, timenow);
	  gulps[gnum].dtid = 0;
	  gulps[gnum].time = timenow;
	  gulps[gnum].state = SmartSamplerIF::Allocated;
	  break;
	case SmartSamplerIF::Allocated:
	  // Same as case above, but keep it separate in case we change something.
	  ssdbg(DBG_CMD)("SmartSampler -- Reallocating gulper %d from DT %d to direct firing mechanism at t=%.4fs",
			gnum, gulps[gnum].dtid, timenow);
	  gulps[gnum].dtid = 0;
	  gulps[gnum].time = timenow;
	  break;
	case SmartSamplerIF::Fired:
	  ssdbg(DBG_CMD)("SmartSampler -- Cannot Reallocate spent gulper %d to direct firing mechanism at t=%.4fs",
			gnum, gulps[gnum].dtid, timenow);	 
	  break;
	}// end case (gulper state)
	break; // End of allocate command.

      default:
	break;
      } // end cases (process)
    } // end if argsok

    if(cmd_queue_rd.msgsPending())
      ssdbg(DBG_CMD)("SmartSampler:: %d messages pending.",cmd_queue_rd.msgsPending());
  }
}

// Here we spit out state for the server to pick up.
void SmartSampler::dumpOutput(){
  int i,j;
  for(i=0;i<NGULPERS;i++){
    shared_mem_wr.data.gulperInfo[i].time = gulps[i].time;
    shared_mem_wr.data.gulperInfo[i].curstate = gulps[i].state;
    shared_mem_wr.data.gulperInfo[i].dtid = gulps[i].dtid;
  }
  shared_mem_wr.data.dtNum = dtinfo.size()-1; // Don't count direct firing mechanism.
  shared_mem_wr.write();
}

void SmartSampler::dumpDTState(short dtid){
  char suspended[16],gulperstr[32],istr[2];
  int i;
  if(dtinfo[dtid].suspend){
    strcpy(suspended,"suspended");
  }else{
    strcpy(suspended,"not suspended");
  }
  strcpy(gulperstr,"");
  for(i=0;i<NGULPERS;i++){
    if(gulps[i].dtid == dtid){
      if(strlen(gulperstr) != 0){
	strcat(gulperstr,", ");
      }
      sprintf(istr,"%d",i);
      strcat(gulperstr,istr);
    }
  }
  if(strlen(gulperstr) == 0){
    Syslog::write("DT%d (\"%s\") is %s.  Last fired at t=%.4fs (minwait=%.4fs,maxwait=%.4fs).  No gulpers allocated.",
		  dtid,dtinfo[dtid].name,suspended,dtinfo[dtid].lasttime,
		  dtinfo[dtid].minwait,dtinfo[dtid].maxwait);
  }else{
    Syslog::write("DT%d (\"%s\") is %s.  Last fired at t=%.4fs (minwait=%.4fs,maxwait=%.4fs).  Allocated to %s.",
		  dtid,dtinfo[dtid].name,suspended,dtinfo[dtid].lasttime,
		  dtinfo[dtid].minwait,dtinfo[dtid].maxwait,gulperstr);
  }
}

int SmartSampler::nextGulper(short dtid){
  int i;
  for(i=0;i<NGULPERS;i++){
    if((gulps[i].dtid == dtid) && (gulps[i].state == SmartSamplerIF::Allocated)){
      break;
    }
  }
  return(i);
}

// Want to put something in which takes the pitch angle from nav and
// signals whether we think we're going down, up, or neither.
// NavigationIF::Attitude *attitude