#include "MGMHandlerRM3100.h"
#include <fsfw/devicehandlers/DeviceHandlerMessage.h>
#include <fsfw/objectmanager/SystemObjectIF.h>
#include <fsfw/returnvalues/HasReturnvaluesIF.h>


MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId,
        object_id_t deviceCommunication, CookieIF* comCookie):
		DeviceHandlerBase(objectId, deviceCommunication, comCookie) {
}

MGMHandlerRM3100::~MGMHandlerRM3100() {}

void MGMHandlerRM3100::doStartUp() {
	if(internalState == STATE_NONE) {
		internalState = STATE_CONFIGURE_CMM;
	}

	switch(internalState) {
	case(STATE_CONFIGURE_CMM): {
		break;
	}
	case(STATE_READ_CMM): {
		break;
	}
	case(STATE_CONFIGURE_TMRC): {
		break;
	}
	case(STATE_READ_TMRC): {
		break;
	}
	}
}

void MGMHandlerRM3100::doShutDown() {
}

ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand(
		DeviceCommandId_t *id) {
	if(internalState == STATE_CONFIGURE_CMM) {
		*id = RM3100::CONFIGURE_CMM;
		commandBuffer[0] = RM3100::CMM_REGISTER;
		commandBuffer[1] = RM3100::CMM_VALUE;
		this->rawPacket = commandBuffer;
		this->rawPacketLen = 2;
	}

	if(internalState == STATE_READ_CMM) {
		*id = RM3100::READ_CMM;
		commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK;
		commandBuffer[1] = 0;
		this->rawPacket = commandBuffer;
		this->rawPacketLen = 2;
	}
	return RETURN_OK;
}


ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand(
		DeviceCommandId_t *id) {
	return RETURN_OK;
}

ReturnValue_t MGMHandlerRM3100::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t *commandData,
		size_t commandDataLen) {
	return RETURN_OK;
}

ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start,
        size_t len, DeviceCommandId_t *foundId,
		size_t *foundLen) {

	*foundId = this->getPendingCommand();
	*foundLen = this->rawPacketLen;

	// Data with SPI Interface has always this answer
	if (start[0] == 0b11111111) {
		return RETURN_OK;
	}
	else {
		return DeviceHandlerIF::INVALID_DATA;
	}
}

ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
		DeviceCommandId_t id, const uint8_t *packet) {
	if(id == RM3100::CONFIGURE_CMM or id == RM3100::CONFIGURE_TMRC) {
		commandExecuted = true;
	}
	else if(id == RM3100::READ_CMM) {
		if(packet[1] == cmmRegValue) {
			commandExecuted = true;
		}
	}
	else if(id == RM3100::READ_TMRC) {
		if(packet[1] == tmrcRegValue) {
			commandExecuted = true;
		}
		else {
			// Attempt reconfiguration.
			internalState = STATE_CONFIGURE_TMRC;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
	}
	else {
		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
	}

	return RETURN_OK;
}