#include "MessageQueue.h"
#include "RtemsBasic.h"

#include "../../serviceinterface/ServiceInterface.h"
#include "../../objectmanager/ObjectManager.h"

#include <cstring>

MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
		id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(nullptr) {
	rtems_name name = ('Q' << 24) + (queueCounter++ << 8);
	rtems_status_code status = rtems_message_queue_create(name, message_depth,
			max_message_size, 0, &(this->id));
	if (status != RTEMS_SUCCESSFUL) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
		sif::error << "MessageQueue::MessageQueue: Creating Queue " << std::hex
				<< name << std::dec << " failed with status:"
				<< (uint32_t) status << std::endl;
#endif
		this->id = 0;
	}
}

MessageQueue::~MessageQueue() {
	rtems_message_queue_delete(id);
}

ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
		MessageQueueMessageIF* message, bool ignoreFault) {
	return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
}

ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
	return sendToDefaultFrom(message, this->getId());
}

ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
	if (this->lastPartner != 0) {
		return sendMessage(this->lastPartner, message, this->getId());
	} else {
		return NO_REPLY_PARTNER;
	}
}

ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
		MessageQueueId_t* receivedFrom) {
	ReturnValue_t status = this->receiveMessage(message);
	*receivedFrom = this->lastPartner;
	return status;
}

ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
	size_t size = 0;
	rtems_status_code status = rtems_message_queue_receive(id,
			message->getBuffer(),&size,
			RTEMS_NO_WAIT, 1);
	if (status == RTEMS_SUCCESSFUL) {
		message->setMessageSize(size);
		this->lastPartner = message->getSender();
		//Check size of incoming message.
		if (message->getMessageSize() < message->getMinimumMessageSize()) {
			return HasReturnvaluesIF::RETURN_FAILED;
		}
	} else {
		//No message was received. Keep lastPartner anyway, I might send something later.
		//But still, delete packet content.
		memset(message->getData(), 0, message->getMaximumDataSize());
	}
	return convertReturnCode(status);
}

MessageQueueId_t MessageQueue::getLastPartner() const {
	return this->lastPartner;
}

ReturnValue_t MessageQueue::flush(uint32_t* count) {
	rtems_status_code status = rtems_message_queue_flush(id, count);
	return convertReturnCode(status);
}

MessageQueueId_t MessageQueue::getId() const {
	return this->id;
}

void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
	this->defaultDestination = defaultDestination;
}

ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
		MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
		bool ignoreFault) {

	message->setSender(sentFrom);
	rtems_status_code result = rtems_message_queue_send(sendTo,
			message->getBuffer(), message->getMessageSize());

	//TODO: Check if we're in ISR.
	if (result != RTEMS_SUCCESSFUL && !ignoreFault) {
		if (internalErrorReporter == nullptr) {
			internalErrorReporter = ObjectManager::instance()->get<InternalErrorReporterIF>(
					objects::INTERNAL_ERROR_REPORTER);
		}
		if (internalErrorReporter != nullptr) {
			internalErrorReporter->queueMessageNotSent();
		}
	}

	ReturnValue_t returnCode = convertReturnCode(result);
	if(result == MessageQueueIF::EMPTY){
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	return returnCode;
}

ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
		MessageQueueId_t sentFrom, bool ignoreFault) {
	return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
}

MessageQueueId_t MessageQueue::getDefaultDestination() const {
	return this->defaultDestination;
}

bool MessageQueue::isDefaultDestinationSet() const {
	return (defaultDestination != NO_QUEUE);
}

ReturnValue_t MessageQueue::convertReturnCode(rtems_status_code inValue){
		switch(inValue){
		case RTEMS_SUCCESSFUL:
			return HasReturnvaluesIF::RETURN_OK;
		case  RTEMS_INVALID_ID:
			return HasReturnvaluesIF::RETURN_FAILED;
		case RTEMS_TIMEOUT:
			return HasReturnvaluesIF::RETURN_FAILED;
		case RTEMS_OBJECT_WAS_DELETED:
			return HasReturnvaluesIF::RETURN_FAILED;
		case RTEMS_INVALID_ADDRESS:
			return HasReturnvaluesIF::RETURN_FAILED;
		case RTEMS_INVALID_SIZE:
			return HasReturnvaluesIF::RETURN_FAILED;
		case RTEMS_TOO_MANY:
			return MessageQueueIF::FULL;
		case RTEMS_UNSATISFIED:
			return MessageQueueIF::EMPTY;
		default:
			return HasReturnvaluesIF::RETURN_FAILED;
		}

}



uint16_t MessageQueue::queueCounter = 0;