#include "stdafx.h"
#include "simplegrid.h"
#include <list>
#include <algorithm>

using namespace std;

CSimpleGrid::CSimpleGrid(void)
{
	 m_numx=0; //列数
	 m_numy=0; //行数
	 m_x0=0; //起点x0
	 m_y0=0; //起点y0
	 m_dx=0; //x步长
	 m_dy=0; //y步长
}
CSimpleGrid::~CSimpleGrid(void)
{
	Clear();
}
void CSimpleGrid::Clear(void)
{
	m_numx = 0; //列数
	m_numy = 0; //行数
	m_x0 = 0; //起点x0
	m_y0 = 0; //起点y0
	m_dx = 0; //x步长
	m_dy = 0; //y步长
	m_points.clear();
	delete[]m_nodePointsArray;
}

double CSimpleGrid::X(int i)
{
	return m_x0 + i * m_dx;
}
double CSimpleGrid::Y(int j)
{
	return m_y0 + j * m_dy;
}

void CSimpleGrid::Create(double x0, double y0, double dx, double dy, int xnum, int ynum)
{
	Clear();
	m_x0 = x0;
	m_y0 = y0;
	m_dx = dx;
	m_dy = dy;
	m_numx = xnum;
	m_numy = ynum;
	m_deltS = pow(std::min<double>(m_dx, m_dy), 2.0);
	m_nodePointsArray = new vector<int>[m_numx*m_numy];

}

void CSimpleGrid::WriteGridLines(CString strout)
{
	FILE* fw = fopen(strout, "w");

	double x1, y1, x2, y2;
	for (int j = 0; j < ynum(); j++)
	{
		x1 = m_x0;
		y1 = Y(j);

		x2 = X(xnum() - 1);
		y2 = y1;
		fprintf(fw, "Pline\n");
		fprintf(fw, "%.12g,%.12g\n", x1, y1);
		fprintf(fw, "%.12g,%.12g\n", x2, y2);
		fprintf(fw, "\n");

	}

	for (int i = 0; i < xnum(); i++)
	{
		x1 = X(i);
		y1 = m_y0;

		x2 = x1;
		y2 = Y(ynum()-1);
		fprintf(fw, "Pline\n");
		fprintf(fw, "%.12g,%.12g\n", x1, y1);
		fprintf(fw, "%.12g,%.12g\n", x2, y2);
		fprintf(fw, "\n");
	}




	fclose(fw);
}

void CSimpleGrid::ReadPoints(const vector<CPointXYZ>& pts)
{
	m_points.assign(pts.begin(),pts.end() );

	CPointXYZ pt;
	int ix = 0; 
	int iy = 0;
	int NX = xnum();
	int NY = ynum();
	int idx = -1;
	for (int i = 0; i < pts.size(); i++)
	{
		pt = pts[i];
		ix = std::floor((pt.x0 - m_x0) / m_dx + 1e-8);
		iy = std::floor((pt.y0 - m_y0) / m_dy + 1e-8);
		if (ix < 0 || iy < 0)
			continue;


		if (ix > NX - 2 && ix < NX)
			ix = NX - 2;
		if (iy > NY - 2 && iy < NY)
			iy = NY - 2;

		idx = GetNodeIndex(ix, iy);

		m_nodePointsArray[idx].push_back(i);
			
	}


}

int CSimpleGrid::GetNodeIndex(int ix, int iy)
{
	if (ix > xnum() - 1 || iy > ynum() - 1)
		return -1;

	return iy*xnum() + ix;
}

vector<int>& CSimpleGrid::GetNodePointsVec(int ix, int iy)
{
	int idx = GetNodeIndex(ix, iy);
	if (idx < 0)
	{
		static vector<int> tmp;
		return tmp;
	}
	return m_nodePointsArray[idx];
}


//每个格子最多保留1-2个点，
void  CSimpleGrid::SimplifyNodePointsArray()
{
	for (int j = 0; j < ynum() - 1; j++)
	{
		for (int i = 0; i < xnum() - 1; i++)
		{
			SimplifySeedGrid(i, j);
			SimplifyNeighborGrid(i, j, i + 1, j);
			SimplifyNeighborGrid(i, j, i + 1, j+1);
			SimplifyNeighborGrid(i, j, i, j+1);
			SimplifyNeighborGrid(i, j, i - 1, j + 1);

		}
	}
}


