#include "GyroADIS16507Handler.h"
#include <fsfw/action/HasActionsIF.h>
#include <fsfw/datapool/PoolReadGuard.h>

#if OBSW_ADIS16507_LINUX_COM_IF == 1
#include "fsfw_hal/linux/utility.h"
#include "fsfw_hal/linux/spi/SpiCookie.h"
#include "fsfw_hal/linux/spi/SpiComIF.h"
#include "fsfw_hal/linux/UnixFileGuard.h"
#include <sys/ioctl.h>
#include <unistd.h>
#endif

GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
        object_id_t deviceCommunication, CookieIF * comCookie):
        DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
        configDataset(this), breakCountdown() {
#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1
    debugDivider = new PeriodicOperationDivider(5);
#endif

#if OBSW_ADIS16507_LINUX_COM_IF == 1
    SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
    if(cookie != nullptr) {
        cookie->setCallbackMode(&spiSendCallback, this);
    }
#endif
}

void GyroADIS16507Handler::doStartUp() {
    // Initial 310 ms start up time after power-up
    if(internalState == InternalState::STARTUP) {
        if(not commandExecuted) {
            breakCountdown.setTimeout(ADIS16507::START_UP_TIME);
            commandExecuted = true;
        }
        if(breakCountdown.hasTimedOut()) {
            internalState = InternalState::CONFIG;
            commandExecuted = false;
        }
    }

    // Read all configuration registers first
    if(internalState == InternalState::CONFIG) {
        if(commandExecuted) {
            commandExecuted = false;
            internalState = InternalState::IDLE;
        }
    }

    if(internalState == InternalState::IDLE) {
        setMode(MODE_NORMAL);
        // setMode(MODE_ON);
    }
}

void GyroADIS16507Handler::doShutDown() {
    commandExecuted = false;
    setMode(_MODE_POWER_DOWN);
}

ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
    *id = ADIS16507::READ_SENSOR_DATA;
    return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
    switch(internalState) {
    case(InternalState::CONFIG): {
        *id = ADIS16507::READ_OUT_CONFIG;
        buildCommandFromCommand(*id, nullptr, 0);
        break;
    }
    case(InternalState::STARTUP): {
        return NOTHING_TO_SEND;
        break;
    }
    default: {
        /* Might be a configuration error. */
        sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
                "Unknown internal state!" << std::endl;
        return HasReturnvaluesIF::RETURN_OK;
    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
        const uint8_t *commandData, size_t commandDataLen) {
    switch(deviceCommand) {
    case(ADIS16507::READ_OUT_CONFIG): {
        this->rawPacketLen = ADIS16507::CONFIG_READOUT_SIZE;
        uint8_t regList[5];
        regList[0] = ADIS16507::DIAG_STAT_REG;
        regList[1] = ADIS16507::FILTER_CTRL_REG;
        regList[2] = ADIS16507::MSC_CTRL_REG;
        regList[3] = ADIS16507::DEC_RATE_REG;
        regList[4] = ADIS16507::PROD_ID_REG;
        prepareReadCommand(regList, sizeof(regList));
        this->rawPacket = commandBuffer.data();
        break;
    }
    case(ADIS16507::READ_SENSOR_DATA): {
        if(breakCountdown.isBusy()) {
            // A glob command is pending and sensor data can't be read currently
            return NO_REPLY_EXPECTED;
        }
        std::memcpy(commandBuffer.data(), ADIS16507::BURST_READ_ENABLE.data(),
                ADIS16507::BURST_READ_ENABLE.size());
        std::memset(commandBuffer.data() + 2, 0, 10 * 2);
        this->rawPacketLen = ADIS16507::SENSOR_READOUT_SIZE;
        this->rawPacket = commandBuffer.data();
        break;
    }
    case(ADIS16507::SELF_TEST_SENSORS): {
        if(breakCountdown.isBusy()) {
            // Another glob command is pending
            return HasActionsIF::IS_BUSY;
        }
        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SENSOR_SELF_TEST, 0x00);
        breakCountdown.setTimeout(ADIS16507::SELF_TEST_BREAK);
        break;
    }
    case(ADIS16507::SELF_TEST_MEMORY): {
        if(breakCountdown.isBusy()) {
            // Another glob command is pending
            return HasActionsIF::IS_BUSY;
        }
        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_TEST, 0x00);
        breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_TEST_BREAK);
        break;
    }
    case(ADIS16507::UPDATE_NV_CONFIGURATION): {
        if(breakCountdown.isBusy()) {
            // Another glob command is pending
            return HasActionsIF::IS_BUSY;
        }
        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
        breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_UPDATE_BREAK);
        break;
    }
    case(ADIS16507::RESET_SENSOR_CONFIGURATION): {
        if(breakCountdown.isBusy()) {
            // Another glob command is pending
            return HasActionsIF::IS_BUSY;
        }
        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FACTORY_CALIBRATION, 0x00);
        breakCountdown.setTimeout(ADIS16507::FACTORY_CALIBRATION_BREAK);
        break;
    }
    case(ADIS16507::SW_RESET): {
        if(breakCountdown.isBusy()) {
            // Another glob command is pending
            return HasActionsIF::IS_BUSY;
        }
        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SOFTWARE_RESET, 0x00);
        breakCountdown.setTimeout(ADIS16507::SW_RESET_BREAK);
        break;
    }
    case(ADIS16507::PRINT_CURRENT_CONFIGURATION): {
#if OBSW_VERBOSE_LEVEL >= 1
        PoolReadGuard pg(&configDataset);
        sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) <<
                std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
        sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x" <<
                configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x" <<
                configDataset.filterSetting.value << " | DEC_RATE: 0x" <<
                configDataset.decRateReg.value << std::setfill(' ') << std::endl;
