The Sensor Filter Problem

main.cpp

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <memory>
#include <chrono>

/*
 * @brief Packet struct is the main struct that contains the data to send to the filter
 * @warn DO NOT EDIT THIS STRUCT IT IS THE BACKBONE OF THE SYSTEM
 */
struct Packet{
  // Current time of the system in a uint64 format
  uint64_t time;
  // is_valid tells us if the packet data is valid. If this is false we should disregard this packet
  bool is_valid;
  // The data that the packet contains
  uint16_t data;
};

/*
 * @brief The IIR Filter is a simple First Order IIR filter to process data coming out of the sensor
 *
 * It implements the formula described in the following article 
 * https://en.wikipedia.org/wiki/Infinite_impulse_response
 *
 * 
 * We are going to be implementing a first order filter using the following equation:
 *     y[n]= (b0 * x[n]) + (b1 * x[n−1]) − (a1 * y[n−1])
 *
 * @param a1 -> feedback filter coefficient from the last output.
 * @param b0 -> feedforward filter coefficient from the current input.
 * @param b1 -> feedforward filter coefficient from the previous input.
 */ 
class IIRFilter
{
public:
  explicit IIRFilter(const float a1, const float b0, const float b1):
    a1{a1},
    b0{b0},
    b1{b1}
  {
  }

  /*
   * @brief update the filter with this value and save last timestamp
   * @param x The data to add to the filter
   * @warn This function wants you to implement ONLY the filter equation:
   *           y[n]= (b0 * x[n]) + (b1 * x[n−1]) − (a1 * y[n−1])
   *       DO NOT IMPLEMENT THE AVERAGING DESCRIBED IN THE EXAMPLE. ONLY THE FILTER EQUATION
   * @warn THIS FUNCTION IS USED IN TEST SUITE: IIR Filter 
   *       Feel free to change the internals but try to keep the method signature the same
   * @note After implementing this, you should pass the first set of tests. Also known as the IIR
   *       filter tests.
   */
  void update(float x)
  {
    output_buffer = (b0 * x) + (b1 * input_buffer) - (a1 * output_buffer);
    x = input_buffer;
  }

  /*
   * @brief gets the current value of the filter
   */
  float getCurrentValue() const
  {
    return output_buffer;
  }

private:
  // Buffer to store previous output values
  /// Current value of the filter
  float output_buffer;
  // Buffer to store previous input values
  float input_buffer;
  /// Feedback param from the previous output
  float a1;
  /// Feedforward for the current input
  float b0;
  /// Feedforward for the previous input
  float b1;
};


/*
 * @brief Reads all the packets in a packet vector and gets a series of averaged packets from the averaging algorithm outlined below
   * For every packet this method does the following steps:
   * 1. Forward simulates time until the latest packet time matches current simulated time.
   *    If the current timestep is over one period, gather the running average and add it to the output vector
   * 2. Processes the latest packet to merge the data with other packets in this period using a running average
   *         current_avg = current_avg × n + new_data
   *                       --------------------------
   *                              (n + 1)
   *    where n is the number of packets in the average so far, and new_data is the data in the packet.
   * 3. If the packet arrived at the averaging period timestep, add the current average to the output vector
   * 4. At the end of parsing all the packets, simulate till the end of the next period (i.e. add the running average left over)
 * @warn THIS FUNCTION IS USED IN TEST SUITE: Averaging Function
 *       Feel free to change the internals but try to keep the method signature the same
 * @param packets: A vector of packets. These Packets will be read one by one
 * @param period: The period over which the data will be averaged for eg. period = 2 will be averaged at 2, 4, 6 .. fuck you
  * @example for data packets={1, !is_valid, 3, 4}, period=2 output should be {1, 3.5}
 *                   (during the first period 1 is the only running average)
 * @example for data packets={1, !is_valid, 3, 4, 5}, period=2 output should be {1, 3.5, 5}
 *                   (during the last period 5 is treated alone)
 * @example for data packets={!is_valid, !is_valid, 3, 4, 5}, period=2 output should be {0, 3.5, 5}
 *                   (as no data is available in the first period i.e. 0)

 *                period = 5 will be averaged at 5, 10, 15, 20 ....
 * @return A vector of floats. These floats represent the averaged data that need to be sent to the filter
 * @note The output data is NOT filtered at this point
 */
