#include "q7sGpioCallbacks.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 "busConf.h"

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

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

  GpioCookie* spiMuxGpios = new GpioCookie;

  GpiodRegularByLineName* spiMuxBit = nullptr;
  /** Setting mux bit 1 to low will disable IC21 on the interface board */
  spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_0_PIN, "SPI Mux Bit 1",
                                         Direction::OUT, Levels::HIGH);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_0, spiMuxBit);
  /** Setting mux bit 2 to low disables IC1 on the TCS board */
  spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_1_PIN, "SPI Mux Bit 2",
                                         Direction::OUT, Levels::HIGH);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_1, spiMuxBit);
  /** Setting mux bit 3 to low disables IC2 on the TCS board and IC22 on the interface board */
  spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_2_PIN, "SPI Mux Bit 3",
                                         Direction::OUT, Levels::LOW);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_2, spiMuxBit);

  /** The following gpios can take arbitrary initial values */
  spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_3_PIN, "SPI Mux Bit 4",
                                         Direction::OUT, Levels::LOW);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_3, spiMuxBit);
  spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_4_PIN, "SPI Mux Bit 5",
                                         Direction::OUT, Levels::LOW);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_4, spiMuxBit);
  spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_5_PIN, "SPI Mux Bit 6",
                                         Direction::OUT, Levels::LOW);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_5, spiMuxBit);
  GpiodRegularByLineName* enRwDecoder = new GpiodRegularByLineName(
      q7s::gpioNames::EN_RW_CS, "EN_RW_CS", Direction::OUT, Levels::HIGH);
  spiMuxGpios->addGpio(gpioIds::EN_RW_CS, enRwDecoder);

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