#pragma once
/********************************************************************
	created:	2008/06/12
	created:	12:6:2008   9:44
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\LocalMap.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	LocalMap
	file ext:	h
	author:		GaoYang
	
	purpose:	局部路徑規(guī)劃中用到的局部地圖，以及地圖的寫入、查找內(nèi)容函數(shù)。
	            地圖支持包括原來的圖像處理格式和激光雷達(dá)掃描到的格式
*********************************************************************/
//#include "stdafx.h"
#include <vector>
//#include "stdlib.h"
#include "ObjectAngleCheck.h"
#include "ShareObject.h"
#include <math.h>

using namespace personal_ShareObject_pathPlanUsingSpace;
namespace PthPlSpa=personal_ShareObject_pathPlanUsingSpace;

class CLocalVisionMap:virtual public ObjectAngleCheck
{
public:
	CLocalVisionMap(void);
public:
	~CLocalVisionMap(void);
	
	/***********************圖像處理格式的地圖寫入*********************************/
	//注意m_nFreRoadMap[i]中的內(nèi)容為一些號，這些號代表了一個個物體，
	//其排序方式為一個接一個，并無空缺，比如1號物體被判無效了則m_nFreRoadMap[i]中的
	//1所代表的物體將不是1號物體可能是2或3物體但2并非代表2號物體，可能
	unsigned short m_nFreRoadMap[361];///<障礙物號及其分布[0,360]
	/**
	* @ brief  ExpandObstacleRegion函數(shù)用來膨脹擴(kuò)充障礙物區(qū)域使不用另考慮機(jī)器人大小。
	*
	* 首先將需要擴(kuò)充的障礙物信息另存一份到m_aObstacleStruct。然后再m_aObstacleStruct中擴(kuò)充，
	* 將障礙物距離減少，而左右邊界角增加。距離減少后竟然小于0，即由于某種誤差機(jī)器人已經(jīng)碰上了
	* 物體，則表示膨脹失敗，不膨脹
	* @ parm pObstacleList是需要擴(kuò)充的區(qū)域指針，
	* @ parm indexOfmaObstacleStruct是該指針應(yīng)存在m_aObstacleStruct中位置的下標(biāo).
	*/
	bool ExpandObstacleRegion(PthPlSpa::ObjectPolePos *pObstacleTobeExpand);
	double m_dExpandRadius;//膨脹時用到的機(jī)器人的半徑，即計算中的膨脹半徑，在構(gòu)造函數(shù)中賦值
	int m_iSizeOfObjectPolePos;///<PthPlSpa::ObjectPolePos的類型大小，在構(gòu)造函數(shù)中初始化=sizeof(m_iSizeOfObjectPolePos)
	::std::vector<int>i;
	::std::vector<PthPlSpa::ObjectPolePos> m_aObstacleStruct;///<將經(jīng)過確認(rèn)的障礙物數(shù)組，經(jīng)過膨脹處理后再將數(shù)據(jù)數(shù)據(jù)全存過來,每多一次存一個
	//unsigned short m_iObstacleNumAfterCheck;///<記錄經(jīng)過檢查后確認(rèn)的障礙物個數(shù)
	/**
	*判斷障礙物的有效性，不能過近過遠(yuǎn)，以及角度的0~360度
	*/
	bool ConfirmObstacleList(PthPlSpa::ObjectPolePos* Obstacle,float TargetDis);
	/**
	* @ brief  SetRoadMap()函數(shù)用于將外部輸入的障礙物信息反映到成員數(shù)組m_nFreRoadMap中
	*
	* @ parm pObstacleList障礙物鏈表
	* @ parm nObstacleMaxNum 最大障礙物數(shù)
	* @ parm TargetDis 目標(biāo)的距離
	*/
	void SetRoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum,float TargetDis);

	/**
	*@brief m_apObstaclePointerContainer中存放所有經(jīng)確認(rèn)有效的障礙物，按先后順序編號，1開始，本類中直接調(diào)用SetRoadMap
	*	函數(shù)獲得，并將其寫入地圖，后續(xù)動態(tài)物體考慮時有可能其中的部分標(biāo)號并未寫入地圖
	*
	*	m_apObstaclePointerContainer[15]中內(nèi)容與對應(yīng)m_nFreRoadMap中所用編號的說明：
	*	這個一維數(shù)組中存放著所有有效的障礙物的指針。
	*	按照輸入的鏈表的順序，有效的障礙物指針依次存入其中。
	*	m_apObstaclePointerContainer[0]中未存指針，其內(nèi)容始終為0。
	*	m_apObstaclePointerContainer[1]中存的障礙物的編號即為1號，
	*            m_nFreRoadMap中若有單元存了1則表示在該對應(yīng)角度方向上有1號障礙物。
	*            因此對應(yīng)i度方向的障礙物為：m_apObstaclePointerContainer[m_nFreRoadMap[i]]所指向的障礙物。
	*            障礙物編號從1開始，按障礙物鏈表順序，第一個經(jīng)確認(rèn)有效的障礙物為1號
	*	類似繼續(xù)....
	*/
	//PthPlSpa::ObjectPolePos * m_apObstaclePointerContainer[10];///<該數(shù)組中存放著所有障礙物的指針，最大存15個
	/**
	*@brief 對障礙物預(yù)處理,包括檢查有效性、膨脹、記錄膨脹后的指針等，若發(fā)現(xiàn)無效障礙物則返回false
	*
	*/
	bool PretreatObstacle(PthPlSpa::ObjectPolePos *pObstacleList,short int numOb,float TargetDis)
	{
        if (ExpandObstacleRegion(pObstacleList))
		{
			//std::vector<PthPlSpa::ObjectPolePos>::iterator itExpanded=m_aObstacleStruct.end()-1;
		//	m_apObstaclePointerContainer[numOb+1]=&(*itExpanded);//>將有效障礙物的指針保存，同時該障礙物在m_apObstaclePointerContainer中的編號即numOb+1
		}
		else
		{
			/*std::vector<PthPlSpa::ObjectPolePos>::iterator itExpanded=m_aObstacleStruct.end()-1;
			m_apObstaclePointerContainer[numOb+1]=&(*itExpanded);*/
		}
		return true;
	}
	/**
	*@brief 從ApfPathPlan類SetRoadMap函數(shù)中分離出來的部分功能:將指定障礙物及其編號寫入地圖,并記錄該障礙物指針
	*@parm pObstacleList 指定障礙物指針
	*@parm numOb 障礙物在m_nFreRoadMap中的編號-1
	*@parm TargetDis 目標(biāo)的距離
	*/
	bool AddObject2RoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int numOb,float TargetDis);
	void WriteObject2Map(std::vector<PthPlSpa::ObjectPolePos>::iterator pObstacleAferExpand,unsigned IndexOfAngle,unsigned NumInstedObjInRoadMap)
	{
		if ((int)m_nFreRoadMap[IndexOfAngle]!=0)//已經(jīng)有過障礙物在該角度賦過值
		{
			if (pObstacleAferExpand->distance<m_aObstacleStruct[m_nFreRoadMap[IndexOfAngle]-1].distance)//新的障礙物更近，需要更新的話
			{
				m_nFreRoadMap[IndexOfAngle]=NumInstedObjInRoadMap;//0對應(yīng)m_nFreRoadMap[361]中的第0號數(shù)據(jù)（第1個），180度對應(yīng)第360號（第361個）
			}//新的更遠(yuǎn)的話則不更新m_nFreRoadMap
		}
		else//第一次賦
		{		
			m_nFreRoadMap[IndexOfAngle]=NumInstedObjInRoadMap;//該角度方向的障礙物為numOb+1號				
		}
	}
};
// ***************************************************************
// CLocalMapFromLaser   version:  1.0   ·  date: 06/13/2008
//  -------------------------------------------------------------
//  
//  -------------------------------------------------------------
//  Copyright (C) 2008 - All Rights Reserved
// ***************************************************************
// 激光雷達(dá)地圖類,包含了地圖以及寫入函數(shù)
// ***************************************************************
#define MAX_DETECT_RANGE 8000///<激光雷達(dá)最大探測距離,單位毫米
#define ONE_SCANNER_USED ///<使用了一個激光雷達(dá),因此僅用0~180度的角度,分辨率為0.5度,因此
#define ANGLE_RESOLVING 0.5///<激光雷達(dá)角度精度單位度
#define NumOfData 361///<數(shù)據(jù)的個數(shù)
class CLocalMapFromLaser
{
public:
	CLocalMapFromLaser(void);
public:
	~CLocalMapFromLaser(void);
public:
	/**
	*@brief 接口函數(shù)，將外部掃描得到的數(shù)據(jù)經(jīng)檢驗、膨脹后寫入LocalLaserMap
	*若達(dá)到最大掃描距離的角度則認(rèn)為該角度物物體
	*/
	bool WriteLocalLaserMap(float* pLaserDataChunk,float TargetDis);
protected:
	/**
	* @ brief  ExpandObstacleCell函數(shù)用來膨脹擴(kuò)充障礙物區(qū)域使不用另考慮機(jī)器人大小并將結(jié)果寫入到LocalLaserMap。
	*
	* 將每一方向經(jīng)ConfirmLaserData驗證的掃描值輸入本函數(shù),函數(shù)內(nèi)依據(jù)它的距離擴(kuò)充,包括領(lǐng)域,若領(lǐng)域內(nèi)已有距離值且比自己小則
	*不填充進(jìn)去.
	* @ parm pLaserData是輸入的角度的物距值，
	* @ parm iIndexInLocalMapStruct是角度對應(yīng)在LocalLaserMap中的位置即下標(biāo)值.
	*/
	bool ExpandObstacleCell(float* pLaserData,unsigned IndexInLocalMapStruct);
	/**
	*@brief 在ExpandObstacleCell中調(diào)用的用來在該物體角度左右AngleRegion范圍內(nèi)擴(kuò)充，以實現(xiàn)擴(kuò)充障礙物
	*
	*@parm AngleRegion,該物體影響的角度范圍，
	*@parm Dis 該物體的距離
	*@parm IndexInLocalMapStruct 該物體在LocalLaserMap中的位置即下標(biāo)值
	*/
	void ExpandObstacleNeighborCell(float AngleRegion,float Dis,unsigned IndexInLocalMapStruct);
	/**
	*@brief 在ExpandObstacleNeighborCell中調(diào)用的用來在判斷制定角度是否可以擴(kuò)充,可以則擴(kuò)充寫入,否則返回false
	*
	*@parm Dis 物距
	*@parm CurrentIndex當(dāng)前需要判斷是否能擴(kuò)充的位置,或者叫LocalLaserMap中的位置即下標(biāo)值.
	*/
	bool CheckExpandAble(float Dis,unsigned CurrentIndex)
	{
		if (CurrentIndex<361&&CurrentIndex>0)//未超出雷達(dá)數(shù)據(jù)的界限
		{
			if (LocalLaserMap[CurrentIndex]<0.000001||
				LocalLaserMap[CurrentIndex]>Dis)//沒有障礙物或者障礙物更遠(yuǎn)
			{
				LocalLaserMap[CurrentIndex]=Dis;
				return true;
			}
			else
			{
				return false;
			}
		}
		else
		{
			return false;
		}
	}
	bool ConfirmLaserData(float* LaserData,float TargetDis)
	{
		if (*(LaserData)>TargetDis||*(LaserData)>=MAX_DETECT_RANGE)
		{
			return false;
		}
		return true;
	}

protected:
	double m_dExpandRadius;//膨脹時用到的機(jī)器人的半徑，即計算中的膨脹半徑，在構(gòu)造函數(shù)中賦值
	float LocalLaserMap[NumOfData];///<局部地圖,源自1臺激光掃描儀的數(shù)據(jù)為0~180度0~361個數(shù)分辨率為0.5度,其內(nèi)部存儲有每一角度上的距離值，沒有物體的角度則其值為0,為與視覺格式一致,可考慮擴(kuò)充0~359.5度,

private:
};