#include "gpioCallbacks.h"

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

#include "busConf.h"

namespace gpioCallbacks {

GpioIF* gpioComInterface;

void initSpiCsDecoder(GpioIF* gpioComIF) {
  ReturnValue_t result;

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

  gpioComInterface = gpioComIF;

  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",
                                         gpio::DIR_OUT, gpio::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",
                                         gpio::DIR_OUT, gpio::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",
                                         gpio::DIR_OUT, gpio::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",
                                         gpio::DIR_OUT, gpio::LOW);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_3, spiMuxBit);
  spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_4_PIN, "SPI Mux Bit 5",
                                         gpio::DIR_OUT, gpio::LOW);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_4, spiMuxBit);
  spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_5_PIN, "SPI Mux Bit 6",
                                         gpio::DIR_OUT, gpio::LOW);
  spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_5, spiMuxBit);
  GpiodRegularByLineName* enRwDecoder =
      new GpiodRegularByLineName(q7s::gpioNames::EN_RW_CS, "EN_RW_CS", gpio::DIR_OUT, gpio::HIGH);
  spiMuxGpios->addGpio(gpioIds::EN_RW_CS, enRwDecoder);

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

void spiCsDecoderCallback(gpioId_t gpioId, gpio::GpioOperation gpioOp, gpio::Levels value,
                          void* args) {
  if (gpioComInterface == nullptr) {
    sif::debug << "spiCsDecoderCallback: No gpioComIF specified. Call initSpiCsDecoder "
               << "to specify gpioComIF" << std::endl;
    return;
  }

  /* Reading is not supported by the callback function */
  if (gpioOp == gpio::GpioOperation::READ) {
    return;
  }

  if (value == gpio::HIGH) {
    switch (gpioId) {
      case (gpioIds::RTD_IC_3): {
        disableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_4): {
        disableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_5): {
        disableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_6): {
        disableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_7): {
        disableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_8): {
        disableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_9): {
        disableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_10): {
        disableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_11): {
        disableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_12): {
        disableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_13): {
        disableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_14): {
        disableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_15): {
        disableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_16): {
        disableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_17): {
        disableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_18): {
        disableDecoderTcsIc2();
        break;
      }
      case (gpioIds::CS_SUS_0): {
        disableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_1): {
        disableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_2): {
        disableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_3): {
        disableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_4): {
        disableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_5): {
        disableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_6): {
        disableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_7): {
        disableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_8): {
        disableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_9): {
        disableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_10): {
        disableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_11): {
        disableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_RW1): {
        disableRwDecoder();
        break;
      }
      case (gpioIds::CS_RW2): {
        disableRwDecoder();
        break;
      }
      case (gpioIds::CS_RW3): {
        disableRwDecoder();
        break;
      }
      case (gpioIds::CS_RW4): {
        disableRwDecoder();
        break;
      }
      default:
        sif::debug << "spiCsDecoderCallback: Invalid gpio id " << gpioId << std::endl;
    }
  } else if (value == gpio::LOW) {
    switch (gpioId) {
      case (gpioIds::RTD_IC_3): {
        selectY7();
        enableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_4): {
        selectY6();
        enableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_5): {
        selectY5();
        enableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_6): {
        selectY4();
        enableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_7): {
        selectY3();
        enableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_8): {
        selectY2();
        enableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_9): {
        selectY1();
        enableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_10): {
        selectY0();
        enableDecoderTcsIc1();
        break;
      }
      case (gpioIds::RTD_IC_11): {
        selectY7();
        enableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_12): {
        selectY6();
        enableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_13): {
        selectY5();
        enableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_14): {
        selectY4();
        enableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_15): {
        selectY3();
        enableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_16): {
        selectY2();
        enableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_17): {
        selectY1();
        enableDecoderTcsIc2();
        break;
      }
      case (gpioIds::RTD_IC_18): {
        selectY0();
        enableDecoderTcsIc2();
        break;
      }
      case (gpioIds::CS_SUS_0): {
        selectY0();
        enableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_1): {
        selectY1();
        enableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_2): {
        selectY2();
        enableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_3): {
        selectY3();
        enableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_4): {
        selectY4();
        enableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_5): {
        selectY5();
        enableDecoderInterfaceBoardIc1();
        break;
      }
      case (gpioIds::CS_SUS_6): {
        selectY0();
        enableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_7): {
        selectY1();
        enableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_8): {
        selectY2();
        enableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_9): {
        selectY3();
        enableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_10): {
        selectY4();
        enableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_SUS_11): {
        selectY5();
        enableDecoderInterfaceBoardIc2();
        break;
      }
      case (gpioIds::CS_RW1): {
        selectY0();
        enableRwDecoder();
        break;
      }
      case (gpioIds::CS_RW2): {
        selectY1();
        enableRwDecoder();
        break;
      }
      case (gpioIds::CS_RW3): {
        selectY2();
        enableRwDecoder();
        break;
      }
      case (gpioIds::CS_RW4): {
        selectY3();
        enableRwDecoder();
        break;
      }
      default:
        sif::debug << "spiCsDecoderCallback: Invalid gpio id " << gpioId << std::endl;
    }
  } else {
    sif::debug << "spiCsDecoderCallback: Invalid value. Must be 0 or 1" << std::endl;
  }
}

void enableDecoderTcsIc1() {
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_1);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void enableDecoderTcsIc2() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_1);
}

void enableDecoderInterfaceBoardIc1() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void enableDecoderInterfaceBoardIc2() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2);
}

void disableDecoderTcsIc1() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_1);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void disableDecoderTcsIc2() {
  // DO NOT CHANGE THE ORDER HERE
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_1);
}

void disableDecoderInterfaceBoardIc1() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_1);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void disableDecoderInterfaceBoardIc2() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_1);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void enableRwDecoder() { gpioComInterface->pullHigh(gpioIds::EN_RW_CS); }

void disableRwDecoder() { gpioComInterface->pullLow(gpioIds::EN_RW_CS); }

void selectY0() {
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5);
}

void selectY1() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5);
}

void selectY2() {
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5);
}

void selectY3() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5);
}

void selectY4() {
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5);
}

void selectY5() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5);
}

void selectY6() {
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5);
}

void selectY7() {
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4);
  gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5);
}

void disableAllDecoder() {
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_0);
  gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1);
  gpioComInterface->pullLow(gpioIds::EN_RW_CS);
}

}  // namespace gpioCallbacks