#pragma once
#include "ApfPathPlan.h"
#include "ObjectTracker.h"
#include "UncentedKalmanFilt/UKF_COMMAN_HEAD.h"
#include "GuauessProbCalcu.h"
/********************************************************************
	created:	2008/03/14
	created:	14:3:2008   9:53
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005\PathPlanInDynamic.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005
	file base:	PathPlanInDynamic
	file ext:	h
	author:		GaoYang
	
	purpose:	動態環境中的路徑規劃類，取代了ApfPathPlan類．其中繼承了ApfPathPlan，以及目標跟蹤類．
	　　　　　　其中所有的速度都是遠離為正接近為負．同目標跟蹤類中的定義
    todo:       目前在本類中由兩套單位系統,因此帶來很多麻煩以及不必要的運算,在ObjectPolePos的結構體中,即輸入觀測數據中
				距離單位是米,角度單位是度,而在目標跟蹤器中所用的單位是:距離單位為毫米,角度單位為度或者弧度.需要統一!!
				另:線速度的定義為遠離為正,接近位負
*********************************************************************/
//using namespace personal_ShareObject_pathPlanUsingSpace;
namespace PthPlSpa=personal_ShareObject_pathPlanUsingSpace;
//using namespace personal_ShareObject_pathPlanUsingSpace;

using personal_ShareObject_pathPlanUsingSpace::TargetStyle;
#define OBSTACLE_MAX_ALTER_ANGLE 50
//typedef double[2] OBSTCLE_CENTER;
#define SAME_OBSTACLE_MAX_DIS 1//單位米
#define SUDDEN_OBSTACLE_MIN_DIS 500
#define MIN_LEAVE_SPEED_FOR_IGNORE 200//一幀內不考慮機器人自身移動即濾波結果中的線速度，遠離的距離大于這值則不用考慮其威脅，單位毫米/s
class CPathPlanInDynamic:virtual public CObjectTracker,virtual public ApfPathPlan,public BOOST_DEBUG_TOOL,public CGuauessProbCalcu
{
public:
	CPathPlanInDynamic(void);
public:
	~CPathPlanInDynamic(void);
public:
	/**
	*@brief 初始化濾波器,設定初值等,
	*/
	inline void InitTracker(void);
	/**
	*@brief 開始運行跟蹤，觀測，獲得速度
	*@parm dl 觀測者左輪位移
	*@parm dr 觀測者右輪位移
	*@parm OBSERVEDATA 觀測數據,boost中的vector格式,目前僅有2個內容,其中的角度,輸入時必須為弧度制,內部會依據USE_DEGREE_ANGLE宏是否定義來確定是否需要再轉化為角度
	*/
	void RunTrackInDyn(double dl,double dr,OBSERVE_STYLE& OBSERVEDATA);
	
public:
	/**
	*@brief開始局部路徑規劃,是主要接口函數,內部會調用跟蹤目標函數進行目標跟蹤
	*
	*1找出所有有效物體，并將其存入m_vpConfirmedObject，從上一次的m_vLastFiltedCenterPos或m_mFilterResult中找出每一個有效物體上一時刻的位置
	*找到的進行跟蹤，沒找到的視為突然物體，不濾。利用濾出來的速度及位置寫入地圖并搜索，計算危險系數，計算左右輪速。
	*事實上是在ApfPathPlan::FreeRoad基礎上修改而來的,作為動態環境蔽障的接口
	*輸入參數增加了左右輪子移動路徑長度的輸入
	*/
	 bool PathPlanMov(PthPlSpa::ObjectPolePos *pObstacleList,
		int nObstacleMaxNum,
		PthPlSpa::ObjectPolePos* Target,
		bool bControlBall,
		unsigned uSpeedLevel123_High,bool LeftForbidden,
		bool RightForbidden, bool ReverseMoveDirect,
		double dl,double dr,
		double& TimeStep);
	
protected:
		std::vector<int>aa;
	// 保存傳入的障礙物鏈表頭
	PthPlSpa::ObjectPolePos* m_pObstacleListHead;
	OBSERVE_STYLE m_OBSERVEDATA;///<傳入的觀測數據
	//STATE_STYLE m_mFilterResult;///<跟蹤結果
		//struct  ObjectCorelationMod:PthPlSpa::ObjectPolePos
	//{
	//	int CorrelateToNumOfLastTime;
	//};
	
	//std::vector<double> m_vObstCenterDis;///<物體的中心極坐標距離值
	//std::vector<double> m_vObstCenterAngle;///<物體的中心極坐標角度值
	bool SetRoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum);
protected:
	/**
	*@brief 搜索出所有的有效障礙物,并將其指針按順序保存,也編了號,若一個有效的都沒有則返回假
	*/
	bool ConfirmAllObstacle(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum);
	/**
	*@brief依據本次觀測和上一次濾波的結果確認本次的所有障礙物分別對應上一次的哪些?
	*
	*目前簡單的假設自己不動,一一計算本次觀測物體位置與上一次的物體位置的幾何距離,距離最近的而且距離小于閾值SAME_OBSTACLE_MAX_DIS
	*即認為兩者是同一物體,若距離最近的都大于SUDDEN_OBSTACLE_MIN_DIS則認為該物體上一次沒有觀測到,指該位m_viRelateParm為0
	*若沒有找到一個相關的物體,則返回假,否則返回真
	*/
	bool FindRelatedPosInLastStep(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum);
	/**
	*@brief 計算兩個點的歐氏距離
	*/
	double CalCulateDisIn2Pos(PthPlSpa::ObjectPolePos *pObstacle1,PthPlSpa::ObjectPolePos *pObstacle2);
	///**
	//*@brief 為sort函數準備的比較兩個距離的大小
	//*/
	//inline bool DisCompare(int DisIndex1,int DisIndex2);
