using System;
using System.Collections.Generic;
using System.Text;
using System.Runtime.InteropServices;
using System.ComponentModel;
using System.Drawing;
using CsGL.OpenGL;
using MyGeometry;
using System.IO;


namespace BilateralNormalFiltering
{
	[TypeConverterAttribute(typeof(DeformerConverter))]
	public unsafe class BilateralNormalFilter : IDisposable
	{
		#region IMPORT FUNCTIONS
		[DllImport("taucs.dll")]
		static extern unsafe void* CreaterCholeskySolver(int n, int nnz, int* rowIndex, int* colIndex, double* value);
		[DllImport("taucs.dll")]
		static extern unsafe int Solve(void* solver, double* x, double* b);
		[DllImport("taucs.dll")]
		static extern unsafe double SolveEx(void* solver, double* x, int xIndex, double* b, int bIndex);
		[DllImport("taucs.dll")]
		static extern unsafe int FreeSolver(void* sp);

		[DllImport("taucs.dll")]
		static extern unsafe void* CreaterSymbolicSolver(int n, int nnz, int* rowIndex, int* colIndex, double* value);
		[DllImport("taucs.dll")]
		static extern unsafe void FreeSymbolicSolver(void* sp);
		[DllImport("taucs.dll")]
		static extern unsafe int NumericFactor(void* sp, int n, int nnz, int* rowIndex, int* colIndex, double* value);
		[DllImport("taucs.dll")]
		static extern unsafe void FreeNumericFactor(void* sp);
		[DllImport("taucs.dll")]
		static extern unsafe int NumericSolve(void* sp, double* x, double* b);
		#endregion

		private Mesh mesh = null;
		private double[] vcolor = null;
		private double[] tcolor = null;
        private double[] originalPos = null;
		private double[] vposWeight = null;
		private double[] fposWeight = null;
		private double[] flapWeight = null;
		private SparseMatrix L = null;

		public SmoothOption opt = null;
		public class SmoothOption
		{
			public enum SmoothMethod
			{
				GlobalBilateralNormalFilter,		// Global Bilateral Normal Filtering
				LocalBilateralNormalFilter,			// Local Bilateral Normal Filtering
			}
			public enum OneRingType
			{
				ONERING = 0,	// share common edges, only 3 neighbors per tri.
				TWORING,
			}
			private SmoothMethod smoothStyle = SmoothMethod.LocalBilateralNormalFilter;
			private OneRingType oneRingtype = OneRingType.TWORING;
			public bool useGeometricWeights = true;
			public bool useSpatialWeights = true;
			public bool useSimilarityWeights = true;
			public bool nbilateralTest = true;
			public bool useAdaptiveSpatialDeviation = false;
			public double vrtposWeight = 5.0;
			public double lapPosWeight = 1.0;
			public double nomPosWeight = 1.0;

			// TVCG'07 TVCG
			private int nIterator = 5;	 // # of normal update.
			private int vIterator = 20;	 // # of vertex update.


			[Category("0.General Options")]
			public SmoothMethod Filter
			{
				get { return smoothStyle; }
				set { smoothStyle = value; }
			}
			[Category("0.General Options")] 
			public OneRingType NeighborHood
			{
				get { return oneRingtype; }
				set { oneRingtype = value; }
			}
			[Category("1.Local Normal Bilateral")]
			public int NormalIteration
			{
				get { return nIterator; }
				set { nIterator = value; }
			}


			[Category("3.Vertex Iteration")]
			public int VertexIteration
			{
				get { return vIterator; }
				set { vIterator = value; }
			}

			// Normal field Bilateral Filter
			private double gc = 1; // first term gaussian width.
			private double gs = 0.35; // second term gaussian width.
			private double smoothNess = 0.1;
			
			[Category("2.Global Normal Bilateral")]
			public double GC
			{
				get { return gc; }
				set
				{
					gc = value;
				}
			}
			[Category("2.Global Normal Bilateral")]
			public double GS
			{
				get { return gs; }
				set
				{
					gs = value;
				}
			}
			[Category("2.Global Normal Bilateral")]
			public double SmoothNess
			{
				get { return smoothNess; }
				set { smoothNess = value; }
			}

		}


