/*
 * peripherals_ir_sensors.hpp
 *
 *  Created on: Apr 10, 2021
 *      Author: erki
 */

#ifndef SKULLC_PERIPHERALS_IR_SENSORS_HPP_
#define SKULLC_PERIPHERALS_IR_SENSORS_HPP_

#include <cmath>
#include <initializer_list>
#include <tuple>

#include <peripherals_adc.hpp>

namespace Peripherals
{

template<typename G, std::size_t N, std::size_t Average>
class IrSensors
{
public:
  using gpio = G;

  Adc<N> adc;

  IrSensors() = delete;

  template<typename... Args>
  explicit IrSensors(ADC_HandleTypeDef* hadc,
                     const std::array<std::uint32_t, N>& channels,
                     Args&&... gpios)
      : adc(hadc), channels_(channels), gpios_{std::forward<Args>(gpios)...}
  {
    static_assert(sizeof...(Args) == N, "Not enough GPIOs passed.");
  }

  void startReading()
  {
    for (auto& gpio : gpios_)
      gpio.set(true);

    adc.startDma();
  }

  void stopReading()
  {
    for (auto& gpio : gpios_)
      gpio.set(false);

    adc.stopDma();
  }

  void updateReadings()
  {
    for (std::size_t i = 0; i < N; i++)
    {
      averages_[i] -= averages_[i] / Average;
      averages_[i] += adc.readings[i] / Average;
    }
  }

  std::array<std::pair<float, bool>, N> getDistanceData()
  {
    std::array<std::pair<float, bool>, N> data;

    for (std::size_t i = 0; i < N; i++)
    {
      const float interim = averages_[i] - offsets_[i];

      data[i].first = multipliers_[i] * std::pow(interim, exponents_[i]);
      data[i].second = data[i].first < wall_thresholds_[i];
    }

    return data;
  }

  void calibrate(const std::uint32_t samples, const std::uint32_t sample_time)
  {
    std::array<std::uint32_t, N> measurements;

    for (std::uint32_t i = 0; i < samples; i++)
    {
      for (std::uint32_t ch = 0; ch < channels_.size(); ch++)
        measurements[ch] += adc.read(channels_[ch], sample_time, 100);
    }

    for (std::uint32_t ch = 0; ch < channels_.size(); ch++)
      offsets_[ch] = std::uint16_t(measurements[ch] / samples);
  }

  void setExponents(const std::array<float, N>& e)
  {
    exponents_ = e;
  }

  void setMultipliers(const std::array<float, N>& m)
  {
    multipliers_ = m;
  }

  void setWallThresholds(const std::array<float, N>& t)
  {
    wall_thresholds_ = t;
  }

private:
  std::array<std::uint32_t, N> channels_;
  std::array<gpio, N> gpios_;
  std::array<std::uint16_t, N> averages_;

  std::array<std::uint16_t, N> offsets_;
  std::array<float, N> exponents_;
  std::array<float, N> multipliers_;
  std::array<float, N> wall_thresholds_;
};

}// namespace Peripherals


#endif /* SKULLC_PERIPHERALS_IR_SENSORS_HPP_ */