public:
	std::vector<STATE_STYLE> m_mFilterResult;///<對所有物體的跟蹤結果挨個存入,本輪沒有進行跟蹤的靜止物體也要存入
	std::vector<VARIANCE> m_mFilterResultVar;///<對所有物體的跟蹤結果的方差挨個存入
	std::vector<VARIANCE> m_mModelNoiseVar;///<對所有物體的自適應跟蹤后,模型噪聲挨個存入
	std::vector<CAdaptiveOptimaser>m_vAdaptiveOptimizerVec;///<自適應調節器隊列，每個跟蹤的障礙物一個
	//std::vector<LARGE_INTEGER>m_vPreSystemCountNumVec;///<每一個障礙物上一次進行跟蹤時的系統時鐘點數
protected:
	/*******************************注意，容易混淆處說明*********************************************************
	*注意m_mFilterResult與m_vpConfirmedObject是一致的內容，順序也一致，其實兩者有重復，但因用到地方不同，現不考慮刪除
	*另外它們與ApfPathPlan::m_apObstaclePointerContainer中的內容也是一致的，但m_apObstaclePointerContainer中的內容以
	*下標1開始存儲。
	*
	*/
	std::vector<PthPlSpa::ObjectPolePos> m_vLastFiltedCenterPos;///<上一次濾波后的物體中心位置
	std::vector<PthPlSpa::ObjectPolePos*> m_vpConfirmedObject;///<經確認有效的所有物體指針
	std::vector<int> m_viRelateParm;///<本次觀測數據對應上一次濾波后的物體中心位置中的哪一個.若為0則表示該項觀測上一次沒有對應項.從1開始
	//比如下標1單元存放數3,則表示當前經確認有效的障礙物列表m_vpConfirmedObject中第1個物體對應上一次的保存的位置結果中的下標為3-1的物體
	
	/**
	*@brief 從ApfPathPlan類SetRoadMap函數中分離出來的部分功能:將指定障礙物及其編號寫入地圖,并記錄該障礙物指針
	*@parm pObstacleList 指定障礙物指針
	*@parm nObstacleMaxNum 障礙物編號
	*/
	/*bool AddObject2RoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int numOb);*/
	/**
	*考慮獲得的速度將指定的物體設定到環境地圖
	*
	*@parm ObjectDisplace為物體距離變化率，考慮了機器人自身移動的影響
	*/
	bool SetLocalDynamicMap(int numOb,PthPlSpa::ObjectPolePos* pObstacle,STATE_STYLE FilterResult,double &TimeStep);//,float ObjectDisplace
	/**
	*@brief在已經確認本次觀測各個物體在前一時刻位置的基礎上，進行跟蹤并依據估計速度，構造地圖。
	**/
	void ConstructRoadMap(double dl,double dr,double& TimeStep);
	public:
		unsigned m_uTrackedNum;///<經過跟蹤的物體數量

	/**
	*@brief 計算危險系數
	*
	*@parm IndexOfObstacle為最近的障礙物的地圖內編號，因m_mFilterResult與m_vpConfirmedObject是一致的，所以該編號也同樣
	*可以在m_mFilterResult中使用，用來尋找物體的速度等參數
	*
	*/
	double CalCulateDanger(int IndexOfObstacle);
	double m_dLengthPerTimeStep;///<一個時間步長內，機器人移動的距離,
	//std::vector<double> m_mFilterResult;
public:
	/**
	*考慮速度,估計自己到障礙物附近時其預計位置
	*以障礙物除以當前一個時間步內的移動位移來計算所需的到達時間
	*當物體運動線速度為負，且幅度大于MIN_LEAVE_SPEED_FOR_IGNORE，返回false，則后面忽略該物體,不寫入地圖。
	*
	*/
	bool AlterObstaclePosOnVel(PthPlSpa::ObjectPolePos* pObstacle,STATE_STYLE& FilterResult,double &TimeStep);
	/**
	*考慮速度以及速度估計的概率分布,估計自己到障礙物附近時其預計位置
	*由于假設了速度分布為高斯分布，因此以其80%可能性的速度值分布擴充物體。
	*
	*若由方差判斷未收斂則返回false,否則進行擴充返回true;
	*/
	bool EnlargeObstaclePosOnVel_Probaility(PthPlSpa::ObjectPolePos* pObstacle,CObjectTracker* pMyTracker,float ArriveTime);
public:
	// 是否值得進行跟蹤的判斷函數，本函數在FindRelatedPosInLastStep中調用，若輸入的為值得跟蹤則返回true
	bool ConfirmTrackableObject(PthPlSpa::ObjectPolePos *pObstacleList);
protected:
	// 物體相對機器人自身的距離變化率，既考慮了機器人自身移動因素，又考慮了物體移動的因素，注意需與m_vpConfirmedObject的內容一致
	//std::vector<float> m_vDisplaceOfObject2Self;
};