//
// Created by erki on 12/11/22.
//

#include <catch2/catch.hpp>

#include "utility_filters.hpp"

TEST_CASE("LowPassFilter works as expected.")
{
  const int threshold = 50;
  Utility::LowPassFilter<int> filter{threshold};

  SECTION("Initial value is 0.")
  {
    REQUIRE(filter.currentValue() == 0);
  }

  SECTION("Passing a number higher than threshold gets filtered.")
  {
    filter.update(60);
    REQUIRE(filter.currentValue() == 0);

    SECTION("Filter updates state as expected.")
    {
      filter.update(40);
      REQUIRE(filter.currentValue() == 40);
    }
  }

  SECTION("Passing a number lower than threshold passes filter.")
  {
    filter.update(40);
    REQUIRE(filter.currentValue() == 40);

    SECTION("Filter retains state as expected.")
    {
      filter.update(60);
      REQUIRE(filter.currentValue() == 40);
    }
  }
}

TEST_CASE("HighPassFilter works as expected.")
{
  const int threshold = 50;
  Utility::HighPassFilter<int> filter{threshold};

  SECTION("Initial value is 0.")
  {
    REQUIRE(filter.currentValue() == 0);
  }

  SECTION("Passing a number higher than threshold passes filter.")
  {
    filter.update(60);
    REQUIRE(filter.currentValue() == 60);

    SECTION("Filter retains state as expected.")
    {
      filter.update(40);
      REQUIRE(filter.currentValue() == 60);
    }
  }

  SECTION("Passing a number lower than threshold gets filtered.")
  {
    filter.update(40);
    REQUIRE(filter.currentValue() == 0);

    SECTION("Filter updates state as expected.")
    {
      filter.update(60);
      REQUIRE(filter.currentValue() == 60);
    }
  }
}

TEST_CASE("Two filters can be linked together.")
{
  const int threshold_low = 40;
  const int threshold_high = 50;

  Utility::LowPassFilter<int> low_pass{threshold_high};
  Utility::HighPassFilter<int> high_pass{threshold_low};

  high_pass.assignPrecedingFilter(low_pass);

  SECTION("Initial value is 0.")
  {
    REQUIRE(high_pass.currentValue() == 0);
  }

  SECTION("Values outside of band are filtered out.")
  {
    high_pass.update(30);
    REQUIRE(high_pass.currentValue() == 0);

    high_pass.update(60);
    REQUIRE(high_pass.currentValue() == 0);

    SECTION("Filter updates with in-band value.")
    {
      high_pass.update(45);
      REQUIRE(high_pass.currentValue() == 45);
    }
  }

  SECTION("Values inside of band are passed through.")
  {
    high_pass.update(44);
    REQUIRE(high_pass.currentValue() == 44);

    high_pass.update(46);
    REQUIRE(high_pass.currentValue() == 46);

    SECTION("Out of band values are filtered out.")
    {
      high_pass.update(30);
      REQUIRE(high_pass.currentValue() == 46);

      high_pass.update(60);
      REQUIRE(high_pass.currentValue() == 46);
    }
  }

  SECTION("Decoupling filters works as expected.")
  {
    high_pass.update(60);
    REQUIRE(high_pass.currentValue() == 0);

    high_pass.clearPrecedingFilter();
    high_pass.update(60);
    REQUIRE(high_pass.currentValue() == 60);

    high_pass.update(30);
    REQUIRE(high_pass.currentValue() == 60);
  }
}

TEST_CASE("Median filter works as expected.")
{
  Utility::MedianFilter<int, 5> filter;

  SECTION("Initial value is 0.")
  {
    REQUIRE(filter.currentValue() == 0);
  }

  SECTION("The median is filtered appropriately.")
  {
    std::array<int, 5> data = {5, 1, 4, 3, 2};
    std::array<int, 5> expected_median = {0, 0, 1, 3, 3};
    for (int i = 0; i < 5; i++)
    {
      const int x = data[i];
      filter.update(x);
      REQUIRE(filter.currentValue() == expected_median[i]);
    }
  }
}

TEST_CASE("Rolling average filter works as expected.")
{
  Utility::RollingAverageFilter<int> filter{5};

  SECTION("Initial value is 0.")
  {
    REQUIRE(filter.currentValue() == 0);
  }

  SECTION("Value below step size is filtered out for int filter.")
  {
    filter.update(4);
    REQUIRE(filter.currentValue() == 0);
  }

  SECTION("Value is updated appropriately.")
  {
    filter.update(20);
    REQUIRE(filter.currentValue() == 20 / 5);

    SECTION("Saturation is reached appropriately")
    {
      for (int i = 0; i < 5 * 2 - 1; i++)
        filter.update(20);

      REQUIRE(filter.currentValue() == 20);
    }
  }
}