/*
 *  msg.cpp
 *
 *  the msg file 
 *
 *  Copyright (C) 2002-2003 rui.peng, BeiJing Carsmart Corporation
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 2 as
 *  published by the Free Software Foundation.
 */
//C library header file
#include <stdio.h>                      //printf
#include <string.h>


#include "msg.hpp"
#include "error.h"
#include "config.h"

int msg::msg_inited = 0;

msg_info_st msg::msg_info[CFG_MSG_MAX];

/******************************************************************************
*	Function:		msg_init
*	Description:	init msg
*	Param:			no
*	Return:			0				-	success
					-ERR_SYS		- 	system error
					-ERR_BUSY		- 	module opend
*	Comment:
 ******************************************************************************/
int msg::msg_init(void)
{
    int i;
    int ret = 0;

    if (0 != msg::msg_inited) 
    {
        ret = -ERR_BUSY;
        goto error;
    }

    //init sem and mutex
    for (i=0; i<CFG_MSG_MAX; i++) 
    {
        if (sem_init(&msg_info[i].sem, 0, 0)) 
        {
            ret = -ERR_SYS;
            goto error;
        }
        if (pthread_mutex_init(&msg_info[i].mutex, NULL)) 
        {
            ret = -ERR_SYS;
            goto error;
        }
    }
    msg::msg_inited = 1;
error:
    return ret;
}

/******************************************************************************
*	Function:		msg_send
*	Description:	send msg
*	Param:			id				-	msg id
					msg				-	msg info
					prio			-	msg prio(0 is lowest,99 is highest)
*	Return:			0				-	success
					-ERR_SYS		-	system error
					-ERR_NOINIT		-	no init
					-ERR_NODEV		-	no device
					-ERR_INVAL		-	invalid pram
					-ERR_NOMEM		-	msg is full
*	Comment:
 ******************************************************************************/
