#include <iostream>
#include <list>
#include <set>
#include <cstdlib>
#include <cassert>
#include <cstdio>
#include <ctime>
#include "floodfill_mdf.h"
#ifndef QT_GUI_LIB
int main(int /*argc*/, char * /*argv*/[])
{
	using namespace std;
	srand(time(0));


	//0,1联通矩阵，0是障碍，1是可通区域
	vector<vector<char> > v_mat;
	const int rows = 24, cols=39;
	for (int i=0;i<rows;++i)
	{
		vector<char>  v_row;
		v_row.resize(cols,1);
		if (rand()%100<30)
		for (int j=0;j<4;++j)
			v_row[rand()%cols] = 0;
		v_mat.push_back(move(v_row));
	}
	//起点、终点
	const int
			startx = rand() % cols,
			starty = rand() % rows;
	const int
			endx = (startx + rand() % (cols-3) + 2) % cols,
			endy = (starty + rand() % (rows-3) + 2) % rows;

	v_mat[starty][startx] = 1;
	v_mat[endy][endx] = 1;

	std::vector<int> cx,cy,idx;
	//找路径
	if (min_distance_find(v_mat,
						  startx,starty,
						  endx,endy,
						  &cx,&cy,&idx)>0)
	{
		vector<vector<char> > disp = v_mat;
		const int psz = cx.size();
		for (int i=0;i<psz;++i)
		{
			assert(cx[i]>=0 && cx[i] < cols);
			assert(cy[i]>=0 && cy[i] < rows);
			disp[cy[i]][cx[i]] = -1;
		}
		const int impsz = idx.size();
		for (int i=0;i<impsz;++i)
		{
			const int impx = cx[idx[i]];
			const int impy = cy[idx[i]];
			assert(impx>=0 && impx < cols);
			assert(impy>=0 && impy < rows);
			disp[impy][impx] = i + 2;
		}
		disp[starty][ startx] = -2;
		disp[endy][ endx] = -3;
		for (int i=0;i<rows;++i)
		{
			for (int j=0;j<cols;++j)
			{
				if (disp[i][j] == 1)
					cout << "  ";
				else if (disp[i][j] >=2)
					printf ("%02X",(unsigned int)(disp[i][j]-2) % 0xFFu);
				else if (disp[i][j] == -1)
					cout << "[]";
				else if (disp[i][j] == -2)
					cout << "SS";
				else if (disp[i][j] == -3)
					cout << "EE";
				else
					cout << "XX";
			}
			cout << endl;
		}
	}
	else
	{
		cout<<"不可达!";
	}




	return 0;
}

#endif
/*!
 * \brief mdf_rev_fill 反向搜索，从终点处找起点
 * \param v_mat   障碍地形，1是平地，0是墙
 * \param startx  开始位置x
 * \param starty  开始位置y
 * \param endx    结束位置x
 * \param endy    结束位置y
 * \param p_rev   保存搜索步长的v_mat等尺寸矩阵
 * \return 能否找到起点
 */
bool mdf_rev_fill(
		const std::vector<std::vector<char> >  & v_mat ,
		const int startx,
		const int starty,
		const int endx,
		const int endy,
		std::vector<std::vector<unsigned int> > * p_rev
		)
{
	std::vector<std::vector<unsigned int> >  & v_rev = *p_rev;
	p_rev->clear();
	const int rows = v_mat.size();
	const int cols = v_mat[0].size();
	for (int i=0;i<rows;++i)
	{
		std::vector<unsigned int> row;
		row.resize(cols,0xffffffffu);
		p_rev->push_back(std::move(row));
	}
	//反向着色
	std::list<int> currX,currY;
	unsigned int step = 1;
	currX.push_back(endx);
	currY.push_back(endy);
	v_rev[endy][endx] = 0;
	bool arrival = false;
	while (currX.size() && !arrival)
	{
		const int rev_dirt[4][2] = {
			{1,0},{0,1},{-1,0},{0,-1}
		};
		const int tasks = currX.size();
		for (int t = 0; t< tasks&& !arrival; ++t)
		{
			const int cx = *currX.begin();
			const int cy = *currY.begin();
			currX.pop_front();
			currY.pop_front();
			for (int i=0;i<4&& !arrival;++i)
			{
				const int nx = cx + rev_dirt[i][0];
				const int ny = cy + rev_dirt[i][1];
				if (nx == startx && ny == starty)
					arrival = true;
				if (nx >= cols || nx < 0)
					continue;
				if (ny >= rows || ny < 0)
					continue;
				if (v_mat[ny][nx]==0)
					continue;
				if (v_rev[ny][nx]<0xffffffffu)
					continue;
				v_rev[ny][nx] = step;
				currX.push_back(nx);
				currY.push_back(ny);
			}//end for (int i=0;i<4&& !arrival;++i)
		}//end for (int t = 0; t< tasks&& !arrival; ++t)
		++step;
	}//end while (currX.size() && !arrival)
	return  arrival;
}
/*!
 * \brief mdf_path_find 前向搜索路径
 * \param v_rev   反向寻找后生成的距离矩阵
 * \param startx  起点x
 * \param starty  起点y
 * \param cx      存储路径坐标的x向量
 * \param cy      存储路径坐标的y向量
 * \return  是否成功搜索生成路径
 */
