predictenginebase.hpp

#ifndef DDF_PREDICTENGINEBASE_HPP
#define DDF_PREDICTENGINEBASE_HPP

#include <BayesFilter/bayesFlt.hpp>
#include <deque>
#include "infocontainer.hpp"
#include "definitions.hpp"
#include "tools.hpp"
#include "debugging.hpp"

namespace jafar
{
  namespace ddf
  {

    typedef enum{FS_UPDATE, FS_PREDICT} FS_STORE_TYPE;

    template<typename PARAMS>
    class FSStorageBase
    {
    private:
      FS_STORE_TYPE m_fs_type;  // tells if the storage occurs after a prediction or an update
      
    public:
      InfoContainer m_MkIik;  // store the increment MkI and ik only
      PARAMS    m_params;

    public:
      FSStorageBase(unsigned short sv_size, FS_STORE_TYPE st_type, PARAMS const& params): m_fs_type(st_type),m_MkIik(sv_size){ m_params = params; }
      FSStorageBase(unsigned short sv_size, FS_STORE_TYPE st_type): m_fs_type(st_type),m_MkIik(sv_size){ }
      FSStorageBase(FSStorageBase const& other): m_MkIik(other.GetSize()) {
        if (this == &other) return;
        this->m_MkIik     = other.m_MkIik;
        this->m_fs_type   = other.m_fs_type;
        this->m_params  = other.m_params;
      }

      FS_STORE_TYPE GetType() const { return m_fs_type; }

      unsigned short GetSize() const { return m_MkIik.GetSize(); }
      time GetTime() const { return m_MkIik.GetTime(); }
      bool IsStoreUpdate() { return m_fs_type == FS_UPDATE; }
      bool IsStorePredict() { return m_fs_type == FS_PREDICT; }
      void Print() const {
        JFR_VDEBUG("Time: " << m_MkIik.m_time << " Type: " << (m_fs_type == FS_UPDATE ? "update" : "predict") << "\n");
        if(m_fs_type == FS_PREDICT)
        {
          PrettyPrintMatrix(m_MkIik.m_InfoMat, "Yk");
          PrettyPrintVector(m_MkIik.m_info,"yk");
        }
        else  {
          PrettyPrintMatrix(m_MkIik.m_InfoMat, "Sum I");
          PrettyPrintVector(m_MkIik.m_info,"Sum i");
        }
      }
    };

    class PredictionEngineBase
    {
    protected:
   
      bool    m_predict_available;  // it is set to true in the Predict function and false when the filter is updated
      time    m_horizon;      // absolute time associated to prediction (time corresponding to step k+1)
      time    m_current_time;   // time stamp of the current state (after an update/prediction has been done)
      mutable boost::mutex  m_info_mutex; // protect asynchronous access to members
      bool          m_store_fstate;

    public:
   
      PredictionEngineBase(bool store_fstate = false): m_horizon(0,0),m_current_time(0,0),m_store_fstate(store_fstate) {}
      virtual ~PredictionEngineBase() {}

      void StoreStates(bool yesorno) { m_store_fstate = yesorno; }

      void SetCurrentStateTime(time const& t, bool use_mutex) { if (use_mutex) {boost::mutex::scoped_lock lock(m_info_mutex); m_current_time = t;}
        else m_current_time = t;}
      time const& GetCurrentStateTime() const { boost::mutex::scoped_lock lock(m_info_mutex); return m_current_time; }

      virtual void InitState(VEC const& x, MSYM const& X)=0;

      virtual void InitInformation(VEC const& y, MSYM const& Y)=0;
      virtual void Init(InfoContainer const& inityY)=0;

      virtual void Predict(time const& horizon)=0;

      virtual bool InsertDelayedMeasure(InfoContainer const& ICont, time const& avail_time)=0;

      void Predict(InfoContainer &info, time const& horizon)
      {
        Init(info);
        Predict(horizon);
        info = GetPrediction();
      }

      virtual InfoContainer const& GetPrediction()=0;

      virtual InfoContainer const& GetInversePrediction()=0;

      virtual void SetModelParams(time const& from, time const& horizon)=0;
   
      virtual void PrintModel()=0;

      virtual void UpdatePredictionWith(InfoContainer const& info)=0;

      virtual bool IsPSD(bool use_mutex)=0;
    };


    template<typename PARAMS>
    class PredictionEngineGeneric : public PredictionEngineBase
    {
   
    protected:
   
      typedef std::deque<FSStorageBase<PARAMS> >  FSStorageBuf;
      PARAMS    m_params;     // the set of parameters needed for updating the model (typically a structure)
      bool      m_model_need_update;  // if the parameters change, then the model need to be updated
      time      m_histo_buf_length; // the range of the history buffer
      FSStorageBuf  m_fs_storage;   // history of the filter (for delayed measurement integration)
   
    public:

      PredictionEngineGeneric(): m_model_need_update(true), m_histo_buf_length(3,0) {}
      virtual ~PredictionEngineGeneric() {}

      virtual void UpdateModel()=0;
      
      void UpdateModel(PARAMS const& params) {
        m_params = params;
        m_model_need_update = true;
        UpdateModel();
      }

      void SetHistoBufLength(time const& length) { m_histo_buf_length = length; }
      time GetHistoBufLength() const { return m_histo_buf_length; }
      void ResizeHistory()
      {
        typename FSStorageBuf::iterator  it_beg;
        typename FSStorageBuf::reverse_iterator it_back;
        time  limit;

        if (m_fs_storage.empty()) return;

        it_beg  = m_fs_storage.begin();
        it_back = m_fs_storage.rbegin();
       
        if ((*it_back).GetTime() - (*it_beg).GetTime() < m_histo_buf_length) return;

        limit = (*it_back).GetTime() - m_histo_buf_length;
        while ((*it_beg).GetTime() < limit) {
          m_fs_storage.pop_front();
          it_beg++;
        }
      }

      time GetHistoryLength() {
        typename FSStorageBuf::iterator  it_beg;    //, it_back got an error with it_back = m_fs_storage.back();
        typename FSStorageBuf::reverse_iterator it_back;

        if (m_fs_storage.empty())
          return time::null();

        it_beg = m_fs_storage.begin();
        it_back = m_fs_storage.rbegin();
        return (*it_back).GetTime() - (*it_beg).GetTime();
      }

      bool GetHistoryClosestInfo(time const&t, InfoContainer & iCont)
      {
        typename FSStorageBuf::iterator  it;

        if (m_fs_storage.empty()) {
          JFR_VDEBUG("empty " << " t: " << t << "\n");
          return false;
        }
        it  = m_fs_storage.begin();
       
        if ((*it).GetTime() >= t) {
          iCont = (*it).m_MkIik;
      }
        else if (m_fs_storage.back().GetTime() <= t) {
          iCont = m_fs_storage.back().m_MkIik;
        }
        else
        {
          while ((*it).GetTime() < t) it++;
      
          it--;
          iCont = (*it).m_MkIik;
        }
        JFR_VDEBUG("iCont time: " << iCont.GetTime() << " t: " << t << "\n");

        return true;
      }
    };

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