#include "DanceFoot.h"

/*
* 初始化脚步的数据
*/
DanceFoot::DanceFoot(int LEFT_OR_RIGHT)
{
	left_or_right = LEFT_OR_RIGHT;

	foot_rssi = 0;
	foot_zupt = 1;

	rssi_zupt = 1;
	
	max_rssi = 0;
	min_rssi = 100;

	memset(attitude, 0, 3 * sizeof(float));
	memset(globalPos, 0, 3 * sizeof(float));
	memset(offsetPos, 0, 3 * sizeof(float));
	memset(last_pos_g, 0, 2 * sizeof(float));
	memset(pos, 0, 2 * sizeof(float));

	last_pos[0] = 0;  last_pos[1] = 0;

	last_zupt = 1;

	danceTraj[0] = 0; danceTraj[1] = 0;

	rotate_err = 0;

	rotateCor[0] = 1.0; rotateCor[1] = 0.0; rotateCor[2] = 0.0; rotateCor[3] = 1.0;

	zupt_count = 1;

	pos_index = -1;
}

/*
* 设置脚的姿态，当前位置，鞋子上来的位置偏移量, rssi, zupt变量
*/

int DanceFoot::setAttitude(float  att[3])
{
	memcpy(attitude, att, 3 * sizeof(float));

	return 0;
}

int DanceFoot::setPosGlobal(float pos[3])
{
	memcpy(globalPos, pos, 3 * sizeof(float));

	return 0;
}

int DanceFoot::setPosOffset(float pos[3])
{
	memcpy(offsetPos, pos, 3 * sizeof(float));

	return 0;
}

int DanceFoot::setFootZupt(int zupt)
{
	foot_zupt = zupt;

	return 0;
}

int DanceFoot::setFootRssi(int rssi)
{
	foot_rssi = rssi;
	
	return 0;
}

int DanceFoot::setMaxRssi(int rssi)
{
	max_rssi = rssi;

	return 0;
}

int DanceFoot::setMinRssi(int rssi)
{
	min_rssi = rssi;
	
	return 0;
}

/*
*  通过判断rssi的变化范围来确定是否没有位移，
*  原地踏步返回0 ，有明显变化的返回1
*/
int DanceFoot::isMoveRssi(int rssi, int zupt)
{
	int result = 1;

	if (zupt == 0)
	{
		if (rssi > max_rssi)
		{
			setMaxRssi(rssi);
		}

		if (rssi < min_rssi)
		{
			setMinRssi(rssi);
		}
	}
	else
	{
		if (rssi_zupt == 0)
		{
			if (max_rssi - min_rssi < 3)
			{
				result = 0;
			}
		}

		setMaxRssi(0);
		setMinRssi(100);
	}
	
	rssi_zupt = zupt;

	return result;
}
/*
* 处理全局位置的边界问题，全局位置应为40cmX40cm区域
*/
int DanceFoot::posBoundary(float pos[3])
{
	float posLength = sqrt(pos[0] * pos[0] + pos[1] * pos[1]);

	if (posLength > 0.25)
	{
		pos[0] = pos[0] / posLength * 0.25;
		pos[1] = pos[1] / posLength * 0.25;
	}

	return 0;
}

/*
* 通过角度来判断跳舞毯的方向
* 
*/
int DanceFoot::getGameDiretion(float *pos)
{
	float x = pos[0] ;
	float y = pos[1] ;

	if (left_or_right == LEFT_FOOT)
	{
		std::cout << "left_foot ";
	}
	else
	{
		std::cout << "right_foot ";
	}

	if((left_or_right == LEFT_FOOT&& sqrt((x+0.15) * (x+0.15) + y * y) < 0.1)
		|| (left_or_right == RIGHT_FOOT && sqrt((x - 0.15) * (x - 0.15) + y * y) < 0.1))
	{
		if (left_or_right == LEFT_FOOT)
		{
			pos[0] = -0.5f;
		}
		else
		{
			pos[0] = 0.5f;
		}
		pos[1] = 0.0f;

		std::cout << "danceGame result: zeros point. " << std::endl;
		return MOTION_STEP;
	}

	float angle = atan2(y, x);


	if (angle > 0.0f)
	{
		if (angle < 1.5708)
		{
			std::cout << "danceGame result: right up. " << std::endl;
			return MOTION_RIGHT_UP;
		}
		else
		{
			std::cout << "danceGame result: left up. " << std::endl;
			return MOTION_LEFT_UP;
		}
	}
	else
	{
		if (angle > -1.5708)
		{
			std::cout << "danceGame result: right down. " << std::endl;
			return MOTION_RIGHT_DOWN;
		}
		else
		{
			std::cout << "danceGame result: left down. " << std::endl;
			return MOTION_LEFT_DOWN;
		}
	}

	return MOTION_STEP;
}




