/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "graphslam-precomp.h"  // Precompiled headers
//
// Implementattion file for TSlidingWindow struct
#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/graphslam/misc/TSlidingWindow.h>
#include <mrpt/serialization/CArchive.h>

#include <algorithm>
#include <numeric>

using namespace mrpt::graphslam;

TSlidingWindow::TSlidingWindow(const std::string& name /* = "window" */) : m_name(name) {}

double TSlidingWindow::getMedian()
{
  MRPT_START

  double median_out = 0.0;
  if (m_measurements_vec.empty())
  {
    return 0.0;
  }

  if (m_median_updated)
  {
    median_out = m_median_cached;
  }
  else
  {
    // copy the current vector, sort it and return value in middle
    std::vector<double> vec_sorted(m_measurements_vec);
    std::sort(vec_sorted.begin(), vec_sorted.end());

    median_out = vec_sorted.at(vec_sorted.size() / 2);

    m_median_cached = median_out;
    m_median_updated = true;
  }

  return median_out;

  MRPT_END
}
double TSlidingWindow::getMean()
{
  MRPT_START

  double mean_out = 0.0;

  if (m_mean_updated)
  {
    mean_out = m_mean_cached;
  }
  else
  {
    mean_out = std::accumulate(m_measurements_vec.begin(), m_measurements_vec.end(), 0.0);
    mean_out /= m_measurements_vec.size();

    m_mean_cached = mean_out;
    m_mean_updated = true;
  }

  return mean_out;

  MRPT_END
}
double TSlidingWindow::getStdDev()
{
  MRPT_START

  double std_dev_out = 0.0;

  if (m_std_dev_updated)
  {  // return the cached version?
    std_dev_out = m_std_dev_cached;
  }
  else
  {
    double mean = this->getMean();

    double sum_of_sq_diffs = 0;
    for (auto it = m_measurements_vec.begin(); it != m_measurements_vec.end(); ++it)
    {
      sum_of_sq_diffs += std::pow(*it - mean, 2);
    }
    std_dev_out = sqrt(sum_of_sq_diffs / m_win_size);

    m_std_dev_cached = std_dev_out;
    m_std_dev_updated = true;
  }

  return std_dev_out;
  MRPT_END
}

bool TSlidingWindow::evaluateMeasurementInGaussian(double measurement)
{
  // get the boundaries for acceptance of measurements - [-3sigma, 3sigma]
  // with
  // regards to the mean
  double low_lim = this->getMean() - 3 * this->getStdDev();
  double upper_lim = this->getMean() + 3 * this->getStdDev();

  return measurement > low_lim && measurement < upper_lim;
}
bool TSlidingWindow::evaluateMeasurementAbove(double value)
{
  MRPT_START

  double threshold = this->getMean();
  return (value > threshold);

  MRPT_END
}
bool TSlidingWindow::evaluateMeasurementBelow(double value)
{
  return !evaluateMeasurementAbove(value);
}

void TSlidingWindow::addNewMeasurement(double measurement)
{
  MRPT_START

  m_is_initialized = true;

  // if I haven't already filled up to win_size the vector, just add it
  if (m_win_size > m_measurements_vec.size())
  {
    m_measurements_vec.push_back(measurement);
  }
  else
  {
    // remove first element - add it as last element
    m_measurements_vec.erase(m_measurements_vec.begin());
    m_measurements_vec.push_back(measurement);
  }

  m_mean_updated = false;
  m_median_updated = false;
  m_std_dev_updated = false;

  MRPT_END
}
void TSlidingWindow::resizeWindow(size_t new_size)
{
  MRPT_START

  size_t curr_size = m_measurements_vec.size();
  if (new_size < curr_size)
  {
    // remove (curr_size - new_size) elements from the beginning of the
    // measurements vector
    m_measurements_vec.erase(
        m_measurements_vec.begin(), m_measurements_vec.begin() + (curr_size - new_size));

    m_mean_updated = false;
    m_median_updated = false;
  }

  m_win_size = new_size;

  MRPT_END
}
void TSlidingWindow::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
  MRPT_START

  size_t sliding_win_size = source.read_int(section, "sliding_win_size", 10, false);
  this->resizeWindow(sliding_win_size);

  MRPT_END
}
void TSlidingWindow::dumpToTextStream(std::ostream& out) const
{
  MRPT_START

  out << mrpt::format("-----------[ %s: Sliding Window Properties ]-----------\n", m_name.c_str());
  out << "Measurements Vector: \n";
  for (double it : m_measurements_vec)
  {
    out << mrpt::format("\t%.2f\n", it);
  }
  out << "\n";

  out << mrpt::format("m_name              : %s\n", m_name.c_str());
  out << mrpt::format("m_mean_cached       : %.2f\n", m_mean_cached);
  out << mrpt::format("m_median_cached     : %.2f\n", m_median_cached);
  out << mrpt::format("m_std_dev_cached    : %.2f\n", m_std_dev_cached);
  out << mrpt::format("m_mean_updated      : %s\n", m_mean_updated ? "TRUE" : "FALSE");
  out << mrpt::format("m_median_updated    : %s\n", m_median_updated ? "TRUE" : "FALSE");
  out << mrpt::format("m_std_dev_updated   : %s\n", m_std_dev_updated ? "TRUE" : "FALSE");
  out << mrpt::format("m_win_size          : %lu\n", m_win_size);
  out << mrpt::format("m_is_initialized    : %s\n", m_is_initialized ? "TRUE" : "FALSE");

  MRPT_END
}

size_t TSlidingWindow::getWindowSize() const { return m_win_size; }
bool TSlidingWindow::windowIsFull() const { return (m_win_size == m_measurements_vec.size()); }
