using System;
using System.Collections.Generic;
using System.Linq;
using FlaxEngine;
using Console = Cabrito.Console;

namespace Game
{
	[Flags]
	public enum TriangleAttributes
	{
		NONE,
		NORMAL = 2,
		TEXCOORD = 4,
		COLOR = 8
	};

	public class Triangle
	{
		public Int3 v;
		public Vector3 n;
		//public Vector3[3] uvs;
		public Vector4 err;
		public bool dirty;
		public bool deleted;
		public TriangleAttributes attr;
		public int material;
	}

	public class Vertex
	{
		public Vector3 p;
		public int tstart,tcount;
		public SymetricMatrix q;
		public bool border;
	};

	public class Ref
	{
		public int tid, tvertex;

		/*public Ref()
		{
			tid = 0;
			tvertex = 0;
		}*/
	}

	public class SymetricMatrix
	{
		// Constructor

		public SymetricMatrix(double c = 0)
		{
			m = new double[10];
			for (int i=0; i<10; i++) m[i] = c;
		}

		public SymetricMatrix(	double m11, double m12, double m13, double m14,
			double m22, double m23, double m24,
			double m33, double m34,
			double m44)
		{
			m = new double[10];
			m[0] = m11;  m[1] = m12;  m[2] = m13;  m[3] = m14;
			m[4] = m22;  m[5] = m23;  m[6] = m24;
			m[7] = m33;  m[8] = m34;
			m[9] = m44;
		}

		// Make plane

		public SymetricMatrix(double a,double b,double c,double d)
		{
			m = new double[10];
			m[0] = a*a;  m[1] = a*b;  m[2] = a*c;  m[3] = a*d;
			m[4] = b*b;  m[5] = b*c;  m[6] = b*d;
			m[7 ] =c*c; m[8 ] = c*d;
			m[9 ] = d*d;
		}

		public double this[int index]
		{
			get { return m[index]; }
			set
			{
				m[index] = value;
			}
		}
		// Determinant

		public double det(	int a11, int a12, int a13,
			int a21, int a22, int a23,
			int a31, int a32, int a33)
		{
			double det =  m[a11]*m[a22]*m[a33] + m[a13]*m[a21]*m[a32] + m[a12]*m[a23]*m[a31]
			              - m[a13]*m[a22]*m[a31] - m[a11]*m[a23]*m[a32]- m[a12]*m[a21]*m[a33];
			return det;
		}



		public static SymetricMatrix operator+(SymetricMatrix o, SymetricMatrix n)
		{
			return new SymetricMatrix( o.m[0]+n[0],   o.m[1]+n[1],   o.m[2]+n[2],   o.m[3]+n[3],
				o.m[4]+n[4],   o.m[5]+n[5],   o.m[6]+n[6],
				o.m[ 7]+n[ 7], o.m[ 8]+n[8 ],
				o.m[ 9]+n[9 ]);
		}


		/*public static SymetricMatrix operator+=(SymetricMatrix o, SymetricMatrix n)
		{
			m[0]+=n[0];   m[1]+=n[1];   m[2]+=n[2];   m[3]+=n[3];
			m[4]+=n[4];   m[5]+=n[5];   m[6]+=n[6];   m[7]+=n[7];
			m[8]+=n[8];   m[9]+=n[9];
			return *this;
		}*/

		double[] m;
	};

	public static class ListExtras
	{
		//    list: List<T> to resize
		//    size: desired new size
		// element: default value to insert

		public static void Resize<T>(this List<T> list, int size, T element = default(T)) where T : new()
		{
			int count = list.Count;

			if (size < count)
			{
				list.RemoveRange(size, count - size);
			}
			else if (size > count)
			{
				if (size > list.Capacity)   // Optimization
					list.Capacity = size;

				list.AddRange(Enumerable.Repeat(element, size - count));
				for (int i = count; i < size; i++)
					list[i] = new T();
			}



		}
	}

	// Fast Quadratic Mesh Simplification
	public class MeshSimplifier
	{
		private float ratio;
		private float agressiveness;

