#include "app_queue.h"

#ifdef CONFIG_XIP
#define ATTRIBUTE_DATA __attribute__((section(".ram.code")))
#else
#define ATTRIBUTE_DATA
#endif

//  初始化队列
bool QueueCreate(volatile QueueStruct *q , uint32_t *p_buf , int buf_size)
{
    q->qu = p_buf;
    q->head = 0;
    q->tail = 0;
    q->qlength = 0;
    q->maxsize = buf_size;
    return true;
}

// 清空队列中所有数据
bool QueueReset(volatile QueueStruct *q)
{
    q->head = 0;
    q->tail = 0;
    q->qlength = 0;
	return true;
}

// 入队
ATTRIBUTE_DATA bool QueueWriteVal(volatile QueueStruct *q , uint32_t *p_buf , int len)
{
	while(len--){
		if(q->qlength < q->maxsize) {  // 队列不满才插入
			//给队伍最尾添加赋值
			q->qu[q->tail] = *p_buf++;
			//队尾向后移动一位，如果超过最大值，则从0开始，变成队尾
			q->tail = (q->tail +1)%q->maxsize;
			//队伍长度+1
			q->qlength ++;
		}else{
			return false;
		}
	}
	return true;
}

// 出队
ATTRIBUTE_DATA int QueueReadVal(volatile QueueStruct *q, uint32_t *p_buf , int len)
{
	int rd_len = 0;
	while(len--){
		if (q->qlength > 0) {
			*p_buf++ = q->qu[q->head];
			//队头向后移动一位 超过最大值又重新 从0 开始计算
			q->head = (q->head +1)%q->maxsize;
			// 队伍长度-1
			q->qlength -- ;
			rd_len ++;
		}else{
			// 队列中数据不够，补充0
			*p_buf++ = 0x0;
		}
    }
	return rd_len;
}

// 获取队列最后一次填入的数据
uint32_t  QueueTailValue(volatile QueueStruct *q)
{
	int index;
	if (q->qlength > 0) {
		index = q->tail;
		if(index > 0){
			index --;
		}else{
			index = q->maxsize - 1;
		}
		return q->qu[index] ;
	}else{
		return 0x00;
	}
}

// 获取队列的最大长度
int QueueMaxBufLength(volatile QueueStruct *q)
{
	return q->maxsize;
}

// 判断空队列，长度为0则为空
bool QueueIsEmpty(volatile QueueStruct *q)
{
   return  q->qlength == 0 ? true : false;
}

// 判断满队列，长度=最大容量为满
bool QueueIsFull(volatile QueueStruct *q)
{
    return q->qlength == q->maxsize ? true : false;
}

// 获取队列中数据长度
int QueueDataLength(volatile QueueStruct *q)
{
    return q->qlength;
}

// 获取队列中剩余空间
int QueueResidualSpace(volatile QueueStruct *q)
{
	if(q->maxsize >= q->qlength){
		return (q->maxsize - q->qlength);
	}else{
		// 队列出错了.....
		return 0;
	}
}

