using System;
using System.Collections.Generic;
using System.Drawing;

namespace MyGeometry
{
    public class Vertex3
    {
        public Vector3d vertexCoord;

        public Vertex3(Vector3d coord)
        {
            vertexCoord = coord;
        }
    }

    public class Face
    {
        public List<Vertex3> vertices = new List<Vertex3>();
        public List<Vertex3> currVertices = new List<Vertex3>();
        public Vector3d normal;
        public Vector3d currNormal;
        public int index;
        public int verticesNum;
        public Vector3d center;
        public Vector3d currCenter;
		public Vector3d[] currCoord = new Vector3d[3];
		public Matrix3d H = new Matrix3d();
        public MatrixNd mat;
		public double depth = 0.0d;
		public List<Vertex3> screenCoord = new List<Vertex3>();
		public Vector3d centroid = new Vector3d(0.0d, 0.0d, 0.0d);

		public Face(List<Vertex3> pnts, List<Vertex3> currPnts, int indFace)
        {
            for (int i = 0; i < pnts.Count; ++i)
            {
                this.vertices.Add(pnts[i]);
				currVertices.Add(currPnts[i]);
            }
            this.index = indFace;
            //center = the center of the line connecting frist and middle point
            int mid = (int)(pnts.Count / 2);
            this.center = (pnts[0].vertexCoord + pnts[mid].vertexCoord) / 2;
			this.currCenter = (currPnts[0].vertexCoord + currPnts[mid].vertexCoord) / 2;
			this.verticesNum = this.vertices.Count;

            this.ComputeFaceNormal();

            this.getCurrCoord();

			for (int i = 0; i < verticesNum; i++)
			{
				this.screenCoord.Add(new Vertex3(pnts[i].vertexCoord));
			}

			double[,] array = new double[4, verticesNum];
			for (int i = 0; i < verticesNum; i++)
			{
				array[0, i] = vertices[i].vertexCoord.x;
				array[1, i] = vertices[i].vertexCoord.y;
				array[2, i] = vertices[i].vertexCoord.z;
				array[3, i] = 1.0d;
			}
			mat = new MatrixNd(array, 4, verticesNum);
        }

		public Face(List<Vertex3> pnts, List<Vertex3> currPnts, int indFace, Vector3d shapeCentroid)
		{
			for (int i = 0; i < pnts.Count; ++i)
			{
				this.vertices.Add(pnts[i]);
				currVertices.Add(currPnts[i]);
			}
			this.index = indFace;
			//center = the center of the line connecting frist and middle point
			int mid = (int)(pnts.Count / 2);
			this.center = (pnts[0].vertexCoord + pnts[mid].vertexCoord) / 2;
			this.currCenter = (currPnts[0].vertexCoord + currPnts[mid].vertexCoord) / 2;
			this.verticesNum = this.vertices.Count;

			this.centroid = shapeCentroid;
			this.ComputeFaceNormal();

			this.getCurrCoord();

			for (int i = 0; i < verticesNum; i++)
			{
				this.screenCoord.Add(new Vertex3(pnts[i].vertexCoord));
			}

			double[,] array = new double[4, verticesNum];
			for (int i = 0; i < verticesNum; i++)
			{
				array[0, i] = vertices[i].vertexCoord.x;
				array[1, i] = vertices[i].vertexCoord.y;
				array[2, i] = vertices[i].vertexCoord.z;
				array[3, i] = 1.0d;
			}
			mat = new MatrixNd(array, 4, verticesNum);
		}
		