		private List<Triangle> triangles;
		private List<Vertex> vertices;
		private List<Ref> refs = new List<Ref>();

		public MeshSimplifier(float ratio = 1.0f, float agressiveness = 7.0f)
		{
			this.ratio = ratio;
			this.agressiveness = agressiveness;
		}

		public List<Vector3> Simplify(Vector3[] input)
		{
			triangles = new List<Triangle>(input.Length / 3);
			vertices = new List<Vertex>();

			// TODO: no overlapping vertices, vertices must be unique


			{

				Dictionary<Vector3, int> verticeMap = new Dictionary<Vector3, int>();
				for (int i = 0; i < input.Length; i++)
				{
					if (!verticeMap.ContainsKey(input[i]))
						verticeMap[input[i]] = verticeMap.Count;
				}

				for (int i = 0; i < input.Length; i += 3)
				{
					int i1 = i + 0;
					int i2 = i + 1;
					int i3 = i + 2;
					Vector3 v1 = input[i1];
					Vector3 v2 = input[i2];
					Vector3 v3 = input[i3];

					if (verticeMap.ContainsKey(v1))
						i1 = verticeMap[v1];
					else
						verticeMap.Add(v1, i1);

					if (verticeMap.ContainsKey(v2))
						i2 = verticeMap[v2];
					else
						verticeMap.Add(v2, i2);

					if (verticeMap.ContainsKey(v3))
						i3 = verticeMap[v3];
					else
						verticeMap.Add(v3, i3);

					triangles.Add(new Triangle()
					{
						v = new Int3(i1, i2, i3),
					});
				}

				foreach (KeyValuePair<Vector3,int> kvp in verticeMap)
				{
					vertices.Add(new Vertex()
					{
						p = kvp.Key,
						q = new SymetricMatrix(),
					});
				}


				/*foreach (var vec in input)
				{
					vertices.Add(new Vertex()
					{
						p = vec,
						q = new SymetricMatrix(),
					});
				}*/
			}
			return Simplify();
		}

		public List<Vector3> Simplify(Vector3[] verts, int[] indices)
		{
			triangles = new List<Triangle>(indices.Length / 3);
			vertices = new List<Vertex>(verts.Length);

			{
				foreach (var vec in verts)
				{
					vertices.Add(new Vertex()
					{
						p = vec,
						q = new SymetricMatrix(),
					});
				}

				for (int i = 0; i < indices.Length; i += 3)
				{
					triangles.Add(new Triangle()
					{
						v = new Int3(indices[i]-1, indices[i+1]-1, indices[i+2]-1),
					});
				}
			}
			return Simplify();
		}

