#include "gpioCallbacks.h"

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

void gpioCallbacks::spiCsDecoderCallback(gpioId_t gpioId, gpio::GpioOperation gpioOp,
                                         gpio::Levels value, void* args) {
  GpioIF* gpioIF = reinterpret_cast<GpioIF*>(args);
  if (gpioIF == 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::Levels::HIGH) {
    switch (gpioId) {
      case (gpioIds::RTD_IC_3): {
        disableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_4): {
        disableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_5): {
        disableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_6): {
        disableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_7): {
        disableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_8): {
        disableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_9): {
        disableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_10): {
        disableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_11): {
        disableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_12): {
        disableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_13): {
        disableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_14): {
        disableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_15): {
        disableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_16): {
        disableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_17): {
        disableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_18): {
        disableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_0): {
        disableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_1): {
        disableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_2): {
        disableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_3): {
        disableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_4): {
        disableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_5): {
        disableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_6): {
        disableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_7): {
        disableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_8): {
        disableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_9): {
        disableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_10): {
        disableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_11): {
        disableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_RW1): {
        disableRwDecoder(gpioIF);
        break;
      }
      case (gpioIds::CS_RW2): {
        disableRwDecoder(gpioIF);
        break;
      }
      case (gpioIds::CS_RW3): {
        disableRwDecoder(gpioIF);
        break;
      }
      case (gpioIds::CS_RW4): {
        disableRwDecoder(gpioIF);
        break;
      }
      default:
        sif::debug << "spiCsDecoderCallback: Invalid gpio id " << gpioId << std::endl;
    }
  } else if (value == gpio::Levels::LOW) {
    switch (gpioId) {
      case (gpioIds::RTD_IC_3): {
        selectY7(gpioIF);
        enableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_4): {
        selectY6(gpioIF);
        enableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_5): {
        selectY5(gpioIF);
        enableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_6): {
        selectY4(gpioIF);
        enableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_7): {
        selectY3(gpioIF);
        enableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_8): {
        selectY2(gpioIF);
        enableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_9): {
        selectY1(gpioIF);
        enableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_10): {
        selectY0(gpioIF);
        enableDecoderTcsIc1(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_11): {
        selectY7(gpioIF);
        enableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_12): {
        selectY6(gpioIF);
        enableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_13): {
        selectY5(gpioIF);
        enableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_14): {
        selectY4(gpioIF);
        enableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_15): {
        selectY3(gpioIF);
        enableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_16): {
        selectY2(gpioIF);
        enableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_17): {
        selectY1(gpioIF);
        enableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::RTD_IC_18): {
        selectY0(gpioIF);
        enableDecoderTcsIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_0): {
        selectY0(gpioIF);
        enableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_1): {
        selectY1(gpioIF);
        enableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_2): {
        selectY2(gpioIF);
        enableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_3): {
        selectY3(gpioIF);
        enableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_4): {
        selectY4(gpioIF);
        enableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_5): {
        selectY5(gpioIF);
        enableDecoderInterfaceBoardIc1(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_6): {
        selectY0(gpioIF);
        enableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_7): {
        selectY1(gpioIF);
        enableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_8): {
        selectY2(gpioIF);
        enableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_9): {
        selectY3(gpioIF);
        enableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_10): {
        selectY4(gpioIF);
        enableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_SUS_11): {
        selectY5(gpioIF);
        enableDecoderInterfaceBoardIc2(gpioIF);
        break;
      }
      case (gpioIds::CS_RW1): {
        selectY0(gpioIF);
        enableRwDecoder(gpioIF);
        break;
      }
      case (gpioIds::CS_RW2): {
        selectY1(gpioIF);
        enableRwDecoder(gpioIF);
        break;
      }
      case (gpioIds::CS_RW3): {
        selectY2(gpioIF);
        enableRwDecoder(gpioIF);
        break;
      }
      case (gpioIds::CS_RW4): {
        selectY3(gpioIF);
        enableRwDecoder(gpioIF);
        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 gpioCallbacks::enableDecoderTcsIc1(GpioIF* gpioIF) {
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_0);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_1);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void gpioCallbacks::enableDecoderTcsIc2(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_2);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_0);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_1);
}

void gpioCallbacks::enableDecoderInterfaceBoardIc1(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_1);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void gpioCallbacks::enableDecoderInterfaceBoardIc2(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_1);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_2);
}

void gpioCallbacks::disableDecoderTcsIc1(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_1);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_2);
}

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

void gpioCallbacks::disableDecoderInterfaceBoardIc1(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_1);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void gpioCallbacks::disableDecoderInterfaceBoardIc2(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_0);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_1);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_2);
}

void gpioCallbacks::enableRwDecoder(GpioIF* gpioIF) { gpioIF->pullHigh(gpioIds::EN_RW_CS); }

void gpioCallbacks::disableRwDecoder(GpioIF* gpioIF) { gpioIF->pullLow(gpioIds::EN_RW_CS); }

void gpioCallbacks::selectY0(GpioIF* gpioIF) {
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_3);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_4);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_5);
}

void gpioCallbacks::selectY1(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_3);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_4);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_5);
}

void gpioCallbacks::selectY2(GpioIF* gpioIF) {
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_3);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_4);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_5);
}

void gpioCallbacks::selectY3(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_3);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_4);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_5);
}

void gpioCallbacks::selectY4(GpioIF* gpioIF) {
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_3);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_4);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_5);
}

void gpioCallbacks::selectY5(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_3);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_4);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_5);
}

void gpioCallbacks::selectY6(GpioIF* gpioIF) {
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_3);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_4);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_5);
}

void gpioCallbacks::selectY7(GpioIF* gpioIF) {
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_3);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_4);
  gpioIF->pullHigh(gpioIds::SPI_MUX_BIT_5);
}

void gpioCallbacks::disableAllDecoder(GpioIF* gpioIF) {
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_2);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_0);
  gpioIF->pullLow(gpioIds::SPI_MUX_BIT_1);
  gpioIF->pullLow(gpioIds::EN_RW_CS);
}