/*
* 计算鞋子的全局位置
*/

int DanceFoot::calGlobalPos(float pos_g[3], int rssi, int zupt, int press, int* acc)
{


	float pos_zero[2] = { 0, 0 };

	float pos_offset[2] = {0, 0};

	float pos_g_offset[2] = { 0,0 };

	pos_g_offset[0] = pos_g[0] - last_pos_g[0];
	pos_g_offset[1] = pos_g[1] - last_pos_g[1];

	if (zupt == 1)
	{
		if (last_zupt == 1)
		{
			pos_g_offset[0] = 0;
			pos_g_offset[1] = 0;
		}
	}//略去惯导的补偿项，这样是避免补偿过大，导致跳舞的轨迹有拖尾情况

	last_pos_g[0] = pos_g[0];
	last_pos_g[1] = pos_g[1];

	pos[0] += pos_g_offset[0];
	pos[1] += pos_g_offset[1]; 


	pos_offset[0] = pos[0] - last_pos[0];
	pos_offset[1] = pos[1] - last_pos[1];

	/*
	if (fabsf(pos_offset[0]) > 10 || fabsf(pos_offset[1]) > 10)
	{
		std::cout << "dancegame receive unnormal pos_offset: " << pos_offset[0] << " " << pos_offset[1] << std::endl;
		pos_offset[0] = 0; pos_offset[1] = 0;
	}
	*/

	if (last_zupt == 1 && zupt == 1)
	{
		data.clear();
		
	}
	if (zupt == 0 || (zupt == 1 && last_zupt == 0))
	{
		posData pos_data;
		pos_data.pos_x = rotateCor[0] * (danceTraj[0] + pos_offset[0]) + rotateCor[1] * (danceTraj[1] + pos_offset[1]);
		pos_data.pos_y = rotateCor[2] * (danceTraj[0] + pos_offset[0]) + rotateCor[3] * (danceTraj[1] + pos_offset[1]);

		pos_data.rssi = rssi;

		data.push_back(pos_data);
	}

	if (zupt == 1 && rssi < DANCEGAME_MIN_RSSI)
	{
		setLastPos(pos);

		setDanceTraj(pos_zero);
	}



	if (zupt == 1 && last_zupt == 0)
	{
		/*
		* 检测rssi的信号变化，为直线特征做准备
		*/
		int rssi_min = data[0].rssi;
		int rssi_max = data[0].rssi;

		int rssi_min_index = 0;
		int rssi_max_index = 0;

		float pos_x_min = data[0].pos_x;
		float pos_x_max = data[0].pos_x;

		float pos_y_min = data[0].pos_y;
		float pos_y_max = data[0].pos_y;

		int len = data.size();

		for (int i = 0; i < data.size(); i++)
		{
			if (data[i].rssi < rssi_min)
			{
				rssi_min = data[i].rssi;
				rssi_min_index = i;
			}

			if (data[i].rssi >= rssi_max)
			{
				rssi_max = data[i].rssi;

				rssi_max_index = i;
			}

			if (data[i].pos_x > pos_x_max)
			{
				pos_x_max = data[i].pos_x;
			}

			if (data[i].pos_x < pos_x_min)
			{
				pos_x_min = data[i].pos_x;
			}

			if (data[i].pos_y > pos_y_max)
			{
				pos_y_max = data[i].pos_y;
			}

			if (data[i].pos_y < pos_y_min)
			{
				pos_y_min = data[i].pos_y;
			}
		}

		if (data.size() > 5)
		{
			int rssiSignal = rssiFeature(data);

			switch(rssiSignal)
			{
				case 1:

					break;
				case 2://回到原点
					if(left_or_right == LEFT_FOOT)
						danceTraj[0] = -0.15; 
					else
						danceTraj[0] = 0.15;

					danceTraj[1] = 0.0;

					pos_offset[0] = 0.0; pos_offset[1] = 0.0;
					break;

				case 0:

					if (fabsf(data[0].pos_y - data[len - 1].pos_y) > 0.3&& fabsf(pos_x_max - pos_x_min) < 0.2)
					{
						/////////////////检测到有跨越象限的情况，左脚当然会经过y = 0的时候, 而x = 0.1则为一个经验值

						std::cout << "detect pass special point" << std::endl;

						float special_offset[2] = { 0, 0 };

						float mid_x = 0.5 * (data[0].pos_x + data[len - 1].pos_x);

						float mid_y = 0.5 * (data[0].pos_y + data[len - 1].pos_y);


						if (pos_x_max - pos_x_min < 0.2)
						{
							if (left_or_right == LEFT_FOOT)
							{
								special_offset[0] = -0.2 - mid_x;
							}
							else
							{
								special_offset[0] = 0.2 - mid_x;
							}

						}

						special_offset[1] = -mid_y;


						pos_offset[0] = pos_offset[0] + special_offset[0];

						pos_offset[1] = pos_offset[1] + special_offset[1];

					}
					/**/
					//检测到横移的情况
					else if (fabs(data[0].pos_x - data[len - 1].pos_x) > 0.3)
					{
						std::cout << "detected horizontal line by pos_x" << std::endl;

						float special_offset[2] = { 0,0 };

						float mid_x = 0.5 * (data[0].pos_x + data[len - 1].pos_x);

						float mid_y = 0.5 * (data[0].pos_y + data[len - 1].pos_y);

						if (pos_y_max - pos_y_min < 0.15)
						{
							if (left_or_right == LEFT_FOOT)
							{
								special_offset[0] = -0.15 - mid_x;

							}
							else
							{
								special_offset[0] = 0.15 - mid_x;
							}

							if (data[0].pos_y > 0)
							{
								special_offset[1] = 0.2 - mid_y;
							}
							else
							{
								special_offset[1] = -0.2 - mid_y;
							}

							std::cout << "debug :" << special_offset[0] << " " << special_offset[1] << std::endl;
							pos_offset[0] = pos_offset[0] + special_offset[0];

							pos_offset[1] = pos_offset[1] + special_offset[1];
						}
						/*
						else
						{
							special_offset[0] = -mid_x;

							if (rssi_min <= 35 && data[0].rssi - rssi_min > 10 && data[len - 1].rssi - rssi_min > 10)
							{
								special_offset[1] = -mid_y;
							}

							pos_offset[0] = pos_offset[0] + special_offset[0];

							pos_offset[1] = pos_offset[1] + special_offset[1];

							std::cout << "debug2 :" << special_offset[0] << " " << special_offset[1] << std::endl;

						}
						*/

					}


					break;
				default:
						break;
		
	
			}

		}
		
		if ((sqrt(pos_offset[0] * pos_offset[0] + pos_offset[1] * pos_offset[1]) < 0.1))
		{
			pos_offset[0] = 0; pos_offset[1] = 0;

			if (acc_status == 0) 
			{
				std::cout << "appear error command on zero_vel status (dance__foot) " << endl;
			}
		}

		danceTraj[0] = danceTraj[0] + pos_offset[0];
		danceTraj[1] = danceTraj[1] + pos_offset[1];

		
		if (rssi < DANCEGAME_MIN_RSSI)
		{
			setDanceTraj(pos_zero);
		}

		setLastPos(pos);

		pos_offset[0] = 0.0; pos_offset[1] = 0.0;

		float dancePos[2];

		dancePos[0] = rotateCor[0] * danceTraj[0] + rotateCor[1] * danceTraj[1];
		dancePos[1] = rotateCor[2] * danceTraj[0] + rotateCor[3] * danceTraj[1];

		pos_index = getGameDiretion(dancePos);
	}

	float dancePos[2];

	dancePos[0] = rotateCor[0] * (danceTraj[0] + pos_offset[0]) + rotateCor[1] * (danceTraj[1] + pos_offset[1]);
	dancePos[1] = rotateCor[2] * (danceTraj[0] + pos_offset[0]) + rotateCor[3] * (danceTraj[1] + pos_offset[1]);


	setLastZupt(zupt);

	gamePos[0] = dancePos[0];
	gamePos[1] = dancePos[1];
	gamePos[2] = pos_g[2];

	if (zupt == 1)
	{
		zupt_count = 3;
	}
	else
	{
		zupt_count--;
	}

	if (zupt_count <= 0)
	{
		pos_index = -1;
	}

	acc_status = getAccStatus(zupt, acc, acc_max, acc_min);

	return pos_index;

}