		private List<Vector3> Simplify()
		{

			// main iteration loop
			int deleted_triangles=0;
			List<bool> deleted0 = new List<bool>();
			List<bool> deleted1 = new List<bool>();
			int triangle_start_count = triangles.Count;

			int target_count = (int)(ratio * (float)triangle_start_count);

			//int iteration = 0;
			//loop(iteration,0,100)
			int iteration;
			for (iteration = 0; iteration < 9999; iteration++)
			{
				if (ratio < 1.0f && triangle_start_count-deleted_triangles<=target_count)
					break;

				if (ratio >= 1.0f || iteration % 5 == 0)
					update_mesh(iteration);
				// clear dirty flag
				for (int i = 0; i < triangles.Count; i++)
					triangles[i].dirty=false;
				//
				// All triangles with edges below the threshold will be removed
				//
				// The following numbers works well for most models.
				// If it does not, try to adjust the 3 parameters
				//
				//double threshold = 0.001; //1.0E-3 EPS;
				double threshold = 1.0E-3;//1.0E-9;
				if (ratio < 1.0f)
					threshold = 0.000000001 * Math.Pow((double)(iteration+3),agressiveness);
				//if (verbose) {
				//	printf("lossless iteration %d\n", iteration);
				//}

				// remove vertices & mark deleted triangles
				for (int i = 0; i < triangles.Count; i++)
				{
					Triangle t = triangles[i];
					if (t.err[3] > threshold)
					{
						t = t;
						continue;
					}

					if(t.deleted)
						continue;
					if(t.dirty)
						continue;

					for (int j = 0; j < 3; j++)
					{
						if (t.err[j] > threshold)
							continue;

						int i0=t.v[ j     ];
						Vertex v0 = vertices[i0];
						int i1=t.v[(j+1)%3];
						Vertex v1 = vertices[i1];

						// Border check
						if(v0.border != v1.border)
							continue;

						// Compute vertex to collapse to
						Vector3 p = new Vector3();
						calculate_error(i0,i1,ref p);

						deleted0.Resize(v0.tcount); // normals temporarily
						deleted1.Resize(v1.tcount); // normals temporarily

						// don't remove if flipped
						if( flipped(ref p,i0,i1,ref v0,deleted0) )
							continue;
						if( flipped(ref p,i1,i0,ref v1,deleted1) )
							continue;

						if ( (t.attr & TriangleAttributes.TEXCOORD) == TriangleAttributes.TEXCOORD )
						{
							update_uvs(i0,ref v0,ref p,deleted0);
							update_uvs(i0,ref v1,ref p,deleted1);
						}

						// not flipped, so remove edge
						v0.p=p;
						v0.q=v1.q+v0.q;
						int tstart=refs.Count;

						update_triangles(i0,ref v0,deleted0,ref deleted_triangles);
						update_triangles(i0,ref v1,deleted1,ref deleted_triangles);

						int tcount=refs.Count-tstart;

						if(tcount<=v0.tcount)
						{
							// save ram
							if (tcount != 0)
							{
								for (int m = 0; m < tcount; m++)
									refs[v0.tstart] = refs[tstart];
							}
						}
						else
							// append
							v0.tstart=tstart;

						v0.tcount=tcount;
						break;
					}
					if (ratio < 1.0f && triangle_start_count-deleted_triangles<=target_count)
						break;
				}

				if (ratio >= 1.0f)
				{
					if (deleted_triangles <= 0)
						break;
					deleted_triangles = 0;
				}
			} //for each iteration
			// clean up mesh
			compact_mesh();

			if (triangles.Count == 0)
				return null;

			List<Vector3> finalVerts = new List<Vector3>();
			foreach (var t in triangles)
			{
				finalVerts.Add(vertices[t.v[0]].p);
				finalVerts.Add(vertices[t.v[1]].p);
				finalVerts.Add(vertices[t.v[2]].p);
			}

			return finalVerts;
		}

		// Check if a triangle flips when this edge is removed

		private bool flipped(ref Vector3 p,int i0,int i1,ref Vertex v0, List<bool> deleted)
		{
			for (int k = 0; k < v0.tcount; k++)
			{
				Triangle t = triangles[refs[v0.tstart+k].tid];
				if(t.deleted)
					continue;

				int s=refs[v0.tstart+k].tvertex;
				int id1=t.v[(s+1)%3];
				int id2=t.v[(s+2)%3];

				if(id1==i1 || id2==i1) // delete ?
				{
					deleted[k]=true;
					continue;
				}
				Vector3 d1 = vertices[id1].p-p; d1.Normalize();
				Vector3 d2 = vertices[id2].p-p; d2.Normalize();
				if(Mathf.Abs(Vector3.Dot(d1, d2))>0.999) return true;
				Vector3 n = Vector3.Cross(d1, d2);
				n.Normalize();
				deleted[k]=false;
				if(Vector3.Dot(n, t.n)<0.2) return true;
			}
			return false;
		}

	    // update_uvs

	    private void update_uvs(int i0, ref Vertex v, ref Vector3 p, List<bool> deleted)
		{
			for (int k = 0; k < v.tcount; k++)
			{
				Ref r=refs[v.tstart+k];
				Triangle t=triangles[r.tid];
				if(t.deleted)continue;
				if(deleted[k])continue;
				Vector3 p1=vertices[t.v[0]].p;
				Vector3 p2=vertices[t.v[1]].p;
				Vector3 p3=vertices[t.v[2]].p;
				//t.uvs[r.tvertex] = interpolate(p,p1,p2,p3,t.uvs);
			}
		}

