#include "gpioCallbacks.h"
#include "busConf.h"
#include <devices/gpioIds.h>

#include <fsfw_hal/linux/gpio/LinuxLibgpioIF.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>
#include <fsfw/serviceinterface/ServiceInterface.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_1_PIN, "SPI Mux Bit 1",
            gpio::OUT, gpio::HIGH);
    spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_1, spiMuxBit);
    /** Setting mux bit 2 to low disables IC1 on the TCS board */
    spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_2_PIN, "SPI Mux Bit 2", gpio::OUT, gpio::HIGH);
    spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_2, 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_3_PIN, "SPI Mux Bit 3", gpio::OUT, gpio::LOW);
    spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_3, spiMuxBit);
    /** The following gpios can take arbitrary initial values */
    spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_4_PIN, "SPI Mux Bit 4", gpio::OUT, gpio::LOW);
    spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_4, spiMuxBit);
    spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_5_PIN, "SPI Mux Bit 5", gpio::OUT, gpio::LOW);
    spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_5, spiMuxBit);
    spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_6_PIN, "SPI Mux Bit 6", gpio::OUT, gpio::LOW);
    spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_6, spiMuxBit);
    GpiodRegularByLineName* enRwDecoder = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_CS,
            "EN_RW_CS", gpio::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_1): {
			disableDecoderInterfaceBoardIc1();
			break;
		}
		case(gpioIds::CS_SUS_2): {
			disableDecoderInterfaceBoardIc1();
			break;
		}
		case(gpioIds::CS_SUS_3): {
			disableDecoderInterfaceBoardIc2();
			break;
		}
		case(gpioIds::CS_SUS_4): {
			disableDecoderInterfaceBoardIc2();
			break;
		}
		case(gpioIds::CS_SUS_5): {
			disableDecoderInterfaceBoardIc2();
			break;
		}
		case(gpioIds::CS_SUS_6): {
			disableDecoderInterfaceBoardIc1();
			break;
		}
		case(gpioIds::CS_SUS_7): {
			disableDecoderInterfaceBoardIc1();
			break;
		}
		case(gpioIds::CS_SUS_8): {
			disableDecoderInterfaceBoardIc2();
			break;
		}
		case(gpioIds::CS_SUS_9): {
			disableDecoderInterfaceBoardIc1();
			break;
		}
		case(gpioIds::CS_SUS_10): {
			disableDecoderInterfaceBoardIc1();
			break;
		}
		case(gpioIds::CS_SUS_11): {
			disableDecoderInterfaceBoardIc2();
			break;
		}
		case(gpioIds::CS_SUS_12): {
			disableDecoderInterfaceBoardIc2();
			break;
		}
		case(gpioIds::CS_SUS_13): {
			disableDecoderInterfaceBoardIc1();
			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_1): {
        	selectY0();
            enableDecoderInterfaceBoardIc1();
            break;
        }
        case(gpioIds::CS_SUS_2): {
        	selectY1();
            enableDecoderInterfaceBoardIc1();
            break;
        }
        case(gpioIds::CS_SUS_3): {
        	selectY0();
            enableDecoderInterfaceBoardIc2();
            break;
        }
        case(gpioIds::CS_SUS_4): {
        	selectY1();
            enableDecoderInterfaceBoardIc2();
            break;
        }
        case(gpioIds::CS_SUS_5): {
        	selectY2();
            enableDecoderInterfaceBoardIc2();
            break;
        }
        case(gpioIds::CS_SUS_6): {
        	selectY2();
            enableDecoderInterfaceBoardIc1();
            break;
        }
        case(gpioIds::CS_SUS_7): {
        	selectY3();
            enableDecoderInterfaceBoardIc1();
            break;
        }
        case(gpioIds::CS_SUS_8): {
        	selectY3();
            enableDecoderInterfaceBoardIc2();
            break;
        }
        case(gpioIds::CS_SUS_9): {
        	selectY4();
            enableDecoderInterfaceBoardIc1();
            break;
        }
        case(gpioIds::CS_SUS_10): {
        	selectY5();
            enableDecoderInterfaceBoardIc1();
            break;
        }
        case(gpioIds::CS_SUS_11): {
        	selectY4();
            enableDecoderInterfaceBoardIc2();
            break;
        }
        case(gpioIds::CS_SUS_12): {
        	selectY5();
            enableDecoderInterfaceBoardIc2();
            break;
        }
        case(gpioIds::CS_SUS_13): {
        	selectY6();
            enableDecoderInterfaceBoardIc1();
            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_1);
    gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2);
    gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
}

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

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

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

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

void disableDecoderTcsIc2() {
    gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_1);
    gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
    gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2);
}

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

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

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

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

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

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

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

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

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

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

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

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

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

}