#include "SpiTestClass.h"
#include "OBSWConfig.h"

#include "devices/gpioIds.h"

#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>

#include <fsfw_hal/linux/utility.h>
#include <fsfw_hal/linux/UnixFileGuard.h>
#include <fsfw_hal/common/gpio/gpioDefinitions.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>

#include <linux/spi/spidev.h>
#include <fcntl.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::GYRO_L3GD20H;
    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(mgm2Lis3mdlChipSelect);
        break;
    }
    case(TestModes::MGM_RM3100): {
        performRm3100Test(mgm3Rm3100ChipSelect);
        break;
    }
    case(TestModes::GYRO_L3GD20H): {
        performL3gTest(gyro1L3gd20ChipSelect);
        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 = 976'000;
    uint8_t rm3100revidReg = 0x36;
    spi::SpiModes rm3100mode = spi::SpiModes::MODE_3;

#ifdef RASPBERRY_PI
    std::string deviceName = "/dev/spidev0.0";
#else
    std::string deviceName = "/dev/spidev2.0";
#endif
    int fileDescriptor = 0;


    UnixFileGuard 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 = readRegister(fileDescriptor, currentGpioId, rm3100revidReg);
    sif::info << "SpiTestClass::performRm3100Test: Revision ID 0b" << std::bitset<8>(revId) <<
            std::endl;

    /* Write configuration to CMM register */
    writeRegister(fileDescriptor, currentGpioId, 0x01, 0x75);
    uint8_t cmmRegister = readRm3100Register(fileDescriptor , currentGpioId, 0x01);
    sif::info << "SpiTestClass::performRm3100Test: CMM register value: " <<
            std::hex << "0x" << static_cast<int>(cmmRegister) << std::dec << std::endl;

    /* Read the cycle count registers */
    uint8_t cycleCountsRaw[6];
    readMultipleRegisters(fileDescriptor, currentGpioId, 0x04, cycleCountsRaw, 6);

    uint16_t cycleCountX = cycleCountsRaw[0] << 8 | cycleCountsRaw[1];
    uint16_t cycleCountY = cycleCountsRaw[2] << 8 | cycleCountsRaw[3];
    uint16_t cycleCountZ = cycleCountsRaw[4] << 8 | cycleCountsRaw[5];

    sif::info << "Cycle count X: " << cycleCountX << std::endl;
    sif::info << "Cycle count Y: " << cycleCountY << std::endl;
    sif::info << "Cycle count z: " << cycleCountZ << std::endl;

    writeRegister(fileDescriptor, currentGpioId, 0x0B, 0x95);
    uint8_t tmrcReg = readRm3100Register(fileDescriptor, currentGpioId, 0x0B);
    sif::info << "SpiTestClass::performRm3100Test: TMRC register value: " <<
            std::hex << "0x" << static_cast<int>(tmrcReg) << std::dec << std::endl;

    TaskFactory::delayTask(10);
    uint8_t statusReg = readRm3100Register(fileDescriptor, currentGpioId, 0x34);
    sif::info << "SpiTestClass::performRm3100Test: Status Register 0b" <<
            std::bitset<8>(statusReg) << std::endl;
    /* This means that data is not ready */
    if((statusReg & 0b1000'0000) == 0) {
        sif::warning << "SpiTestClass::performRm3100Test: Data not ready!" << std::endl;
        TaskFactory::delayTask(10);
        uint8_t statusReg = readRm3100Register(fileDescriptor, currentGpioId, 0x34);
        if((statusReg & 0b1000'0000) == 0) {
            return;
        }
    }

    uint32_t rm3100DefaultCycleCout = 0xC8;
    /* Gain scales lineary with cycle count and is 38 for cycle count 100 */
    float rm3100Gain = rm3100DefaultCycleCout / 100.0 * 38.0;
    float scaleFactor = 1 / rm3100Gain;
    uint8_t rawValues[9];
    readMultipleRegisters(fileDescriptor, currentGpioId, 0x24, rawValues, 9);

    /* The sensor generates 24 bit signed values */
    int32_t rawX = ((rawValues[0] << 24) | (rawValues[1] << 16) | (rawValues[2] << 8)) >> 8;
    int32_t rawY = ((rawValues[3] << 24) | (rawValues[4] << 16) | (rawValues[5] << 8)) >> 8;
    int32_t rawZ = ((rawValues[6] << 24) | (rawValues[7] << 16) | (rawValues[8] << 8)) >> 8;

    float fieldStrengthX = rawX * scaleFactor;
    float fieldStrengthY = rawY * scaleFactor;
    float fieldStrengthZ = rawZ * scaleFactor;

    sif::info << "RM3100 measured field strenghts in microtesla:" << std::endl;
    sif::info << "Field Strength X: " << fieldStrengthX << " \xC2\xB5T" << std::endl;
    sif::info << "Field Strength Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl;
    sif::info << "Field Strength Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl;
}

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

    /* Adapt accordingly */
    if(lis3Id != mgm0Lis3mdlChipSelect 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;
    uint8_t whoAmIRegExpectedVal = 0b0011'1101;
    if(chipSelectPin == mgm0Lis3mdlChipSelect) {
        currentGpioId = gpioIds::MGM_0_LIS3_CS;
    }
    else {
        currentGpioId = gpioIds::MGM_2_LIS3_CS;
    }
    uint32_t spiSpeed = 3'900'000;
    spi::SpiModes spiMode = spi::SpiModes::MODE_3;
#ifdef RASPBERRY_PI
    std::string deviceName = "/dev/spidev0.0";
#else
    std::string deviceName = "/dev/spidev2.0";
#endif
    int fileDescriptor = 0;

    UnixFileGuard 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);
    spiTransferStruct.delay_usecs = 0;

    uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false);
    sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I register 0b" <<
            std::bitset<8>(whoAmIRegVal) << std::endl;
    if(whoAmIRegVal != whoAmIRegExpectedVal) {
        sif::warning << "SpiTestClass::performLis3MdlTest: WHO AM I register invalid!"
                << std::endl;
    }

}


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

    l3gId = gyro2L3gd20ChipSelect;

    /* Adapt accordingly */
    if(l3gId != gyro1L3gd20ChipSelect and l3gId != gyro2L3gd20ChipSelect) {
        sif::warning << "SpiTestClass::performLis3MdlTest: Invalid MGM ID!" << std::endl;
    }
    gpioId_t currentGpioId = 0;
    uint8_t chipSelectPin = l3gId;
    uint8_t whoAmIReg = 0b0000'1111;
    uint8_t whoAmIRegExpectedVal = 0b1101'0111;

    if(chipSelectPin == gyro1L3gd20ChipSelect) {
        currentGpioId = gpioIds::GYRO_1_L3G_CS;
    }
    else {
        currentGpioId = gpioIds::GYRO_3_L3G_CS;
    }
    uint32_t spiSpeed = 3'900'000;
    spi::SpiModes spiMode = spi::SpiModes::MODE_3;
