//tbh:原算法
#include"expOperate.h"
#include<iostream>
#include<vector>
#include<map>
#include<sstream>
#include<string>

std::map<int, std::vector<float> > sensorData;
bool pause=false;
unsigned int systemTime=0;
int times=0;

//tbh:显示模块
std::string printData = "";//tbh:显示数据

//tbh:执行模块
std::map<int,int> motorData;//tbh:马达数据
std::map<int,int> mapColoredLight;//tbh:彩灯数据

//tbh:输出执行数据
void GetData(char *data)
{
	std::string string="";
	std::map<int,int>::iterator it;
	char buf[10];
	int port,speed;

	for(it=motorData.begin();it!=motorData.end();it++)
	{
		port=it->first;
		sprintf(buf,"%d*",port);
		string+=buf;

		speed=it->second;
		sprintf(buf,"%d*",speed);
		string+=buf;
		string+=" ";
	}

	//tbh:彩灯数据
	string += "l ";
	for(it=mapColoredLight.begin();it!=mapColoredLight.end();it++)
	{
		port=it->first;
		sprintf(buf,"%d*",port);
		string+=buf;

		speed=it->second;
		sprintf(buf,"%d*",speed);
		string+=buf;
		string+=" ";
	}

	//tbh:显示模块
	string += "p ";
	if(printData != "")
	{
		string += printData;
		string += " ";
	};

	//tbh:原算法
	strcpy(data,string.c_str());
	data[string.size()]='\0';
}

//tbh:原算法
bool IsPause()
{
	return pause;
}
void updateTimes()
{
	times++;
	if(times>1)//100
	{
		excute();
		times=0;
	};
}
void excute()
{
	pause=true;
}
void Start()
{
	pause=false;
}
void Run()
{
	times=0;
	main();
	while(true)
		excute();
}
void SetData(const char *data)
{
	std::stringstream dataStream(data);
	std::string dataString = "";

	std::getline(dataStream, dataString, ' ');
	systemTime = (unsigned int)atoi(dataString.c_str());

	while(std::getline(dataStream, dataString, ' '))
	{
		std::stringstream sensorStream(dataString);
		std::string sensorString = "";

		std::getline(sensorStream, sensorString, '*');
		int port = atoi(sensorString.c_str());
		sensorData[port].clear();

		while(std::getline(sensorStream, sensorString, '*'))
			sensorData[port].push_back(atof(sensorString.c_str()));
	}

	//std::cout<<data<<std::endl;
}
void delay(unsigned long dt)
{
	unsigned long startTime = systemTime;
	while(true)
	{
		updateTimes();
		if(systemTime >= startTime + dt)
			break;
	}
}
void E2RCU_Init()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////

//tbh:执行模块
void Set3CLed(int which, int Color)//tbh:设彩灯
{
	updateTimes();
	mapColoredLight[which]=Color;
}
void SetMotor(int which, int speed)//tbh:设马达
{
	updateTimes();
	motorData[which]=speed;
}
//tbh:打印模块
void SetLCDChar(unsigned char lx)
{
	updateTimes();
	char buf[10];
	sprintf(buf, "%d", lx);
	printData = buf;
}
void SetLCDInt(unsigned int lx)
{
	updateTimes();
	char buf[10];
	sprintf(buf, "%d", lx);
	printData = buf;
}
void SetLCDDouble(double fl)//tbh:浮点数据显示
{
	updateTimes();
	char buf[100];
	sprintf(buf, "%f", fl);
	printData = buf;
}