		private SparseMatrix CotLaplacianMatrix()
		{
			int vn = this.mesh.VertexCount;
			int fn = this.mesh.FaceCount;
			SparseMatrix A = new SparseMatrix(vn, vn);
			for (int i = 0, j = 0; i < fn; i++, j += 3)
			{
				int c1 = this.mesh.FaceIndex[j], c2 = this.mesh.FaceIndex[j + 1], c3 = this.mesh.FaceIndex[j + 2];
				Vector3d v1 = new Vector3d(this.mesh.VertexPos, c1 * 3);
				Vector3d v2 = new Vector3d(this.mesh.VertexPos, c2 * 3);
				Vector3d v3 = new Vector3d(this.mesh.VertexPos, c3 * 3);
				double cot1 = (v2 - v1).Dot(v3 - v1) / (v2 - v1).Cross(v3 - v1).Length();
				double cot2 = (v3 - v2).Dot(v1 - v2) / (v3 - v2).Cross(v1 - v2).Length();
				double cot3 = (v1 - v3).Dot(v2 - v3) / (v1 - v3).Cross(v2 - v3).Length();
				A.AddValueTo(c2, c2, -cot1); A.AddValueTo(c2, c3, cot1);
				A.AddValueTo(c3, c3, -cot1); A.AddValueTo(c3, c2, cot1);
				A.AddValueTo(c3, c3, -cot2); A.AddValueTo(c3, c1, cot2);
				A.AddValueTo(c1, c1, -cot2); A.AddValueTo(c1, c3, cot2);
				A.AddValueTo(c1, c1, -cot3); A.AddValueTo(c1, c2, cot3);
				A.AddValueTo(c2, c2, -cot3); A.AddValueTo(c2, c1, cot3);
			}
			//  A.SortElement();
			//	A.UnifyRows();
			return A;
		}
		
		
		private bool postSmoothing = false;
		private double[] triArea = null;
		private double[] perFaceSpatialDist = null;
		private double avgtArea = 0;
		private double[] prevvNormals = null;
		private double[] prevfNormals = null;
		private double[] origvnormals = null;
		private double[] origfnormals = null;

		private double avgeLen = 0;
		private List<Set<int>> neighbors = null;
		
