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

#include <fsfw/globalfunctions/bitutility.h>
#include <fsfw/devicehandlers/DeviceHandlerMessage.h>
#include <fsfw/objectmanager/SystemObjectIF.h>
#include <fsfw/returnvalues/HasReturnvaluesIF.h>


MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId,
        object_id_t deviceCommunication, CookieIF* comCookie):
        DeviceHandlerBase(objectId, deviceCommunication, comCookie),
        primaryDataset(this) {
#if OBSW_VERBOSE_LEVEL >= 1
    debugDivider = new PeriodicOperationDivider(10);
#endif
}

MGMHandlerRM3100::~MGMHandlerRM3100() {}

void MGMHandlerRM3100::doStartUp() {
    switch(internalState) {
    case(InternalState::STATE_NONE): {
        internalState = InternalState::STATE_CONFIGURE_CMM;
        break;
    }
    case(InternalState::STATE_CONFIGURE_CMM): {
        internalState = InternalState::STATE_READ_CMM;
        break;
    }
    case(InternalState::STATE_READ_CMM): {
        if(commandExecuted) {
            internalState = InternalState::STATE_CONFIGURE_TMRC;
        }
        break;
    }
    case(InternalState::STATE_CONFIGURE_TMRC): {
        internalState = InternalState::STATE_READ_TMRC;
        break;
    }
    case(InternalState::STATE_READ_TMRC): {
        if(commandExecuted) {
            internalState = InternalState::STATE_NORMAL;
            //setMode(_MODE_TO_ON);
            setMode(MODE_NORMAL);
        }
        break;
    }
    default: {
        break;
    }
    }
}

void MGMHandlerRM3100::doShutDown() {
    setMode(_MODE_POWER_DOWN);
}

ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand(
        DeviceCommandId_t *id) {
    switch(internalState) {
    case(InternalState::STATE_NONE):
    case(InternalState::STATE_NORMAL): {
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(InternalState::STATE_CONFIGURE_CMM): {
        *id = RM3100::CONFIGURE_CMM;
        break;
    }
    case(InternalState::STATE_READ_CMM): {
        *id = RM3100::READ_CMM;
        break;
    }
    case(InternalState::STATE_CONFIGURE_TMRC): {
        *id = RM3100::CONFIGURE_TMRC;
        break;
    }
    case(InternalState::STATE_READ_TMRC): {
        *id = RM3100::READ_TMRC;
        break;
    }
    default:
        // might be a configuration error.
        sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown "
        << "internal state!" << std::endl;
        return HasReturnvaluesIF::RETURN_OK;
    }

    return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t MGMHandlerRM3100::buildCommandFromCommand(
        DeviceCommandId_t deviceCommand, const uint8_t *commandData,
        size_t commandDataLen) {
    switch(deviceCommand) {
    case(RM3100::CONFIGURE_CMM): {
        commandBuffer[0] = RM3100::CMM_REGISTER;
        commandBuffer[1] = RM3100::CMM_VALUE;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        break;
    }
    case(RM3100::READ_CMM): {
        commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK;
        commandBuffer[1] = 0;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        break;
    }
    case(RM3100::CONFIGURE_TMRC): {
        return handleTmrcConfigCommand(deviceCommand, commandData,
                commandDataLen);
    }
    case(RM3100::READ_TMRC): {
        commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK;
        commandBuffer[1] = 0;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        break;
    }
    case(RM3100::CONFIGURE_CYCLE_COUNT): {
        return handleCycleCountConfigCommand(deviceCommand, commandData,
                commandDataLen);
    }
    case(RM3100::READ_CYCLE_COUNT): {
        commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER | RM3100::READ_MASK;
        std::memset(commandBuffer + 1, 0, 6);
        rawPacket = commandBuffer;
        rawPacketLen = 7;
        break;
    }
    case(RM3100::READ_DATA): {
        commandBuffer[0] = RM3100::MEASUREMENT_REG_START | RM3100::READ_MASK;
        std::memset(commandBuffer + 1, 0,  9);
        rawPacketLen = 10;
        break;
    }
    default:
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
    return RETURN_OK;
}

ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand(
        DeviceCommandId_t *id) {
    *id = RM3100::READ_DATA;
    return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start,
        size_t len, DeviceCommandId_t *foundId,
        size_t *foundLen) {

    /* For SPI, ID will always be the one of the last sent command. */
    *foundId = this->getPendingCommand();
    *foundLen = len;
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
        DeviceCommandId_t id, const uint8_t *packet) {
    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
    switch(id) {
    case(RM3100::CONFIGURE_CMM):
    case(RM3100::CONFIGURE_CYCLE_COUNT):
    case(RM3100::CONFIGURE_TMRC): {
        /* We can only check whether write was successful with read operation. */
        break;
    }
    case(RM3100::READ_CMM): {
        uint8_t cmmValue = packet[1];
        /* We clear the seventh bit in any case
         * because this one is zero sometimes for some reason */
        bitutil::bitClear(&cmmValue, 6);
        if(cmmValue == cmmRegValue) {
            commandExecuted = true;
        }
        else {
            /* Attempt reconfiguration. */
            internalState = InternalState::STATE_CONFIGURE_CMM;
            return DeviceHandlerIF::DEVICE_REPLY_INVALID;
        }
        break;
    }
    case(RM3100::READ_TMRC): {
        if(packet[1] == tmrcRegValue) {
            commandExecuted = true;
            /* Reading TMRC was commanded. Trigger event to inform ground. */
            if(mode != _MODE_START_UP) {
                triggerEvent(tmrcSet, tmrcRegValue, 0);
            }
        }
        else {
            /* Attempt reconfiguration. */
            internalState = InternalState::STATE_CONFIGURE_TMRC;
            return DeviceHandlerIF::DEVICE_REPLY_INVALID;
        }
        break;
    }
    case(RM3100::READ_CYCLE_COUNT): {
        uint16_t cycleCountX = packet[1] << 8 | packet[2];
        uint16_t cycleCountY = packet[3] << 8 | packet[4];
        uint16_t cycleCountZ = packet[5] << 8 | packet[6];
        if(cycleCountX != cycleCountRegValueX or cycleCountY != cycleCountRegValueY or
                cycleCountZ != cycleCountRegValueZ) {
            return DeviceHandlerIF::DEVICE_REPLY_INVALID;
        }
        /* Reading TMRC was commanded. Trigger event to inform ground. */
        if(mode != _MODE_START_UP) {
            uint32_t eventParam1 = (cycleCountX << 16) | cycleCountY;
            triggerEvent(cycleCountersSet, eventParam1, cycleCountZ);
        }
        break;
    }
    case(RM3100::READ_DATA): {
        result = handleDataReadout(packet);
        break;
    }
    default:
        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }

    return result;
}

ReturnValue_t MGMHandlerRM3100::handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand,
        const uint8_t *commandData, size_t commandDataLen) {
    if(commandData == nullptr) {
        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
    }

    // Set cycle count
    if(commandDataLen == 2) {
        handleCycleCommand(true, commandData, commandDataLen);
    }
    else if(commandDataLen == 6) {
        handleCycleCommand(false, commandData, commandDataLen);
    }
    else {
        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
    }

    commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE;
    std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2);
    std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2);
    std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2);
    rawPacketLen = 7;
    rawPacket = commandBuffer;
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t MGMHandlerRM3100::handleCycleCommand(bool oneCycleValue,
        const uint8_t *commandData, size_t commandDataLen) {
    RM3100::CycleCountCommand command(oneCycleValue);
    ReturnValue_t result = command.deSerialize(&commandData, &commandDataLen,
            SerializeIF::Endianness::BIG);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        return result;
    }

    /* Data sheet p.30 "while noise limits the useful upper range to ~400 cycle counts." */
    if(command.cycleCountX > 450 ) {
        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
    }

    if(not oneCycleValue and (command.cycleCountY > 450 or command.cycleCountZ > 450)) {
        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
    }

    cycleCountRegValueX = command.cycleCountX;
    cycleCountRegValueY = command.cycleCountY;
    cycleCountRegValueZ = command.cycleCountZ;
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand(
        DeviceCommandId_t deviceCommand, const uint8_t *commandData,
        size_t commandDataLen) {
    if(commandData == nullptr) {
        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
    }

    if(commandDataLen != 1) {
        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
    }

    commandBuffer[0] = RM3100::TMRC_REGISTER;
    commandBuffer[1] = commandData[1];
    rawPacketLen = 2;
    rawPacket = commandBuffer;
    return HasReturnvaluesIF::RETURN_OK;
}

void MGMHandlerRM3100::fillCommandAndReplyMap() {
    insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1);
    insertInCommandAndReplyMap(RM3100::READ_CMM, 1);

    insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1);
    insertInCommandAndReplyMap(RM3100::READ_TMRC, 1);

    insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1);
    insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1);

    insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset);
}

void MGMHandlerRM3100::modeChanged(void) {
    internalState = InternalState::STATE_NONE;
}

ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool(
        localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
    localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_X, new PoolEntry<float>({0.0}));
    localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Y, new PoolEntry<float>({0.0}));
    localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Z, new PoolEntry<float>({0.0}));
    return HasReturnvaluesIF::RETURN_OK;
}

uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) {
    return 60000;
}

ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) {
    /* Analyze data here. The sensor generates 24 bit signed values so we need to do some bitshift
     * trickery here to calculate the raw values first */
    int32_t fieldStrengthRawX = ((packet[1] << 24) | (packet[2] << 16) | (packet[3] << 8)) >> 8;
    int32_t fieldStrengthRawY = ((packet[4] << 24) | (packet[5] << 16) | (packet[6] << 8)) >> 8;
    int32_t fieldStrengthRawZ  = ((packet[7] << 24) | (packet[8] << 16) | (packet[3] << 8)) >> 8;

    /* Now scale to physical value in microtesla */
    float fieldStrengthX = fieldStrengthRawX * scaleFactorX;
    float fieldStrengthY = fieldStrengthRawY * scaleFactorX;
    float fieldStrengthZ = fieldStrengthRawZ * scaleFactorX;

#if OBSW_VERBOSE_LEVEL >= 1
    if(debugDivider->checkAndIncrement()) {
        sif::info << "MGMHandlerLIS3: Magnetic field strength in"
                " microtesla:" << std::endl;
        /* Set terminal to utf-8 if there is an issue with micro printout. */
        sif::info << "X: " << fieldStrengthX << " \xC2\xB5T" << std::endl;
        sif::info << "Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl;
        sif::info << "Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl;
    }
#endif

    /* TODO: Sanity check on values */
    PoolReadGuard readGuard(&primaryDataset);
    if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
        primaryDataset.fieldStrengthX = fieldStrengthX;
        primaryDataset.fieldStrengthY = fieldStrengthY;
        primaryDataset.fieldStrengthZ = fieldStrengthZ;
        primaryDataset.setValidity(true, true);
    }
    return RETURN_OK;
}