#include "gpioInit.h"

#include <devices/gpioIds.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>
#include <fsfw_hal/common/gpio/GpioIF.h>

#include "definitions.h"
#include "fsfw_hal/linux/rpi/GpioRPi.h"

#ifdef RASPBERRY_PI

struct MuxInfo {
  MuxInfo(gpioId_t gpioId, int bcmNum, std::string consumer)
      : gpioId(gpioId), bcmNum(bcmNum), consumer(consumer) {}
  gpioId_t gpioId;
  int bcmNum;
  std::string consumer;
};

void rpi::gpio::initSpiCsDecoder(GpioIF* gpioComIF) {
  using namespace ::gpio;
  ReturnValue_t result;

  if (gpioComIF == nullptr) {
    sif::debug << "initSpiCsDecoder: Invalid gpioComIF" << std::endl;
    return;
  }

  std::array<::MuxInfo, 6> muxInfo{
      MuxInfo(gpioIds::SPI_MUX_BIT_0, SPI_MUX_0_BCM, "SPI_MUX_0"),
      MuxInfo(gpioIds::SPI_MUX_BIT_1, SPI_MUX_1_BCM, "SPI_MUX_1"),
      MuxInfo(gpioIds::SPI_MUX_BIT_2, SPI_MUX_2_BCM, "SPI_MUX_2"),
      MuxInfo(gpioIds::SPI_MUX_BIT_3, SPI_MUX_3_BCM, "SPI_MUX_3"),
      MuxInfo(gpioIds::SPI_MUX_BIT_4, SPI_MUX_4_BCM, "SPI_MUX_4"),
      MuxInfo(gpioIds::SPI_MUX_BIT_5, SPI_MUX_5_BCM, "SPI_MUX_5"),
  };
  GpioCookie* spiMuxGpios = new GpioCookie;

  for (const auto& info : muxInfo) {
    result = createRpiGpioConfig(spiMuxGpios, info.gpioId, info.bcmNum, info.consumer,
                                 Direction::OUT, Levels::LOW);
    if (result != returnvalue::OK) {
      sif::error << "Creating Raspberry Pi SPI Mux GPIO failed with code " << result << std::endl;
      return;
    }
  }

  result = gpioComIF->addGpios(spiMuxGpios);
  if (result != returnvalue::OK) {
    sif::error << "initSpiCsDecoder: Failed to add mux bit gpios to gpioComIF" << std::endl;
    return;
  }
}

#endif