#include "gpioInit.h"
#include "definitions.h"

#include <devices/gpioIds.h>

#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>
#include "fsfw_hal/linux/rpi/GpioRPi.h"
#include <fsfw_hal/common/gpio/GpioIF.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 != HasReturnvaluesIF::RETURN_OK) {
      sif::error << "Creating Raspberry Pi SPI Mux GPIO failed with code " << result << std::endl;
      return;
    }
  }

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

#endif