#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/devices/RadiationSensorHandler.h>
#include <OBSWConfig.h>

RadiationSensorHandler::RadiationSensorHandler(object_id_t objectId, object_id_t comIF,
        CookieIF * comCookie) :
        DeviceHandlerBase(objectId, comIF, comCookie), dataset(
                this) {
    if (comCookie == NULL) {
        sif::error << "RadiationSensorHandler: Invalid com cookie" << std::endl;
    }
}

RadiationSensorHandler::~RadiationSensorHandler() {
}


void RadiationSensorHandler::doStartUp(){
    if (internalState == InternalState::CONFIGURED) {
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
        setMode(MODE_NORMAL);
#else
        setMode(_MODE_TO_ON);
#endif
    }
}

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

ReturnValue_t RadiationSensorHandler::buildNormalDeviceCommand(
        DeviceCommandId_t * id) {

    switch (communicationStep) {
    case CommunicationStep::START_CONVERSION: {
        *id = RAD_SENSOR::START_CONVERSION;
        communicationStep = CommunicationStep::READ_CONVERSIONS;
        break;
    }
    case CommunicationStep::READ_CONVERSIONS: {
        *id = RAD_SENSOR::READ_CONVERSIONS;
        communicationStep = CommunicationStep::START_CONVERSION;
        break;
    }
    default: {
        sif::debug << "RadiationSensorHandler::buildNormalDeviceCommand: Unknown communication "
                << "step" << std::endl;
        return HasReturnvaluesIF::RETURN_OK;
    }
    }
    return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t RadiationSensorHandler::buildTransitionDeviceCommand(
        DeviceCommandId_t * id){
    if (internalState == InternalState::SETUP) {
        *id = RAD_SENSOR::WRITE_SETUP;
    }
    else {
        return HasReturnvaluesIF::RETURN_OK;
    }
    return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t RadiationSensorHandler::buildCommandFromCommand(
        DeviceCommandId_t deviceCommand, const uint8_t * commandData,
        size_t commandDataLen) {
    switch(deviceCommand) {
    case(RAD_SENSOR::WRITE_SETUP): {
        cmdBuffer[0] = RAD_SENSOR::SETUP_DEFINITION;
        rawPacket = cmdBuffer;
        rawPacketLen = 1;
        internalState = InternalState::CONFIGURED;
        return RETURN_OK;
    }
    case(RAD_SENSOR::START_CONVERSION): {
        /* First the fifo will be reset here */
        cmdBuffer[0] = RAD_SENSOR::RESET_DEFINITION;
        cmdBuffer[1] = RAD_SENSOR::CONVERSION_DEFINITION;
        rawPacket = cmdBuffer;
        rawPacketLen = 2;
        return RETURN_OK;
    }
    case(RAD_SENSOR::READ_CONVERSIONS): {
        cmdBuffer[0] = RAD_SENSOR::DUMMY_BYTE;
        cmdBuffer[1] = RAD_SENSOR::DUMMY_BYTE;
        cmdBuffer[2] = RAD_SENSOR::DUMMY_BYTE;
        cmdBuffer[3] = RAD_SENSOR::DUMMY_BYTE;
        cmdBuffer[4] = RAD_SENSOR::DUMMY_BYTE;
        cmdBuffer[5] = RAD_SENSOR::DUMMY_BYTE;
        rawPacket = cmdBuffer;
        rawPacketLen = RAD_SENSOR::READ_SIZE;
        return RETURN_OK;
    }
    default:
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
    return HasReturnvaluesIF::RETURN_FAILED;
}

void RadiationSensorHandler::fillCommandAndReplyMap() {
    this->insertInCommandMap(RAD_SENSOR::WRITE_SETUP);
    this->insertInCommandMap(RAD_SENSOR::START_CONVERSION);
    this->insertInCommandAndReplyMap(RAD_SENSOR::READ_CONVERSIONS, 1, &dataset,
            RAD_SENSOR::READ_SIZE);
}

ReturnValue_t RadiationSensorHandler::scanForReply(const uint8_t *start,
        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
    *foundId = this->getPendingCommand();
    *foundLen = remainingSize;
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t RadiationSensorHandler::interpretDeviceReply(DeviceCommandId_t id,
        const uint8_t *packet) {
    switch (id) {
    case RAD_SENSOR::READ_CONVERSIONS: {
        PoolReadGuard readSet(&dataset);
        dataset.temperatureCelcius = (*(packet) << 8 | *(packet + 1)) * 0.125;
        dataset.channel0 = (*(packet + 2) << 8 | *(packet + 3));
        dataset.channel1 = (*(packet + 4) << 8 | *(packet + 5));
#if OBSW_VERBOSE_LEVEL >= 1 && DEBUG_RAD_SENSOR
        sif::info << "Radiation sensor temperature: " << dataset.temperatureCelcius << " °C"
                << std::endl;
        sif::info << "Radiation sensor temperature ADC value channel 0: " << dataset.channel0
                << std::endl;
        sif::info << "Radiation sensor temperature ADC value channel 1: " << dataset.channel1
                << std::endl;
#endif
        break;
    }
    default: {
        sif::debug << "RadiationSensorHandler::interpretDeviceReply: Unknown reply id" << std::endl;
        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }

    }
    return HasReturnvaluesIF::RETURN_OK;
}

void RadiationSensorHandler::setNormalDatapoolEntriesInvalid(){

}

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

ReturnValue_t RadiationSensorHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {
    localDataPoolMap.emplace(RAD_SENSOR::TEMPERATURE_C, new PoolEntry<float>( { 0.0 }));
    localDataPoolMap.emplace(RAD_SENSOR::CHANNEL_0, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(RAD_SENSOR::CHANNEL_1, new PoolEntry<uint16_t>( { 0 }));
    return HasReturnvaluesIF::RETURN_OK;
}