sensornodegeneric.hpp

#ifndef DDF_SENSORNODEGENERIC_HPP
#define DDF_SENSORNODEGENERIC_HPP

#include "BayesFilter/bayesFlt.hpp"

#include "predictenginebase.hpp"
#include "predictmodelfactory.hpp"
#include "commfactory.hpp"
#include "infocontainer.hpp"
#include "periodicthread.hpp"
#include "channelfilterbase.hpp"
#include "definitions.hpp"
#include "ctime.hpp"
#include "debugging.hpp"

namespace jafar
{
  namespace ddf
  {

    /* Implement a sensor node with three functions: a local filter (nodal filter), a sensor manager (connection
       de-connection of other sensor nodes) and a synchronization mecanisme for fusing info from the other nodes
       (through channel filters) A sensor node must be able to do state prediction (the same as in the channel filters)

       author: Pierre Lamon */

    // Definitions of structures for connection/deconnection of sensor nodes
    typedef enum {SN_CONNECT=1, SN_DISCONNECT, SN_CONFIRM_CONNECTION, SN_CONFIRM_DECONNECTION} SN_CONNECT_TYPE;
    typedef enum {SN_CODE_VOID, SN_CODE_OK, SN_CODE_ALREADY_CONNECTED} SN_CODE_TYPE;
    typedef enum {SN_RET_OK,SN_RET_NOT_READY,SN_RET_FAIL} SN_RET_TYPE;

    const char SN_CONNECT_STRING[][30] = {"SN_VOID","Request connect","Request disconnect","Confirm connect", "Confirm disconnect"};
    const char SN_CODE_STRING[][30] = {"SN_CODE_VOID","ok","node already connected"};

    // Used to store all what is needed for state prediction and update (used by PurgeFifo)
    template<typename PARAMS>
    class StoreiI
    {
    public:
      InfoContainer   m_info_cntr;
      PARAMS       m_params;

      StoreiI(unsigned short size) : m_info_cntr(size) {}
      ~StoreiI(){}
    };


    class SensorNodeBase
    {
    protected:
      typedef std::list<ChannelFilterBase *>   ChanFiltersBuf;
      static unsigned short  m_static_id;      // incremented each time a SensorNodeIPC is instancied

      unsigned short            m_id;                // node's id (must be unique)
      unsigned short            m_sv_size;           // size of the state vector (redundant but handy, avoid calling .size() each time)
      PredictionEngineBase      *m_pred_engine;      // contains the local information state and a prediction model
      PredictModelFactoryBase   *m_pred_factory;     // prediction model generator (used for both the local filter and channel filters)
      CommFactoryBase           *m_comm_factory;     // communication object generator (used for channel filter creation)
      ChanFiltersBuf            m_chan_filters;      // array containing the channel filters
      time                      m_next_sync_horizon; // the channel filters sync will occur at this time
      time                      m_time_orig;         // for debug only (time origin)
      time                      m_sync_period;       // the interval of time between two channel filters synchronization
      time                      m_last_sync;         // time of the last synchronization
      time                      m_time_eps;          // epsilon for merging info with similar timestamps
      bool                      m_just_sync;         // tells if the sensor node has just synchronized
      bool                      m_log_chan;          // activate/de-activate channel filter log
      mutable boost::mutex      m_chan_mutex;        // channel filters can be accessed asynchronoulsy (creation and synchronizatioin)
      mutable boost::try_mutex  m_local_mutex;       // for the local filter (asynchronous access: measurements and channel filters)
      mutable boost::mutex      m_next_horiz_mutex;  // for m_next_sync_horizon
   
    public:
      SensorNodeBase(unsigned short sv_size, unsigned short id, time const& sync_period, PredictModelFactoryBase  *pred_factory, \
                  CommFactoryBase *comm_factory, bool delayed_data);
   
      virtual ~SensorNodeBase();

      bool IsJustSync() const { return m_just_sync; }
      void SetNotSync() { m_just_sync = false; }
      time GetLastSyncTime() const { return m_last_sync; }
      void SetLastSyncTime(time const& last) { m_last_sync = last; }
      void SetEpsilon(time const& epsilon) { m_time_eps = epsilon; }
      time GetEpsilon() const { return m_time_eps; }