/*
* 打印pos和RSSI的
*/

void DanceFoot::printPosData(vector<posData> data)
{
	for (int i = 0; i < data.size(); i++)
	{
		std::cout << data[i].pos_x << " " <<data[i].pos_y << " "<< data[i].rssi << std::endl;
	}
}

/*
* 通过位移以及RSSI来判断是否为原地踏步
*/

int DanceFoot::isMove(vector<posData> data)
{
	int n = data.size();

	if (n <= 1)
	{
		return 0;
	}

	int rssi_left = data[0].rssi;
	int rssi_right = data[(n-1)].rssi;

	float x = data[0].pos_x - data[(n-1)].pos_x;
	float y = data[0].pos_y - data[(n-1)].pos_y;

	int rssi_min = rssi_left;

	for (int i = 0; i < n; i++)
	{
		if (rssi_min > data[i].rssi)
			rssi_min = data[i].rssi;
	}
		
	if (abs(rssi_right - rssi_left) > 10 && rssi_min > 40
		&& fabsf(x) > 0.1 )
	{
		std::cout << "检测到左右横移" << std::endl;

		if (x < 0)
			return -1;
		else
			return 1;
	}

	return 0;
}

/*
* 设置上一次的last_pos
*/
void DanceFoot::setLastPos(float* lastPos)
{
	last_pos[0] = lastPos[0];
	last_pos[1] = lastPos[1];
}