// 向队列中添加一个采样点
ATTRIBUTE_DATA bool QueueAddOnePoint(volatile QueueStruct *q)
{
	int index,index_temp;
	bool  falg;
	uint32_t  last_val,cur_val;
	uint16_t  adj_len,k;
	short s_temp1,s_temp2,s_temp3;
	int i_temp4,i_temp5,i_temp6;

	falg = false;
	if(q->qlength < q->maxsize)
	{  // 队列不满，而且要有数据才会调节
		if(q->qlength > 0)
		{    // 队列不是空，获取当前队列最后一次写入的值
			index = q->tail;
			if(index > 0){
				index --;
			}else{
				index = q->maxsize - 1;
			}
			last_val = q->qu[index] ;
		}else {
			// 队列空
			last_val = 0x00;
		}
		//-----------------------------------
		//  把队列最后一个值赋值给队尾
		index = q->tail ;
		q->qu[q->tail] = last_val;
		//队尾向后移动一位，如果超过最大值，则从0开始，变成队尾
		q->tail = (q->tail +1)%q->maxsize;
		//队伍长度+1
		q->qlength ++;
		//-----------------------------------
		if(q->qlength > 8)
		{    // 调节之前的采样点
			adj_len = q->qlength - 4;
			if(adj_len > 64)
			{	// 为防止单次消耗时间太长，最多调节64个采样点
				adj_len = 64;
			}
			for(k=0;k<adj_len;k++)
			{
				if(index > 0){
					index --;
				}else{
					index = q->maxsize - 1;
				}

				if(index > 0){
					index_temp = index -1;
				}else{
					index_temp = q->maxsize - 1;
				}
				cur_val = q->qu[index_temp] ;

				s_temp1 = (last_val&0xffff);   // e
				s_temp2 = (cur_val&0xffff);    // d

				s_temp3 = s_temp1 - s_temp2;
				i_temp4 = (int)s_temp3 * (k + 1);
				i_temp4 /= (adj_len + 1);			//  (e-d)*1/4

				i_temp5 = i_temp4 + s_temp2 ;		//  (e-d)*1/4  + d
				i_temp5 &= 0xffff;
				//------------------------------------
				s_temp1 = ((last_val>>16)&0xffff);
				s_temp2 = ((cur_val>>16)&0xffff);

				s_temp3 = s_temp1 - s_temp2;
				i_temp4 = (int)s_temp3 * (k + 1);
				i_temp4 /= (adj_len + 1);

				i_temp6 = i_temp4 + s_temp2;
				i_temp6 <<= 16;
				i_temp6 &= 0xffff0000;
				//--------------------
				q->qu[index] = i_temp5 | i_temp6;
				last_val = cur_val;
			}
		}
		falg = true;
	}else{
		falg = false;
	}
	return falg;
}

// 在队列接近空时，调节数据长度
bool QueueEmptyAdjust(volatile QueueStateCtrlStruce  * pQueueCtrl)
{
	int length;
	bool Flag = false ;

	length = QueueDataLength(pQueueCtrl->pQueue);
	if(length <= pQueueCtrl->SetLowThreshold)
	{
		if((pQueueCtrl->NeedFillNum == 0)
				&&(pQueueCtrl->NeedLostNum == 0)
				&&(pQueueCtrl->MinIdleNum == 0)){
			pQueueCtrl->NeedFillNum = pQueueCtrl->SetAdjustLength;
			pQueueCtrl->MinIdleNum = pQueueCtrl->SetMinIdleLength;
			pQueueCtrl->Tick = pQueueCtrl->SetTick;
			pQueueCtrl->NeedLostNum = 0;
		}
	}

	if(pQueueCtrl->NeedFillNum > 0)
	{
		if(pQueueCtrl->Tick > 0)
		{
			pQueueCtrl->Tick --;
		}else {
			pQueueCtrl->Tick = pQueueCtrl->SetTick;
			QueueAddOnePoint(pQueueCtrl->pQueue);
			pQueueCtrl->NeedFillNum --;
			pQueueCtrl->AdjAddTotalNum ++; 
			Flag = true ;
		}

	}else{
		if(pQueueCtrl->MinIdleNum > 0)
		{
			pQueueCtrl->MinIdleNum --;
		}else{
			pQueueCtrl->Tick = 0;
		}
	}
	return Flag;
}