        public void ComputeFaceNormal()
        {
            this.verticesNum = this.vertices.Count;
			Vector3d v1 = this.vertices[1].vertexCoord - this.vertices[0].vertexCoord;
			Vector3d v2 = this.vertices[this.verticesNum - 1].vertexCoord - this.vertices[0].vertexCoord;
            Vector3d fNormal = v1.Cross(v2).Normalize();
            this.normal = new Vector3d(fNormal.x, fNormal.y, fNormal.z);

			v1 = this.currVertices[1].vertexCoord - this.currVertices[0].vertexCoord;
			v2 = this.currVertices[verticesNum - 1].vertexCoord - this.currVertices[0].vertexCoord;
			fNormal = v1.Cross(v2).Normalize();
            this.currNormal = new Vector3d(fNormal.x, fNormal.y, fNormal.z);

			//double thresh = 1e-5;
			//if (Math.Abs(centroid.x) < thresh && Math.Abs(centroid.y) < thresh && Math.Abs(centroid.z) < thresh)
			//    return;
			//Vector3d pointToCentroid = this.centroid - this.center;
			//double angle = Math.Acos(pointToCentroid.Dot(this.normal));
			//// Make sure all normals point to outside (of the mesh)
			//if (index == 1 && angle > thresh)
			//{
			//    this.normal = -1.0 * this.normal;
			//    this.currNormal = -1.0 * this.currNormal;
			//}

        }

        public void updateMatrix()
        {
			for (int i = 0; i < verticesNum; i++)
			{
				mat[0, i] = vertices[i].vertexCoord.x;
				mat[1, i] = vertices[i].vertexCoord.y;
				mat[2, i] = vertices[i].vertexCoord.z;
				mat[3, i] = 1.0d;
			}
        }

		// Used when pull a plane polygon
        public void updateVertices(List<Vertex3> pnts, List<Vertex3> currPnts)
        {
            for (int i = 0; i < pnts.Count; i++)
            {
                this.vertices[i].vertexCoord.x = pnts[i].vertexCoord.x;
				this.vertices[i].vertexCoord.y = pnts[i].vertexCoord.y;
				this.vertices[i].vertexCoord.z = pnts[i].vertexCoord.z;

				this.currVertices[i].vertexCoord.x = currPnts[i].vertexCoord.x;
				this.currVertices[i].vertexCoord.y = currPnts[i].vertexCoord.y;
				this.currVertices[i].vertexCoord.z = currPnts[i].vertexCoord.z; 
            }
			int mid = (int)(pnts.Count / 2);
			this.center = (pnts[0].vertexCoord + pnts[mid].vertexCoord) / 2;

			this.currCenter = (currPnts[0].vertexCoord + currPnts[mid].vertexCoord) / 2;
			this.ComputeFaceNormal();
			this.verticesNum = this.vertices.Count;

			this.updateMatrix();
			this.getCurrCoord();
        }

		public void updateVertices2()
		{
			//this.centroid = shapeCentroid;
			int mid = (int)(verticesNum / 2);
			this.center = (vertices[0].vertexCoord + vertices[mid].vertexCoord) / 2;

			this.currCenter = (currVertices[0].vertexCoord + currVertices[mid].vertexCoord) / 2;
			this.ComputeFaceNormal();

			this.updateMatrix();
			this.getCurrCoord();
		}

		// After transformation of the coordinate, re-calculate the current 3D vertices and related info
        public void updateLocalInfo(MatrixNd newMat)
        {
            for (int j = 0; j < verticesNum; j++)
            {
                currVertices[j].vertexCoord.x = newMat[0, j] / newMat[3, j];
                currVertices[j].vertexCoord.y = newMat[1, j] / newMat[3, j];
                currVertices[j].vertexCoord.z = newMat[2, j] / newMat[3, j];
            }
            Vector3d v1 = this.currVertices[1].vertexCoord - this.currVertices[0].vertexCoord;
            Vector3d v2 = this.currVertices[currVertices.Count - 1].vertexCoord - this.currVertices[0].vertexCoord;
            this.currNormal = v1.Cross(v2).Normalize();
            int mid = (int)(currVertices.Count / 2);
            this.currCenter = (currVertices[0].vertexCoord + currVertices[mid].vertexCoord) / 2;
            this.getCurrCoord();
        }

        public void getCurrCoord()
        {
			// X-Y-Z axis
            //currCoord[0] = (this.currVertices[1].vertexCoord - this.currVertices[0].vertexCoord).Normalize();
			//currCoord[0] = (this.currVertices[verticesNum - 1].vertexCoord - this.currVertices[0].vertexCoord).Normalize();
			currCoord[0] = (this.currCenter - this.currVertices[0].vertexCoord).Normalize();
            currCoord[2] = this.currNormal;
            currCoord[1] = currCoord[0].Cross(currCoord[2]).Normalize();
        }

