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


//WAITRIGHT==1 时，无论右鞋数据包是否丢失，左鞋数据包都上传，不等待
#define WAITRIGHT 1

typedef struct {
	unsigned char imubuff[72];
	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);

#define UNION_PACK {\
										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; \
										if(rev_io.ts%2==0) \
										send_protocol(0, 4, rev_io.imubuff, rev_io.imubuff_len);	\
}



extern uint16_t lose_pack_all;
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);
        }
				else if(ph->element_count > 0)
				{
					#if GAMEDATA_TO_RAWDATA
					if(gamedata_to_rawdata){
						UNION_PACK
					}else{
						queue_out(ph, &outp->h);
					}
					#else
					queue_out(ph, &outp->h);
					#endif			
				}
        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);
								
//										#if LOSSPACK_ENANBLE
//										extern uint16_t lose_pack_all;
//										lose_pack_all++;
//										#endif
										R_timestamp_re++;			
									
//										#if WAITRIGHT //右鞋数据包丢失，左鞋数据包存在，发送左鞋数据包和右鞋（上一个相同的数据包）			
//										UNION_PACK
//										#else
//										queue_out(ph, &outp->h);
//										#endif


										#if GAMEDATA_TO_RAWDATA
										if(gamedata_to_rawdata){
											UNION_PACK
										}else{
											queue_out(ph, &outp->h);
										}
										#else
										queue_out(ph, &outp->h);
										#endif
										
										if (R_timestamp_re == outp->h.ts)
										{
												UNION_PACK
												break;
										}
										if (ph->element_count <= 0)
										{
												sta = 0;
												break;
										}
								} while (1);
						}
						else
						{
								UNION_PACK
						}
				}	
        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, int32_t time_stamp, int16_t* _acc,int16_t* _gry, int16_t* front_mag, int16_t* back_mag, uint8_t _rssi)
{

	process_motion(time_stamp, _acc, _gry, front_mag, back_mag, _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);
	}
}