/**
 * @defgroup   TIN
 *
 * @brief      Generation of a Triangular Irregular Network (TIN) from a dense DEM grid
 *
 * @author     Yi Zhang
 * @date       2021-09-15
 */

#ifndef _TIN_DELAUNAY_H
#define _TIN_DELAUNAY_H
#include "cmath"
#include "vector"

#include "iostream"

#define ZERO 1e-5

// Start vertex definition
struct vertex2dc
{
	unsigned int id; // index of the vertex
	double x, y; // position of the vertex
	double elev; // elevation at the vertex

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

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

// Start edge definition
struct edge
{
	vertex2dc *vert[2]; // vertex of the edge

	edge() {vert[0] = vert[1] = nullptr;}
	edge(vertex2dc *v0ptr, vertex2dc *v1ptr) {set(v0ptr, v1ptr);}
	void set(vertex2dc *v0ptr, vertex2dc *v1ptr)
	{
		vert[0] = v0ptr; vert[1] = v1ptr;
		return;
	}
};

bool operator ==(const edge &a, const edge &b) // overload the == operator for edge type
{
	if((a.vert[0] == b.vert[0] && a.vert[1] == b.vert[1]) || 
		(a.vert[0] == b.vert[1] && a.vert[1] == b.vert[0]))
	{
		return true;
	}
	return false;
}
// End edge definition

// Start triangle definition
struct triangle
{
	vertex2dc *vert[3]; // vertex of the triangle
	double cx, cy; // center of the triangle's circumcircle
	double cr; // radius of the circumcircle

	triangle() {vert[0] = vert[1] = vert[2] = nullptr;}
	triangle(vertex2dc *v0ptr, vertex2dc *v1ptr, vertex2dc *v2ptr) {set(v0ptr, v1ptr, v2ptr);}
	void set(vertex2dc *v0ptr, vertex2dc *v1ptr, vertex2dc *v2ptr)
	{
		vert[0] = v0ptr; vert[1] = v1ptr; vert[2] = v2ptr;

		double s = 0.5 / ((vert[1]->x - vert[0]->x) * (vert[2]->y - vert[0]->y) - (vert[1]->y - vert[0]->y) * (vert[2]->x - vert[0]->x));
		double m = vert[1]->x * vert[1]->x - vert[0]->x * vert[0]->x + vert[1]->y * vert[1]->y - vert[0]->y * vert[0]->y;
		double u = vert[2]->x * vert[2]->x - vert[0]->x * vert[0]->x + vert[2]->y * vert[2]->y - vert[0]->y * vert[0]->y;

		cx = ((vert[2]->y - vert[0]->y) * m + (vert[0]->y - vert[1]->y) * u) * s;
		cy = ((vert[0]->x - vert[2]->x) * m + (vert[1]->x - vert[0]->x) * u) * s;
		cr = (vert[0]->x - cx) * (vert[0]->x - cx) + (vert[0]->y - cy) * (vert[0]->y - cy); // not need to sqrt() here
		return;
	}

	bool 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)
			{
				return false;
			}
		}
		return true;
	}

	double 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);
	}
};
// End triangle definition

/**
 * @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-2
 */
void dem2tin(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 = 1e-2)
{
	if (!out_verts.empty()) out_verts.clear();
	if (!out_tris.empty())  out_tris.clear();

	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;

	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*> assit_vert, corner_vert;

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

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

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

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

	triangle *tmp_tri = nullptr;
	std::vector<triangle*> cnst_tri;
	std::vector<triangle*>::iterator t_iter;

	tmp_tri = new triangle(assit_vert[0], assit_vert[1], assit_vert[2]); // order the vertex anti-clock wise
	out_tris.push_back(tmp_tri);

	tmp_tri = new triangle(assit_vert[0], assit_vert[2], assit_vert[3]); // order the vertex anti-clock wise
	out_tris.push_back(tmp_tri);

	int now_maxi_id;
	double now_x, now_y, now_err;
	double now_maxi_err;

	bool removed;
	double dist;
	edge tmp_edge;
	std::vector<edge> cnst_edge;
	std::vector<edge>::iterator e_iter;

	// We first add the four corner points of the DEM to the TIN
	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);

	for (int i = 0; i < 4; ++i)
	{
		// determine triangles that include the point and add the triangle to the cnst_tri and remove the triangle from out_tris
		// this is also a part that could take a lot of time if we are working with a large amount of points. We will fix this later
		cnst_tri.clear();
		for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
		{
			tmp_tri = *t_iter;
			dist = (tmp_tri->cx - out_verts[i]->x) * (tmp_tri->cx - out_verts[i]->x) 
				+ (tmp_tri->cy - out_verts[i]->y) * (tmp_tri->cy - out_verts[i]->y);
			if ((dist - tmp_tri->cr) <= ZERO) // Points on the circumcircle are also included
			{
				t_iter = out_tris.erase(t_iter);
				cnst_tri.push_back(tmp_tri);
			}
			else t_iter++;
		}

		// loop to remove duplicate edges
		cnst_edge.clear();
		for (int c = 0; c < cnst_tri.size(); ++c)
		{
			for (int e = 0; e < 3; ++e)
			{
				tmp_edge.set(cnst_tri[c]->vert[e], cnst_tri[c]->vert[(e+1)%3]);

				removed = false;
				for (e_iter = cnst_edge.begin(); e_iter != cnst_edge.end(); )
				{
					if (tmp_edge == *e_iter) // duplicate edge, remove from cnst_edge
					{
						e_iter = cnst_edge.erase(e_iter);
						removed = true;
						break; // no need to search more
					}
					else e_iter++;
				}

				if (!removed) // not a duplicate edge, add to the cnst_edge
				{
					cnst_edge.push_back(tmp_edge);
				}
			}
		}

		// construct new triangles and add to out_tris
		for (int c = 0; c < cnst_edge.size(); ++c)
		{
			tmp_tri = new triangle(cnst_edge[c].vert[0], cnst_edge[c].vert[1], out_verts[i]); // order the vertex anti-clock wise
			out_tris.push_back(tmp_tri);
		}

		// destroy memories used by cnst_edge
		for (int c = 0; c < cnst_tri.size(); ++c)
		{
			tmp_tri = cnst_tri[c];
			delete tmp_tri; tmp_tri = nullptr;
		}
	}

	do // quit til the threshold is meet
	{
		// loop all DEM data to find the location with maximal error
		// this part is very time consuming. We will fix it later
		now_maxi_err = -1.0;
		for (int i = 0; i < xnum*ynum; ++i)
		{
			now_x = (i%xnum)*dx + xmin;
			now_y = (i/xnum)*dy + ymin;
			for (int e = 0; e < out_tris.size(); ++e)
			{
				if (out_tris[e]->bound_location(now_x, now_y))
				{
					now_err = fabs(out_tris[e]->interpolate(now_x, now_y) - dem[i]);
					if (now_err > now_maxi_err)
					{
						now_maxi_err = now_err;
						now_maxi_id  = i;
					}
					break;
				}
			}
		}

		// create a new vertex
		now_x = (now_maxi_id%xnum)*dx + xmin;
		now_y = (now_maxi_id/xnum)*dy + ymin;
		tmp_vert = new vertex2dc(now_x, now_y, dem[now_maxi_id], out_verts.size());
		out_verts.push_back(tmp_vert);

		// determine triangles that include the point and add the triangle to the cnst_tri and remove the triangle from out_tris
		// this is also a part that could take a lot of time if we are working with a large amount of points. We will fix this later
		cnst_tri.clear();
		for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
		{
			tmp_tri = *t_iter;
			dist = (tmp_tri->cx - now_x) * (tmp_tri->cx - now_x) + (tmp_tri->cy - now_y) * (tmp_tri->cy - now_y);
			if ((dist - tmp_tri->cr) <= ZERO) // Points on the circumcircle are also included
			{
				t_iter = out_tris.erase(t_iter);
				cnst_tri.push_back(tmp_tri);
			}
			else t_iter++;
		}

		// loop to remove duplicate edges
		cnst_edge.clear();
		for (int c = 0; c < cnst_tri.size(); ++c)
		{
			for (int e = 0; e < 3; ++e)
			{
				tmp_edge.set(cnst_tri[c]->vert[e], cnst_tri[c]->vert[(e+1)%3]);

				removed = false;
				for (e_iter = cnst_edge.begin(); e_iter != cnst_edge.end(); )
				{
					if (tmp_edge == *e_iter) // duplicate edge, remove from cnst_edge
					{
						e_iter = cnst_edge.erase(e_iter);
						removed = true;
						break; // no need to search more
					}
					else e_iter++;
				}

				if (!removed) // not a duplicate edge, add to the cnst_edge
				{
					cnst_edge.push_back(tmp_edge);
				}
			}
		}

		// construct new triangles and add to out_tris
		for (int c = 0; c < cnst_edge.size(); ++c)
		{
			tmp_tri = new triangle(cnst_edge[c].vert[0], cnst_edge[c].vert[1], out_verts.back()); // order the vertex anti-clock wise
			out_tris.push_back(tmp_tri);
		}

		// destroy memories used by cnst_edge
		for (int c = 0; c < cnst_tri.size(); ++c)
		{
			tmp_tri = cnst_tri[c];
			delete tmp_tri; tmp_tri = nullptr;
		}
	} while (now_maxi_err >= maxi_err);

	// remove any triangles has an assistant vertex from out_tris
	for (t_iter = out_tris.begin(); t_iter != out_tris.end(); )
	{
		tmp_tri = *t_iter;
		if (tmp_tri->vert[0] == assit_vert[0] || tmp_tri->vert[0] == assit_vert[1] || tmp_tri->vert[0] == assit_vert[2] || tmp_tri->vert[0] == assit_vert[3] || 
			tmp_tri->vert[1] == assit_vert[0] || tmp_tri->vert[1] == assit_vert[1] || tmp_tri->vert[1] == assit_vert[2] || tmp_tri->vert[1] == assit_vert[3] || 
			tmp_tri->vert[2] == assit_vert[0] || tmp_tri->vert[2] == assit_vert[1] || tmp_tri->vert[2] == assit_vert[2] || tmp_tri->vert[2] == assit_vert[3])
		{
			// 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++;
	}

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

#endif // _TIN_DELAUNAY_H