//
// PID regulation tools
//
// Copyright 2026, Jakob Østergaard Hegelund <joe@unthought.net>
// -----------------------------------------------------------------------
// This file is part of FanMan.
//
// FanMan is free software: you can redistribute it and/or modify it
// under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// FanMan is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FanMan. If not, see <https://www.gnu.org/licenses/>.
// -----------------------------------------------------------------------
//
#ifndef _PID_HH
#define _PID_HH

#include <deque>
#include <numeric>

//
// Store a history of process outputs in a signed integer format. All
// getters return double precision floating point
//
template <typename B = int32_t>
class ProcessHistory {
public:
  ProcessHistory(size_t samples_per_second,
                 size_t samples_in_history)
    : m_sps(samples_per_second)
    , m_sih(samples_in_history)
  {
    if (samples_per_second >= samples_in_history)
      throw std::runtime_error("Must have more than a second of samples in history");
  }

  // Add new element
  void add(B e) {
    if (m_history.empty()) {
      m_history.resize(m_sih, e);
      m_sum = e * m_sih;
    } else {
      m_sum -= m_history.back();
      m_history.pop_back();
      m_history.push_front(e);
      m_sum += e;
    }
  }

  // Get most recent half second median, in an effort to filter out
  // the worst of the noise
  double get() {
    m_window.clear();
    for (size_t e = 0; e != m_sps/2; ++e)
      m_window.push_back(m_history[e]);
    std::sort(m_window.begin(), m_window.end());
    return m_window[m_window.size()/2];
  }
  // Get integral (sum)
  double getint() {
    return m_sum / double(m_sps);
  }
  // Get filtered differential; we use a Savitzky-Golay filter to get
  // the first derivative
  double getdiffSG5() {
    return 1. / (12. / m_sps)
      * double(m_history[4]
               - 8 * m_history[3]
               + 8 * m_history[1]
               - m_history[0]);
  }
  double getdiffSG7() {
    return 1. / (252. / m_sps)
      * double(22 * m_history[6]
               - 67 * m_history[5]
               - 58 * m_history[4]
               + 58 * m_history[2]
               + 67 * m_history[1]
               - 22 * m_history[0]);
  }
  double getdiffSG9() {
    return 1. / (1188. / m_sps)
      * double(86 * m_history[8]
               - 142 * m_history[7]
               - 193 * m_history[6]
               - 126 * m_history[5]
               + 126 * m_history[3]
               + 193 * m_history[2]
               + 142 * m_history[1]
               - 86 * m_history[0]);
  }

private:
  // Our constants from construction
  const size_t m_sps;
  const size_t m_sih;
  // Historical output
  std::deque<B> m_history;
  // Sum of history
  B m_sum;
  // Work buffer for medians
  std::vector<B> m_window;
};

#endif
