/**
 *   ___________   __
 *  /_  __/  _/ | / /
 *   / /  / //  |/ / 
 *  / / _/ // /|  /  
 * /_/ /___/_/ |_/ 
 * 
 * C++ library of the Triangular Irregular Network (TIN)
 *
 * Copyright (c) 2021-2031 Yi Zhang (zhangyiss@icloud.com)
 * All rights reserved.
 *
 */

#include "tin.h"

// Start vertex definition
vertex2dc::vertex2dc()
{
    x = y = elev = NAN;
    id = 0;
}

vertex2dc::vertex2dc(double inx, double iny, double inelev, unsigned int inid)
{
    set(inx, iny, inelev, inid);
}

void vertex2dc::set(double inx, double iny, double inelev, unsigned int inid)
{
    x = inx; y = iny; elev = inelev; id = inid;
    return;
}

// overload the == operator for vertex2dc type
bool operator ==(const vertex2dc &a, const vertex2dc &b) 
{
	if(fabs(a.x - b.x) <= ZERO && fabs(a.y - b.y) <= ZERO)
	{
		return true;
	}
	return false;
}

// Test if the three points are on the same line
bool is_collinear(vertex2dc *a_ptr, vertex2dc *b_ptr, vertex2dc *c_ptr)
{
	// ｜(y3−y1)(x2−x1)−(y2−y1)(x3−x1)｜
	if (fabs((c_ptr->y - a_ptr->y)*(b_ptr->x - a_ptr->x) - (b_ptr->y - a_ptr->y)*(c_ptr->x - a_ptr->x)) <= ZERO)
	{
		return true;
	}
	return false;
}

// calculate the circumcircle from three points
void circumcircle(vertex2dc *v0, vertex2dc *v1, vertex2dc *v2, double &cx, double &cy, double &cr)
{
	double s = 0.5 / ((v1->x - v0->x) * (v2->y - v0->y) - (v1->y - v0->y) * (v2->x - v0->x));
	double m = v1->x*v1->x - v0->x*v0->x + v1->y*v1->y - v0->y*v0->y;
	double u = v2->x*v2->x - v0->x*v0->x + v2->y*v2->y - v0->y*v0->y;

	cx = ((v2->y - v0->y)*m + (v0->y - v1->y)*u)*s;
	cy = ((v0->x - v2->x)*m + (v1->x - v0->x)*u)*s;
	cr = (v0->x - cx)*(v0->x - cx) + (v0->y - cy)*(v0->y - cy); // not need to calculate the squared root here
	return;
}
// End vertex definition

// Start DEM definition
dem_point::dem_point()
{
    x = y = elev = NAN;
    err = 0.0;
    host = nullptr;
}

dem_point::dem_point(double inx, double iny, double inelev)
{
    set(inx, iny, inelev);
}

void dem_point::set(double inx, double iny, double inelev)
{
    x = inx; y = iny; elev = inelev; err = 0.0; host = nullptr;
    return;
}

bool compare_dem_point(dem_point *a, dem_point *b) // determination function for std::sort
{
	if (a->err > b->err) return true;
	return false;
}
// End DEM definition

// Start triangle definition
triangle::triangle()
{
    vert[0] = vert[1] = vert[2] = nullptr;
    neigh[0] = neigh[1] = neigh[2] = nullptr;
}

triangle::triangle(vertex2dc *v0ptr, vertex2dc *v1ptr, vertex2dc *v2ptr)
{
    set(v0ptr, v1ptr, v2ptr);
}

void triangle::set(vertex2dc *v0ptr, vertex2dc *v1ptr, vertex2dc *v2ptr)
{
    vert[0] = v0ptr; vert[1] = v1ptr; vert[2] = v2ptr;
    neigh[0] = neigh[1] = neigh[2] = nullptr;
    circumcircle(vert[0], vert[1], vert[2], cx, cy, cr);
    return;
}

void triangle::set_neighbor(triangle *n0ptr, triangle *n1ptr, triangle *n2ptr)
{
    neigh[0] = n0ptr; neigh[1] = n1ptr; neigh[2] = n2ptr;
    return;
}

// Test if the location is inside the triangle
bool triangle::bound_location(double inx, double iny)
{
    double l1x, l1y, l2x, l2y;
    for (int i = 0; i < 3; i++)
    {
        l1x = vert[(i+1)%3]->x - vert[i]->x;
        l1y = vert[(i+1)%3]->y - vert[i]->y;
        l2x = inx - vert[i]->x;
        l2y = iny - vert[i]->y;

        if ((l1x*l2y - l1y*l2x) < 0) // This condition includes points on the triangle's edge
        {
            return false;
        }
    }
    return true;
}

// Interpolate the elevation of the given location inside the triangle
double triangle::interpolate(double inx, double iny)
{
    double a1 = 0.5 * ((vert[1]->x - inx)*(vert[2]->y - iny) - (vert[1]->y - iny)*(vert[2]->x - inx));
    double a2 = 0.5 * ((vert[2]->x - inx)*(vert[0]->y - iny) - (vert[2]->y - iny)*(vert[0]->x - inx));
    double a3 = 0.5 * ((vert[0]->x - inx)*(vert[1]->y - iny) - (vert[0]->y - iny)*(vert[1]->x - inx));
    return (a1*vert[0]->elev + a2*vert[1]->elev + a3*vert[2]->elev)/(a1 + a2 + a3);
}

/**
 * @brief    Flip neighboring triangles and their neighbors
 * 
 * original
 * 
 *        /\
 *       /  \
 *      /    \
 *     /   t  \
 *  t_id-------\  t_id+1 (0, 1 or 2)
 *    \--------/
 *     \      /
 *      \  n /
 *       \  /
 *        \/
 *        n_id (0, 1 or 2)
 * 
 * flipped
 * 
 *        /|\
 *       / | \
 *      /  |  \
 *     /   |   \
 *  t_id   |    \  t_id (0, 1 or 2)
 *    \  t | n  /
 *     \   |   /
 *      \  |  /
 *       \ | /
 *        \|/
 *        n_id (0, 1 or 2)
 * 
 * @param t      target triangle
 * @param n      neighboring triangle
 * @param t_vid  reference index of the target triangle
 * @param n_vid  reference index of the neighboring triangle
 */