      // Set/Get functions
      void SetNow(time const& t_orig);
      time const& GetNow() const { return m_time_orig; }
      unsigned short GetID() { return m_id; }
      unsigned short GetSvSize() { return m_sv_size; }
      time const& GetSyncPeriod() { return m_sync_period; }
      void SetLogChanFilters(bool log) { m_log_chan = log; }
      bool GetLogChanFilters() { return m_log_chan; }

      void SetInitWithState(VEC const& x, MSYM const& X) { m_pred_engine->InitState(x, X); }

      void SetInitWithInfo(VEC const& y, MSYM const& Y) { m_pred_engine->InitInformation(y, Y); }
      void SetNextHorizon(time const& horizon) { boost::mutex::scoped_lock lock(m_next_horiz_mutex); m_next_sync_horizon = horizon; }
      time const& GetNextHorizon() { boost::mutex::scoped_lock lock(m_next_horiz_mutex); return m_next_sync_horizon; }

      time const& GetLocalInfoTime() const { return m_pred_engine->GetCurrentStateTime(); }

      void SetLocalInfoTime(time const& t);
      InfoContainer const& GetLocalInfo() const { return m_pred_engine->GetPrediction(); }
      InfoContainer const& GetLocalState() const { return m_pred_engine->GetInversePrediction(); }

      void PrintPredModel();
   
      bool IsNodeConnected(unsigned short other_node);
   
      virtual SN_CODE_TYPE  RequestConnection(unsigned short node_id)=0;

      virtual void SetupChanMgrComm()=0;
      virtual void Listen()=0;
      virtual void CleanChanMgrComm()=0;

      virtual void PurgeFifo()=0;

      void ExecSyncThreadFunc(time const& horizon, time const& elapsed, int call_nr);

      CommBase & CreateChannelFilter(unsigned short link_with);
      void CreateChannelFilter(key_t chan_key, unsigned short link_with);


      //private:
      void UpdateWithChannelFilters(time const& horizon);

    protected:
      ChannelFilterBase   * GetChanFltbyNodeID(unsigned short node_id);
    };

    template<typename PARAMS>
    class SensorNodeGeneric : public SensorNodeBase
    {
      typedef typename std::deque<StoreiI<PARAMS> >  StoreiIBuffer;
   
      StoreiIBuffer         m_iI_fifo;   // store iI in case m_local_mutex is not available immediately, avoid blocking in FeedWithSensorData
      mutable boost::mutex  m_fifo_mutex;

    public:
      SensorNodeGeneric(unsigned short sv_size, unsigned short id, time const& sync_period, \
                    PredictModelFactoryBase *pred_factory, CommFactoryBase *comm_factory, bool delayed_data)
        : SensorNodeBase(sv_size, id, sync_period, pred_factory, comm_factory, delayed_data) { }
   
      virtual ~SensorNodeGeneric() { }

      void SetHistoryLength(time const& hlength) { (dynamic_cast<PredictionEngineGeneric<PARAMS> * >(m_pred_engine))->SetHistoBufLength(hlength);}
      time GetHistoryLength() { return (dynamic_cast<PredictionEngineGeneric<PARAMS> * >(m_pred_engine))->GetHistoryLength(); }
      void ResizeHistory() { (dynamic_cast<PredictionEngineGeneric<PARAMS> * >(m_pred_engine))->ResizeHistory(); }
      bool GetHistoryClosestInfo(time const&t, InfoContainer &iCont) {
        return (dynamic_cast<PredictionEngineGeneric<PARAMS> * >(m_pred_engine))->GetHistoryClosestInfo(t, iCont);
      }

      void FeedWithSensorData(InfoContainer const& iI, PARAMS & params)
      {
        try // to get the mutex
        {
          boost::try_mutex::scoped_try_lock  lock(m_local_mutex);

          if(IsJustSync()) // modify h because a sync just occured
          {
            time local_time = GetLocalInfoTime();
            if (iI.GetTime() > local_time)   params.h = iI.GetTime() - local_time;
            // else this is a delayed data -> will be fused in PurgeFifo
            SetNotSync();
          }
          PushInFifo(iI, params);
          PurgeFifo();
        }
        catch(boost::lock_error)
        {
          JFR_VDEBUG("Can't get mutex!\n");
          PushInFifo(iI, params);                  // then store info, process later
        }
      }
   
