#include <mission/devices/Tmp1075Handler.h>
#include <mission/devices/devicedefinitions/Tmp1075Definitions.h>
#include <OBSWConfig.h>

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

Tmp1075Handler::~Tmp1075Handler() {
}


void Tmp1075Handler::doStartUp(){
	if(mode == _MODE_START_UP){
		setMode(MODE_ON);
	}
}

void Tmp1075Handler::doShutDown(){

}

ReturnValue_t Tmp1075Handler::buildNormalDeviceCommand(
		DeviceCommandId_t * id) {

	if(communicationStep == CommunicationStep::START_ADC_CONVERSION) {
		*id = TMP1075::START_ADC_CONVERSION;
		communicationStep = CommunicationStep::GET_TEMPERATURE;
		return buildCommandFromCommand(*id, NULL, 0);
	}
	else {
		*id = TMP1075::GET_TEMP;
		communicationStep = CommunicationStep::START_ADC_CONVERSION;
		return buildCommandFromCommand(*id, NULL, 0);
	}
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t Tmp1075Handler::buildTransitionDeviceCommand(
		DeviceCommandId_t * id){
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t Tmp1075Handler::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
		size_t commandDataLen) {
	switch(deviceCommand) {
	case(TMP1075::START_ADC_CONVERSION): {
		std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
		prepareAdcConversionCommand();
		rawPacket = cmdBuffer;
		rawPacketLen = TMP1075::CFGR_CMD_SIZE;
		return RETURN_OK;
	}
	case(TMP1075::GET_TEMP): {
		std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
		prepareGetTempCommand();
		rawPacket = cmdBuffer;
		rawPacketLen = TMP1075::POINTER_REG_SIZE;
		rememberCommandId = TMP1075::GET_TEMP;
		return RETURN_OK;
	}
	default:
		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
	}
	return HasReturnvaluesIF::RETURN_FAILED;
}

void Tmp1075Handler::fillCommandAndReplyMap(){
	this->insertInCommandMap(TMP1075::START_ADC_CONVERSION);
	this->insertInCommandAndReplyMap(TMP1075::GET_TEMP, 1, &dataset,
			TMP1075::GET_TEMP_REPLY_SIZE);
}

ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start,
        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
	switch(rememberCommandId) {
	case(TMP1075::GET_TEMP):
			*foundId = TMP1075::GET_TEMP;
			*foundLen = TMP1075::GET_TEMP_REPLY_SIZE;
			rememberCommandId = TMP1075::NONE;
		break;
	default:
		return IGNORE_REPLY_DATA;
	}
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id,
        const uint8_t *packet) {
	switch (id) {
	case TMP1075::GET_TEMP: {
		int16_t tempValueRaw = 0;
		tempValueRaw = packet[0] << 4 | packet[1] >> 4;
		float tempValue = ((static_cast<float>(tempValueRaw)) * 0.0625);
#if OBSW_VERBOSE_LEVEL >= 1
		sif::info << "Tmp1075 with object id: 0x" << std::hex << getObjectId()
				<< ": Temperature: " << tempValue<< " °C"
				<< std::endl;
#endif
        ReturnValue_t result = dataset.read();
        if(result == HasReturnvaluesIF::RETURN_OK) {
            dataset.temperatureCelcius = tempValue;
            dataset.commit();
        }
		break;
	}

	default: {
		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
	}

	}
	return HasReturnvaluesIF::RETURN_OK;
}

void Tmp1075Handler::setNormalDatapoolEntriesInvalid(){

}

void Tmp1075Handler::prepareAdcConversionCommand(){
	cmdBuffer[0] = TMP1075::CFGR_ADDR;
	cmdBuffer[1] = TMP1075::ONE_SHOT_MODE >> 8;
	cmdBuffer[2] = TMP1075::ONE_SHOT_MODE & 0xFF;
}

void Tmp1075Handler::prepareGetTempCommand(){
	cmdBuffer[0] = TMP1075::TEMP_REG_ADDR;
}

uint32_t Tmp1075Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
	return 500;
}

ReturnValue_t Tmp1075Handler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {
    localDataPoolMap.emplace(TMP1075::TEMPERATURE_C_TMP1075_1, new PoolEntry<float>( { 0.0 }));
    return HasReturnvaluesIF::RETURN_OK;
}