std::vector<float> getAveragedPacketData(const std::vector<Packet>& packets, const int period)
{
  std::vector<float> averaged_packet_data{};
  uint64_t current_timestamp = 0;
  int16_t merged_data = 0;
  uint64_t no_of_merging_packets = 0;
  for (const auto& packet: packets)
  {
    if (!packet.is_valid)
    {
      continue;
    }


    while (current_timestamp < packet.time)
    {
      current_timestamp++;
      if ((current_timestamp % period) == 0)
      {
        averaged_packet_data.push_back(merged_data);
        merged_data = 0;
        no_of_merging_packets = 0;
      }

    }

    merged_data = (merged_data * (no_of_merging_packets-1) + static_cast<float>(packet.data)) / (no_of_merging_packets);
    no_of_merging_packets++;

  }
  if (no_of_merging_packets != 0)
  {
    averaged_packet_data.push_back(merged_data);
  }
  return averaged_packet_data;
}

/*
 * @brief Reads all the packets in a packet vector and gets a series of filtered values.
 *
 * @param filter: The IIR Filter we want to use.
 * @param period: This is the period of averaging we want to run the sensor over
 * @param packets: All the packets we would like to parse. The packets must be sent in forward order of the timestamp 
 *
 * @return We get the data of the filter every time the filter is updated and add them to a vector
 *
 * @note  This should return the filter value for every change in the filter, not for every incoming packet
 *        The run of the filter should be simulated from the time "0" until the last timestamp on the last VALID packet of data
 *        Packets of data are considered valid if the is_valid in them is set to true
 * 
 * @warn Packets being sent to this function are expected to be in increasing order of timestamp.
 *       This function is not expected to perform correctly if VALID packets are handed in non increasing order of timestamp 
 *
 * @warn THIS FUNCTION IS USED EXTENSIVELY IN TESTS: Read, Average & Filter
 *       Feel free to change the internals but try to keep the method signature the same
 */
std::vector<float> readAllPacketsAndGetFilteredData(IIRFilter& filter, const float period, const std::vector<Packet>& packets)
{
  std::vector<float>* filtered_values = new std::vector<float>{};
  for (const auto& data: getAveragedPacketData(packets, period))
  {
    filter.update(data);
    filtered_values->push_back(filter.getCurrentValue());
  }
  return *filtered_values;
}


/* 
 * @brief Basic test suite to test the output of the filter against expected data.
 * @note  Try not to make changes this class. This is the testing suite we are judging your code against
*/
class TestSuite
{
public:

  /*
   * @brief Simple basic test of only IIR filter
   */
  bool iir_first()
  {
    IIRFilter filter{0.1, 0.7, 0.2};
    std::vector<float> filter_output;
    for(uint16_t i=0; i<6 ;i++)
    {
      filter.update(static_cast<float>(i)*10.0);
      filter_output.push_back(filter.getCurrentValue());
    }
    std::vector<float> predicted_output{0, 7, 15.3, 23.47, 31.653, 39.8347};
    return isEqual(filter_output, predicted_output);
  }

  /*
   * @brief Does the IIR filter decrease with 0s?
   */
  bool iir_second()
  {
    IIRFilter filter{0.1, 0.7, 0.2};
    std::vector<float> filter_output;
    filter.update(10.0);
    filter_output.push_back(filter.getCurrentValue());
    for(uint16_t i = 0; i < 5; i++)
    {
      filter.update(0.0);
      filter_output.push_back(filter.getCurrentValue());
    }
    std::vector<float> predicted_output{7, 1.3, -.13, 0.013, -0.0013, 0.00013};
    return isEqual(filter_output, predicted_output);
  }