		// Update triangle connections and edge error after a edge is collapsed

		private void update_triangles(int i0,ref Vertex v,List<bool> deleted,ref int deleted_triangles)
		{
			Vector3 p = new Vector3();
			for (int k = 0; k < v.tcount; k++)
			{
				Ref r=refs[v.tstart+k];
				Triangle t=triangles[r.tid];
				if(t.deleted)continue;
				if(deleted[k])
				{
					t.deleted=true;
					deleted_triangles++;
					continue;
				}
				t.v[r.tvertex]=i0;
				t.dirty=true;
				t.err[0]=calculate_error(t.v[0],t.v[1],ref p);
				t.err[1]=calculate_error(t.v[1],t.v[2],ref p);
				t.err[2]=calculate_error(t.v[2],t.v[0],ref p);
				t.err[3]=Math.Min(t.err[0],Math.Min(t.err[1],t.err[2]));
				triangles[r.tid] = t;
				refs.Add(r);
			}
		}

		// compact triangles, compute edge error and build reference list

		private void update_mesh(int iteration)
		{
			if(iteration>0) // compact triangles
			{
				int dst=0;
				for (int i = 0; i < triangles.Count; i++)
					if(!triangles[i].deleted)
					{
						triangles[dst++]=triangles[i];
					}
				triangles.Resize(dst);
			}
			//
			// Init Quadrics by Plane & Edge Errors
			//
			// required at the beginning ( iteration == 0 )
			// recomputing during the simplification is not required,
			// but mostly improves the result for closed meshes
			//
			if( iteration == 0 )
			{
				//for (int i = 0; i < vertices.Count; i++)
				//	vertices[i].q=new SymetricMatrix();//vertices[i].q=Matrix(0.0);

				for (int i = 0; i < triangles.Count; i++)
				{
					Triangle t=triangles[i];

					Vector3[] p = new Vector3[3];
					for (int j = 0; j<3; j++)
						p[j]=vertices[t.v[j]].p;

					Vector3 n = Vector3.Cross(p[1]-p[0],p[2]-p[0]);
					n.Normalize();
					t.n=n;
					for (int j = 0; j<3; j++)
						vertices[t.v[j]].q = vertices[t.v[j]].q + new SymetricMatrix(n.X,n.Y,n.Z,Vector3.Dot(-n, p[0]));
				}
				for (int i = 0; i < triangles.Count; i++)
				{
					// Calc Edge Error
					Triangle t=triangles[i];
					Vector3 p = new Vector3();
					for (int j = 0; j<3; j++) t.err[j]=calculate_error(t.v[j],t.v[(j+1)%3], ref p);
					t.err[3]=Math.Min(t.err[0],Math.Min(t.err[1],t.err[2]));
				}
			}

			// Init Reference ID list
			for (int i = 0; i < vertices.Count; i++)
			{
				vertices[i].tstart=0;
				vertices[i].tcount=0;
			}
			for (int i = 0; i < triangles.Count; i++)
			{
				Triangle t=triangles[i];
				for (int j = 0; j<3; j++)
					vertices[t.v[j]].tcount++;
			}
			int tstart=0;
			for (int i = 0; i < vertices.Count; i++)
			{
				Vertex v=vertices[i];
				v.tstart=tstart;
				tstart+=v.tcount;
				v.tcount=0;
			}

			// Write References
			refs.Resize(triangles.Count*3);
			for (int i = 0; i < triangles.Count; i++)
			{
				Triangle t=triangles[i];
				for (int j = 0; j<3; j++)
				{
					Vertex v=vertices[t.v[j]];
					refs[v.tstart+v.tcount].tid=i;
					refs[v.tstart+v.tcount].tvertex=j;
					v.tcount++;
				}
			}

			// Identify boundary : vertices[].border=0,1
			if( iteration == 0 )
			{
				List<int> vcount = new List<int>();
				List<int> vids = new List<int>();

				for (int i = 0; i < vertices.Count; i++)
					vertices[i].border=false;

				for (int i = 0; i < vertices.Count; i++)
				{
					Vertex v=vertices[i];
					vcount.Clear();
					vids.Clear();
					for (int j = 0; j < v.tcount; j++)
					{
						int kk=refs[v.tstart+j].tid;
						Triangle t=triangles[kk];
						for (int k = 0; k<3; k++)
						{
							int ofs=0,id=t.v[k];
							while(ofs<vcount.Count)
							{
								if(vids[ofs]==id)break;
								ofs++;
							}
							if(ofs==vcount.Count)
							{
								vcount.Add(1);
								vids.Add(id);
							}
							else
								++vcount[ofs];
						}
					}
					for (int j = 0; j< vcount.Count; j++) if(vcount[j]==1)
						vertices[vids[j]].border=true;
				}
			}
		}