//查找距离(x0,y0)最近的点，返回序号
int CSimpleGrid::GetNearestPoint(double x0, double y0, vector<int>& pts, double* distance)
{
	int idx = pts[0];
	CPointXYZ pt = m_points[idx];
	double dist2;
	double mindist2 = pt.GetDist2(x0, y0);
	int minidx = idx;
	for (int i = 1; i < pts.size(); i++)
	{
		idx = pts[i];
		pt = m_points[idx];
		dist2 = pt.GetDist2(x0, y0);
		if (dist2 < mindist2)
		{
			mindist2 = dist2;
			minidx = idx;
		}
	}

	if(distance)
		*distance = sqrt(mindist2);

	return minidx;
}

//查找距离(x0,y0)最远的点，返回序号,和距离distance
int CSimpleGrid::GetFarthestPoint(double x0, double y0, vector<int>& pts, double* distance)
{
	int idx = pts[0];
	CPointXYZ pt = m_points[idx];
	double dist2;
	double maxdist2 = pt.GetDist2(x0, y0);
	int maxidx = idx;
	for (int i = 1; i < pts.size(); i++)
	{
		idx = pts[i];
		pt = m_points[idx];
		dist2 = pt.GetDist2(x0, y0);
		if (dist2 > maxdist2)
		{
			maxdist2 = dist2;
			maxidx = idx;
		}
	}
	if (distance)
		*distance = sqrt(maxdist2);

	return maxidx;
}


void CSimpleGrid::SimplifySeedGrid(int ix, int iy)
{
	vector<int>& vec = GetNodePointsVec(ix, iy);
	if (vec.size() < 2) //少于两个点，返回
		return;
	vector<int> dstIdxes;
	double x0 = X(ix);
	double y0 = Y(iy);
	double distance = 0;
	int seedPt = GetNearestPoint(x0, y0, vec);
	dstIdxes.push_back(seedPt);
	
	x0 = m_points[seedPt].x0; 
	y0 = m_points[seedPt].y0;
	//查找距离seedPt最远的点
	int second = GetFarthestPoint(x0, y0, vec,&distance);
	
	if (distance > min(m_dx, m_dy))
		dstIdxes.push_back(second);

	vec.assign(dstIdxes.begin(), dstIdxes.end());
	

}

void CSimpleGrid::SimplifyNeighborGrid(int seedX, int seedY, int ix, int iy)
{
	vector<int>& seedsVec = GetNodePointsVec(seedX, seedY);
	if (seedsVec.empty())
		return;
	vector<int>& subjVec = GetNodePointsVec(ix,iy);
	if (subjVec.empty())
		return;
	double x0, y0;
	double dist2 = 0;
	list<int> sublst(subjVec.begin(), subjVec.end());
	list<int>::iterator iter; 
	for (int j = 0; j < seedsVec.size(); j++)
	{
		if (sublst.empty())
			break;

		x0 = m_points[seedsVec[j]].x0;
		y0 = m_points[seedsVec[j]].y0;

		////begin test
		//if (int(x0 + 0.000001) == 591091 && int(y0 + 0.000001) == 5134095)
		//{
		//	TRACE("\n");
		//	for (auto p : sublst)
		//	{
		//		TRACE("%.12g,%.12g, dist = %.12g\n", m_points[p].x0 , m_points[p].y0 , 
		//			m_points[p].GetDist(x0,y0) );
		//	}
		//}
		////end test

		iter = sublst.begin();
		while (iter != sublst.end())
		{

			dist2 = m_points[*iter].GetDist2(x0, y0);
			if (dist2 < m_deltS)
				sublst.erase(iter++);
			else
				iter++;
		}



	}

	subjVec.assign(sublst.begin(), sublst.end());




}


void CSimpleGrid::GetAllNodePointIndexes(list<int>& ptIndexes) // 获取所有相关点序号
{
	int N = xnum() *ynum();
	for (int i = 0; i < N; i++)
	{
		ptIndexes.insert(ptIndexes.end(), m_nodePointsArray[i].begin(),
			m_nodePointsArray[i].end() );
	}
}