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


static unsigned char USEdtalige = 1;

void IMU_Dtalige_Rowdata_ON(void)
{
	USEdtalige = 0;
}

void IMU_Dtalige_Rowdata_OFF(void)
{
	USEdtalige = 1;
}

//.....................重写运动库接口.............................
unsigned char left_right_buff[60];
unsigned char left_right_data_len;

static void d_integrate_left_right(uint8_t* left_buff, uint8_t left_len, uint8_t* right_buff, uint8_t right_len)
{
	unsigned char L=0;
	for(int i=0;i<left_len;i++)
	{
		left_right_buff[L++]=left_buff[i];
	}
	for(int i=0;i<right_len;i++)
	{
		left_right_buff[L++]=right_buff[i];
	}
	left_right_data_len=L;
}

static void d_get_game_package(uint8_t* buff_address, uint8_t * buff_len)
{
	for(int i=0;i<left_right_data_len;i++)
	{
		buff_address[i]=left_right_buff[i];
	}
	*buff_len=left_right_data_len;
}

unsigned char mofoot_buff[30];
unsigned char mofoot_data_len;

static void d_process_motion(int32_t time_stamp, int16_t _acc[3], int16_t _gry[3],  int16_t front_mag[3], int16_t back_mag[3], uint8_t _rssi)
{
	int L=0;
	
	mofoot_buff[L++]=_acc[0]>>8;
	mofoot_buff[L++]=_acc[0];
	mofoot_buff[L++]=_acc[1]>>8;
	mofoot_buff[L++]=_acc[1];
	mofoot_buff[L++]=_acc[2]>>8;
	mofoot_buff[L++]=_acc[2];
	
	mofoot_buff[L++]=_gry[0]>>8;
	mofoot_buff[L++]=_gry[0];
	mofoot_buff[L++]=_gry[1]>>8;
	mofoot_buff[L++]=_gry[1];
	mofoot_buff[L++]=_gry[2]>>8;
	mofoot_buff[L++]=_gry[2];
	
	mofoot_buff[L++]=front_mag[0]>>8;
	mofoot_buff[L++]=front_mag[0];
	mofoot_buff[L++]=front_mag[1]>>8;
	mofoot_buff[L++]=front_mag[1];
	mofoot_buff[L++]=front_mag[2]>>8;
	mofoot_buff[L++]=front_mag[2];
	
	mofoot_buff[L++]=back_mag[0]>>8;
	mofoot_buff[L++]=back_mag[0];
	mofoot_buff[L++]=back_mag[1]>>8;
	mofoot_buff[L++]=back_mag[1];
	mofoot_buff[L++]=back_mag[2]>>8;
	mofoot_buff[L++]=back_mag[2];
	
//	mofoot_buff[L++]=_rssi;
//	
//	mofoot_buff[L++]=time_stamp>>24;
//	mofoot_buff[L++]=time_stamp>>16;
//	mofoot_buff[L++]=time_stamp>>8;
//	mofoot_buff[L++]=time_stamp;
	
	mofoot_data_len=L;
}

static void d_get_foot_data(uint8_t *buf, uint8_t *buff_len)
{
	for(int i=0;i<mofoot_data_len;i++)
	{
		buf[i]=mofoot_buff[i];
	}
	*buff_len=mofoot_data_len;
}
//.....................重写运动库接口.............................end

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;
										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%2==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%2==0)
								send_protocol(0, 4, rev_io.imubuff, rev_io.imubuff_len);
						}
				}       
        break;
    }
}

void IMU_Dtalige(void)
{
	if( USEdtalige )
	{
		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);
		if( USEdtalige )
		{
			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)
{
	if( USEdtalige )
	{
		process_motion(time_stamp, _acc, _gry, front_mag, back_mag, _rssi);

		get_foot_data(rev.imubuff,&rev.imubuff_len);
	}
	else
	{
		d_process_motion(time_stamp, _acc, _gry, front_mag, back_mag, _rssi);

		d_get_foot_data(rev.imubuff,&rev.imubuff_len);
	}
	rev.ts++;		
		
	if(IS_HOST)
	{
		if( USEdtalige )
		{
			queue_in(&IMU_DAT_H_queue,&rev);
		}
		else
		{
			d_integrate_left_right(rev.imubuff, rev.imubuff_len, rev_s.imubuff, rev_s.imubuff_len);
			d_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);
		}
	}
	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);
	}
}