int msg::msg_send(uint8_t id, msg_t *msg, uint8_t prio)
{
    int ret = 0;

    if (0 == msg_inited) 
    {
        ret = -ERR_NOINIT;
        goto error;
    }

    if (id > CFG_MSG_MAX - 1) 
    {
        ret = -ERR_NODEV;
        goto error;
    }

    if (prio > MSG_PRIO_MAX) 
    {
        ret = -ERR_INVAL;
        goto error;
    }

	//get mutex
    if (pthread_mutex_lock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }

	//check msg wether full or not
    if (msg_info[id].head == (msg_info[id].tail + 1) % CFG_MSG_REALSIZE) 
    {
        ret = -ERR_NOMEM;
        goto error1;
    }
    //write the time ticks of send msg
    clock_gettime(CLOCK_MONOTONIC, &(msg->time_spec));
    //write msg
    memcpy(&msg_info[id].message[msg_info[id].tail].msg, msg, sizeof(msg_t));
    msg_info[id].message[msg_info[id].tail].prio = prio;
    //msg queue +1
    msg_info[id].tail = (msg_info[id].tail + 1) % CFG_MSG_REALSIZE;
error1:
    //unlock mutex
    if (pthread_mutex_unlock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
    //pose sem
    if (sem_post (&msg_info[id].sem)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
error:
    return ret;
}

/******************************************************************************
*	Function:		msg_recv
*	Description:	recv msg
*	Param:			id				-	msg id
					msg				-	msg recv
					timeout			-	timeout time(0:block, 0xffff:noblock, other:wait times second£©
*	Return:			0				-	success
					-ERR_SYS		-	system error
					-ERR_NODEV		-	no such msg queue
					-ERR_NOINIT		-	no init
					-ERR_TIMEOUT	-	timeout
*	Comment:
 ******************************************************************************/
int msg::msg_recv(uint8_t id, msg_t *msg, uint16_t timeout)
{
    int ret = 0;
    //wait time
    struct timespec to;

    if (0 == msg_inited) 
    {
        ret = -ERR_NOINIT;
        goto error;
    }

    if (id > CFG_MSG_MAX - 1) 
    {
        ret = -ERR_NODEV;
        goto error;
    }

    //wait sem
    if (MSG_RECV_BLOCK == timeout) 
    {
        if (sem_wait (&msg_info[id].sem)) 
        {
            ret = -ERR_SYS;
            goto error;
        }
    }
    else if (MSG_RECV_NONBLOCK == timeout) 
    {
        if (sem_trywait (&msg_info[id].sem)) 
        {
            ret = -ERR_TIMEOUT;             //timeout
            goto error;
        }
    } else {
        to.tv_sec = time(NULL) + timeout;
        to.tv_nsec = 0;
        if (sem_timedwait (&msg_info[id].sem, &to)) 
        {
            ret = -ERR_TIMEOUT;
            goto error;
        }
    }

    //lock mutex
    if (pthread_mutex_lock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
    //check msg queue is empty or not
    if (msg_info[id].head == msg_info[id].tail) 
    {
        ret = -ERR_SYS;
        goto error1;
    }
    //read message queue
    memcpy(msg, &msg_info[id].message[msg_info[id].head].msg, sizeof(msg_t));
    //queue head+1
    msg_info[id].head = (msg_info[id].head + 1) % CFG_MSG_REALSIZE;

error1:
    //unlock mutex
    if (pthread_mutex_unlock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
error:
    return ret;
}

/******************************************************************************
*	Function:		msg_recv_prio
*	Description:	recv msg with high prio
*	Param:			id				-	msg id
					msg				-	msg info recv
					timeout			-	timeout time (0:block,0xffff:noblock,other:wait second time)
*	Return:			0				-	success
					-ERR_SYS		-	system error
					-ERR_NODEV		-	no msg queue
					-ERR_NOINIT		-	no init
					-ERR_TIMEOUT	-	recv timeout
*	Comment:
 ******************************************************************************/
int msg::msg_recv_prio(uint8_t id, msg_t *msg, uint16_t timeout)
{
    int ret = 0;
    int i;
    //search highest prio msg
    uint32_t index, head, tail;
    s8 prio = -1;
    //wait time
    struct timespec to;

    if (0 == msg_inited) 
    {
        ret = -ERR_NOINIT;
        goto error;
    }
    if (id > CFG_MSG_MAX - 1) 
    {
        ret = -ERR_NODEV;
        goto error;
    }
    //wait sem
    if (MSG_RECV_BLOCK == timeout) 
    {
        if (sem_wait (&msg_info[id].sem)) 
        {
            ret = -ERR_SYS;
            goto error;
        }
    }
    else if (MSG_RECV_NONBLOCK == timeout) 
    {
        if (sem_trywait (&msg_info[id].sem)) 
        {
            ret = -ERR_TIMEOUT;             //no msg
            goto error;
        }
    } else {
        to.tv_sec = time(NULL) + timeout;
        to.tv_nsec = 0;
        if (sem_timedwait (&msg_info[id].sem, &to)) 
        {
            ret = -ERR_TIMEOUT;
            goto error;
        }
    }

    //lock mutex
    if (pthread_mutex_lock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
    head = msg_info[id].head;
    tail = msg_info[id].tail;
    index = head;

    //check wether msg queue is empty or not
    if (head == tail) 
    {
        ret = -ERR_SYS;
        goto error1;
    }
    //search highest msg info,index is the find msg index value
    i = head;
    do 
    {
    	if ((s8)(msg_info[id].message[i].prio) > prio) 
    	{
            prio = (s8)(msg_info[id].message[i].prio);
            index = i;
        }
        i = (i + 1) % CFG_MSG_REALSIZE;
    }
    while(i != tail);
    //read this msg and deal other msg
    memcpy(msg, &msg_info[id].message[index].msg, sizeof(msg_t));
    if (index == head) 
    {
        msg_info[id].head = (head + 1) % CFG_MSG_REALSIZE;
    }
    else if (index > head) 
    {
        memmove(&msg_info[id].message[head] + 1, &msg_info[id].message[head], (index - head) * sizeof(msg_prio_t));
    	msg_info[id].head = (head + 1) % CFG_MSG_REALSIZE;
    } else {
        memmove(&msg_info[id].message[index], &msg_info[id].message[index] + 1, (tail - index - 1) * sizeof(msg_prio_t));
        if(0 == tail) 
        {
            msg_info[id].tail = CFG_MSG_REALSIZE - 1;
        } else {
            msg_info[id].tail = tail - 1;
        }
    }
error1:
	//unlock mutex
    if (pthread_mutex_unlock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
error:
    return ret;
}

/******************************************************************************
*	Function:		msg_clear
*	Description:	clear msg
*	Param:			id				-	msg id
*	Return:			0				-	success
					-ERR_SYS		-	system error
					-ERR_NODEV		-	no msg queue
					-ERR_NOINIT		-	no init
*	Comment:
 ******************************************************************************/
int msg::msg_clear(uint8_t id)
{
    int ret = 0;

    if (0 == msg_inited) 
    {
        ret = -ERR_NOINIT;
        goto error;
    }

    if (id > CFG_MSG_MAX - 1) 
    {
        ret = -ERR_NODEV;
        goto error;
    }
    //init sem
    if (sem_init(&msg_info[id].sem, 0, 0)) 
    {
		ret = -ERR_SYS;
		goto error;
	}
    //lock mutex
    if (pthread_mutex_lock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
    //clear msg queue
    msg_info[id].head = msg_info[id].tail;
    //lock mutex
    if (pthread_mutex_unlock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
error:
    return ret;
}

/******************************************************************************
*	Function:		msg_getsize
*	Description:	get msg count
*	Param:			id				-	msg id
*	Return:			>0				-	msg size
					-ERR_SYS		-	system error
					-ERR_NODEV		-	no msg queue
					-ERR_NOINIT		-	no init
*	Comment:
 ******************************************************************************/
int msg::msg_getsize(uint8_t id)
{
    int ret = 0;
    int size;

    if (0 == msg_inited) 
    {
        ret = -ERR_NOINIT;
        goto error;
    }

    if (id > CFG_MSG_MAX - 1) 
    {
        ret = -ERR_NODEV;
        goto error;
    }
    //lock mutex
    if (pthread_mutex_lock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
    //get the size of msg queue
    size = (int)msg_info[id].tail - (int)msg_info[id].head;
    if(size < 0) 
    {
        size = size + CFG_MSG_REALSIZE;
    }
    //unlock mutex
    if (pthread_mutex_unlock (&msg_info[id].mutex)) 
    {
        ret = -ERR_SYS;
        goto error;
    }
    ret = size;
error:
    return ret;
}