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

#ifndef PERIPHERALS_IR_SENSORS_HPP_
#define 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 /* PERIPHERALS_IR_SENSORS_HPP_ */