//tbh:竞赛模块
void GoLineForRight(//tbh:走线数右线
	int leftMotor,//左马达端口
	int rightMotor, //右马达端口
	int trace,//寻机卡端口
	unsigned int count)//右线数,寻机卡最右位置读到的黑线数超过count时,则停止并跳出函数
{
	//查反转情况
	int iMotor1 =100;
	int iMotor2 =100;

	//查寻机卡
	unsigned int iTurn=2;
	unsigned int iIn=0;
	unsigned int iFor=0;
	while (1)
	{
		unsigned int iGet[8];
		for(int i=0;i<8;i++)
			iGet[i]=GetTrace(trace,i+1);
		
		//设马达
		unsigned int iLeft=iGet[0]+iGet[1]+iGet[2];
		unsigned int iRight=iGet[6]+iGet[5]+iGet[4];
		unsigned int iMid=iGet[3]+iGet[7];
		if (iMid)
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,iMotor2);
		}
		else if (iRight)
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,0);
		}
		else if (iLeft)
		{
			SetMotor(leftMotor,0);
			SetMotor(rightMotor,iMotor2);
		}
		
		//没线时回头
		else if (iTurn==1)
		{
			SetMotor(leftMotor,0);
			SetMotor(rightMotor,iMotor2);
			
			//记录过左线情况
			if(iIn)
			{
				iFor++;
				iIn=0;
			};
		}
		else if (iTurn==2)
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,0);
			
			//记录过左线情况
			if(iIn)
			{
				iFor++;
				iIn=0;
			};
		};
		
		//记录转弯情况
		if(iGet[6])
		{
			iTurn=2;
			iIn=1;
		}
		else if(iGet[0])
			iTurn=1;

		//够目标线数
		if(iFor==count-1)
			break;
	}

	//马达静止
	SetMotor(leftMotor,0);
	SetMotor(rightMotor,0);
}
void GoLineForLeft(//tbh:走线数左线
	int leftMotor,//左马达端口
	int rightMotor, //右马达端口
	int trace,//寻机卡端口
	unsigned int count)//左线数,寻机卡最左位置读到的黑线数超过count时,则停止并跳出函数
{
	//查反转情况
	int iMotor1 =100;
	int iMotor2 =100;
	
	//找线
	unsigned int iLine=0;
	unsigned int iGet[8];
	for(int i=0;i<8;i++)
		iLine+=GetTrace(trace,i+1);

	if(!iLine)
	{
		SetMotor(leftMotor,iMotor1);
		SetMotor(rightMotor,iMotor2);
		
		//查寻机卡
		while (1)
		{
			iLine=0;
			for(int i=0;i<8;i++)
				iLine+=GetTrace(trace,i+1);
			
			//设马达
			if (iLine)
				break;
		}
	};

	//查寻机卡
	unsigned int iTurn=1;
	unsigned int iIn=0;
	unsigned int iFor=0;
	while (1)
	{
		for(int i=0;i<8;i++)
			iGet[i]=GetTrace(trace,i+1);
		
		//设马达
		unsigned int iLeft=iGet[0]+iGet[1]+iGet[2];
		unsigned int iRight=iGet[6]+iGet[5]+iGet[4];
		unsigned int iMid=iGet[3]+iGet[7];
		if (iMid)
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,iMotor2);
		}
		else if (iLeft)
		{
			SetMotor(leftMotor,0);
			SetMotor(rightMotor,iMotor2);
		}
		else if (iRight)
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,0);
		}
		
		//没线时回头
		else if (iTurn==1)
		{
			SetMotor(leftMotor,0);
			SetMotor(rightMotor,iMotor2);
			
			//记录过左线情况
			if(iIn)
			{
				iFor++;
				iIn=0;
			};
		}
		else if (iTurn==2)
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,0);
			
			//记录过左线情况
			if(iIn)
			{
				iFor++;
				iIn=0;
			};
		};
		
		//记录转弯情况
		if(iGet[0])
		{
			iTurn=1;
			iIn=1;
		}
		else if(iGet[6])
			iTurn=2;

		//够目标线数
		if(iFor==count)
			break;
	}
	    
	//马达静止
	SetMotor(leftMotor,0);
	SetMotor(rightMotor,0);
}
void GoLine(//tbh:走线
	int leftMotor,//左马达端口
	int rightMotor, //右马达端口
	int trace,//寻机卡端口
	unsigned int count)//横线数,寻机卡读到的横线数超过count时,则停止并跳出函数
{
	//查反转情况
	int iMotor1 =100;
	int iMotor2 =100;
	
	//找线
	unsigned int iLine=0;
	unsigned int iGet[8];
	unsigned int iIn=0;

	//查寻机卡
	while (1)
	{
		for(int i=0;i<8;i++)
			iGet[i]=GetTrace(trace,i+1);
		unsigned int iMid=iGet[3]+iGet[7];
		unsigned int iLeft=iGet[0]+iGet[1]+iGet[2];
		unsigned int iRight=iGet[6]+iGet[5]+iGet[4];

		//tbh:撞够横行则退出
		if (iGet[6]+iGet[5]&&
			iGet[0]+iGet[1])
		{
			if(iIn==0)
			{
				iLine++;
				if(iLine==count)
					break;
			};
			iIn=1;
		}
		else
			iIn=0;
		
		//设马达
		if (iMid)
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,iMotor2);
		}
		else if (iLeft&&
			!iGet[6])
		{
			SetMotor(leftMotor,0);
			SetMotor(rightMotor,iMotor2);
		}
		else if (iRight&&
			!iGet[0])
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,0);
		}
		else
		{
			SetMotor(leftMotor,iMotor1);
			SetMotor(rightMotor,iMotor2);
		}
	}
	    
	//马达静止
	SetMotor(leftMotor,0);
	SetMotor(rightMotor,0);
}
void TurnRight(//tbh:右转
	int leftMotor,//左马达端口
	int rightMotor, //右马达端口
	int compass, //指南针端口
	unsigned int angle)//指南针的读数大于angle时,则停止并跳出函数
{
	//查反转情况
	int iMotor1 =100;
	int iMotor2 =-100;
	    
	//直走
	SetMotor(leftMotor,iMotor1);
	SetMotor(rightMotor,iMotor2);

	//查指南针
	while (1)
	{
		unsigned int iGet=GetCompassB(compass);
		unsigned int iAway;
		if(angle>iGet)
			iAway=angle-iGet;
		else
			iAway=iGet-angle;
		if(iAway<30||
			iAway>330)
    		break;
	}
	    
	//马达静止
	SetMotor(leftMotor,0);
	SetMotor(rightMotor,0);
}
void TurnLeft(//tbh:左转
	int leftMotor,//左马达端口
	int rightMotor, //右马达端口
	int compass, //指南针端口
	unsigned int angle)//指南针的读数小于angle时,则停止并跳出函数
{
	//查反转情况
	int iMotor1 =-100;
	int iMotor2 =100;
	    
	//直走
	SetMotor(leftMotor,iMotor1);
	SetMotor(rightMotor,iMotor2);

	//查指南针
	while (1)
	{
		unsigned int iGet=GetCompassB(compass);
		unsigned int iAway;
		if(angle>iGet)
			iAway=angle-iGet;
		else
			iAway=iGet-angle;
		if(iAway<30||
			iAway>330)
    		break;
	}
	    
	//马达静止
	SetMotor(leftMotor,0);
	SetMotor(rightMotor,0);
}
void Back(//tbh:后退
	int leftMotor,//左马达端口
	int rightMotor, //右马达端口
	int ray, //红外测距端口
	unsigned int distance)//红外测距的读数大于distance时,则停止并跳出函数
{
	//查反转情况
	int iMotor1 =-100;
	int iMotor2 =-100;
	    
	//直走
	SetMotor(leftMotor,iMotor1);
	SetMotor(rightMotor,iMotor2);

	//查红外距离
	while (1)
	{
		unsigned int iGet=GetIRDist(ray);
		if(iGet>distance)
    		break;
	}
	    
	//马达静止
	SetMotor(leftMotor,0);
	SetMotor(rightMotor,0);
}
void Go(//tbh:直走
	int leftMotor,//左马达端口
	int rightMotor, //右马达端口
	int ray, //红外测距端口
	unsigned int distance)//红外测距的读数小于distance时,则停止并跳出函数
{
	//查反转情况
	int iMotor1 =100;
	int iMotor2 =100;
	    
	//直走
	SetMotor(leftMotor,iMotor1);
	SetMotor(rightMotor,iMotor2);

	//查红外距离
	while (1)
	{
		unsigned int iGet=GetIRDist(ray);
		if(iGet<distance)
    		break;
	}
	    
	//马达静止
	SetMotor(leftMotor,0);
	SetMotor(rightMotor,0);
}
//tbh:原算法
int GetLightSensor(int which)
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 2)
			return 0;

		int type = (int)sensorData[which].at(0);
		if(type != 4)
			return 0;
		else
			return (unsigned int)sensorData[which].at(1)*16;
	}
	catch(...)
	{
		return 0;
	}
}
void SetWaitforDark(int which, int value)
{
	while(true)
	{
		int l = GetLightSensor(which);
		if(l < value)
			break;
	}
}
void SetWaitforLight(int which, int value)
{
	while(true)
	{
		int l = GetLightSensor(which);
		if(l > value)
			break;
	}
}
void SetWaitforDarker(int which, int value)
{
	int start = GetLightSensor(which);
	while(true)
	{
		int l = start - GetLightSensor(which);
		if(l > value)
			break;
	}
}
void SetWaitforLighter(int which, int value)
{
	int start = GetLightSensor(which);
	while(true)
	{
		int l = GetLightSensor(which) - start;
		if(l > value)
			break;
	}
}
int GetTouch(int which)
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 2)
			return 0;

		int type = (int)sensorData[which].at(0);
		if(type != 1)
			return 0;
		else
			return (unsigned int)sensorData[which].at(1);
	}
	catch(...)
	{
		return 0;
	}
}
void SetWaitForPress(int which)
{
	while(true)
		if(GetTouch(which) == 1)
			break;
}
void SetWaitForRelease(int which)
{
	while(true)
		if(GetTouch(which) == 0)
			break;
}
void SetWaitForBump(int which)
{
	while(true)
		if(GetTouch(which) == 1)
			break;

	while(true)
		if(GetTouch(which) == 0)
			break;
}
void SetWaitForTime(float s)
{
	unsigned long startTime = systemTime;
	if(s == 0)
		return;
	else if(s<0.01)
		s = 0.01;

	unsigned long delayTime = (unsigned long)(s * 100);
	while(true)
	{
		updateTimes();
		if(systemTime >= startTime + delayTime)
			break;
	}
}
///////////////////////////////////////////////////////////////////////////////////////////////////