/*
* 设置上一次的zupt
*/

void DanceFoot::setLastZupt(int lastZupt)
{
	last_zupt = lastZupt;
}

/*
* 设置跳舞轨迹
*/

void DanceFoot::setDanceTraj(float* traj)
{
	if(left_or_right == LEFT_FOOT)
		danceTraj[0] = traj[0] - 0.15;
	else
		danceTraj[0] = traj[0] + 0.15;

	danceTraj[1] = traj[1];
}

/*
* 计算斜率
*/

float DanceFoot::calSlope(vector<posData> data)
{
	int i = 0;
	int len = data.size();

	float x2_sum = 0, x_sum = 0, xy_sum = 0, y_sum = 0;

	float slope = 0.0f;

	for (i = 0; i < len; i++)
	{
		x2_sum += (data[i].pos_x * data[i].pos_x);

		x_sum += (data[i].pos_x);
		
		y_sum += (data[i].pos_y);

		xy_sum += (data[i].pos_x * data[i].pos_y);
	}

	//b = (x2_sum * y_sum - x_sum * xy_sum) / ((len * x2_sum) - x_sum * x_sum);

	slope = (len * xy_sum - x_sum * y_sum) / (len * x2_sum - x_sum * x_sum);

	std::cout << len << " " << x2_sum << " " << x_sum << " " << y_sum << " " << xy_sum << " " << atan(slope) << std::endl;

	return slope;

}