#ifdef RASPBERRY_PI
    std::string deviceName = "/dev/spidev0.0";
#else
    std::string deviceName = "/dev/spidev2.0";
#endif
    int fileDescriptor = 0;

    UnixFileGuard 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 register 0b" <<
            std::bitset<8>(whoAmIRegVal) << std::endl;
    if(whoAmIRegVal != whoAmIRegExpectedVal) {
        sif::warning << "SpiTestClass::performL3gTest: Read WHO AM I register invalid!" <<
                std::endl;
    }

    uint8_t ctrlReg1Addr = 0b0010'0000;
    {
        uint8_t commandRegs[5];
        commandRegs[0] = 0b0000'1111;
        commandRegs[1] = 0x0;
        commandRegs[2] = 0x0;
        /* Configure big endian data format */
        commandRegs[3] = 0b0100'0000;
        commandRegs[4] = 0x0;
        writeMultipleStmRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, commandRegs,
                sizeof(commandRegs));
        uint8_t readRegs[5];
        readMultipleRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, readRegs,
                sizeof(readRegs));
        for(uint8_t idx = 0; idx < sizeof(readRegs); idx++) {
            if(readRegs[idx] != commandRegs[0]) {
                sif::warning << "SpiTestClass::performL3gTest: Read control register " <<
                        static_cast<int>(idx + 1) << " not equal to configured value" << std::endl;
            }
        }
    }

    uint8_t readOutBuffer[14];
    readMultipleStmRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, readOutBuffer,
            sizeof(readOutBuffer));

    uint8_t statusReg = readOutBuffer[7];
    sif::info << "SpiTestClass::performL3gTest: Status Register 0b" <<
            std::bitset<8>(statusReg) << std::endl;

    uint16_t l3gRange = 245;
    float scaleFactor = static_cast<float>(l3gRange) / INT16_MAX;
    /* The sensor spits out little endian */
    int16_t angVelocRawX = (readOutBuffer[8] << 8) | readOutBuffer[9];
    int16_t angVelocRawY = (readOutBuffer[10] << 8) | readOutBuffer[11];
    int16_t angVelocRawZ = (readOutBuffer[12] << 8) | readOutBuffer[13];

    float angVelocX = scaleFactor * angVelocRawX;
    float angVelocY = scaleFactor * angVelocRawY;
    float angVelocZ = scaleFactor * angVelocRawZ;

    sif::info << "Angular velocities for the L3GD20H in degrees per second:" << std::endl;
    sif::info << "X: " << angVelocX << std::endl;
    sif::info << "Y: " << angVelocY << std::endl;
    sif::info << "Z: " << angVelocZ << std::endl;

}

void SpiTestClass::acsInit() {
    GpioCookie* gpioCookie = new GpioCookie();
    GpiodRegular* gpio = nullptr;
#ifdef RASPBERRY_PI
    std::string rpiGpioName = "gpiochip0";
    gpio = new GpiodRegular(rpiGpioName, mgm0Lis3mdlChipSelect, "MGM_0_LIS3",
            gpio::Direction::OUT, 1);
    gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);

    gpio = new GpiodRegular(rpiGpioName, mgm1Rm3100ChipSelect, "MGM_1_RM3100",
            gpio::Direction::OUT, 1);
    gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);

    gpio = new GpiodRegular(rpiGpioName, gyro0AdisChipSelect, "GYRO_0_ADIS",
            gpio::Direction::OUT, 1);
    gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);

    gpio = new GpiodRegular(rpiGpioName, gyro1L3gd20ChipSelect, "GYRO_1_L3G",
            gpio::Direction::OUT, 1);
    gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);

    gpio = new GpiodRegular(rpiGpioName, gyro2L3gd20ChipSelect, "GYRO_2_L3G",
            gpio::Direction::OUT, 1);
    gpioCookie->addGpio(gpioIds::GYRO_2_L3G_CS, gpio);

    gpio = new GpiodRegular(rpiGpioName, mgm2Lis3mdlChipSelect, "MGM_2_LIS3",
            gpio::Direction::OUT, 1);
    gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);

    gpio = new GpiodRegular(rpiGpioName, mgm3Rm3100ChipSelect, "MGM_3_RM3100",
            gpio::Direction::OUT, 1);
    gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
#elif defined(XIPHOS_Q7S)
    std::string q7sGpioName5 = "gpiochip5";
    std::string q7sGpioName6 = "gpiochip6";

    gpio = new GpiodRegular(q7sGpioName5, mgm0Lis3mdlChipSelect, "MGM_0_LIS3",
            gpio::Direction::OUT, gpio::HIGH);
    gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);

    gpio = new GpiodRegular(q7sGpioName5, mgm1Rm3100ChipSelect, "MGM_1_RM3100",
            gpio::Direction::OUT, gpio::HIGH);
    gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);

    gpio = new GpiodRegular(q7sGpioName5, gyro0AdisChipSelect, "GYRO_0_ADIS",
            gpio::Direction::OUT, gpio::HIGH);
    gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);

    gpio = new GpiodRegular(q7sGpioName5, gyro1L3gd20ChipSelect, "GYRO_1_L3G",
            gpio::Direction::OUT, gpio::HIGH);
    gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);

    gpio = new GpiodRegular(q7sGpioName5, gyro2L3gd20ChipSelect, "GYRO_2_L3G",
            gpio::Direction::OUT, gpio::HIGH);
    gpioCookie->addGpio(gpioIds::GYRO_3_L3G_CS, gpio);

    gpio = new GpiodRegular(q7sGpioName6, mgm2Lis3mdlChipSelect, "MGM_2_LIS3",
            gpio::Direction::OUT, gpio::HIGH);
    gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);

    gpio = new GpiodRegular(q7sGpioName5, mgm3Rm3100ChipSelect, "MGM_3_RM3100",
            gpio::Direction::OUT, gpio::HIGH);
    gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
#endif
    if(gpioIF != nullptr) {
        gpioIF->addGpios(gpioCookie);
    }
}

void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) {
    int mode_test = SPI_MODE_3;
    int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//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!");
    }
}

void SpiTestClass::writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value) {
    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::writeRegister: Write failed");
    }
    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
        gpioIF->pullHigh(chipSelect);
    }
}

void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value,
        bool autoIncrement) {
    if(autoIncrement) {
        reg |= STM_AUTO_INCR_MASK;
    }
    writeRegister(fd, chipSelect, reg, value);
}

void SpiTestClass::writeMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg,
        uint8_t *values, size_t len) {
    if(values == nullptr) {
        return;
    }

    reg |= STM_AUTO_INCR_MASK;
    /* Clear read mask */
    reg &= ~STM_READ_MASK;
    writeMultipleRegisters(fd, chipSelect, reg, values, len);

}

void SpiTestClass::writeMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg,
        uint8_t *values, size_t len) {
    if(values == nullptr) {
        return;
    }

    sendBuffer[0] = reg;
    std::memcpy(sendBuffer.data() + 1, values, len);
    spiTransferStruct.len = len + 1;

    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::readRegister: Read failed");
    }
    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
        gpioIF->pullHigh(chipSelect);
    }
}

uint8_t SpiTestClass::readRm3100Register(int fd, gpioId_t chipSelect, uint8_t reg) {
    return readStmRegister(fd, chipSelect, reg, false);
}


void SpiTestClass::readMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *reply,
        size_t len) {
    reg |= STM_AUTO_INCR_MASK;
    readMultipleRegisters(fd, chipSelect, reg, reply, len);
}

void SpiTestClass::readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *reply,
        size_t len) {
    if(reply == nullptr) {
        return;
    }

    spiTransferStruct.len = len + 1;
    sendBuffer[0] = reg | STM_READ_MASK;

    for(uint8_t idx = 0; idx < len ; idx ++) {
        sendBuffer[idx + 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::readRegister: Read failed");
    }
    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
        gpioIF->pullHigh(chipSelect);
    }
    std::memcpy(reply, recvBuffer.data() + 1, len);
}

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;
    }
    return readRegister(fd, chipSelect, reg);
}


uint8_t SpiTestClass::readRegister(int fd, gpioId_t chipSelect, uint8_t reg) {
    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::readRegister: Read failed");
    }
    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
        gpioIF->pullHigh(chipSelect);
    }
    return recvBuffer[1];
}