int GetRemoIR(int which)//火焰检测
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 2)
			return 0;

		int type = (int)sensorData[which].at(0);
		if(type != 5)
			return 0;
		else
			return (unsigned int)sensorData[which].at(1);
	}
	catch(...)
	{
		return 0;
	}
}
int GetIRDete(int which)//红外测障
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 2)
			return 0;

		int type = (int)sensorData[which].at(0);
		if(type != 0)
			return 0;
		else
			return (unsigned int)sensorData[which].at(1);
	}
	catch(...)
	{
		return 0;
	}
}
int GetCompassB(int which)//指南针
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 2)
			return 0;

		int type = (int)sensorData[which].at(0);
		if(type != 2)
			return 0;
		else
			return (unsigned int)sensorData[which].at(1);
	}
	catch(...)
	{
		return 0;
	}
}
int GetIRDist(int which)//红外测距
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 2)
			return 0;

		int type = (int)sensorData[which].at(0);
		if(type != 3)
			return 0;
		else
			return (unsigned int)sensorData[which].at(1);
	}
	catch(...)
	{
		return 0;
	}
}
int GetTrace(int which,int index)//寻机卡
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 9 ||
			(index<1||
				index>8))
			return 255;

		int type = (int)sensorData[which].at(0);
		if(type != 7)
			return 255;
		else
			return (unsigned int)sensorData[which].at(index);
	}
	catch(...)
	{
		return 255;
	}
}
int GetCompoI(int which,int command)//复眼
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 8 ||
			(command>13&&
				command<254))
			return 255;

		int type = (int)sensorData[which].at(0);
		if(type != 8)
			return 255;

		unsigned char res[7];
		for(int i=0; i<7; i++)
			res[i] = (unsigned char)sensorData[which].at(i+1);

		if(command>=1 &&
			command<=7)
			return res[command];

		else if(command == 8)
		{
			int maxPort = 0;
			int max = 1;
			for(int i=0; i<7; i++)
				if(res[i] > max)
				{
					max = res[i];
					maxPort = i+1;
				};

			return maxPort;
		}

		else if(command == 9)
		{
			int max = 1;
			for(int i=0; i<7; i++)
				if(res[i] > max)
					max = res[i];

			return max-1;
		}

		else if(command == 10)
		{
			int minPort = 0;
			int min = 1;
			for(int i=0; i<7; i++)
				if(res[i] < min)
				{
					min = res[i];
					minPort = i+1;
				};

			return minPort;
		}

		else if(command == 11)
		{
			int min = 1;
			for(int i=0; i<7; i++)
				if(res[i] < min)
					min = res[i];

			return min-1;
		}

		else if(command == 12)
		{
			int sum = 0;
			for(int i=0; i<7; i++)
				sum += res[i]-1;

			return sum / 7;
		}

		else
			return 0;
	}
	catch(...)
	{
		return 255;
	}
}
int GetInfrared(int which)//生命探测仪
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 2)
			return 255;

		int type = (int)sensorData[which].at(0);
		if(type != 9)
			return 255;
		else
			return (unsigned int)sensorData[which].at(1);
	}
	catch(...)
	{
		return 255;
	}
}
int GetVolume(int which)//tbh:声音传感器
{
	updateTimes();
	try
	{
		int size = sensorData[which].size();
		if(size != 2)
			return 0;

		int type = (int)sensorData[which].at(0);
		if(type != 6)
			return 0;
		else
			return (unsigned int)sensorData[which].at(1);
	}
	catch(...)
	{
		return 255;
	}
}
///////////////////////////////////////////////////////////////////////////////////////////////////

unsigned int GetSysTime()
{
	return systemTime*10;
}
//unsigned int GetValue(int which)
//{
//	int p = which;
//	return atoi(sensorData[p]._value.c_str());
//}
