Statistic.h

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material without having a written signed agreement with Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
 
#ifndef MOMENTUM_STATISTIC_H
#define MOMENTUM_STATISTIC_H
 
#include "momentum_export.h"
#include "momentum_namespace.h"
#include <agx/Vector.h>
 
namespace v1
{
  class MOMENTUM_EXPORT Statistic
  {
  public:
    virtual double update(double observation) = 0;
 
    virtual double get() const = 0;
 
    virtual ~Statistic() {}
  };
 
 
  class MOMENTUM_EXPORT ExponentialMovingAverageStatistic : public Statistic
  {
  public:
    ExponentialMovingAverageStatistic(double smoothFactor, double initialObservation = 0)
      : m_smoothFactor(smoothFactor), m_s(initialObservation) {}
 
    virtual inline double update(double observation)
    {
      m_s += m_smoothFactor * (observation - m_s);
      return m_s;
    }
 
    void set(double val) { m_s = val; }
 
    inline double get() const { return m_s; }
 
    inline double getSmoothFactor() const { return m_smoothFactor; }
 
    inline void setSmoothFactor(double smoothFactor) { m_smoothFactor = smoothFactor; }
 
    virtual ~ExponentialMovingAverageStatistic() {}
 
  protected:
    double m_smoothFactor;
    double m_s;
  };
 
  class MOMENTUM_EXPORT MedianStatistic : public Statistic
  {
  public:
    MedianStatistic(size_t maxHistorySize)
      : m_maxHistorySize(maxHistorySize)
    {
      m_historyByAge.reserve(m_maxHistorySize);
      m_historyByValue.reserve(m_maxHistorySize);
    }
 
    virtual double get() const override
    {
      size_t n = this->getCurrentHistorySize();
 
      if (n == 0) {
        return double(0.0);
      }
 
      size_t middle = n / 2;
      if (n % 2 == 0) {
        // Two in the middle, return their average.
        return (m_historyByValue[middle - 1] + m_historyByValue[middle]) / double(2.0);
      }
      else {
        return m_historyByValue[middle];
      }
    }
 
    virtual double update(double observation) override
    {
      // Remove old values.
      while (this->getCurrentHistorySize() >= this->getMaxHistorySize()) {
        double v = m_historyByAge.back();
        m_historyByAge.pop_back();
        m_historyByValue.findAndErase(v, false);
      }
 
      // Insert new value.
      using namespace std;
      auto insertAt = lower_bound(begin(m_historyByValue), end(m_historyByValue), observation);
      m_historyByValue.insert(insertAt, observation);
      m_historyByAge.insert(size_t(0), observation);
 
      return this->get();
    }
 
    void clear()
    {
      m_historyByAge.clear();
      m_historyByValue.clear();
    }
 
 
    size_t getMaxHistorySize() const
    {
      return m_maxHistorySize;
    }
 
    size_t getCurrentHistorySize() const
    {
      agxAssert(m_historyByAge.size() == m_historyByValue.size());
      size_t n = m_historyByAge.size();
      return n;
    }
 
  private:
    size_t m_maxHistorySize;
    agx::RealVector m_historyByAge;
    agx::RealVector m_historyByValue;
  };
 
}
#endif