  /*
   * @brief What if I allocate using a shared_ptr
   */
  bool iir_third()
  {
    std::shared_ptr<IIRFilter> filter = std::make_shared<IIRFilter>(0.1, 0.7, 0.2);
    std::vector<float> filter_output;
    for(uint16_t i=0; i<6 ;i++)
    {
      filter->update(i*10.0);
      filter_output.push_back(filter->getCurrentValue());
    }
    std::vector<float> predicted_output{0, 7, 15.3, 23.47, 31.653, 39.8347};
    return isEqual(filter_output, predicted_output);
  }

  /*
   * @brief Simple basic test of running average algorithm
   */
  bool averaging_first()
  {
    std::vector<Packet> input;
    uint64_t current_timestamp = 0;
    for (uint16_t i = 0; i < 6; i++)
    {
      input.push_back(Packet{current_timestamp, true, static_cast<uint16_t>(i+20)});
      current_timestamp++;
    }
    std::vector<float> averaged_packets = getAveragedPacketData(input, 2);

    std::vector<float> predicted_output{20.5, 22.5, 24.5}; 
    return isEqual(averaged_packets, predicted_output);
  }

  /*
   * @brief Running average but with bad packets
   */
  bool averaging_second()
  {
    uint64_t current_timestamp = 0;
    std::vector<Packet> input;
    input.push_back(Packet{current_timestamp, true, 20});
    current_timestamp++;
    for (uint16_t i = 0; i < 6; i++)
    {
      // This data should be ignored because they are deemed not good
      input.push_back(Packet{current_timestamp, false, static_cast<uint16_t>(i+20)});
      current_timestamp++;
    }
    input.push_back(Packet{current_timestamp, true, 25});
    std::vector<float> averaged_packets = getAveragedPacketData(input, 2);
    std::vector<float> predicted_output{20, 0, 0, 25};
    return isEqual(averaged_packets, predicted_output);
  }

  /*
   * @brief Running average but skipping packets in the middle
   */
  bool averaging_third()
  {
    uint64_t current_timestamp = 0;
    std::vector<Packet> input;
    input.push_back(Packet{current_timestamp, true, 20});
    // Forwarding time by 20 steps but life is going to be easy peasy
    // My trusty algorithm will fill 0s in the remaining time
    input.push_back(Packet{current_timestamp + 20, true, 25});
    std::vector<float> averaged_packets = getAveragedPacketData(input, 5);
    std::vector<float> predicted_output{20, 0, 0, 0, 25};
    return isEqual(averaged_packets, predicted_output);
  }

  /*
   * @brief Running average with large data
   */
  bool averaging_fourth()
  {
    uint64_t current_timestamp = 0;
    std::vector<Packet> input;
    input.push_back(Packet{current_timestamp, true, 65535});
    input.push_back(Packet{current_timestamp + 1, true, 65535});
    std::vector<float> averaged_packets = getAveragedPacketData(input, 2);
    std::vector<float> predicted_output{65535};
    return isEqual(averaged_packets, predicted_output);
  }

  /*
   * @brief Simple basic test of the whole system
   */
  bool first()
  {
    IIRFilter filter{0.1, 0.7, 0.2};
    uint64_t timestamp= 1;
    std::vector<Packet> input;
    input.push_back(Packet{timestamp, true, 20});
    input.push_back(Packet{timestamp+10, true, 30});
    input.push_back(Packet{timestamp+30, true, 25});
    auto filter_output = readAllPacketsAndGetFilteredData(filter, 5, input);
    std::vector<float> predicted_output{14, 2.6, 20.74, 3.926, -.3926, 0.03926, 17.4961};
    return isEqual(filter_output, predicted_output);
  }


private:

  template<typename T>
  bool isEqual(std::vector<T> const &v1, std::vector<T> const &v2)
  {
    if (v1.size() != v2.size())
    {
      std::cout << "Output is not the same size. Our Result size: " << v2.size() << " Your Result Size: " << v1.size()  << std::endl;
      return false;
    }
    auto pair = std::mismatch(v1.begin(), v1.end(), v2.begin(), [](const T& a, const T& b){ return fabs(a - b) < 1.0e-4; });
    if ((pair.first == v1.end() && pair.second == v2.end()))
    {
      return true;
    }
    std::cout << "The vectors are different at index " << (pair.first - v1.begin()) << " Values are: Our Result: " << *pair.second << " vs Your Result: " << *pair.first << std::endl;
    return false;
  }
};

int main() {
  auto ts = TestSuite();

  std::cout << "\033[;36mTest Suite IIR\033[0m (edit class IIRFilter if failed):" << std::endl;
  std::cout << "IIR Test 1: " << std::flush;
  auto iir_first = ts.iir_first();
  if(iir_first) { std::cout << "\u2705 Passed first test" << std::endl;}
  std::cout << "IIR Test 2: " << std::flush;
  auto iir_second = ts.iir_second();
  if(iir_second) { std::cout << "\u2705 Passed second test" << std::endl;}
  std::cout << "IIR Test 3: " << std::flush;
  auto iir_third = ts.iir_third();
  if(iir_third) { std::cout << "\u2705 Passed third test" << std::endl;}

  // Not doing other tests till the IIR tests are finished
  if (!iir_first || !iir_second || !iir_third)
  {
    std::cerr << "\033[1;31mFailed IIR Filter Test. Please edit the class IIRFilter\033[0m" << std::endl;
    return 1;
  }
  std::cout << std::endl;
  std::cout << "\033[42m\033[97mYou passed test suite 1. IIR Filter. Suite 2: Averaging Function starting\033[0m" << std::endl;

  std::cout << std::endl;
  std::cout << "\033[;36mTest Suite Averaging Function\033[0m (edit function getAveragedPacketData if failed):" << std::endl;
  std::cout << "Averaging Function Test 1: " << std::flush;
  auto averaging_first = ts.averaging_first();
  if(averaging_first) { std::cout << "\u2705 Passed first test" << std::endl;}
  std::cout << "Averaging Function Test 2: " << std::flush;
  auto averaging_second = ts.averaging_second();
  if(averaging_second) { std::cout << "\u2705 Passed second test" << std::endl;}
  std::cout << "Averaging Function Test 3: " << std::flush;
  auto averaging_third = ts.averaging_third();
  if(averaging_third){ std::cout << "\u2705 Passed third test" << std::endl;}
  std::cout << "Averaging Function Test 4: " << std::flush;
  auto averaging_fourth = ts.averaging_fourth();
  if(averaging_fourth){ std::cout << "\u2705 Passed fourth test" << std::endl;}


  if (!averaging_first || !averaging_second || !averaging_third || !averaging_fourth)
  {
    std::cerr << "\033[1;31mFailed Averaging Function Test. Please edit the function getAveragedPacketData\033[0m" << std::endl;
    return 1;
  }

  std::cout << std::endl;
  std::cout << "\033[42m\033[97mYou passed test suite 2. Averaging Function. Suite 3: Whole system test starting\033[0m" << std::endl;

  std::cout << std::endl;
  std::cout << "\033[;36mTest Suite Read, Average & Filter Function\033[0m (edit function readAllPacketsAndGetFilteredData if failed):" << std::endl;
  std::cout << std::endl;

  std::cout << "Whole System Test 1: " << std::flush;
  auto whole_test_1 = ts.first();
  if(whole_test_1) { std::cout << "\u2705 Passed first test" << std::endl;}

  if (!whole_test_1)
  {
    std::cerr << "\033[1;31mFailed Read, Average & Filter. Please edit the function readAllPacketsAndGetFilteredData\033[0m" << std::endl;
    return 1;
  }
  std::cout << "\033[42m\033[97mYou all test suites. Congratulations. \033[0m" << std::endl;
  return 0;
}