		// Finally compact mesh before exiting

		private void compact_mesh()
		{
			int dst=0;
			for (int i = 0; i < vertices.Count; i++)
			{
				vertices[i].tcount=0;
			}
			for (int i = 0; i < triangles.Count; i++)
			if(!triangles[i].deleted)
			{
				Triangle t=triangles[i];
				triangles[dst++]=t;
				for (int j = 0; j<3; j++)vertices[t.v[j]].tcount=1;
			}
			triangles.Resize(dst);
			dst=0;
			for (int i = 0; i < vertices.Count; i++)
			if(vertices[i].tcount != 0)
			{
				vertices[i].tstart=dst;
				vertices[dst].p=vertices[i].p;
				dst++;
			}
			for (int i = 0; i < triangles.Count; i++)
			{
				Triangle t=triangles[i];
				for (int j = 0; j<3; j++)t.v[j]=vertices[t.v[j]].tstart;
			}
			vertices.Resize(dst);
		}

		// Error between vertex and Quadric

		private double vertex_error(ref SymetricMatrix q, double x, double y, double z)
		{
 			return   q[0]*x*x + 2*q[1]*x*y + 2*q[2]*x*z + 2*q[3]*x + q[4]*y*y
 			     + 2*q[5]*y*z + 2*q[6]*y + q[7]*z*z + 2*q[8]*z + q[9];
		}

		// Error for one edge

		private float calculate_error(int id_v1, int id_v2, ref Vector3 p_result)
		{
			// compute interpolated vertex

			SymetricMatrix q = vertices[id_v1].q + vertices[id_v2].q;
			bool   border = vertices[id_v1].border && vertices[id_v2].border;
			double error=0;
			double det = q.det(0, 1, 2, 1, 4, 5, 2, 5, 7);
			if ( det != 0 && !border )
			{

				// q_delta is invertible
				p_result.X = (float)(-1/det*(q.det(1, 2, 3, 4, 5, 6, 5, 7, 8)));	// vx = A41/det(q_delta)
				p_result.Y = (float)( 1/det*(q.det(0, 2, 3, 1, 5, 6, 2, 7, 8)));	// vy = A42/det(q_delta)
				p_result.Z = (float)(-1/det*(q.det(0, 1, 3, 1, 4, 6, 2, 5, 8)));	// vz = A43/det(q_delta)

				error = vertex_error(ref q, p_result.X, p_result.Y, p_result.Z);
			}
			else
			{
				// det = 0 -> try to find best result
				Vector3 p1=vertices[id_v1].p;
				Vector3 p2=vertices[id_v2].p;
				Vector3 p3=(p1+p2)*0.5f;
				double error1 = vertex_error(ref q, p1.X,p1.Y,p1.Z);
				double error2 = vertex_error(ref q, p2.X,p2.Y,p2.Z);
				double error3 = vertex_error(ref q, p3.X,p3.Y,p3.Z);
				error = Math.Min(error1, Math.Min(error2, error3));
				if (error1 == error) p_result=p1;
				if (error2 == error) p_result=p2;
				if (error3 == error) p_result=p3;
			}
			return (float)error;
		}
	}
}