#include "GlobalMap.h"

CGlobalMap::CGlobalMap(void)//:m_pfCheckPossibleParticle(&IsAPossibleLocate)
{
//	m_pfCheckPossibleParticle=&CGlobalMap::IsAPossibleLocate;
}
CGlobalMap::CGlobalMap(unsigned Height,unsigned Width)//:m_pfCheckPossibleParticle(IsAPossibleLocate)
{
	if (!ReadMapFromFile(Height,Width))//讀地圖
	{
		
	}
	ReadBoolMapFromFile(Height,Width);//讀二值地圖
	//ExpandObstacleOnboolMap(Height,Width,0.5*RADIOUS*1000/ONE_PIXEL_LENGTH);//擴充障礙物點，獲得新的二值地圖,0.5*RADIOUS*100/ONE_PIXEL_LENGTH為機器人半徑的像素大小
	X_DisOfWordCord2MapCoord=0.5*Width;

	Y_DisOfWordCord2MapCoord=0.5*Height;
	
}
CGlobalMap::~CGlobalMap(void)
{
}
bool CGlobalMap::ReadMapFromFile(unsigned Height,unsigned Width)
{
	std::ifstream in("Mapdata.txt");
	for(unsigned row=0; row < Height; row++)
	{
		if (row>=m_mGolbalMap.size())//輸入的行數超過了m_mGolbalMap中的行數則添加1行
		{
			std::vector<float>RowVector;//定義一個行的空vector，供m_mGolbalMap壓入
			m_mGolbalMap.push_back(RowVector);
		}
		for(unsigned column=0; column < Width; column++)
		{
			float CurrentPixelTmp;
			in >> CurrentPixelTmp;
			m_mGolbalMap[row].push_back(CurrentPixelTmp);//將文件中保存的row行column列的元素壓入地圖
		//	DCCompatible.SetPixel(column,row,rgb);
		}
	}
	in.close();
	return true;
}
bool CGlobalMap::ReadBoolMapFromFile(unsigned Height,unsigned Width)
{
	std::ifstream in("BoolMap.txt");
//	in.open();
	for(unsigned row=0; row < Height; row++)
	{
		if (row>=m_BoolGolbalMap.size())//輸入的行數超過了m_mGolbalMap中的行數則添加1行
		{
			std::vector<bool>RowVector;//定義一個行的空vector，供m_mGolbalMap壓入
			m_BoolGolbalMap.push_back(RowVector);
#ifdef JUDGE_EDGE_POINT_IN_MAP//需要考慮邊界點的化
			m_bvEdgePointMap.push_back(RowVector);
#endif
			
		}
		for(unsigned column=0; column < Width; column++)
		{
			
			unsigned CurrentPixelTmp;
			in >> CurrentPixelTmp;
#ifdef JUDGE_EDGE_POINT_IN_MAP//需要考慮邊界點的化
			if (CurrentPixelTmp==EDGE_POINT_VAL_IN_BOOL_MAP)//找到一個邊界點，首先由于該點是障礙物點，所以先把CurrentPixelTmp改為0
			{
				CurrentPixelTmp=0;
				m_bvEdgePointMap[row].push_back(true);
			}
			else
			{
				m_bvEdgePointMap[row].push_back(false);
			}
#endif		
			m_BoolGolbalMap[row].push_back(CurrentPixelTmp);//將文件中保存的row行column列的元素壓入地圖
			
			//	DCCompatible.SetPixel(column,row,rgb);
		}
	}
	in.close();
	//__asm int 3;
	return true;

}
void CGlobalMap::ExpandObstacleOnboolMap(unsigned Height,unsigned Width,unsigned ExpandWidthInPixel)
{
	BoolMapType bmapAfterExpand(m_BoolGolbalMap);
	for(unsigned row=0; row < Height; row++)
	{
	
		for(unsigned column=0; column < Width; column++)
		{
			if (!m_BoolGolbalMap[row][column])//是障礙物就擴充周邊
			{
				//擴充
				for (int i=row-ExpandWidthInPixel;i<row+ExpandWidthInPixel;i++)//行
				{
					for (int j=column-ExpandWidthInPixel;j<column+ExpandWidthInPixel;j++)//列
					{
						if (i>0&&i<Height&&j>0&&j<Width)//沒超出范圍的話
						{
							bmapAfterExpand[i][j]=0;
						}
					}
				}
			}			//	DCCompatible.SetPixel(column,row,rgb);
		}
	}
	m_BoolGolbalMap=bmapAfterExpand;//用擴充后的圖代替擴充前的圖
}