#include <bsp_q7s/spiCallbacks/rwSpiCallback.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <mission/devices/RwHandler.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>
#include <fsfw_hal/linux/UnixFileGuard.h>
#include "devices/gpioIds.h"

namespace rwSpiCallback {

ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData,
        size_t sendLen, void* args) {

    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;

    RwHandler* handler = reinterpret_cast<RwHandler*>(args);
    if(handler == nullptr) {
        sif::error << "rwSpiCallback::spiCallback: Pointer to handler is invalid"
                << std::endl;
        return HasReturnvaluesIF::RETURN_FAILED;
    }

    uint8_t writeBuffer[2];
    uint8_t writeSize = 0;

    int fileDescriptor = 0;
    std::string device = cookie->getSpiDevice();
    UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "rwSpiCallback::spiCallback");
    if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
        sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl;
        return SpiComIF::OPENING_FILE_FAILED;
    }
    spi::SpiModes spiMode = spi::SpiModes::MODE_0;
    uint32_t spiSpeed = 0;
    cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
    comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);

    gpioId_t gpioId = cookie->getChipSelectPin();
    GpioIF* gpioIF =  comIf->getGpioInterface();
    MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
    uint32_t timeoutMs = 0;
    MutexIF* mutex = comIf->getMutex(&timeoutType, &timeoutMs);
    if(mutex == nullptr or gpioIF == nullptr) {
        sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
        return HasReturnvaluesIF::RETURN_FAILED;
    }

    result = mutex->lockMutex(timeoutType, timeoutMs);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl;
        return result;
    }

    /** Disconnect PS SPI peripheral and select AXI SPI core */
    if(gpioIF->pullHigh(gpioIds::SPI_MUX) != HasReturnvaluesIF::RETURN_OK) {
        sif::error << "rwSpiCallback::spiCallback: Failed to pull spi mux gpio high" << std::endl;
    }

    /** Sending frame start sign */
    writeBuffer[0] = 0x7E;
    writeSize = 1;

    // Pull SPI CS low. For now, no support for active high given
    if(gpioId != gpio::NO_GPIO) {
        if(gpioIF->pullLow(gpioId) != HasReturnvaluesIF::RETURN_OK) {
            sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl;
        }
    }

    if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
        sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
        closeSpi(gpioId, gpioIF, mutex);
        return RwHandler::SPI_WRITE_FAILURE;
    }

    /** Encoding and sending command */
    size_t idx = 0;
    while(idx < sendLen) {
        switch(*(sendData + idx)) {
        case 0x7E:
            writeBuffer[0] = 0x7D;
            writeBuffer[1] = 0x5E;
            writeSize = 2;
            break;
        case 0x7D:
            writeBuffer[0] = 0x7D;
            writeBuffer[1] = 0x5D;
            writeSize = 2;
            break;
        default:
            writeBuffer[0] = *(sendData + idx);
            writeSize = 1;
            break;
        }
        if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
            sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
            closeSpi(gpioId, gpioIF, mutex);
            return RwHandler::SPI_WRITE_FAILURE;
        }
        idx++;
    }

    /** Sending frame end sign */
    writeBuffer[0] = 0x7E;
    writeSize = 1;

    if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
        sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
        closeSpi(gpioId, gpioIF, mutex);
        return RwHandler::SPI_WRITE_FAILURE;
    }

    uint8_t* rxBuf = nullptr;
    result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        closeSpi(gpioId, gpioIF, mutex);
        return result;
    }

    size_t replyBufferSize = cookie->getMaxBufferSize();

    /** There must be a delay of 20 ms after sending the command */
    usleep(RwDefinitions::SPI_REPLY_DELAY);

    /**
     * The reaction wheel responds with empty frames while preparing the reply data.
     * However, receiving more than 5 empty frames will be interpreted as an error.
     */
    uint8_t byteRead = 0;
    for (int idx = 0; idx < 10; idx++) {
        if(read(fileDescriptor, &byteRead, 1) != 1) {
            sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
            closeSpi(gpioId, gpioIF, mutex);
            return RwHandler::SPI_READ_FAILURE;
        }

        if (byteRead != 0x7E) {
            break;
        }

        if (idx == 9) {
            sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl;
            closeSpi(gpioId, gpioIF, mutex);
            return RwHandler::NO_REPLY;
        }
    }

    size_t decodedFrameLen = 0;
    while(decodedFrameLen < replyBufferSize) {

        /** First byte already read in */
        if (decodedFrameLen != 0) {
            byteRead = 0;
            if(read(fileDescriptor, &byteRead, 1) != 1) {
                sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
                result = RwHandler::SPI_READ_FAILURE;
                break;
            }
        }

        if (byteRead == 0x7E) {
            /** Reached end of frame */
            break;
        }
        else if (byteRead == 0x7D) {
            if(read(fileDescriptor, &byteRead, 1) != 1) {
                sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
                result = RwHandler::SPI_READ_FAILURE;
                break;
            }
            if (byteRead == 0x5E) {
                *(rxBuf + decodedFrameLen) = 0x7E;
                decodedFrameLen++;
                continue;
            }
            else if (byteRead == 0x5D) {
                *(rxBuf + decodedFrameLen) = 0x7D;
                decodedFrameLen++;
                continue;
            }
            else {
                sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl;
                closeSpi(gpioId, gpioIF, mutex);
                result = RwHandler::INVALID_SUBSTITUTE;
                break;
            }
        }
        else {
            *(rxBuf + decodedFrameLen) = byteRead;
            decodedFrameLen++;
            continue;
        }

        /**
         * There might be the unlikely case that each byte in a get-telemetry reply has been
         * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E.
         * Otherwise there might be something wrong.
         */
        if (decodedFrameLen == replyBufferSize) {
            if(read(fileDescriptor, &byteRead, 1) != 1) {
                sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl;
                result = RwHandler::SPI_READ_FAILURE;
                break;
            }
            if (byteRead != 0x7E) {
                sif::error << "rwSpiCallback::spiCallback: Missing end sign 0x7E" << std::endl;
                decodedFrameLen--;
                result = RwHandler::MISSING_END_SIGN;
                break;
            }
        }
        result = HasReturnvaluesIF::RETURN_OK;
    }

    cookie->assignTransferSize(decodedFrameLen);

    closeSpi(gpioId, gpioIF, mutex);

    return result;
}

void closeSpi (gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) {
    if(gpioId != gpio::NO_GPIO) {
        if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
            sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
        }
    }
    if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
        sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl;;
    }

    /** Route SPI interface again to PS SPI peripheral */
    if(gpioIF->pullLow(gpioIds::SPI_MUX) != HasReturnvaluesIF::RETURN_OK) {
        sif::error << "rwSpiCallback::spiCallback: Failed to pull spi mux gpio low" << std::endl;
    }
}
}