#include "Max31865PT1000Handler.h"

#include "fsfw/datapool/PoolReadGuard.h"

#include <bitset>
#include <cmath>

Max31865PT1000Handler::Max31865PT1000Handler(object_id_t objectId, object_id_t comIF,
        CookieIF *comCookie):
        DeviceHandlerBase(objectId, comIF, comCookie), sensorDataset(this),
        sensorDatasetSid(sensorDataset.getSid()) {
#if OBSW_VERBOSE_LEVEL >= 1
    debugDivider = new PeriodicOperationDivider(0);
#endif
}

Max31865PT1000Handler::~Max31865PT1000Handler() {
}

void Max31865PT1000Handler::doStartUp() {
    if(internalState == InternalState::NONE) {
        internalState = InternalState::WARMUP;
        Clock::getUptime(&startTime);
    }

    if(internalState == InternalState::WARMUP) {
        dur_millis_t timeNow = 0;
        Clock::getUptime(&timeNow);
        if(timeNow - startTime >= 100) {
            internalState = InternalState::CONFIGURE;
        }
    }

    if(internalState == InternalState::CONFIGURE) {
        if(commandExecuted) {
            commandExecuted = false;
            internalState = InternalState::REQUEST_CONFIG;
        }
    }

    if(internalState == InternalState::REQUEST_CONFIG) {
        if (commandExecuted) {
            commandExecuted = false;
            internalState = InternalState::CONFIG_HIGH_THRESHOLD;
        }
    }

    if(internalState == InternalState::CONFIG_HIGH_THRESHOLD) {
        if(commandExecuted) {
            internalState = InternalState::REQUEST_HIGH_THRESHOLD;
            commandExecuted = false;
        }
    }

    if(internalState == InternalState::REQUEST_HIGH_THRESHOLD) {
        if(commandExecuted) {
            internalState = InternalState::CONFIG_LOW_THRESHOLD;
            commandExecuted = false;
        }
    }

    if(internalState == InternalState::CONFIG_LOW_THRESHOLD) {
        if(commandExecuted) {
            internalState = InternalState::REQUEST_LOW_THRESHOLD;
            commandExecuted = false;
        }
    }

    if(internalState == InternalState::REQUEST_LOW_THRESHOLD) {
        if(commandExecuted) {
            setMode(MODE_ON);
            setMode(MODE_NORMAL);
            internalState = InternalState::RUNNING;
            commandExecuted = false;
        }
    }
}

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

ReturnValue_t Max31865PT1000Handler::buildNormalDeviceCommand(
        DeviceCommandId_t *id) {
    if(internalState == InternalState::RUNNING) {
        *id = Max31865Definitions::REQUEST_RTD;
        return buildCommandFromCommand(*id, nullptr, 0);
    }
    else if(internalState == InternalState::REQUEST_FAULT_BYTE) {
        *id = Max31865Definitions::REQUEST_FAULT_BYTE;
        return buildCommandFromCommand(*id, nullptr, 0);
    }
    else if(internalState == InternalState::CLEAR_FAULT_BYTE) {
        *id = Max31865Definitions::CLEAR_FAULT_BYTE;
        return buildCommandFromCommand(*id, nullptr, 0);
    }
    else {
        return DeviceHandlerBase::NOTHING_TO_SEND;
    }
}

ReturnValue_t Max31865PT1000Handler::buildTransitionDeviceCommand(
        DeviceCommandId_t *id) {
    switch(internalState) {
    case(InternalState::NONE):
    case(InternalState::WARMUP):
    case(InternalState::RUNNING):
    return DeviceHandlerBase::NOTHING_TO_SEND;
    case(InternalState::CONFIGURE): {
        *id = Max31865Definitions::CONFIG_CMD;
        uint8_t config[1] = {DEFAULT_CONFIG};
        return buildCommandFromCommand(*id, config, 1);
    }
    case(InternalState::REQUEST_CONFIG): {
        *id = Max31865Definitions::REQUEST_CONFIG;
        return buildCommandFromCommand(*id, nullptr, 0);
    }
    case(InternalState::CONFIG_HIGH_THRESHOLD): {
        *id = Max31865Definitions::WRITE_HIGH_THRESHOLD;
        return buildCommandFromCommand(*id, nullptr, 0);
    }
    case(InternalState::REQUEST_HIGH_THRESHOLD): {
        *id = Max31865Definitions::REQUEST_HIGH_THRESHOLD;
        return buildCommandFromCommand(*id, nullptr, 0);
    }
    case(InternalState::CONFIG_LOW_THRESHOLD): {
        *id = Max31865Definitions::WRITE_LOW_THRESHOLD;
        return buildCommandFromCommand(*id, nullptr, 0);
    }
    case(InternalState::REQUEST_LOW_THRESHOLD): {
        *id = Max31865Definitions::REQUEST_LOW_THRESHOLD;
        return buildCommandFromCommand(*id, nullptr, 0);
    }

    default:
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::error << "Max31865PT1000Handler: Invalid internal state" << std::endl;
#else
        sif::printError("Max31865PT1000Handler: Invalid internal state\n");
#endif
        return HasReturnvaluesIF::RETURN_FAILED;
    }
}

ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(
        DeviceCommandId_t deviceCommand, const uint8_t *commandData,
        size_t commandDataLen) {
    switch(deviceCommand) {
    case(Max31865Definitions::CONFIG_CMD) : {
        commandBuffer[0] = static_cast<uint8_t>(Max31865Definitions::CONFIG_CMD);
        if(commandDataLen == 1) {
            commandBuffer[1] = commandData[0];
            DeviceHandlerBase::rawPacketLen = 2;
            DeviceHandlerBase::rawPacket = commandBuffer.data();
            return HasReturnvaluesIF::RETURN_OK;
        }
        else {
            return DeviceHandlerIF::NO_COMMAND_DATA;
        }
    }
    case(Max31865Definitions::CLEAR_FAULT_BYTE): {
        commandBuffer[0] = static_cast<uint8_t>(Max31865Definitions::CONFIG_CMD);
        commandBuffer[1] = Max31865Definitions::CLEAR_FAULT_BIT_VAL;
        DeviceHandlerBase::rawPacketLen = 2;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Max31865Definitions::REQUEST_CONFIG): {
        commandBuffer[0] = static_cast<uint8_t>(
                Max31865Definitions::REQUEST_CONFIG);
        commandBuffer[1] = 0x00; // dummy byte
        DeviceHandlerBase::rawPacketLen = 2;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Max31865Definitions::WRITE_HIGH_THRESHOLD): {
        commandBuffer[0] = static_cast<uint8_t>(
                Max31865Definitions::WRITE_HIGH_THRESHOLD);
        commandBuffer[1] = static_cast<uint8_t>(HIGH_THRESHOLD >> 8);
        commandBuffer[2] = static_cast<uint8_t>(HIGH_THRESHOLD & 0xFF);
        DeviceHandlerBase::rawPacketLen = 3;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Max31865Definitions::REQUEST_HIGH_THRESHOLD): {
        commandBuffer[0] = static_cast<uint8_t>(
                Max31865Definitions::REQUEST_HIGH_THRESHOLD);
        commandBuffer[1] = 0x00; //dummy byte
        commandBuffer[2] = 0x00; //dummy byte
        DeviceHandlerBase::rawPacketLen = 3;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Max31865Definitions::WRITE_LOW_THRESHOLD): {
        commandBuffer[0] = static_cast<uint8_t>(
                Max31865Definitions::WRITE_LOW_THRESHOLD);
        commandBuffer[1] = static_cast<uint8_t>(LOW_THRESHOLD >> 8);
        commandBuffer[2] = static_cast<uint8_t>(LOW_THRESHOLD & 0xFF);
        DeviceHandlerBase::rawPacketLen = 3;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Max31865Definitions::REQUEST_LOW_THRESHOLD): {
        commandBuffer[0] = static_cast<uint8_t>(
                Max31865Definitions::REQUEST_LOW_THRESHOLD);
        commandBuffer[1] = 0x00; //dummy byte
        commandBuffer[2] = 0x00; //dummy byte
        DeviceHandlerBase::rawPacketLen = 3;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Max31865Definitions::REQUEST_RTD): {
        commandBuffer[0] = static_cast<uint8_t>(
                Max31865Definitions::REQUEST_RTD);
        // two dummy bytes
        commandBuffer[1] = 0x00;
        commandBuffer[2] = 0x00;
        DeviceHandlerBase::rawPacketLen = 3;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Max31865Definitions::REQUEST_FAULT_BYTE): {
        commandBuffer[0] = static_cast<uint8_t>(
                Max31865Definitions::REQUEST_FAULT_BYTE);
        commandBuffer[1] = 0x00;
        DeviceHandlerBase::rawPacketLen = 2;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return HasReturnvaluesIF::RETURN_OK;
    }
    default:
        //Unknown DeviceCommand
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
}

void Max31865PT1000Handler::fillCommandAndReplyMap() {
    insertInCommandAndReplyMap(Max31865Definitions::CONFIG_CMD, 3);
    insertInCommandAndReplyMap(Max31865Definitions::REQUEST_CONFIG, 3);
    insertInCommandAndReplyMap(Max31865Definitions::WRITE_LOW_THRESHOLD, 3);
    insertInCommandAndReplyMap(Max31865Definitions::REQUEST_LOW_THRESHOLD, 3);
    insertInCommandAndReplyMap(Max31865Definitions::WRITE_HIGH_THRESHOLD, 3);
    insertInCommandAndReplyMap(Max31865Definitions::REQUEST_HIGH_THRESHOLD, 3);
    insertInCommandAndReplyMap(Max31865Definitions::REQUEST_RTD, 3,
            &sensorDataset);
    insertInCommandAndReplyMap(Max31865Definitions::REQUEST_FAULT_BYTE, 3);
    insertInCommandAndReplyMap(Max31865Definitions::CLEAR_FAULT_BYTE, 3);
}

ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start,
        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
    size_t rtdReplySize = 3;
    size_t configReplySize = 2;

    if(remainingSize == rtdReplySize and
            internalState == InternalState::RUNNING) {
        *foundId = Max31865Definitions::REQUEST_RTD;
        *foundLen = rtdReplySize;
        return RETURN_OK;
    }

    if(remainingSize == 3) {
        switch(internalState) {
        case(InternalState::CONFIG_HIGH_THRESHOLD): {
            *foundLen = 3;
            *foundId = Max31865Definitions::WRITE_HIGH_THRESHOLD;
            commandExecuted = true;
            return RETURN_OK;
        }
        case(InternalState::REQUEST_HIGH_THRESHOLD): {
            *foundLen = 3;
            *foundId = Max31865Definitions::REQUEST_HIGH_THRESHOLD;
            return RETURN_OK;
        }
        case(InternalState::CONFIG_LOW_THRESHOLD): {
            *foundLen = 3;
            *foundId = Max31865Definitions::WRITE_LOW_THRESHOLD;
            commandExecuted = true;
            return RETURN_OK;
        }
        case(InternalState::REQUEST_LOW_THRESHOLD): {
            *foundLen = 3;
            *foundId = Max31865Definitions::REQUEST_LOW_THRESHOLD;
            return RETURN_OK;
        }
        default: {
            sif::debug << "Max31865PT1000Handler::scanForReply: Unknown internal state"
                    << std::endl;
            return RETURN_OK;
        }
        }
    }

    if(remainingSize == configReplySize) {
        if(internalState == InternalState::CONFIGURE) {
            commandExecuted = true;
            *foundLen = configReplySize;
            *foundId = Max31865Definitions::CONFIG_CMD;
        }
        else if(internalState == InternalState::REQUEST_FAULT_BYTE) {
            *foundId = Max31865Definitions::REQUEST_FAULT_BYTE;
            *foundLen = 2;
            internalState = InternalState::RUNNING;
        }
        else if(internalState == InternalState::CLEAR_FAULT_BYTE) {
            *foundId = Max31865Definitions::CLEAR_FAULT_BYTE;
            *foundLen = 2;
            internalState = InternalState::RUNNING;
        }
        else {
            *foundId = Max31865Definitions::REQUEST_CONFIG;
            *foundLen = configReplySize;
        }
    }

    return RETURN_OK;
}

ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(
        DeviceCommandId_t id, const uint8_t *packet) {
    switch(id) {
    case(Max31865Definitions::REQUEST_CONFIG): {
        if(packet[1] != DEFAULT_CONFIG) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
            // it propably would be better if we at least try one restart..
            sif::error << "Max31865PT1000Handler: Object ID: " << std::hex << this->getObjectId()
            		<< ": Invalid configuration reply!" << std::endl;
#else
            sif::printError("Max31865PT1000Handler: Invalid configuration reply!\n");
#endif
            return HasReturnvaluesIF::RETURN_OK;
        }
        // set to true for invalid configs too for now.
        if(internalState == InternalState::REQUEST_CONFIG) {
            commandExecuted = true;
        }
        else if(internalState == InternalState::RUNNING) {
            // we should propably generate a telemetry with the config byte
            // as payload here.
        }
        break;
    }
    case(Max31865Definitions::REQUEST_LOW_THRESHOLD): {
        uint16_t readLowThreshold = packet[1] << 8 | packet[2];
        if(readLowThreshold != LOW_THRESHOLD) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
            sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " <<
            		std::hex << this->getObjectId() << ": Missmatch between " <<
                    "written and readback value of low threshold register" << std::endl;
#else
            sif::printWarning("Max31865PT1000Handler::interpretDeviceReply: Missmatch between "
                    "written and readback value of low threshold register\n");
#endif
#endif
        }
        commandExecuted = true;
        break;
    }
    case(Max31865Definitions::REQUEST_HIGH_THRESHOLD): {
        uint16_t readHighThreshold = packet[1] << 8 | packet[2];
        if(readHighThreshold != HIGH_THRESHOLD) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
            sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: "
            		<< std::hex << this->getObjectId() << ": Missmatch between " <<
                    "written and readback value of high threshold register" << std::endl;
#else
            sif::printWarning("Max31865PT1000Handler::interpretDeviceReply: Missmatch between "
                    "written and readback value of high threshold register\n");
#endif
#endif
        }
        commandExecuted = true;
        break;
    }
    case(Max31865Definitions::REQUEST_RTD): {
        // first bit of LSB reply byte is the fault bit
        uint8_t faultBit = packet[2] & 0b0000'0001;
        if(resetFaultBit) {
            internalState = InternalState::CLEAR_FAULT_BYTE;
            resetFaultBit = false;
        }
        else if(faultBit == 1) {
            // Maybe we should attempt to restart it?
            internalState = InternalState::REQUEST_FAULT_BYTE;
            resetFaultBit = true;
        }

        // RTD value consists of last seven bits of the LSB reply byte and
        // the MSB reply byte
        uint16_t adcCode = ((packet[1] << 8) | packet[2]) >> 1;
        // do something with rtd value, will propably be stored in
        // dataset.
        float rtdValue = adcCode * RTD_RREF_PT1000 / INT16_MAX;
        // calculate approximation
        float approxTemp = adcCode / 32.0 - 256.0;

#if OBSW_DEBUG_RTD == 1
#if OBSW_VERBOSE_LEVEL >= 1
        if(debugDivider->checkAndIncrement()) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
            sif::info << "Max31865: Object ID: " << std::hex << this->getObjectId()
            		<< ": Measured resistance is " << rtdValue << " Ohms." << std::endl;
            sif::info << "Approximated temperature is " << approxTemp << " C" << std::endl;
#else
            sif::printInfo("Max31685: Measured resistance is %f Ohms\n", rtdValue);
            sif::printInfo("Approximated temperature is %f C\n", approxTemp);
#endif
        }
#endif
#endif
        PoolReadGuard pg(&sensorDataset);
        if(pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
            // Configuration error
#if FSFW_CPP_OSTREAM_ENABLED == 1
            sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: "
            		<< std::hex << this->getObjectId() << ": Error reading dataset!"
                    << std::endl;
#else
            sif::printWarning("Max31865PT1000Handler::interpretDeviceReply: "
            		"Error reading dataset!\n");
#endif
            return pg.getReadResult();
        }

        if(not sensorDataset.isValid()) {
            sensorDataset.setValidity(true, false);
            sensorDataset.rtdValue.setValid(true);
            sensorDataset.temperatureCelcius.setValid(true);
        }

        sensorDataset.rtdValue = rtdValue;
        sensorDataset.temperatureCelcius = approxTemp;
        break;
    }
    case(Max31865Definitions::REQUEST_FAULT_BYTE): {
        faultByte = packet[1];
#if OBSW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: "
        		<< std::hex << this->getObjectId() << ": Fault byte"
                " is: 0b" << std::bitset<8>(faultByte) << std::endl;
#else
        sif::printWarning("Max31865PT1000Handler::interpretDeviceReply: Fault byte"
                " is: 0b" BYTE_TO_BINARY_PATTERN "\n", BYTE_TO_BINARY(faultByte));
#endif
#endif
        ReturnValue_t result = sensorDataset.read();
        if(result != HasReturnvaluesIF::RETURN_OK) {
            // Configuration error
#if FSFW_CPP_OSTREAM_ENABLED == 1
            sif::debug << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex
            		<< this->getObjectId() << ":"
                    "Error reading dataset!" << std::endl;
#else
            sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: "
                    "Error reading dataset!\n");
#endif
            return result;
        }
        sensorDataset.errorByte.setValid(true);
        sensorDataset.errorByte = faultByte;
        if(faultByte != 0) {
            sensorDataset.temperatureCelcius.setValid(false);
        }

        result = sensorDataset.commit();
        if(result != HasReturnvaluesIF::RETURN_OK) {
            // Configuration error
#if FSFW_CPP_OSTREAM_ENABLED == 1
            sif::debug << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex
            		<< this->getObjectId() << ": Error commiting dataset!" << std::endl;
#else
            sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: "
                    "Error commiting dataset!\n");
#endif
            return result;
        }

        break;
    }
    default:
        break;
    }
    return HasReturnvaluesIF::RETURN_OK;
}

void Max31865PT1000Handler::debugInterface(uint8_t positionTracker,
        object_id_t objectId, uint32_t parameter) {
}

uint32_t Max31865PT1000Handler::getTransitionDelayMs(
        Mode_t modeFrom, Mode_t modeTo) {
    return 25000;
}

ReturnValue_t Max31865PT1000Handler::getSwitches(
        const uint8_t **switches, uint8_t *numberOfSwitches) {
    return DeviceHandlerBase::NO_SWITCH;
}

void Max31865PT1000Handler::doTransition(Mode_t modeFrom,
        Submode_t subModeFrom) {
    DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
}

ReturnValue_t Max31865PT1000Handler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {
    localDataPoolMap.emplace(Max31865Definitions::PoolIds::RTD_VALUE, new PoolEntry<float>({0}));
    localDataPoolMap.emplace(Max31865Definitions::PoolIds::TEMPERATURE_C,
            new PoolEntry<float>({0}, 1, true));
    localDataPoolMap.emplace(Max31865Definitions::PoolIds::FAULT_BYTE,
            new PoolEntry<uint8_t>({0}));
//    poolManager.subscribeForPeriodicPacket(sensorDatasetSid,
//            false, 4.0, false);
    return HasReturnvaluesIF::RETURN_OK;
}

void Max31865PT1000Handler::modeChanged() {
    internalState = InternalState::NONE;
}