bool mdf_path_find(
		std::vector<std::vector<unsigned int> > & v_rev,
		const int startx,
		const int starty,
		std::vector<int> * cx,
		std::vector<int> * cy
		)
{
	const int rows = v_rev.size();
	const int cols = v_rev[0].size();
	unsigned int v = v_rev[starty][startx];
	cx->clear();
	cy->clear();
	cx->push_back(startx);
	cy->push_back(starty);
	int tx = startx, ty= starty;
	v_rev[ty][tx] = 0xffffffffu;
	while (v>0 && v <  0xffffffffu)
	{
		const int rev_dirt[8][2] = {
			{1,0},{0,1},{-1,0},{0,-1},
			{1,1},{-1,1},{1,-1},{-1,-1}
		};
		unsigned int minv = 0xffffffffu;
		int mind = 0;
		for (int i=0;i<8;++i)
		{
			const int nx = tx + rev_dirt[i][0];
			const int ny = ty + rev_dirt[i][1];
			if (nx >= cols || nx < 0)
				continue;
			if (ny >= rows || ny < 0)
				continue;
			if (v_rev[ny][nx]<minv)
			{
				minv = v_rev[ny][nx];
				mind = i;
			}
		}
		v = minv;
		tx += rev_dirt[mind][0];
		ty += rev_dirt[mind][1];
		cx->push_back(tx);
		cy->push_back(ty);
		assert(tx>=0 && tx < cols);
		assert(ty>=0 && ty < rows);
	}
	return  (v==0);
}

/*!
 * \brief min_dis_opt 归并路径缩短距离
 * \param v_mat 障碍地形，1是平地，0是墙
 * \param rx 非最优路径x（直接搜索出来的）
 * \param ry 非最优路径y（直接搜索出来的）
 * \param cx 较优路径x
 * \param cy 较优路径y
 * \param pidx 关键waypoint下表(相对于cx,cy)，连接两个下标的cx,cy的为直线。
 * \param safe_join 是否考虑矩形边角的遮挡。一般需要考虑。
 * \return  优化后路径大小
 */