      // local filter mutex must be locked GetNextHorizon won't change during the execution of the procedure
      void PurgeFifo()
      {
        typename StoreiIBuffer::iterator it;
        std::ostringstream stream;
        PredictionEngineGeneric<PARAMS> *pPred;
        time   obs_time, local_time, next_horiz;
      
        boost::mutex::scoped_lock   lockfifo(m_fifo_mutex);
        if(m_iI_fifo.empty()) return;

        pPred = dynamic_cast< PredictionEngineGeneric<PARAMS> * >(m_pred_engine);
        next_horiz = GetNextHorizon();
      
        // loop until either the buffer is empty or the observation timestamp > next_horiz
        for(it = m_iI_fifo.begin(); it != m_iI_fifo.end(); it++)
        {
          obs_time = (*it).m_info_cntr.GetTime();     // observation time, sensed time
          local_time = pPred->GetCurrentStateTime();

          JFR_VDEBUG("obs time: " << obs_time << " next_horiz: " << next_horiz << " m_last_sync:" << m_last_sync);

          if(obs_time <= m_last_sync && m_last_sync != time::infty())//next_horiz)
          {
            stream << "* Delayed data -> obs_time < m_last_sync " << obs_time <<  " < " << m_last_sync;
            
            if (!pPred->InsertDelayedMeasure((*it).m_info_cntr, local_time))
              stream << " ... Pending";
            else
              stream << " ... Done";

            JFR_DEBUG(stream.str());
            m_iI_fifo.pop_front();
          }
          else if(obs_time.check_close(local_time, m_time_eps))
          {
            JFR_DEBUG("~= local time");
            if(local_time > obs_time)
              (*it).m_info_cntr.SetTime(local_time);
            else
              pPred->SetCurrentStateTime(obs_time, false);

            // Update filter
            pPred->UpdatePredictionWith((*it).m_info_cntr);
            m_iI_fifo.pop_front();
          }
          else
            if(obs_time.check_close(next_horiz, m_time_eps))
            {
              JFR_DEBUG("~= next_horizon");
              // if the observation time is close to the next time horizon then we consider the same time
              (*it).m_info_cntr.SetTime(next_horiz);

              // predict the current state to the next horizon
              JFR_PRECOND(pPred->GetCurrentStateTime() <= next_horiz, "bad horizon");
              pPred->SetModelParams(pPred->GetCurrentStateTime(), next_horiz);
              pPred->Predict(next_horiz);

              // update
              pPred->UpdatePredictionWith((*it).m_info_cntr);
              m_iI_fifo.pop_front();
            }
            else if(obs_time != next_horiz)
            {
              // normal predict/update step
              JFR_VDEBUG("Normal fusion");
              pPred->UpdateModel((*it).m_params);

              try
              {
                pPred->Predict(obs_time);
              }
              catch(Bayesian_filter::Numeric_exception)
              {
                JFR_DEBUG("--- not PD node " << GetID() << "\n");
                throw;
              }
              pPred->UpdatePredictionWith((*it).m_info_cntr);
              m_iI_fifo.pop_front();
            }
            else if(obs_time > next_horiz)
            {
              // this data will be fused later
              JFR_DEBUG("obs_time > next_horiz");
              return;
            }
            else
            {
              JFR_WARNING("Case not supported\n");
              exit(EXIT_FAILURE);
            }
        }
      }

      void PushInFifo(InfoContainer const& iI, PARAMS & params)
      {
        boost::mutex::scoped_lock   lockfifo(m_fifo_mutex);
        StoreiI<PARAMS> st_iI(GetSvSize());
      
        st_iI.m_info_cntr = iI;
        st_iI.m_params = params;
        m_iI_fifo.push_back(st_iI);
      }
    };


  } // namespace ddf
} // namespace jafar
#endif