		public void setScreen(Vector3d[] screenPos)
		{
			double sum = 0.0;
			for (int i = 0; i < verticesNum; i++)
			{
				screenCoord[i].vertexCoord.x = screenPos[i].x;
				screenCoord[i].vertexCoord.y = screenPos[i].y;
				screenCoord[i].vertexCoord.z = screenPos[i].z;
				sum += screenPos[i].z;
				//this.depth = Math.Max(this.depth, screenPos[i].z);
			}
			this.depth = sum / verticesNum;
		}

	}

    public class Shapes
    {
        public List<Face> faces = new List<Face>();
        public List<Vertex3> vertices = new List<Vertex3>();
		public List<Vertex3> currVertices = new List<Vertex3>();
        public int faceNum;
        public int vertexNum;
        public int index;
		public Vector3d centroid = new Vector3d(0.0d, 0.0d, 0.0d);
		// Embedded shape
		public List<int> components = new List<int>();

        public Shapes(List<Vertex3> pnts, List<Vertex3> currPnts, int indBuild)
        {
			for (int i = 0; i < pnts.Count; i++)
			{
				this.vertices.Add(pnts[i]);
				this.currVertices.Add(currPnts[i]);
			}
			this.vertexNum = pnts.Count;
            this.faces.Add(new Face(this.vertices, this.currVertices, 0));
            this.faceNum = 1;           
            this.index = indBuild;
        }

        public void ReShapes(double liftDist)
        {
            int num = this.vertexNum;
            this.vertexNum *= 2;
            List<Vertex3> addVertices = new List<Vertex3>();
			List<Vertex3> addLocalV = new List<Vertex3>();

            for (int i = 0; i < num; i++)
            {
                Vector3d coord = this.vertices[i].vertexCoord + liftDist * this.faces[0].normal;
                Vertex3 addVertex = new Vertex3(coord);
                this.vertices.Add(addVertex);
                addVertices.Add(addVertex);

				Vector3d localC = this.currVertices[i].vertexCoord + liftDist * this.faces[0].currNormal;
				Vertex3 addLocalC = new Vertex3(localC);
				this.currVertices.Add(addLocalC);
				addLocalV.Add(addLocalC);
            }
			this.computeCentroid();
            this.faces.Add(new Face(addVertices, addLocalV, 1));

            // Add side faces
            for (int i = 0; i < num; i++)
            {
                int[] currIndex = { i, (i + 1) % num, num + (i + 1 + num) % num, num + (i + num) % num };
                List<Vertex3> currPoints = new List<Vertex3>();
                for (int j = 0; j < 4; j++)
                    currPoints.Add(this.vertices[currIndex[j]]);
				List<Vertex3> currLocalPoints = new List<Vertex3>();
                for (int j = 0; j < 4; j++)
                    currLocalPoints.Add(this.currVertices[currIndex[j]]);

                this.faces.Add(new Face(currPoints, currLocalPoints, i + 2));
            }
            this.faceNum = num + 2;

			
        }

		public void moveShapes(double liftDist, int indexFace, Vector3d direction, Vector3d localDirection)
        {
			double thresh = 1e-5;
			if (Math.Abs(direction.x) < thresh && Math.Abs(direction.y) < thresh && Math.Abs(direction.z) < thresh)
			{
				direction = this.faces[indexFace].normal;
				localDirection = this.faces[indexFace].currNormal;
			}
			 for (int i = 0; i < this.faces[indexFace].verticesNum; i++)
			 {
				 this.faces[indexFace].vertices[i].vertexCoord += liftDist * direction;
				 this.faces[indexFace].currVertices[i].vertexCoord += liftDist * localDirection;
			 }
			 //this.computeCentroid();
			 this.faces[indexFace].updateVertices2();
			 
        }

		public void computeCentroid()
		{
			for (int i = 0; i < this.vertexNum; i++)
				this.centroid += this.vertices[i].vertexCoord;
			this.centroid /= this.vertexNum;
		}
    }
}