#endif /* OBSW_VERBOSE_LEVEL >= 1 */
    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

void GyroADIS16507Handler::fillCommandAndReplyMap() {
    insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset);
    insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
    insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 1);
    insertInCommandAndReplyMap(ADIS16507::SELF_TEST_MEMORY, 1);
    insertInCommandAndReplyMap(ADIS16507::UPDATE_NV_CONFIGURATION, 1);
    insertInCommandAndReplyMap(ADIS16507::RESET_SENSOR_CONFIGURATION, 1);
    insertInCommandAndReplyMap(ADIS16507::SW_RESET, 1);
    insertInCommandAndReplyMap(ADIS16507::PRINT_CURRENT_CONFIGURATION, 1);
}

ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
        DeviceCommandId_t *foundId, size_t *foundLen) {
    /* For SPI, the ID will always be the one of the last sent command. */
    *foundId = this->getPendingCommand();
    *foundLen = this->rawPacketLen;

    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
        const uint8_t *packet) {
    switch(id) {
    case(ADIS16507::READ_OUT_CONFIG): {
        PoolReadGuard rg(&configDataset);
        uint16_t readProdId = packet[10] << 8 | packet[11];
        if (readProdId != ADIS16507::PROD_ID) {
#if OBSW_VERBOSE_LEVEL >= 1
            sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
                    << std::endl;
#endif
            return HasReturnvaluesIF::RETURN_FAILED;
        }
        configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
        configDataset.filterSetting.value =  packet[4] << 8 | packet[5];
        configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7];
        configDataset.decRateReg.value = packet[8] << 8 | packet[9];
        configDataset.setValidity(true, true);
        if(internalState == InternalState::CONFIG) {
            commandExecuted = true;
        }
        break;
    }
    case(ADIS16507::READ_SENSOR_DATA): {
        return handleSensorData(packet);
    }

    }
    return HasReturnvaluesIF::RETURN_OK;
}


ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
    BurstModes burstMode = getBurstMode();
    switch(burstMode) {
    case(BurstModes::BURST_16_BURST_SEL_1):
    case(BurstModes::BURST_32_BURST_SEL_1): {
        sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Analysis with BURST_SEL1"
                " not implemented!" << std::endl;
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(BurstModes::BURST_16_BURST_SEL_0): {
        uint16_t checksum = packet[20] << 8 | packet[21];
        /* Now verify the read checksum with the expected checksum
        according to datasheet p. 20 */
        uint16_t calcChecksum = 0;
        for(size_t idx = 2; idx < 20; idx ++) {
            calcChecksum += packet[idx];
        }
        if(checksum != calcChecksum) {
#if OBSW_VERBOSE_LEVEL >= 1
            sif::warning << "GyroADIS16507Handler::interpretDeviceReply: "
                    "Invalid checksum detected!" << std::endl;
#endif
            return HasReturnvaluesIF::RETURN_FAILED;
        }

        ReturnValue_t result = configDataset.diagStatReg.read();
        if(result == HasReturnvaluesIF::RETURN_OK) {
            configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
            configDataset.diagStatReg.setValid(true);
        }
        configDataset.diagStatReg.commit();

        {
            PoolReadGuard pg(&primaryDataset);
            int16_t angVelocXRaw = packet[4] << 8 | packet[5];
            primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
                    ADIS16507::GYRO_RANGE;
            int16_t angVelocYRaw = packet[6] << 8 | packet[7];
            primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
                    ADIS16507::GYRO_RANGE;
            int16_t angVelocZRaw = packet[8] << 8 | packet[9];
            primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
                    ADIS16507::GYRO_RANGE;

            int16_t accelXRaw = packet[10] << 8 | packet[11];
            primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
                    ADIS16507::ACCELEROMETER_RANGE;
            int16_t accelYRaw = packet[12] << 8 | packet[13];
            primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
                    ADIS16507::ACCELEROMETER_RANGE;
            int16_t accelZRaw = packet[14] << 8 | packet[15];
            primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
                    ADIS16507::ACCELEROMETER_RANGE;

            int16_t temperatureRaw = packet[16] << 8 | packet[17];
            primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
            // Ignore data counter for now
            primaryDataset.setValidity(true, true);
        }

#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1
        if(debugDivider->checkAndIncrement()) {
            sif::info << "GyroADIS16507Handler: Angular velocities in deg / s" << std::endl;
            sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
            sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
            sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
            sif::info << "GyroADIS16507Handler: Accelerations in m / s^2: " << std::endl;
            sif::info << "X: " << primaryDataset.accelX.value << std::endl;
            sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
            sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
        }
#endif

        break;
    }
    case(BurstModes::BURST_32_BURST_SEL_0): {
        break;
    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
    return 5000;
}

void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
        uint8_t valueTwo) {
    uint8_t secondReg = startReg + 1;
    startReg |= ADIS16507::WRITE_MASK;
    secondReg |= ADIS16507::WRITE_MASK;
    commandBuffer[0] = startReg;
    commandBuffer[1] = valueOne;
    commandBuffer[2] = secondReg;
    commandBuffer[3] = valueTwo;
    this->rawPacketLen = 4;
    this->rawPacket = commandBuffer.data();
}

void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
    for(size_t idx = 0; idx < len; idx++) {
        commandBuffer[idx * 2] = regList[idx];
        commandBuffer[idx * 2 + 1] = 0x00;
    }
    commandBuffer[len * 2] = 0x00;
    commandBuffer[len * 2 + 1] = 0x00;
}

ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
        LocalDataPoolManager &poolManager) {
    localDataPoolMap.emplace(ADIS16507::ANG_VELOC_X, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(ADIS16507::ACCELERATION_X, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(ADIS16507::ACCELERATION_Y, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(ADIS16507::ACCELERATION_Z, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(ADIS16507::TEMPERATURE, new PoolEntry<float>({0.0}));

    localDataPoolMap.emplace(ADIS16507::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
    localDataPoolMap.emplace(ADIS16507::FILTER_SETTINGS, new PoolEntry<uint8_t>());
    localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
    localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
    return HasReturnvaluesIF::RETURN_OK;
}

GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
    configDataset.mscCtrlReg.read();
    uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
    configDataset.mscCtrlReg.commit();
    if((currentCtrlReg & ADIS16507::BURST_32_BIT) == ADIS16507::BURST_32_BIT) {
        if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
            return BurstModes::BURST_32_BURST_SEL_1;
        }
        else {
            return BurstModes::BURST_32_BURST_SEL_0;
        }
    }
    else {
        if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
            return BurstModes::BURST_16_BURST_SEL_1;
        }
        else {
            return BurstModes::BURST_16_BURST_SEL_0;
        }
    }
}

#if OBSW_ADIS16507_LINUX_COM_IF == 1

ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
        const uint8_t *sendData, size_t sendLen, void *args) {
    GyroADIS16507Handler* handler = reinterpret_cast<GyroADIS16507Handler*>(args);
    if(handler == nullptr) {
        sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
                << std::endl;
        return HasReturnvaluesIF::RETURN_FAILED;
    }
    DeviceCommandId_t currentCommand = handler->getPendingCommand();
    switch(currentCommand) {
    case(ADIS16507::READ_SENSOR_DATA): {
        return comIf->performRegularSendOperation(cookie, sendData, sendLen);
    }
    case(ADIS16507::READ_OUT_CONFIG):
    default: {
        ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
        int retval = 0;
        // Prepare transfer
        int fileDescriptor = 0;
        std::string device = cookie->getSpiDevice();
        UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
        if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
            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);
        cookie->assignWriteBuffer(sendData);
        cookie->setTransferSize(2);

        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) {
#if OBSW_VERBOSE_LEVEL >= 1
            sif::warning << "GyroADIS16507Handler::spiSendCallback: "
                    "Mutex or GPIO interface invalid" << std::endl;
            return HasReturnvaluesIF::RETURN_FAILED;
#endif
        }

        if(gpioId != gpio::NO_GPIO) {
            result = mutex->lockMutex(timeoutType, timeoutMs);
            if (result != RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
                sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
#endif
                return result;
            }
        }

        size_t idx = 0;
        while(idx < sendLen) {
            // Pull SPI CS low. For now, no support for active high given
            if(gpioId != gpio::NO_GPIO) {
                gpioIF->pullLow(gpioId);
            }

            // Execute transfer
            // Initiate a full duplex SPI transfer.
            retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
            if(retval < 0) {
                utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
                result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
            }
#if FSFW_HAL_SPI_WIRETAPPING == 1
            comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */

            if(gpioId != gpio::NO_GPIO) {
                gpioIF->pullHigh(gpioId);
            }

            idx += 2;
            if(idx < sendLen) {
                usleep(ADIS16507::STALL_TIME_MICROSECONDS);
            }
            spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
            transferStruct->tx_buf += 2;
            transferStruct->rx_buf += 2;
        }

        if(gpioId != gpio::NO_GPIO) {
            mutex->unlockMutex();
        }
    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */