#include <ubixos/message.h>
#include <lib/kmalloc.h>
#include <lib/string.h>


static int inited = 0;
#define __MESSAGE_CONNECTION_COUNT 4
static const unsigned int connectionCount = __MESSAGE_CONNECTION_COUNT;
static int messageCount = 0;
static connection_t * connections;

static void messageInit()
{
    unsigned int i;
    
    if (inited)
	return;
    
    connections = (connection_t *) kmalloc(connectionCount * sizeof(connection_t), -1);

    for (i = 0; i < connectionCount; i++)
    {
	kmemset(&connections[i], '\0', sizeof(connection_t));
	//waitQueueInit(&connections[i].message_queue, 0);
	connections[i].message_queue = new Queue<message_t *>();
	//waitQueueInit(&connections[i].old_queue, 0);
	connections[i].old_queue = new Queue<message_t *>();
    }

    inited = 1;
}

int connectionCreate(char * cname, uInt32 * cid, uInt8 flags)
{
    unsigned int i;
    
    if (!inited)
	messageInit();

    for (i = 0; i < connectionCount; i++)
	if (connections[i].used == 0)
	    break;
    if (connections[i].used != 0)
	return -1;
    
    connections[i].used = 1;
    kstrncpy(connections[i].unique_string, cname, 256);
    connections[i].owning_pid = _current->id;
    
    *cid = i;
    
    return 0;
}

int connectionOpen(char * cname, uInt32 * cid)
{
    unsigned int i;
    if (!inited)
	messageInit();

    for (i = 0; i < connectionCount; i++)
	if (connections[i].used == 1)
	    if (0 == kstrncmp(connections[i].unique_string, cname, 256))
		break;

    if (connections[i].used == 0)
	return -1;
    if (0 != kstrncmp(connections[i].unique_string, cname, 256))
	return -1;

    *cid = i;
    return 0;
}

int messageRecv(uInt32 cid, uInt32 * sender, uInt32 * cmd, uInt32 * size, uInt8 * data)
{
    message_t * temp = NULL;
    //int		notUsed;
    
    if (!inited)
	return -1;
    
    if (cid >= connectionCount)
	return -1;
    
    //if (connections[cid].used == 0);
    //	return -1;
    
    if (connections[cid].owning_pid != _current->id)
    	return -1;
    do
    {
	//temp = (message_t *) waitQueueRemove(&connections[cid].message_queue, &notUsed);
	temp = connections[cid].message_queue->pop();
	
	if (temp == NULL)
	{
	    setTaskStatus_task(_current, MSGWAIT);
	    schedule();
	}
    }
    while(temp == NULL);
    
    kmemcpy(data, temp->data, temp->size);
    *cmd = temp->cmd;
    *size = temp->size;
    *sender = temp->send_taskId;
    
    //waitQueueInsert(&connections[cid].old_queue, temp, temp->id);
    connections[cid].old_queue->insert(temp->id, temp);
    
    return temp->id;
}

int messageSend(uInt32 cid, uInt32 * cmd, uInt32 * size, uInt8 * data)
{
    message_t * temp;
    kTask_t * task;
    
    if (!inited)
	return -1;

    if (cid >= connectionCount)
	return -1;

    if (connections[cid].used == 0)
	return -1;

    temp = (message_t *) kmalloc(sizeof(message_t), -1);
    
    temp->cmd = *cmd;
    temp->size = *size;
    kmemcpy((void *) temp->data, (void *) data, *size);
    temp->send_taskId = _current->id;
    temp->id = messageCount;
    messageCount++;
    temp->flags = MSG_SEND;

    //waitQueueInsert(&connections[cid].message_queue, (void *) temp, temp->id);
    connections[cid].message_queue->insert(temp->id, temp);
    
    task = getTask(connections[cid].owning_pid);
    if (task != NULL)
    {
	if (task->status == MSGWAIT)
	    setTaskStatus_task(task, READY);
    }
    
    while((temp->flags & MSG_REPLY) != MSG_REPLY)
    {
	setTaskStatus_task(_current, MSGREPLY);
	schedule();
    }

    *cmd = temp->cmd;
    *size= temp->size;
    kmemcpy((void *) data, (void *) temp->data, *size);
    
    kfree((void *)temp);

    return 0;
    
}

int messageReply(uInt32 cid, uInt32 mid, uInt32 cmd, uInt32 size, uInt8 * data)
{
    message_t * temp = NULL;
    if (!inited)
	return -1;
    if (cid >= connectionCount)
	return -1;
    if (connections[cid].used == 0)
    	return -1;
    //temp = (message_t *) waitQueueFind(&connections[cid].old_queue, mid);
    temp = connections[cid].old_queue->find(mid);
    if (temp == NULL)
	return -1;
    temp->flags |= MSG_REPLY;
    temp->size = size;
    temp->cmd = cmd;
    kmemcpy(temp->data, data, cmd);
    
    setTaskStatus(temp->send_taskId, READY);

    return 0;
}