/*
 * DBG.c
 *
 *  Created on: 2018年4月10日
 *      Author: Administrator
 */

#include <string.h>

#include "osType.h"
#include "osDBG.h"
#include "osLibc.h"
#include "MT_Uart.h"

#define DBG_UART_FIFO        0

#define FIFO_SIZE            512
#define PACKAGE_FRAME_SIZE    5

static uint8     fifo[FIFO_SIZE];
static int        Counter = 0;

static uint8     GetFCS(uint8 *pdata, int len);
static int         FindSOF(uint8 *pdata, int startIndex, int len);
static int        CheckPackage(uint8 *pdata, int len);

int MT_Uart_InsertFIFO(uint8 *pdata, int len)
{
    int remain,temp;
    temp = Counter+len;
    if(temp>FIFO_SIZE)
    {
        remain = temp - FIFO_SIZE;
        Counter -= remain;
        osMemcpy(fifo,&fifo[remain],Counter);
    }
    osMemcpy(&fifo[Counter],pdata,len);
    Counter += len;
    osPrintb(DBG_UART_FIFO,"MT_Uart_InsertFIFO:",fifo,Counter);
    return osSucceed;
}
int MT_Uart_ReadPackage(uint8 *out,int len)
{
    int remain;
    int    index = 0;
    int slen;
    uint8    *p;
    osPrintb(DBG_UART_FIFO,"MT_Uart_InsertFIFO:",fifo,Counter);
    osPrintf(DBG_UART_FIFO,"MT_Uart_ReadPackage Counter:%d \r\n",Counter);
    while(index<Counter)
    {
        remain = (Counter-index);
        index = FindSOF(fifo,index,remain);
        if(index>=0)
        {
            osPrintf(DBG_UART_FIFO,"MT_Uart_ReadPackage GOT SOF at:%d \r\n",index);
            if(CheckPackage(&fifo[index],remain)==osSucceed)
            {
                slen = fifo[index+1] + PACKAGE_FRAME_SIZE;//total package length
                if(len>=slen)
                    len = slen;
                osMemcpy(out,&fifo[index],len);
                Counter -= slen;
                osMemcpy(&fifo[index],&fifo[index+slen],(Counter-index));
                return slen;
            }
            else
                index++;//move to next byte
        }
        else//can't find SOF
            break;
    }
    return 0;
}

static int FindSOF(uint8 *pdata, int startIndex, int len)
{
    int i;
    for(i=0;i<len;i++)
    {
        if(pdata[i+startIndex]==0xFE)
            return i;
    }
    return -1;
}
static int    CheckPackage(uint8 *pdata, int len)
{
    int ret = osFailed;
    uint8 slen = pdata[1]+PACKAGE_FRAME_SIZE;
    uint8 fcs,sum;
    if(slen>len)//package length error!
    {
        return osFailed;
    }
    fcs = pdata[slen-1];
    sum = GetFCS(&pdata[1],(slen-2));
    if(fcs==sum)
        ret = osSucceed;
    return ret;
}
static uint8 GetFCS(uint8 *pdata, int len)
{

    int     i;
    uint8    sum = 0;
    for(i=0; i<len ;i++)
    {
        sum ^= pdata[i];
    }
    return sum;
}