#include "SpiTestClass.h"

#include <fsfwconfig/devices/gpioIds.h>
#include <fsfw/serviceinterface/ServiceInterface.h>

#include <linux/spi/spidev.h>
#include <fcntl.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <linux/gpio/gpioDefinitions.h>
#include <linux/gpio/GpioCookie.h>
#include <linux/utility/Utility.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <bitset>


SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF* gpioIF): TestTask(objectId),
        gpioIF(gpioIF) {
    if(gpioIF == nullptr) {
        sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl;
    }
    testMode = TestModes::MGM_LIS3MDL;
    spiTransferStruct.rx_buf = reinterpret_cast<__u64>(recvBuffer.data());
    spiTransferStruct.tx_buf = reinterpret_cast<__u64>(sendBuffer.data());
}

ReturnValue_t SpiTestClass::performOneShotAction() {
    switch(testMode) {
    case(TestModes::NONE): {
        break;
    }
    case(TestModes::MGM_LIS3MDL): {
        performLis3MdlTest(mgm0Lis3ChipSelect);
        break;
    }
    case(TestModes::MGM_RM3100): {
        performRm3100Test(mgm1Rm3100ChipSelect);
        break;
    }
    case(TestModes::GYRO_L3GD20H): {
        break;
    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SpiTestClass::performPeriodicAction() {
    return HasReturnvaluesIF::RETURN_OK;
}

void SpiTestClass::performRm3100Test(uint8_t mgmId) {
    /* Configure all SPI chip selects and pull them high */
    acsInit();

    /* Adapt accordingly */
    if(mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) {
        sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl;
    }
    gpioId_t currentGpioId = 0;
    uint8_t chipSelectPin = mgmId;
    if(chipSelectPin == mgm1Rm3100ChipSelect) {
        currentGpioId = gpioIds::MGM_1_RM3100_CS;
    }
    else {
        currentGpioId = gpioIds::MGM_3_RM3100_CS;
    }
    uint32_t rm3100speed = 3'900'000;
    uint8_t rm3100revidReg = 0x36;
    spi::SpiMode rm3100mode = spi::SpiMode::MODE_3;
    //spiTransferStruct.speed_hz = rm3100Speed;
#ifdef RASPBERRY_PI
    std::string deviceName = "/dev/spidev0.0";
#else
    std::string deviceName = "placeholder";
#endif
    int fileDescriptor = 0;


    utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR,
            "SpiComIF::initializeInterface: ");
    if(fileHelper.getOpenResult()) {
        sif::error << "SpiTestClass::performRm3100Test: File descriptor could not be opened!"
                << std::endl;
        return;
    }
    setSpiSpeedAndMode(fileDescriptor, rm3100mode, rm3100speed);

    uint8_t revId = readStmRegister(fileDescriptor, currentGpioId, rm3100revidReg, false);
    sif::info << "SpiTestClass::performRm3100Test: Revision ID 0b" << std::bitset<8>(revId) <<
            std::endl;
}

void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) {
    /* Configure all SPI chip selects and pull them high */
    acsInit();

    /* Adapt accordingly */
    if(lis3Id != mgm0Lis3ChipSelect and lis3Id != mgm2Lis3mdlChipSelect) {
        sif::warning << "SpiTestClass::performLis3MdlTest: Invalid MGM ID!" << std::endl;
    }
    gpioId_t currentGpioId = 0;
    uint8_t chipSelectPin = lis3Id;
    uint8_t whoAmIReg = 0b0000'1111;
    if(chipSelectPin == mgm0Lis3ChipSelect) {
        currentGpioId = gpioIds::MGM_0_LIS3_CS;
    }
    else {
        currentGpioId = gpioIds::MGM_2_LIS3_CS;
    }
    uint32_t spiSpeed = 3'900'000;
    spi::SpiMode spiMode = spi::SpiMode::MODE_3;
#ifdef RASPBERRY_PI
    std::string deviceName = "/dev/spidev0.0";
#else
    std::string deviceName = "placeholder";
#endif
    int fileDescriptor = 0;

    utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR,
            "SpiComIF::initializeInterface: ");
    if(fileHelper.getOpenResult()) {
        sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!"
                << std::endl;
        return;
    }
    setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);

    uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false);
    sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I Regiter 0b" <<
            std::bitset<8>(whoAmIRegVal) << std::endl;
}


void SpiTestClass::acsInit() {
    GpioCookie* gpioCookie = new GpioCookie();
    std::string rpiGpioName = "gpiochip0";
    {
        GpiodRegular gpio(rpiGpioName, mgm0Lis3ChipSelect, "MGM_0_LIS3",
                gpio::Direction::OUT, 1);
        gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);
    }
    {
        GpiodRegular gpio(rpiGpioName, mgm1Rm3100ChipSelect, "MGM_1_RM3100",
                gpio::Direction::OUT, 1);
        gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);
    }
    {
        GpiodRegular gpio(rpiGpioName, gyro0AdisChipSelect, "GYRO_0_ADIS",
                gpio::Direction::OUT, 1);
        gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);
    }
    {
        GpiodRegular gpio(rpiGpioName, gyro1L3gd20ChipSelect, "GYRO_1_L3G",
                gpio::Direction::OUT, 1);
        gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);
    }
    {
        GpiodRegular gpio(rpiGpioName, gyro2L3gd20ChipSelect, "GYRO_2_L3G",
                gpio::Direction::OUT, 1);
        gpioCookie->addGpio(gpioIds::GYRO_2_L3G_CS, gpio);
    }
    {
        GpiodRegular gpio(rpiGpioName, mgm2Lis3mdlChipSelect, "MGM_2_LIS3",
                gpio::Direction::OUT, 1);
        gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);
    }
    {
        GpiodRegular gpio(rpiGpioName, mgm3Rm3100ChipSelect, "MGM_3_RM3100",
                gpio::Direction::OUT, 1);
        gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
    }
    if(gpioIF != nullptr) {
        gpioIF->addGpios(gpioCookie);
    }
}

void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value,
        bool autoIncrement) {
    if(autoIncrement) {
        reg |= STM_AUTO_INCR_MASK;
    }
    spiTransferStruct.len = 2;
    sendBuffer[0] = reg;
    sendBuffer[1] = value;

    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
        gpioIF->pullLow(chipSelect);
    }
    int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if(retval != 0) {
        utility::handleIoctlError("SpiTestClass::writeStmRegister: Write failed");
    }
    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
        gpioIF->pullHigh(chipSelect);
    }
}

void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) {
    int retval = ioctl(spiFd, SPI_IOC_WR_MODE, reinterpret_cast<uint8_t*>(&mode));
    if(retval != 0) {
        utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!");
    }

    retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
    if(retval != 0) {
        utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!");
    }
}

uint8_t SpiTestClass::readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg,
        bool autoIncrement) {
    reg |= STM_READ_MASK;
    if(autoIncrement) {
        reg |= STM_AUTO_INCR_MASK;
    }
    spiTransferStruct.len = 2;
    sendBuffer[0] = reg;
    sendBuffer[1] = 0;

    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
        gpioIF->pullLow(chipSelect);
    }
    int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if(retval < 0) {
        utility::handleIoctlError("SpiTestClass::readStmRegiste: Read failed");
    }
    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
        gpioIF->pullHigh(chipSelect);
    }
    return recvBuffer[1];
}