		private Set<int> GetNeighbors(int i)
		{
			Set<int> fset = new Set<int>();
			switch (opt.NeighborHood)
			{
				case SmoothOption.OneRingType.ONERING:
					foreach (int f in mesh.AdjFF[i])
						fset.Add(f);
					fset.Add(i);
					break;
				case SmoothOption.OneRingType.TWORING:
					for (int j = 0; j < 3; ++j)
					{
						int b = i*3;
						int c = mesh.FaceIndex[b+j];
						foreach (int f in mesh.AdjVF[c])
							fset.Add(f);
					}
					break;
			}
			return fset;
		}
		private void GetNeighbors()
		{
			if (this.neighbors == null)
				this.neighbors = new List<Set<int>>();
			else
				this.neighbors.Clear();
			for (int i = 0; i < mesh.FaceCount; ++i)
				neighbors.Add(GetNeighbors(i));
		}
		private double ObtainAvgELength()
		{
			double totl = 0; int count = 0;
			for (int i = 0; i < mesh.FaceCount; ++i)
			{
				Vector3d pi = new Vector3d(mesh.DualVertexPos, i*3);
				foreach (int j in mesh.AdjFF[i])
				{
					Vector3d pj = new Vector3d(mesh.DualVertexPos, j*3);
					totl += (pj - pi).Length();
					count++;
				}
			}
			return totl / count;
		}
		private void InitialAreasAndWeights()
		{
			int vn = mesh.VertexCount, fn = mesh.FaceCount;
			triArea = new double[fn];
			// compute face areas
			double sum = 0;
			for (int i = 0; i < fn; ++i)
			{
				triArea[i] = mesh.ComputeFaceArea(i);
				sum += triArea[i];
				if (double.IsNaN(triArea[i]))
					throw new ArgumentException("irregular triangle area!");
			}
			this.avgtArea = sum / fn;

			// compute per-face average distance.
			perFaceSpatialDist = new double[fn];
			for (int i = 0; i < fn; ++i)
			{
				Vector3d u = new Vector3d(mesh.DualVertexPos, i * 3);
				perFaceSpatialDist[i] = 0;
				foreach (int adj in mesh.AdjFF[i])
				{
					Vector3d v = new Vector3d(mesh.DualVertexPos, adj * 3);
					perFaceSpatialDist[i] += (u - v).Length();
				}
				perFaceSpatialDist[i] /= mesh.AdjFF[i].Length;
			}

			this.fposWeight = FacePosWeights();
			
		}
		private void CheckModel()
		{
			for (int i = 0; i < mesh.FaceCount; ++i)
			{
				if (mesh.AdjFF[i].Length == 0)
				{
					throw new ArgumentException("isolated face: " + i.ToString());
				}
				if (triArea[i] == 0)
				{
					throw new ArgumentException("0 area face: " + i.ToString());
				}
			}
			for (int i = 0; i < mesh.VertexCount; ++i)
			{
				if (mesh.AdjVV[i].Length == 0)
				{
					throw new ArgumentException("isolated vertex: " + i.ToString());
				}
			}
		}
		private void RecalcVNormals()
		{
			mesh.ComputeNormals();
		}
        private double[] FacePosWeights()
        {
			int fn = mesh.FaceCount; 
			double[] w = new double[fn];
            for (int i = 0; i < fn; ++i) // face
            {
				w[i] = (triArea[i] / avgtArea);
            }
            return w;
        }
		private double FaceWeight(int f, int nbr)
		{
			int i = f * 3, j = nbr * 3;
			Vector3d f1 = new Vector3d(mesh.FaceNormal, i);
			Vector3d f2 = new Vector3d(mesh.FaceNormal, j);
			Vector3d v1 = new Vector3d(mesh.DualVertexPos, i);
			Vector3d v2 = new Vector3d(mesh.DualVertexPos, j);

			double w = 1;
			double ss = triArea[nbr];
			double eij = (v1 - v2).Length();

			// find commone edge
			if (!opt.nbilateralTest)
			{
				int vt1 = -1, vt2 = -1;
				for (int k = 0; k < 3; ++k)
				{
					int index = mesh.FaceIndex[i + k];
					if (index != mesh.FaceIndex[j] &&
						index != mesh.FaceIndex[j + 1] &&
						index != mesh.FaceIndex[j + 2])
					{
						vt1 = mesh.FaceIndex[i + (k + 1) % 3];
						vt2 = mesh.FaceIndex[i + (k + 2) % 3];
						break;
					}
				}
				if (vt1 != -1 && vt2 != -1) // avoid the same face
				{
					Vector3d u1 = new Vector3d(mesh.VertexPos, vt1 * 3);
					Vector3d u2 = new Vector3d(mesh.VertexPos, vt2 * 3);
					double e12 = (u1 - u2).Length();

					w = opt.useGeometricWeights ? ss * e12 : 1.0;
				}
			}
			else
			{
				w = opt.useGeometricWeights ? ss : 1.0;// / eij;
			}


			double x = (f1 - f2).Length(); // 1 - tcos;
			double l = opt.useSpatialWeights ? eij : 0;

			double spatial = opt.useAdaptiveSpatialDeviation ?
				perFaceSpatialDist[f] : avgeLen;

			double d2 = opt.GS * opt.GS, d1 = opt.GC * opt.GC * spatial * spatial;


			double e = w * Math.Exp(-l * l / d1 / 2);// *Math.Exp(-x * x / d2 / 2); /--> modified here

			if (opt.useSimilarityWeights)
			{
				e = e * Math.Exp(-x * x / d2 / 2);
			}
			
			return e;
		}


		#region INTERFACE
		public void Process()
		{
			this.InitialAreasAndWeights();
			this.CheckModel(); // check if the model is valid for smoothing.
			this.RecalcVNormals();


			this.avgeLen = ObtainAvgELength();

			
			// timing.
			Win32.HiPerfTimer timer = new Win32.HiPerfTimer();
			timer.Start();

			switch (opt.Filter)
			{
				case SmoothOption.SmoothMethod.GlobalBilateralNormalFilter:
					GlobalBilateralNormalSmoothing();
					break;
				
				case SmoothOption.SmoothMethod.LocalBilateralNormalFilter:
					LocalBilateralNormalSmoothing();
					break;
				
			}

			timer.Stop();
			Program.PrintText("CPU time: " + timer.Duration.ToString() + "\n");

			this.RecalcVNormals();

		}
		public void Display()
		{
			
		}
		#endregion
		
		
		public BilateralNormalFilter(Mesh m, SmoothOption opt)
		{
			this.mesh = m;
			this.opt = opt;
			this.vcolor = new double[3 * m.VertexCount];
			this.tcolor = new double[3 * m.FaceCount];

			// clone for restoration.
            this.originalPos = this.mesh.VertexPos.Clone() as double[];
			this.origvnormals = mesh.VertexNormal.Clone() as double[];
			this.origfnormals = mesh.FaceNormal.Clone() as double[];
			this.prevvNormals = mesh.VertexNormal.Clone() as double[];
			this.prevfNormals = mesh.FaceNormal.Clone() as double[];
			this.vposWeight = new double[m.VertexCount];
			this.fposWeight = new double[m.FaceCount];
			this.flapWeight = new double[m.FaceCount];

			if (null != opt)
			{
				for (int i = 0; i < m.VertexCount; ++i)
				{
					vposWeight[i] = opt.vrtposWeight;
				}
				for (int i = 0; i < m.FaceCount; ++i)
				{
					fposWeight[i] = opt.nomPosWeight;
					flapWeight[i] = opt.lapPosWeight;
				}
			}

			this.GetNeighbors();
			
		}
		~BilateralNormalFilter()
		{
			Dispose();
		}