void flip_neighboring_triangles(triangle *t, triangle *n, int t_id, int n_id)
{
	t->vert[(t_id+1)%3] = n->vert[n_id]; // flip t
	circumcircle(t->vert[0], t->vert[1], t->vert[2], t->cx, t->cy, t->cr); // update circumcircle

	n->vert[(n_id+2)%3] = t->vert[(t_id+2)%3]; // flip n
	circumcircle(n->vert[0], n->vert[1], n->vert[2], n->cx, n->cy, n->cr); // update circumcircle
	
	// set side neighbors
	t->neigh[t_id] = n->neigh[(n_id+2)%3];
	n->neigh[(n_id+1)%3] = t->neigh[(t_id+1)%3];

	// set opposite neighbors
	t->neigh[(t_id+1)%3] = n;
	n->neigh[(n_id+2)%3] = t;

	// set oppsite neighbors
	if (t->neigh[t_id] != nullptr)
	{
		for (int i = 0; i < 3; i++)
		{
			if (t->neigh[t_id]->neigh[i] == n)
			{
				t->neigh[t_id]->neigh[i] = t;
				break;
			}
		}
	}
	
	if (n->neigh[(n_id+1)%3] != nullptr)
	{
		for (int i = 0; i < 3; i++)
		{
			if (n->neigh[(n_id+1)%3]->neigh[i] == t)
			{
				n->neigh[(n_id+1)%3]->neigh[i] = n;
				break;
			}
		}
	}

	// move hosted DEM points
	dem_point *tmp_dem;
	std::vector<dem_point*>::iterator d_iter;
	for (d_iter = t->hosted_dem.begin(); d_iter != t->hosted_dem.end(); )
	{
		tmp_dem = *d_iter;
		if (n->bound_location(tmp_dem->x, tmp_dem->y))
		{
			tmp_dem->host = n;
			n->hosted_dem.push_back(tmp_dem);
			d_iter = t->hosted_dem.erase(d_iter);
		}
		else d_iter++;
	}
	
	for (d_iter = n->hosted_dem.begin(); d_iter != n->hosted_dem.end(); )
	{
		tmp_dem = *d_iter;
		if (t->bound_location(tmp_dem->x, tmp_dem->y))
		{
			tmp_dem->host = t;
			t->hosted_dem.push_back(tmp_dem);
			d_iter = n->hosted_dem.erase(d_iter);
		}
		else d_iter++;
	}

	// update errors for hosted DEM data
	for (int i = 0; i < n->hosted_dem.size(); i++)
	{
		tmp_dem = n->hosted_dem[i];
		tmp_dem->err = fabs(n->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
	}

	for (int i = 0; i < t->hosted_dem.size(); i++)
	{
		tmp_dem = t->hosted_dem[i];
		tmp_dem->err = fabs(t->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
	}

	// Sort maximal errors for triangles t and n
	std::sort(t->hosted_dem.begin(), t->hosted_dem.end(), compare_dem_point);
	std::sort(n->hosted_dem.begin(), n->hosted_dem.end(), compare_dem_point);
	return;
}

/**
 * @brief    Make sure that the input triangle meets the empty circumcircle condition
 * 
 * @param t  Input triangle
 */
void make_delaunay(triangle *t)
{
	double dist;
	vertex2dc *n_vert;
	triangle *n_tri;
	dem_point *tmp_dem;
	for (int n = 0; n < 3; n++)
	{
		if (t->neigh[n] != nullptr) // must has neighbor on this side
		{
			n_tri = t->neigh[n];
			for (int v = 0; v < 3; v++)
			{
				n_vert = n_tri->vert[v];
				if (n_vert != t->vert[n] && n_vert != t->vert[(n+1)%3]) // find the opposite vertex
				{
					dist = (t->cx - n_vert->x) * (t->cx - n_vert->x) + 
						(t->cy - n_vert->y) * (t->cy - n_vert->y);

					// We have to use a very restrict condition here. The testing point must be really inside the circumcircle. 
					// Otherwise, this recursive function may fall into endless callings till the segment fails.
					if ((dist - t->cr) < -1.0*ZERO)
					{
						flip_neighboring_triangles(t, n_tri, n, v);
						
						// Make sure the triangles also meet the empty circumcircle condition after flipping
						make_delaunay(t);
						make_delaunay(n_tri);
						return; // Neighborhood changed. The current loop is not valid any more.
					}
					break; // no need to search more
				}
			}
		}
	}
	return;
}

triangle *split_triangle(vertex2dc *v, triangle *t, triangle *new_t[4])
{
	vertex2dc *tmp_vert;
	triangle *tmp_tri;

	new_t[0] = new_t[1] = new_t[2] = new_t[3] = nullptr;

	// Check for collinear
	for (int i = 0; i < 3; i++)
	{
		if (is_collinear(t->vert[i], t->vert[(i+1)%3], v)) // the new vertex is on edge
		{
			if (t->neigh[i] == nullptr) // no neighboring triangle. create two new triangles
			{
				tmp_tri = new triangle(t->vert[i], v, t->vert[(i+2)%3]);       new_t[0] = tmp_tri;
				tmp_tri = new triangle(t->vert[(i+2)%3], v, t->vert[(i+1)%3]); new_t[1] = tmp_tri;
				new_t[0]->set_neighbor(nullptr, new_t[1], t->neigh[(i+2)%3]);
				new_t[1]->set_neighbor(new_t[0], nullptr, t->neigh[(i+1)%3]);

				for (int n = 0; n < 2; n++)
				{
					if (new_t[n]->neigh[2] != nullptr)
					{
						for (int k = 0; k < 3; ++k) // replace neighbor for the oppositing triangle 
						{
							if (new_t[n]->neigh[2]->neigh[k] == t)
							{
								new_t[n]->neigh[2]->neigh[k] = new_t[n];
								break;
							}
						}
					}
				}
				return nullptr;
			}

			// has a neighboring triangle. create four new triangles
			for (int k = 0; k < 3; k++)
			{
				tmp_vert = t->neigh[i]->vert[k];
				if (tmp_vert != t->vert[i] && tmp_vert != t->vert[(i+1)%3])
				{
					tmp_tri = new triangle(t->vert[i], v, t->vert[(i+2)%3]);       new_t[0] = tmp_tri;
					tmp_tri = new triangle(t->vert[(i+2)%3], v, t->vert[(i+1)%3]); new_t[1] = tmp_tri;
					tmp_tri = new triangle(tmp_vert, v, t->vert[i]);               new_t[2] = tmp_tri;
					tmp_tri = new triangle(t->vert[(i+1)%3], v, tmp_vert);         new_t[3] = tmp_tri;
					new_t[0]->set_neighbor(new_t[2], new_t[1], t->neigh[(i+2)%3]);
					new_t[1]->set_neighbor(new_t[0], new_t[3], t->neigh[(i+1)%3]);
					new_t[2]->set_neighbor(new_t[3], new_t[0], t->neigh[i]->neigh[(k+2)%3]);
					new_t[3]->set_neighbor(new_t[1], new_t[2], t->neigh[i]->neigh[k]);

					for (int n = 0; n < 2; n++)
					{
						if (new_t[n]->neigh[2] != nullptr)
						{
							for (int k = 0; k < 3; ++k) // replace neighbor for the oppositing triangle 
							{
								if (new_t[n]->neigh[2]->neigh[k] == t)
								{
									new_t[n]->neigh[2]->neigh[k] = new_t[n];
									break;
								}
							}
						}
					}

					for (int n = 2; n < 4; n++)
					{
						if (new_t[n]->neigh[2] != nullptr)
						{
							for (int k = 0; k < 3; ++k) // replace neighbor for the oppositing triangle 
							{
								if (new_t[n]->neigh[2]->neigh[k] == t->neigh[i])
								{
									new_t[n]->neigh[2]->neigh[k] = new_t[n];
									break;
								}
							}
						}
					}
					break;
				}
			}
			return t->neigh[i]; // Return the neighboring tiangle to be deleted
		}
	}

	// The new vertex is inside the triangle. create three new triangles
	for (int n = 0; n < 3; ++n)
	{
		tmp_tri = new triangle(t->vert[n], t->vert[(n+1)%3], v);
		new_t[n] = tmp_tri;
	}

	// sort neighbors for new triangles
	for (int n = 0; n < 3; ++n)
	{
		if (t->neigh[n] == nullptr)
		{
			new_t[n]->set_neighbor(nullptr, new_t[(n+1)%3], new_t[(n+2)%3]);
		}
		else
		{
			new_t[n]->set_neighbor(t->neigh[n], new_t[(n+1)%3], new_t[(n+2)%3]);
			for (int k = 0; k < 3; ++k) // replace neighbor for the oppositing triangle 
			{
				if (t->neigh[n]->neigh[k] == t)
				{
					t->neigh[n]->neigh[k] = new_t[n];
					break;
				}
			}
		}
	}
	return nullptr;
}
// End triangle definition

/**
 * @brief      Test if the input triangle is inside of the given polygon
 * 
 * @param tri_p       Pointer of a test triangle
 * @param poly_vert   Vertexes of a polygon. The vertexes should be orderly stored
 * @return true       The test triangle is inside of the polygon
 * @return false      The test triangle is outside of the polygon
 */
bool triangle_inside_polygon(triangle *tri_p, std::vector<vertex2dc> *poly_vert)
{
	// If any vertexes of the input triangle is outside of the polygon, return false. Otherwise, return true
	if (poly_vert->size() < 3)
	{
		return true;
	}
	else
	{
		int pnum = poly_vert->size();
		//确定外接矩形
		double xmin = poly_vert->at(0).x, ymin = poly_vert->at(0).y;
		double xmax = poly_vert->at(0).x, ymax = poly_vert->at(0).y;
		for (int j = 0; j < pnum; ++j)
		{
			xmin = std::min(xmin, poly_vert->at(j).x);
			xmax = std::max(xmax, poly_vert->at(j).x);
			ymin = std::min(ymin, poly_vert->at(j).y);
			ymax = std::max(ymax, poly_vert->at(j).y);
		}

		int count;
		double tmp_x;
		vertex2dc *one_p;
		for (int t = 0; t < 3; ++t)
		{
			one_p = tri_p->vert[t];
			// the testing point is outside of the surrounding box, return false
			if (one_p->x < xmin || one_p->x > xmax || one_p->y < ymin || one_p->y > ymax)
			{
				return false;
			}
			else
			{
				count = 0;

				for (int i = 0; i < pnum; ++i)
				{
					if (((one_p->y >= poly_vert->at(i).y && one_p->y < poly_vert->at((i+1)%pnum).y) ||
						(one_p->y <= poly_vert->at(i).y && one_p->y > poly_vert->at((i+1)%pnum).y)) &&
						poly_vert->at(i).y != poly_vert->at((i+1)%pnum).y)
					{
						tmp_x = (poly_vert->at((i+1)%pnum).x - poly_vert->at(i).x) 
							* (one_p->y - poly_vert->at(i).y)/(poly_vert->at((i+1)%pnum).y - poly_vert->at(i).y) 
							+ poly_vert->at(i).x;

						if (one_p->x < tmp_x)
							count++;
					}
				}

				if (count != 0 && pow(-1, count) > 0) return false;
			}
		}
		
		// all vertexes are inside the polygon
		return true;
	}
}

/**
 * @brief      Test if the input locaiton is inside of the given polygon
 * 
 * @param x           Input x coordinate
 * @param y           Input y coordinate
 * @param poly_vert   Vertexes of a polygon. The vertexes should be orderly stored
 * @return true       The test triangle is inside of the polygon
 * @return false      The test triangle is outside of the polygon
 */
bool location_inside_polygon(double x, double y, std::vector<vertex2dc> *poly_vert)
{
	// If any vertexes of the input triangle is outside of the polygon, return false. Otherwise, return true
	if (poly_vert->size() < 3)
	{
		return true;
	}
	else
	{
		int pnum = poly_vert->size();
		//确定外接矩形
		double xmin = poly_vert->at(0).x, ymin = poly_vert->at(0).y;
		double xmax = poly_vert->at(0).x, ymax = poly_vert->at(0).y;
		for (int j = 0; j < pnum; ++j)
		{
			xmin = std::min(xmin, poly_vert->at(j).x);
			xmax = std::max(xmax, poly_vert->at(j).x);
			ymin = std::min(ymin, poly_vert->at(j).y);
			ymax = std::max(ymax, poly_vert->at(j).y);
		}

		int count;
		double tmp_x;
		
		// the testing point is outside of the surrounding box, return false
		if (x < xmin || x > xmax || y < ymin || y > ymax)
		{
			return false;
		}
		else
		{
			count = 0;

			for (int i = 0; i < pnum; ++i)
			{
				if (((y >= poly_vert->at(i).y && y < poly_vert->at((i+1)%pnum).y) ||
					(y <= poly_vert->at(i).y && y > poly_vert->at((i+1)%pnum).y)) &&
					poly_vert->at(i).y != poly_vert->at((i+1)%pnum).y)
				{
					tmp_x = (poly_vert->at((i+1)%pnum).x - poly_vert->at(i).x) 
						* (y - poly_vert->at(i).y)/(poly_vert->at((i+1)%pnum).y - poly_vert->at(i).y) 
						+ poly_vert->at(i).x;

					if (x < tmp_x) count++;
				}
			}

			if (count != 0 && pow(-1, count) > 0) return false;
		}
		
		return true;
	}
}


/**
 * @brief      Generate the TIN from the DEM grid
 *
 * @param[in]  dem          Input DEM grid (Ordered from lower left corner to the upper right corner)
 * @param[in]  xmin         The minimal coordinate of the DEM grid on the x-axis
 * @param[in]  xmax         The maximal coordinate of the DEM grid on the x-axis
 * @param[in]  ymin         The minimal coordinate of the DEM grid on the y-axis
 * @param[in]  ymax         The maximal coordinate of the DEM grid on the y-axis
 * @param[in]  dx           Data spacing of the DEM grid on the x-axis
 * @param[in]  dy           Data spacing of the DEM grid on the y-axis
 * @param      out_verts    The output vector of vertex's pointers. The user need to destroy the memories allocated by the function before destroy the vector
 * @param      out_tris     The output vector of triangle's pointers. The user need to destroy the memories allocated by the function before destroy the vector
 * @param[in]  maxi_err     Threshold to quit the algorithm. The default is 1e-0
 * @param[in]  err_records  If this pointer is not NULL, record maximal error values after each insertion of vertex.
 */
void grd2tin(const std::vector<double> &dem, double xmin, double xmax, double ymin, double ymax, double dx, 
	double dy, std::vector<vertex2dc*> &out_verts, std::vector<triangle*> &out_tris, double maxi_err, 
	std::vector<vertex2dc> *outline_poly, std::vector<double> *err_records)
{
	if (!out_verts.empty()) out_verts.clear();
	if (!out_tris.empty())  out_tris.clear();
	if (err_records != nullptr && !err_records->empty()) err_records->clear();

	if (dx <= 0.0 || dy <= 0.0 || maxi_err <= 0.0) return;
	if (xmin >= xmax || ymin >= ymax || (xmin + dx) > xmax || (ymin + dy) > ymax) return;

	int xnum = round((xmax - xmin)/dx) + 1;
	int ynum = round((ymax - ymin)/dy) + 1;

	if (dem.size() != xnum*ynum) return;

	// Prepare the DEM points
	dem_point *tmp_dem = nullptr;
	dem_point *maxi_err_dem = nullptr;
	std::vector<dem_point*>::iterator d_iter;

	vertex2dc *tmp_vert = nullptr;
	tmp_vert = new vertex2dc(xmin, ymin, dem[0], out_verts.size()); // lower left corner
	out_verts.push_back(tmp_vert);

	tmp_vert = new vertex2dc(xmax, ymin, dem[xnum-1], out_verts.size()); // lower right corner
	out_verts.push_back(tmp_vert);

	tmp_vert = new vertex2dc(xmax, ymax, dem[xnum*ynum-1], out_verts.size()); // upper right corner
	out_verts.push_back(tmp_vert);

	tmp_vert = new vertex2dc(xmin, ymax, dem[xnum*(ynum-1)], out_verts.size()); // upper left corner
	out_verts.push_back(tmp_vert);

	triangle *old_tri = nullptr, *tmp_tri = nullptr;
	triangle *cnst_tri[4];
	std::vector<triangle*>::iterator t_iter;

	if (!is_collinear(out_verts[0], out_verts[1], out_verts[2])) // Do not create triangle if the vertexes are collinear
	{
		tmp_tri = new triangle(out_verts[0], out_verts[1], out_verts[2]); // order the vertex anti-clock wise
		out_tris.push_back(tmp_tri); tmp_tri = nullptr;
	}

	if (!is_collinear(out_verts[0], out_verts[2], out_verts[3]))
	{
		tmp_tri = new triangle(out_verts[0], out_verts[2], out_verts[3]); // order the vertex anti-clock wise
		out_tris.push_back(tmp_tri); tmp_tri = nullptr;
	}

	if (out_tris.size() != 2) return;

	out_tris[0]->set_neighbor(nullptr, nullptr, out_tris[1]);
	out_tris[1]->set_neighbor(out_tris[0], nullptr, nullptr);

	// Find host triangle for all DEM locations
	int tmp_id;
	if (outline_poly == nullptr)
	{
		for (int i = 0; i < ynum; ++i)
		{
			for (int j = 0; j < xnum; ++j)
			{
				tmp_id = j + i*xnum;
				if (tmp_id != 0 && tmp_id != (xnum-1) && tmp_id != (xnum*ynum-1) && tmp_id != (xnum*(ynum-1))) // the four corners are already used
				{
					tmp_dem = new dem_point(xmin + dx*j, ymin + dy*i, dem[j + i*xnum]);
					for (int t = 0; t < out_tris.size(); ++t)
					{
						if (out_tris[t]->bound_location(tmp_dem->x, tmp_dem->y))
						{
							tmp_dem->host = out_tris[t];
							tmp_dem->err = fabs(out_tris[t]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
							out_tris[t]->hosted_dem.push_back(tmp_dem);
							break; // already found, no need to search more
						}
					}
				}
			}
		}
	}
	else
	{
		for (int i = 0; i < ynum; ++i)
		{
			for (int j = 0; j < xnum; ++j)
			{
				tmp_id = j + i*xnum;
				if (tmp_id != 0 && tmp_id != (xnum-1) && 
					tmp_id != (xnum*ynum-1) && tmp_id != (xnum*(ynum-1)) && 
					location_inside_polygon(xmin+dx*j, ymin+dy*i, outline_poly)) // the four corners are already used
				{
					tmp_dem = new dem_point(xmin + dx*j, ymin + dy*i, dem[j + i*xnum]);
					for (int t = 0; t < out_tris.size(); ++t)
					{
						if (out_tris[t]->bound_location(tmp_dem->x, tmp_dem->y))
						{
							tmp_dem->host = out_tris[t];
							tmp_dem->err = fabs(out_tris[t]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
							out_tris[t]->hosted_dem.push_back(tmp_dem);
							break; // already found, no need to search more
						}
					}
				}
			}
		}
	}

	// Sort hosted_dem in the desceding order with respect to the error. Get pointer of the dem_point with maximal error
	double maxi_err_tmp = -1.0;
	for (int t = 0; t < out_tris.size(); ++t)
	{
		std::sort(out_tris[t]->hosted_dem.begin(), out_tris[t]->hosted_dem.end(), compare_dem_point);
		if (out_tris[t]->hosted_dem[0]->err > maxi_err_tmp)
		{
			maxi_err_tmp = out_tris[t]->hosted_dem[0]->err; 
			maxi_err_dem = out_tris[t]->hosted_dem[0];
		}
	}

	while (maxi_err_dem->err >= maxi_err) // quit til the threshold is meet
	{
		if (err_records != nullptr)
		{
			err_records->push_back(maxi_err_dem->err);
		}

		// find the triangle that includes maxi_err_dem and remove it from out_tris
		for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
		{
			old_tri = *t_iter;
			if (old_tri == maxi_err_dem->host)
			{
				t_iter = out_tris.erase(t_iter);
				break;
			}
			else t_iter++;
		}

		// remove maxi_err_dem from its host triangle's hosted DEM list
		for (d_iter = old_tri->hosted_dem.begin(); d_iter != old_tri->hosted_dem.end(); )
		{
			if (maxi_err_dem == *d_iter)
			{
				d_iter = old_tri->hosted_dem.erase(d_iter);
				break;
			}
			else d_iter++;
		}

		// create a new vertex
		tmp_vert = new vertex2dc(maxi_err_dem->x, maxi_err_dem->y, maxi_err_dem->elev, out_verts.size());
		out_verts.push_back(tmp_vert);

		// Delete maxi_err_dem
		delete maxi_err_dem; maxi_err_dem = nullptr;

		// build new triangles
		tmp_tri = split_triangle(tmp_vert, old_tri, cnst_tri);
		for (int n = 0; n < 4; ++n)
		{
			if (cnst_tri[n] != nullptr)
			{
				out_tris.push_back(cnst_tri[n]);
			}
		}

		if (tmp_tri != nullptr)
		{
			for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
			{
				if (tmp_tri == *t_iter)
				{
					t_iter = out_tris.erase(t_iter);
					break;
				}
				else t_iter++;
			}

			// build hosted dem for the new triangles
			for (int d = 0; d < old_tri->hosted_dem.size(); d++)
			{
				tmp_dem = old_tri->hosted_dem[d];
				for (int n = 0; n < 4; n++)
				{
					if (cnst_tri[n] != nullptr && cnst_tri[n]->bound_location(tmp_dem->x, tmp_dem->y))
					{
						tmp_dem->host = cnst_tri[n];
						tmp_dem->err = fabs(cnst_tri[n]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
						cnst_tri[n]->hosted_dem.push_back(tmp_dem);
						break;
					}
				}
			}

			for (int d = 0; d < tmp_tri->hosted_dem.size(); d++)
			{
				tmp_dem = tmp_tri->hosted_dem[d];
				for (int n = 0; n < 4; n++)
				{
					if (cnst_tri[n] != nullptr && cnst_tri[n]->bound_location(tmp_dem->x, tmp_dem->y))
					{
						tmp_dem->host = cnst_tri[n];
						tmp_dem->err = fabs(cnst_tri[n]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
						cnst_tri[n]->hosted_dem.push_back(tmp_dem);
						break;
					}
				}
			}

			for (int n = 0; n < 4; n++)
			{
				if (cnst_tri[n] != nullptr)
				{
					std::sort(cnst_tri[n]->hosted_dem.begin(), cnst_tri[n]->hosted_dem.end(), compare_dem_point);
				}
			}

			// delete the old triangle
			old_tri->hosted_dem.clear();
			delete old_tri; old_tri = nullptr;

			tmp_tri->hosted_dem.clear();
			delete tmp_tri; tmp_tri = nullptr;
		}
		else
		{
			// build hosted dem for the new triangles
			for (int d = 0; d < old_tri->hosted_dem.size(); d++)
			{
				tmp_dem = old_tri->hosted_dem[d];
				for (int n = 0; n < 4; n++)
				{
					if (cnst_tri[n] != nullptr && cnst_tri[n]->bound_location(tmp_dem->x, tmp_dem->y))
					{
						tmp_dem->host = cnst_tri[n];
						tmp_dem->err = fabs(cnst_tri[n]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
						cnst_tri[n]->hosted_dem.push_back(tmp_dem);
						break;
					}
				}
			}

			for (int n = 0; n < 4; n++)
			{
				if (cnst_tri[n] != nullptr)
				{
					std::sort(cnst_tri[n]->hosted_dem.begin(), cnst_tri[n]->hosted_dem.end(), compare_dem_point);
				}
			}

			// delete the old triangle
			old_tri->hosted_dem.clear();
			delete old_tri; old_tri = nullptr;
		}

		// Make sure cnst_tri meet the empty circumcircle condition
		for (int n = 0; n < 4; ++n)
		{
			if (cnst_tri[n] != nullptr)
			{
				make_delaunay(cnst_tri[n]);
			}
		}

		// get maximal errors from out_tris
		maxi_err_tmp = -1.0;
		for (int t = 0; t < out_tris.size(); t++)
		{
			if (!out_tris[t]->hosted_dem.empty() && out_tris[t]->hosted_dem[0]->err > maxi_err_tmp)
			{
				maxi_err_tmp = out_tris[t]->hosted_dem[0]->err;
				maxi_err_dem = out_tris[t]->hosted_dem[0];
			}
		}
	}

	if (err_records != nullptr)
	{
		err_records->push_back(maxi_err_dem->err);
	}

	// Cut outline if there is one
	if (outline_poly != nullptr)
	{
		for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
		{
			tmp_tri = *t_iter;
			if (!triangle_inside_polygon(tmp_tri, outline_poly))
			{
				// set neighbors
				for (int i = 0; i < 3; ++i)
				{
					if (tmp_tri->neigh[i] != nullptr)
					{
						for (int j = 0; j < 3; ++j)
						{
							if (tmp_tri->neigh[i]->neigh[j] == tmp_tri)
							{
								tmp_tri->neigh[i]->neigh[j] = nullptr;
								break;
							}
						}
					}
				}
				// destroy the memories located and remove from the vector
				t_iter = out_tris.erase(t_iter);
				delete tmp_tri; tmp_tri = nullptr;
			}
			else t_iter++;
		}
	}

	// assign triangles index
	for (int i = 0; i < out_tris.size(); i++)
	{
		out_tris[i]->id = i;
		// destroy remaining DEM data
		for (int d = 0; d < out_tris[i]->hosted_dem.size(); d++)
		{
			tmp_dem = out_tris[i]->hosted_dem[d];
			delete tmp_dem; tmp_dem = nullptr;
		}
	}
	return;
}


/**
 * @brief      Generate the TIN from random DEM points
 * 
 * @param      dem          Input DEM points
 * @param      out_verts    The output vector of vertex's pointers. The user need to destroy the memories allocated by the function before destroy the vector
 * @param      out_tris     The output vector of triangle's pointers. The user need to destroy the memories allocated by the function before destroy the vector
 * @param[in]  maxi_err     Threshold to quit the algorithm. The default is 1e-0
 * @param[in]  outline_poly If this pointer is not NULL, Cut triangle outside of or intersected with the polygon.
 * @param[in]  err_records  If this pointer is not NULL, record maximal error values after each insertion of vertex.
 */
void rnd2tin(const std::vector<dem_point> &dem, std::vector<vertex2dc*> &out_verts, std::vector<triangle*> &out_tris, 
	double maxi_err, std::vector<vertex2dc> *outline_poly, std::vector<double> *err_records)
{
	if (dem.size() < 3) return;
	if (maxi_err <= 0.0) return;

	if (!out_verts.empty()) out_verts.clear();
	if (!out_tris.empty())  out_tris.clear();
	if (err_records != nullptr && !err_records->empty()) err_records->clear();

	// locate the surrounding box and initiate the staring two triangles
	double xmin = dem[0].x, xmax = dem[0].x;
	double ymin = dem[0].y, ymax = dem[0].y;
	for (int i = 0; i < dem.size(); ++i)
	{
		xmin = std::min(xmin, dem[i].x);
		xmax = std::max(xmax, dem[i].x);
		ymin = std::min(ymin, dem[i].y);
		ymax = std::max(ymax, dem[i].y);
	}

	double midx = 0.5*(xmin + xmax);
	double midy = 0.5*(ymin + ymax);
	double maxi_s = std::max(xmax - xmin, ymax - ymin); // use an four times bigger rectangle to include all points

	vertex2dc *tmp_vert = nullptr;
	std::vector<vertex2dc*> box_vert;

	tmp_vert = new vertex2dc(midx - maxi_s, midy - maxi_s, 0.0, 0); // lower left corner
	box_vert.push_back(tmp_vert);

	tmp_vert = new vertex2dc(midx + maxi_s, midy - maxi_s, 0.0, 1); // lower right corner
	box_vert.push_back(tmp_vert);

	tmp_vert = new vertex2dc(midx + maxi_s, midy + maxi_s, 0.0, 2); // upper right corner
	box_vert.push_back(tmp_vert);

	tmp_vert = new vertex2dc(midx - maxi_s, midy + maxi_s, 0.0, 3); // upper left corner
	box_vert.push_back(tmp_vert);

	triangle *old_tri = nullptr, *tmp_tri = nullptr;
	triangle *cnst_tri[4];
	std::vector<triangle*>::iterator t_iter;

	if (!is_collinear(box_vert[0], box_vert[1], box_vert[2])) // Do not create triangle if the vertexes are collinear
	{
		tmp_tri = new triangle(box_vert[0], box_vert[1], box_vert[2]); // order the vertex anti-clock wise
		out_tris.push_back(tmp_tri); tmp_tri = nullptr;
	}

	if (!is_collinear(box_vert[0], box_vert[2], box_vert[3]))
	{
		tmp_tri = new triangle(box_vert[0], box_vert[2], box_vert[3]); // order the vertex anti-clock wise
		out_tris.push_back(tmp_tri); tmp_tri = nullptr;
	}

	if (out_tris.size() != 2) return;

	out_tris[0]->set_neighbor(nullptr, nullptr, out_tris[1]);
	out_tris[1]->set_neighbor(out_tris[0], nullptr, nullptr);

	// Prepare the DEM points
	dem_point *tmp_dem = nullptr;
	dem_point *maxi_err_dem = nullptr;
	std::vector<dem_point*>::iterator d_iter;

	// Find host triangle for all DEM locations
	if (outline_poly == nullptr)
	{
		for (int d = 0; d < dem.size(); ++d)
		{
			tmp_dem = new dem_point(dem[d].x, dem[d].y, dem[d].elev);
			for (int t = 0; t < out_tris.size(); ++t)
			{
				if (out_tris[t]->bound_location(tmp_dem->x, tmp_dem->y))
				{
					tmp_dem->host = out_tris[t];
					tmp_dem->err = fabs(out_tris[t]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
					out_tris[t]->hosted_dem.push_back(tmp_dem);
					break; // already found, no need to search more
				}
			}
		}
	}
	else
	{
		for (int d = 0; d < dem.size(); ++d)
		{
			if (location_inside_polygon(dem[d].x, dem[d].y, outline_poly))
			{
				tmp_dem = new dem_point(dem[d].x, dem[d].y, dem[d].elev);
				for (int t = 0; t < out_tris.size(); ++t)
				{
					if (out_tris[t]->bound_location(tmp_dem->x, tmp_dem->y))
					{
						tmp_dem->host = out_tris[t];
						tmp_dem->err = fabs(out_tris[t]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
						out_tris[t]->hosted_dem.push_back(tmp_dem);
						break; // already found, no need to search more
					}
				}
			}
		}
	}

	// Sort hosted_dem in the desceding order with respect to the error. Get pointer of the dem_point with maximal error
	double maxi_err_tmp = -1.0;
	for (int t = 0; t < out_tris.size(); ++t)
	{
		std::sort(out_tris[t]->hosted_dem.begin(), out_tris[t]->hosted_dem.end(), compare_dem_point);
		if (out_tris[t]->hosted_dem[0]->err > maxi_err_tmp)
		{
			maxi_err_tmp = out_tris[t]->hosted_dem[0]->err; 
			maxi_err_dem = out_tris[t]->hosted_dem[0];
		}
	}

	while (maxi_err_dem->err >= maxi_err) // quit til the threshold is meet
	{
		if (err_records != nullptr)
		{
			err_records->push_back(maxi_err_dem->err);
		}

		// find the triangle that includes maxi_err_dem and remove it from out_tris
		for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
		{
			old_tri = *t_iter;
			if (old_tri == maxi_err_dem->host)
			{
				t_iter = out_tris.erase(t_iter);
				break;
			}
			else t_iter++;
		}

		// remove maxi_err_dem from its host triangle's hosted DEM list
		for (d_iter = old_tri->hosted_dem.begin(); d_iter != old_tri->hosted_dem.end(); )
		{
			if (maxi_err_dem == *d_iter)
			{
				d_iter = old_tri->hosted_dem.erase(d_iter);
				break;
			}
			else d_iter++;
		}

		// create a new vertex
		tmp_vert = new vertex2dc(maxi_err_dem->x, maxi_err_dem->y, maxi_err_dem->elev, out_verts.size());
		out_verts.push_back(tmp_vert);

		// Delete maxi_err_dem
		delete maxi_err_dem; maxi_err_dem = nullptr;

		// build new triangles
		tmp_tri = split_triangle(tmp_vert, old_tri, cnst_tri);
		for (int n = 0; n < 4; ++n)
		{
			if (cnst_tri[n] != nullptr)
			{
				out_tris.push_back(cnst_tri[n]);
			}
		}

		if (tmp_tri != nullptr)
		{
			for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
			{
				if (tmp_tri == *t_iter)
				{
					t_iter = out_tris.erase(t_iter);
					break;
				}
				else t_iter++;
			}

			// build hosted dem for the new triangles
			for (int d = 0; d < old_tri->hosted_dem.size(); d++)
			{
				tmp_dem = old_tri->hosted_dem[d];
				for (int n = 0; n < 4; n++)
				{
					if (cnst_tri[n] != nullptr && cnst_tri[n]->bound_location(tmp_dem->x, tmp_dem->y))
					{
						tmp_dem->host = cnst_tri[n];
						tmp_dem->err = fabs(cnst_tri[n]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
						cnst_tri[n]->hosted_dem.push_back(tmp_dem);
						break;
					}
				}
			}

			for (int d = 0; d < tmp_tri->hosted_dem.size(); d++)
			{
				tmp_dem = tmp_tri->hosted_dem[d];
				for (int n = 0; n < 4; n++)
				{
					if (cnst_tri[n] != nullptr && cnst_tri[n]->bound_location(tmp_dem->x, tmp_dem->y))
					{
						tmp_dem->host = cnst_tri[n];
						tmp_dem->err = fabs(cnst_tri[n]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
						cnst_tri[n]->hosted_dem.push_back(tmp_dem);
						break;
					}
				}
			}

			for (int n = 0; n < 4; n++)
			{
				if (cnst_tri[n] != nullptr)
				{
					std::sort(cnst_tri[n]->hosted_dem.begin(), cnst_tri[n]->hosted_dem.end(), compare_dem_point);
				}
			}

			// delete the old triangle
			old_tri->hosted_dem.clear();
			delete old_tri; old_tri = nullptr;

			tmp_tri->hosted_dem.clear();
			delete tmp_tri; tmp_tri = nullptr;
		}
		else
		{
			// build hosted dem for the new triangles
			for (int d = 0; d < old_tri->hosted_dem.size(); d++)
			{
				tmp_dem = old_tri->hosted_dem[d];
				for (int n = 0; n < 4; n++)
				{
					if (cnst_tri[n] != nullptr && cnst_tri[n]->bound_location(tmp_dem->x, tmp_dem->y))
					{
						tmp_dem->host = cnst_tri[n];
						tmp_dem->err = fabs(cnst_tri[n]->interpolate(tmp_dem->x, tmp_dem->y) - tmp_dem->elev);
						cnst_tri[n]->hosted_dem.push_back(tmp_dem);
						break;
					}
				}
			}

			for (int n = 0; n < 4; n++)
			{
				if (cnst_tri[n] != nullptr)
				{
					std::sort(cnst_tri[n]->hosted_dem.begin(), cnst_tri[n]->hosted_dem.end(), compare_dem_point);
				}
			}

			// delete the old triangle
			old_tri->hosted_dem.clear();
			delete old_tri; old_tri = nullptr;
		}

		// Make sure cnst_tri meet the empty circumcircle condition
		for (int n = 0; n < 4; ++n)
		{
			if (cnst_tri[n] != nullptr)
			{
				make_delaunay(cnst_tri[n]);
			}
		}

		// get maximal errors from out_tris
		maxi_err_tmp = -1.0;
		for (int t = 0; t < out_tris.size(); t++)
		{
			if (!out_tris[t]->hosted_dem.empty() && out_tris[t]->hosted_dem[0]->err > maxi_err_tmp)
			{
				maxi_err_tmp = out_tris[t]->hosted_dem[0]->err;
				maxi_err_dem = out_tris[t]->hosted_dem[0];
			}
		}
	}

	if (err_records != nullptr)
	{
		err_records->push_back(maxi_err_dem->err);
	}

	// remove any triangles has an box vertex from out_tris
	for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
	{
		tmp_tri = *t_iter;
		if (tmp_tri->vert[0] == box_vert[0] || tmp_tri->vert[0] == box_vert[1] || tmp_tri->vert[0] == box_vert[2] || tmp_tri->vert[0] == box_vert[3])
		{
			if (tmp_tri->neigh[1] != nullptr)
			{
				for (int k = 0; k < 3; ++k)
				{
					if (tmp_tri->neigh[1]->neigh[k] == tmp_tri)
					{
						tmp_tri->neigh[1]->neigh[k] = nullptr;
						break;
					}
				}
			}
			// destroy the memories located and remove from the vector
			t_iter = out_tris.erase(t_iter);
			delete tmp_tri; tmp_tri = nullptr;
		}
		else if (tmp_tri->vert[1] == box_vert[0] || tmp_tri->vert[1] == box_vert[1] || tmp_tri->vert[1] == box_vert[2] || tmp_tri->vert[1] == box_vert[3])
		{
			if (tmp_tri->neigh[2] != nullptr)
			{
				for (int k = 0; k < 3; ++k)
				{
					if (tmp_tri->neigh[2]->neigh[k] == tmp_tri)
					{
						tmp_tri->neigh[2]->neigh[k] = nullptr;
						break;
					}
				}
			}
			// destroy the memories located and remove from the vector
			t_iter = out_tris.erase(t_iter);
			delete tmp_tri; tmp_tri = nullptr;
		}
		else if (tmp_tri->vert[2] == box_vert[0] || tmp_tri->vert[2] == box_vert[1] || tmp_tri->vert[2] == box_vert[2] || tmp_tri->vert[2] == box_vert[3])
		{
			if (tmp_tri->neigh[0] != nullptr)
			{
				for (int k = 0; k < 3; ++k)
				{
					if (tmp_tri->neigh[0]->neigh[k] == tmp_tri)
					{
						tmp_tri->neigh[0]->neigh[k] = nullptr;
						break;
					}
				}
			}
			// destroy the memories located and remove from the vector
			t_iter = out_tris.erase(t_iter);
			delete tmp_tri; tmp_tri = nullptr;
		}
		else t_iter++;
	}

	// Cut outline if there is one
	if (outline_poly != nullptr)
	{
		for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
		{
			tmp_tri = *t_iter;
			if (!triangle_inside_polygon(tmp_tri, outline_poly))
			{
				// set neighbors
				for (int i = 0; i < 3; ++i)
				{
					if (tmp_tri->neigh[i] != nullptr)
					{
						for (int j = 0; j < 3; ++j)
						{
							if (tmp_tri->neigh[i]->neigh[j] == tmp_tri)
							{
								tmp_tri->neigh[i]->neigh[j] = nullptr;
								break;
							}
						}
					}
				}
				// destroy the memories located and remove from the vector
				t_iter = out_tris.erase(t_iter);
				delete tmp_tri; tmp_tri = nullptr;
			}
			else t_iter++;
		}
	}

	// assign triangles index
	for (int i = 0; i < out_tris.size(); i++)
	{
		out_tris[i]->id = i;
		// destroy remaining DEM data
		for (int d = 0; d < out_tris[i]->hosted_dem.size(); d++)
		{
			tmp_dem = out_tris[i]->hosted_dem[d];
			delete tmp_dem; tmp_dem = nullptr;
		}
	}

	// destroy memories located for box_vert
	for (int i = 0; i < 4; ++i)
	{
		delete box_vert[i]; box_vert[i] = nullptr;
	}
	return;
}
