#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;
        communicationStep = CommunicationStep::READ_CONVERSIONS;
        break;
    }
    default: {
        sif::debug << "RadiationSensorHandler::buildNormalDeviceCommand: Unknwon 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;
    }
	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): {
	    cmdBuffer[0] = RAD_SENSOR::CONVERSION_DEFINITION;
 		rawPacket = cmdBuffer;
		rawPacketLen = 1;
		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;
	    cmdBuffer[6] = 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;
}