// 从队列中移除一个采样点
ATTRIBUTE_DATA int QueueReduceOnePoint(volatile QueueStruct *q)
{
	uint32_t last_data,cur_data;
	short s_temp1,s_temp2,s_temp3;
	int i_temp4,i_temp5,i_temp6;
	uint16_t  adj_len,k;
	int point;
	if (q->qlength > 0)
	{	// 队列中有数据才调节
		last_data = q->qu[q->head];		// 获取队列头的数据
		//队头向后移动一位 超过最大值又重新 从0 开始计算
		q->head = (q->head +1)%q->maxsize;
		// 队伍长度-1
		q->qlength -- ;

		if (q->qlength > 8)
		{
			adj_len = q->qlength - 2;
			if(adj_len > 64)
			{	// 为防止单次消耗时间太长，最多调节64个采样点
				adj_len = 64;
			}
			point = q->head;
			for(k=0;k<adj_len;k++)
			{
				cur_data = q->qu[point];
				s_temp1 = (last_data&0xffff);
				s_temp2 = (cur_data&0xffff);
				s_temp3 = s_temp2 - s_temp1;
				i_temp4 = (int)s_temp3 * (k + 1);
				i_temp4 /= (adj_len + 1);
				i_temp5 = i_temp4 + s_temp1;
				i_temp5 &= 0xffff;
				//------------------------------------
				s_temp1 = ((last_data>>16)&0xffff);
				s_temp2 = ((cur_data>>16)&0xffff);
				s_temp3 = s_temp2 - s_temp1;
				i_temp4 = (int)s_temp3 * (k + 1);
				i_temp4 /= (adj_len + 1);
				i_temp6 = i_temp4 + s_temp1;
				i_temp6 <<= 16;
				i_temp6 &= 0xffff0000;
				//--------------------
				q->qu[point] = i_temp5 | i_temp6;
				point = (point + 1)%q->maxsize;
				last_data = cur_data;
			}
		}
	}
	return 1;
}

// 在队列接近满时，调节数据长度
ATTRIBUTE_DATA bool QueueFullAdjust(volatile QueueStateCtrlStruce  * pQueueCtrl)
{
	int length;
	bool Flag = false ;

	length = QueueDataLength(pQueueCtrl->pQueue);
	if(length >= pQueueCtrl->SetHighThreshold)
	{
		if((pQueueCtrl->NeedLostNum == 0)
				&&(pQueueCtrl->NeedFillNum == 0)
				&&(pQueueCtrl->MinIdleNum == 0)){
			pQueueCtrl->NeedLostNum = pQueueCtrl->SetAdjustLength;
			pQueueCtrl->MinIdleNum = pQueueCtrl->SetMinIdleLength;
			pQueueCtrl->Tick = pQueueCtrl->SetTick;
			pQueueCtrl->NeedFillNum = 0;
		}
	}

	if(pQueueCtrl->NeedLostNum > 0)
	{
		if(pQueueCtrl->Tick > 0)
		{
			pQueueCtrl->Tick --;
		}else {
			pQueueCtrl->Tick = pQueueCtrl->SetTick;
			QueueReduceOnePoint(pQueueCtrl->pQueue);
			pQueueCtrl->NeedLostNum --;
			pQueueCtrl->AdjSubTotalNum ++;
			Flag = true ;
		}
	}else{
		if(pQueueCtrl->MinIdleNum >0)
		{
			pQueueCtrl->MinIdleNum --;
		}else{
			pQueueCtrl->Tick = 0;
		}
	}
	return Flag;
}

// 队列调节记录清零
void  QueueResetAdjRecord(volatile QueueStateCtrlStruce  * pQueueCtrl)
{
	pQueueCtrl->BackAdjAddTotalNum = pQueueCtrl->AdjSubTotalNum;	// 备份
	pQueueCtrl->BackAdjSubTotalNum = pQueueCtrl->AdjAddTotalNum;	// 备份
	pQueueCtrl->AdjSubTotalNum = 0x00;
	pQueueCtrl->AdjAddTotalNum = 0x00;
}

// 获取队列丢掉的数据
uint32_t QueueGetAdjSubNum(volatile QueueStateCtrlStruce  * pQueueCtrl)
{
	return pQueueCtrl->AdjSubTotalNum ;
}

// 获取队列添加的数据
uint32_t QueueGetAdjAddNum(volatile QueueStateCtrlStruce  * pQueueCtrl)
{
	return pQueueCtrl->AdjAddTotalNum ;
}