#include	<stdio.h>
#include	<string.h>
#include	<netinet/in.h>
#include	<sys/ipc.h>
#include	<sys/msg.h>
#include	<mqueue.h>
#include	"dsgwd_pi.h"
#include	"node.h"

char	PTT_On;

extern	struct usb_dev_handle *udev;	
extern	mqd_t	rf_qid;
extern	mqd_t	inet_qid;

extern	char	NullVoice0[12];
extern	char	NullVoice1[12];
extern	char	LastFrame0[6];
extern	char	LastFrame1[6];
char 	LastFrameCheckTx[6];


char	JitterBuffer[JitterBufferSize][12];		
unsigned char	JitterBufferPnt;
unsigned char	CurJitterBufferPnt;
char	VoicePacketSW;
unsigned char	SequenceTemp;


void	from_inet(void)
{
	struct	packet	q_pkt;
	long	mtype;
	unsigned int	prio;

	if (mq_receive(inet_qid, (char *)&q_pkt, sizeof(q_pkt), &prio) > 0)
	{
		if (q_pkt.B_header.seq == 0x80)
		{
			send_header_USB((char*)&q_pkt.header);
		}
		else if (q_pkt.B_header.seq & 0x1f)
		{
			send_voice_USB(q_pkt.voice.voice, q_pkt.B_header.seq);
		}
                if (q_pkt.B_header.seq & 0x40)
                {
                        Ptt_Off();
                }
	}
	if (VoicePacketSW && (JitterBufferPnt != CurJitterBufferPnt)) JitterBufferSend();
}

void	send_header_USB(char pkt[])
{
	if (JitterBufferPnt != CurJitterBufferPnt) JitterBufferSend();

	usb_control_msg(udev, 0x40, SET_MyCALL2, 0, 0, &pkt[35], 4, 100);
	usb_control_msg(udev, 0x40, SET_MyCALL, 0, 0, &pkt[27], 8, 100);
	usb_control_msg(udev, 0x40, SET_YourCALL, 0, 0, &pkt[19], 8, 100);
	usb_control_msg(udev, 0x40, SET_RPT1CALL, 0, 0, &pkt[11], 8, 100);
	usb_control_msg(udev, 0x40, SET_RPT2CALL, 0, 0, &pkt[3], 8, 100);
   	usb_control_msg(udev, 0x40, SET_FLAGS, 0, 0, &pkt[0], 3, 100);

	JitterBufferPnt = 0;
	CurJitterBufferPnt = 0;
	SequenceTemp = 20;
	memcpy (&JitterBuffer[0][0], NullVoice0, 12);
	memcpy (&JitterBuffer[20][0], NullVoice1, 12);
	VoicePacketSW = FALSE;
	PTT_On = FALSE;
}

void	send_voice_USB (char pkt[], char seq)
{
	char	i;

	JitterBufferSave (pkt, seq); 

	if (!memcmp (&pkt[9], LastFrame0, 6))
	{
		Ptt_Off();
		return;
	}  
	for (i = 0 ; i < 12 ; i++)
	{
		LastFrameCheckTx[0] = LastFrameCheckTx[1];
		LastFrameCheckTx[1] = LastFrameCheckTx[2];
		LastFrameCheckTx[2] = LastFrameCheckTx[3];
		LastFrameCheckTx[3] = LastFrameCheckTx[4];
		LastFrameCheckTx[4] = LastFrameCheckTx[5];
		LastFrameCheckTx[5] = pkt[i];
		if (!memcmp(LastFrameCheckTx, LastFrame0, 6) || !memcmp(LastFrameCheckTx, LastFrame1, 6))
		{
			Ptt_Off();
			return;
		}		
	}
}

void	Ptt_Off( void )
{
	char	dummy;

	while (JitterBufferPnt != CurJitterBufferPnt)
	{
		VoicePacketSW = TRUE;
		JitterBufferSend();
	}
	JitterBufferPnt = 0;
	CurJitterBufferPnt = 0;
	SequenceTemp = 20;
	memcpy (&JitterBuffer[0][0], NullVoice0, 12);
	memcpy (&JitterBuffer[20][0], NullVoice1, 12);
	/* PTT OFF */
	usb_control_msg(udev, 0x40, SET_PTT, OFF, 0, &dummy, 0, 100);
	PTT_On = FALSE;
	VoicePacketSW = FALSE;
 	return;
}

void	JitterBufferSave (char VoicePacket[], unsigned char sequence)
{
	unsigned char	SequenceSave;
	JitterBufferPnt = sequence & 0x1f;
	


//	if (JitterBufferPnt && !memcmp (&VoicePacket[9], Sync, 3)) memset (&VoicePacket[9], 0x66, 3);

	SequenceSave = SequenceTemp;
	SequenceTemp++;
	if (SequenceTemp > 20) SequenceTemp = 0;
	if (SequenceTemp != JitterBufferPnt) memcpy (&JitterBuffer[SequenceTemp++][0], &JitterBuffer[SequenceSave][0], 9);
	if (SequenceTemp > 20) SequenceTemp = 0;
	while (SequenceTemp != JitterBufferPnt)
	{
		memcpy (&JitterBuffer[SequenceTemp++][0], VoicePacket, 9);
		if (SequenceTemp > 20) SequenceTemp = 0;
	}
	memcpy (&JitterBuffer[JitterBufferPnt][0], VoicePacket, 12);
	if (JitterBufferPnt > 15) VoicePacketSW = TRUE;
	SequenceTemp = JitterBufferPnt;
}

void	JitterBufferSend (void)
{
	unsigned char	len;
	unsigned char	dummy;
	extern	char	headerRead;

	if (JitterBufferPnt == CurJitterBufferPnt) return;
	if (!PTT_On)
	{
		if (headerRead)
		{
			JitterBufferPnt = 0;
			CurJitterBufferPnt = 0;
			SequenceTemp = 20;
			memcpy (&JitterBuffer[0][0], NullVoice0, 12);
			memcpy (&JitterBuffer[20][0], NullVoice1, 12);
			VoicePacketSW = FALSE;
			return;
		}	
		while (1)
		{
		   	usb_control_msg(udev, 0xC0, GET_REMAINSPACE, 0, 0, &len ,1, 100);
			if (len >=95)
			{
				usb_control_msg(udev, 0x40, SET_PTT, ON, 0, &dummy, 0, 100);
				PTT_On = TRUE;
				usleep (150000);
				break;
			} else {
				usleep (200000);
			}
		}
	}	

	while (JitterBufferPnt != CurJitterBufferPnt)
	{
	   	usb_control_msg(udev, 0xC0, GET_REMAINSPACE, 0, 0, &len ,1, 100);
		if (len >= 12)
		{
   			usb_control_msg(udev, 0x40, PUT_DATA, 0, 0, &JitterBuffer[CurJitterBufferPnt][0],12, 100);
			if (CurJitterBufferPnt == 0) memcpy (&JitterBuffer[0][0], NullVoice0, 9);
			else  memcpy (&JitterBuffer[CurJitterBufferPnt][0], NullVoice1, 12);
			CurJitterBufferPnt++;
			if (CurJitterBufferPnt >= JitterBufferSize) CurJitterBufferPnt = 0;
		} else {
			break;
		}
	}
}
