//			else//第一次賦
//			{		
//				m_nFreRoadMap[j]=numOb+1;//該角度方向的障礙物為numOb+1號				
//			}
//			//				robot_console_printf("AngleOfObstacleNumIsBehind=%d",j);
//			//				robot_console_printf("   NumberOfObstacleNumIsBehind=%d\n",numOb);
//
//		}			
//	}
//	else//>障礙物并非橫跨后方左右時
//	{
//		for (int j=(int)pObstacleAferExpand->LBoundaryAngle;j<=pObstacleAferExpand->RBoundaryAngle;j++)
//		{
//			if ((int)m_nFreRoadMap[j]!=0)//已經有過障礙物在該角度賦過值
//			{
//				//		robot_console_printf("!!!!!!m_nFreRoadMap[180+j]=%d,j=%d\n",m_nFreRoadMap[180+j],j);
//
//				if (pObstacleAferExpand->distance<m_apObstaclePointerContainer[m_nFreRoadMap[j]]->distance)//新的障礙物更近，需要更新的話
//				{
//
//					m_nFreRoadMap[j]=numOb+1;//-180對應m_nFreRoadMap[361]中的第0號數據（第1個），180度對應第360號（第361個）
//				}//新的更遠的話則不更新m_nFreRoadMap
//			}
//			else//第一次賦
//			{		
//				m_nFreRoadMap[j]=numOb+1;
//			}
//		}
//	}
//	return true;
//}
bool CPathPlanInDynamic::SetRoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum)
{
	memset(m_nFreRoadMap,0,sizeof(m_nFreRoadMap));
	int numOb;//障礙物的編號
	numOb=0;//0表示沒障礙物

	
	return false;
}
bool CPathPlanInDynamic::AlterObstaclePosOnVel(PthPlSpa::ObjectPolePos* pObstacle,STATE_STYLE& FilterResult,double &TimeStep)//,float ObjectDisplace
{
#ifdef  DEBUG_USE_FLAG
	ViewVector(FilterResult);
	m_mSigma_Viewer;
#endif
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
	robot_console_printf("FilterResult(3)=%f",FilterResult(3));
	robot_console_printf("FilterResult(2)=%f",FilterResult(2));
	robot_console_printf("FilterResult(1)=%f",FilterResult(1));
	robot_console_printf("FilterResult(0)=%f",FilterResult(0));
	robot_console_printf("pObstacle->angle=%f",pObstacle->angle);
#endif

	if (FilterResult(2)>MIN_LEAVE_SPEED_FOR_IGNORE)//物體高速遠離可以忽略
	{
		return false;
	}
	else
	{
	//	double DisInFiltedAngleAndRawAngle=GetAngulardisplacementIn2Angle(pObstacle->angle,FilterResult(1));//濾波后的中心與原始數據中心的角度差
		double Displace=0;
		float ArriveTime=0;
		if (fabs(m_dLengthPerTimeStep)>0.0001)//機器人移動過位置ObjectDisplace為負表示接近
		{
#ifdef CONSIDER_TIME_IN_VEL
	ArriveTime=FilterResult(0)/m_dLengthPerTimeStep*TimeStep*0.001;//預計到達時間單位為秒
	//robot_console_printf("TimeStep=%f\n",TimeStep);
#else
	ArriveTime=FilterResult(0)/m_dLengthPerTimeStep;//預計到達時間單位為當前時間步長
#endif
			
			Displace=ArriveTime*FilterResult(3);//暫用自身移動速度來代表接近速度，有正負，當自身倒退時
			//	double arriveTime=FilterResult(0)/FilterResult(3);
			//	double Displace=arriveTime*FilterResult(4);
			//	double DistanceDisplace=arriveTime*FilterResult(3);
			if (Displace<-OBSTACLE_MAX_ALTER_ANGLE)//限幅
			{
				Displace=-OBSTACLE_MAX_ALTER_ANGLE;
			}
			else if (Displace>OBSTACLE_MAX_ALTER_ANGLE)
			{
				Displace=OBSTACLE_MAX_ALTER_ANGLE;
			}	
			//__asm int 3;
			EnlargeObstaclePosOnVel_Probaility(pObstacle,this,ArriveTime);

		}
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
	robot_console_printf("m_dLengthPerTimeStep=%f\n",m_dLengthPerTimeStep);
	robot_console_printf("Displace=%f\n",Displace);
#endif
		//if (Displace>0)//增加方向擴充
		//{
		//	pObstacle->RBoundaryAngle=pObstacle->RBoundaryAngle+Displace;
		//	LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
		//}
		//else
		//{
		//	pObstacle->LBoundaryAngle=pObstacle->LBoundaryAngle+Displace;
		//	LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
		//}
		return true;
	}
}
#define VAR_CONVERGENT_THRES_PARM 0.1///<用于判斷是否收斂的方差系數,若位置方差值小于此系數乘初始值即收斂
bool CPathPlanInDynamic::EnlargeObstaclePosOnVel_Probaility(PthPlSpa::ObjectPolePos* pObstacle,CObjectTracker* pMyTracker,float ArriveTime)
{
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(pMyTracker->my_filter.m_mStateCov);
	m_mSigma_Viewer;
#endif
	const float AngularVInitVar=TransAngleToDegree(pMyTracker->m_vInitStateCov(3,3));
	if (pMyTracker->my_filter.m_mStateCov(3,3)>VAR_CONVERGENT_THRES_PARM*AngularVInitVar)
	{
		//robot_console_printf("AngleVVar=%f\n",pMyTracker->my_filter.m_mStateCov(3,3));
		return false;
	}
	else
	{
		
		GausVaribleAdd(pMyTracker->my_filter.m_mStateMeanResult(1),pMyTracker->my_filter.m_mStateMeanResult(3),
			pMyTracker->my_filter.m_mStateCov(1,1),pMyTracker->my_filter.m_mStateCov(3,3),pMyTracker->my_filter.m_mStateCov(1,3),
			ArriveTime);//獲得位置角度與角速度乘時間值相加后的變量分布
		float StdVar=sqrt(m_sGausVaribleAddResult.Variance);
		float Displace=m_sGausVaribleAddResult.Expect-pMyTracker->my_filter.m_mStateMeanResult(1);
		//robot_console_printf("Displace=%f\n",Displace);
		if (StdVar>20)//方差太大，不應按照方差來移動擴充物體
		{
				if (Displace<-OBSTACLE_MAX_ALTER_ANGLE)//限幅
				{
					Displace=-OBSTACLE_MAX_ALTER_ANGLE;
				}
				else if (Displace>OBSTACLE_MAX_ALTER_ANGLE)
				{
					Displace=OBSTACLE_MAX_ALTER_ANGLE;
				}	
				if (Displace>0)//增加方向擴充
				{
					pObstacle->RBoundaryAngle=pObstacle->RBoundaryAngle+Displace;
					LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
				}
				else
				{
					pObstacle->LBoundaryAngle=pObstacle->LBoundaryAngle+Displace;
					LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
				}
		}
		else
		{
			//__asm int 3;
			GetDisbuteRangeGivenProb(m_sGausVaribleAddResult.Expect,StdVar);
			pObstacle->distance=pMyTracker->my_filter.m_mStateMeanResult(0)*0.001;//位置暫時不動
			pObstacle->angle=m_sGausVaribleAddResult.Expect;//角度用濾波結果+潛在移動代替
			pObstacle->RBoundaryAngle=pObstacle->RBoundaryAngle+Displace+m_fsDisbuteRange[1];//右邊界角度為大角加上速度潛在移動,以及總體波動范圍的上界影響
			pObstacle->LBoundaryAngle=pObstacle->LBoundaryAngle+Displace+m_fsDisbuteRange[0];//左邊界角度為大角加上速度潛在移動,以及總體波動范圍的下界影響
			LimitAngleIn0_360Degree(pObstacle->angle);
			LimitAngleIn0_360Degree(pObstacle->RBoundaryAngle);
			LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
			if (pObstacle->LBoundaryAngle>pObstacle->RBoundaryAngle)
			{
				float tmp=pObstacle->LBoundaryAngle;
				pObstacle->LBoundaryAngle=pObstacle->RBoundaryAngle;
				pObstacle->RBoundaryAngle=tmp;
			}
		}	
		if (fabs(pObstacle->LBoundaryAngle)>400)
		{
			__asm int 3
		}

		return true;
	}
	
}
std::vector<double> DisVec;///<距離的向量,存上一次物體與當前看到物體的距離
///**
//*@brief 為sort函數準備的比較兩個距離的大小
//*/
bool DisCompare(int DisIndex1,int DisIndex2)
{
	return DisVec[DisIndex1-1]<DisVec[DisIndex2-1];
}
bool CPathPlanInDynamic::FindRelatedPosInLastStep(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum)
{
	//robot_console_printf("FindRelationNow\n");
//	__asm int 3;
	if (m_viRelateParm.size()>0)
	{
		m_viRelateParm.clear();//清除所有元素
	}	
	if(!ConfirmAllObstacle(pObstacleList,nObstacleMaxNum))
	{
		//robot_console_printf("ConfirmedNumOfObj=%d",m_vpConfirmedObject.size());
		return false;
	}
	else if (m_vLastFiltedCenterPos.size()<1)
	{
		return false;
	}
	std::vector<PthPlSpa::ObjectPolePos> ::iterator p_LastPos= m_vLastFiltedCenterPos.begin();
	std::vector<PthPlSpa::ObjectPolePos*> ::iterator p_CurrentPos= m_vpConfirmedObject.begin();
	
	std::vector<int> DisIndex;
	std::vector<int>::iterator itDisIndex;
	
	//robot_console_printf("Confirmed\n");
	for (int i=1;i<=m_vLastFiltedCenterPos.size();i++)
	{
		DisIndex.push_back(i);//DisIndex中存的對應的物體號碼1開始,1代表的物體是m_vLastFiltedCenterPos中的0下標物體
	}
	while (p_CurrentPos!=m_vpConfirmedObject.end())
	{
		//robot_console_printf("Confirmed,and there is object");
		if (ConfirmTrackableObject(*p_CurrentPos))//確認當前輸入的物體是否值得跟蹤，若否則當作突然出現的物體不跟蹤
		{
			DisVec.clear();
			//DisIndex.clear();
			p_LastPos= m_vLastFiltedCenterPos.begin();
			//robot_console_printf("ConfirmedObjectAngle=%f",(*p_CurrentPos)->angle);
			int i=0;
			while(p_LastPos!=m_vLastFiltedCenterPos.end())//遍歷上一次存下來的點
			{
				DisVec.push_back(CalCulateDisIn2Pos(*p_CurrentPos,&(*p_LastPos)));
				p_LastPos++;
			}//DisVec中現在存了當前該物體與前一是刻存的所有物體點的幾何距離
			itDisIndex=min_element(DisIndex.begin ( ),DisIndex.end ( ),DisCompare);//上一時刻所有點中找到當前點最近的點的編號，編號1開始
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
			robot_console_printf("RelatedObIndex=%d\n",*itDisIndex);
			robot_console_printf("NearestDis=%f\n",DisVec[(*itDisIndex)-1]);
#endif

			if (DisVec[(*itDisIndex)-1]>SAME_OBSTACLE_MAX_DIS)
			{
				m_viRelateParm.push_back(0);//突然出現的物體,上一次找不到相應項則該項置0值
//				m_vDisplaceOfObject2Self.push_back(0);//將該物體的接近速度存起,此時認為是０
			}
			else
			{
				m_viRelateParm.push_back(*itDisIndex);//找到相應項,存對應的物體號碼1開始
				//m_vDisplaceOfObject2Self.push_back((*p_CurrentPos).distance-m_mFilterResult[*itDisIndex-1](0));//將該物體的接近速度存起接近為正

			}
		}
		else
		{
			m_viRelateParm.push_back(0);//不應跟蹤，當作突然出現的物體,相應項則該項置0值
			//m_vDisplaceOfObject2Self.push_back(0);//將該物體的接近速度存起,此時認為是０
		}
		
		p_CurrentPos++;
	}	
	int maxIndex=(*max_element(m_viRelateParm.begin(),m_viRelateParm.end()));
	if (maxIndex=0)//一個都沒有找到
	{
		return false;
	}
	return true;
}
bool CPathPlanInDynamic::ConfirmAllObstacle(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum)
{
	m_vpConfirmedObject.clear();//刪除所有元素
	for (int i=0;i<nObstacleMaxNum;i++) 
	{
		if (!pObstacleList)
		{
			break;
		}
		if (!ConfirmObstacleList(pObstacleList,m_aPathTarget.distance))//>若無效
		{	
			pObstacleList=pObstacleList->next;//去掉無效的
			continue;
		}
		m_vpConfirmedObject.push_back(pObstacleList);
		pObstacleList=pObstacleList->next;
		//numOb++;//>經確認的障礙物個數為numOb+1個
	}
	if (m_vpConfirmedObject.size()<1)
	{
		return false;
	}
	
}
double  CPathPlanInDynamic::CalCulateDisIn2Pos(PthPlSpa::ObjectPolePos *pObstacle1,PthPlSpa::ObjectPolePos *pObstacle2)
{
	double Angle1=TransAngleToArc(pObstacle1->angle);
	double Angle2=TransAngleToArc(pObstacle2->angle);

	double x1=pObstacle1->distance*cos(Angle1);
	double y1=pObstacle1->distance*sin(Angle1);

	double x2=pObstacle2->distance*cos(Angle2);
	double y2=pObstacle2->distance*sin(Angle2);
	return sqrt(pow(x1-x2,2)+pow(y1-y2,2));
}


// 是否值得進行跟蹤的判斷函數，本函數在FindRelatedPosInLastStep中調用，若輸入的為值得跟蹤則返回true
bool CPathPlanInDynamic::ConfirmTrackableObject(PthPlSpa::ObjectPolePos *pObstacle)
{
	
	if (pObstacle->angle<0||pObstacle->angle>180) 
	{
		//return false;
	}
	else if(pObstacle->distance>3&&pObstacle->distance<1)
	{		
		return false;//該障礙物無效	
	}
	else
	{
		return true;
	}	
	return true;
}