		#region Our Global Iterative Normal Filtering
		private void GlobalBilateralNormalSmoothing()
		{
			int vn = mesh.VertexCount, fn = mesh.FaceCount;
			
			// update the normal field, one-pass
			TBasedNormalSmoothing();
			FEFPVertexUpdate(opt.VertexIteration);

		}
		// Build weighted face-based laplacian matrix
		private SparseMatrix TBasedWNormalLapMatrix()
		{
			int fn = this.mesh.FaceCount;
			SparseMatrix A = new SparseMatrix(fn, fn);
			double r = 1-opt.SmoothNess;
			Set<int> fSet = new Set<int>();
			for (int i = 0; i < fn; ++i)
			{
				int b = i*3;
				switch (this.opt.NeighborHood)
				{
					case SmoothOption.OneRingType.ONERING:
						{
							foreach (int f in mesh.AdjFF[i])
								fSet.Add(f);
						}
						break;
					case SmoothOption.OneRingType.TWORING:
						{
							for (int j = 0; j < 3; ++j)
							{
								int c = mesh.FaceIndex[b + j];
								foreach (int f in mesh.AdjVF[c])
									fSet.Add(f);
							}
							fSet.Remove(i);
						}
						break;
				}

				double rr = fposWeight[i]*r;

				double sum = 0; int count = 0;
				double[] w = new double[fSet.Count];
				foreach (int f in fSet)
				{
					w[count] = FaceWeight(i, f);
					sum += w[count++];
				}
				A.SetValueTo(i, i, -1 * flapWeight[i] * rr);
				count = 0;
				if (sum != 0)
					foreach (int f in fSet)
					{
						double e = w[count++] * flapWeight[i] / sum;
						A.AddValueTo(i, f, e * rr);
					}
				fSet.Clear();
			}
			return A;
		}
		// Triangle based laplacian normal field smoothing,
		// first order --> neighbor faces that share common edgs(basic 3). 
		// second order --> all neighbor faces that share common vertex
		private void TBasedNormalSmoothing()
		{
			this.L = this.TBasedWNormalLapMatrix();
			TNSmooth();
		}
		private void TNSmooth()
		{
			int n = mesh.FaceCount;
			double[] newpos = new double[3 * n];
			double[] normal = mesh.FaceNormal;
			double r = opt.SmoothNess;

			List<int> psPoints = new List<int>();
			for (int i = 0; i < n; ++i)
				psPoints.Add(i);
			if (!postSmoothing)
				foreach (int v in psPoints)
				{
					L.AddRow();
					L.AddElement(L.RowSize - 1, v, fposWeight[v] * r);
				}
			else
			{
				int m = n;
				foreach (int v in psPoints)
				{
					L.SetValueTo(m++, v, fposWeight[v] * r);
				}
			}

			CCSMatrix ccsA = new CCSMatrix(L);
			CCSMatrix ccsATA = ccsA.MultiplyATA();
			void* solver = Factorization(ccsATA);

			double[] x = new double[n];
			double[] b = new double[n * 2];
			double[] ATb = new double[n];
			for (int i = 0; i < 3; i++)
			{
				int m = n;
				foreach (int index in psPoints)
					b[m++] = normal[index * 3 + i] * fposWeight[index] * r;
				ccsA.PreMultiply(b, ATb);
				fixed (double* _x = x, _ATb = ATb)
					Solve(solver, _x, _ATb);
				for (int j = 0, k = 0; j < n; j++, k += 3)
					newpos[k + i] = x[j];
			}
			if (solver != null) FreeSolver(solver);

			// normalize the obtained normals
			for (int i = 0, j = 0; i < n; i++, j += 3)
			{
				double e = newpos[j], f = newpos[j + 1], g = newpos[j + 2];
				double l = Math.Sqrt(e * e + f * f + g * g);
				newpos[j] /= l;
				newpos[j+1] /= l;
				newpos[j+2] /= l;
			}
			this.mesh.FaceNormal = newpos;
		}

		
		#endregion


