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

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);

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);
        }
        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;
//										static uint32_t last_R_timestamp_re = 0;
//										static uint32_t lost_sum = 0;
//										
//										if((R_timestamp_re - last_R_timestamp_re) == 1){
//											lost_sum++;
//										}else{
//											if(lose_pack_all < lost_sum)lose_pack_all = lost_sum;
//											lost_sum = 0;
//										}
//										last_R_timestamp_re = R_timestamp_re;
										lose_pack_all++;
										#endif
										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;
												if(rev_io.ts%3==0)
												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;
								if(rev_io.ts%3==0)
								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, 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);
	}
}