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

#if OBSW_ADIS16507_LINUX_COM_IF == 1
#include "fsfw_hal/linux/spi/SpiCookie.h"
#include "fsfw_hal/linux/spi/SpiComIF.h"
#endif

GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
        object_id_t deviceCommunication, CookieIF * comCookie):
            DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
            configDataset(this) {
#if OBSW_ADIS16507_LINUX_COM_IF == 1
    SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
    if(cookie != nullptr) {
        cookie->setCallbackMode(&spiSendCallback, this);
    }
#endif
}

void GyroADIS16507Handler::doStartUp() {
    if(internalState == InternalState::STARTUP) {
        if(commandExecuted) {
            commandExecuted = false;
            internalState = InternalState::IDLE;
        }
    }
}

void GyroADIS16507Handler::doShutDown() {
}

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::STARTUP): {
        *id = ADIS16507::READ_OUT_CONFIG;
        buildCommandFromCommand(*id, nullptr, 0);
        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[4];
        regList[0] = ADIS16507::FILTER_CTRL_REG;
        regList[1] = ADIS16507::MSC_CTRL_REG;
        regList[2] = ADIS16507::DEC_RATE_REG;
        regList[3] = ADIS16507::PROD_ID_REG;
        prepareReadCommand(regList, sizeof(regList));
        this->rawPacket = commandBuffer.data();
        break;
    }
    case(ADIS16507::READ_SENSOR_DATA): {
        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;
    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

void GyroADIS16507Handler::fillCommandAndReplyMap() {
}

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[6] << 8 | packet[7];
        if (readProdId != ADIS16507::PROD_ID) {
#if OBSW_VERBOSE_LEVEL >= 1
            sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
                    << std::endl;
#endif
            return HasReturnvaluesIF::RETURN_FAILED;
        }
        configDataset.filterSetting.value =  packet[0] << 8 | packet[1];
        configDataset.mscCtrlReg.value = packet[2] << 8 | packet[3];
        configDataset.decRateReg.value = packet[4] << 8 | packet[5];
        if(internalState == InternalState::STARTUP) {
            commandExecuted = true;
        }
        break;
    }
    case(ADIS16507::READ_SENSOR_DATA): {

    }

    }
    return HasReturnvaluesIF::RETURN_OK;
}

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

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::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;
}

#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): {

    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */