#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 NOTHING_TO_SEND;
    }
    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;
        std::memset(cmdBuffer, RAD_SENSOR::DUMMY_BYTE, RAD_SENSOR::READ_SIZE);
        rawPacket = cmdBuffer;
        rawPacketLen = RAD_SENSOR::READ_SIZE;
        return RETURN_OK;
    }
//    case(RAD_SENSOR::AIN0_AND_TMP_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;
//    }
    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();

    switch (*foundId) {
    case RAD_SENSOR::START_CONVERSION:
    case RAD_SENSOR::WRITE_SETUP:
        return IGNORE_REPLY_DATA;
    default:
        break;
    }

    *foundLen = remainingSize;

    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t RadiationSensorHandler::interpretDeviceReply(DeviceCommandId_t id,
        const uint8_t *packet) {
    switch (id) {
    case RAD_SENSOR::READ_CONVERSIONS: {
        uint8_t offset = 0;
        PoolReadGuard readSet(&dataset);
        dataset.temperatureCelcius = (*(packet + offset) << 8 | *(packet + offset + 1)) * 0.125;
        offset += 2;
        dataset.ain0 = (*(packet + offset) << 8 | *(packet + offset +1));
        offset += 2;
        dataset.ain1 = (*(packet + offset) << 8 | *(packet + offset + 1));
        offset += 6;
        dataset.ain4 = (*(packet + offset) << 8 | *(packet + offset + 1));
        offset += 2;
        dataset.ain5 = (*(packet + offset) << 8 | *(packet + offset + 1));
        offset += 2;
        dataset.ain6 = (*(packet + offset) << 8 | *(packet + offset + 1));
        offset += 2;
        dataset.ain7 = (*(packet + offset) << 8 | *(packet + offset + 1));

#if OBSW_VERBOSE_LEVEL >= 1 && DEBUG_RAD_SENSOR
        sif::info << "Radiation sensor temperature: " << dataset.temperatureCelcius << " °C"
                << std::endl;
        sif::info << "Radiation sensor ADC value channel 0: " << dataset.ain0
                << std::endl;
        sif::info << "Radiation sensor ADC value channel 1: " << dataset.ain1
                << std::endl;
        sif::info << "Radiation sensor ADC value channel 4: " << dataset.ain4
                << std::endl;
        sif::info << "Radiation sensor ADC value channel 5: " << dataset.ain5
                << std::endl;
        sif::info << "Radiation sensor ADC value channel 6: " << dataset.ain6
                << std::endl;
        sif::info << "Radiation sensor ADC value channel 7: " << dataset.ain7
                << 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::AIN0, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(RAD_SENSOR::AIN1, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(RAD_SENSOR::AIN4, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(RAD_SENSOR::AIN5, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(RAD_SENSOR::AIN6, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(RAD_SENSOR::AIN7, new PoolEntry<uint16_t>( { 0 }));
    return HasReturnvaluesIF::RETURN_OK;
}