		#region Our Local Iterative Normal Filtering
		private void LocalBilateralNormalSmoothing()
		{
			for (int i = 0; i < opt.NormalIteration; ++i)
			{
				StepNormalFiltering();
			}
			FEFPVertexUpdate(opt.VertexIteration);
		}
		private void StepNormalFiltering()
		{
			int fn = this.mesh.FaceCount;
			double r = 1 - opt.SmoothNess;
			Set<int> fSet = new Set<int>();
			Vector3d[] updatedNormal = new Vector3d[fn];
			for (int i = 0; i < fn; ++i)
			{
				int b = i * 3;
				switch (this.opt.NeighborHood)
				{
					case SmoothOption.OneRingType.ONERING:
						{
							foreach (int f in mesh.AdjFF[i])
								fSet.Add(f);
						}
						break;
					case SmoothOption.OneRingType.TWORING:
						{
							for (int j = 0; j < 3; ++j)
							{
								int c = mesh.FaceIndex[b + j];
								foreach (int f in mesh.AdjVF[c])
									fSet.Add(f);
							}
							fSet.Remove(i);
						}
						break;
				}
				//fSet.Add(i);

				double sum = 0; int count = 0;
				double[] w = new double[fSet.Count];
				foreach (int f in fSet)
				{
					w[count] = FaceWeight(i, f);
					sum += w[count++];
				}
				count = 0;
				if (sum != 0)
				{
					updatedNormal[i] = new Vector3d();
					foreach (int f in fSet)
					{
						Vector3d nf = new Vector3d(mesh.FaceNormal, f*3);
						double e = w[count++] * flapWeight[i] / sum;
						updatedNormal[i] = updatedNormal[i] + e * nf;
					}
					/*
					Vector3d ni = new Vector3d(mesh.FaceNormal, i*3);
					double cos = ni.Dot(updatedNormal[i]) / updatedNormal[i].Length();
					r = Math.Acos(cos) / Math.PI;
					updatedNormal[i] = updatedNormal[i] * r + ni * (1 - r) * fposWeight[i];
					 * */
				}
				else
				{
					updatedNormal[i] = new Vector3d(mesh.FaceNormal, 3 * i);
				}
				fSet.Clear();
			}
			for (int i = 0; i < fn; ++i)
			{
				updatedNormal[i] = updatedNormal[i].Normalize();
				mesh.FaceNormal[3 * i] = updatedNormal[i].x;
				mesh.FaceNormal[3 * i + 1] = updatedNormal[i].y;
				mesh.FaceNormal[3 * i + 2] = updatedNormal[i].z;
			}
		}
		#endregion


		#region TVCG'07 Fast and Efficient Feature-Preserving Mesh Denoising, vertex update
		// Update the vertex positions according to the new normals.
		private void FEFPVertexUpdate(int maxIterator)
		{
			int n = mesh.VertexCount;
			double[] newpos = new double[n * 3];
			for (int itr = 0; itr < maxIterator; ++itr)
			{
				mesh.ComputeDualPosition();
				for (int i = 0; i < n; ++i)
				{
					Vector3d X = new Vector3d(mesh.VertexPos, 3 * i);
					int VF = mesh.AdjVF[i].Length;
					foreach (int f in mesh.AdjVF[i])
					{
						Vector3d N = new Vector3d(mesh.FaceNormal, 3 * f);
						Vector3d C = new Vector3d(mesh.DualVertexPos, 3 * f);
						double offset = N.Dot(C - X) / VF;
						X = X + N * offset;
					}
					// update real position
					newpos[3 * i] = X.x;
					newpos[3 * i + 1] = X.y;
					newpos[3 * i + 2] = X.z;
				}
				mesh.VertexPos = newpos.Clone() as double[];
			}
		}
		#endregion

		
		#region help functions
		private void* Factorization(CCSMatrix C)
		{
			fixed (int* ri = C.RowIndex, ci = C.ColIndex)
			fixed (double* val = C.Values)
				return CreaterCholeskySolver(C.ColumnSize, C.NumNonZero, ri, ci, val);
		}
		#endregion


		#region IDisposable Members
		public void Dispose()
		{

		}
		#endregion
	}
}
