#include "ble_comm.h"
#include "vl53l1.h"
#include "tool.h"

typedef struct {
	unsigned char imubuff[64];
	unsigned char imubuff_len;
	uint16_t        ts;
}IMU_Hal_t;

typedef struct {
        IMU_Hal_t         h;
        IMU_Hal_t         s;
}IMU_Hal_RL_t;

typedef void (*process_motion_t)(uint8_t* left_buff,uint8_t left_len, uint8_t* right_buff, uint8_t right_len);

extern void get_game_package(uint8_t* buff_address, uint8_t * buff_len);
extern void integrate_left_right(uint8_t* left_buff,uint8_t left_len, uint8_t* right_buff, uint8_t right_len);
extern void get_foot_data(uint8_t *buf, uint8_t *buff_len);

IMU_Hal_t rev_s={.ts=0};
IMU_Hal_t rev={.ts=0};
IMU_Hal_t rev_io;
IMU_Hal_RL_t temp_IMU_DAT;
QUEUE_DEF(IMU_Hal_t, IMU_DAT_H_queue, size_8, QUEUE_MODE_OVERFLOW);
QUEUE_DEF(IMU_Hal_t, IMU_DAT_S_queue, size_8, QUEUE_MODE_OVERFLOW);

void IMU_dtalige(IMU_Hal_RL_t *outp, queue_t *ph, queue_t *ps, process_motion_t process_p)
{
    static char sta = 0;
    static unsigned short R_timestamp_re = 0;
    switch (sta)
    {
    case 0:
        if (ps->element_count > 0)
        {
            BLE_PRINT( "IMU_dtalige init.\r\n");
            sta = 1;
            queue_reset(ps);
            queue_reset(ph);
        }
        break;
    case 1:  
				while (ps->element_count > ph->element_count)
				{
						queue_out(ps, &outp->s);
						R_timestamp_re = outp->s.ts;
				}
				if (ps->element_count > 0)
				{
						queue_out(ps, &outp->s);
						R_timestamp_re++;
						if (R_timestamp_re != outp->s.ts)
						{
								do
								{
										BLE_PRINT( "lose packet %d\r\n", R_timestamp_re);
										extern uint8_t lost_packs_num;
										lost_packs_num++;
									
										R_timestamp_re++;
										queue_out(ph, &outp->h);
										if (R_timestamp_re == outp->h.ts)
										{
												queue_out(ph, &outp->h);
												process_p(outp->h.imubuff, outp->h.imubuff_len, outp->s.imubuff, outp->s.imubuff_len);
												get_game_package(rev_io.imubuff, &rev_io.imubuff_len); 
											
												rev_io.ts++;
												rev_io.imubuff[rev_io.imubuff_len + 1] = rev_io.ts;
												rev_io.imubuff[rev_io.imubuff_len] = rev_io.ts >> 8;
											
												send_protocol(0, 4, rev_io.imubuff, rev_io.imubuff_len);
												break;
										}
										if (ph->element_count <= 0)
										{
												sta = 0;
												break;
										}
								} while (1);
						}
						else
						{
								queue_out(ph, &outp->h);
							
								process_p(outp->h.imubuff, outp->h.imubuff_len, outp->s.imubuff, outp->s.imubuff_len);
								get_game_package(rev_io.imubuff, &rev_io.imubuff_len);
							
								rev_io.ts++;
								rev_io.imubuff[rev_io.imubuff_len + 1] = rev_io.ts;
								rev_io.imubuff[rev_io.imubuff_len] = rev_io.ts >> 8;
							
								send_protocol(0, 4, rev_io.imubuff, rev_io.imubuff_len);
						}
				}       
        break;
    }
}

void IMU_Dtalige(void)
{
		IMU_dtalige(&temp_IMU_DAT, &IMU_DAT_H_queue, &IMU_DAT_S_queue, integrate_left_right);
}

void IMU_Rec_data(uint8_t* pdat,uint8_t len)
{
	if(pdat[3]==4)
	{		
		memcpy(rev_s.imubuff,&pdat[4],pdat[1]);
		rev_s.imubuff_len=pdat[1]-7;
		rev_s.ts=((uint16_t)pdat[pdat[1]-3]<<8) |((uint16_t)pdat[pdat[1]-2]<<0);;
		queue_in(&IMU_DAT_S_queue,&rev_s);	
	}
}

void IMU_Process_motion_queue(uint8_t IS_HOST,int16_t* _acc,int16_t* _gry,int16_t* _mag,uint16_t _press,uint8_t _rssi)
{
	static uint8_t times = 0;
	times++;
	if(times == 10)
	{
//		uint16_t  distand = VL53L1_MEASURE();
//		if(distand > 4000)distand = 0;//如果times采集时间太短，出现溢出值。
//		SEGGER_RTT_printf(0,"distand=%d\r\n",distand);
//		JS_RTT_Print(distand,0,0);
		times = 0;
	}
			process_motion(_acc,_gry,_mag,_press,_rssi);
			get_foot_data(rev.imubuff,&rev.imubuff_len);
			rev.ts++;		
			
			if(IS_HOST)
			{
				queue_in(&IMU_DAT_H_queue,&rev);
			}
			else
			{
				rev.imubuff[rev.imubuff_len+1]=rev.ts;
				rev.imubuff[rev.imubuff_len]=rev.ts>>8;
				send_protocol(0,4,rev.imubuff,rev.imubuff_len+2);
			}
}