int min_dis_opt(
		const std::vector<std::vector<char> >  & v_mat ,
		const std::vector<int> & rx,
		const std::vector<int> & ry,
		std::vector<int> * cx,
		std::vector<int> * cy,
		std::vector<int> * pidx,
		bool safe_join
		)
{
	const int rows = v_mat.size();
	const int cols = v_mat[0].size();


	std::vector<int> lx = rx, ly = ry;
	std::set<int> important ;


	size_t test_begin = 0;
	//本次最差的目标
	int opt_tar = test_begin + 2;
	important.insert(test_begin);
	while (test_begin < lx.size()-2)
	{
		int test_cur = test_begin + 2;
		const int pns = lx.size();
		bool good = true;
		while (test_cur < pns && good)
		{
			const int x1 = lx[test_begin], y1 = ly[test_begin],x2 = lx[test_cur], y2 = ly[test_cur];
			const int dx1 = (x2 - x1), dy1 = (y2 - y1);
			const int absx = dx1>=0?dx1:-dx1, absy = dy1>=0?dy1:-dy1;
			const int maxp = (absx + absy) * 3;
			for (int i=0;i<maxp && good;++i)
			{
				//为不打擦边球，要求周围1格子也没有障碍才能优化。
				for (int d = 0; d< 5 && good; ++d)
				{
					const int rev_dirt[9][2] = {
						{0,0},
						{1,0},{0,1},{-1,0},{0,-1},
						{1,1},{-1,1},{1,-1},{-1,-1}
					};
					const int tx = x1 + (i * dx1 ) / (maxp-1) + rev_dirt[d][0];
					const int ty = y1 + (i * dy1 ) / (maxp-1) + rev_dirt[d][1];
					if (tx >= cols || tx < 0)
						continue;
					if (ty >= rows || ty < 0)
						continue;
					if (v_mat[ty][tx] == 0)
						good = false;
					if (tx ==x1 && ty==y1)
						break;
					if (tx ==x2 && ty==y2)
						break;
					if (!safe_join)
						break;
				}

			}
			if (!good)
				break;
			++test_cur;
		}
		if (test_cur > opt_tar)
		{
			important.insert(test_begin);
			std::vector<int> newx, newy;
			for (size_t i=0;i<test_begin;++i)
			{
				newx.push_back(lx[i]);
				newy.push_back(ly[i]);
			}
			const int x1 = lx[test_begin], y1 = ly[test_begin],x2 = lx[test_cur-1], y2 = ly[test_cur-1];
			const int dx1 = (x2 - x1), dy1 = (y2 - y1);
			const int absx = dx1>=0?dx1:-dx1, absy = dy1>=0?dy1:-dy1;
			const int maxp = (absx + absy) * 3;
			int last_x = -1, last_y = -1;
			for (int i=0;i<maxp;++i)
			{
				const int tx = x1 + (i * dx1) / (maxp-1);
				const int ty = y1 + (i * dy1) / (maxp-1);
				if (tx != last_x || ty !=last_y)
				{
					last_x = tx;
					last_y = ty;
					assert(tx>=0 && tx < cols);
					assert(ty>=0 && ty < rows);
					assert(v_mat[ty][tx]);
					newx.push_back(tx);
					newy.push_back(ty);
				}
			}
			//下次务必从这里开始
			opt_tar = newx.size();

			for (int i=test_cur;i<pns;++i)
			{
				newx.push_back(lx[i]);
				newy.push_back(ly[i]);
			}
			lx = std::move(newx);
			ly = std::move(newy);

		}
		if (good)
			break;

		++test_begin;
	}
	important.insert(lx.size()-1);

	*cx = std::move(lx);
	*cy = std::move(ly);

	pidx->clear();
	for (auto p : important)
	{
		pidx->push_back(p);
	}

	return cx->size();
}

/*!
 * \brief min_distance_find 避障路径搜索函数
 * \param v_mat  障碍地形，1是平地，0是墙
 * \param startx 起点x
 * \param starty 起点y
 * \param endx   终点x
 * \param endy   终点y
 * \param x      路径x
 * \param y      路径y
 * \param pidx   关键waypoint下表(相对于cx,cy)，连接两个下标的cx,cy的为直线。
 * \param join   是否做直线归并优化。
 * \param safe_join 是否考虑直线边缘的碰撞
 * \param max_itertimers 直线归并最大迭代次数
 * \return cx大小
 */
int min_distance_find(
		const std::vector<std::vector<char> >  & v_mat ,
		const int startx,
		const int starty,
		const int endx,
		const int endy,
		std::vector<int>  *x,
		std::vector<int>  *y,
		std::vector<int> * pidx,
		bool join,
		bool safe_join,
		int max_itertimers
		)
{
	std::vector<std::vector<unsigned int> >  v_rev ;

	if (!mdf_rev_fill(v_mat,startx,starty,endx,endy, &v_rev))
		return 0;
	std::vector<int> cx,cy;

	if (!mdf_path_find(v_rev,startx,starty,&cx,&cy))
		return 0;
	if (join)
	{
		bool res = true;
		int iter = 0;
		while (res && iter<max_itertimers)
		{
			++iter;
			res = res && min_dis_opt(v_mat,cx,cy,x,y,pidx,safe_join);
			if (!res)
				break;
			if (cx.size()==x->size())
			{
				bool same = true;
				const size_t szxc = cx.size();
				for (size_t i=0;i<szxc && same;++i)
				{
					if ((*x)[i]!=cx[i] || (*y)[i]!=cy[i])
						same = false;
				}
				if (same)
					break;
			}
			pidx->clear();
			cx = std::move(*x);
			cy = std::move(*y);
		}
		return res?iter:0;
	}

	size_t sz = cx.size();
	pidx->clear();
	for (size_t i=0;i<sz;++i)
		pidx->push_back(i);
	*x = std::move(cx);
	*y = std::move(cy);
	return x->size();
}
