#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>

ReturnValue_t rwSpiCallback(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: 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: ");
    if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
        sif::error << "rwSpiCallback: 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: Mutex or GPIO interface invalid" << std::endl;
        return HasReturnvaluesIF::RETURN_FAILED;
    }

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

    /** 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: Failed to pull chip select low" << std::endl;
        }
    }

    if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
        sif::error << "rwSpiCallback: Write failed!" << std::endl;
        if(gpioId != gpio::NO_GPIO) {
            if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
                sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
            }
        }
        if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
            sif::error << "rwSpiCallback: Failed to unlock mutex" << std::endl;;
        }
        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: Write failed!" << std::endl;
            if(gpioId != gpio::NO_GPIO) {
                if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
                    sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
                }
            }
            if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
                sif::error << "rwSpiCallback: Failed to unlock mutex";
            }
            return RwHandler::SPI_WRITE_FAILURE;
        }
        idx++;
    }

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

    if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
        sif::error << "rwSpiCallback: Write failed!" << std::endl;
        if(gpioId != gpio::NO_GPIO) {
            if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
                sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
            }
        }
        if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
            sif::error << "rwSpiCallback: Failed to unlock mutex";
        }
        return RwHandler::SPI_WRITE_FAILURE;
    }

    uint8_t* rxBuf = nullptr;
    result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf);
    if(result != HasReturnvaluesIF::RETURN_OK) {
    	if(gpioId != gpio::NO_GPIO) {
			if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
				sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
			}
		}
		if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
			sif::error << "rwSpiCallback: Failed to unlock 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);

    /** Receiving reply data */
    uint8_t byteRead = 0;
    /** Reading the reply frame */
//    if(read(fileDescriptor, &byteRead, 1) != 1) {
//    	if(gpioId != gpio::NO_GPIO) {
//			if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
//				sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
//			}
//		}
//		if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
//			sif::error << "rwSpiCallback: Failed to unlock mutex";
//		}
//        sif::error << "rwSpiCallback: Failed to read first byte of reply frame" << std::endl;
//        return RwHandler::SPI_READ_FAILURE;
//    }

    uint8_t readbuffer[10];
    if(read(fileDescriptor, readbuffer, 10) != 10) {
    	sif::error << "Failed to read all bytes" << std::endl;;
    }

    /** First byte must be the start sign 0x7E */
    if (byteRead != 0x7E) {
        sif::error << "rwSpiCallback: First byte of reply is not 0x7E" << std::endl;
        if(gpioId != gpio::NO_GPIO) {
			if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
				sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
			}
		}
		if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
			sif::error << "rwSpiCallback: Failed to unlock mutex";
		}
        return RwHandler::MISSING_START_SIGN;
    }

    /**
     * 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.
     */
    for (int idx = 0; idx < 10; idx++) {
    	sif::error << "rwSpiCallback: Empty frame timeout";
    	return RwHandler::NO_REPLY;
    }

    size_t decodedFrameLen = 0;
    for (; decodedFrameLen < replyBufferSize; decodedFrameLen++) {
    	byteRead = 0;
        if(read(fileDescriptor, &byteRead, 1) != 1) {
            sif::error << "rwSpiCallback: Read failed" << std::endl;
            return RwHandler::SPI_READ_FAILURE;
        }

        if (byteRead == 0x7E) {
            /** Reached end of frame */
            break;
        }
        else if (byteRead == 0xD) {
            if(read(fileDescriptor, &byteRead, 1) != 1) {
                sif::error << "rwSpiCallback: 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: Invalid substitute" << std::endl;
                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: Failed to read last byte" << std::endl;
                result = RwHandler::SPI_READ_FAILURE;
                break;
            }
            if (byteRead != 0x7E) {
                sif::error << "rwSpiCallback: Missing end sign 0x7E" << std::endl;
                decodedFrameLen--;
                result = RwHandler::MISSING_END_SIGN;
                break;
            }
        }
        result = HasReturnvaluesIF::RETURN_OK;
    }

    cookie->assignTransferSize(decodedFrameLen);

    if(gpioId != gpio::NO_GPIO) {
        if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
            sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
        }
    }

    if(gpioId != gpio::NO_GPIO) {
        if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
            sif::error << "rwSpiCallback: Failed to unlock mutex" << std::endl;
        }
    }
    return result;
}