float DanceFoot::calCor(vector<posData> data)
{
	//printPosData(data);

	float start_x = data[0].pos_x;
	float start_y = data[0].pos_y;

	int len = data.size();

	float end_x = data[len - 1].pos_x;
	float end_y = data[len - 1].pos_y;

	float a = start_y - end_y;
	float b = -(start_x - end_x);

	float c = start_x * end_y - start_y * end_x;

	float d = 1 / sqrt(a * a + b * b);

	float  distance = 0;
	
	for (int i = 0; i < len - 1; i++)
	{
		float diatance_tmp = (a * data[i].pos_x + b * data[i].pos_y + c) * d;

		if (fabsf(diatance_tmp) > fabsf(distance))
		{
			distance = diatance_tmp;
		}
	}

	return distance;
}


float DanceFoot::getGamePos(int index)
{
	if (index < 0 || index > 2)
		return -1;
	return gamePos[index];
}

int DanceFoot::rssiFeature(vector<posData> data)
{
	//printPosData(data);

	int startRssi = data[0].rssi;
	int dataLength = data.size();

	int endRssi = data[dataLength - 1].rssi;

	int minDistance = 99;

	int minIndex = 0;//默认为从原点触发

	int i = 0;

	int min_rssi = data[0].rssi;

	while (i < dataLength)
	{
		if (min_rssi > data[i].rssi)
		{
			min_rssi = data[i].rssi;

			minIndex = i;
		}

		i++;
	}

	i = 0;

	int maybeStraight = 0;

	if (min_rssi < 25)
	{
		min_rssi = min_rssi + 2;
	}

	while (i < dataLength)
	{
		if (min_rssi >= data[i].rssi)
		{
			if (fabs(data[dataLength - 1].pos_y - data[i].pos_y) > 0.1 && fabs(data[0].pos_y - data[i].pos_y) > 0.1)
			{
				maybeStraight = 1;
				break;
			}
		}
		i++;
	}

	float length_tmp = sqrt((data[0].pos_y - data[dataLength - 1].pos_y) * (data[0].pos_y - data[dataLength - 1].pos_y)
		+ (data[0].pos_x - data[dataLength - 1].pos_x) * (data[0].pos_x - data[dataLength - 1].pos_x));

	if (endRssi < 28)
	{
		return 2;
	}

	if ((maybeStraight)|| (fabsf(data[0].pos_y - data[dataLength - 1].pos_y)>0.4 && fabsf(data[0].pos_x - data[dataLength - 1].pos_x) < 0.1))
	{
		std::cout << "Maybe straight line" << std::endl;
		return 0;
	}

	if (startRssi < 28 && endRssi - startRssi > 10 && length_tmp > 0.08)
	{
		std::cout << "function debug: zero start!!!!!" << std::endl;
		return 1; //标志为从原点出发
	}

	if ( endRssi < 28 && startRssi - endRssi > 10 && length_tmp > 0.08)
	{
		std::cout << "function debug: zero end!!!!!" << std::endl;
		return 2; //标志回到原点
	}


	std::cout << "startRssi: " << startRssi << " endRssi: " << endRssi << " min_rssi: " << min_rssi << " length_tmp: " << length_tmp <<std::endl;
	return 0; //则是标志为其他的特征
}

int DanceFoot::getAccStatus(int zupt, int* acc, int* max_acc, int* min_acc)
{
	/*
	* 在游戏测试的时候发现有左右乱飞的情况，现在以及乱跳的情况，现在用加速度过滤这一个情况
	* 尽量不在嵌入式上修改
	*/
	if (zupt)
	{
		memcpy(max_acc, acc, 3 * sizeof(int));
		memcpy(min_acc, acc, 3 * sizeof(int));
	}
	else
	{
		for (int i = 0; i < 3; i++)
		{
			if (max_acc[i] < acc[i])
			{
				max_acc[i] = acc[i];
			}

			if (min_acc[i] > acc[i])
			{
				min_acc[i] = acc[i];
			}
		}
	}

	for (int i = 0; i < 3; i++)
	{
		if (max_acc[i] - min_acc[i] > 1024)
		{
			return 1;
		}
	}

	return 0;
}