using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;
using Tao.OpenGl;
using Tao.Platform.Windows;
using MyGeometry;
using System.IO;
using System.Drawing.Imaging;
using Emgu.CV;
using Emgu.Util;
using AutoDiff;
using ClipperLib;

//using dotnet;
using System.Runtime.InteropServices;


namespace SmartLayout
{
	public unsafe class MeshView : SimpleOpenGlControl
	{

		[DllImport("opengl32", EntryPoint = "wglUseFontBitmaps", CallingConvention=CallingConvention.Winapi)]
		public static extern bool wglUseFontBitmaps(
		IntPtr hDC,
		[MarshalAs(UnmanagedType.U4)] UInt32 first,
		[MarshalAs(UnmanagedType.U4)] UInt32 count,
		[MarshalAs(UnmanagedType.U4)] UInt32 listBase
		);

		[DllImport("GDI32.DLL", EntryPoint = "SelectObject",
		CallingConvention = CallingConvention.Winapi)]
		public static extern IntPtr SelectObject(
		[In] IntPtr hDC,
		[In] IntPtr font
		);
		

		public interface IMeshDisplay
		{
			void Display();
			void SetData();
		}
		public enum EnumOperationMode { Viewing, Selection, Picking, Sketching, MoveWall, ChooseWall, Switch};
		public delegate void OnPickingHandler(object sender, int index);

		public enum DisplayMode {TwoD, ThreeD};


		// private fields
		private MeshRecord meshRecord = null;
		private EnumOperationMode currentMode = EnumOperationMode.Viewing;
		private DisplayMode currentDisplay = DisplayMode.TwoD;

		// font
		private bool initFont = false;
		public UInt32 fontBase = 0;
		private void BuildFont(PaintEventArgs pe)
		{
			IntPtr dc = pe.Graphics.GetHdc();
			IntPtr oldFontH = IntPtr.Zero;
			System.Drawing.Font font =
				new Font(
				"Arial",
				8F,
				System.Drawing.FontStyle.Bold,
				System.Drawing.GraphicsUnit.Point,
				((System.Byte)(0)));

			fontBase = (uint)Gl.glGenLists(128);

			IntPtr fontH = font.ToHfont();
			oldFontH = SelectObject(dc, fontH);

			bool ret = wglUseFontBitmaps(
				dc,
				0,
				128,
				fontBase);

			SelectObject(dc, oldFontH);						// Selects The Font We Want

			pe.Graphics.ReleaseHdc(dc);

			if (!ret) throw new Exception();
		}

		// display properties
		private bool showVertices = false;
		private bool showHiddenLine = false;
		private bool smoothShading = true;
		private bool antialiasing = true;
		private float pointSize = 4.0f;
		private float lineWidth = 1.0f;
		private Color pointColor = Color.Red;
		private Color lineColor = Color.Black;

		/////////////////LH//////////////////////

		public int hlShape = -1, hlFace = -1;
		Vector3d clickPoint = new Vector3d();
		Vector3d releasePoint = new Vector3d();
		public int currShape = -1, currFace = -1;
		private bool showTexture = false;

        /////////////////LH//////////////////////

		public int selectedFace = -1;
		//private Color meshColor = Color.CornflowerBlue;

		// for viewing
		private Trackball ball = new Trackball(100, 100);
		private double scaleRatio;
		private Vector2d currMousePosition = new Vector2d();
		private Vector2d prevMousePosition = new Vector2d();
		private Vector2d mouseDownPosition = new Vector2d();
		private bool isMouseDown = false;
		private double depthTolerance = -0.0001;
		public Matrix4d currTransformation = Matrix4d.IdentityMatrix();
		

		// for sketching
		private List<Vector2d> screenStrokePoints = null;
		private List<List<Vector2d>> allScreenStrokePoints = new List<List<Vector2d>>();
		private List<List<int>> allStrokeFaces3d = new List<List<int>>();
		private List<List<int>> allStrokePoints3d = new List<List<int>>();
		private List<int> currStrokeFaces = null;
		private List<int> currStrokePoints = null;


		private List<Vector2d> screenCurvePoints = null;

		// public properties
		public List<MeshRecord> meshes = new List<MeshRecord>();
		public MeshRecord MeshRecord
		{
			get { return meshRecord; }
			set 
			{
				if (this.meshRecord != null)
					this.meshRecord.CurrentTransformation = this.currTransformation;
				this.meshRecord = value;
                if (this.meshRecord != null) 
                    this.currTransformation = this.meshRecord.CurrentTransformation;
				this.Refresh(); 
			}
		}
		public EnumOperationMode CurrentMode
		{
			get { return currentMode; }
			set { currentMode = value; }
		}
		public DisplayMode CurrentDisplay
		{
			get { return currentDisplay; }
			set { currentDisplay = value; }
		}
		public bool ShowVertices
		{
			get { return showVertices; }
			set { showVertices = value; this.Refresh(); }
		}

		public bool ShowTexture
		{
			get { return showTexture; }
			set { showTexture = value; this.Refresh(); }
		}

		public bool ShowHiddenLine 
		{
			get { return showHiddenLine; }
			set { showHiddenLine = value; this.Refresh(); }
		}
		public bool SmoothShading
		{
			get { return smoothShading; }
			set { smoothShading = value; this.Refresh(); }
		}
		public bool Antialiasing
		{
			get { return antialiasing; }
			set 
			{ 
				antialiasing = value;
				if (antialiasing)
				{
					Gl.glEnable(Gl.GL_POINT_SMOOTH);
					Gl.glEnable(Gl.GL_LINE_SMOOTH);
					Gl.glEnable(Gl.GL_BLEND);
				}
				else
				{
					Gl.glDisable(Gl.GL_POINT_SMOOTH);
					Gl.glDisable(Gl.GL_LINE_SMOOTH);
					Gl.glDisable(Gl.GL_BLEND);
				}
				this.Refresh(); 
			}
		}

		public MeshView()
		{
			InitializeComponent();

			this.AutoMakeCurrent = true;
			this.InitializeContexts();
			this.InitOpenGL();

			this.InitBorder();

		}
		private void InitializeComponent()
		{
			this.SuspendLayout();
			// 
			// MeshView
			// 
		//	this.Name = "MeshView";
            
			this.ResumeLayout(false);
		}

		protected override void OnSizeChanged(EventArgs e)
		{
			base.OnSizeChanged(e);

			Size s = this.Size;
			if (s.Width == 0 || s.Height == 0) return;

			this.scaleRatio = (s.Width > s.Height) ? s.Height : s.Width;
		//	this.InitMatrix();
			this.ball.SetBounds(s.Width * 1.0, s.Height * 1.0);
			this.Refresh();
		}
		protected override void OnMouseDown(System.Windows.Forms.MouseEventArgs e)
		{
			base.OnMouseDown(e);

			Program.SetTabObject(this.MeshRecord);

			if (this.meshRecord != null)
			{
				Program.SetPropertyGridObject(this.meshRecord);
			}

			this.currMousePosition = new Vector2d(e.X, this.Height - e.Y);
			this.mouseDownPosition = currMousePosition;
			this.isMouseDown = true;

			switch (this.currentMode)
			{
                case EnumOperationMode.Viewing:
                    {
						if (currentDisplay == DisplayMode.ThreeD)
						{
							switch (e.Button)
							{
								case MouseButtons.Left: ball.Click(currMousePosition, Trackball.MotionType.Rotation); break;
								case MouseButtons.Middle: ball.Click(currMousePosition / this.scaleRatio, Trackball.MotionType.Pan); break;
								case MouseButtons.Right: ball.Click(currMousePosition, Trackball.MotionType.Scale); break;
							}
						}
						else
						{
							if (e.Button == MouseButtons.Middle)
								ball.Click(currMousePosition / this.scaleRatio, Trackball.MotionType.Pan);
							else
								ball.Click(currMousePosition, Trackball.MotionType.Scale);
						}
                    }
                    break;
				case EnumOperationMode.Sketching:
					if (e.Button == MouseButtons.Left)
					{
						this.currStrokeFaces = new List<int>();
						this.currStrokePoints = new List<int>();
						this.screenStrokePoints = new List<Vector2d>();
						this.screenStrokePoints.Add(currMousePosition);

						prevMousePosition.x = currMousePosition.x;
						prevMousePosition.y = currMousePosition.y;
					}
					break;
				case EnumOperationMode.Picking:
					break;
                case EnumOperationMode.Selection:
                    break;
			}
		}
		protected override void OnMouseMove(System.Windows.Forms.MouseEventArgs e)
		{
			base.OnMouseMove(e);

			this.currMousePosition = new Vector2d(e.X, this.Height - e.Y);

			switch (this.currentMode)
			{
                case EnumOperationMode.Viewing:
                    {
                        if (isMouseDown)
                        {
							if (currentDisplay == DisplayMode.ThreeD)
							{
								switch (e.Button)
								{
									case MouseButtons.Left: ball.Drag(currMousePosition); break;
									case MouseButtons.Middle: ball.Drag(currMousePosition / this.scaleRatio); break;
									case MouseButtons.Right: ball.Drag(currMousePosition); break;
								}
								this.Refresh();
							}
							else
							{
								if (e.Button == MouseButtons.Middle)
								{
									ball.Drag(currMousePosition / this.scaleRatio);
									this.Refresh();
								}
								else
								{ 
									ball.Drag(currMousePosition);
									this.Refresh();
								}
							}
                        }
                    }
                    break;

				case EnumOperationMode.MoveWall: 
					{
                        if (currentDisplay == DisplayMode.TwoD)
                        {
							if (isMouseDown)
								MoveCuboid();
							else
								moveCuboid = SelectCuboid();
                        }
						this.Refresh();
						break;
					}

				case EnumOperationMode.ChooseWall:
					{
						hlCuboid = SelectCuboid();
						this.Refresh();
						hlCuboid[0] = -1;
                        hlCuboid[1] = -1;
						break;
					}
			}
		}

		protected override void OnMouseUp(System.Windows.Forms.MouseEventArgs e)
		{
			base.OnMouseUp(e);

			this.currMousePosition = new Vector2d(e.X, this.Height - e.Y);
			this.isMouseDown = false;

			switch (this.currentMode)
			{
                case EnumOperationMode.Viewing:
                    {
                        if (currMousePosition == mouseDownPosition) break;
						Matrix4d m = ball.GetMatrix();
						this.currTransformation = m * this.currTransformation;
						//Matrix3d cvmat;
						//Vector3d[] screenCoord;
						//currMat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
						//MatrixNd newMat = currMat * XYPlane.vertexMat;
						//this.GetHomoMatrix(this.XYPlane, out cvmat, out screenCoord);
						//XYPlane.UpdateCurrVertices(newMat);
						//XYPlane.SetScreenCoordinate(screenCoord);
						//// In 2D, select walls[activeFloor] (use of currVertices for selection; in 3D, don't
						//if (currentDisplay == DisplayMode.TwoD)
						//{
						//    if (this.cuboids != null)
						//    {
						//        for (int i = 0; i < this.cuboids.Count; i++)
						//        {
						//            for (int j = 0; j < cuboids[i].Count; j++)
						//            {
						//                int nump = this.cuboids[i][j].planes.Count;
						//                if (this.currentDisplay == DisplayMode.TwoD) nump = 1;
						//                for (int k = 0; k < nump; k++)
						//                {
						//                    MatrixNd planeMat = currMat * cuboids[i][j].planes[k].vertexMat;
						//                    cuboids[i][j].planes[k].UpdateCurrVertices(planeMat);
						//                    this.GetHomoMatrix(cuboids[i][j].planes[k], out cvmat, out screenCoord);
						//                    cuboids[i][j].planes[k].SetScreenCoordinate(screenCoord);
						//                }
						//            }
						//        }
						//    }
						//}
						//else
						//{
						//    if (walls != null)
						//    {
						//        for (int i = 0; i < walls.Count; i++)
						//            for (int j = 0; j < walls[i].Count; j++)
						//                for (int k = 0; k < walls[i][j].casts.Count; k++)
						//                {
						//                    MatrixNd planeMat = currMat * walls[i][j].casts[k].vertexMat;
						//                    walls[i][j].casts[k].UpdateCurrVertices(planeMat);
						//                }
						//    }
						//}
						this.ball.End();
						this.Refresh();
                    }
                    break;

				case EnumOperationMode.MoveWall:
					{
						// Optimization
						int[] tmw = new int[2], tmc = new int[2];
						tmw[0] = moveWall[0];
						tmw[1] = moveWall[1];
						tmc[0] = moveCuboid[0];
						tmc[1] = moveCuboid[1];
						bool opt = OnMoveWall(activeFloor);
						ReBuildWalls(activeFloor);
						//if (!opt)
						//{
						//    moveWall[0] = tmw[0];
						//    moveWall[1] = tmw[1];
						//    moveCuboid[0] = tmc[0];
						//    moveCuboid[1] = tmc[1];
						//    ReBuildWalls(activeFloor);
						//}
						ReBuildWalls(activeFloor);
						this.Refresh();
                        moveWall[0] = -1;
                        moveWall[1] = -1;
                        moveCuboid[0] = -1;
                        moveCuboid[1] = -1;
                        times = 0;
						break;
					}

				case EnumOperationMode.ChooseWall:
					{
						if (currentDisplay == DisplayMode.TwoD)
						{
							currCuboid = SelectCuboid();
							if (currCuboid[1] != -1)
							{
								double[] values = new double[4];
								values[0] = cuboids[currCuboid[0]][currCuboid[1]].height;
								values[1] = cuboids[currCuboid[0]][currCuboid[1]].width;
								values[2] = cuboids[currCuboid[0]][currCuboid[1]].thickness;
								values[3] = cuboids[currCuboid[0]][currCuboid[1]].elevation;
								Program.SetTextbox(values);
							}
						}
						this.Refresh();
						break;
					}
				case EnumOperationMode.Switch:
					{
						int[] inds = SelectRoom();
						if (toswitch[0, 0] == -1)
						{
							toswitch[0, 0] = inds[0];
							toswitch[0, 1] = inds[1];
						}
						else
						{
							toswitch[1, 0] = inds[0];
							toswitch[1, 1] = inds[1];
						}
						this.Refresh();
						if (toswitch[0, 0] != -1 && toswitch[1, 0] != -1)
						{
							if (toswitch[0, 0] != toswitch[1, 0])
							{
								MessageBox.Show("Wrong choice: The two rooms are not on the same floor!");
							}
							else
							{
								SwitchRooms();
							}
						}
						this.Refresh();
						break;
					}
				case EnumOperationMode.Selection:
					{
						SelectCasts();
						this.Refresh();
						break;
					}
			}
		}

		private void InitLighting()
		{
			Gl.glEnable(Gl.GL_LIGHTING);
			Gl.glEnable(Gl.GL_NORMALIZE);

			float diffuse = 1.0f;
			float specular = 0.1f;
			int shinness = 128;

			float[] LightDiffuse = { diffuse, diffuse, diffuse, 1.0f };
			float[] LightSpecular = { specular, specular, specular, 1f };
			float[] LightPosition = { (float)5, (float)5, (float)5, 1f };
			Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_DIFFUSE, LightDiffuse);
			Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_SPECULAR, LightSpecular);
			Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_POSITION, LightPosition);
			Gl.glEnable(Gl.GL_LIGHT0);


			Gl.glEnable(Gl.GL_COLOR_MATERIAL);
			Gl.glColorMaterial(Gl.GL_FRONT, Gl.GL_AMBIENT_AND_DIFFUSE);
			//Gl.glColorMaterial(Gl.GL_FRONT, Gl.GL_SPECULAR);
			//Gl.glColorMaterial(Gl.GL_FRONT, Gl.GL_DIFFUSE);
			float[] mdiffuse = { 0.9f, 0.9f, 0.9f, 1f };
			float[] mspecular = { 0.9f, 0.9f, 0.99f, 1f };
			float[] mambient = { 0.9f, 0.9f, 0.99f, 1f };
			Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_SPECULAR, mspecular);
			Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_DIFFUSE, mdiffuse);
			Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, mambient);
			Gl.glMaterialf(Gl.GL_FRONT, Gl.GL_SHININESS, shinness);

		}
		protected override void OnPaint(PaintEventArgs e)
		{

			if (!initFont)
			{
				BuildFont(e);
				initFont = true;
			}
			Color c = SystemColors.Control;
            //Color c = meshRecord.BackgroundColor;

			if (this.DesignMode == true)
			{
				base.OnPaint(e); 
				return;
			}

			this.MakeCurrent();

			//if (this.meshRecord == null || this.meshRecord.Mesh == null)
			//{
			//    Gl.glClearColor(c.R / 255f, c.G / 255f, c.B / 255f, 0.0f);
			//    Gl.glClear(Gl.GL_COLOR_BUFFER_BIT | Gl.GL_DEPTH_BUFFER_BIT);

			//    this.SwapBuffers();
			//    //Gl.glEnable(Gl.GL_BLEND); 
			//    return;
			//}
			
			this.InitMatrix();

			Matrix4d m = ball.GetMatrix() * currTransformation;
			Gl.glClearColor(c.R / 255f, c.G / 255f, c.B / 255f, 0.0f);
            Gl.glClearColor(1.0f, 1.0f, 1.0f, 0.0f);
			Gl.glClear(Gl.GL_COLOR_BUFFER_BIT | Gl.GL_DEPTH_BUFFER_BIT);
			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			if (!this.useGLOrthoProjection)
			{
				Gl.glPushMatrix();
				Gl.glMultMatrixd(m.Transpose().ToArray());
			}
			else
			{
				Gl.glLoadMatrixd(m.Transpose().ToArray());
			}

			Gl.glEnable(Gl.GL_POLYGON_OFFSET_FILL);
			if (this.meshRecord != null && this.meshRecord.Mesh != null)
			{
				if (this.smoothShading)
				{
					DrawSmoothShaded();
				}
				else
				{
					//DrawTansparent();
					DrawFlatShaded();
				}

				if (this.showHiddenLine)
				{
					DrawWireframe();
				}

				if (this.showVertices)
				{
					DrawPoints();
				}

				foreach (IMeshDisplay md in meshRecord.AlgorithmObjects)
				{
					md.Display();
				}

				//
				if (this.currentMode == EnumOperationMode.Selection && isMouseDown)
				{
					SelectVertexByRect();
					DrawSelectedVertice_ByPoint();
				}
			}

			/////////////////////////////////LH/////////////////////////////////////////
			if (currentDisplay == DisplayMode.ThreeD)
			{

				if (this.walls != null && this.walls.Count > 0)
					for (int i = 0; i < numFloor; i++)
                    {
                        drawFloor = i;
						for (int j = 0; j < doors[drawFloor].Count; j++)
							DrawWall(3, i, j, -1, 0);
                        for (int j = 0; j < walls[i].Count; j++)
						    if (walls[i][j].cuboid != null && !walls[i][j].open)
							    DrawWall(1, i, walls[i][j].cuboid.wallIndex, -1, 0);
                        
                    }
				if (pickCasts != null && pickCasts.Count > 0)
				{
 					for (int i = 0; i < pickCasts.Count; i++)
						DrawWall(4, pickCasts[i][0], pickCasts[i][1], pickCasts[i][2], 0);
				}
				if (this.showTexture)
					DrawTexture();
				if (drawLabel)
					DrawSlabLabel();
			}
			else // 2D
			{
				if (recRooms != null && recRooms.Count > 0)
					DrawInit();

                if (this.walls != null && this.walls.Count > 0)
                    for (int i = 0; i < walls.Count; i++)
                    {
                        drawFloor = i;
                        for (int j = 0; j < walls[i].Count; j++)
                        {
                            if (walls[i][j].cuboid != null)// && !walls[i][j].open)
                                DrawWall(walls[i][j].cuboid.wallIndex, 0);
                        }
                        for (int j = 0; j < doors[drawFloor].Count; j++)
                            DrawCuboid(2, false, doors[drawFloor][j].cuboid);
                    }

				if (this.moveWall[0] != -1)
				{
					if (walls[moveWall[0]][moveWall[1]].cuboid != null)
                        DrawCuboid(1, true, walls[moveWall[0]][moveWall[1]].cuboid);
				}

                if (this.moveCuboid[1] != -1)
                    DrawCuboid(cuboids[moveCuboid[0]][moveCuboid[1]].label, true, cuboids[moveCuboid[0]][moveCuboid[1]]);
				if (drawLabel)
					DrawSlabLabel();
			}

			/////////////////////////////////LH/////////////////////////////////////////

			DrawStrokes();
			
			if (!this.useGLOrthoProjection)
				Gl.glPopMatrix();
			this.SwapBuffers();
		}

		private bool useGLOrthoProjection = false;
		private OpenGLProjector glProjector = null;
		public void ObtainGLProjector()
		{
			if (!this.useGLOrthoProjection)
			{
				Gl.glPushMatrix();
				Matrix4d mat = ball.GetMatrix() * currTransformation;
				Gl.glMultMatrixd(mat.Transpose().ToArray());
				this.glProjector = new OpenGLProjector();
				Gl.glPopMatrix();
			}
			else
			{
				this.glProjector = new OpenGLProjector();
			}
		}

		public void GetMeshPointsFromStroke()
		{
			if (this.screenStrokePoints.Count < 1) return;

			this.ObtainGLProjector();
			OpenGLProjector projector = this.glProjector;
			
			Rectangle viewport = new Rectangle(0, 0, this.Width, this.Height);
			Mesh m = this.MeshRecord.Mesh;

			double eps = -0.0001;
			foreach (Vector2d pos in this.screenStrokePoints)
			{
				int minIndex = -1;
				for (int i = 0, j = 0; i < m.FaceCount; i++, j += 3)
				{
					int c0 = m.FaceIndex[j];
					int c1 = m.FaceIndex[j + 1];
					int c2 = m.FaceIndex[j + 2];
					Vector3d v0 = projector.Project(m.VertexPos, c0*3);
					Vector3d v1 = projector.Project(m.VertexPos, c1*3);
					Vector3d v2 = projector.Project(m.VertexPos, c2*3);
					Vector2d u0 = new Vector2d(v0.x, v0.y);
					Vector2d u1 = new Vector2d(v1.x, v1.y);
					Vector2d u2 = new Vector2d(v2.x, v2.y);
					
					if (!viewport.Contains((int)v0.x, (int)v0.y)) continue;
					if (projector.GetDepthValue((int)v0.x, (int)v0.y) - v0.z < eps) continue;
					
					if (PointInTriangle(pos, u0, u1, u2))
					{
						minIndex = i;

						if (!this.currStrokeFaces.Contains(minIndex))
						{
							this.currStrokeFaces.Add(minIndex);

							double d0 = (pos - u0).Length();
							double d1 = (pos - u1).Length();
							double d2 = (pos - u2).Length();

							int maxi0 = d0 < d1 ? c0 : c1;
							int maxi1 = Math.Min(d0, d1) < d2 ? maxi0 : c2;

							if (!this.currStrokePoints.Contains(maxi1))
								this.currStrokePoints.Add(maxi1);
						}

						break;
					}
				}
			}

		}
		private bool PointInTriangle(Vector2d p, Vector2d a, Vector2d b, Vector2d c)
		{
			if (SameSide(p,a,b,c) && SameSide(p,b,a,c) && SameSide(p,c, a,b))
				return true;
			return false;
		}
		private bool SameSide(Vector2d p1, Vector2d p2, Vector2d a, Vector2d b)
		{
			Vector3d A = new Vector3d(a);
			Vector3d B = new Vector3d(b);
			Vector3d P = new Vector3d(p1);
			Vector3d Q = new Vector3d(p2);
			Vector3d E = B-A;
			Vector3d cp1 = E.Cross(P-A);
			Vector3d cp2 = E.Cross(Q-A);
			if (cp1.Dot(cp2) >= 0) 
				return true;
			return false;
		}

		private List<int> front1, front2;
		public void GetFrontsFromStroke()
		{
			if (this.screenStrokePoints.Count < 1) return;

			this.ObtainGLProjector();
			OpenGLProjector projector = this.glProjector;

			Rectangle viewport = new Rectangle(0, 0, this.Width, this.Height);
			Mesh m = this.MeshRecord.Mesh;

			double offset = 20;
			List<Vector2d> offset1 = new List<Vector2d>();
			List<Vector2d> offset2 = new List<Vector2d>();
			int n = this.screenStrokePoints.Count;
			for (int i = 0; i < n; ++i)
			{
				Vector2d normal = new Vector2d();
				Vector2d p = this.screenStrokePoints[i];
				if (i == 0)
				{
					Vector2d u = this.screenStrokePoints[i + 1] - p;
					normal = (new Vector2d(u.y, -u.x)).Normalize();
				}
				else if (i == n - 1)
				{
					Vector2d u = p - this.screenStrokePoints[i - 1];
					normal = (new Vector2d(u.y, -u.x)).Normalize();
				}
				else
				{
					Vector2d u = (this.screenStrokePoints[i + 1] - p).Normalize();
					Vector2d v = (p - this.screenStrokePoints[i - 1]).Normalize();
					Vector2d w = u + v;
					normal = (new Vector2d(w.y, -w.x)).Normalize();
				}

				Vector2d offv1 = p + offset * normal;
				Vector2d offv2 = p - offset * normal;

				offset1.Add(offv1);
				offset2.Add(offv2);
			}

			this.front1 = new List<int>();
			foreach (Vector2d pos in offset1)
			{
				int v = this.PickSelectedVertexByPoint(pos);
				if (v != -1)
				{
					this.front1.Add(v);
				}
			}

			this.front2 = new List<int>();
			foreach (Vector2d pos in offset2)
			{
				int v = this.PickSelectedVertexByPoint(pos);
				if (v != -1)
				{
					this.front2.Add(v);
				}
			}
		}

		private void InitOpenGL()
		{
		//	this.MakeCurrent();
			
			Color c = SystemColors.Control;
			Gl.glClearColor(c.R / 255f, c.G / 255f, c.B / 255f, 0.0f);     // BackGround Color         
			Gl.glShadeModel(Gl.GL_FLAT);         // Set Smooth Shading                 
			Gl.glClearDepth(1.0f);                 // Depth buffer setup             
			Gl.glEnable(Gl.GL_DEPTH_TEST);         // Enables Depth Testing             
			Gl.glDepthFunc(Gl.GL_LEQUAL);             // The Type Of Depth Test To Do     
			Gl.glHint(Gl.GL_PERSPECTIVE_CORRECTION_HINT, Gl.GL_NICEST);     /* Really Nice Perspective Calculations */
			Gl.glEnable(Gl.GL_CULL_FACE);
			Gl.glPolygonOffset(1f, 1f);
			Gl.glBlendFunc(Gl.GL_SRC_ALPHA, Gl.GL_ONE_MINUS_SRC_ALPHA);

			Gl.glEnable(Gl.GL_POINT_SMOOTH);
			Gl.glEnable(Gl.GL_LINE_SMOOTH);
			Gl.glEnable(Gl.GL_BLEND);

			float[] LightDiffuse = { 1f, 1f, 1f, 0f };
			float[] LightSpecular = { 0.7f, 0.7f, 0.7f, 1f };
			float[] SpecularRef = { 0.5f, 0.5f, 0.5f, 1f };
			Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_DIFFUSE, LightDiffuse);
			Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_SPECULAR, LightSpecular);
			Gl.glEnable(Gl.GL_LIGHT0);
			Gl.glEnable(Gl.GL_COLOR_MATERIAL);
			Gl.glColorMaterial(Gl.GL_FRONT, Gl.GL_DIFFUSE);
			Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_SPECULAR, SpecularRef);
			Gl.glMateriali(Gl.GL_FRONT, Gl.GL_SHININESS, 128);

			Gl.glClearColor(0, 0, 0, 0);
			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glLoadIdentity();
			Gl.glOrtho(0.0, 1.0, 0.0, 1.0, -1.0, 1.0);				

	//		Size s = this.Size;
	//		ball = new Trackball(s.Width * 1.0, s.Height * 1.0);
		}

		private void InitMatrix_Ortho()
		{
			Size s = this.Size;
			double w = s.Width;
			double h = s.Height;

		//	this.MakeCurrent();
			
			Gl.glViewport(0, 0, s.Width, s.Height);
			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glLoadIdentity();

			if (w > h)
			{
				double ratio = (w / h) / 2.0;
				Gl.glOrtho(-ratio, ratio, -0.5, 0.5, -100, 100);
			}
			else
			{
				double ratio = (h / w) / 2.0;
				Gl.glOrtho(-0.5, 0.5, -ratio, ratio, -100, 100);
			}

			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			Gl.glLoadIdentity();
		}

		private void InitMatrix()
		{
			if (this.useGLOrthoProjection)
			{
				InitMatrix_Ortho();
				return;
			}

			double w = Size.Width;
			double h = Size.Height;
			Gl.glViewport(0, 0, Size.Width, Size.Height);
			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glLoadIdentity();
			
			Glu.gluPerspective(60.0, w / h, 0.01, 100);

			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			Gl.glLoadIdentity();

			Glu.gluLookAt(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
		}

		private void DrawPoints()
		{
			Mesh m = this.meshRecord.Mesh;

			this.MakeCurrent();
			Gl.glPointSize(pointSize);
			Color c = pointColor;
			Gl.glColor3ub(c.R, c.G, c.B);
			Gl.glEnableClientState(Gl.GL_VERTEX_ARRAY);
			fixed (double* vp = m.VertexPos)
			{
				Gl.glVertexPointer(3, Gl.GL_DOUBLE, 0, new IntPtr(vp));
				Gl.glDrawArrays(Gl.GL_POINTS, 0, m.VertexCount);
			}
			Gl.glDisableClientState(Gl.GL_VERTEX_ARRAY);
		}

		private void DrawWireframe()
		{
			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINE);
			Gl.glDisable(Gl.GL_CULL_FACE);
			Gl.glEnable(Gl.GL_LINE_SMOOTH);
			Color c = this.lineColor;
			Gl.glColor3ub(c.R, c.G, c.B);

			Mesh m = this.meshRecord.Mesh;

			Gl.glLineWidth(this.lineWidth);
			Gl.glEnableClientState(Gl.GL_VERTEX_ARRAY);
			fixed (double* vp = m.VertexPos)
			fixed (int* index = m.FaceIndex)
			{
				Gl.glVertexPointer(3, Gl.GL_DOUBLE, 0, new IntPtr(vp));
				Gl.glDrawElements(Gl.GL_TRIANGLES, m.FaceCount * 3, Gl.GL_UNSIGNED_INT, new IntPtr(index));
			}
			Gl.glDisableClientState(Gl.GL_VERTEX_ARRAY);
			Gl.glEnable(Gl.GL_CULL_FACE);
		}

		private void DrawFlatShaded()
		{
			Gl.glShadeModel(Gl.GL_FLAT);
			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);
			Gl.glEnable(Gl.GL_LIGHTING);
			Gl.glEnable(Gl.GL_NORMALIZE);

			Mesh m = this.meshRecord.Mesh;

			//Color c = this.meshColor;

			//Color c = this.meshRecord.ModelColor;
			//Gl.glColor3ub(c.R, c.G, c.B);

			Gl.glColor3ub(159, 121, 238);
			fixed (double* vp = m.VertexPos)
			fixed (double* np = m.FaceNormal)
			fixed (int* fi = m.FaceIndex)
			{
				Gl.glBegin(Gl.GL_TRIANGLES);
				for (int i = 0, j = 0; i < m.FaceCount; i++, j += 3)
				{
					Gl.glNormal3dv(new IntPtr(np + j));
					Gl.glVertex3dv(new IntPtr(vp + fi[j] * 3));
					Gl.glVertex3dv(new IntPtr(vp + fi[j + 1] * 3));
					Gl.glVertex3dv(new IntPtr(vp + fi[j + 2] * 3));
				}
				Gl.glEnd();
			}
			Gl.glDisable(Gl.GL_LIGHTING);
		}

		private void DrawSmoothShaded()
		{
			Gl.glShadeModel(Gl.GL_FLAT);
			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);
			Gl.glEnable(Gl.GL_LIGHTING);
			Gl.glEnable(Gl.GL_NORMALIZE);

			Mesh m = this.meshRecord.Mesh;

			//Color c = this.meshColor;
            Color c = this.meshRecord.ModelColor;
			Gl.glColor3ub(c.R, c.G, c.B);
			Gl.glEnableClientState(Gl.GL_VERTEX_ARRAY);
			Gl.glEnableClientState(Gl.GL_NORMAL_ARRAY);
			fixed (double* vp = m.VertexPos)
			fixed (double* np = m.VertexNormal)
			fixed (int* index = m.FaceIndex)
			{
				Gl.glVertexPointer(3, Gl.GL_DOUBLE, 0, new IntPtr(vp));
				Gl.glNormalPointer(Gl.GL_DOUBLE, 0, new IntPtr(np));
				Gl.glDrawElements(Gl.GL_TRIANGLES, m.FaceCount * 3, Gl.GL_UNSIGNED_INT, new IntPtr(index));
			}
			Gl.glDisableClientState(Gl.GL_VERTEX_ARRAY);
			Gl.glDisableClientState(Gl.GL_NORMAL_ARRAY);
			Gl.glDisable(Gl.GL_LIGHTING);
		}

		private void DrawTansparent()
		{
			Mesh m = this.meshRecord.Mesh;
			Color c = Color.WhiteSmoke;
			byte alphaChannel = 60;

            Gl.glPushClientAttrib(Gl.GL_CLIENT_VERTEX_ARRAY_BIT);
            Gl.glPushAttrib(Gl.GL_ALL_ATTRIB_BITS);

            Gl.glShadeModel(Gl.GL_FLAT);
            Gl.glEnable(Gl.GL_NORMALIZE);
            Gl.glDisable(Gl.GL_LIGHTING);
            Gl.glEnable(Gl.GL_BLEND);
            Gl.glBlendFunc(Gl.GL_SRC_ALPHA, Gl.GL_ONE_MINUS_SRC_ALPHA);
            Gl.glEnable(Gl.GL_CULL_FACE);
            Gl.glDisable(Gl.GL_DEPTH_TEST);
            Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);

            Gl.glColor4ub(c.R, c.G, c.B, alphaChannel);

            Gl.glEnableClientState(Gl.GL_VERTEX_ARRAY);
            Gl.glEnableClientState(Gl.GL_NORMAL_ARRAY);
            fixed (double* vp = m.VertexPos)
            fixed (double* np = m.VertexNormal)
            fixed (int* index = m.FaceIndex)
            {
              Gl.glVertexPointer(3, Gl.GL_DOUBLE, 0, (IntPtr)vp);
              Gl.glNormalPointer(Gl.GL_DOUBLE, 0, (IntPtr)np);
              Gl.glDrawElements(Gl.GL_TRIANGLES, m.FaceCount * 3, Gl.GL_UNSIGNED_INT, (IntPtr)index);
            }
            Gl.glDisableClientState(Gl.GL_VERTEX_ARRAY);
            Gl.glDisableClientState(Gl.GL_NORMAL_ARRAY);

			//////////////////////////////////////////////////////////////////////////             

			Gl.glEnable(Gl.GL_LIGHTING);
			Gl.glDepthFunc(Gl.GL_EQUAL);

			float[] LightDiffuse = { 1.0f, 1.0f, 1.0f, 1.0f };
			float[] LightSpecular = { 1.0f, 1.0f, 1.0f, 1.0f };
			Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_DIFFUSE, LightDiffuse);
			Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_SPECULAR, LightSpecular);
			Gl.glEnable(Gl.GL_LIGHT0);

			float[] Ambient = { 0.0f, 0.0f, 0.0f, 1.0f };
			float[] Diffuse = { 0.0f, 0.0f, 0.0f, 1.0f };
			float[] SpecularRef = { 1.0f, 1.0f, 1.0f, 1.0f };
			
			Gl.glColorMaterial(Gl.GL_FRONT, Gl.GL_DIFFUSE);
			Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_DIFFUSE, Diffuse);
			Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, Ambient);
			Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_SPECULAR, SpecularRef);
			Gl.glMateriali(Gl.GL_FRONT, Gl.GL_SHININESS, 128);

			Gl.glColor4ub(c.R, c.G, c.B, alphaChannel);

			Gl.glEnableClientState(Gl.GL_VERTEX_ARRAY);
			Gl.glEnableClientState(Gl.GL_NORMAL_ARRAY);
			fixed (double* vp = m.VertexPos)
			fixed (double* np = m.VertexNormal)
			fixed (int* index = m.FaceIndex)
			{
				Gl.glVertexPointer(3, Gl.GL_DOUBLE, 0, (IntPtr)vp);
				Gl.glNormalPointer(Gl.GL_DOUBLE, 0, (IntPtr)np);
				Gl.glDrawElements(Gl.GL_TRIANGLES, m.FaceCount * 3, Gl.GL_UNSIGNED_INT, (IntPtr)index);
			}
			Gl.glDisableClientState(Gl.GL_VERTEX_ARRAY);
			Gl.glDisableClientState(Gl.GL_NORMAL_ARRAY);

			Gl.glPopAttrib();
			Gl.glPopClientAttrib();

		}

		private void DrawSelectedVertice_ByPoint()
		{
			Mesh m = this.meshRecord.Mesh;

			Gl.glColor3f(1.0f, 0.4f, 0.4f);
			Gl.glPointSize(this.pointSize + 4);
			fixed (double* vp = m.VertexPos)
			{
				Gl.glEnableClientState(Gl.GL_VERTEX_ARRAY);
				Gl.glVertexPointer(3, Gl.GL_DOUBLE, 0, new IntPtr(vp));
				Gl.glBegin(Gl.GL_POINTS);
				for (int i = 0; i < m.VertexCount; i++)
				{
					if (m.Flag[i] == 0) continue;


					switch (m.Flag[i] % 12)
					{
						case 0: Gl.glColor3f(1.0f, 0.0f, 1.0f); break;
						case 1: Gl.glColor3f(1.0f, 0.0f, 0.0f); break;
						case 2: Gl.glColor3f(0.0f, 1.0f, 0.0f); break;
						case 3: Gl.glColor3f(0.0f, 0.0f, 1.0f); break;
						case 4: Gl.glColor3f(1.0f, 1.0f, 0.0f); break;
						case 5: Gl.glColor3f(0.0f, 1.0f, 1.0f); break;
						case 6: Gl.glColor3f(0.5f, 0.0f, 0.0f); break;
						case 7: Gl.glColor3f(0.0f, 0.5f, 0.0f); break;
						case 8: Gl.glColor3f(0.0f, 0.0f, 0.5f); break;
						case 9: Gl.glColor3f(0.5f, 0.5f, 0.0f); break;
						case 10: Gl.glColor3f(0.0f, 0.5f, 0.5f); break;
						case 11: Gl.glColor3f(0.5f, 0.0f, 0.5f); break;
					}

					//Gl.glColor3f(0.0f, 1.0f, 0.0f);
					//Gl.glArrayElement(i);
					Gl.glVertex3dv(new IntPtr(vp + i * 3));
				}
				Gl.glEnd();
				Gl.glDisableClientState(Gl.GL_VERTEX_ARRAY);
			}
		}

		private void DrawSelectionRect()
		{
			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glPushMatrix();
			Gl.glLoadIdentity();
			Glu.gluOrtho2D(0, this.Width, 0, this.Height);
			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			Gl.glPushMatrix();
			Gl.glLoadIdentity();

			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINE);
			Gl.glDisable(Gl.GL_CULL_FACE);
			Gl.glDisable(Gl.GL_DEPTH_TEST);
			Gl.glColor3f(0.0f, 0.0f, 0.0f);
			Gl.glRectd(mouseDownPosition.x, mouseDownPosition.y, currMousePosition.x, currMousePosition.y);
			Gl.glEnable(Gl.GL_CULL_FACE);
			Gl.glEnable(Gl.GL_DEPTH_TEST);

			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glPopMatrix();
			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			Gl.glPopMatrix();
		}

		private void SelectVertexByRect()
		{
			Vector2d minV = Vector2d.Min(mouseDownPosition, currMousePosition);
			Vector2d size = Vector2d.Max(mouseDownPosition, currMousePosition) - minV;
			Rectangle rect = new Rectangle((int)minV.x, (int)minV.y, (int)size.x, (int)size.y);
			Rectangle viewport = new Rectangle(0, 0, this.Width, this.Height);
			OpenGLProjector projector = new OpenGLProjector();
			Mesh m = this.meshRecord.Mesh;
			bool laser = true;
			double eps = this.depthTolerance;

			if ((Control.ModifierKeys & Keys.Shift) == Keys.Shift)
				for (int i = 0, j = 0; i < m.VertexCount; i++, j += 3)
				{
					Vector3d v = projector.Project(m.VertexPos, j);
					if (viewport.Contains((int)v.x, (int)v.y))
					{
						bool flag = rect.Contains((int)v.x, (int)v.y);
						flag &= (laser || projector.GetDepthValue((int)v.x, (int)v.y) - v.z >= eps);
						if (flag) m.Flag[i] = (byte)1;
					}
				}

			else if ((Control.ModifierKeys & Keys.Control) == Keys.Control)
				for (int i = 0, j = 0; i < m.VertexCount; i++, j += 3)
				{
					Vector3d v = projector.Project(m.VertexPos, j);
					if (viewport.Contains((int)v.x, (int)v.y))
					{
						bool flag = rect.Contains((int)v.x, (int)v.y);
						flag &= (laser || projector.GetDepthValue((int)v.x, (int)v.y) - v.z >= eps);
						if (flag) m.Flag[i] = (byte)0;
					}
				}

			else
				for (int i = 0, j = 0; i < m.VertexCount; i++, j += 3)
				{
					Vector3d v = projector.Project(m.VertexPos, j);
					if (viewport.Contains((int)v.x, (int)v.y))
					{
						bool flag = rect.Contains((int)v.x, (int)v.y);
						flag &= (laser || projector.GetDepthValue((int)v.x, (int)v.y) - v.z >= eps);
						m.Flag[i] = (byte)((flag) ? 1 : 0);
					}
				}
		}		

		private void SelectVertexByPoint(Vector2d u)
		{
			Mesh m = this.meshRecord.Mesh;
			double eps = this.depthTolerance;

			this.ObtainGLProjector();
			OpenGLProjector projector = this.glProjector;
			
			
			Rectangle viewport = new Rectangle(0, 0, this.Width, this.Height);

			double minDis = double.MaxValue;
			int minIndex = -1;

			for (int i = 0, j = 0; i < m.VertexCount; i++, j += 3)
			{
				m.Flag[i] = 0;

				Vector3d v = projector.Project(m.VertexPos, j);
				if (viewport.Contains((int)v.x, (int)v.y) &&
					projector.GetDepthValue((int)v.x, (int)v.y) - v.z >= eps)
				{
					double dx = (u.x - v.x);
					double dy = (u.y - v.y);
					double dis = dx * dx + dy * dy;
					if (dis < minDis)
					{
						minDis = dis;
						minIndex = i;
					}
				}
			}

			if (minIndex != -1)
				m.Flag[minIndex] = 1;
		}

		private void SelectFaceByPoint()
		{
			Mesh m = this.meshRecord.Mesh;
			double eps = this.depthTolerance;

			OpenGLProjector projector = new OpenGLProjector();
			Rectangle viewport = new Rectangle(0, 0, this.Width, this.Height);

			double minDis = double.MaxValue;
			int minIndex = -1;
			Vector2d u = currMousePosition;

			for (int i = 0, j = 0; i < m.FaceCount; i++, j += 3)
			{
				Vector3d v = projector.Project(m.DualVertexPos, j);
				if (viewport.Contains((int)v.x, (int)v.y) &&
					projector.GetDepthValue((int)v.x, (int)v.y) - v.z >= eps)
				{
					double dx = (u.x - v.x);
					double dy = (u.y - v.y);
					double dis = dx * dx + dy * dy;
					if (dis < minDis)
					{
						minDis = dis;
						minIndex = i;
					}
				}
			}

			this.selectedFace = minIndex;

			Program.OutputText("Selected face: " + minIndex.ToString(), true);
		}

		private int PickSelectedVertexByPoint(Vector2d u)
		{
			Mesh m = this.meshRecord.Mesh;
			double eps = this.depthTolerance;

			this.ObtainGLProjector();
			OpenGLProjector projector = this.glProjector;

			Rectangle viewport = new Rectangle(0, 0, this.Width, this.Height);

			double minDis = double.MaxValue;
			int minIndex = -1;


			for (int i = 0, j = 0; i < m.VertexCount; i++, j += 3)
			{
				m.Flag[i] = 0; // reset flag

				Vector3d v = projector.Project(m.VertexPos, j);
				if (//m.Flag[i] != 0 &&
					viewport.Contains((int)v.x, (int)v.y) &&
					projector.GetDepthValue((int)v.x, (int)v.y) - v.z >= eps)
				{
					double dx = (u.x - v.x);
					double dy = (u.y - v.y);
					double dis = dx * dx + dy * dy;
					if (dis < minDis)
					{
						minDis = dis;
						minIndex = i;
					}
				}
			}

			if (minIndex != -1)
				m.Flag[minIndex] = 1;

			return minIndex;
		}

        ///////////////////////////////////////////////////////////////NEW_ADD
        public void SaveTransformationMatrix(StreamWriter sw)
        {
            for (int i = 0; i < 4; ++i)
                for (int j = 0; j < 4; ++j)
                    sw.WriteLine(this.currTransformation[i, j]);
        }
        public void LoadTransformationMatrix(StreamReader sw)
        {
            this.currTransformation = new Matrix4d();

            for (int k = 0; k < 16; ++k)
            {
                string s = sw.ReadLine();
                int i = k / 4;
                int j = k % 4;
                this.currTransformation[i, j] = double.Parse(s);
            }
        }

		private Color strokeColor = Color.Coral;
		public void SetMainStrokeColor(Color c)
		{
			strokeColor = c;
		}
		private void DrawStrokes()
		{
			DrawStroke(screenStrokePoints, this.strokeColor); 
			DrawStroke(this.screenCurvePoints, Color.DarkGray); 
		}
		private void DrawStroke(List<Vector2d> stroke, Color c)
		{
			if (stroke == null || stroke.Count < 1) return;

			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glPushMatrix();
			Gl.glLoadIdentity();
			Glu.gluOrtho2D(0, this.Width, 0, this.Height);
			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			Gl.glPushMatrix();
			Gl.glLoadIdentity();

			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINES);
			Gl.glDisable(Gl.GL_CULL_FACE);
			Gl.glDisable(Gl.GL_DEPTH_TEST);

			Gl.glColor3ub(c.R, c.G, c.B);
			Gl.glLineWidth(5.0f);
			Gl.glEnable(Gl.GL_LINE_SMOOTH);
			Gl.glBegin(Gl.GL_LINE_STRIP);
			foreach (Vector2d u in stroke)
			{
				Gl.glVertex2d(u.x, u.y);
			}
			Gl.glEnd();

			Gl.glPointSize(5.0f);
			Gl.glBegin(Gl.GL_POINTS);
			foreach (Vector2d u in stroke)
			{
				Gl.glVertex2d(u.x, u.y);
			}
			Gl.glEnd();

			Gl.glDisable(Gl.GL_LINE_SMOOTH);
			Gl.glEnable(Gl.GL_CULL_FACE);
			Gl.glEnable(Gl.GL_DEPTH_TEST);

			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glPopMatrix();
			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			Gl.glPopMatrix();
		}

		/////////////////Build Room---LH//////////////////////
		Plane XYPlane;
		// Each wall is a cuboid, constitute the whole room
		List<List<Plane>> sideWallPlanes; 
		public List<Vector3d> t_vertices, t_endPoints, t_currVertices; // Vertices before move
		Vector3d lastMove = new Vector3d();
		public double wallThickness, wallHeight, newWidth, wallElevation, alignment = 0;
		public int times = 0, hlWall = -1, editWall = -1, hlPlane = -1, walltype = 0;
        int[] moveCuboid = {-1, -1}, hlCuboid = {-1, -1}, currCuboid = {-1, -1}, moveWall = {-1, -1};
		double[] boundary = { double.MaxValue, double.MinValue, double.MaxValue, double.MinValue };
		public int currWall = -1, currPlane = -1;
		public int[,] toswitch;
		public bool cast3D = false, drawLabel = false;
		double castThickness;
		public void InitParams()
		{
			// Initializtion
			recRooms = new List<List<Room>>();
			walls = new List<List<Wall>>();
			cuboids = new List<List<Cuboid>>();
			doors = new List<List<Door>>();
			thresh = 1e-4;
			// Rooms
			wallThickness = 0.02;
			wallHeight = 0.36;
			wallElevation = 0;
			newWidth = 0.0;
			Area = 0;
			for (int i = 0; i < numFloor; i++)
				oriArea.Add(0);
			sideWallPlanes = new List<List<Plane>>();
			if (!slabdefined)
			{
				blocks[0] = 0.05;
				blocks[1] = 0.06;
				blocks[2] = 0.12;
				slabHeight = 0.36; // Height of the block
			}
			castThickness = 0.01;
			toswitch = new int[2, 2]{{-1,-1},{-1,-1}};
			pickCasts = new List<int[]>();
			//drawLabel = false;
			this.currTransformation = Matrix4d.IdentityMatrix();
		}

		private static bool PointInPoly(Vector3d p, List<Vertex> poly)
		{
			bool c = false;
			int num = poly.Count;
			for (int i = 0, j = num - 1; i < num; j = i++)
			{
				if (((poly[i].Coord.y > p.y) != (poly[j].Coord.y > p.y)) &&
						(p.x < (poly[j].Coord.x - poly[i].Coord.x) * (p.y - poly[i].Coord.y)
						/ (poly[j].Coord.y - poly[i].Coord.y) + poly[i].Coord.x))
					c = !c;
			}
			return c;
		}

		public Vector3d intersect(Vector3d u1, Vector3d u2, Vector3d v1, Vector3d v2)
		{
			// u1, u2: endpoints of grid
			// v1, v2: endpoints of floor
			double t;
			if (((u1.x - u2.x) * (v1.y - v2.y) - (u1.y - u2.y) * (v1.x - v2.x)) == 0)
					return (new Vector3d(0, 0, 0));
			t = ((u1.x - v1.x) * (v1.y - v2.y) - (u1.y - v1.y) * (v1.x - v2.x)) / ((u1.x - u2.x) * (v1.y - v2.y) - (u1.y - u2.y) * (v1.x - v2.x));
			Vector3d vec = u1; 
			vec.x += (u2.x - u1.x) * t;
			vec.y += (u2.y - u1.y) * t;
			if (dot_online_in(vec, u1, u2) == 1 && dot_online_in(vec, v1, v2) == 1 &&
				(vec - u1).Length() > 1e-5 && (vec - u2).Length() > 1e-5)
				return (vec);
			else
				return (new Vector3d(0, 0, 0));
		}

		public double xmult(Vector3d p1, Vector3d p2, Vector3d p0)
		{
			return (p1.x - p0.x) * (p2.y - p0.y) - (p2.x - p0.x) * (p1.y - p0.y);
		}

		public int dot_online_in(Vector3d p, Vector3d l1, Vector3d l2)
		{
			double eps = 1e-8;
			if (Math.Abs(xmult(p, l1, l2)) < eps && (l1.x - p.x) * (l2.x - p.x) < eps && (l1.y - p.y) * (l2.y - p.y) < eps)
				return 1;
			else return 0;
		}

		////////////////////////// Precast Layout //////////////////////////

		public void GetHomoMatrix(Plane currPlane, out Matrix3d cvmat, out Vector3d[] screenCoord)
		{
			OpenGLProjector projector = new OpenGLProjector();
			Vector3d xcoord = currPlane.currCoord[0];
			Vector3d ycoord = currPlane.currCoord[1];
			Vector2d currV = new Vector2d();
			PointF[] points2D = new PointF[currPlane.currVertices.Count];
			screenCoord = new Vector3d[currPlane.currVertices.Count];

			for (int i = 0; i < currPlane.currVertices.Count; i++)
			{
				// Local 2D coord
				Vector3d iPoint = currPlane.currVertices[i].Coord;
				screenCoord[i] = projector.Project(iPoint);
				iPoint = iPoint - currPlane.currCenter;
				currV.x = iPoint.Dot(xcoord);
				currV.y = iPoint.Dot(ycoord);
				points2D[i].X = (float)currV.x;
				points2D[i].Y = (float)currV.y;
			}

			PointF[] screen = new PointF[currPlane.currVertices.Count];
			for (int i = 0; i < currPlane.currVertices.Count; i++)
			{
				screen[i].X = (float)screenCoord[i].x;
				screen[i].Y = (float)screenCoord[i].y;
			}

			Matrix<double> homomat = CameraCalibration.FindHomography(screen, points2D, Emgu.CV.CvEnum.HOMOGRAPHY_METHOD.RANSAC, 1e-3);
			double[,] homoarray = new double[3, 3];
			for (int i = 0; i < 3; i++)
				for (int j = 0; j < 3; j++)
					homoarray[i, j] = homomat[i, j];
			// The homography matrix between screen coordinates and current Plane
			cvmat = new Matrix3d(homoarray);
		}

		private void MouseMovePos2(Plane plane)
		{
			Vector3d mouseDownVec = new Vector3d(mouseDownPosition.x, mouseDownPosition.y, 1.0);
			Vector3d mouseUpVec = new Vector3d(currMousePosition.x, currMousePosition.y, 1.0);
			Vector3d getPoint3D = new Vector3d();
			Matrix3d cvmat;
			Vector3d[] screenCoord;
			this.GetHomoMatrix(plane, out cvmat, out screenCoord);

			getPoint3D = cvmat * mouseDownVec;
			clickPoint.x = getPoint3D[0] / getPoint3D[2];
			clickPoint.y = getPoint3D[1] / getPoint3D[2];
			clickPoint.z = 0.0;

			Matrix3d m = new Matrix3d(plane.currCoord[0], plane.currCoord[1], plane.currCoord[2]);
			m = m.Transpose();
			clickPoint = m.Inverse() * clickPoint;
			clickPoint += plane.currCenter;

			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			Matrix4d newMat = mat.Inverse();
			clickPoint = (newMat * new Vector4d(clickPoint, 1)).ToVector3d();

			getPoint3D = cvmat * mouseUpVec;
			releasePoint.x = getPoint3D[0] / getPoint3D[2];
			releasePoint.y = getPoint3D[1] / getPoint3D[2];
			releasePoint.z = 0.0;
			releasePoint = m.Inverse() * releasePoint;
			releasePoint += plane.currCenter;
			releasePoint = (newMat * new Vector4d(releasePoint, 1)).ToVector3d();
		}
		private void MouseMovePos(Plane plane)
		{
			Vector2d off = this.currMousePosition - this.mouseDownPosition;

			double w = this.Size.Width;
			double h = this.Size.Height;

			clickPoint = new Vector3d((this.mouseDownPosition.x / w*1.2 - 0.6) , this.mouseDownPosition.y / h*0.8 - 0.4 , 0);
			releasePoint = new Vector3d(this.currMousePosition.x / w*1.2 -0.6, this.currMousePosition.y / h*0.8-0.4 , 0);

		}

		private int[] SelectWall()
		{
			Matrix3d cvmat;
			int i, j;
			Matrix<double> getPoint3D = new Matrix<double>(3, 1);
			Vector3d[] screenCoord;
			double depth = double.MaxValue;
			int[] indWall = {-1, -1};
			bool inside = false;

			for (i = 0; i < walls.Count; i++)
                for (j = 0; j < walls[i].Count; j++)
                {
                    Plane currPlane = this.walls[i][j].cuboid.planes[0];
                    this.GetHomoMatrix(currPlane, out cvmat, out screenCoord);
                    currPlane.SetScreenCoordinate(screenCoord);
                    inside = PointInPoly(currMousePosition, currPlane.screenCoord);
                    if (inside && currPlane.depth < depth)
                    {
                        depth = currPlane.depth;
                        indWall[0] = i;
                        indWall[1] = j;
                        break;
                    }
                    inside = false;
                }
			return(indWall);
		}

		private int[] SelectRoom()
		{
			int i, j;
			int[] indRoom = new int[2]{-1,-1};
			bool inside = false;

			this.ObtainGLProjector();
			OpenGLProjector projector = this.glProjector;
			for (i = 0; i < recRooms.Count; i++)
				for (j = 0; j < recRooms[i].Count; j++)
				{
					List<Vector2d> verts = new List<Vector2d>();
					Vector3d v = new Vector3d();
					for (int t = 0; t < recRooms[i][j].vertices.Count; t++)
					{
						v = projector.Project(recRooms[i][j].vertices[t].Coord);
						verts.Add(new Vector2d(v.x, v.y));
					}
					inside = PointInPoly(mouseDownPosition, verts);
					if (inside)
					{
						indRoom[0] = i;
						indRoom[1] = j;
						break;
					}
					inside = false;
				}
			return (indRoom);
		}

		private int[] SelectCuboid() //2D
		{
			int f, i, j;
			int[] indCuboid = { -1, -1 };
			bool inside = false;
			this.ObtainGLProjector();
			OpenGLProjector projector = this.glProjector;
			List<Vector2d> verts;
			Vector3d v;
			Vector2d v1, v2;
			bool onBound;

			if (walls == null || walls.Count == 0)
				return (indCuboid);
			for (f = 0; f < numFloor; f++)
			{
				if (walls[f] == null) continue;
				for (i = 0; i < walls[f].Count; i++)
					{
						verts = new List<Vector2d>();
						for (j = 0; j < cuboids[f][i].vertices.Count / 2; j++)
						{
							v = projector.Project(cuboids[f][i].vertices[j].Coord);
							verts.Add(new Vector2d(v.x, v.y));
						}
						inside = PointInPoly(currMousePosition, verts);
						if (inside)
						{
							v1 = new Vector2d(walls[f][i].endPoints[0].Coord.x, walls[f][i].endPoints[0].Coord.y);
							v2 = new Vector2d(walls[f][i].endPoints[1].Coord.x, walls[f][i].endPoints[1].Coord.y);
							onBound = OnBoundary(f, v1, v2);
							if (!onBound)
							{
								indCuboid[0] = f;
								indCuboid[1] = i;
							}
							break;
						}
					}
				if (inside)
					break;
			}
			activeFloor = indCuboid[0];
			return (indCuboid);
		}

		public int[] GetNeighborWall(int floor, int ind, int type)
		{
			// Indices of related walls[floor] and which endpoints
			int[] wallInds = new int[4];
			int i;
			bool i0 = false, i1 = false;
			List<int> inters;
			for (i = 0; i < walls[floor].Count; i++)
			{
				if (i0 && i1) break;
				Distinct(walls[floor][i].roomIndices);
				inters = Intersect(walls[floor][i].roomIndices, walls[floor][ind].roomIndices);
				if (i == ind || inters.Count() == 0) continue;
				// when the two walls[floor] belong to the same room
				if (walls[floor][i].direction.Dot(walls[floor][ind].direction) < thresh)
				{
					if (!i0)
					{
						if ((walls[floor][i].endPoints[0].Coord - walls[floor][ind].endPoints[0].Coord).Length() < thresh)
						{
							wallInds[0] = i;
							wallInds[1] = 0;
							i0 = true;
						}
						else if ((walls[floor][i].endPoints[1].Coord - walls[floor][ind].endPoints[0].Coord).Length() < thresh)
						{
							wallInds[0] = i;
							wallInds[1] = 1;
							i0 = true;
						}
					}
					if (!i1)
					{
						if ((walls[floor][i].endPoints[0].Coord - walls[floor][ind].endPoints[1].Coord).Length() < thresh)
						{
							wallInds[2] = i;
							wallInds[3] = 0;
							i1 = true;
						}
						else if ((walls[floor][i].endPoints[1].Coord - walls[floor][ind].endPoints[1].Coord).Length() < thresh)
						{
							wallInds[2] = i;
							wallInds[3] = 1;
							i1 = true;
						}
					}
				}
			}
			return (wallInds);
		}

		public bool VertexInSet(List<Vector3d> verts, Vector3d v)
		{
			bool inside = false;
			for (int i = 0; i < verts.Count; i++)
			{
				if ((verts[i] - v).Length() < 1e-10)
				{
					inside = true;
					break;
				}
			}
			return (inside);
		}
		public void ReBuildWalls(int floor, Vector3d v0, int label, Vector3d translate, int ind)
		{
			int i, j, k, num, count = 0, record = 0; 
			bool vertical, i0, i1;
			List<int> connect = new List<int>(), conns = new List<int>();
			List<Vector3d> verts = new List<Vector3d>();
			Vector3d v1 = new Vector3d(), v2 = new Vector3d();
			verts.Add(v0);
			conns.Add(ind);

			while (conns.Count > record)
			{
				record = conns.Count;
				// due to the incontinuous wall order, some walls may not be included into the connect set
				for (i = 0; i < walls[floor].Count; i++)
				{
					if (conns.IndexOf(i) != -1) continue;
					vertical = false;
					if (Math.Abs(translate.Dot(walls[floor][i].direction)) < thresh)
						vertical = true;
					i0 = VertexInSet(verts, walls[floor][i].endPoints[0].Coord);
					i1 = VertexInSet(verts, walls[floor][i].endPoints[1].Coord);
					if (!i0 && !i1) continue;
					if (vertical)
					{
						if (i0 || i1)
						{
							connect.Add(i);
							connect.Add(0);
							connect.Add(i);
							connect.Add(1);
							if (i0) // endpoints[0] already inside the set
								verts.Add(walls[floor][i].endPoints[1].Coord);
							else
								verts.Add(walls[floor][i].endPoints[0].Coord);
							conns.Add(i);
						}
					}
					else
					{
						if (i0)
						{
							connect.Add(i);
							connect.Add(0);
							conns.Add(i);
						}
						else if (i1)
						{
							connect.Add(i);
							connect.Add(1);
							conns.Add(i);
						}
					}

					if (i0 && (walls[floor][i].endPoints[0].Coord - v0).Length() < thresh)
					{
						walls[floor][i].visit_v[0] = true;
						walls[floor][i].visit_h[0] = true;
					}
					if (i1 && (walls[floor][i].endPoints[1].Coord - v0).Length() < thresh)
					{
						walls[floor][i].visit_v[1] = true;
						walls[floor][i].visit_h[1] = true;
					}
					if (Math.Abs(translate.x) < thresh) // Y-dir
					{
						if (vertical)
						{
							walls[floor][i].visit_v[0] = true;
							walls[floor][i].visit_v[1] = true;
						}
						else
						{
							if (i0)
								walls[floor][i].visit_v[0] = true;
							else if (i1)
								walls[floor][i].visit_v[1] = true;
						}
					}
					else //X-dir
					{
						if (vertical)
						{
							walls[floor][i].visit_h[0] = true;
							walls[floor][i].visit_h[1] = true;
						}
						else
						{
							if (i0)
								walls[floor][i].visit_h[0] = true;
							else if (i1)
								walls[floor][i].visit_h[1] = true;
						}
					}
				}
			}

			num = connect.Count / 2;
			for (k = 0; k < num; k++)
			{
				i = connect[k * 2];
				j = connect[k * 2 + 1];
				if (k > 0 && i == connect[(k - 1) * 2])
					continue;
				vertical = false;
				if (Math.Abs(translate.Dot(walls[floor][i].direction)) < thresh)
					vertical = true;
				// The point is connected on the line
				// If the wall is perpendicular to the active wall, both endpoints need to be tranlated
				walls[floor][i].endPoints[j].Coord += translate;
				count = walls[floor][i].cuboid.planeVertices.Count;
				if (vertical) // all vertices translate
				{
					walls[floor][i].endPoints[1 - j].Coord += translate;
					for (int t = 0; t < count; t++)
						walls[floor][i].cuboid.planeVertices[t].Coord += translate;
				}
				else
				{
					if (j == 0)
					{
						v1 = walls[floor][i].cuboid.planeVertices[0].Coord;
						v2 = walls[floor][i].cuboid.planeVertices[1].Coord;
						walls[floor][i].cuboid.planeVertices[0].Coord += translate;
						walls[floor][i].cuboid.planeVertices[1].Coord += translate;
					}
					else
					{
						v1 = walls[floor][i].cuboid.planeVertices[count - 1].Coord;
						v2 = walls[floor][i].cuboid.planeVertices[count - 2].Coord;
						walls[floor][i].cuboid.planeVertices[count - 1].Coord += translate;
						walls[floor][i].cuboid.planeVertices[count - 2].Coord += translate;
					}
					if (walls[floor][i].vertexType[j] == 2) // 2 vertices
					{
						if (j == 0)
						{
							v2 = walls[floor][i].cuboid.planeVertices[2].Coord;
							walls[floor][i].cuboid.planeVertices[2].Coord += translate;
						}
						else
						{
							v2 = walls[floor][i].cuboid.planeVertices[count - 3].Coord;
							walls[floor][i].cuboid.planeVertices[count - 3].Coord += translate;
						}
					}
				}
				walls[floor][i].cuboid.UpdatePlaneVertices();
				if (vertical)  // vertical to the active wall, the doors need to be moved
				{
					for (int t = 0; t < walls[floor][i].doors.Count; t++)
					{
						walls[floor][i].doors[t].endPoints[0].Coord += translate;
						walls[floor][i].doors[t].endPoints[1].Coord += translate;
						for (int p = 0; p < walls[floor][i].doors[t].cuboid.vertices.Count; p++)
							walls[floor][i].doors[t].cuboid.vertices[p].Coord += translate;
					}
					// If it has already been casted
					if (walls[floor][i].casts != null && walls[floor][i].casts.Count > 0)
					{
						for (int t = 0; t < walls[floor][i].casts.Count; t++)
						{
							for (int p = 0; p < walls[floor][i].casts[t].vertices.Count; p++)
								walls[floor][i].casts[t].vertices[p].Coord += translate;
						}
					}
				}
				else // in case of deadlock, the visited wall is changed again
				{
					if (walls[floor][i].casts != null && walls[floor][i].casts.Count > 0)
					{
						Vector2d v21 = new Vector2d(v1.x, v1.y), v22 = new Vector2d(v2.x, v2.y);
						for (int t = 0; t < walls[floor][i].casts.Count; t++)
						{
							for (int p = 0; p < walls[floor][i].casts[t].vertices.Count; p++)
							{
								Vector2d v20 = new Vector2d(walls[floor][i].casts[t].vertices[p].Coord.x,
									walls[floor][i].casts[t].vertices[p].Coord.y);
								if ((v21 - v20).Length() < thresh || (v22 - v20).Length() < thresh)
									walls[floor][i].casts[t].vertices[p].Coord += translate;
							}
						}
					}
				}
			}
		}

		int totald = 0, totalw = 0;
		public double PrecastConcrete(int ind)
		{
			double mv,wallMove = 0;
			if (walls[0][0].casts != null && walls[0][0].casts.Count > 0)
				return (wallMove );
			types = new int[blocks.Length * 2];
			if (ind >= 0 && walls[activeFloor][ind].casts.Count > 0)
			{
				for (int i = 0; i < blocks.Length * 2; i++)
					types[i] = walls[activeFloor][ind].slabType[i];
				return (0);
			}
			ncuts = 0;
			wcuts = 0; //windows & doors
			for (int i = 0; i < numFloor; i++)
			{
				mv = CastAfloor(i);
				wallMove += mv;
			}
			for (int i = 0; i < numFloor; i++)
			{
				for (int j = 0; j < walls[i].Count; j++)
				{
					for (int k = 0; k < walls[i][j].slabType.Length; k++)
						types[k] += walls[i][j].slabType[k];
				}
			}
			Program.SetSlabText(types, 0);
			// Construct 3D casts
			Vector3d dir = new Vector3d(), v;
			List<Vector3d> castVerts;
			for (int i = 0; i < numFloor; i++)
				for (int j = 0; j < walls[i].Count; j++)
				{
					for (int k = 0; k < walls[i][j].num_front_cast; k++)
					{
						castVerts = new List<Vector3d>();
						if (Math.Abs(walls[i][j].direction.x) < thresh)
						{
							dir = new Vector3d(walls[i][j].cuboid.vertices[0].Coord.x -
								walls[i][j].cuboid.vertices[1].Coord.x, 0, 0);
							dir = dir.Normalize();
							if (Math.Abs(walls[i][j].casts[k].vertices[0].Coord.x - 
								(walls[i][j].cuboid.endPoints[0].Coord.x - dir.x * wallThickness/2) ) < thresh)
								dir = new Vector3d(-dir.x, -dir.y, -dir.z);
						}
						else if (Math.Abs(walls[i][j].direction.y) < thresh)
						{
							dir = new Vector3d(0, walls[i][j].cuboid.vertices[0].Coord.y -
								walls[i][j].cuboid.vertices[1].Coord.y, 0);
							dir = dir.Normalize();
							if (Math.Abs(walls[i][j].casts[k].vertices[0].Coord.y -
								(walls[i][j].cuboid.endPoints[0].Coord.y - dir.y * wallThickness / 2)) < thresh)
								dir = new Vector3d(-dir.x, -dir.y, -dir.z);
						}
						for (int t = 0; t < walls[i][j].casts[k].vertices.Count; t++)
							castVerts.Add(new Vector3d(walls[i][j].casts[k].vertices[t].Coord));
						List<Vertex> verts = new List<Vertex>();
						for (int t = 0; t < walls[i][j].casts[k].vertices.Count; t++)
						{
							v = walls[i][j].casts[k].vertices[t].Coord + dir * castThickness;
							castVerts.Add(new Vector3d(v));
							verts.Add(new Vertex(v));
						}
						walls[i][j].castPlanes.Add(new List<Plane>());
						int nc = walls[i][j].castPlanes.Count - 1;
						walls[i][j].castPlanes[nc].Add(new Plane(walls[i][j].casts[k]));
						walls[i][j].castPlanes[nc].Add(new Plane(verts, verts));
						verts = new List<Vertex>();
						verts.Add(new Vertex(castVerts[0]));
						verts.Add(new Vertex(castVerts[1]));
						verts.Add(new Vertex(castVerts[5]));
						verts.Add(new Vertex(castVerts[4]));
						walls[i][j].castPlanes[nc].Add(new Plane(verts, verts));
						verts = new List<Vertex>();
						verts.Add(new Vertex(castVerts[1]));
						verts.Add(new Vertex(castVerts[2]));
						verts.Add(new Vertex(castVerts[6]));
						verts.Add(new Vertex(castVerts[5]));
						walls[i][j].castPlanes[nc].Add(new Plane(verts, verts));
						verts = new List<Vertex>();
						verts.Add(new Vertex(castVerts[2]));
						verts.Add(new Vertex(castVerts[3]));
						verts.Add(new Vertex(castVerts[7]));
						verts.Add(new Vertex(castVerts[6]));
						walls[i][j].castPlanes[nc].Add(new Plane(verts, verts));
						verts = new List<Vertex>();
						verts.Add(new Vertex(castVerts[3]));
						verts.Add(new Vertex(castVerts[0]));
						verts.Add(new Vertex(castVerts[4]));
						verts.Add(new Vertex(castVerts[7]));
						walls[i][j].castPlanes[nc].Add(new Plane(verts, verts));
					}
				}
			drawLabel = true;
			return (wallMove);
		}

		List<int[]> pickCasts = new List<int[]>();
		public void SelectCasts()
		{
			int i, j, k, t;
			int[] index = new int[3];
			Vector3d v0, v;
			List<Vector2d> verts = new List<Vector2d>();
			bool inside = false;
			double depth = double.MaxValue, dep;
			this.ObtainGLProjector();
			OpenGLProjector projector = this.glProjector;
			MouseMovePos(XYPlane);
			v0 = projector.Project(releasePoint);

			for (i = 0; i < walls.Count; i++)
			{
				for (j = 0; j < walls[i].Count; j++)
				{
					for (k = 0; k < walls[i][j].casts.Count; k++)
					{
						verts = new List<Vector2d>();
						v = new Vector3d();
						dep = 0;
						for (t = 0; t < walls[i][j].casts[k].vertices.Count; t++)
						{
							v = projector.Project(walls[i][j].casts[k].vertices[t].Coord);
							verts.Add(new Vector2d(v.x, v.y));
							dep += v.z;
						}
						dep /= walls[i][j].casts[k].vertices.Count;
						v /= walls[i][j].casts[k].vertices.Count;
						inside = PointInPoly(mouseDownPosition, verts);
						if (inside)
						{
							if (dep < depth)
							{
								index[0] = i;
								index[1] = j;
								index[2] = k;
								depth = dep;
							}
						}
					}
				}
			}
			for (i = 0; i < pickCasts.Count(); i++)
			{
				t = pickCasts[i][0];
				j = pickCasts[i][1];
				k = pickCasts[i][2];
				if (index[0] == t && index[1] == j && index[2] == k)
				{
					i = -1;
					break;
				}
			}
			if (i != -1)
				pickCasts.Add(index);
		}

		public void MergeCasts(int floor)
		{
			int i, j, k, t, w, r, label;
			double value, cvalue, thresh = 0.01;
			List<int[]> casts = new List<int[]>();
			int[] records;

			for (i = 0; i < bdInds.Count(); i++)
			{
				if (i == 0 || i == 2)
				{
					label = 1;
					value = bounds[floor][i].x;
				}
				else
				{
					label = 2;
					value = bounds[floor][i].y;
				}
				if (i == 0 || i == 1)
					cvalue = value + wallThickness/2;
				else
					cvalue = value - wallThickness/2;
				for (j = 0; j < bdInds[i].Count(); j++)
				{
					r = bdInds[i][j]; // boundary room
					for (k = 0; k < recRooms[floor][r].wallIndices.Count; k++)
					{
						w = recRooms[floor][r].wallIndices[k];
						// decide whether the wall is on the boundary
						// only keep the cast on the top of window/door
						// the cast below the window is eliminated for faciliatating order
						if (label == 1 && Math.Abs(walls[floor][w].direction.x) < thresh)
						{
							if (Math.Abs(walls[floor][w].endPoints[0].Coord.x - value) > thresh)
								continue;
							for (t = 0; t < walls[floor][w].casts.Count(); t++)
							{
								if ((walls[floor][w].casts[t].vertices[0].Coord -
									walls[floor][w].casts[t].vertices[1].Coord).Length() < slabHeight - thresh
									&& walls[floor][w].casts[t].vertices[0].Coord.z < thresh)
									continue;
								if (Math.Abs(walls[floor][w].casts[t].vertices[0].Coord.x -
									walls[floor][w].casts[t].vertices[3].Coord.x) < thresh &&
									Math.Abs(walls[floor][w].casts[t].vertices[0].Coord.x - cvalue) < thresh)
								{
									records = new int[3];
									records[0] = floor; records[1] = w;
									records[2] = t;
									pickCasts.Add(records);
								}
							}
						}
						else if (label == 2 && Math.Abs(walls[floor][w].direction.y) < thresh)
						{
							if (Math.Abs(walls[floor][w].endPoints[0].Coord.y - value) > thresh)
								continue;
							for (t = 0; t < walls[floor][w].casts.Count(); t++)
							{
								if ((walls[floor][w].casts[t].vertices[0].Coord -
									walls[floor][w].casts[t].vertices[1].Coord).Length() < slabHeight - thresh
									&& walls[floor][w].casts[t].vertices[0].Coord.z < thresh)
									continue;
								if (Math.Abs(walls[floor][w].casts[t].vertices[0].Coord.y -
									walls[floor][w].casts[t].vertices[3].Coord.y) < thresh &&
									Math.Abs(walls[floor][w].casts[t].vertices[0].Coord.y - cvalue) < thresh)
								{
									records = new int[3];
									records[0] = floor; records[1] = w;
									records[2] = t;
									pickCasts.Add(records);
								}
							}
						}
					}// each boundary wall
				}// each boundary room
				// order the casts
				OrderCasts(label);
				while(orderCasts!= null && orderCasts.Count() > 0)
				{
					pickCasts = orderCasts[0];
					MergeGivenCasts();
					orderCasts.RemoveAt(0);
				}
				pickCasts = new List<int[]>();
			}// each boundary line
			
			// Merge walls caused by open walls
			List<int> lwalls;
			for (i = 0; i < jointVertices.Count; i++)
			{
				if (jointVertices[i].Item2 != floor) continue;
				Vector3d dir;
				if (i % 2 == 0)
					dir = (jointVertices[i + 1].Item1 - jointVertices[i].Item1).Normalize();
				else
					dir = (jointVertices[i - 1].Item1 - jointVertices[i].Item1).Normalize();
				lwalls = new List<int>();
				for (j = 0; j < walls[floor].Count; j++)
				{
					value = dir.Dot(walls[floor][j].direction);
					if (Math.Abs(value) > thresh) continue;
					if ((walls[floor][j].endPoints[0].Coord - jointVertices[i].Item1).Length() < thresh ||
						(walls[floor][j].endPoints[1].Coord - jointVertices[i].Item1).Length() < thresh)
						lwalls.Add(j);
				}
				// At most 4 walls are connected in the same endpoint
				// At most 3 walls after deleting one open wall
				// At most 2 walls lie on the same line
				if (lwalls.Count < 2) continue;
				k = -1;
				bool found = false;
				for (j = 0; j < lwalls.Count - 1; j++)
				{
					for (k = j + 1; k < lwalls.Count; k++)
					{
						if (Math.Abs(walls[floor][lwalls[j]].direction.Dot(walls[floor][lwalls[k]].normal)) < thresh)
						{
							found = true;
							break;
						}
					}
					if (found)
						break;
				}
				if (j >= lwalls.Count || k >= lwalls.Count || lwalls[j] == lwalls[k]) continue;

				if (Math.Abs(dir.x) < thresh) // the direction of walls to be handled are perpendicular to dir, so inverse label
				{
					label = 2;
					value = jointVertices[i].Item1.y + dir.y * wallThickness / 2;
				}
				else 
				{
					label = 1;
					value = jointVertices[i].Item1.x + dir.x * wallThickness / 2;
				}
				int[] ws = new int[2] { lwalls[j], lwalls[k] };
				for (j = 0; j < 2; j++)
					for (k = 0; k < walls[floor][ws[j]].casts.Count; k++)
					{
						if ((label == 2 && Math.Abs(walls[floor][ws[j]].casts[k].vertices[0].Coord.y - value) < thresh) ||
							(label == 1 && Math.Abs(walls[floor][ws[j]].casts[k].vertices[1].Coord.x - value) < thresh))
						{
							records = new int[3];
							records[0] = floor; records[1] = ws[j]; records[2] = k;
							pickCasts.Add(records);
						}
					}
				OrderCasts(label);
				while (orderCasts.Count() > 0)
				{
					pickCasts = orderCasts[0];
					MergeGivenCasts();
					orderCasts.RemoveAt(0);
				}
				pickCasts = new List<int[]>();
			}
		}
		List<List<int[]>> orderCasts;
		public void OrderCasts(int label)
		{
			int i, f, w, c, first = -1;
			double value = double.MaxValue, cvalue = double.MaxValue;
			List<int[]> casts = new List<int[]>();
			int[] records;

			if (pickCasts.Count == 0) return;
			orderCasts = new List<List<int[]>>();
			// Find the first cast
			records = new int[3];
			// from smallest to largest
			f = pickCasts[0][0];
			for (i = 0; i < pickCasts.Count; i++)
			{
				w = pickCasts[i][1];
				c = pickCasts[i][2];
				Console.WriteLine(walls[f][w].casts[c].vertices[0].Coord.x);
				Console.WriteLine(walls[f][w].casts[c].vertices[3].Coord.x);
				if ((label == 1 && (walls[f][w].casts[c].vertices[0].Coord.y < cvalue ||
					walls[f][w].casts[c].vertices[3].Coord.y < cvalue)) ||
					(label == 2 && (walls[f][w].casts[c].vertices[0].Coord.x < cvalue ||
					walls[f][w].casts[c].vertices[3].Coord.x < cvalue)))
				{
					records[0] = f;
					records[1] = w;
					records[2] = c;
					if (label == 1 && walls[f][w].casts[c].vertices[0].Coord.y < cvalue)
					{
						cvalue = walls[f][w].casts[c].vertices[0].Coord.y;
						value = walls[f][w].casts[c].vertices[3].Coord.y;
					}
					else if (label == 1 && walls[f][w].casts[c].vertices[3].Coord.y < cvalue)
					{
						cvalue = walls[f][w].casts[c].vertices[3].Coord.y;
						value = walls[f][w].casts[c].vertices[0].Coord.y;
					}
					else if (label == 2 && walls[f][w].casts[c].vertices[0].Coord.x < cvalue)
					{
						cvalue = walls[f][w].casts[c].vertices[0].Coord.x;
						value = walls[f][w].casts[c].vertices[3].Coord.x;
					}
					else
					{
						cvalue = walls[f][w].casts[c].vertices[3].Coord.x;
						value = walls[f][w].casts[c].vertices[0].Coord.x;
					}
					first = i;
				}
			}
			casts.Add(records);
			pickCasts.RemoveAt(first);
			if (value < cvalue)
				value = cvalue;
			i = 0;
			while (pickCasts.Count > 0 && i < pickCasts.Count)
			{
				f = pickCasts[i][0];
				w = pickCasts[i][1];
				c = pickCasts[i][2];
				
				if ((label == 1 && (Math.Abs(walls[f][w].casts[c].vertices[0].Coord.y - value) < thresh ||
					Math.Abs(walls[f][w].casts[c].vertices[3].Coord.y - value) < thresh)) ||
					(label == 2 && (Math.Abs(walls[f][w].casts[c].vertices[0].Coord.x - value) < thresh ||
					Math.Abs(walls[f][w].casts[c].vertices[3].Coord.x - value) < thresh)))
				{
					records = new int[3];
					records[0] = f;
					records[1] = w;
					records[2] = c;
					casts.Add(records);
					pickCasts.RemoveAt(i);
					i = 0;
					if (label == 1 && Math.Abs(walls[f][w].casts[c].vertices[0].Coord.y - value) < thresh)
						value = walls[f][w].casts[c].vertices[3].Coord.y;
					else if (label == 1 && Math.Abs(walls[f][w].casts[c].vertices[3].Coord.y - value) < thresh)
						value = walls[f][w].casts[c].vertices[0].Coord.y;
					else if (label == 2 && Math.Abs(walls[f][w].casts[c].vertices[0].Coord.x - value) < thresh)
						value = walls[f][w].casts[c].vertices[3].Coord.x;
					else
						value = walls[f][w].casts[c].vertices[0].Coord.x;

					// Meet a break -- window/door
					if (casts.Count > 1)
					{
						int lf = casts[casts.Count - 2][0], lw = casts[casts.Count - 2][1], lc = casts[casts.Count - 2][2];
						if (Math.Abs(walls[lf][lw].casts[lc].vertices[0].Coord.z - walls[lf][lw].casts[lc].vertices[1].Coord.z) < slabHeight)
						{
							casts.RemoveAt(casts.Count - 1);
							casts.RemoveAt(casts.Count - 1);
							if (casts.Count > 1)
								orderCasts.Add(casts);
							casts = new List<int[]>();
							casts.Add(records);
						}
					}
				}
				else
					i++;
			}
			if (casts.Count > 0)
				orderCasts.Add(casts);
		}

		public int Dismatch()
		{
			// Test whether the number of casts equals to the number of cast types
			int dis = -1;
			for (int i = 0; i < numFloor; i++)
			{
				for (int j = 0; j < walls[i].Count; j++)
				{
					if (walls[i][j].casts.Count != walls[i][j].castType.Count)
					{
						dis = j;
						break;
					}
				}
				if (dis != -1)
					break;
			}
			return (dis);
		}
		public void MergeGivenCasts()
		{
			// After casting, some wall segments are merged due to open wall deletion
			// and boundary walls, as the casting is implemented on a single wall
			int i, j, k, label, dir, floor, icut;
			int[] nums, numf = new int[blocks.Length];
			double length = 0, len, minv, maxv;
			double min = double.MaxValue, max = double.MinValue;
			double[] useblocks;
			List<List<int>> bwalls = new List<List<int>>();
			List<Plane> casts = new List<Plane>();

			if (pickCasts.Count < 2)
				return;
			floor = pickCasts[0][0];
			icut = 0;
			for (i = 0; i < pickCasts.Count; i++)
			{
				j = pickCasts[i][1];
				k = pickCasts[i][2];
				casts.Add(walls[floor][j].casts[k]);
				if (Math.Abs((walls[floor][j].casts[k].vertices[0].Coord -
					walls[floor][j].casts[k].vertices[1].Coord).Length() - slabHeight) > thresh)
					continue;
				len = (walls[floor][j].casts[k].vertices[0].Coord -
					walls[floor][j].casts[k].vertices[3].Coord).Length();
				length += len;
				if (Math.Abs(len - blocks[0])< thresh || Math.Abs(len - blocks[1]) < thresh || Math.Abs(len - blocks[2]) < thresh)
					continue;
				icut++;
			}
			if (Math.Abs(casts[0].vertices[0].Coord.x - casts[0].vertices[3].Coord.x) < thresh)
			{
				label = 1;
				if (casts[0].vertices[0].Coord.y > casts[0].vertices[3].Coord.y)
					dir = -1;
				else
					dir = 1;
			}
			else
			{
				label = 2;
				if (casts[0].vertices[0].Coord.x > casts[0].vertices[3].Coord.x)
					dir = -1;
				else
					dir = 1;
			}
			if (length < blocks[0])
				numf[0]++;
			else
			{
				nums = PutRods(blocks, length);
				for (i = 0; i < blocks.Length; i++)
					numf[i] = nums[i + 1];
			}
			double diff = W2 - length;
			if (icut == 0 || (Math.Abs(diff) > thresh && icut <= 1)) // No need to merge
			{
				return;
			}
			// Begin the merge
			int t = 0;
			for (i = 0; i < pickCasts.Count; i++)
			{
				j = pickCasts[i][1];
				k = pickCasts[i][2];
				if (label == 1)
				{
					maxv = Math.Max(casts[t].vertices[0].Coord.y, casts[t].vertices[3].Coord.y);
					minv = Math.Min(casts[t].vertices[0].Coord.y, casts[t].vertices[3].Coord.y);
				}
				else
				{
					maxv = Math.Max(casts[t].vertices[0].Coord.x, casts[t].vertices[3].Coord.x);
					minv = Math.Min(casts[t].vertices[0].Coord.x, casts[t].vertices[3].Coord.x);
				}
				t++;
				if (maxv > max)
					max = maxv;
				if (minv < min)
					min = minv;
				for (int p = 0; p < blocks.Length; p++)
					if (Math.Abs((walls[floor][j].casts[k].vertices[0].Coord -
						walls[floor][j].casts[k].vertices[3].Coord).Length() - blocks[p]) < thresh)
					{
						walls[floor][j].slabType[p]--;
						break;
					}
				walls[floor][j].casts.RemoveAt(k);
				walls[floor][j].castType.RemoveAt(k);
				
				for (int p = i + 1; p < pickCasts.Count; p++)
				{
					if (j == pickCasts[p][1])
					{
						if (pickCasts[p][2] > k)
							pickCasts[p][2]--;
					}
				}
				for (int q = 0; q < orderCasts.Count; q++)
				{
					for (int p = 0; p < orderCasts[q].Count; p++)
					{
						if (orderCasts[q][p][0] == floor && orderCasts[q][p][1] == j &&
							orderCasts[q][p][2] > k)
							orderCasts[q][p][2]--; 
					}
				}
			}
			int total = Sum(numf);
			useblocks = new double[total];
			k = 0;
			for (i = 0; i < blocks.Length; i++)
			{
				for (j = 0; j < numf[i]; j++)
					useblocks[k++] = blocks[i];
			}
			j = pickCasts[0][1];                                                                       
			double startx, starty;
			if (label == 1)
			{
				startx = casts[0].vertices[0].Coord.x;
				if (dir == 1)
					starty = min;
				else
					starty = max;
			}
			else
			{
				if (dir == 1)
					startx = min;
				else
					startx = max;
				starty = casts[0].vertices[0].Coord.y;
			}
			for (i = 0; i < useblocks.Length; i++)
			{
				List<Vertex> verts = new List<Vertex>();
				if (label == 1)
				{
					verts.Add(new Vertex(startx, starty, 0));
					verts.Add(new Vertex(startx, starty, slabHeight));
					verts.Add(new Vertex(startx, starty + useblocks[i] * dir, slabHeight));
					verts.Add(new Vertex(startx, starty + useblocks[i] * dir, 0));
					starty = verts[3].Coord.y;
					if (Math.Abs(diff) > thresh && i == useblocks.Length - 1)
					{
						verts[2].Coord.y -= diff * dir;
						verts[3].Coord.y -= diff * dir;
						icut--;
					}
				}
				else
				{
					verts.Add(new Vertex(startx, starty, 0));
					verts.Add(new Vertex(startx, starty, slabHeight));
					verts.Add(new Vertex(startx + useblocks[i] * dir, starty, slabHeight));
					verts.Add(new Vertex(startx + useblocks[i] * dir, starty, 0));
					startx = verts[3].Coord.x;
					if (Math.Abs(diff) > thresh && i == useblocks.Length - 1)
					{
						verts[2].Coord.x -= diff * dir;
						verts[3].Coord.x -= diff * dir;
						icut--;
					}
				}
				Plane plane = new Plane(verts, verts);
				plane.UpdateVertexRelatedInfo();
				walls[floor][j].casts.Add(plane);
				//walls[floor][j].casts[walls[floor][j].casts.Count - 1].castVisit = true;
				for (int p = 0; p < blocks.Length; p++)
					if (Math.Abs(useblocks[i] - blocks[p]) < thresh)
					{
						walls[floor][j].slabType[p]++;
						walls[floor][j].castType.Add(p);
						break;
					}
			}
			ncuts -= icut;			
			pickCasts = new List<int[]>();
		}
		public void MergeSelectedCasts()
		{
			int i, j, k, label, dir, floor, icut;
			int[] nums, numf = new int[blocks.Length];
			double length = 0, len, minv, maxv;
			double min = double.MaxValue, max = double.MinValue;
			double[] useblocks;
			List<List<int>> bwalls = new List<List<int>>();
			List<Plane> casts = new List<Plane>();

			if (pickCasts.Count < 2)
			{
				MessageBox.Show("Please select at least two casts!");
				pickCasts = new List<int[]>();
				return;
			}
			floor = pickCasts[0][0];
			icut = 0;
			for (i = 0; i < pickCasts.Count; i++)
			{
				j = pickCasts[i][1];
				k = pickCasts[i][2];
				casts.Add(walls[floor][j].casts[k]);
				if (Math.Abs((walls[floor][j].casts[k].vertices[0].Coord -
					walls[floor][j].casts[k].vertices[1].Coord).Length() - slabHeight) > thresh)
					continue;
				len = (walls[floor][j].casts[k].vertices[0].Coord -
					walls[floor][j].casts[k].vertices[3].Coord).Length();
				if (len < blocks[0])
					icut++;
				length += len;
			}
			if (Math.Abs(casts[0].vertices[0].Coord.x - casts[0].vertices[3].Coord.x) < thresh)
			{
				label = 1;
				if (casts[0].vertices[0].Coord.y > casts[0].vertices[3].Coord.y)
					dir = -1;
				else
					dir = 1;
			}
			else
			{
				label = 2;
				if (casts[0].vertices[0].Coord.x > casts[0].vertices[3].Coord.x)
					dir = -1;
				else
					dir = 1;
			}
			nums = PutRods(blocks, length);
			for (i = 0; i < blocks.Length; i++)
				numf[i] = nums[i + 1];
			double diff = W2 - length;
			if ( icut == 0 || ( Math.Abs(diff) > thresh && icut <= 1)) // No need to merge
			{
				MessageBox.Show("The merge won't decrease the number of cuts.");
				for (i = 0; i < pickCasts.Count; i++)
				{
					j = pickCasts[i][1];
					k = pickCasts[i][2];
					walls[floor][j].casts[k].castVisit = true;
				}
				pickCasts = new List<int[]>();
				return;
			}
			int t = 0;
			for (i = 0; i < pickCasts.Count; i++)
			{
				j = pickCasts[i][1];
				k = pickCasts[i][2];
				if (label == 1)
				{
					maxv = Math.Max(casts[t].vertices[0].Coord.y, casts[t].vertices[3].Coord.y);
					minv = Math.Min(casts[t].vertices[0].Coord.y, casts[t].vertices[3].Coord.y);
				}
				else
				{
					maxv = Math.Max(casts[t].vertices[0].Coord.x, casts[t].vertices[3].Coord.x);
					minv = Math.Min(casts[t].vertices[0].Coord.x, casts[t].vertices[3].Coord.x);
				}
				t++;
				if (maxv > max)
					max = maxv;
				if (minv < min)
					min = minv;
				for (int p = 0; p < blocks.Length; p++)
					if (Math.Abs((walls[floor][j].casts[k].vertices[0].Coord -
						walls[floor][j].casts[k].vertices[3].Coord).Length() - blocks[p]) < thresh)
					{
						walls[floor][j].slabType[p]--;
						break;
					}
				walls[floor][j].casts.RemoveAt(k);
				for (int p = i + 1; p < pickCasts.Count; p++)
				{
					if (j == pickCasts[p][1])
					{
						if (pickCasts[p][2] > k)
							pickCasts[p][2]--;
					}
				}
			}
			int total = Sum(numf);
			useblocks = new double[total];
			k = 0;
			for (i = 0; i < blocks.Length; i++)
			{
				for (j = 0; j < numf[i]; j++)
					useblocks[k++] = blocks[i];
			}
			j = pickCasts[0][1];
			double startx, starty;
			if (label == 1)
			{
				startx = casts[0].vertices[0].Coord.x;
				if (dir == 1)
					starty = min;
				else
					starty = max;
			}
			else
			{
				if (dir == 1)
					startx = min;
				else
					startx = max;
				starty = casts[0].vertices[0].Coord.y;
			}
			for (i = 0; i < useblocks.Length; i++)
			{
				List<Vertex> verts = new List<Vertex>();
				if (label == 1)
				{
					verts.Add(new Vertex(startx, starty, 0));
					verts.Add(new Vertex(startx, starty, slabHeight));
					verts.Add(new Vertex(startx, starty + useblocks[i] * dir, slabHeight));
					verts.Add(new Vertex(startx, starty + useblocks[i] * dir, 0));
					starty = verts[3].Coord.y;
					if (Math.Abs(diff) > thresh && i == useblocks.Length - 1)
					{
						verts[2].Coord.y -= diff * dir;
						verts[3].Coord.y -= diff * dir;
						icut--;
					}
				}
				else
				{
					verts.Add(new Vertex(startx, starty, 0));
					verts.Add(new Vertex(startx, starty, slabHeight));
					verts.Add(new Vertex(startx + useblocks[i] * dir, starty, slabHeight));
					verts.Add(new Vertex(startx + useblocks[i] * dir, starty, 0));
					startx = verts[3].Coord.x;
					if (Math.Abs(diff) > thresh && i == useblocks.Length - 1)
					{
						verts[2].Coord.x -= diff * dir;
						verts[3].Coord.x -= diff * dir;
						icut--;
					}
				}
				Plane plane = new Plane(verts, verts);
				plane.UpdateVertexRelatedInfo();
				walls[floor][j].casts.Add(plane);
				walls[floor][j].casts[walls[floor][j].casts.Count - 1].castVisit = true;
				for (int p = 0; p < blocks.Length; p++)
					if (Math.Abs(useblocks[i] - blocks[p]) < thresh)
					{
						walls[floor][j].slabType[p]++;
						break;
					}
			}
			
			ncuts -= icut;
			for (i = 0; i < types.Length; i++)
				types[i] = 0;
			for (i = 0; i < numFloor; i++)
			{
				for (j = 0; j < walls[i].Count; j++)
				{
					for (k = 0; k < walls[i][j].slabType.Length; k++)
						types[k] += walls[i][j].slabType[k];
				}
			}
			pickCasts = new List<int[]>();
		}
		public void FinishMerge()
		{
			// Change the colors of casts
			for (int f = 0; f < numFloor; f++)
				for (int i = 0; i < walls[f].Count; i++)
					for (int j = 0; j < walls[f][i].casts.Count; j++)
						walls[f][i].casts[j].castVisit = false;
		}
		public bool trad = false;
		public double CastAfloor(int floor)
		{
			int i, j, k;
			Vertex v, v1, v2;
			int wind, h;
			Vector3d dir;
			List<Vertex> verts;
			double wallMove = 0, diff, len;

			for (i = 0; i < walls[floor].Count; i++)
			{
				walls[floor][i].casts = new List<Plane>();
				walls[floor][i].slabType = new int[blocks.Length];
				for (j = 0; j < walls[floor][i].slabType.Count(); j++)
					walls[floor][i].slabType[j] = 0;
				if (!trad)
					diff = PutSlab(floor, i);
				else
					diff = TraditionPutSlab(floor, i);
				if (diff == -1) return (0);
				wallMove += diff;
			}
			// Merge possible casts to decrease the cuts
			MergeCasts(floor);
			for (i = 0; i <walls[floor].Count; i++)
				walls[floor][i].num_front_cast = walls[floor][i].casts.Count;
			Console.WriteLine("Total distance of traslating walls[floor] to cast concretes:");
			Console.WriteLine(wallMove);
			// Cast side wall planes generated by open wall
			// It happens when one endpoint of a wall has no neighbor
			AddSideWallPlanes(floor); // re-calculate side wall positions
			for (i = 0; i < sideWallPlanes[floor].Count; i++)
			{
				verts = new List<Vertex>();
				v = sideWallPlanes[floor][i].vertices[0];
				wind = sideWallPlanes[floor][i].wallInd;
				h = (int)Math.Round(walls[floor][wind].cuboid.height / slabHeight);
				dir = sideWallPlanes[floor][i].vertices[1].Coord - sideWallPlanes[floor][i].vertices[0].Coord;
				len = dir.Length();
				dir = dir.Normalize();
				v1 = new Vertex(v);

				for (k = 0; k < h; k++)
				{
					verts = new List<Vertex>();
					verts.Add(new Vertex(v1));
					verts.Add(new Vertex(v1.Coord.x, v1.Coord.y, v1.Coord.z + slabHeight));
					v2 = new Vertex(sideWallPlanes[floor][i].vertices[1]);
					verts.Add(new Vertex(v2.Coord.x, v2.Coord.y, v2.Coord.z + slabHeight));
					verts.Add(v2);
					walls[floor][wind].casts.Add(new Plane(verts, verts));
					walls[floor][wind].slabType[0]++;
					walls[floor][wind].castType.Add(blocks.Length + 1);
					v1.Coord.z += slabHeight;
					if (Math.Abs(len - blocks[0]) > thresh)
						ncuts++;
				}
			}
			
			// Cast door/window fringes
			for (i = 0; i < walls[floor].Count; i++)
			{
				if (walls[floor][i].doors.Count == 0) continue;
				// bottom face for windows
				if (walls[floor][i].doors[0].elevation > thresh)
				{
					verts = new List<Vertex>();
					verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[3]));
					verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[2]));
					verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[1]));
					verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[0]));
					walls[floor][i].casts.Add(new Plane(verts, verts));
					walls[floor][i].slabType[0]++;
					walls[floor][i].castType.Add(blocks.Length + 1);
					//wcuts++;
					wcuts += 4;
					totalw += 4;
				}
				else
				{
					wcuts += 2;
					totald += 2;
				}
				verts = new List<Vertex>();
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[7]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[6]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[5]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[4]));
				walls[floor][i].casts.Add(new Plane(verts, verts));
				walls[floor][i].slabType[0]++;
				walls[floor][i].castType.Add(blocks.Length + 1);
				//wcuts++;
				// 2 side faces
				verts = new List<Vertex>();
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[1]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[0]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[4]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[5]));
				walls[floor][i].casts.Add(new Plane(verts, verts));
				walls[floor][i].castType.Add(blocks.Length + 1);
				verts = new List<Vertex>();
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[3]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[2]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[6]));
				verts.Add(new Vertex(walls[floor][i].doors[0].cuboid.vertices[7]));
				walls[floor][i].casts.Add(new Plane(verts, verts));
				walls[floor][i].castType.Add(blocks.Length + 1);
				// height
				walls[floor][i].slabType[0] += 2;
				//wcuts += 2;
			}

			for (i = 0; i < walls[floor].Count; i++)
			{
				for (j = 0; j < walls[floor][i].casts.Count; j++)
					walls[floor][i].casts[j].UpdateVertexRelatedInfo();
			}
			return (wallMove);
		}

		List<int> indwalls;
		int[] types;
		double W1 = 0, W2 = 0, H1 = 0;
		public int ncuts, wcuts;

		public double PutSlab(int floor, int index)
		{
			// Calculate the area of each wall to put suitable number of casts
			// side = 1: inside wall
			// side = 2: outside wall

			List<Plane> backSlabs = new List<Plane>();
			List<int> backslabType = new List<int>();
            int[] nums = new int[blocks.Length+1], numf = new int[blocks.Length], numw = new int[3];
			int i, j, k = 0, prec = 100, total = 0, sum = 0;
			double distW = 0, ndw = 0, wid = 0, diff = 0, ww = 0, wallmove = 0;
			int label = -1; // XZ or YZ plane
			int num, v1 = -1, v2 = -1;
			bool iscut = false;
			indwalls = new List<int>();

			if (walls[floor][index].label != 1) return(0);
			num = walls[floor][index].cuboid.vertices.Count/2;

			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			// Length of outside wall:  Vertices 0 & num - 1
            W1 = (walls[floor][index].cuboid.vertices[0].Coord - walls[floor][index].cuboid.vertices[num - 1].Coord).Length();
			// Fin the other side of the wall: Vertices v1 & v2
			Vector3d vd1 = walls[floor][index].cuboid.vertices[num - 1].Coord - walls[floor][index].cuboid.vertices[0].Coord;
			vd1 = vd1.Normalize();
			for (i = 1; i < num - 1; i++)
			{
				j = (i + 1) % walls[floor][index].cuboid.vertices.Count;
				Vector3d vd2 = walls[floor][index].cuboid.vertices[j].Coord - walls[floor][index].cuboid.vertices[i].Coord;
				vd2 = vd2.Normalize();
				if (vd1.Cross(vd2).Length() < 1e-3)
				{
					v1 = i;
					v2 = j;
				}
			}
			/////// debug
			if (v1 == -1 || v2 == -1)
			{
				return(-1);
			}
			double W1_back = (walls[floor][index].cuboid.vertices[v1].Coord - walls[floor][index].cuboid.vertices[v2].Coord).Length();              
            
			// The smallest size of block needs to be the size of half thickness of the wall to make sure inside and outside
			// walls[floor] can be filled meantime
			H1 = walls[floor][index].cuboid.height;
			int h1 = (int)Math.Round(H1 * prec), hb = (int)Math.Round(slabHeight * prec), num2 = 0;
			if (Math.Abs(walls[floor][index].cuboid.endPoints[0].Coord.x - walls[floor][index].cuboid.endPoints[1].Coord.x) < thresh)
				label = 1; // Parallel to Y-axis
			else label = 2;

			// Step 1: I1 ---> I1' (width of the wall)
			if (walls[floor][index].doors.Count > 0)
			{
				distW = (walls[floor][index].doors[0].cuboid.vertices[1].Coord - walls[floor][index].doors[0].cuboid.vertices[2].Coord).Length();
				W1 -= ww;
				W1_back -= ww;
			}
			
			// It is recommended that the difference of two blocks is half the thickness of wall, e.g., 500mm, 600mm
			double difflen = Math.Abs(W1 - W1_back);
			if (W1 < blocks[0])
			{
				numf[0]++;
				W2 = blocks[0];
			}
			else
			{
				nums = PutRods(blocks, W1);
				for (i = 1; i < blocks.Length + 1; i++)
					numf[i - 1] = nums[i];
			}
			total = CalSum(numf);
			if (total < 2 && walls[floor][index].doors.Count > 0)
			{
				for (i = 0; i < numf.Length; i++)
					numf[i] = 0;
				numf[0] = 2;
				W2 = blocks[0] * 2;
			}
			total = CalSum(numf);
			// W1: original width/length of the wall
			// W2: after adjustment
			Vector3d vec, translate;
			double tol = W2 - W1;
			diff = Math.Abs(W2 - W1);
			
			bool hori = true;
			if (Math.Abs(walls[floor][index].endPoints[0].Coord.x - walls[floor][index].endPoints[1].Coord.x) < thresh)
				hori = false;

			// There exist some walls are deadlock: around walls have been traversed and changed,
			// so theier size can not be changed. Solution: select a slab to change the size/tolerance.
			bool dead = false;
			if (hori && diff > thresh)
			{
				if (walls[floor][index].visit_h[0] && walls[floor][index].visit_h[1])
					dead = true;
			}
			else if (!hori && diff > thresh)
			{
				if (walls[floor][index].visit_v[0] && walls[floor][index].visit_v[1])
					dead = true;
			}

			if (diff > thresh && !dead)
			{
				if ((hori && !walls[floor][index].visit_h[1]) ||
					(!hori && !walls[floor][index].visit_v[1]))// Move endpoint 1 first
				{
					vec = new Vector3d(walls[floor][index].cuboid.endPoints[1].Coord);
					translate = walls[floor][index].direction * (W2 - W1);
					walls[floor][index].cuboid.endPoints[1].Coord += translate;

					int count = walls[floor][index].cuboid.planeVertices.Count;
					walls[floor][index].cuboid.planeVertices[count - 1].Coord += translate;
					walls[floor][index].cuboid.planeVertices[count - 2].Coord += translate;
					if (walls[floor][index].vertexType[1] == 2)
						walls[floor][index].cuboid.planeVertices[count - 3].Coord += translate;
					walls[floor][index].cuboid.UpdatePlaneVertices();
					ReBuildWalls(floor, vec, label, translate, index);
				}
				else if ((hori && !walls[floor][index].visit_h[0]) ||
					(!hori && !walls[floor][index].visit_v[0]))
				{
					vec = new Vector3d(walls[floor][index].cuboid.endPoints[0].Coord);
					translate = walls[floor][index].direction * (W1 - W2);
					walls[floor][index].cuboid.endPoints[0].Coord += translate;

					walls[floor][index].cuboid.planeVertices[0].Coord += translate;
					walls[floor][index].cuboid.planeVertices[1].Coord += translate;
					if (walls[floor][index].vertexType[0] == 2)
						walls[floor][index].cuboid.planeVertices[2].Coord += translate;
					walls[floor][index].cuboid.UpdatePlaneVertices();
					ReBuildWalls(floor, vec, label, translate, index);
				}
				wallmove = diff;
				diff = 0; // the wall is translated
				tol = 0;
			}
			if (hori) // horizontal move fixed
			{
				walls[floor][index].visit_h[0] = true;
				walls[floor][index].visit_h[1] = true;
			}
			else	// vertical move fixed
			{
				walls[floor][index].visit_v[0] = true;
				walls[floor][index].visit_v[1] = true;
			}
			for (i = 0; i < walls[floor].Count; i++)
			{
				if (i == index) continue;
				for (j = 0; j < 2; j++) // once the endpoint is moved, it will be fixed
				{
					if ((walls[floor][index].endPoints[j].Coord - walls[floor][index].endPoints[0].Coord).Length() < thresh ||
						(walls[floor][index].endPoints[j].Coord - walls[floor][index].endPoints[1].Coord).Length() < thresh)
					{
						if (hori)
							walls[floor][index].visit_h[j] = true;
						else
							walls[floor][index].visit_v[j] = true;
					}
				}
			}

			// Step 2: I2 ---> I2' (slabHeight of the wall): same slabHeight of blocks
			if (index == 0) // uniform the wall height
				for (i = 0; i < walls[floor].Count; i++)
				{
					walls[floor][index].cuboid.height = slabHeight;
					walls[floor][index].UpdateWall(mat);
				}
			// Step 3: Snap windows or doors
			double[] useblocks = new double[total];
			int[] numblocks;
			double[] leftblocks = new double[total];
			int n0 = 0, n2 = num2; // n0: left dist (x/y), n1: below window (z), n2: beyond window (z)
			double[] cx2; // Occupied X to left and current window 
			int c2 = 0;
			double startx = 0, starty = 0, direc = 0, 
				   startx_back = 0, starty_back = 0;
			List<Vertex> vertices;

			startx = walls[floor][index].cuboid.vertices[0].Coord.x;	// start coordinate of x
			starty = walls[floor][index].cuboid.vertices[0].Coord.y;	// start coordinate of y
			startx_back = walls[floor][index].cuboid.vertices[v1].Coord.x;
			starty_back = walls[floor][index].cuboid.vertices[v1].Coord.y;
			if (label == 1)
				direc = walls[floor][index].cuboid.direction.y;
			else
				direc = walls[floor][index].cuboid.direction.x;
			for (i = 0; i < blocks.Length; i++)
			{
				if (k >= total) break;
				for (j = 0; j < numf[i]; j++)
				{
					if (k >= total) break;
					useblocks[k] = blocks[i];
					leftblocks[k++] = blocks[i];
				}
			}

			// label = 1: parallel to Y; label = 0: parallel to X
			// no order here -- only one door/window on one wall
			for (i = 0; i < walls[floor][index].doors.Count; i++)
			{
				// Horizontal location of the window
				wid = (walls[floor][index].doors[i].cuboid.vertices[3].Coord - walls[floor][index].doors[i].cuboid.vertices[0].Coord).Length();
				numblocks = new int[leftblocks.Count()];
				if (i == 0)	// the distance between the left of the wall and the door
				{
					if (label == 1)
						distW = walls[floor][index].doors[i].cuboid.vertices[0].Coord.y - walls[floor][index].cuboid.vertices[0].Coord.y;
					else
						distW = walls[floor][index].doors[i].cuboid.vertices[0].Coord.x - walls[floor][index].cuboid.vertices[0].Coord.x;
				}
				else // the distance between two door(s)/window(s)
				{
					if (label == 1)
					{
						distW = walls[floor][index].doors[i].cuboid.vertices[0].Coord.y - walls[floor][index].doors[i - 1].cuboid.vertices[3].Coord.y;
						startx = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.x;
						starty = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.y;
					}
					else
					{
						distW = walls[floor][index].doors[i].cuboid.vertices[0].Coord.x - walls[floor][index].doors[i - 1].cuboid.vertices[3].Coord.x;
						startx = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.x;
						starty = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.y;
					}
				}
				
				c2 = 0;
				cx2 = new double[0];

				ndw = 0;
				distW = Math.Abs(distW);
				numblocks = FitWindow(distW, leftblocks);
				sum = CalSum(numblocks);

				if (sum == 0)
					numblocks[0] = 1;

				n0 = sum;
				cx2 = new double[sum];
				useblocks = new double[leftblocks.Count() - sum];
				ndw = 0;
				k = 0;
				for (j = 0; j < leftblocks.Count(); j++)
				{
					if (numblocks[j] != 0)
					{
						ndw += leftblocks[j];
						cx2[c2++] = leftblocks[j];
					}
					else
						useblocks[k++] = leftblocks[j];
				}
				tol = ndw - distW;
				diff = Math.Abs(tol);
				if (leftblocks.Length > 2)
				{
					walls[floor][index].doors[i].cuboid.endPoints[0].Coord += walls[floor][index].doors[i].cuboid.direction * (ndw - distW);
					walls[floor][index].doors[i].cuboid.endPoints[1].Coord += walls[floor][index].doors[i].cuboid.direction * (ndw - distW);
					walls[floor][index].doors[i].UpdateDoor(mat);
					tol = 0;
					diff = 0;
				}
				
				//Add casts
				// n0: left dist (x/y), n1: below window (z), n2: beyond window (z)
				// c0: how many in one column, c2: to the left window/wall c1: current window
				
				if (label == 1)
				{
					for (int p = 0; p < c2; p++)
					{
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty, 0));
						vertices.Add(new Vertex(startx, starty, slabHeight));
						vertices.Add(new Vertex(startx, starty + cx2[p] * direc, slabHeight));
						vertices.Add(new Vertex(startx, starty + cx2[p] * direc, 0));
						starty += cx2[p] * direc;
						walls[floor][index].castType.Add(0);
						iscut = false;
						if (diff > thresh && p == c2 - 1)
						{
							starty -= tol * direc;
							vertices[2].Coord.y = starty;
							vertices[3].Coord.y = starty;
							ncuts++;
							iscut = true;
							walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
						}
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						
						for (int r = 0; r < blocks.Length; r++)
							if (Math.Abs(cx2[p] - blocks[r]) < thresh)
							{
								walls[floor][index].slabType[r]++;
								if (!iscut)
									walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
								break;
							}
					}
					double height = walls[floor][index].doors[0].elevation;
					if (height > thresh)
					{
						// below the window
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty, 0));
						vertices.Add(new Vertex(startx, starty, height));
						vertices.Add(new Vertex(startx, starty + wid * direc, height));
						vertices.Add(new Vertex(startx, starty + wid * direc, 0));
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						// for the back side wall
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx_back, starty, 0));
						vertices.Add(new Vertex(startx_back, starty, height));
						vertices.Add(new Vertex(startx_back, starty + wid * direc, height));
						vertices.Add(new Vertex(startx_back, starty + wid * direc, 0));
						backSlabs.Add(new Plane(vertices, vertices));
						for (int r = 0; r < blocks.Length; r++)
							if ((wid <= blocks[r] && height <= slabHeight) ||
								(height <= blocks[r] && wid <= slabHeight))
							{
								walls[floor][index].slabType[r] += 2;
								walls[floor][index].castType.Add(blocks.Length);
								backslabType.Add(blocks.Length);
								break;
							}
					}
					// beyond the window/door
					height = walls[floor][index].doors[0].elevation + walls[floor][index].doors[0].height;
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty, height));
					vertices.Add(new Vertex(startx, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx, starty + wid * direc, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx, starty + wid * direc, height));
					walls[floor][index].casts.Add(new Plane(vertices, vertices));
					// for the back side wall
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx_back, starty, height));
					vertices.Add(new Vertex(startx_back, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx_back, starty + wid * direc, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx_back, starty + wid * direc, height));
					backSlabs.Add(new Plane(vertices, vertices));
					height = walls[floor][index].cuboid.height - height;
					for (int r = 0; r < blocks.Length; r++)
						if ((wid <= blocks[r] && height <= slabHeight) ||
							(height <= blocks[r] && wid <= slabHeight))
						{
							walls[floor][index].slabType[r] += 2;
							walls[floor][index].castType.Add(blocks.Length);
							backslabType.Add(blocks.Length);
							break;
						}
					starty += wid * direc;
				}
				else // Parallel to X-axis
				{
					for (int p = 0; p < c2; p++)
					{
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty, 0));
						vertices.Add(new Vertex(startx, starty, slabHeight));
						vertices.Add(new Vertex(startx + cx2[p] * direc, starty, slabHeight));
						vertices.Add(new Vertex(startx + cx2[p] * direc, starty, 0));
						startx += cx2[p] * direc;
						walls[floor][index].castType.Add(0);
						iscut = false;
						if (diff > thresh && p == c2 - 1)
						{
							startx -= tol * direc;
							vertices[2].Coord.x = startx;
							vertices[3].Coord.x = startx;
							ncuts++;
							iscut = true;
							walls[floor][index].castType[walls[floor][index].castType.Count-1] = blocks.Length + 1; // cut
						}
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						for (int r = 0; r < blocks.Length; r++)
							if (Math.Abs(cx2[p] - blocks[r]) < thresh)
							{
								walls[floor][index].slabType[r]++;
								if (!iscut)
									walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
								break;
							}
					}
					double height = walls[floor][index].doors[0].elevation;
					if (height > thresh)
					{
						// below the window
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty, 0));
						vertices.Add(new Vertex(startx, starty, height));
						vertices.Add(new Vertex(startx + wid * direc, starty, height));
						vertices.Add(new Vertex(startx + wid * direc, starty, 0));
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						
						// back side of wall
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty_back, 0));
						vertices.Add(new Vertex(startx, starty_back, height));
						vertices.Add(new Vertex(startx + wid * direc, starty_back, height));
						vertices.Add(new Vertex(startx + wid * direc, starty_back, 0));
						backSlabs.Add(new Plane(vertices, vertices));
						for (int r = 0; r < blocks.Length; r++)
							if ((wid <= blocks[r] && height <= slabHeight) ||
								(height <= blocks[r] && wid <= slabHeight))
							{
								walls[floor][index].slabType[r] += 2;
								backslabType.Add(blocks.Length);
								walls[floor][index].castType.Add(blocks.Length);
								break;
							}
					}
					// beyond the window/door
					height = walls[floor][index].doors[0].elevation + walls[floor][index].doors[0].height;
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty, height));
					vertices.Add(new Vertex(startx, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx + wid * direc, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx + wid * direc, starty, height));
					walls[floor][index].casts.Add(new Plane(vertices, vertices));
					// back wall
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty_back, height));
					vertices.Add(new Vertex(startx, starty_back, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx + wid * direc, starty_back, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx + wid * direc, starty_back, height));
					backSlabs.Add(new Plane(vertices, vertices));
					for (int r = 0; r < blocks.Length; r++)
						if ((wid <= blocks[r] && height <= slabHeight) ||
							(height <= blocks[r] && wid <= slabHeight))
						{
							walls[floor][index].slabType[r] += 2;
							walls[floor][index].castType.Add(blocks.Length);
							backslabType.Add(blocks.Length);
							break;
						}
					startx += wid * direc;
				}
				
			}
			// Add the last casts
			if (walls[floor][index].doors.Count > 0)
			{
				if (label == 1)
					distW = Math.Abs(walls[floor][index].cuboid.vertices[num - 1].Coord.y - walls[floor][index].doors[0].cuboid.vertices[3].Coord.y);
				else
					distW = Math.Abs(walls[floor][index].cuboid.vertices[num - 1].Coord.x - walls[floor][index].doors[0].cuboid.vertices[3].Coord.x);
				startx = walls[floor][index].doors[0].cuboid.vertices[3].Coord.x;
				starty = walls[floor][index].doors[0].cuboid.vertices[3].Coord.y;
				if (distW < blocks[0])
				{
					numf = new int[blocks.Length];
					numf[0] = 1;
					W2 = blocks[0];
				}
				else
				{
					nums = PutRods(blocks, distW);
					for (j = 1; j < blocks.Length + 1; j++)
						numf[j - 1] = nums[j];
				}
				tol = W2 - distW;
				diff = Math.Abs(tol);
				useblocks = new double[Sum(numf)];
				k = 0;
				for (j = 0; j < blocks.Length; j++)
					for (int t = 0; t < numf[j]; t++)
						useblocks[k++] = blocks[j];
			}

			if (label == 1)
			{
				for (int p = 0; p < useblocks.Count(); p++)
				{
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty, 0));
					vertices.Add(new Vertex(startx, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx, starty + useblocks[p] * direc, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx, starty + useblocks[p] * direc, 0));
					walls[floor][index].castType.Add(0);
					iscut = false;
					if (diff > thresh && p == useblocks.Count() - 1)
					{
						vertices[2].Coord.y -= tol * direc;
						vertices[3].Coord.y -= tol * direc;
						ncuts++;
						iscut = true;
						walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
					}
					
					walls[floor][index].casts.Add(new Plane(vertices, vertices));
					for (int r = 0; r < blocks.Length; r++)
						if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
						{
							walls[floor][index].slabType[r]++;
							if (!iscut)
								walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
							break;
						}
					starty = vertices[3].Coord.y;
				}
				
			}
			else
			{
				for (int p = 0; p < useblocks.Count(); p++)
				{
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty, 0));
					vertices.Add(new Vertex(startx, starty, slabHeight));
					vertices.Add(new Vertex(startx + useblocks[p] * direc, starty, slabHeight));
					vertices.Add(new Vertex(startx + useblocks[p] * direc, starty, 0));
					walls[floor][index].castType.Add(0);
					iscut = false;
					if (diff > thresh && p == useblocks.Count() - 1)
					{
						vertices[2].Coord.x -= tol * direc;
						vertices[3].Coord.x -= tol * direc;
						ncuts++;
						iscut = true;
						walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
					}
					walls[floor][index].casts.Add(new Plane(vertices, vertices));
					for (int r = 0; r < 3; r++)
						if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
						{
							walls[floor][index].slabType[r]++;
							if (!iscut)
								walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
							break;
						}
					startx = vertices[3].Coord.x;
				}
			}
			walls[floor][index].UpdateWall(mat);
			// The other side of the wall
			// Since the parts with window(s)/door(s) are already casted, only cosider the vacant wall
			double length = 0;
			walls[floor][index].num_front_cast = walls[floor][index].casts.Count;
			for (i = 0; i < walls[floor][index].doors.Count + 1; i++)
			{
				if (i > 0 && walls[floor][index].doors.Count > 0)
				{
					if (label == 1)
						starty_back = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.y;
					else
						startx_back = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.x;
				}
				if (walls[floor][index].doors.Count == 0)
					length = (walls[floor][index].cuboid.vertices[v2].Coord - walls[floor][index].cuboid.vertices[v1].Coord).Length();
				else
				{
					if (i == 0)
						if (label == 1)
							length = Math.Abs(walls[floor][index].doors[0].cuboid.vertices[1].Coord.y - walls[floor][index].cuboid.vertices[v1].Coord.y);
						else
							length = Math.Abs(walls[floor][index].doors[0].cuboid.vertices[1].Coord.x - walls[floor][index].cuboid.vertices[v1].Coord.x);
					else 
						if (label == 1)
							length = Math.Abs(walls[floor][index].doors[0].cuboid.vertices[2].Coord.y
							- walls[floor][index].cuboid.vertices[v2].Coord.y);
						else
							length = Math.Abs(walls[floor][index].doors[0].cuboid.vertices[2].Coord.x
							- walls[floor][index].cuboid.vertices[v2].Coord.x);
				}

				if (length < blocks[0])
				{
					numf = new int[blocks.Length];
					numf[0]++;
					W2 = blocks[0];
				}
				else
				{
					nums = PutRods(blocks, length);
					for (j = 1; j < blocks.Length + 1; j++)
						numf[j - 1] = nums[j];
				}
				tol = W2 - length;
				diff = Math.Abs(tol);
				useblocks = new double[Sum(numf)];
				k = 0;
				for (j = 0; j < blocks.Length; j++)
					for (int t = 0; t < numf[j]; t++)
						useblocks[k++] = blocks[j];
				if (label == 1)
				{
					for (int p = 0; p < useblocks.Count(); p++)
					{
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx_back, starty_back, 0));
						vertices.Add(new Vertex(startx_back, starty_back, walls[floor][index].cuboid.height));
						vertices.Add(new Vertex(startx_back, starty_back + useblocks[p] * direc, walls[floor][index].cuboid.height));
						vertices.Add(new Vertex(startx_back, starty_back + useblocks[p] * direc, 0));
						walls[floor][index].castType.Add(0);
						iscut = false;
						if (diff > thresh && p == useblocks.Count() - 1)
						{
							vertices[2].Coord.y -= tol * direc;
							vertices[3].Coord.y -= tol * direc;
							ncuts++;
							iscut = true;
							walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
						}

						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						for (int r = 0; r < 3; r++)
							if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
							{
								walls[floor][index].slabType[r]++;
								if (!iscut)
									walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
								break;
							}
						starty_back = vertices[3].Coord.y;
					}
				}
				else // Parallel to X-axis
				{
					for (int p = 0; p < useblocks.Count(); p++)
					{
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx_back, starty_back, 0));
						vertices.Add(new Vertex(startx_back, starty_back, walls[floor][index].cuboid.height));
						vertices.Add(new Vertex(startx_back + useblocks[p] * direc, starty_back, walls[floor][index].cuboid.height));
						vertices.Add(new Vertex(startx_back + useblocks[p] * direc, starty_back, 0));
						walls[floor][index].castType.Add(0);
						iscut = false;
						if (diff > thresh && p == useblocks.Count() - 1)
						{
							vertices[2].Coord.x -= tol * direc;
							vertices[3].Coord.x -= tol * direc;
							ncuts++;
							iscut = true;
							walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
						}
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						for (int r = 0; r < 3; r++)
							if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
							{
								walls[floor][index].slabType[r]++;
								if (!iscut)
									walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
								break;
							}
						startx_back = vertices[3].Coord.x;
					}
				}
			}
			if (backSlabs.Count > 0)
			{
				for (i = 0; i < backSlabs.Count; i++)
				{
					walls[floor][index].casts.Add(backSlabs[i]);
					walls[floor][index].castType.Add(backslabType[i]);
				}
			}
			return (wallmove);
		}

		public double TraditionPutSlab(int floor, int index)
		{
			// Calculate the area of each wall to put suitable number of casts
			// side = 1: inside wall
			// side = 2: outside wall

			List<Plane> backSlabs = new List<Plane>();
			int[] nums = new int[blocks.Length + 1], numf = new int[blocks.Length], numw = new int[3];
			int i, j, k = 0, prec = 100, total = 0, sum = 0;
			double distW = 0, wid = 0, diff = 0;
			int label = -1; // XZ or YZ plane
			int num, v1 = -1, v2 = -1;
			bool iscut;
			indwalls = new List<int>();

			if (walls[floor][index].label != 1) return (0);
			num = walls[floor][index].cuboid.vertices.Count / 2;

			List<int> backslabType = new List<int>();
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			// Length of outside wall:  Vertices 0 & num - 1
			W1 = (walls[floor][index].cuboid.vertices[0].Coord - walls[floor][index].cuboid.vertices[num - 1].Coord).Length();
			// Fin the other side of the wall: Vertices v1 & v2
			Vector3d vd1 = walls[floor][index].cuboid.vertices[num - 1].Coord - walls[floor][index].cuboid.vertices[0].Coord;
			vd1 = vd1.Normalize();
			for (i = 1; i < num - 1; i++)
			{
				j = (i + 1) % walls[floor][index].cuboid.vertices.Count;
				Vector3d vd2 = walls[floor][index].cuboid.vertices[j].Coord - walls[floor][index].cuboid.vertices[i].Coord;
				vd2 = vd2.Normalize();
				if (vd1.Cross(vd2).Length() < 1e-3)
				{
					v1 = i;
					v2 = j;
				}
			}
			/////// debug
			if (v1 == -1 || v2 == -1)
			{
				return (-1);
			}
			double W1_back = (walls[floor][index].cuboid.vertices[v1].Coord - walls[floor][index].cuboid.vertices[v2].Coord).Length();

			// The smallest size of block needs to be the size of half thickness of the wall to make sure inside and outside
			// walls[floor] can be filled meantime
			H1 = walls[floor][index].cuboid.height;
			int h1 = (int)Math.Round(H1 * prec), hb = (int)Math.Round(slabHeight * prec), num2 = 0;
			if (Math.Abs(walls[floor][index].cuboid.endPoints[0].Coord.x - walls[floor][index].cuboid.endPoints[1].Coord.x) < thresh)
				label = 1; // Parallel to Y-axis
			else label = 2;

			if (W1 < blocks[0])
			{
				numf[0]++;
				W2 = blocks[0];
			}
			else
			{
				nums = PutRods(blocks, W1);
				for (i = 1; i < blocks.Length + 1; i++)
					numf[i - 1] = nums[i];
			}
			// W1: original width/length of the wall
			// W2: after adjustment
			double tol = W2 - W1;
			diff = Math.Abs(W2 - W1);

			// Step 2: I2 ---> I2' (slabHeight of the wall): same slabHeight of blocks
			if (index == 0) // uniform the wall height
				for (i = 0; i < walls[floor].Count; i++)
				{
					walls[floor][index].cuboid.height = slabHeight;
					walls[floor][index].UpdateWall(mat);
				}
			// Step 3: Snap windows or doors
			double[] useblocks = new double[total];
			double[] leftblocks = new double[total];
			int n2 = num2; // n0: left dist (x/y), n1: below window (z), n2: beyond window (z)
			double startx = 0, starty = 0, direc = 0,
				   startx_back = 0, starty_back = 0;
			List<Vertex> vertices;

			sum = Sum(numf);
			useblocks = new double[sum];
			k = 0;
			for (int t = 0; t < numf.Length; t++)
				for (j = 0; j < numf[t]; j++)
					useblocks[k++] = blocks[t];

			startx = walls[floor][index].cuboid.vertices[0].Coord.x;	// start coordinate of x
			starty = walls[floor][index].cuboid.vertices[0].Coord.y;	// start coordinate of y
			startx_back = walls[floor][index].cuboid.vertices[v1].Coord.x;
			starty_back = walls[floor][index].cuboid.vertices[v1].Coord.y;
			if (label == 1)
				direc = walls[floor][index].cuboid.direction.y;
			else
				direc = walls[floor][index].cuboid.direction.x;

			// label = 1: parallel to Y; label = 0: parallel to X
			// no order here -- only one door/window on one wall
			for (i = 0; i < walls[floor][index].doors.Count; i++)
			{
				// Horizontal location of the window
				wid = (walls[floor][index].doors[i].cuboid.vertices[3].Coord - walls[floor][index].doors[i].cuboid.vertices[0].Coord).Length();
				if (i == 0)	// the distance between the left of the wall and the door
				{
					if (label == 1)
						distW = walls[floor][index].doors[i].cuboid.vertices[0].Coord.y - walls[floor][index].cuboid.vertices[0].Coord.y;
					else
						distW = walls[floor][index].doors[i].cuboid.vertices[0].Coord.x - walls[floor][index].cuboid.vertices[0].Coord.x;
				}
				else // the distance between two door(s)/window(s)
				{
					if (label == 1)
					{
						distW = walls[floor][index].doors[i].cuboid.vertices[0].Coord.y - walls[floor][index].doors[i - 1].cuboid.vertices[3].Coord.y;
						startx = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.x;
						starty = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.y;
					}
					else
					{
						distW = walls[floor][index].doors[i].cuboid.vertices[0].Coord.x - walls[floor][index].doors[i - 1].cuboid.vertices[3].Coord.x;
						startx = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.x;
						starty = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.y;
					}
				}
				distW = Math.Abs(distW);
				if (distW < blocks[0])
				{
					useblocks = new double[1];
					useblocks[0] = blocks[0];
					W2 = useblocks[0];
				}
				else
				{
					nums = PutRods(blocks, distW);
					for (int t = 1; t < blocks.Length + 1; t++)
						numf[t - 1] = nums[t];
					sum = Sum(numf);
					useblocks = new double[sum];
					k = 0;
					W2 = 0;
					for (int t = 0; t < numf.Length; t++)
						for (j = 0; j < numf[t]; j++)
						{
							useblocks[k++] = blocks[t];
							W2 += blocks[t];
						}
				}
				tol = W2 - distW;
				diff = Math.Abs(tol);
				//Add casts
				// n0: left dist (x/y), n1: below window (z), n2: beyond window (z)
				// c0: how many in one column, c2: to the left window/wall c1: current window
				if (label == 1)
				{
					for (int p = 0; p < useblocks.Length; p++)
					{
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty, 0));
						vertices.Add(new Vertex(startx, starty, slabHeight));
						vertices.Add(new Vertex(startx, starty + useblocks[p] * direc, slabHeight));
						vertices.Add(new Vertex(startx, starty + useblocks[p] * direc, 0));
						starty += useblocks[p] * direc;
						iscut = false;
						walls[floor][index].castType.Add(0);
						if (diff > thresh && p == useblocks.Length - 1)
						{
							starty -= tol * direc;
							vertices[2].Coord.y = starty;
							vertices[3].Coord.y = starty;
							ncuts++;
							iscut = true;
							walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
						}
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						for (int r = 0; r < blocks.Length; r++)
							if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
							{
								walls[floor][index].slabType[r]++;
								if (!iscut)
									walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
								break;
							}
					}
					double height = walls[floor][index].doors[0].elevation;
					if (height > thresh)
					{
						// below the window
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty, 0));
						vertices.Add(new Vertex(startx, starty, height));
						vertices.Add(new Vertex(startx, starty + wid * direc, height));
						vertices.Add(new Vertex(startx, starty + wid * direc, 0));
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						// for the back side wall
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx_back, starty, 0));
						vertices.Add(new Vertex(startx_back, starty, height));
						vertices.Add(new Vertex(startx_back, starty + wid * direc, height));
						vertices.Add(new Vertex(startx_back, starty + wid * direc, 0));
						backSlabs.Add(new Plane(vertices, vertices));
						for (int r = 0; r < blocks.Length; r++)
							if ((wid <= blocks[r] && height <= slabHeight) ||
								(height <= blocks[r] && wid <= slabHeight))
							{
								walls[floor][index].slabType[r] += 2;
								walls[floor][index].castType.Add(blocks.Length);
								backslabType.Add(blocks.Length);
								break;
							}
					}
					// beyond the window/door
					height = walls[floor][index].doors[0].elevation + walls[floor][index].doors[0].height;
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty, height));
					vertices.Add(new Vertex(startx, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx, starty + wid * direc, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx, starty + wid * direc, height));
					walls[floor][index].casts.Add(new Plane(vertices, vertices));
					// for the back side wall
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx_back, starty, height));
					vertices.Add(new Vertex(startx_back, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx_back, starty + wid * direc, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx_back, starty + wid * direc, height));
					backSlabs.Add(new Plane(vertices, vertices));
					height = walls[floor][index].cuboid.height - height;
					for (int r = 0; r < blocks.Length; r++)
						if ((wid <= blocks[r] && height <= slabHeight) ||
							(height <= blocks[r] && wid <= slabHeight))
						{
							walls[floor][index].slabType[r] += 2;
							walls[floor][index].castType.Add(blocks.Length);
							backslabType.Add(blocks.Length);
							break;
						}
					starty += wid * direc;
				}
				else // Parallel to X-axis
				{
					for (int p = 0; p < useblocks.Length; p++)
					{
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty, 0));
						vertices.Add(new Vertex(startx, starty, slabHeight));
						vertices.Add(new Vertex(startx + useblocks[p] * direc, starty, slabHeight));
						vertices.Add(new Vertex(startx + useblocks[p] * direc, starty, 0));
						startx += useblocks[p] * direc;
						walls[floor][index].castType.Add(0);
						iscut = false;
						if (diff > thresh && p == useblocks.Length - 1)
						{
							startx -= tol * direc;
							vertices[2].Coord.x = startx;
							vertices[3].Coord.x = startx;
							ncuts++;
							iscut = true;
							walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1; // cut
						}

						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						for (int r = 0; r < blocks.Length; r++)
							if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
							{
								walls[floor][index].slabType[r]++;
								if (!iscut)
									walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
								break;
							}
						
					}
					double height = walls[floor][index].doors[0].elevation;
					if (height > thresh)
					{
						// below the window
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty, 0));
						vertices.Add(new Vertex(startx, starty, height));
						vertices.Add(new Vertex(startx + wid * direc, starty, height));
						vertices.Add(new Vertex(startx + wid * direc, starty, 0));
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						// back side of wall
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx, starty_back, 0));
						vertices.Add(new Vertex(startx, starty_back, height));
						vertices.Add(new Vertex(startx + wid * direc, starty_back, height));
						vertices.Add(new Vertex(startx + wid * direc, starty_back, 0));
						backSlabs.Add(new Plane(vertices, vertices));
						for (int r = 0; r < blocks.Length; r++)
							if ((wid <= blocks[r] && height <= slabHeight) ||
								(height <= blocks[r] && wid <= slabHeight))
							{
								walls[floor][index].slabType[r] += 2;
								walls[floor][index].castType.Add(blocks.Length);
								backslabType.Add(blocks.Length);
								break;
							}
					}
					// beyond the window/door
					height = walls[floor][index].doors[0].elevation + walls[floor][index].doors[0].height;
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty, height));
					vertices.Add(new Vertex(startx, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx + wid * direc, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx + wid * direc, starty, height));
					walls[floor][index].casts.Add(new Plane(vertices, vertices));
					// back wall
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty_back, height));
					vertices.Add(new Vertex(startx, starty_back, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx + wid * direc, starty_back, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx + wid * direc, starty_back, height));
					backSlabs.Add(new Plane(vertices, vertices));
					for (int r = 0; r < blocks.Length; r++)
						if ((wid <= blocks[r] && height <= slabHeight) ||
							(height <= blocks[r] && wid <= slabHeight))
						{
							walls[floor][index].slabType[r] += 2;
							walls[floor][index].castType.Add(blocks.Length);
							backslabType.Add(blocks.Length);
							break;
						}
					startx += wid * direc;
				}

			}
			// Add the last casts
			if (walls[floor][index].doors.Count > 0)
			{
				if (label == 1)
					distW = Math.Abs(walls[floor][index].cuboid.vertices[num - 1].Coord.y - walls[floor][index].doors[0].cuboid.vertices[3].Coord.y);
				else
					distW = Math.Abs(walls[floor][index].cuboid.vertices[num - 1].Coord.x - walls[floor][index].doors[0].cuboid.vertices[3].Coord.x);
				startx = walls[floor][index].doors[0].cuboid.vertices[3].Coord.x;
				starty = walls[floor][index].doors[0].cuboid.vertices[3].Coord.y;
				if (distW < blocks[0])
				{
					numf = new int[blocks.Length];
					numf[0] = 1;
					W2 = blocks[0];
				}
				else
				{
					nums = PutRods(blocks, distW);
					for (j = 1; j < blocks.Length + 1; j++)
						numf[j - 1] = nums[j];
				}
				tol = W2 - distW;
				diff = Math.Abs(tol);
				useblocks = new double[Sum(numf)];
				k = 0;
				for (j = 0; j < blocks.Length; j++)
					for (int t = 0; t < numf[j]; t++)
						useblocks[k++] = blocks[j];
			}

			if (label == 1)
			{
				for (int p = 0; p < useblocks.Count(); p++)
				{
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty, 0));
					vertices.Add(new Vertex(startx, starty, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx, starty + useblocks[p] * direc, walls[floor][index].cuboid.height));
					vertices.Add(new Vertex(startx, starty + useblocks[p] * direc, 0));
					starty = vertices[3].Coord.y;
					walls[floor][index].castType.Add(0);
					iscut = false;
					if (diff > thresh && p == useblocks.Length - 1)
					{
						starty -= tol * direc;
						vertices[2].Coord.y = starty;
						vertices[3].Coord.y = starty;
						iscut = true;
						walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
					}
					walls[floor][index].casts.Add(new Plane(vertices, vertices));
					for (int r = 0; r < blocks.Length; r++)
						if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
						{
							walls[floor][index].slabType[r]++;
							if (!iscut)
								walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
							break;
						}
				}
			}
			else
			{
				for (int p = 0; p < useblocks.Count(); p++)
				{
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(startx, starty, 0));
					vertices.Add(new Vertex(startx, starty, slabHeight));
					vertices.Add(new Vertex(startx + useblocks[p] * direc, starty, slabHeight));
					vertices.Add(new Vertex(startx + useblocks[p] * direc, starty, 0));
					startx = vertices[3].Coord.x;
					walls[floor][index].castType.Add(0);
					iscut = false;
					if (diff > thresh && p == useblocks.Length - 1)
					{
						startx -= tol * direc;
						vertices[2].Coord.x = startx;
						vertices[3].Coord.x = startx;
						ncuts++;
						iscut = true;
						walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
					}
					walls[floor][index].casts.Add(new Plane(vertices, vertices));
					for (int r = 0; r < 3; r++)
						if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
						{
							walls[floor][index].slabType[r]++;
							if (!iscut)
								walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
							break;
						}
				}
			}
			walls[floor][index].UpdateWall(mat);
			// The other side of the wall
			// Since the parts with window(s)/door(s) are already casted, only cosider the vacant wall
			double length = 0;
			walls[floor][index].num_front_cast = walls[floor][index].casts.Count;
			for (i = 0; i < walls[floor][index].doors.Count + 1; i++)
			{
				if (i > 0 && walls[floor][index].doors.Count > 0)
				{
					if (label == 1)
						starty_back = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.y;
					else
						startx_back = walls[floor][index].doors[i - 1].cuboid.vertices[2].Coord.x;
				}
				if (walls[floor][index].doors.Count == 0)
					length = (walls[floor][index].cuboid.vertices[v2].Coord - walls[floor][index].cuboid.vertices[v1].Coord).Length();
				else
				{
					if (i == 0)
						if (label == 1)
							length = Math.Abs(walls[floor][index].doors[0].cuboid.vertices[1].Coord.y - walls[floor][index].cuboid.vertices[v1].Coord.y);
						else
							length = Math.Abs(walls[floor][index].doors[0].cuboid.vertices[1].Coord.x - walls[floor][index].cuboid.vertices[v1].Coord.x);
					else
						if (label == 1)
							length = Math.Abs(walls[floor][index].doors[0].cuboid.vertices[2].Coord.y
							- walls[floor][index].cuboid.vertices[v2].Coord.y);
						else
							length = Math.Abs(walls[floor][index].doors[0].cuboid.vertices[2].Coord.x
							- walls[floor][index].cuboid.vertices[v2].Coord.x);
				}

				if (length < blocks[0])
				{
					numf = new int[blocks.Length];
					numf[0]++;
					W2 = blocks[0];
				}
				else
				{
					nums = PutRods(blocks, length);
					for (j = 1; j < blocks.Length + 1; j++)
						numf[j - 1] = nums[j];
				}
				tol = W2 - length;
				diff = Math.Abs(tol);
				useblocks = new double[Sum(numf)];
				k = 0;
				for (j = 0; j < blocks.Length; j++)
					for (int t = 0; t < numf[j]; t++)
						useblocks[k++] = blocks[j];
				if (label == 1)
				{
					for (int p = 0; p < useblocks.Count(); p++)
					{
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx_back, starty_back, 0));
						vertices.Add(new Vertex(startx_back, starty_back, walls[floor][index].cuboid.height));
						vertices.Add(new Vertex(startx_back, starty_back + useblocks[p] * direc, walls[floor][index].cuboid.height));
						vertices.Add(new Vertex(startx_back, starty_back + useblocks[p] * direc, 0));
						walls[floor][index].castType.Add(0);
						iscut = false;
						if (diff > thresh && p == useblocks.Length - 1)
						{
							vertices[2].Coord.y -= tol * direc;
							vertices[3].Coord.y = vertices[2].Coord.y;
							ncuts++;
							iscut = true;
							walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
						}
						starty_back = vertices[3].Coord.y;
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						for (int r = 0; r < 3; r++)
							if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
							{
								walls[floor][index].slabType[r]++;
								if (!iscut)
									walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
								break;
							}
						
					}
				}
				else // Parallel to X-axis
				{
					for (int p = 0; p < useblocks.Count(); p++)
					{
						vertices = new List<Vertex>();
						vertices.Add(new Vertex(startx_back, starty_back, 0));
						vertices.Add(new Vertex(startx_back, starty_back, walls[floor][index].cuboid.height));
						vertices.Add(new Vertex(startx_back + useblocks[p] * direc, starty_back, walls[floor][index].cuboid.height));
						vertices.Add(new Vertex(startx_back + useblocks[p] * direc, starty_back, 0));
						walls[floor][index].castType.Add(0);
						iscut = false;
						if (diff > thresh && p == useblocks.Length - 1)
						{
							vertices[2].Coord.x -= tol * direc;
							vertices[3].Coord.x = vertices[2].Coord.x;
							ncuts++;
							iscut = true;
							walls[floor][index].castType[walls[floor][index].castType.Count - 1] = blocks.Length + 1;
						}
						startx_back = vertices[3].Coord.x;
						walls[floor][index].casts.Add(new Plane(vertices, vertices));
						for (int r = 0; r < 3; r++)
							if (Math.Abs(useblocks[p] - blocks[r]) < thresh)
							{
								walls[floor][index].slabType[r]++;
								if (!iscut)
									walls[floor][index].castType[walls[floor][index].castType.Count - 1] = r;
								break;
							}
					}
				}
			}
			if (backSlabs.Count > 0)
			{
				for (i = 0; i < backSlabs.Count; i++)
				{
					walls[floor][index].casts.Add(backSlabs[i]);
					walls[floor][index].castType.Add(backslabType[i]);
				}
			}
			return (diff);
		}
		public int[] FitWindow(double dist, double[] rods)
		{
			// Find out how many rods fit the width of the window/door
			// nums[i] = 0 -> not use, nums[i] > 0 --> use
			double mdist = double.MaxValue, diff;
			int i, j, k, t, num = rods.Count();
			int[] nums = new int[rods.Count()];
			for (i = 0; i < num; i++)
			{
				diff = Math.Abs(rods[i] - dist);
				if (diff < mdist)	//only one rod 
				{
					mdist = diff;
					for (t = 0; t< num; t++) nums[t] = 0;
					nums[i]++;
				}
				for (j = i + 1; j < num; j++)
				{
					diff = Math.Abs(rods[i] + rods[j] - dist);
					if (diff < mdist)	// Two rods
					{
						mdist = diff;
						for (t = 0; t < num; t++) nums[t] = 0;
						nums[i]++;
						nums[j]++;
					}
					for (k = j+1; k < num; k++)
					{
						diff = Math.Abs(rods[i] + rods[j] + rods[k] - dist);
						if (diff < mdist)	// Three rods
						{
							mdist = diff;
							for (t = 0; t < num; t++) nums[t] = 0;
							nums[i]++;
							nums[j]++;
							nums[k]++;
						}
					}
				}
			}
			return (nums);
		}
		public int CalSum(int[] data)
		{
			int sum = 0;
			for (int i = 0; i < data.Count(); i++)
				sum += data[i];
			return (sum);
		}
		public int ToInteger(int a, int b)
		{
			// Round the height/width to be adjusted to the multiple of given size
			int d = a / b;
			int m = a % b;
			int num;
			if (m > b / 2)
				num = d + 1;
			else
				num = d;
			return (num);
		}

		public int[] PutRods(double[] rods, double L)
		{
			// 1D problem of put casts, also unbounded knapsack problems
			// rods are the size of given rods, L is the length of big rod to fill
			// Transform the numbers to integer by times 10
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			int i=0, j=0, r = 0, c = 0, num = rods.Length, I = (int)Math.Round(L * 100); // Transfer to Int
			int I2 = I * 2, diff = int.MaxValue;
			int[] Rods = new int[num + 1], nums = new int[num + 1];
			bool[,] f = new bool[num + 1, I2 + 1];
			int[,] track = new int[num + 1, I2 + 1];

			if (Math.Abs(L) < thresh) return (nums);
			for (i = 1; i < num + 1; i++)
				Rods[i] = (int)Math.Round(rods[i-1] * 100 );
			for (i = 0; i < num + 1; i++)
				for (j = 0; j < I2 + 1; j++)
					f[i, j] = false;
			for (i = 0; i < num + 1; i++ )
				f[i, 0] = true;

			// Package
			for (i = 1; i < num + 1; i++)
				for (j = 1; j < I2 + 1; j++)
				{
					if (j >= Rods[i])
					{
						if (f[i, j - Rods[i]])
						{
							f[i, j] = true;
							track[i, j] = i;
						}
						else if (f[i - 1, j])
						{
							f[i, j] = true;
							track[i, j] = track[i - 1, j];
						}
					}
					else if (f[i - 1, j])
					{
						f[i, j] = true;
						track[i, j] = track[i - 1, j];
					}
				}
			// Track
			for (i = 1; i < num + 1; i++)
				for (j = 1; j < I2 + 1; j++)
				{
					if (f[i, j] && Math.Abs(I - j) <= diff)
					{
						diff = Math.Abs(I - j);
						r = i;
						c = j;
					}
				}
			W2 = (double)c / 100;

			//Vertex e0 = new Vertex(cu.endPoints[0]), e1;
			while (true)
			{
				r = track[r,c];
				nums[r] = 1;
				c -= Rods[r];
				//e1 = new Vertex(e0.Coord + cu.direction * (double)Rods[r]/100);
				////wallrods.Add(new Cuboid(6, 1, 0, e0, e1, cu.thickness, cu.height, cu.elevation, cu.direction, mat));
				//e0 = e1;
				while( track[r,c] == r)
				{
					nums[r]++;
					c -= Rods[r];
					//e1 = new Vertex(e0.Coord + cu.direction * (double)Rods[r]/100);
					////wallrods.Add(new Cuboid(6, 1, 0, e0, e1, cu.thickness, cu.height, cu.elevation, cu.direction, mat));
					//e0 = e1;
				}
				if (c <= 0 || track[r,c] == 0) break;
			}
			return (nums);
		}
		////////////////////////// Precast Layout //////////////////////////

		//////////////// Since Novermber ////////////////
		List<List<Vector2d>> bounds, drawBounds;
		double space = 1.5; // the distance from floor i to floor i+1 // just for drawing
		double[,] blimits;
		public void InitBoundary(int ind, int numfloor)
		{
			// A fix boundary of the building
			double[] x = {0.6, 0.5,
						  0.6, 0.5,
						  0.6, 0.4,
						  1, 0.4,
						 0.6, 0.8,
						 0.6, 0.8}; 
			minLenOfWall = Math.Max(x[(ind-1) * 2], x[(ind-1) * 2+ 1]) * 2 /10;
			bounds = new List<List<Vector2d>>();
			bounds.Add(new List<Vector2d>());

			bounds[0].Add(new Vector2d(x[ind * 2 - 2], -x[ind * 2 - 1]));
			bounds[0].Add(new Vector2d(x[ind * 2 - 2], x[ind * 2 - 1]));
			bounds[0].Add(new Vector2d(-x[ind * 2 - 2], x[ind * 2 - 1]));
			bounds[0].Add(new Vector2d(-x[ind * 2 - 2], -x[ind * 2 - 1]));

			for (int j = 1; j < numfloor; j++)
			{
				bounds.Add(new List<Vector2d>());
				for (int i = 0; i < 4; i++)
					bounds[j].Add(new Vector2d(bounds[0][i].x + space * j, bounds[0][i].y));
			}
			blimits = new double[numfloor, 4];
			// Boundary check
			for (int f = 0; f < numfloor; f++)
			{
				blimits[f, 0] = double.MaxValue;
				blimits[f, 1] = double.MaxValue;
				blimits[f, 2] = double.MinValue;
				blimits[f, 3] = double.MinValue;
				for (int i = 0; i < bounds[f].Count; i++)
				{
					if (blimits[f, 0] > bounds[f][i].x)
						blimits[f, 0] = bounds[f][i].x;
					if (blimits[f, 1] > bounds[f][i].y)
						blimits[f, 1] = bounds[f][i].y;
					if (blimits[f, 2] < bounds[f][i].x)
						blimits[f, 2] = bounds[f][i].x;
					if (blimits[f, 3] < bounds[f][i].y)
						blimits[f, 3] = bounds[f][i].y;
				}
			}

			drawBounds = new List<List<Vector2d>>();
			for (int i = 0; i < numfloor; i++)
			{ 
				drawBounds.Add(new List<Vector2d>());
				for (int j = 0; j < bounds[i].Count; j++)
					drawBounds[i].Add(new Vector2d(bounds[i][j]));
				// Update the bounds
				drawBounds[i][0] += new Vector2d(1, -1) * wallThickness;
				drawBounds[i][1] += new Vector2d(1, 1) * wallThickness;
				drawBounds[i][2] += new Vector2d(-1, 1) * wallThickness;
				drawBounds[i][3] += new Vector2d(-1, -1) * wallThickness;
			}
		}

		//////////////////////////// For initialization //////////////////////////// 
		////////////// User input is a list of requirements of approximate room area/aspect ratio
		double minLenOfWall = 0; // the minimum wall length
		double totalArea;
		public int LayoutInd;
		List<List<Room>> recRooms = new List<List<Room>>(), tRecRooms = new List<List<Room>>();
		List<List<Vector2d>> arrows = new List<List<Vector2d>>(),
			newArrows = new List<List<Vector2d>>(),
			missArrows = new List<List<Vector2d>>();
		List<List<Graph>> graph = new List<List<Graph>>();
		List<List<int>> groups = new List<List<int>>();

		public class Graph
		{
			public Vector2d center;
			public Vector2d[] vertices = new Vector2d[4];
			public Graph(Vector2d center, Vector2d[] vertices)
			{
				this.center = new Vector2d(center);
				for (int i = 0; i < vertices.Length; i++)
					this.vertices[i] = vertices[i];
			}
		}

		public void Input(int ind, int nfloor)
		{
			int i;
			Vector2d center = new Vector2d();
			List<string> names = new List<string>();
			List<double> lenwid = new List<double>();
			List<List<int>> conns = new List<List<int>>();
			List<List<int>> types = new List<List<int>>();
			walls = new List<List<Wall>>();
			cuboids = new List<List<Cuboid>>();
			doors = new List<List<Door>>();

			LayoutInd = ind;
			InitParams();

			switch (ind)
			{
				case 1: // Layout_1
					{
						names = new List<string>() { "Laundry", "Closet", "Bed_1", "Bath_1", "Utility",
						"Hall_1", "Bath_2", "Entry", "Hall_2", "Living", "Bed_2", "Foyer", "Dining", "Kitchen"};
						conns = new List<List<int>>() { new List<int> { 5 }, new List<int>{ 5 }, new List<int>{ 5 }, 
						new List<int>{ 8 }, new List<int>{ 8 }, new List<int>{0, 1, 2, 6, 9, 10 }, new List<int>{ 5 }, 
						new List<int>{ 11 }, new List<int>{ 3, 4, 9, 11, 12}, new List<int>{ 5, 8}, new List<int>{ 5 }, 
						new List<int>{ 7, 8}, new List<int>{ 8, 13}, new List<int>{ 12 }};
						types = new List<List<int>>() {new List<int> { 1 }, new List<int>{ 1 }, new List<int>{ 1 }, 
						new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{1, 1, 1, 1, 2, 1 }, new List<int>{ 1 }, 
						new List<int>{ 1 }, new List<int>{ 1, 1, 2, 2, 2}, new List<int>{ 2, 2}, new List<int>{ 1 }, 
						new List<int>{ 1, 2}, new List<int>{ 2, 1}, new List<int>{ 1} };
						lenwid = new List<double>() { 2, 1, 3, 2, 4, 5, 3, 2, 1, 2, 1, 4, 3, 3, 2, 2, 4, 1, 5, 5, 4, 5, 2, 2, 3, 3, 3, 3 };
						totalArea = 120;
						InitBoundary(ind, nfloor);
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						totalArea /= 100;
						for (i = 0; i < bounds[0].Count; i++)
							center += bounds[0][i];
						center /= bounds[0].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[0].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));
						break;
					}
				case 2: // Layout_2
					{
						names = new List<string>() { "Closet", "Porch", "Entry", "Foyer", "Garage", "Laundry",
						    "Study", "Living", "Bath", "Stair", "Kitchen",  "Dining", "Nook"};
						conns = new List<List<int>>() { new List<int> { 3 }, new List<int>{ 2, 3 }, new List<int>{ 1 }, 
						    new List<int>{ 0, 1, 4, 6, 7 }, new List<int>{ 3 }, new List<int>{ 7 }, new List<int>{ 3 }, 
						    new List<int>{ 3, 5, 8, 10 }, new List<int>{ 7 }, new List<int>{ 11 }, new List<int>{ 11, 12 }, 
						     new List<int>{9, 10, 12}, new List<int>{ 10, 11 }};
						types = new List<List<int>>() {new List<int> { 1 }, new List<int>{ 2, 1 }, new List<int>{ 2 }, 
						    new List<int>{ 1, 1, 1, 1, 2 }, new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{ 1 }, 
						    new List<int>{ 2, 1, 1, 3 }, new List<int>{ 1 }, new List<int>{ 2 }, new List<int>{ 2, 2 }, 
						     new List<int>{2, 2, 2}, new List<int>{ 2, 2 }};

						lenwid = new List<double>() {1.5, 2, 3, 4, 3, 2, 3, 1.5, 8, 4.5, 3, 2, 4, 4, 
						6, 4, 3, 2, 4, 1, 5, 2, 4, 4, 2, 3};
						totalArea = 140;

						InitBoundary(ind, nfloor);
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						totalArea /= 100;
						for (i = 0; i < bounds[0].Count; i++)
							center += bounds[0][i];
						center /= bounds[0].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[0].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));

						// Second floor					
						names = new List<string>() { "Stair", "Hall", "Living", "Bed_1", "Bed_2", "Closet", "Bath" };
						conns = new List<List<int>>() { new List<int> { 1 }, new List<int>{ 0, 2, 3, 4 }, new List<int>{ 1 }, 
						    new List<int>{ 1, 5 }, new List<int>{ 1, 6 }, new List<int>{ 3 }, new List<int>{ 4,5 }};
						types = new List<List<int>>() { new List<int> { 2 }, new List<int>{ 2, 2, 1, 1 }, new List<int>{ 2 }, 
						    new List<int>{ 1, 1 }, new List<int>{ 1, 1 }, new List<int>{ 1 }, new List<int>{ 1,1 }};
						lenwid = new List<double>() { 4, 1, 4, 1, 3, 4, 6, 5, 5, 6, 4, 3, 4, 2 };
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						center = new Vector2d();
						for (i = 0; i < bounds[1].Count; i++)
							center += bounds[1][i];
						center /= bounds[1].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[1].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));
						break;
					}
				case 3: // Layout_3
					{
						names = new List<string>() { "Entry", "Book", "Bath", "Hall", "Bed_1",
						"Dining", "Living", "Bed_2", "Kitchen", "Balcony"};
						conns = new List<List<int>>() { new List<int> { 3 }, new List<int>{ 6,9 }, new List<int>{ 3 }, 
						new List<int>{ 0, 2, 4, 6}, new List<int>{ 3 }, new List<int>{ 6, 8 }, new List<int>{ 3, 5, 7}, 
						new List<int>{ 6 }, new List<int>{ 5}, new List<int>{1}};
						types = new List<List<int>>() { new List<int> { 1 }, new List<int>{ 1,1 }, new List<int>{ 1 }, 
						new List<int>{ 1, 1, 1, 2 }, new List<int>{ 1 }, new List<int>{ 2, 1 }, new List<int>{ 2, 2, 1 }, 
						new List<int>{ 1 }, new List<int>{ 1}, new List<int>{1}};

						lenwid = new List<double>() { 1, 4, 2, 2, 3, 4, 3, 1, 5, 4, 3, 4, 6, 5, 5, 4, 2, 4,  2, 2 };
						totalArea = 120;

						InitBoundary(ind, nfloor);
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						totalArea /= 100;
						for (i = 0; i < bounds[0].Count; i++)
							center += bounds[0][i];
						center /= bounds[0].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[0].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));
						break;
					}
				case 4: // Layout 4
					{
						names = new List<string>() { "Open area", "Hall_1", "Office#1", "Conference", "Office#2",
						"Hall_2", "Office#3", "Men's Room", "Women'sRoom", "TeaRoom", "Entry"};
						conns = new List<List<int>>() { new List<int> { 1, 2, 3, 5, 6 }, new List<int>{ 0, 2, 6, 7, 9, 10}, new List<int>{ 0, 1 }, 
						new List<int>{ 0, 4}, new List<int>{ 3 }, new List<int>{ 0, 4, 6, 8 }, new List<int>{ 0, 1, 5, 10}, 
						new List<int>{ 1 }, new List<int>{5}, new List<int>{ 1}, new List<int>{1, 6}};
						types = new List<List<int>>() { new List<int> { 2, 1, 1, 2, 2 }, new List<int>{ 2, 1, 2, 1, 1, 2 }, new List<int>{ 1, 1 }, 
						new List<int>{ 1, 3 }, new List<int>{ 3 }, new List<int>{ 2, 3, 2, 1 }, new List<int>{ 2, 2, 2, 3 }, 
						new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{ 1}, new List<int>{2, 3}};

						lenwid = new List<double>() { 3, 3, 1, 15, 4, 3, 2, 4, 3, 15, 0.5, 16, 3, 15, 3, 5, 3, 5, 2, 2, 3, 2};
						totalArea = 160;

						InitBoundary(ind, nfloor);
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						totalArea /= 100;
						for (i = 0; i < bounds[0].Count; i++)
							center += bounds[0][i];
						center /= bounds[0].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[0].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));
						break;
					}
				case 5:
					{
						names = new List<string>() { "Dining", "Bath_1", "Bed_1", "Bed_2", "Bed_3", "Hall_1",
						    "Bath_2", "Kitchen", "Hall_2", "Bath_3", "Living", "Entry", "Porch", "Stair"};
						conns = new List<List<int>>() { new List<int> { 3,5,7,8,10,11 }, new List<int>{ 2 }, new List<int>{ 1 }, new List<int>{ 0,5,6}, 
						    new List<int>{ 5,9 }, new List<int>{ 0,3,4,6 }, new List<int>{ 3,5 }, new List<int>{ 0 }, new List<int>{ 0,9 }, new List<int>{ 4,8 }, 
						    new List<int>{ 0 }, new List<int>{ 0, 12 }, new List<int>{ 11}, new List<int>{  }};

						types = new List<List<int>>() {new List<int> { 1,2,1,2,3,2 }, new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{ 1,1,1 }, 
						    new List<int>{ 1,1 }, new List<int>{ 2,1,1,1 }, new List<int>{ 1,1 }, new List<int>{ 1 }, new List<int>{ 2,1 }, new List<int>{ 1,1 }, 
						     new List<int>{ 2}, new List<int>{ 2, 1 }, new List<int>{ 1}, new List<int>{  } };

						lenwid = new List<double>() {4, 5, 2, 5, 5, 5, 4, 5, 4, 5, 3, 2, 3, 3, 3, 4, 4, 1, 2, 5, 5, 5, 2, 3, 2, 2, 5, 2};
						totalArea = 195;

						InitBoundary(ind, nfloor);
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						totalArea /= 100;
						for (i = 0; i < bounds[0].Count; i++)
							center += bounds[0][i];
						center /= bounds[0].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[0].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));

						// Second floor					
						names = new List<string>() { "Hall_1", "Closet", "Bath_1", "Bed_1", "Laundry", "Bed_2", "Hall_2",
						"Kitchen", "Dining", "Living", "Balcony", "Bath_2", "Study", "Bed_3", "Stair"};
						conns = new List<List<int>>() { new List<int>{ 3, 6, 7, 8, 9, 11}, new List<int>{ 3 }, 
						    new List<int>{ 3 }, new List<int>{ 0, 1, 2}, new List<int>{ 6 }, new List<int>{ 6 }, new List<int>{ 0, 4, 5, 10, 12 }, 
							new List<int>{ 0 }, new List<int>{0}, new List<int>{0, 10},new List<int>{6, 9},new List<int>{0},
							new List<int>{ 6, 13 },new List<int>{12}, new List<int> { }};

						types = new List<List<int>>() { new List<int>{ 1, 2, 2, 2, 2, 1 }, new List<int>{ 1 }, 
						    new List<int>{ 1 }, new List<int>{ 1, 1, 1 }, new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{ 2, 1, 1, 2, 1 },
							new List<int>{1}, new List<int>{2},new List<int>{2, 2},new List<int>{2, 2},new List<int>{1},
							new List<int>{1, 1},new List<int>{1}, new List<int> { }};
						lenwid = new List<double>() { 5, 1, 2, 2, 2, 2, 4, 3, 1, 2, 3, 4, 2, 2, 3, 2, 4, 3, 4, 5, 4, 2, 3, 2, 3, 2, 5, 4, 5, 2 };
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						center = new Vector2d();
						for (i = 0; i < bounds[1].Count; i++)
							center += bounds[1][i];
						center /= bounds[1].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[1].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));

						// Third floor
						names = new List<string>() { "Bath_1", "Bed_1", "Closet", "Bed_2", "Hall_1", "Utils", "Hall_2",
						"Dining", "Kitchen", "Laundry", "Hall_3", "Nook", "Living", "Bath_2", "Stair"};
						conns = new List<List<int>>() { new List<int>{ 4}, new List<int>{ 4 }, new List<int>{ 3 }, new List<int>{ 2, 4}, 
							new List<int>{ 0, 1, 3, 6 }, new List<int>{ 6 }, new List<int>{ 4, 5, 7, 10 }, new List<int>{ 6, 8, 11 }, 
							new List<int>{7}, new List<int>{10},new List<int>{6, 9, 12, 13},new List<int>{7},
							new List<int>{ 10 },new List<int>{10}, new List<int> { }};

						types = new List<List<int>>() { new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{ 1, 1 }, 
							new List<int>{ 1, 1, 1, 2 }, new List<int>{ 1 }, new List<int>{ 2, 1, 2, 2 }, new List<int>{2, 2, 2}, 
							new List<int>{2},new List<int>{1},new List<int>{2, 1, 2, 1},new List<int>{1},
							new List<int>{2},new List<int>{1}, new List<int> { }};
						lenwid = new List<double>() { 3, 2, 3, 5, 3, 2, 4, 5, 3, 2, 3, 2, 5, 1, 3, 5, 3, 4, 4, 2, 4, 1, 3, 3, 5, 4, 2, 3, 5, 2 };
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						center = new Vector2d();
						for (i = 0; i < bounds[2].Count; i++)
							center += bounds[2][i];
						center /= bounds[2].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[2].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));
						break;
					}
				case 6:
					{
						//names = new List<string>() { "Bath_1", "Bed_1", "Foyer", "Bed_2", "Hall_1", "Utils", "Hall_2",
						//"Dining", "Kitchen", "Laundry", "Hall_3", "Nook", "Living", "Bath_2"};
						//conns = new List<List<int>>() { new List<int>{ 4}, new List<int>{ 4 }, new List<int>{ 4 }, new List<int>{ 4}, 
						//    new List<int>{ 0, 1, 2, 3 }, new List<int>{ 6 }, new List<int>{ 5, 7, 10 }, new List<int>{ 6, 8 }, 
						//    new List<int>{7, 10, 11}, new List<int>{10},new List<int>{6, 7, 12, 13},new List<int>{8},
						//    new List<int>{ 10 },new List<int>{10}};

						//types = new List<List<int>>() { new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{ 2 }, new List<int>{ 1 }, 
						//    new List<int>{ 1, 1, 2, 1}, new List<int>{ 1 }, new List<int>{ 1, 2, 2 }, new List<int>{2, 1, 2}, 
						//    new List<int>{1, 1, 1},new List<int>{1},new List<int>{2, 2, 2, 1},new List<int>{1},
						//    new List<int>{2},new List<int>{1}};
						//lenwid = new List<double>() { 3, 2, 3, 5, 3, 2, 4, 5, 3, 2, 3, 2, 5, 1, 3, 5, 3, 4, 4, 2, 4, 1, 3, 3, 5, 4, 2, 3 };
						//totalArea = 180;

						//InitBoundary(ind, nfloor);
						//for (i = 0; i < lenwid.Count; i++)
						//    lenwid[i] /= 10;
						//totalArea /= 100;
						//for (i = 0; i < bounds[0].Count; i++)
						//    center += bounds[0][i];
						//center /= bounds[0].Count;
						//recRooms.Add(new List<Room>());
						//for (i = 0; i < names.Count; i++)
						//    recRooms[0].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));
						//break;
						names = new List<string>() { "Dining", "Bath_1", "Bed_1", "Bed_2", "Bed_3", "Hall_1",
						    "Bath_2", "Kitchen", "Hall_2", "Bath_3", "Living", "Entry", "Porch"};
						conns = new List<List<int>>() { new List<int> { 3,5,7,8,10,11 }, new List<int>{ 2 }, new List<int>{ 1 }, new List<int>{ 0,5,6}, 
						    new List<int>{ 5,9 }, new List<int>{ 0,3,4,6 }, new List<int>{ 3,5 }, new List<int>{ 0 }, new List<int>{ 0,9 }, new List<int>{ 4,8 }, 
						    new List<int>{ 0 }, new List<int>{ 0, 12 }, new List<int>{ 11}};

						types = new List<List<int>>() {new List<int> { 1,2,1,2,3,2 }, new List<int>{ 1 }, new List<int>{ 1 }, new List<int>{ 1,1,1 }, 
						    new List<int>{ 1,1 }, new List<int>{ 2,1,1,1 }, new List<int>{ 1,1 }, new List<int>{ 1 }, new List<int>{ 2,1 }, new List<int>{ 1,1 }, 
						     new List<int>{ 2}, new List<int>{ 2, 1 }, new List<int>{ 1} };

						lenwid = new List<double>() { 4, 5, 2, 5, 5, 5, 4, 5, 4, 5, 3, 2, 3, 3, 3, 4, 4, 1, 2, 5, 5, 5, 2, 3, 2, 2 };
						totalArea = 195;

						InitBoundary(ind, nfloor);
						for (i = 0; i < lenwid.Count; i++)
							lenwid[i] /= 10;
						totalArea /= 100;
						for (i = 0; i < bounds[0].Count; i++)
							center += bounds[0][i];
						center /= bounds[0].Count;
						recRooms.Add(new List<Room>());
						for (i = 0; i < names.Count; i++)
							recRooms[0].Add(new Room(names[i], center, lenwid[i * 2], lenwid[i * 2 + 1], conns[i], types[i]));
						break;
					}

			}
			ConnectGraph(ind);
		}

		public void ConnectGraph(int ind)
		{
			int f, i, j, k = 0;
			double start = -0.3; // recRooms.Count * 1.6;
			double space = 0.3, len = 0, wid = 0;
			Vector2d center0 = new Vector2d(start, -0.45), center;
			Vector2d[] vertices = new Vector2d[4];
			List<List<int>> rows = new List<List<int>>();

			switch (ind)
			{
 				case 1:
					rows = new List<List<int>>() {new List<int> { 3, 4, 4, 3 } };
					break;
				case 2:
					rows = new List<List<int>>() {new List<int> { 3, 3, 3, 2, 2}, new List<int>{ 1, 2, 2, 2} };
					break;
				case 3:
					rows = new List<List<int>>() { new List<int> { 2, 3, 3, 2 } };
					break;
				case 4:
					rows = new List<List<int>>() { new List<int> { 2, 3, 3, 3 } };
					break;
				case 5:
					rows = new List<List<int>>() { new List<int> { 4, 3, 3, 4 }, new List<int>{4, 4, 4, 3}, new List<int>{ 4, 3, 4, 4} };
					break;
				case 6:
					rows = new List<List<int>>() { new List<int> { 4, 3, 3, 3 } };
					break;
			}
			graph = new List<List<Graph>>();
			arrows = new List<List<Vector2d>>();
			newArrows = new List<List<Vector2d>>();
			missArrows = new List<List<Vector2d>>();

			center = new Vector2d(center0);
			//if (ind == 3) center.x += space * 2;
			for (f = 0; f < numFloor; f++)
			{
				i = 0;
				k = 0;
				graph.Add(new List<Graph>());
				while (k < rows[f].Count)
				{
					for (j = i; j < i + rows[f][k]; j++)
					{
						if (recRooms[f][j].length > recRooms[f][j].width)
						{
							len = recRooms[f][j].length;
							wid = recRooms[f][j].width;
						}
						else
						{
							len = recRooms[f][j].width;
							wid = recRooms[f][j].length;
						}
						vertices[0] = new Vector2d(center.x + len / 4, center.y - wid / 4);
						vertices[1] = new Vector2d(center.x + len / 4, center.y + wid / 4);
						vertices[2] = new Vector2d(center.x - len / 4, center.y + wid / 4);
						vertices[3] = new Vector2d(center.x - len / 4, center.y - wid / 4);
						graph[f].Add(new Graph(center, vertices));
						center.x += space;
					}
					i += rows[f][k];
					k++;
					center.x = center0.x;
					center.y += space;
				}
				center0.x += 1.5;
				center = center0;
			}
			// Arrows
			for (f = 0; f < recRooms.Count; f++)
			{
				arrows.Add(new List<Vector2d>());
				for (i = 0; i < recRooms[f].Count; i++)
				{
					for (j = 0; j < recRooms[f][i].conns.Count; j++)
					{
						k = recRooms[f][i].conns[j];
						if (k < i) continue;
						arrows[f].Add(new Vector2d(graph[f][i].center));
						arrows[f].Add(new Vector2d(graph[f][k].center));
					}
				}
			}
		}

		public void ConnectGraphAfter()
		{
			int f, i, j, k;
			bool conn = false;
			Vector2d v = new Vector2d(); // = new Vector2d(1,-0.5) * 0.02;
			// Arrows
			newArrows = new List<List<Vector2d>>();
			missArrows = new List<List<Vector2d>>();
			for (f = 0; f < recRooms.Count; f++)
			{
				newArrows.Add(new List<Vector2d>());
				missArrows.Add(new List<Vector2d>());
				for (i = 0; i < recRooms[f].Count - 1; i++)
				{
					for (j = 0; j < recRooms[f][i].conns.Count; j++)
					{
						k = recRooms[f][i].conns[j];
						if (k < i) continue;
						conn = Connected(f, i, k);
						if (conn)
						{
							newArrows[f].Add(new Vector2d(graph[f][i].center + v));
							newArrows[f].Add(new Vector2d(graph[f][k].center + v));
						}
						else
						{
							missArrows[f].Add(new Vector2d(graph[f][i].center + v));
							missArrows[f].Add(new Vector2d(graph[f][k].center + v));
						}
					}
				}
			}
		}

		public bool dispRoom = false;
		public void RoomLayout()
		{
			int i, j;
			int stair = -1, onbd = -1, stairc = -1;
			List<Vector2d> currbounds;

			if (recRooms == null || recRooms.Count == 0) return;
			InitialLayout(0);
			for (i = 0; i < recRooms[0].Count; i++)
			{
				recRooms[0][i].length = (recRooms[0][i].vertex[0] - recRooms[0][i].vertex[1]).Length();
				recRooms[0][i].width = (recRooms[0][i].vertex[0] - recRooms[0][i].vertex[3]).Length();
			}

			// Find the location of stair
			for (i = 0; i < recRooms[0].Count; i++)
				if (String.Compare(recRooms[0][i].name, "Stair") == 0)
				{
					stair = i;
					break;
				}
			if (stair != -1)
			{
				for (i = 0; i < bounds[0].Count; i++)
				{
					for (j = 0; j < 4; j++)
					{
						if (Point_On_Line(bounds[0][i], bounds[0][(i + 1) % 4], recRooms[0][stair].vertex[j]) &&
							Point_On_Line(bounds[0][i], bounds[0][(i + 1) % 4], recRooms[0][stair].vertex[(j + 1) % 4]))
						{
							onbd = i;
							break;
						}
					}
					if (onbd != -1)
						break;
				}
			}
			for (i = 1; i < bounds.Count; i++)
			{
				currbounds = new List<Vector2d>();
				for (j = 0; j < bounds[i].Count; j++)
					currbounds.Add(new Vector2d(bounds[i][j]));

				switch (onbd)
				{
					case 0:
						{
							bounds[i][0] = new Vector2d(recRooms[0][stair].vertex[3].x + space, bounds[i][0].y);
							bounds[i][1] = new Vector2d(bounds[i][0].x, bounds[i][1].y);
							break;
						}
					case 1:
						{
							bounds[i][1] = new Vector2d(bounds[i][0].x, recRooms[0][stair].vertex[3].y);
							bounds[i][2] = new Vector2d(bounds[i][2].x, bounds[i][1].y);
							break;
						}
					case 2:
						{
							bounds[i][2] = new Vector2d(recRooms[0][stair].vertex[0].x + space, bounds[i][0].y);
							bounds[i][3] = new Vector2d(bounds[i][2].x, bounds[i][3].y);
							break;
						}
					case 3:
						{
							bounds[i][3] = new Vector2d(bounds[i][3].x, recRooms[0][stair].vertex[1].y);
							bounds[i][0] = new Vector2d(bounds[i][0].x, bounds[i][3].y);
							break;
						}
				}

				InitialLayout(i);
				for (int p = 0; p < recRooms[i].Count; p++)
				{
					recRooms[i][p].length = (recRooms[i][p].vertex[0] - recRooms[i][p].vertex[1]).Length();
					recRooms[i][p].width = (recRooms[i][p].vertex[0] - recRooms[i][p].vertex[3]).Length();
				}

				for (j = 0; j < recRooms[i].Count; j++)
					if (String.Compare(recRooms[i][j].name, "Stair") == 0)
					{
						stairc = j;
						break;
					}
				
				for (j = 0; j < recRooms[i].Count; j++)
				{
					if (j == stairc) continue;
					switch (onbd)
					{
						case 0:
							{
								if (Math.Abs(recRooms[i][j].vertex[0].x - bounds[i][0].x) < thresh)
								{
									if (Point_In_Line(recRooms[i][stairc].vertex[2], recRooms[i][stairc].vertex[3], recRooms[i][j].vertex[0]) &&
										(!Point_In_Line(recRooms[i][stairc].vertex[2], recRooms[i][stairc].vertex[3], recRooms[i][j].vertex[1]) &&
										 !Point_On_Line(recRooms[i][stairc].vertex[2], recRooms[i][stairc].vertex[3], recRooms[i][j].vertex[1])))
									{
										recRooms[i][j].vertex.Insert(1, new Vector2d(recRooms[i][stairc].vertex[2]));
										recRooms[i][j].vertex.Insert(2, new Vector2d(recRooms[i][stairc].vertex[1]));
										recRooms[i][j].vertex[3] = new Vector2d(currbounds[0].x, 
											recRooms[i][j].vertex[3].y);
									}
									else if (Point_In_Line(recRooms[i][stairc].vertex[2], recRooms[i][stairc].vertex[3], recRooms[i][j].vertex[1]) &&
										!Point_In_Line(recRooms[i][stairc].vertex[2], recRooms[i][stairc].vertex[3], recRooms[i][j].vertex[0]))
									{
										recRooms[i][j].vertex[0] = new Vector2d(currbounds[0].x, 
											recRooms[i][j].vertex[0].y);
										recRooms[i][j].vertex.Insert(1, new Vector2d(recRooms[i][stairc].vertex[0]));
										recRooms[i][j].vertex.Insert(2, new Vector2d(recRooms[i][stairc].vertex[3]));
									}
									else if (!Point_In_Line(recRooms[i][stairc].vertex[2], recRooms[i][stairc].vertex[3], recRooms[i][j].vertex[1]) &&
										!Point_In_Line(recRooms[i][stairc].vertex[2], recRooms[i][stairc].vertex[3], recRooms[i][j].vertex[0]))
									{
										recRooms[i][j].vertex[0] = new Vector2d(currbounds[0].x, 
											recRooms[i][j].vertex[0].y);
										recRooms[i][j].vertex[1] = new Vector2d(currbounds[0].x, 
											recRooms[i][j].vertex[1].y);
									}
								}
								break;
							}
						case 1:
							{
								if (Math.Abs(recRooms[i][j].vertex[1].y - bounds[i][1].y) < thresh)
								{
			
								}
								break;
							}
					}
				}
				CheckConnectivity(i);

				bounds[i] = new List<Vector2d>();
				for (j = 0; j < currbounds.Count; j++)
					bounds[i].Add(new Vector2d(currbounds[j]));
			}
		}

		public void ApartmentLayout()
		{
			int f, j, stair = -1;
			Vector2d b1, b2;
			Room stairR;

			// Fix the location of stairs
			for (f = 0; f < numFloor; f++)
			{
				stair = -1;
				for (j = 0; j < recRooms[f].Count; j++)
				{
					if (String.Compare(recRooms[f][j].name, "Stair") == 0)
					{
						stair = j;
						break;
					}
				}
				recRooms[f][stair].width = (bounds[f][0] - bounds[f][3]).Length();
				recRooms[f][stair].length = Math.Max( recRooms[f][stair].area / recRooms[f][stair].width, 0.1);
				recRooms[f][stair].center = new Vector2d((bounds[f][0].x + bounds[f][3].x)/2,
					bounds[f][1].y - recRooms[f][stair].length / 2);
				UpdateVertex(f, stair);
				b1 = new Vector2d(bounds[f][1]);
				b2 = new Vector2d(bounds[f][2]);
				bounds[f][1] = new Vector2d(bounds[f][1].x, bounds[f][1].y - recRooms[f][stair].length);
				bounds[f][2] = new Vector2d(bounds[f][2].x, bounds[f][2].y - recRooms[f][stair].length);
				stairR = new Room(recRooms[f][stair]);
				// Remove stair first 
				recRooms[f].RemoveAt(stair);
				InitialLayout(f);
				recRooms[f].Insert(stair, stairR);
				// restore the boundary
				bounds[f][1] = b1;
				bounds[f][2] = b2;
				for (j = 0; j < recRooms[f].Count; j++)
				{
					recRooms[f][j].length = (recRooms[f][j].vertex[0] - recRooms[f][j].vertex[1]).Length();
					recRooms[f][j].width = (recRooms[f][j].vertex[0] - recRooms[f][j].vertex[3]).Length();
				}
			}
		}

		public void InitialLayout(int floor)
		{
			int i, j, stair = -1, stair0 = -1, iter = 0;
			int[] m;
			double len, wid, area1, area2;
			List<Vector2d> stack = new List<Vector2d>();
			List<int> inds = new List<int>(), nums = new List<int>();
			List<List<int>> conns = new List<List<int>>(), tempc;
			Vector2d v1, v2, v;
			bool horiCut = false;
			Room stairR = new Room();

			if (floor > 0 && LayoutInd < 5) // Put stair in the same location
			{
				for (i = 0; i < recRooms[0].Count; i++)
					if (String.Compare(recRooms[0][i].name, "Stair") == 0)
					{
						stair0 = i;
						break;
					}
				for (i = 0; i < recRooms[floor].Count; i++)
					if (String.Compare(recRooms[floor][i].name, "Stair") == 0)
					{
						stair = i;
						break;
					}
				recRooms[floor][stair].vertex = new List<Vector2d>();
				for (i = 0; i < recRooms[0][stair0].vertex.Count; i++)
					recRooms[floor][stair].vertex.Add(new Vector2d(recRooms[0][stair0].vertex[i]
						+ new Vector2d(1, 0) * space));
				recRooms[floor][stair].center = recRooms[0][stair0].center + new Vector2d(1, 0) * space;
				recRooms[floor][stair].length = recRooms[0][stair0].length;
				recRooms[floor][stair].width = recRooms[0][stair0].width;
				recRooms[floor][stair].area = recRooms[0][stair0].area;
				recRooms[floor][stair].asratio = recRooms[0][stair0].asratio;

				// Remove the stair from the rooms and then add back
				stairR = new Room(recRooms[floor][stair]);
				recRooms[floor].RemoveAt(stair);
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					recRooms[floor][i].conns.Remove(stair);
					for (j = 0; j < recRooms[floor][i].conns.Count; j++)
					{
						if (recRooms[floor][i].conns[j] > stair)
							recRooms[floor][i].conns[j]--;
					}
				}
			}

			// Build the stack
			for (i = 0; i < bounds[floor].Count; i++)
				stack.Add(new Vector2d(bounds[floor][i]));
			len = (stack[0] - stack[1]).Length();
			wid = (stack[0] - stack[3]).Length();
			if (len > wid) horiCut = true;
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				inds.Add(i);
				nums.Add(recRooms[floor][i].conns.Count);
			}
			
			while (stack.Count > 0)
			{
				iter++;
				if (inds.Count == 1)
				{
					i = inds[0];
					recRooms[floor][i].vertex = new List<Vector2d>();
					v = new Vector2d();
					for (j = 0; j < 4; j++)
					{
						recRooms[floor][i].vertex.Add(new Vector2d(stack[j]));
						v += stack[j];
					}
					recRooms[floor][i].center = v / 4;
					len = (stack[0] - stack[1]).Length();
					wid = (stack[0] - stack[3]).Length();
					recRooms[floor][i].length = len;
					recRooms[floor][i].width = wid;
					//recRooms[floor][i].area = len * wid ;
					//recRooms[floor][i].asratio = len / wid;
				}
				else
				{
					m = KeyRooms(floor, nums, inds);
					tempc = ConnectedRooms(floor, m[0], m[1], inds);
					conns.Add(tempc[0]);
					conns.Add(tempc[1]);
					// Calculate the area of two parts
					area1 = 0;
					groups.Add(new List<int>());
					for (i = 0; i < tempc[0].Count; i++)
					{
						area1 += recRooms[floor][tempc[0][i]].area;
						groups[groups.Count-1].Add(tempc[0][i]);
					}
					area2 = 0;
					groups.Add(new List<int>());
					for (i = 0; i < tempc[1].Count; i++)
					{
						area2 += recRooms[floor][tempc[1][i]].area;
						groups[groups.Count - 1].Add(tempc[1][i]);
					}
					// Oberserve groups
					Console.WriteLine("{0}  {1} : ", "Iteration", iter);
					Console.WriteLine("Group 1:");
					for (int t = 0; t < groups[groups.Count - 2].Count; t++)
						Console.Write("{0} ", recRooms[floor][groups[groups.Count-2][t]].name);
					Console.WriteLine();
					Console.WriteLine("Group 2:");
					for (int t = 0; t < groups[groups.Count - 1].Count; t++)
						Console.Write("{0} ", recRooms[floor][groups[groups.Count - 1][t]].name);
					Console.WriteLine();

					len = (stack[0] - stack[1]).Length();
					wid = (stack[0] - stack[3]).Length();
					if (len > wid) // horizontal cut
					{
						v1 = new Vector2d(stack[0].x, stack[0].y + len * area2 / (area1 + area2));
						v2 = new Vector2d(stack[3].x, v1.y);
						stack.Add(new Vector2d(v1));
						stack.Add(new Vector2d(stack[1]));
						stack.Add(new Vector2d(stack[2]));
						stack.Add(new Vector2d(v2));

						stack.Add(new Vector2d(stack[0]));
						stack.Add(new Vector2d(v1));
						stack.Add(new Vector2d(v2));
						stack.Add(new Vector2d(stack[3]));
					}
					else // vertical cut
					{
						v1 = new Vector2d(stack[2].x + wid * area1 / (area1 + area2), stack[2].y);
						v2 = new Vector2d(v1.x, stack[3].y);
						stack.Add(new Vector2d(v2));
						stack.Add(new Vector2d(v1));
						stack.Add(new Vector2d(stack[2]));
						stack.Add(new Vector2d(stack[3]));

						stack.Add(new Vector2d(stack[0]));
						stack.Add(new Vector2d(stack[1]));
						stack.Add(new Vector2d(v1));
						stack.Add(new Vector2d(v2));
					}
					horiCut = !horiCut;
				}
				// Pop the first four vertices
				for (i = 0; i < 4; i++)
					stack.RemoveAt(0);
				if (stack.Count == 0) break;
				inds = new List<int>();
				nums = new List<int>();
				for (i = 0; i < conns[0].Count; i++)
				{
					inds.Add(conns[0][i]);
					nums.Add(recRooms[floor][conns[0][i]].conns.Count);
				}
				conns.RemoveAt(0);
			}
			AdjustAspectRatio(floor);
			CheckConnectivity(floor);
			AdjustWallLength(floor);
			
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				Vector2d vc = new Vector2d();
				for (j = 0; j < recRooms[floor][i].vertex.Count; j++)
					vc += recRooms[floor][i].vertex[j];
				recRooms[floor][i].center = vc / recRooms[floor][i].vertex.Count;
			}
			if (floor > 0 && LayoutInd < 5) // for a house with connected multi floors, not a building with independent floor
			{
				recRooms[floor].Insert(stair, stairR);
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					if (i == stair) continue;
					for (j = 0; j < recRooms[floor][i].conns.Count; j++)
						if (recRooms[floor][i].conns[j] >= stair)
							recRooms[floor][i].conns[j]++;
				}
				for (i = 0; i < recRooms[floor][stair].conns.Count; i++)
				{
					j = recRooms[floor][stair].conns[i];
					recRooms[floor][j].conns.Add(stair);
					Sort(recRooms[floor][j].conns);
				}
			}
			SnapRoomWall(floor, 0.05);
		}

		public void AdjustWallLength(int floor)
		{
			// If the length of a wall is shorter than the minmum length, adjust it
			int i, j;
			double len, wid, lift, minx, miny, maxx, maxy;
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				minx = double.MaxValue;
				miny = double.MaxValue;
				maxx = double.MinValue;
				maxy = double.MinValue;
				for (j = 0; j < recRooms[floor][i].vertex.Count; j++)
				{
					minx = Math.Min(recRooms[floor][i].vertex[j].x, minx);
					miny = Math.Min(recRooms[floor][i].vertex[j].y, miny);
					maxx = Math.Max(recRooms[floor][i].vertex[j].x, maxx);
					maxy = Math.Max(recRooms[floor][i].vertex[j].y, maxy);
				}
				len = maxy - miny;
				wid = maxx - minx;
				if (len < minLenOfWall)
				{
					lift = minLenOfWall - len;
					LiftRoom(floor, i, 1, lift);
				}
				if (wid < minLenOfWall)
				{
					lift = minLenOfWall - wid;
					LiftRoom(floor, i, 2, lift);
				}
			}
		}

		public void SnapRoomWall(int floor, double thr)
		{ 
			int i, j, k, t;
			double diff;

			for (i = 0; i < recRooms[floor].Count - 1; i++)
			{
				for (j = i + 1; j < recRooms[floor].Count; j++)
				{
					for (k = 0; k < 4; k++)
					{
						for (t = 0; t < 4; t++)
						{
							diff = Math.Abs(recRooms[floor][i].vertex[k].y - recRooms[floor][j].vertex[t].y);
							if ((Math.Abs(recRooms[floor][i].vertex[k].x - recRooms[floor][j].vertex[t].x) < thresh ||
								string.Compare(recRooms[floor][i].name, "Stair") == 0) && diff < thr)
							{
								if (string.Compare(recRooms[floor][j].name, "Stair") != 0)
								{
									diff = recRooms[floor][j].vertex[t].y - recRooms[floor][i].vertex[k].y;
									LiftRoomWall(floor, i, 1, diff);
								}
								else
								{
									diff = recRooms[floor][i].vertex[k].y - recRooms[floor][j].vertex[t].y;
									LiftRoomWall(floor, j, 1, diff);
									
								}
							}
							diff = Math.Abs(recRooms[floor][i].vertex[k].x - recRooms[floor][j].vertex[t].x);
							if ((Math.Abs(recRooms[floor][i].vertex[k].y - recRooms[floor][j].vertex[t].y) < thresh ||
								string.Compare(recRooms[floor][i].name, "Stair") == 0) && diff < thr)
							{
								if (string.Compare(recRooms[floor][j].name, "Stair") != 0)
								{
									diff = recRooms[floor][j].vertex[t].x - recRooms[floor][i].vertex[k].x;
									LiftRoomWall(floor, i, 2, diff);
								}
								else
								{
									diff = recRooms[floor][i].vertex[k].x - recRooms[floor][j].vertex[t].x;
									LiftRoomWall(floor, j, 2, diff);
									
								}
							}
						}
					}
				}
			}
		}

		public void LiftRoom(int floor, int indr, int lw, double dist)
		{ 
			// indr: which room
			// lw: len or wid (y or x)
			// dist : to lift
			int i;
			bool onb = false;
			double x, y;
			Vector2d xv = new Vector2d(1,0), yv = new Vector2d(0,1), liftv = new Vector2d();
			if (lw == 1) // Len -- Y
			{
				onb = OnBoundary(floor, recRooms[floor][indr].vertex[0],
					recRooms[floor][indr].vertex[3]);
				if (onb) // on boundary, move the other edge
				{
					y = recRooms[floor][indr].vertex[1].y;
					recRooms[floor][indr].vertex[1] += yv * dist;
					recRooms[floor][indr].vertex[2] += yv * dist;
					liftv = yv * dist;
				}
				else
				{ 
					y = recRooms[floor][indr].vertex[0].y;
					recRooms[floor][indr].vertex[0] -= yv * dist;
					recRooms[floor][indr].vertex[3] -= yv * dist;
					liftv = yv * (-dist);
				}
				for (i = 0; i < recRooms[floor].Count; i++)
					{
						if (i == indr) continue;
						if (Math.Abs(recRooms[floor][i].vertex[0].y - y) < thresh &&
							Math.Abs(recRooms[floor][i].vertex[3].y - y) < thresh)
						{
							recRooms[floor][i].vertex[0] += liftv;
							recRooms[floor][i].vertex[3] += liftv;
						}
						else if (Math.Abs(recRooms[floor][i].vertex[1].y - y) < thresh &&
							Math.Abs(recRooms[floor][i].vertex[2].y - y) < thresh)
						{
							recRooms[floor][i].vertex[1] += liftv;
							recRooms[floor][i].vertex[2] += liftv;
						}
					}
			}
			else
			{
				onb = OnBoundary(floor, recRooms[floor][indr].vertex[0],
					recRooms[floor][indr].vertex[1]);
				if (onb) // on boundary, move the other edge
				{
					x = recRooms[floor][indr].vertex[2].x;
					recRooms[floor][indr].vertex[2] -= xv * dist;
					recRooms[floor][indr].vertex[3] -= xv * dist;
					liftv = xv * (-dist);
				}
				else
				{
					x = recRooms[floor][indr].vertex[0].x;
					recRooms[floor][indr].vertex[0] += xv * dist;
					recRooms[floor][indr].vertex[1] += xv * dist;
					liftv = xv * dist;
				}
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					if (i == indr) continue;
					if (Math.Abs(recRooms[floor][i].vertex[0].x - x) < thresh &&
						Math.Abs(recRooms[floor][i].vertex[1].x - x) < thresh)
					{
						recRooms[floor][i].vertex[0] += liftv;
						recRooms[floor][i].vertex[1] += liftv;
					}
					else if (Math.Abs(recRooms[floor][i].vertex[2].x - x) < thresh &&
						Math.Abs(recRooms[floor][i].vertex[3].x - x) < thresh)
					{
						recRooms[floor][i].vertex[2] += liftv;
						recRooms[floor][i].vertex[3] += liftv;
					}
				}
			}
		}

		public void LiftRoomWall(int floor, int indr, int lw, double dist)
		{
			// indr: which room
			// lw: len or wid (y or x)
			// dist : to lift
			int i;
			bool onb = false;
			double x, y;
			Vector2d xv = new Vector2d(1, 0), yv = new Vector2d(0, 1), liftv = new Vector2d();
			if (lw == 1) // Len -- Y
			{
				y = recRooms[floor][indr].vertex[1].y;
				recRooms[floor][indr].vertex[1] += yv * dist;
				recRooms[floor][indr].vertex[2] += yv * dist;
				liftv = yv * dist;

				for (i = 0; i < recRooms[floor].Count; i++)
				{
					if (i == indr) continue;
					if (Math.Abs(recRooms[floor][i].vertex[0].y - y) < thresh &&
						Math.Abs(recRooms[floor][i].vertex[3].y - y) < thresh)
					{
						recRooms[floor][i].vertex[0] += liftv;
						recRooms[floor][i].vertex[3] += liftv;
					}
					else if (Math.Abs(recRooms[floor][i].vertex[1].y - y) < thresh &&
						Math.Abs(recRooms[floor][i].vertex[2].y - y) < thresh)
					{
						recRooms[floor][i].vertex[1] += liftv;
						recRooms[floor][i].vertex[2] += liftv;
					}
				}
			}
			else
			{
				onb = OnBoundary(floor, recRooms[floor][indr].vertex[0],
					recRooms[floor][indr].vertex[1]);
				if (onb) // on boundary, move the other edge
				{
					x = recRooms[floor][indr].vertex[2].x;
					recRooms[floor][indr].vertex[2] -= xv * dist;
					recRooms[floor][indr].vertex[3] -= xv * dist;
					liftv = xv * (-dist);
				}
				else
				{
					x = recRooms[floor][indr].vertex[0].x;
					recRooms[floor][indr].vertex[0] += xv * dist;
					recRooms[floor][indr].vertex[1] += xv * dist;
					liftv = xv * dist;
				}
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					if (i == indr) continue;
					if (Math.Abs(recRooms[floor][i].vertex[0].x - x) < thresh &&
						Math.Abs(recRooms[floor][i].vertex[1].x - x) < thresh)
					{
						recRooms[floor][i].vertex[0] += liftv;
						recRooms[floor][i].vertex[1] += liftv;
					}
					else if (Math.Abs(recRooms[floor][i].vertex[2].x - x) < thresh &&
						Math.Abs(recRooms[floor][i].vertex[3].x - x) < thresh)
					{
						recRooms[floor][i].vertex[2] += liftv;
						recRooms[floor][i].vertex[3] += liftv;
					}
				}
			}
		}

		public void CheckConnectivity(int floor)
		{
			// Change rooms to find a better layout with more correct connections
			int i, j, r1, r2, maxm = 100, lastmiss = 0, minmiss, ind, stair = -1;
			double traito, tr1, tr2;
			bool cons;
			int[] missc = new int[recRooms[floor].Count];
			Vector2d tcenter;
			Random ra = new Random();
			List<Vector2d> tvertex;
			List<Room> currRooms = new List<Room>();

			// Missing connections
			for (i = 0; i < recRooms[floor].Count; i++)
				for (j = 0; j < recRooms[floor][i].conns.Count; j++)
				{
					if (recRooms[floor][i].conns[j] < i) continue; // Already checked
					cons = Connected(floor, i, recRooms[floor][i].conns[j]);
					if (!cons) lastmiss++;
				}
			Console.WriteLine("{0} : {1}", "Miss connections:", lastmiss);

			if (floor > 0 )
			{
				for (j = 0; j < recRooms[floor].Count; j++)
					if (String.Compare(recRooms[floor][j].name, "Stair") == 0)
					{
						stair = j;
						break;
					}
			}
			for (r1 = 0; r1 < recRooms[floor].Count - 1; r1++)
			{
				if (r1 == stair) continue;
				for (r2 = r1 + 1; r2 < recRooms[floor].Count; r2++)
				{
					if (r2 == stair) continue;
					// Save current rooms
					missc[r2] = maxm;
					currRooms = new List<Room>();
					tr1 = recRooms[floor][r1].asratio;
					tr2 = recRooms[floor][r2].asratio;
					if (tr1 > 1) tr1 = 1/tr1;
					if (tr2 > 1) tr2 = 1/tr2;
					if (Math.Abs(recRooms[floor][r1].area - recRooms[floor][r2].area) > 0.08 ||
						Math.Abs(tr1 - tr2) > 0.3) // If the area of two rooms are too different, don't switch
						continue;
					for (i = 0; i < recRooms[floor].Count; i++)
						currRooms.Add(new Room(recRooms[floor][i]));
					//tarea = recRooms[floor][r1].area;
					traito = recRooms[floor][r1].asratio;
					tcenter = recRooms[floor][r1].center;
					tvertex = new List<Vector2d>();
					for (i = 0; i < recRooms[floor][r1].vertex.Count; i++)
						tvertex.Add(new Vector2d(recRooms[floor][r1].vertex[i]));

					//recRooms[floor][r1].area = recRooms[floor][r2].area;
					recRooms[floor][r1].asratio = recRooms[floor][r2].asratio;
					recRooms[floor][r1].center = recRooms[floor][r2].center;
					recRooms[floor][r1].vertex = new List<Vector2d>();
					for (i = 0; i < recRooms[floor][r2].vertex.Count; i++)
						recRooms[floor][r1].vertex.Add(new Vector2d(recRooms[floor][r2].vertex[i]));

					//recRooms[floor][r2].area = tarea;
					recRooms[floor][r2].asratio = traito;
					recRooms[floor][r2].center = tcenter;
					recRooms[floor][r2].vertex = new List<Vector2d>();
					for (i = 0; i < tvertex.Count; i++)
						recRooms[floor][r2].vertex.Add(new Vector2d(tvertex[i]));
					missc[r2] = 0;
					// Missing connections
					for (i = 0; i < recRooms[floor].Count; i++)
						for (j = 0; j < recRooms[floor][i].conns.Count; j++)
						{
							if (recRooms[floor][i].conns[j] < i) continue; // Already checked
							cons = Connected(floor, i, recRooms[floor][i].conns[j]);
							if (!cons) missc[r2]++;
						}

					recRooms[floor] = new List<Room>();
					for (i = 0; i < currRooms.Count; i++)
						recRooms[floor].Add(new Room(currRooms[i]));
				}
				minmiss = maxm;
				ind = -1;
				for (r2 = r1 + 1; r2 < recRooms[floor].Count; r2++)
					if (missc[r2] < minmiss)
					{
						minmiss = missc[r2];
						ind = r2;
					}
				if (ind != -1 && minmiss < lastmiss) // Switch the two rooms
				{
					lastmiss = minmiss;
					r2 = ind;
					//tarea = recRooms[floor][r1].area;
					traito = recRooms[floor][r1].asratio;
					tcenter = recRooms[floor][r1].center;
					tvertex = new List<Vector2d>();
					for (i = 0; i < recRooms[floor][r1].vertex.Count; i++)
						tvertex.Add(new Vector2d(recRooms[floor][r1].vertex[i]));

					//recRooms[floor][r1].area = recRooms[floor][r2].area;
					recRooms[floor][r1].asratio = recRooms[floor][r2].asratio;
					recRooms[floor][r1].center = recRooms[floor][r2].center;
					recRooms[floor][r1].vertex = new List<Vector2d>();
					for (i = 0; i < recRooms[floor][r2].vertex.Count; i++)
						recRooms[floor][r1].vertex.Add(new Vector2d(recRooms[floor][r2].vertex[i]));

					//recRooms[floor][r2].area = tarea;
					recRooms[floor][r2].asratio = traito;
					recRooms[floor][r2].center = tcenter;
					recRooms[floor][r2].vertex = new List<Vector2d>();
					for (i = 0; i < tvertex.Count; i++)
						recRooms[floor][r2].vertex.Add(new Vector2d(tvertex[i]));
				}
			}
			Console.WriteLine("{0} {1}", "Miss connections after optim:", lastmiss);
		}

		public void AdjustAspectRatio(int floor)
		{
			// If there is a room has bad aspect ratio, fix it
			int i, j, k, ci, ind, ind1, ind2, lw = 0;
			double len, wid, diff, r0, r, thr = 0.1;
			Vector2d x = new Vector2d(1, 0), y = new Vector2d(0, 1), vt;
			List<int> modifs = new List<int>();
			List<Vector2d> vs = new List<Vector2d>();

			for (i = 0; i < recRooms[floor].Count; i++)
			{
				len = (recRooms[floor][i].vertex[0] - recRooms[floor][i].vertex[1]).Length();
				wid = (recRooms[floor][i].vertex[0] - recRooms[floor][i].vertex[3]).Length();
				if (recRooms[floor][i].asratio > 1)
					r0 = 1 / recRooms[floor][i].asratio;
				else
					r0 = recRooms[floor][i].asratio;
				r = len / wid;
				if (r > 1)
					r = wid / len;
				diff = r;
				if (LayoutInd == 5)
					thr = 0.2;
				else thr = 0.1;
				if (diff < thr || (floor > 0 && Math.Max(r/r0, r0/r) > 5))
					modifs.Add(i);
			}
			for (i = 0; i < modifs.Count; i++)
			{
				ci = modifs[i];
				len = (recRooms[floor][ci].vertex[0] - recRooms[floor][ci].vertex[1]).Length();
				wid = (recRooms[floor][ci].vertex[0] - recRooms[floor][ci].vertex[3]).Length();
				if (len > wid) lw = 1; // need to change the length --- by horizontal cut
				else lw = 2;
				ind = -1;
				for (j = 0; j < recRooms[floor].Count; j++)
				{
					if (ci == j) continue;
					ind1 = -1;
					ind2 = -1;
					for (k = 0; k < 4; k++)
					{
						// Find the two vertices of the shared edge of room j with the room ci to be modified
 						for (int t = 0; t < 4; t++)
							if ((recRooms[floor][ci].vertex[k] - recRooms[floor][j].vertex[t]).Length() < thresh)
							{
								if (ind1 != -1)
								{
									ind2 = k;
									break;
								}
								else
									ind1 = k;
							}
						if (ind1 == -1 || ind2 == -1) continue;
						if ((lw == 1 && Math.Abs(len -
							(recRooms[floor][ci].vertex[ind1] - recRooms[floor][ci].vertex[ind2]).Length()) < thresh) ||
							(lw == 2 && Math.Abs(wid -
							(recRooms[floor][ci].vertex[ind1] - recRooms[floor][ci].vertex[ind2]).Length()) < thresh))
						{
							ind = j;
							break;
						}
					}
					if (ind != -1)
						break;
				}
				if (ind != -1) // find the one to help modify ratio
				{
					vs = new List<Vector2d>();
					vs.Add(new Vector2d(Math.Max(recRooms[floor][ci].vertex[0].x, recRooms[floor][ind].vertex[0].x),
						Math.Min(recRooms[floor][ci].vertex[0].y, recRooms[floor][ind].vertex[0].y)));
					vs.Add(new Vector2d(vs[0].x,
						Math.Max(recRooms[floor][ci].vertex[1].y, recRooms[floor][ind].vertex[1].y)));
					vs.Add(new Vector2d(Math.Min(recRooms[floor][ci].vertex[2].x, recRooms[floor][ind].vertex[2].x),
						vs[1].y));
					vs.Add(new Vector2d(vs[2].x, vs[0].y));
					diff = recRooms[floor][ci].area / (recRooms[floor][ci].area + recRooms[floor][ind].area);
					if (lw == 1) // horizontal cut
					{		
						len = (vs[0] - vs[1]).Length() * diff;
						recRooms[floor][ind].vertex = new List<Vector2d>();
						recRooms[floor][ind].vertex.Add(new Vector2d(vs[0]));
						recRooms[floor][ind].vertex.Add(new Vector2d(vs[0] + y * len));
						recRooms[floor][ind].vertex.Add(new Vector2d(vs[3] + y * len));
						recRooms[floor][ind].vertex.Add(new Vector2d(vs[3]));
						vt = new Vector2d();
						for (int t = 0; t < 4; t++)
							vt += recRooms[floor][ind].vertex[t];
						recRooms[floor][ind].center = vt / 4;

						recRooms[floor][ci].vertex = new List<Vector2d>();
						recRooms[floor][ci].vertex.Add(new Vector2d(vs[0] + y * len));
						recRooms[floor][ci].vertex.Add(new Vector2d(vs[1]));
						recRooms[floor][ci].vertex.Add(new Vector2d(vs[2]));
						recRooms[floor][ci].vertex.Add(new Vector2d(vs[3] + y * len));
						vt = new Vector2d();
						for (int t = 0; t < 4; t++)
							vt += recRooms[floor][ci].vertex[t];
						recRooms[floor][ci].center = vt / 4;
					}
					else // vertical cut
					{
						wid = (vs[0] - vs[3]).Length() * diff;
						recRooms[floor][ci].vertex = new List<Vector2d>();
						recRooms[floor][ci].vertex.Add(new Vector2d(vs[0]));
						recRooms[floor][ci].vertex.Add(new Vector2d(vs[1]));
						recRooms[floor][ci].vertex.Add(new Vector2d(vs[1] - x * wid));
						recRooms[floor][ci].vertex.Add(new Vector2d(vs[0] - x * wid));
						vt = new Vector2d();
						for (int t = 0; t < 4; t++)
							vt += recRooms[floor][ci].vertex[t];
						recRooms[floor][ci].center = vt / 4;

						recRooms[floor][ind].vertex = new List<Vector2d>();
						recRooms[floor][ind].vertex.Add(new Vector2d(vs[0] - x * wid));
						recRooms[floor][ind].vertex.Add(new Vector2d(vs[1] - x * wid));
						recRooms[floor][ind].vertex.Add(new Vector2d(vs[2]));
						recRooms[floor][ind].vertex.Add(new Vector2d(vs[3]));
						vt = new Vector2d();
						for (int t = 0; t < 4; t++)
							vt += recRooms[floor][ind].vertex[t];
						recRooms[floor][ind].center = vt / 4;
					}
				}
			}
		}

		public List<List<int>> ConnectedRooms(int floor, int ind1, int ind2, List<int> inds)
		{
			// Find all connected rooms with Room ind 
			// for the rooms connected to both ind and ind2,
			// assign it to the key room group with more connections
			List<int> conns1 = new List<int>(), conns2 = new List<int>();
			List<int> inters = new List<int>(), tp1, tp2;
			List<List<int>> conn = new List<List<int>>();
			int i, j;
			bool added;

			for (j = 0; j < inds.Count; j++)
			{
				i = inds[j]; 
				if (i == ind1 || i == ind2)
					continue;
				if (recRooms[floor][i].conns.IndexOf(ind1) != -1 &&
					recRooms[floor][i].conns.IndexOf(ind2) == -1)
					conns1.Add(i);
				if (recRooms[floor][i].conns.IndexOf(ind2) != -1 &&
					recRooms[floor][i].conns.IndexOf(ind1) == -1)
					conns2.Add(i);
				if (recRooms[floor][i].conns.IndexOf(ind1) != -1 &&
					recRooms[floor][i].conns.IndexOf(ind2) != -1)
					inters.Add(i);
			}
			// Belong to both groups
			for (i = 0; i < inters.Count; i++)
			{
				tp1 = Intersect(recRooms[floor][inters[i]].conns, conns1);
				tp2 = Intersect(recRooms[floor][inters[i]].conns, conns2);
				if (tp1.Count > tp2.Count)
					conns1.Add(inters[i]);
				else //if (tp1.Count < tp2.Count)
					conns2.Add(inters[i]);
				//else
				//    if (recRooms[floor][ind1].area > recRooms[floor][ind2].area)
				//        conns1.Add(inters[i]);
				//    else
				//        conns2.Add(inters[i]);
			}
			conns1.Add(ind1);
			conns2.Add(ind2);
			for (j = 0; j < inds.Count; j++)
			{
				i = inds[j];
				if (conns1.IndexOf(i) != -1 || conns2.IndexOf(i) != -1)
					continue;
				added = false;
				for (int k = 0; k < conns1.Count; k++)
					if (recRooms[floor][i].conns.IndexOf(conns1[k]) != -1)
					{
						conns1.Add(i);
						added = true;
						break;
					}
				if (!added)
					conns2.Add(i);
			}
			Sort(conns1);
			Sort(conns2);
			conn.Add(conns1);
			conn.Add(conns2);
			return (conn);
		}

		public int[] KeyRooms(int floor, List<int> nums, List<int> inds)
		{
			// Find the 2 rooms with most connections
			int[] m = new int[2];
			int i, j;
			if (nums[0] > nums[1])
			{
				m[0] = inds[0]; 
				m[1] = inds[1];
			}
			else
			{
				m[0] = inds[1]; 
				m[1] = inds[0];
			}
			for (j = 2; j < inds.Count; j++)
			{
				i = inds[j];
				if (recRooms[floor][i].conns.Count >= m[1])
				{
					m[0] = m[1];
					m[1] = i;
				}
				else if (recRooms[floor][i].conns.Count > m[0])
					m[0] = i;
			}
			return (m);
		}

		public bool Connected(int floor, int ind1, int ind2)
		{
			bool c = false;
			int i, j, num = 0;
			for ( i = 0; i < recRooms[floor][ind1].vertex.Count; i++)
				if (PointInPoly(recRooms[floor][ind1].vertex[i], recRooms[floor][ind2].vertex))
				{
					c = true;
					break;
				}
			if (c) return (c);
			for (i = 0; i < recRooms[floor][ind2].vertex.Count; i++)
				if (PointInPoly(recRooms[floor][ind2].vertex[i], recRooms[floor][ind1].vertex))
				{
					c = true;
					break;
				}
			if (c) return (c);
			for (i = 0; i < recRooms[floor][ind1].vertex.Count; i++)
			{
				for (j = 0; j < recRooms[floor][ind2].vertex.Count; j++)
				{
					if (Point_On_Line(recRooms[floor][ind1].vertex[i], recRooms[floor][ind1].vertex[(i + 1) % recRooms[floor][ind1].vertex.Count],
						recRooms[floor][ind2].vertex[j]))
						num++;
					if (num > 2)
					{
						c = true;
						break;
					}
				}
				if (c) return (c);
			}

			num = 0;
			for (i = 0; i < recRooms[floor][ind2].vertex.Count; i++)
			{
				for (j = 0; j < recRooms[floor][ind1].vertex.Count; j++)
				{
					if (Point_On_Line(recRooms[floor][ind2].vertex[i], recRooms[floor][ind2].vertex[(i + 1) % recRooms[floor][ind2].vertex.Count],
						recRooms[floor][ind1].vertex[j]))
						num++;
					if (num >= 2)
					{
						c = true;
						break;
					}
				}
				if (c) return (c);
			}
			return (c);
		}

		public bool Intersect(Vector2d p1, Vector2d p2, Vector2d p3, Vector2d p4)
		{
			bool inters = false;
			double mult;
			Vector2d v1, v2, v3;
			v1 = p1 - p3;
			v2 = p2 - p3;
			v3 = p4 - p3;
			mult = (v1.x * v3.y - v1.y * v3.x) * (v2.x * v3.y - v2.y * v3.x);
			if (mult <= 0)
				inters = true;
			return (inters);
		}

		public List<Vector3d> FindIncludedPoint(int floor, Vector3d v1, Vector3d v2)
		{
			int i, j;
			double dist;
			List<Vector3d> vs = new List<Vector3d>(), verts = new List<Vector3d>();
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				for (j = 0; j < recRooms[floor][i].vertex.Count; j++)
					if (Point_In_Line(v1, v2, new Vector3d(recRooms[floor][i].vertex[j].x, recRooms[floor][i].vertex[j].y, 0)))
						vs.Add(new Vector3d(recRooms[floor][i].vertex[j].x, recRooms[floor][i].vertex[j].y, 0));
			}
			// sort
			if (vs.Count > 0)
			{
				verts.Add(vs[0]);
				for (i = 1; i < vs.Count; i++)
				{
					dist = (vs[i] - v1).Length();
					for (j = i - 1; j >= 0; j--)
					{
						if (dist > (vs[j] - v1).Length())
						{
							verts.Insert(j, vs[i]);
							break;
						}
					}
					if (j == -1)
						verts.Insert(0, vs[i]);
				}
			}
			return (verts);
		}


		//////////////////////////// For initialization (up) ////////////////////////////

		bool formed0, formed1;
		public void BuildWalls()
		{
			// Construct wall structure

			for (int i = 0; i < numFloor; i++)
			{
				walls.Add(new List<Wall>());
				cuboids.Add(new List<Cuboid>());
				doors.Add(new List<Door>());
				currFloor = i;
				BuildWallOnAFloor(i);
				FindRoomVertices(i);
				FindBoundRoom(i);
				AddConnection(i, true);
			}
			ConnectGraphAfter();
			//ConnectionRepeatCheck();
			this.Refresh();
		}

		public void FindBoundRoom(int floor)
		{
			int i, j;
			bool onbound = false, onboundOpt = false;
			Vector2d v1 = new Vector2d(), v2 = new Vector2d();
			double boundlen;
			int xy = -1;
			bdInds = new List<int>[bounds[floor].Count];
			bdOptInds = new List<int>[bounds[floor].Count];

			for (i = 0; i < bounds[floor].Count; i++)
			{
				bdInds[i] = new List<int>();
				bdOptInds[i] = new List<int>();
				//Console.WriteLine("{0} {1}:", "Number", i);
				boundlen = (bounds[floor][i] - bounds[floor][(i + 1) % bounds[floor].Count]).Length();
				if (Math.Abs(bounds[floor][i].x - bounds[floor][(i + 1) % bounds[floor].Count].x) < thresh)
					xy = 0;
				else
					xy = 1;
				for (j = 0; j < recRooms[floor].Count; j++)
				{
					for (int k = 0; k < recRooms[floor][j].vertices.Count; k++)
					{
						v1 = new Vector2d(recRooms[floor][j].vertices[k].Coord.x,
							recRooms[floor][j].vertices[k].Coord.y);
						v2 = new Vector2d(recRooms[floor][j].vertices[(k + 1) % recRooms[floor][j].vertices.Count].Coord.x,
							recRooms[floor][j].vertices[(k + 1) % recRooms[floor][j].vertices.Count].Coord.y);
						onbound = OnBoundaryInd(floor, i, v1, v2);
						if ((v1 - v2).Length() > 0.5 * recRooms[floor][j].length)
							onboundOpt = true;
						else
							onboundOpt = false;
						if (onbound)
							break;
					}
					if (onbound)
					{
						bdInds[i].Add(j);
						if (onboundOpt)
							bdOptInds[i].Add(j);
						//Console.WriteLine("{0}: {1}", j, recRooms[floor][j].name);
					}
				}
				//Console.WriteLine(boundlen);
				//// Order
				for (j = 0; j < bdInds[i].Count - 1; j++)
					for (int t = j + 1; t < bdInds[i].Count; t++)
					{
						if (xy == 0)
						{
							if (recRooms[floor][bdInds[i][j]].center.y > recRooms[floor][bdInds[i][t]].center.y)
							{
								int tp = bdInds[i][j];
								bdInds[i][j] = bdInds[i][t];
								bdInds[i][t] = tp;
							}
						}
						else
						{
							if (recRooms[floor][bdInds[i][j]].center.x > recRooms[floor][bdInds[i][t]].center.x)
							{
								int tp = bdInds[i][j];
								bdInds[i][j] = bdInds[i][t];
								bdInds[i][t] = tp;
							}
						}
					}
			}
		}

		public void BuildWallOnAFloor(int floor)
		{
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			int i, j, k;
			int n, nw = 0, type;
			List<Vertex> verts;
			Vertex p0, p1;
			Vertex lastp0 = new Vertex(0, 0, 0), lastp1 = new Vertex(0, 0, 0), v = new Vertex(0, 0, 0);
			Vector3d normal, direction;
			List<Vector3d> vmid;
			List<int> indp0, indp1;
			double dist = wallThickness / 2;
			double max, min, value, threshold = 1e-3;
			List<Wall> ws = new List<Wall>();
			// Fix rounding error between walls
			for (i = 0; i < recRooms[floor].Count-1; i++)
			{
				for (int p = 0; p < recRooms[floor][i].vertex.Count; p++)
				{
					for (j = i + 1; j < recRooms[floor].Count; j++)
					{
						for (int q = 0; q < recRooms[floor][j].vertex.Count; q++)
							if (Math.Abs(recRooms[floor][j].vertex[q].x -
								recRooms[floor][i].vertex[p].x) < threshold)
								recRooms[floor][j].vertex[q] = new Vector2d(
									recRooms[floor][i].vertex[p].x, recRooms[floor][j].vertex[q].y);
							else if (Math.Abs(recRooms[floor][j].vertex[q].y -
								recRooms[floor][i].vertex[p].y) < threshold)
								recRooms[floor][j].vertex[q] = new Vector2d(
									 recRooms[floor][j].vertex[q].x, recRooms[floor][i].vertex[p].y);
					}
				}
			}
			// Initialize rough walls[floor] including repetition
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				n = recRooms[floor][i].vertex.Count;
				for (j = 0; j < n; j++)
				{
					p0 = new Vertex(recRooms[floor][i].vertex[j].x, recRooms[floor][i].vertex[j].y, 0);
					p1 = new Vertex(recRooms[floor][i].vertex[(j + 1) % n].x, recRooms[floor][i].vertex[(j + 1) % n].y, 0);
					vmid = FindIncludedPoint(floor, p0.Coord, p1.Coord);
					if (vmid.Count == 0)
						ws.Add(new Wall(p0, p1));
					else
					{
						Vertex vm;
						vmid.Add(p1.Coord);
						Distinct(vmid);
						for (int t = 0; t < vmid.Count; t++)
						{
							vm = new Vertex(vmid[t]);
							ws.Add(new Wall(p0, vm));
							p0 = vm;
						}
					}
				}
			}

			for (i = 0; i < recRooms[floor].Count; i++)
			{
				n = recRooms[floor][i].vertex.Count;
				for (j = 0; j < n; j++)
				{
					verts = new List<Vertex>();
					p0 = new Vertex(recRooms[floor][i].vertex[j].x, recRooms[floor][i].vertex[j].y, 0);
					p1 = new Vertex(recRooms[floor][i].vertex[(j + 1) % n].x, recRooms[floor][i].vertex[(j + 1) % n].y, 0);
					direction = p1.Coord - p0.Coord;
					direction = direction.Normalize();
					// To locate vertices endpoints, find out their neighbors in direction 0,1,2
					normal = CalNormal(p0, p1);
					type = WallType(p0, p1);
					// Already formed a wall
					formed0 = false; formed1 = false;
					indp0 = SearchFormedWallVertices(p0, type, p1, 0);
					indp1 = SearchFormedWallVertices(p1, type, p0, 1);
					for (int t = 0; t < indp0.Count; t++)
						walls[floor][indp0[t]].roomIndices.Add(i);
					for (int t = 0; t < indp1.Count; t++)
						walls[floor][indp1[t]].roomIndices.Add(i);
					if (formed0 && formed1)
						continue;
					else if (formed0)	//endpoint 0 is used
					{
						// Find the point closest to 
						if (type == 0 || type == 1)
						{
							max = double.MinValue;
							for (k = 0; k < indp0.Count; k++)
							{
								for (int p = 0; p < 2; p++)
								{
									if (type == 0) value = walls[floor][indp0[k]].endPoints[p].Coord.y;
									else value = walls[floor][indp0[k]].endPoints[p].Coord.x;
									if (value > max)
									{
										max = value;
										v = walls[floor][indp0[k]].endPoints[p];
									}
								}
							}
						}
						else
						{
							min = double.MaxValue;
							for (k = 0; k < indp0.Count; k++)
							{
								for (int p = 0; p < 2; p++)
								{
									if (type == 2) value = walls[floor][indp0[k]].endPoints[p].Coord.y;
									else value = walls[floor][indp0[k]].endPoints[p].Coord.x;
									if (value < min)
									{
										min = value;
										v = walls[floor][indp0[k]].endPoints[p];
									}
								}
							}
						}

						p0 = new Vertex(v);
					}
					else if (formed1)	//endpoint 1 is used
					{
						if (type == 0 || type == 1)
						{
							min = double.MaxValue;
							for (k = 0; k < indp1.Count; k++)
							{
								if (type == 0) value = walls[floor][indp1[k]].endPoints[0].Coord.y;
								else value = walls[floor][indp1[k]].endPoints[0].Coord.x;
								if (value < min)
								{
									min = value;
									v = walls[floor][indp1[k]].endPoints[0];
								}
								if (type == 0) value = walls[floor][indp1[k]].endPoints[1].Coord.y;
								else value = walls[floor][indp1[k]].endPoints[1].Coord.x;
								if (value < min)
								{
									min = value;
									v = walls[floor][indp1[k]].endPoints[1];
								}
							}
						}
						else
						{
							max = double.MinValue;
							for (k = 0; k < indp1.Count; k++)
							{
								if (type == 2) value = walls[floor][indp1[k]].endPoints[0].Coord.y;
								else value = walls[floor][indp1[k]].endPoints[0].Coord.x;
								if (value > max)
								{
									max = value;
									v = walls[floor][indp1[k]].endPoints[0];
								}
								if (type == 2) value = walls[floor][indp1[k]].endPoints[1].Coord.y;
								else value = walls[floor][indp1[k]].endPoints[1].Coord.x;
								if (value > max)
								{
									max = value;
									v = walls[floor][indp1[k]].endPoints[1];
								}
							}
						}
						p1 = new Vertex(v);
					}

					// Check whether exist a "midpoint"
					vmid = FindIncludedPoint(floor, p0.Coord, p1.Coord);
					Distinct(vmid);
					if (vmid.Count == 0)
					{
						walls[floor].Add(new Wall(p0, p1));
						nw = walls[floor].Count - 1;
						walls[floor][nw].roomIndices.Add(i);
						walls[floor][nw].direction = direction;
						walls[floor][nw].normal = normal;

						verts = NewWall(floor, p0, p1, dist, nw);
						walls[floor][nw].vertexType[0] = vertexType[0];
						walls[floor][nw].vertexType[1] = vertexType[1];
						walls[floor][nw].type = type;
						walls[floor][nw].cuboid = new Cuboid(1, nw, p0, p1, verts, wallThickness, wallHeight, wallElevation,
						walls[floor][nw].direction, walls[floor][nw].normal, mat);
						walls[floor][nw].cuboid.wallIndex = nw;
						cuboids[floor].Add(walls[floor][nw].cuboid);
						walls[floor][nw].cuboidIndex = cuboids[floor].Count - 1;
					}
					else
					{
						vmid.Add(p1.Coord);
						List<int> middle = Intersect(indp0, indp1);
						for (int t = 0; t < vmid.Count; t++)
						{
							Vertex vn = new Vertex(vmid[t]);
							bool jump = false;

							for (int p = 0; p < middle.Count; p++)
							{
								if (((p0.Coord - walls[floor][middle[p]].endPoints[0].Coord).Length() < thresh
									&& (vn.Coord - walls[floor][middle[p]].endPoints[1].Coord).Length() < thresh) ||
									((p0.Coord - walls[floor][middle[p]].endPoints[1].Coord).Length() < thresh
									&& (vn.Coord - walls[floor][middle[p]].endPoints[0].Coord).Length() < thresh))
								{
									jump = true;
									p0 = new Vertex(vn);
									walls[floor][middle[p]].roomIndices.Add(i);
									middle.RemoveAt(p);
									break;
								}
							}
							if (jump)
								continue;

							walls[floor].Add(new Wall(new Vertex(p0), vn));
							nw = walls[floor].Count - 1;
							walls[floor][nw].roomIndices.Add(i);
							verts = NewWall(floor, p0, vn, dist, nw);
							walls[floor][nw].vertexType[0] = vertexType[0];
							walls[floor][nw].vertexType[1] = vertexType[1];
							walls[floor][nw].type = type;
							walls[floor][nw].cuboid = new Cuboid(1, nw, p0, vn, verts, wallThickness, wallHeight, wallElevation,
							walls[floor][nw].direction, walls[floor][nw].normal, mat);
							cuboids[floor].Add(walls[floor][nw].cuboid);
							walls[floor][nw].direction = direction;
							walls[floor][nw].normal = normal;
							walls[floor][nw].cuboidIndex = cuboids[floor].Count - 1;
							p0 = new Vertex(vn);
						}
					}
				}
			}

			for (i = 0; i < recRooms[floor].Count; i++)
				recRooms[floor][i].wallIndices = new List<int>();
			for (i = 0; i < walls[floor].Count; i++)
				Distinct(walls[floor][i].roomIndices);
			FindRoomWalls(floor);
		}

		public int LocateStair(int floor)
		{
			int stair = -1;
			for (int i = 0; i < recRooms[floor].Count; i++)
				if (recRooms[floor][i].name == "Stair")
				{
					stair = i;
					break;
				}
			return (stair);
		}

		List<int> activeRooms;
		public bool OnMoveWall(int floor)
		{
			// React after moving a wall
			bool opt = false;
			Vector2d v1, v2;
			int[] stairs = new int[numFloor];
			Room[] stairRs = new Room[numFloor];
			if (activeFloor == -1 || moveWall[1] == -1) return(false);
			v1 = new Vector2d(orv0.Coord.x, orv0.Coord.y);
			v2 = new Vector2d(orv1.Coord.x, orv1.Coord.y);
			// First, translate the wall to a new location
			MoveWall(1);
			this.Refresh();

			// Find the two rooms
			activeRooms = new List<int>();
			for (int i = 0; i < recRooms[floor].Count; i++)
			{
				if (recRooms[floor][i].wallIndices.IndexOf(moveWall[1]) != -1)
					activeRooms.Add(i);
			}
			moveCuboid[0] = -1;
			moveCuboid[1] = -1;
			moveWall[0] = -1;
			moveWall[1] = -1;
			opt = Optimization(floor);
			return (opt);
		}

		public bool Optimization(int floor)
		{
			int f, s = -1, i;
			bool opt;
			opt = GlobalOptim(floor, false);
			this.Refresh();
			if (!opt) 
				return(opt);
			int second = 200;
			System.Threading.Thread.Sleep(second);
			FindGapOnBound(floor);
			this.Refresh();
			System.Threading.Thread.Sleep(second);
			FixRounding(floor);
			this.Refresh();
			System.Threading.Thread.Sleep(second);
			FindInsideGaps(floor);
			this.Refresh();
			System.Threading.Thread.Sleep(second);
			FindOverlaps(floor,1);
			this.Refresh();
			System.Threading.Thread.Sleep(second);

			SnapWall(floor);
			this.Refresh();

			if (numFloor > 1)
			{
				for (i = 0; i < recRooms[floor].Count; i++)
					if (recRooms[floor][i].name == "Stair")
					{
						s = i;
						break;
					}
				// Adjust stairs
				for (f = 0; f < numFloor; f++)
				{
					if (f == floor) continue;
					for (i = 0; i < recRooms[f].Count; i++)
						if (recRooms[f][i].name == "Stair")
						{
							AdjustStair(f, i, floor, s);
							this.Refresh();
							BuildWalls();
							this.Refresh();
							FindRoomVertices(f);
							// stair
							GetNeigborsOnAFloor(f);
							GlobalOptim(f, true);
							this.Refresh();
							System.Threading.Thread.Sleep(800);
							FindGapOnBound(f);
							FixRounding(f);
							this.Refresh();
							System.Threading.Thread.Sleep(800);
							FindInsideGaps(f);
							this.Refresh();
							System.Threading.Thread.Sleep(800);
							FindOverlaps(f,1);
							this.Refresh();
							System.Threading.Thread.Sleep(800);
							//hasgap = true;
							//hasoverlap = true;
							//while (hasgap || hasoverlap)
							//{
							//    hasgap = FindInsideGaps(f);
							//    hasoverlap = FindOverlaps(f,2);
							//}

							SnapWall(f);
							this.Refresh();
							break;
						}
				}
			}
			BuildWalls();
			for (f = 0; f < recRooms.Count; f++)
			{
				FindRoomVertices(f);
				for (i = 0; i < recRooms[f].Count; i++)
					ApproxRect(f, i);
			}
			//ConnectionRepeatCheck();
			this.Refresh();
			return (opt);
		}

		public void SwitchRooms()
		{
			int floor, r1, r2;
			Vector2d center;
			double len, wid;
			floor = toswitch[0, 0];
			r1 = toswitch[0, 1];
			r2 = toswitch[1, 1];
			
			center = recRooms[floor][r1].center;
			len = recRooms[floor][r1].length;
			wid = recRooms[floor][r1].width;
			recRooms[floor][r1].center = recRooms[floor][r2].center;
			recRooms[floor][r1].length = recRooms[floor][r2].length;
			recRooms[floor][r1].width = recRooms[floor][r2].width;
			recRooms[floor][r2].center = center;
			recRooms[floor][r2].length = len;
			recRooms[floor][r2].width = wid;

			List<int> winds_r1 = recRooms[floor][r1].wallIndices,
				winds_r2 = recRooms[floor][r2].wallIndices;
			int w, wr, i, j;
			for (i = 0; i < recRooms[floor][r1].wallIndices.Count; i++)
			{
				w = recRooms[floor][r1].wallIndices[i];
				wr = walls[floor][w].roomIndices.IndexOf(r1);
				walls[floor][w].roomIndices[wr] = r2;
			}
			for (i = 0; i < recRooms[floor][r2].wallIndices.Count; i++)
			{
				w = recRooms[floor][r2].wallIndices[i];
				wr = walls[floor][w].roomIndices.IndexOf(r2);
				walls[floor][w].roomIndices[wr] = r1;
			}
			recRooms[floor][r1].wallIndices = winds_r2;
			recRooms[floor][r2].wallIndices = winds_r1;
			for (i = 0; i < bdInds.Count(); i++)
				for (j = 0; j < bdInds[i].Count; j++)
				{
					if (bdInds[i][j] == r1)
						bdInds[i][j] = r2;
					else if (bdInds[i][j] == r2)
						bdInds[i][j] = r1;
				}

			UpdateVertex(floor, r1);
			UpdateVertex(floor, r2);
			FindRoomVertices(floor);
			GetNeigborsOnAFloor(floor); // include find rooms onboundary
			//this.Refresh();
			System.Threading.Thread.Sleep(100);
			toswitch = new int[2, 2] { { -1, -1 }, { -1, -1 } };

			if (isOpt)
				Optimization(floor);

		}

		public void GetNeigborsOnAFloor(int floor)
		{
			int i, j;
			neigbs = new List<int>[recRooms[floor].Count * 4];
			List<int>[] nbs;
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				nbs = new List<int>[4];
				// X + dir neighbors --- distance of centers belong to a range
				neigbs[i * 4] = new List<int>();
				nbs = NeighborInFourDir(floor, i, false);

				for (j = 0; j < nbs[0].Count; j++)
				{
					//if (nbs[0][j] < i) continue; //already used
					neigbs[i * 4].Add(nbs[0][j]);
				}
				// X - dir neighbors
				neigbs[i * 4 + 1] = new List<int>();
				for (j = 0; j < nbs[1].Count; j++)
				{
					//if (nbs[1][j] < i) continue; //already used
					neigbs[i * 4 + 1].Add(nbs[1][j]);
				}
				// Y + dir neighbors
				neigbs[i * 4 + 2] = new List<int>();
				for (j = 0; j < nbs[2].Count; j++)
				{
					//if (nbs[2][j] < i) continue; //already used
					neigbs[i * 4 + 2].Add(nbs[2][j]);
				}
				// Y - dir neighbors
				neigbs[i * 4 + 3] = new List<int>();
				for (j = 0; j < nbs[3].Count; j++)
				{
					//if (nbs[3][j] < i) continue; //already used
					neigbs[i * 4 + 3].Add(nbs[3][j]);
				}
			}
			FindBoundRoom(floor);
		}
		public void AdjustStair(int floor, int ind, int fl, int s)
		{
			int i, j, k, ro, bo = -1, ind1 = -1, ind2 = -1;
			List<int>[] nbs = new List<int>[4];
			bool onBound = false;
			double dist = 0;
			Vector2d x = new Vector2d(1, 0), y = new Vector2d(0, 1), v;
			List<Vector2d> verts = new List<Vector2d>();

			nbs = NeighborInFourDir(floor, ind, true);
			for (j = 0; j < recRooms[floor][ind].vertex.Count; j++)
				verts.Add(new Vector2d(recRooms[floor][ind].vertex[j]));
			for (j = 0; j < recRooms[floor][ind].vertex.Count; j++)
			{
				for (k = 0; k < bounds[floor].Count; k++)
				{
					onBound = OnBoundaryInd(floor, k, 
						recRooms[floor][ind].vertex[j], 
						recRooms[floor][ind].vertex[(j+1)%recRooms[floor][ind].vertex.Count]);
					if (onBound)
					{
						bo = k;
						ro = j;
						break;
					}
				}
				if (onBound) break;
			}
			v = new Vector2d();
			for (j = 0; j < recRooms[fl][s].vertex.Count; j++)
				v += recRooms[fl][s].vertex[j];
			recRooms[fl][s].center = v / 4;
			recRooms[fl][s].width = (recRooms[fl][s].vertex[0] - recRooms[fl][s].vertex[3]).Length();
			recRooms[fl][s].length = (recRooms[fl][s].vertex[0] - recRooms[fl][s].vertex[1]).Length();
			recRooms[floor][ind].center.x = recRooms[fl][s].center.x + space * (floor - fl);
			recRooms[floor][ind].center.y = recRooms[fl][s].center.y;
			recRooms[floor][ind].length = recRooms[fl][s].length;
			recRooms[floor][ind].width = recRooms[fl][s].width;
			ResetVertex(floor, ind);
			
			for (j = 0; j < nbs[1].Count; j++) //horizontal left
			{
				k = nbs[1][j];
				for (i = 0; i < recRooms[floor][k].vertex.Count; i++)
				{
					if (Math.Abs(recRooms[floor][k].vertex[i].x -
						recRooms[floor][k].vertex[(i + 1) % recRooms[floor][k].vertex.Count].x) < thresh)
					{
						if (Math.Abs(recRooms[floor][k].vertex[i].x - verts[2].x) < thresh)
						{
							ind1 = i;
							ind2 = (i + 1) % recRooms[floor][k].vertex.Count;
						}
					}
				}
				dist = recRooms[floor][ind].vertex[2].x - recRooms[floor][k].vertex[ind1].x;
				recRooms[floor][k].vertex[ind1] = new Vector2d
					(recRooms[floor][ind].vertex[2].x, recRooms[floor][k].vertex[ind1].y);
				recRooms[floor][k].vertex[ind2] = new Vector2d
					(recRooms[floor][ind].vertex[2].x, recRooms[floor][k].vertex[ind2].y);
				recRooms[floor][k].width += dist;
				recRooms[floor][k].center += dist / 2 * x;
			}
			for (j = 0; j < nbs[0].Count; j++) //horizontal right
			{
				k = nbs[0][j];
				for (i = 0; i < recRooms[floor][k].vertex.Count; i++)
				{
					if (Math.Abs(recRooms[floor][k].vertex[i].x -
						recRooms[floor][k].vertex[(i + 1) % recRooms[floor][k].vertex.Count].x) < thresh)
					{
						if (Math.Abs(recRooms[floor][k].vertex[i].x - verts[0].x) < thresh)
						{
							ind1 = i;
							ind2 = (i + 1) % recRooms[floor][k].vertex.Count;
						}
					}
				}
				dist = recRooms[floor][ind].vertex[0].x - recRooms[floor][k].vertex[ind1].x;
				recRooms[floor][k].vertex[ind1] = new Vector2d
					(recRooms[floor][ind].vertex[0].x, recRooms[floor][k].vertex[ind1].y);
				recRooms[floor][k].vertex[ind2] = new Vector2d
					(recRooms[floor][ind].vertex[0].x, recRooms[floor][k].vertex[ind2].y);
				recRooms[floor][k].width -= dist;
				recRooms[floor][k].center += dist / 2 * x;
			}
			for (j = 0; j < nbs[2].Count; j++) // vertical upper
			{
				k = nbs[2][j];
				for (i = 0; i < recRooms[floor][k].vertex.Count; i++)
				{
					if (Math.Abs(recRooms[floor][k].vertex[i].y -
						recRooms[floor][k].vertex[(i + 1) % recRooms[floor][k].vertex.Count].y) < thresh)
					{
						if (Math.Abs(recRooms[floor][k].vertex[i].y - verts[1].y) < thresh)
						{
							ind1 = i;
							ind2 = (i + 1) % recRooms[floor][k].vertex.Count;
						}
					}
				}
				dist = recRooms[floor][ind].vertex[1].y - recRooms[floor][k].vertex[ind1].y;
				recRooms[floor][k].vertex[ind1] = new Vector2d
					(recRooms[floor][k].vertex[ind1].x, recRooms[floor][ind].vertex[1].y);
				recRooms[floor][k].vertex[ind2] = new Vector2d
					(recRooms[floor][k].vertex[ind2].x, recRooms[floor][ind].vertex[1].y);
				recRooms[floor][k].length -= dist;
				recRooms[floor][k].center += dist / 2 * y;
			}
			for (j = 0; j < nbs[3].Count; j++) // vertical down
			{
				k = nbs[3][j];
				for (i = 0; i < recRooms[floor][k].vertex.Count; i++)
				{
					if (Math.Abs(recRooms[floor][k].vertex[i].y -
						recRooms[floor][k].vertex[(i + 1) % recRooms[floor][k].vertex.Count].y) < thresh)
					{
						if (Math.Abs(recRooms[floor][k].vertex[i].y - verts[0].y) < thresh)
						{
							ind1 = i;
							ind2 = (i + 1) % recRooms[floor][k].vertex.Count;
						}
					}
				}
				dist = recRooms[floor][ind].vertex[0].y - recRooms[floor][k].vertex[1].y;
				recRooms[floor][k].vertex[1] = new Vector2d
					(recRooms[floor][k].vertex[1].x, recRooms[floor][ind].vertex[0].y);
				recRooms[floor][k].vertex[2] = new Vector2d
					(recRooms[floor][k].vertex[2].x, recRooms[floor][ind].vertex[0].y);
				recRooms[floor][k].length += dist;
				recRooms[floor][k].center += dist / 2 * y;
			}
			FindOverlaps(floor,1);
		}

		public List<int>[] NeighborInFourDir(int floor, int ind, bool repeat)
		{
			List<int>[] nbs = new List<int>[4];
			List<int> reps;

			nbs[0] = Neighbors(floor, ind, 0);
			nbs[1] = Neighbors(floor, ind, 1);
			nbs[2] = Neighbors(floor, ind, 2);
			nbs[3] = Neighbors(floor, ind, 3);
			if (repeat) return (nbs);
			// Check repetition -- for some rooms that are not rectangles, 
			// e.g., left and down neigbors of the other one, i.e., left half of 凹
			for (int n1 = 0; n1 < 3; n1++)
				for (int n2 = n1 + 1; n2 < 4; n2++)
				{
					reps = Intersect(nbs[n1], nbs[n2]);
					if (reps.Count > 0)
					{
						for (int n3 = 0; n3 < reps.Count; n3++)
						{
							//if (reps[n3] < ind) continue;
							if (Math.Abs(recRooms[floor][ind].center.x - recRooms[floor][reps[n3]].center.x) >
								Math.Abs(recRooms[floor][ind].center.y - recRooms[floor][reps[n3]].center.y))
							{
								if (n2 == 2 || n2 == 3)
									nbs[n2].Remove(reps[n3]);
								else
									nbs[n1].Remove(reps[n3]);
							}
							else
							{
								if (n2 == 0 || n2 == 1)
									nbs[n2].Remove(reps[n3]);
								else
									nbs[n1].Remove(reps[n3]);
							}
						}
					}
				}
			return (nbs);
		}
		// Optimization
		public int currentFloor = -1, optTimes = 0;
		List<List<IntPoint>> clipPolys = new List<List<IntPoint>>();
		List<int>[] neigbs, bdInds, bdOptInds; // bdInds is all rooms on boundary
		// bdOptInds are those affect optimization --- at least half of its length/width on the boundary
		double[] initialv;

		public bool GlobalOptim(int floor, bool tag)
		{
			int i, j, count, nvar, stair = -1;
			double l, w;
			double[] lw = new double[2], lwn = new double[2];
			List<double[]> cons = new List<double[]>();
			List<int> sign = new List<int>();
			List<int>[] nbs;
			double[,] c;
			int[] ct;
			bool opt = true;
			double newth = thresh;
			int optype = -3;
			double[] x = new double[1];

			currentFloor = floor;

			while (optype < 0)
			{
				cons = new List<double[]>();
				sign = new List<int>();
				// Variables -- center.x/center.y/Length/Width/
				nvar = recRooms[floor].Count * 4;
				initialv = new double[nvar];

				l = (bounds[floor][0] - bounds[floor][1]).Length();
				w = (bounds[floor][0] - bounds[floor][bounds[floor].Count - 1]).Length();

				for (i = 0; i < recRooms[floor].Count; i++)
				{
					if (String.Compare(recRooms[floor][i].name, "Stair") == 0)
						stair = i;
					initialv[i * 4] = recRooms[floor][i].center.x;
					initialv[i * 4 + 1] = recRooms[floor][i].center.y;
					initialv[i * 4 + 2] = recRooms[floor][i].length;
					initialv[i * 4 + 3] = recRooms[floor][i].width;

					nbs = new List<int>[4];
					// X + dir neighbors --- distance of centers belong to a range
					//neigbs[i * 4] = new List<int>();
					//nbs = NeighborInFourDir(floor, i, false);
					nbs[0] = neigbs[i * 4];
					nbs[1] = neigbs[i * 4 + 1];
					nbs[2] = neigbs[i * 4 + 2];
					nbs[3] = neigbs[i * 4 + 3];
					for (j = 0; j < nbs[0].Count; j++)
					{
						cons.Add(new double[nvar + 1]);
						count = cons.Count - 1;
						cons[count][i * 4] = -1;
						cons[count][nbs[0][j] * 4] = 1;
						cons[count][i * 4 + 3] = -0.5;
						cons[count][nbs[0][j] * 4 + 3] = -0.5;
						cons[count][nvar] = newth;
						sign.Add(-1);

						cons.Add(new double[nvar + 1]);
						count = cons.Count - 1;
						cons[count][i * 4] = -1;
						cons[count][nbs[0][j] * 4] = 1;
						cons[count][i * 4 + 3] = -0.5;
						cons[count][nbs[0][j] * 4 + 3] = -0.5;
						cons[count][nvar] = -newth;
						sign.Add(1);
					}
					// X - dir neighbors
					for (j = 0; j < nbs[1].Count; j++)
					{
						cons.Add(new double[nvar + 1]);
						count = cons.Count - 1;
						cons[count][i * 4] = 1;
						cons[count][nbs[1][j] * 4] = -1;
						cons[count][i * 4 + 3] = -0.5;
						cons[count][nbs[1][j] * 4 + 3] = -0.5;
						cons[count][nvar] = newth;
						sign.Add(-1);

						cons.Add(new double[nvar + 1]);
						count = cons.Count - 1;
						cons[count][i * 4] = 1;
						cons[count][nbs[1][j] * 4] = -1;
						cons[count][i * 4 + 3] = -0.5;
						cons[count][nbs[1][j] * 4 + 3] = -0.5;
						cons[count][nvar] = -newth;
						sign.Add(1);
					}
					// Y + dir neighbors
					for (j = 0; j < nbs[2].Count; j++)
					{
						cons.Add(new double[nvar + 1]);
						count = cons.Count - 1;
						cons[count][i * 4 + 1] = -1;
						cons[count][nbs[2][j] * 4 + 1] = 1;
						cons[count][i * 4 + 2] = -0.5;
						cons[count][nbs[2][j] * 4 + 2] = -0.5;
						cons[count][nvar] = newth;
						sign.Add(-1);

						cons.Add(new double[nvar + 1]);
						count = cons.Count - 1;
						cons[count][i * 4 + 1] = -1;
						cons[count][nbs[2][j] * 4 + 1] = 1;
						cons[count][i * 4 + 2] = -0.5;
						cons[count][nbs[2][j] * 4 + 2] = -0.5;
						cons[count][nvar] = -newth;
						sign.Add(1);
					}
					// Y - dir neighbors
					for (j = 0; j < nbs[3].Count; j++)
					{
						cons.Add(new double[nvar + 1]);
						count = cons.Count - 1;
						cons[count][i * 4 + 1] = 1;
						cons[count][nbs[3][j] * 4 + 1] = -1;
						cons[count][i * 4 + 2] = -0.5;
						cons[count][nbs[3][j] * 4 + 2] = -0.5;
						cons[count][nvar] = newth;
						sign.Add(-1);

						cons.Add(new double[nvar + 1]);
						count = cons.Count - 1;
						cons[count][i * 4 + 1] = 1;
						cons[count][nbs[3][j] * 4 + 1] = -1;
						cons[count][i * 4 + 2] = -0.5;
						cons[count][nbs[3][j] * 4 + 2] = -0.5;
						cons[count][nvar] = -newth;
						sign.Add(1);
					}
				}

				// Minimum length of walls
				double minw;
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					cons.Add(new double[nvar + 1]);
					count = cons.Count - 1;
					cons[count][i * 4 + 2] = 1;
					minw = Math.Min(minLenOfWall, initialv[i * 4 + 2]);
					cons[count][nvar] = minw;
					sign.Add(1);
					cons.Add(new double[nvar + 1]);
					count = cons.Count - 1;
					cons[count][i * 4 + 3] = 1;
					minw = Math.Min(minLenOfWall, initialv[i * 4 + 3]);
					cons[count][nvar] = minw;
					sign.Add(1);
				}
				// Stair constraint
				if (tag)
				{
					cons.Add(new double[nvar + 1]);
					count = cons.Count - 1;
					cons[count][stair * 4] = 1;
					cons[count][nvar] = initialv[stair * 4] + newth;
					sign.Add(-1);
					cons.Add(new double[nvar + 1]);
					count = cons.Count - 1;
					cons[count][stair * 4 + 1] = 1;
					cons[count][nvar] = initialv[stair * 4 + 1] - newth;
					sign.Add(1);
					cons.Add(new double[nvar + 1]);
					count = cons.Count - 1;
					cons[count][stair * 4 + 2] = 1;
					cons[count][nvar] = initialv[stair * 4 + 2] + newth;
					sign.Add(-1);
					cons.Add(new double[nvar + 1]);
					count = cons.Count - 1;
					cons[count][stair * 4 + 3] = 1;
					cons[count][nvar] = initialv[stair * 4 + 3] - newth;
					sign.Add(1);
				}
				// Boundary length
				// l1 + l2 +...+lk = L (2 inequlaity constraints)

				ct = sign.ToArray();
				c = new double[cons.Count, nvar + 1];
				for (i = 0; i < cons.Count; i++)
					for (j = 0; j < cons[i].Length; j++)
						c[i, j] = cons[i][j];


				double[] bndl = new double[nvar];
				double[] bndu = new double[nvar];
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					bndl[i * 4 + 2] = Math.Min(minLenOfWall, recRooms[floor][i].length);
					bndl[i * 4 + 3] = Math.Min(minLenOfWall, recRooms[floor][i].width);
					bndu[i * 4 + 2] = Math.Min(l, w);
					bndu[i * 4 + 3] = Math.Min(l, w);
				}
				double fun = GlobalFuncValue(initialv);
				Console.WriteLine("{0}: {1}", "The objective function values is", fun);
				// Optimization
				double epsg = 1e-9;
				double epsf = 0;
				double epsx = 0;

				alglib.minbleicstate state;
				alglib.minbleicreport rep;

				double epso = 1e-6;
				double epsi = 1e-6;

				x = new double[initialv.Length];
				for (int ii = 0; ii < initialv.Length; ii++)
					x[ii] = initialv[ii];
				//alglib.minbleiccreatef(x, diffstep, out state);
				alglib.minbleiccreate(x, out state);
				//alglib.minbleicsetbc(state, bndl, bndu);
				alglib.minbleicsetlc(state, c, ct);
				alglib.minbleicsetinnercond(state, epsg, epsf, epsx);
				alglib.minbleicsetoutercond(state, epso, epsi);
				alglib.minbleicoptimize(state, GlobalFunction, null, null);
				alglib.minbleicresults(state, out x, out rep);

				Console.WriteLine("{0}", rep.terminationtype);
				Console.WriteLine("{0}", alglib.ap.format(x, 2));
				optype = rep.terminationtype;
				newth += 0.01;
			}
			double funv = GlobalFuncValue(x);
			Console.WriteLine("{0}: {1}", "The objective function values is", funv);
			// After optimization
			double wid, len;
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				recRooms[floor][i].center.x = x[i * 4];
				recRooms[floor][i].center.y = x[i * 4 + 1];
				recRooms[floor][i].length = x[i * 4 + 2];
				recRooms[floor][i].width = x[i * 4 + 3];

				wid = recRooms[floor][i].width / 2;
				len = recRooms[floor][i].length / 2;

				recRooms[floor][i].vertex = new List<Vector2d>();
				recRooms[floor][i].vertex.Add(new Vector2d());
				recRooms[floor][i].vertex[0] = new Vector2d(recRooms[floor][i].center.x + wid, recRooms[floor][i].center.y - len);
				recRooms[floor][i].vertex.Add(new Vector2d());
				recRooms[floor][i].vertex[1] = new Vector2d(recRooms[floor][i].center.x + wid, recRooms[floor][i].center.y + len);
				recRooms[floor][i].vertex.Add(new Vector2d());
				recRooms[floor][i].vertex[2] = new Vector2d(recRooms[floor][i].center.x - wid, recRooms[floor][i].center.y + len);
				recRooms[floor][i].vertex.Add(new Vector2d());
				recRooms[floor][i].vertex[3] = new Vector2d(recRooms[floor][i].center.x - wid, recRooms[floor][i].center.y - len);
			}
			walls = new List<List<Wall>>();
			cuboids = new List<List<Cuboid>>();
			doors = new List<List<Door>>();
			return (opt);
		}

		public void GlobalFunction(double[] variables, ref double func, double[] grad, object obj)
		{
			int i, j, k, t, xy, diffc = 0;
			double len, boundlen, diffas = 0, tarea = 0, barea, difftarea;
			double w1 = 1, w2 = 1, w3 = 1, w4 = 1;
			Term funcv = 0, boundlenv, diffnv = 0, diffasv = 0, diffcv = 0, tareav = 0, difftareav;
			Variable[] vars = new Variable[variables.Length];

			for (i = 0; i < variables.Length; i++)
				vars[i] = new Variable();
			func = 0;

			for (i = 0; i < recRooms[currentFloor].Count; i++)
			{
				func += w1 * Math.Pow(variables[i * 4 + 2] * variables[i * 4 + 3] * 100 - recRooms[currentFloor][i].area * 100, 2);
				funcv += w1 * TermBuilder.Power(vars[i * 4 + 2] * vars[i * 4 + 3] * 100 - recRooms[currentFloor][i].area * 100, 2);
				tarea += variables[i * 4 + 2] * variables[i * 4 + 3] * 100;
				tareav = vars[i * 4 + 2] * vars[i * 4 + 3] * 100;
				if (initialv[i * 4 + 2] < initialv[i * 4 + 3])
				{
					diffas += Math.Pow(variables[i * 4 + 2] / variables[i * 4 + 3] - 1, 2);
					diffasv += TermBuilder.Power(vars[i * 4 + 2] / vars[i * 4 + 3] - 1, 2);
				}
				else
				{
					diffas += Math.Pow(variables[i * 4 + 3] / variables[i * 4 + 2] - 1, 2);
					diffasv += TermBuilder.Power(vars[i * 4 + 3] / vars[i * 4 + 2] - 1, 2);
				}
			}
			barea = PolygonArea(bounds[currentFloor]);
			difftarea = Math.Pow(tarea - barea, 2);
			difftareav = TermBuilder.Power(tareav - barea, 2);
			func += w4 * diffas;
			funcv += w4 * diffasv;

			for (i = 0; i < recRooms[currentFloor].Count; i++)
			{
				for (int lw = 2; lw < 4; lw++) // length and width
				{
					int[] nums = PutRods(blocks, variables[i * 4 + lw]);
					double blks = 0;
					Term blksv = 0;
					for (j = 0; j < blocks.Length; j++)
					{
						blks += blocks[j] * nums[j+1];
						blksv += blocks[j] * nums[j+1];
					}
					if (Math.Pow(blks - variables[i * 4 + lw], 2) > Math.Pow(wallThickness, 2))
					{
						diffc++;
						diffcv += 1;
					}
				}
			}
			func += w3 * diffc;
			funcv += w3 * diffcv;

			for (i = 0; i < bounds[currentFloor].Count; i++)
			{
				boundlen = 0;
				boundlenv = 0;
				len = (bounds[currentFloor][i] - bounds[currentFloor][(i + 1) % bounds[currentFloor].Count]).Length();
				if (Math.Abs(bounds[currentFloor][i].x - bounds[currentFloor][(i + 1) % bounds[currentFloor].Count].x) < thresh)
					xy = 0;
				else
					xy = 1;
				for (j = 1; j < bdOptInds[i].Count; j++)
				{
					k = bdOptInds[i][j];
					t = bdOptInds[i][j - 1];
					if (xy == 0)
					{
						boundlen += variables[k * 4 + 1] - variables[t * 4 + 1];
						boundlenv += vars[k * 4 + 1] - vars[t * 4 + 1];
					}
					else
					{
						boundlen += variables[k * 4] - variables[t * 4];
						boundlenv += vars[k * 4] - vars[t * 4];
					}
				}
				k = bdOptInds[i][0];
				t = bdOptInds[i][bdOptInds[i].Count - 1];
				if (xy == 0)
				{
					boundlen += 0.5 * variables[k * 4 + 2] + 0.5 * variables[t * 4 + 2];
					boundlenv += 0.5 * vars[k * 4 + 2] + 0.5 * vars[t * 4 + 2];
				}
				else
				{
					boundlen += 0.5 * variables[k * 4 + 3] + 0.5 * variables[t * 4 + 3];
					boundlenv += 0.5 * vars[k * 4 + 3] + 0.5 * vars[t * 4 + 3];
				}
				func += w2 * Math.Pow(boundlen - len, 2);
				funcv += w2 * TermBuilder.Power(boundlenv - len, 2);
			}

			grad = funcv.Differentiate(vars, variables);
		}

		public double GlobalFuncValue(double[] variables)
		{
			int i, j, k, t, xy, diffc = 0;
			double len, boundlen, func = 0, diffa = 0, diffas = 0, diffb = 0, tarea = 0, barea, difftarea;
			double w1 = 1, w2 = 1, w3 = 1, w4 = 1;

			for (i = 0; i < recRooms[currentFloor].Count; i++)
			{
				diffa += Math.Pow(variables[i * 4 + 2] * variables[i * 4 + 3]*100 - recRooms[currentFloor][i].area*100, 2);
				tarea += variables[i * 4 + 2] * variables[i * 4 + 3] * 100;
				if (initialv[i * 4 + 2] < initialv[i * 4 + 3])
					diffas += Math.Pow(variables[i * 4 + 2] / variables[i * 4 + 3] - 1, 2);
				else
					diffas += Math.Pow(variables[i * 4 + 3] / variables[i * 4 + 2] - 1, 2);
			}
			barea = PolygonArea(bounds[currentFloor]) * 100;
			difftarea = Math.Pow(tarea - barea, 2);

			for (i = 0; i < recRooms[currentFloor].Count; i++)
			{
				for (int lw = 2; lw < 4; lw++) // length and width
				{
					if (variables[i * 4 + lw] < 0)
						break;
					int[] nums = PutRods(blocks, variables[i * 4 + lw]);
					double blks = 0;
					Term blksv = 0;
					for (j = 0; j < blocks.Length; j++)
						blks += blocks[j] * nums[j+1];
					if (Math.Pow(blks - variables[i * 4 + lw], 2) > Math.Pow(wallThickness, 2))
						diffc++;
				}
			}

			for (i = 0; i < bounds[currentFloor].Count; i++)
			{
				boundlen = 0;
				len = (bounds[currentFloor][i] - bounds[currentFloor][(i + 1) % bounds[currentFloor].Count]).Length();
				if (Math.Abs(bounds[currentFloor][i].x - bounds[currentFloor][(i + 1) % bounds[currentFloor].Count].x) < thresh)
					xy = 0;
				else
					xy = 1;
				for (j = 1; j < bdOptInds[i].Count; j++)
				{
					k = bdOptInds[i][j];
					t = bdOptInds[i][j - 1];
					if (xy == 0)
						boundlen += variables[k * 4 + 1] - variables[t * 4 + 1];
					else
						boundlen += variables[k * 4] - variables[t * 4];
				}
				k = bdOptInds[i][0];
				t = bdOptInds[i][bdOptInds[i].Count - 1];
				if (xy == 0)
					boundlen += 0.5 * variables[k * 4 + 2] + 0.5 * variables[t * 4 + 2];
				else
					boundlen += 0.5 * variables[k * 4 + 3] + 0.5 * variables[t * 4 + 3];
				diffb += Math.Pow(boundlen - len, 2);
			}
			func += w1 * diffa + w2 * diffb + w3 * diffc + w4 * diffas;
			return (func);
		}

		public void FindGapOnBound(int floor)
		{
			// First, fix the boundary gaps
			// Second, fix inside gaps
			int i, j, k, xy, lr;
			double dist, threshold = 1e-6;
			double minx = double.MaxValue, miny = double.MaxValue, maxx = double.MinValue, maxy = double.MinValue;
			bool founds = false;
			Vector2d v = new Vector2d();

			for (i = 0; i < bounds[floor].Count; i++)
			{
				if (bounds[floor][i].x < minx)
					minx = bounds[floor][i].x;
				if (bounds[floor][i].y < miny)
					miny = bounds[floor][i].y;
				if (bounds[floor][i].x > maxx)
					maxx = bounds[floor][i].x;
				if (bounds[floor][i].y > maxy)
					maxy = bounds[floor][i].y;
			}
			// Check whether stair on bound
			for (i = 0; i < bdInds.Length; i++)
			{
				if (Math.Abs(bounds[floor][i].x - bounds[floor][(i + 1) % bounds[floor].Count].x) < threshold)
					xy = 0;
				else
					xy = 1;
				for (j = 0; j < bdInds[i].Count(); j++)
				{
					k = bdInds[i][j];
					if (recRooms[floor][k].name == "Stair")
					{
						// Translate all rooms
						if (xy == 0)
						{
							if (bounds[floor][i].x > initialv[k * 4])
								dist = bounds[floor][i].x - recRooms[floor][k].vertex[0].x;
							else
								dist = bounds[floor][i].x - recRooms[floor][k].vertex[3].x;
							v = new Vector2d(1, 0) * dist;
						}
						else
						{
							if (bounds[floor][i].y > initialv[k * 4 + 1])
								dist = bounds[floor][i].y - recRooms[floor][k].vertex[1].y;
							else
								dist = bounds[floor][i].y - recRooms[floor][k].vertex[0].y;
							v = new Vector2d(0, 1) * dist;
						}
						founds = true;
						break;
					}
				}
				if (founds) break;
			}
			// Translate to make stair inside -- keep its location and size for multiple floors
			if (founds)
			{
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					recRooms[floor][i].center = new Vector2d((recRooms[floor][i].vertex[0].x + recRooms[floor][i].vertex[3].x) / 2,
						(recRooms[floor][i].vertex[0].y + recRooms[floor][i].vertex[1].y) / 2);
					for (j = 0; j < recRooms[floor][i].vertex.Count; j++)
						recRooms[floor][i].vertex[j] += v;
					recRooms[floor][i].center += v;
				}
			}
			// Boundary gaps filling or cutting
			// For those are outside of the boundary, pull them back and keep the area
			for (i = 0; i < bdInds.Length; i++)
			{
				if (Math.Abs(bounds[floor][i].x - bounds[floor][(i + 1) % bounds[floor].Count].x) < threshold)
					xy = 0;
				else
					xy = 1;
				for (j = 0; j < bdInds[i].Count(); j++)
				{
					k = bdInds[i][j];
					if (xy == 0)
					{
						if (bounds[floor][i].x > initialv[k * 4])
							lr = 1; // boundary at right
						else lr = 0;
						if (lr == 1)
						{
							dist = bounds[floor][i].x - recRooms[floor][k].vertex[0].x;
							if (Math.Abs(dist) > threshold)
							{
								recRooms[floor][k].center.x += dist;
							}
						}
						else
						{
							dist = recRooms[floor][k].vertex[3].x - bounds[floor][i].x;
							if (Math.Abs(dist) > threshold)
							{
								recRooms[floor][k].center.x -= dist;
							}
						}
					}
					else
					{
						if (bounds[floor][i].y > initialv[k * 4 + 1])
							lr = 1; // boundary at upper
						else lr = 0;
						if (lr == 1)
						{
							dist = bounds[floor][i].y - recRooms[floor][k].vertex[1].y;
							if (Math.Abs(dist) > threshold)
							{
								recRooms[floor][k].center.y += dist;
							}
						}
						else
						{
							dist = recRooms[floor][k].vertex[0].y - bounds[floor][i].y;
							if (Math.Abs(dist) > threshold)
							{
								recRooms[floor][k].center.y -= dist;
							}
						}
					}
				}
			}
			// Reset vertex
			for (i = 0; i < recRooms[floor].Count; i++)
				ResetVertex(floor, i);

			// Boundary gaps filling or cutting -- vertical gap
			for (i = 0; i < bdInds.Length; i++)
			{
				if (Math.Abs(bounds[floor][i].x - bounds[floor][(i + 1) % bounds[floor].Count].x) < threshold)
					xy = 0;
				else
					xy = 1;
				for (j = 0; j < bdInds[i].Count(); j++)
				{
					k = bdInds[i][j];
					if (xy == 0)
					{
						if (bounds[floor][i].x > initialv[k * 4])
							lr = 1;
						else lr = 0;
						if (lr == 1)
						{
							dist = bounds[floor][i].x - recRooms[floor][k].vertex[0].x;
							if (Math.Abs(dist) > threshold)
							{
								//recRooms[floor][k].center.x += dist;
								if (i >= bounds[floor].Count() / 2 && bdInds[i - bdInds[floor].Count() / 2].IndexOf(k) != -1)
								{
									recRooms[floor][k].center.x += dist / 2;
									recRooms[floor][k].width += dist;
								}
								else
									recRooms[floor][k].center.x += dist;
							}
						}
						else
						{
							dist = recRooms[floor][k].vertex[3].x - bounds[floor][i].x;
							if (Math.Abs(dist) > threshold)
							{
								//recRooms[floor][k].center.x -= dist;
								if (i >= bounds[floor].Count() / 2 && bdInds[i - bdInds[floor].Count() / 2].IndexOf(k) != -1)
								{
									recRooms[floor][k].center.x -= dist / 2;
									recRooms[floor][k].width -= dist;
								}
								else
									recRooms[floor][k].center.x -= dist;
							}
						}
					}
					else
					{
						if (bounds[floor][i].y > initialv[k * 4 + 1])
							lr = 1; // boundary at upper
						else lr = 0;
						if (lr == 1)
						{
							dist = bounds[floor][i].y - recRooms[floor][k].vertex[1].y;
							if (Math.Abs(dist) > threshold)
							{
								//recRooms[floor][k].center.y += dist;
								if (i >= bounds[floor].Count() / 2 && bdInds[i - bdInds[floor].Count() / 2].IndexOf(k) != -1)
								{
									recRooms[floor][k].center.y += dist / 2;
									recRooms[floor][k].length += dist;
								}
								else
									recRooms[floor][k].center.y += dist;
							}
						}
						else
						{
							dist = recRooms[floor][k].vertex[0].y - bounds[floor][i].y;
							if (Math.Abs(dist) > threshold)
							{
								recRooms[floor][k].center.y -= dist;
								if (i >= bounds[floor].Count() / 2 && bdInds[i - bdInds[floor].Count() / 2].IndexOf(k) != -1)
								{
									recRooms[floor][k].center.y -= dist / 2;
									recRooms[floor][k].length -= dist;
								}
								else
									recRooms[floor][k].center.y -= dist;
							}
						}
					}
				}
			}
			// Reset vertex
			for (i = 0; i < recRooms[floor].Count; i++)
				ResetVertex(floor, i);

			// Parallel gaps
			int t;
			for (i = 0; i < bdInds.Length; i++)
			{
				if (Math.Abs(bounds[floor][i].x - bounds[floor][(i + 1) % bounds[floor].Count].x) < threshold)
					xy = 0;
				else
					xy = 1;
				for (j = 0; j < bdInds[i].Count() - 1; j++)
				{
					k = bdInds[i][j];
					t = bdInds[i][(j + 1) % bdInds[i].Count];
					if (xy == 0)
					{
						if (Math.Abs(recRooms[floor][k].vertex[1].y - recRooms[floor][t].vertex[0].y) < thresh)
							continue;
						if (recRooms[floor][k].vertex[1].y < recRooms[floor][t].vertex[0].y)
						{
							dist = recRooms[floor][t].vertex[0].y - recRooms[floor][k].vertex[1].y;
							if ((recRooms[floor][k].area - recRooms[floor][k].length * recRooms[floor][k].width) >
								(recRooms[floor][t].area - recRooms[floor][t].length * recRooms[floor][t].width))
							{
								recRooms[floor][k].center.y += 0.5 * dist;
								recRooms[floor][k].length += dist;
								recRooms[floor][k].vertex[1] = new Vector2d(recRooms[floor][k].vertex[1].x,
									recRooms[floor][t].vertex[0].y);
								recRooms[floor][k].vertex[2] = new Vector2d(recRooms[floor][k].vertex[2].x,
									recRooms[floor][t].vertex[0].y);
							}
							else
							{
								recRooms[floor][t].center.y -= 0.5 * dist;
								recRooms[floor][t].length -= dist;
								recRooms[floor][t].vertex[0] = new Vector2d(recRooms[floor][t].vertex[0].x,
									recRooms[floor][k].vertex[1].y);
								recRooms[floor][t].vertex[3] = new Vector2d(recRooms[floor][t].vertex[3].x,
									recRooms[floor][k].vertex[1].y);
							}
						}
					}
					else
					{
						if (Math.Abs(recRooms[floor][k].vertex[0].x - recRooms[floor][t].vertex[3].x) < thresh)
							continue;
						if (recRooms[floor][k].vertex[0].x < recRooms[floor][t].vertex[3].x)
						{
							dist = recRooms[floor][t].vertex[3].x - recRooms[floor][k].vertex[0].x;
							if ((recRooms[floor][k].area - recRooms[floor][k].length * recRooms[floor][k].width) >
								(recRooms[floor][t].area - recRooms[floor][t].length * recRooms[floor][t].width))
							{
								recRooms[floor][k].center.x += 0.5 * dist;
								recRooms[floor][k].width += dist;
								recRooms[floor][k].vertex[0] = new Vector2d(recRooms[floor][t].vertex[3].x,
									recRooms[floor][k].vertex[0].y);
								recRooms[floor][k].vertex[1] = new Vector2d(recRooms[floor][t].vertex[3].x,
									recRooms[floor][k].vertex[1].y);
							}
							else
							{
								recRooms[floor][t].center.x -= 0.5 * dist;
								recRooms[floor][t].width -= dist;
								recRooms[floor][t].vertex[2] = new Vector2d(recRooms[floor][k].vertex[0].x,
									recRooms[floor][t].vertex[2].y);
								recRooms[floor][t].vertex[3] = new Vector2d(recRooms[floor][k].vertex[0].x,
									recRooms[floor][t].vertex[3].y);
							}
						}
					}
				}
			}
		}

		public void FixBound(int floor)
		{
			// First, fix the boundary gaps
			// Second, fix inside gaps
			int i, j, k, t, xy;
			double threshold = 1e-3, dist = 0;
			bool ch;
			// Boundary gaps filling or cutting
			for (i = 0; i < bdInds.Length; i++)
			{
				if (Math.Abs(bounds[floor][i].x - bounds[floor][(i + 1) % bounds[floor].Count].x) < threshold)
					xy = 0;
				else
					xy = 1;
				for (j = 0; j < bdInds[i].Count(); j++)
				{
					k = bdInds[i][j];
					ch = false;
					if (xy == 0)
					{
						for (t = 0; t < recRooms[floor][k].vertex.Count; t++)
							if (Math.Abs(recRooms[floor][k].vertex[t].x - bounds[floor][i].x) < threshold)
							{
								dist = bounds[floor][i].x - recRooms[floor][k].vertex[t].x;
								recRooms[floor][k].vertex[t] = new Vector2d(bounds[floor][i].x, recRooms[floor][k].vertex[t].y);
								ch = true;
							}
						if (ch)
						{
							recRooms[floor][k].center += dist / 2 * new Vector2d(1, 0);
							recRooms[floor][k].width += dist;
						}
					}
					else
					{
						for (t = 0; t < recRooms[floor][k].vertex.Count; t++)
							if (Math.Abs(recRooms[floor][k].vertex[t].y - bounds[floor][i].y) < threshold)
							{
								dist = bounds[floor][i].y - recRooms[floor][k].vertex[t].y;
								recRooms[floor][k].vertex[t] = new Vector2d(recRooms[floor][k].vertex[t].x, bounds[floor][i].y);
								ch = true;
							}
						if (ch)
						{
							recRooms[floor][k].center += dist / 2 * new Vector2d(0, 1);
							recRooms[floor][k].length += dist;
						}
					}
					
				}
			}
		}

		public void FixRounding(int floor)
		{
			int i, j, k;
			double dist, value, threshold = 0.04;
			Vector2d x = new Vector2d(1, 0), y = new Vector2d(0, 1);

			for (i = 0; i < recRooms[floor].Count - 1; i++)
			{
				for (k = 0; k < 4; k+=2)
				{
					value = recRooms[floor][i].vertex[k].x;
					for (j = i + 1; j < recRooms[floor].Count; j++)
					{
						dist = value - recRooms[floor][j].vertex[0].x;
						if (Math.Abs(dist) < threshold)
						{
							recRooms[floor][j].vertex[0] = new Vector2d
								(value, recRooms[floor][j].vertex[0].y);
							recRooms[floor][j].vertex[1] = new Vector2d
								(value, recRooms[floor][j].vertex[1].y);
							recRooms[floor][j].width += dist;
							recRooms[floor][j].center += dist / 2 * x;
						}
						dist = value - recRooms[floor][j].vertex[2].x;
						if (Math.Abs(dist) < threshold)
						{
							recRooms[floor][j].vertex[2] = new Vector2d
								(value, recRooms[floor][j].vertex[2].y);
							recRooms[floor][j].vertex[3] = new Vector2d
								(value, recRooms[floor][j].vertex[3].y);
							recRooms[floor][j].width += dist;
							recRooms[floor][j].center += dist / 2 * x;
						}
					}
				}
				for (k = 0; k < 2; k++)
				{
					value = recRooms[floor][i].vertex[k].y;
					for (j = i + 1; j < recRooms[floor].Count; j++)
					{
						dist = value - recRooms[floor][j].vertex[0].y;
						if (Math.Abs(dist) < threshold)
						{
							recRooms[floor][j].vertex[0] = new Vector2d
								(recRooms[floor][j].vertex[0].x, value);
							recRooms[floor][j].vertex[3] = new Vector2d
								(recRooms[floor][j].vertex[3].x, value);
							recRooms[floor][j].length += dist;
							recRooms[floor][j].center += dist / 2 * y;
						}
						dist = value - recRooms[floor][j].vertex[1].y;
						if (Math.Abs(dist) < threshold)
						{
							recRooms[floor][j].vertex[1] = new Vector2d
								(recRooms[floor][j].vertex[1].x, value);
							recRooms[floor][j].vertex[2] = new Vector2d
								(recRooms[floor][j].vertex[2].x, value);
							recRooms[floor][j].length += dist;
							recRooms[floor][j].center += dist / 2 * y;
						}
					}
				}
			}
		}

		public bool FindInsideGaps(int floor)
		{
			// After optimizaiton, there are gaps inside room layout
			// find out these gaps by grid the boundary polygon
			int i, j, k, bl, r1 = -1, r2 = -1, t, count;
			bool hasgap = false, merged = false;
			double x0, y0, dist, x1, x2, y1, y2, interl, thresh = 1e-3;
			Vector2d v, center;
			Vector2d[] gapv = new Vector2d[4];
			List<int> belongs = new List<int>(), nbs = new List<int>();
			List<int>[] neigbs = new List<int>[4];
			List<double> X = new List<double>(), Y = new List<double>(),
				gaps = new List<double>(), overlaps = new List<double>();

			for (i = 0; i < recRooms[floor].Count; i++)
			{ 
				for (j = 0; j < recRooms[floor][i].vertex.Count; j++)
					if (!Contain(X, recRooms[floor][i].vertex[j].x))
						X.Add(recRooms[floor][i].vertex[j].x);
					else if (!Contain(Y, recRooms[floor][i].vertex[j].y))
						Y.Add(recRooms[floor][i].vertex[j].y);
			}
			Sort(X);
			Sort(Y);
			for (i = 0; i < X.Count - 1; i++)
				for (j = 0; j < Y.Count - 1; j++)
				{
					x0 = (X[i] + X[i + 1]) / 2;
					y0 = (Y[j] + Y[j + 1]) / 2;
					v = new Vector2d(x0, y0);
					bl = 0;
					for (k = 0; k < recRooms[floor].Count; k++)
					{
						if (PointInPoly(v, recRooms[floor][k].vertex))
							bl++;
					}
					// bl == 0 --- gap
					// bl == 1 --- normal
					// bl >= 2 --- overlap
					if (bl == 0)
					{
						gaps.Add(X[i]);
						gaps.Add(X[i+1]);
						gaps.Add(Y[j]);
						gaps.Add(Y[j+1]);
					}
				}
			// Merge small rectangular gaps that have been cut by grid
			//MergeRects(gaps, null, floor);
			if (gaps.Count > 0) hasgap = true;
			else hasgap = false;
			// Fill gaps
			for (i = 0; i < gaps.Count; i+=4)
			{
				x1 = gaps[i];
				x2 = gaps[i+1];
				y1 = gaps[i+2];
				y2 = gaps[i+3];
				// Add a virtual room then delete it when finish
				recRooms[floor].Add(new Room());
				count = recRooms[floor].Count - 1;
				recRooms[floor][count].center = new Vector2d((x1 + x2)/2, (y1 + y2)/2);
				gapv[0] = new Vector2d(x2, y1);
				recRooms[floor][count].vertex.Add(gapv[0]);
				gapv[1] = new Vector2d(x2, y2);
				recRooms[floor][count].vertex.Add(gapv[1]);
				gapv[2] = new Vector2d(x1, y2);
				recRooms[floor][count].vertex.Add(gapv[2]);
				gapv[3] = new Vector2d(x1, y1);
				recRooms[floor][count].vertex.Add(gapv[3]);
				neigbs = NeighborInFourDir(floor, count, true);
				recRooms[floor].RemoveAt(count);
				center = new Vector2d((x1 + x2) / 2, (y1 + y2) / 2);
				interl = double.MaxValue;
				bl = -1;
				merged = GapMerged(gapv, floor);
				if (merged) continue;
				// neigbs 0: x+ dir
				// neigbs 1: x- dir
				// neigbs 2: y+ dir
				// neigbs 3: y- dir
				for (j = 0; j < neigbs[0].Count; j++) 
				{
					k = neigbs[0][j];
					if (recRooms[floor][k].name == "Stair") continue;
					//dist = PolygonArea(recRooms[floor][k].vertex);
					//if (dist < interl)
					//{
					//    interl = dist;
					//    r1 = k;
					//    r2 = 2;
					//    bl = 0;
					//}

					for (t = 0; t < recRooms[floor][k].vertex.Count; t++)
					{
						if (Math.Abs(recRooms[floor][k].vertex[t].x -
							recRooms[floor][k].vertex[(t + 1) % recRooms[floor][k].vertex.Count].x) < thresh &&
							Math.Abs(x2 - recRooms[floor][k].vertex[t].x) < thresh)
						{
							if (Math.Abs(recRooms[floor][k].vertex[t].x - x2) < thresh)
							{
								dist = Math.Abs(center.y - recRooms[floor][k].center.y);
								if (dist < interl)
								{
									interl = dist;
									r1 = k;
									r2 = t;
									bl = 0;
									break;
								}
							}
						}
					}
				}
				for (j = 0; j < neigbs[1].Count; j++) 
				{
					k = neigbs[1][j];
					if (recRooms[floor][k].name == "Stair") continue;
					//dist = PolygonArea(recRooms[floor][k].vertex);
					//if (dist < interl)
					//{
					//    interl = dist;
					//    r1 = k;
					//    r2 = 0;
					//    bl = 1;
					//}
					for (t = 0; t < recRooms[floor][k].vertex.Count; t++)
					{
						if (Math.Abs(recRooms[floor][k].vertex[t].x -
							recRooms[floor][k].vertex[(t + 1) % recRooms[floor][k].vertex.Count].x) < thresh &&
							Math.Abs(x1 - recRooms[floor][k].vertex[t].x) < thresh)
						{
							if (Math.Abs(recRooms[floor][k].vertex[t].x - x1) < thresh)
							{
								dist = Math.Abs(center.y - recRooms[floor][k].center.y);
								if (dist < interl)
								{
									interl = dist;
									r1 = k;
									r2 = t;
									bl = 1;
									break;
								}
							}
						}
					}
				}
				for (j = 0; j < neigbs[2].Count; j++) 
				{
					k = neigbs[2][j];
					if (recRooms[floor][k].name == "Stair") continue;
					//dist = PolygonArea(recRooms[floor][k].vertex);
					//if (dist < interl)
					//{
					//    interl = dist;
					//    r1 = k;
					//    r2 = 3;
					//    bl = 2;
					//}
					for (t = 0; t < recRooms[floor][k].vertex.Count; t++)
					{
						if (Math.Abs(recRooms[floor][k].vertex[t].y -
							recRooms[floor][k].vertex[(t + 1) % recRooms[floor][k].vertex.Count].y) < thresh &&
							Math.Abs(y2 - recRooms[floor][k].vertex[t].y) < thresh)
						{
							if (Math.Abs(recRooms[floor][k].vertex[t].y - y2) < thresh)
							{
								dist = Math.Abs(center.x - recRooms[floor][k].center.x);
								if (dist < interl)
								{
									interl = dist;
									r1 = k;
									r2 = t;
									bl = 2;
									break;
								}
							}
						}
					}
				}
				for (j = 0; j < neigbs[3].Count; j++) 
				{
					k = neigbs[3][j];
					if (recRooms[floor][k].name == "Stair") continue;
					//dist = PolygonArea(recRooms[floor][k].vertex);
					//if (dist < interl)
					//{
					//    interl = dist;
					//    r1 = k;
					//    r2 = 1;
					//    bl = 3;
					//}
					for (t = 0; t < recRooms[floor][k].vertex.Count; t++)
					{
						if (Math.Abs(recRooms[floor][k].vertex[t].y -
							recRooms[floor][k].vertex[(t + 1) % recRooms[floor][k].vertex.Count].y) < thresh &&
							Math.Abs(y1 - recRooms[floor][k].vertex[t].y) < thresh)
						{
							if (Math.Abs(recRooms[floor][k].vertex[t].y - y1) < thresh)
							{
								dist = Math.Abs(x2 - x1 - Math.Abs(recRooms[floor][k].vertex[t].x -
									recRooms[floor][k].vertex[(t + 1) % recRooms[floor][k].vertex.Count].x));
								if (dist < interl)
								{
									interl = dist;
									r1 = k;
									r2 = t;
									bl = 3;
									break;
								}
							}
						}
					}
				}
				if (bl == 0 || bl == 1)
				{
					if (Math.Abs(recRooms[floor][r1].vertex[r2].x - x1) < thresh)
						dist = x2 - x1;
					else
						dist = x1 - x2;
					//if (bl == 1)
					//    dist = x2 - x1;
					//else
					//    dist = x1 - x2;
					recRooms[floor][r1].vertex[r2] = new Vector2d(recRooms[floor][r1].vertex[r2].x + dist,
						recRooms[floor][r1].vertex[r2].y);
					t = (r2 + 1) % recRooms[floor][r1].vertex.Count;
					recRooms[floor][r1].vertex[t] = new Vector2d(recRooms[floor][r1].vertex[t].x + dist,
						recRooms[floor][r1].vertex[t].y);
					recRooms[floor][r1].center.x += dist / 2;
					recRooms[floor][r1].width += Math.Abs(dist);
				}
				else
				{
					if (Math.Abs(recRooms[floor][r1].vertex[r2].y - y1) < thresh)
						dist = y2 - y1;
					else
						dist = y1 - y2;
					//if (bl == 3)
					//    dist = y2 - y1;
					//else
					//    dist = y1 - y2;
					recRooms[floor][r1].vertex[r2] = new Vector2d(recRooms[floor][r1].vertex[r2].x,
						recRooms[floor][r1].vertex[r2].y + dist);
					t = (r2 + 1) % recRooms[floor][r1].vertex.Count;
					recRooms[floor][r1].vertex[t] = new Vector2d(recRooms[floor][r1].vertex[t].x,
						recRooms[floor][r1].vertex[t].y + dist);
					recRooms[floor][r1].center.y += dist / 2;
					recRooms[floor][r1].length += Math.Abs(dist);
				}
	
			}
			return (hasgap);
		}

		public bool GapMerged(Vector2d[] gaps, int floor)
		{
			int i, j, k, t, ins;
			bool inside = false;
			bool[] visited;
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				inside = false;
				ins = 0;
				visited = new bool[gaps.Length];
				for (j = 0; j < gaps.Length; j++)
					if (PointInPoly(gaps[j], recRooms[floor][i].vertex))
					{
						visited[j] = true;
						ins++;
					}
				if (ins == gaps.Length)
				{
					inside = true;
					break;
				}
				for (j = 0; j < recRooms[floor][i].vertex.Count; j++)
				{
					k = (j + 1) % recRooms[floor][i].vertex.Count;
					for (t = 0; t < gaps.Length; t++)
						if (!visited[t] && Point_On_Line(recRooms[floor][i].vertex[j],
							recRooms[floor][i].vertex[k], gaps[t]))
						{
							ins++;
							visited[t] = true;
						}
				}
				if (ins >= gaps.Length)
				{
					inside = true;
					break;
				}
			}
			return (inside);
		}
		public bool FindOverlaps(int floor, int times)
		{
			// After optimizaiton
			int i, j, k, bl, t, p, q, num, ind1 = -1, ind2 = -1;
			double x0, y0, x1, x2, y1, y2, dist, thresh = 1e-3;
			bool hasoverlap = false;
			Vector2d v, v1, v2;
			int[] npts = new int[4];
			List<int> nbs = new List<int>(), belongs, nlap;
			List<List<int>> nlaps = new List<List<int>>();
			List<double> X = new List<double>(), Y = new List<double>(),
				gaps = new List<double>(), overlaps = new List<double>();
			List<Vector2d> laps;

			if (times == 1)
			{
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					for (j = 0; j < recRooms[floor][i].vertex.Count; j++)
						if (!Contain(X, recRooms[floor][i].vertex[j].x))
							X.Add(recRooms[floor][i].vertex[j].x);
						else if (!Contain(Y, recRooms[floor][i].vertex[j].y))
							Y.Add(recRooms[floor][i].vertex[j].y);
				}
			}
			else
			{
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					for (j = 0; j < recRooms[floor][i].vertices.Count; j++)
						if (!Contain(X, recRooms[floor][i].vertices[j].Coord.x))
							X.Add(recRooms[floor][i].vertices[j].Coord.x);
						else if (!Contain(Y, recRooms[floor][i].vertices[j].Coord.y))
							Y.Add(recRooms[floor][i].vertices[j].Coord.y);
				}
			}
			Sort(X);
			Sort(Y);
			for (i = 0; i < X.Count - 1; i++)
				for (j = 0; j < Y.Count - 1; j++)
				{
					x0 = (X[i] + X[i + 1]) / 2;
					y0 = (Y[j] + Y[j + 1]) / 2;
					v = new Vector2d(x0, y0);
					bl = 0;
					nlap = new List<int>();
					for (k = 0; k < recRooms[floor].Count; k++)
						if (PointInPoly(v, recRooms[floor][k].vertex))
						{
							bl++;
							nlap.Add(k);
						}
					// bl == 0 --- gap
					// bl == 1 --- normal
					// bl >= 2 --- overlap
					if (bl >= 2)
					{
						overlaps.Add(X[i]);
						overlaps.Add(X[i + 1]);
						overlaps.Add(Y[j]);
						overlaps.Add(Y[j + 1]);
						nlaps.Add(nlap);
					}
				}
			if (LayoutInd == 2 && floor == 1)
				MergeRects(overlaps, nlaps, floor);
			if (overlaps.Count() > 0)
				hasoverlap = true;
			// Solve overlaps -- Change the vertices
			// After merge gaps, most possible overlaps happen that one vertex 
			for (i = 0; i < overlaps.Count; i += 4)
			{
				x1 = overlaps[i];
				x2 = overlaps[i + 1];
				y1 = overlaps[i + 2];
				y2 = overlaps[i + 3];

				laps = new List<Vector2d>();
				laps.Add(new Vector2d(x1, y1));
				laps.Add(new Vector2d(x1, y2));
				laps.Add(new Vector2d(x2, y2));
				laps.Add(new Vector2d(x2, y1));
				belongs = new List<int>();
				v = new Vector2d((x1 + x2) / 2, (y1 + y2) / 2);
				for (j = 0; j < recRooms[floor].Count; j++)
					if (PointInsidePoly(v, recRooms[floor][j].vertex))
						belongs.Add(j);

				num = belongs.Count - 1; 
				// normally belongs.count = 2, only change one room to delete overlpa
				// if the room will not be changed, then num++, to change next room
				for (j = 0; j < num; j++)
				{
					k = belongs[j];
					if (recRooms[floor][k].name == "Stair")
					{
						num = Math.Min(num + 1, belongs.Count);
						continue;
					}
					// Update the vertex of room k
					nbs = new List<int>();
					for (p = 0; p < 4; p++)
					{
						for (q = 0; q < recRooms[floor][k].vertex.Count; q++)
							if ((laps[p] - recRooms[floor][k].vertex[q]).Length() < thresh)
							{
								nbs.Add(p);
								nbs.Add(q);
							}
					}
					if (nbs.Count == 0)
					{
 						// there must have one room with two vertices overlap with the gaps
						// leave the overlap to that room to merge
						num = Math.Min(num + 1, belongs.Count);
					}
					else if (nbs.Count == 2) // one vertex --> three
					{
						ind1 = nbs[1];
						ind2 = (nbs[0] + 2) % 4;
						v = new Vector2d(laps[ind2]);
						v1 = new Vector2d(laps[(ind2 + 1) % 4]);
						v2 = new Vector2d(laps[(ind2 - 1 + 4) % 4]);
						recRooms[floor][k].vertex.RemoveAt(ind1);
						recRooms[floor][k].vertex.Insert(ind1, v1);
						recRooms[floor][k].vertex.Insert(ind1, v);
						recRooms[floor][k].vertex.Insert(ind1, v2);
						// Order
						t = ind1;
						t = (t - 1 + recRooms[floor][k].vertex.Count) %
							recRooms[floor][k].vertex.Count;
						if (Math.Abs(recRooms[floor][k].vertex[ind1].x -
							recRooms[floor][k].vertex[t].x) > thresh &&
							Math.Abs(recRooms[floor][k].vertex[ind1].y -
							recRooms[floor][k].vertex[t].y) > thresh)
						{
							v1 = new Vector2d(recRooms[floor][k].vertex[ind1]);
							v2 = new Vector2d(recRooms[floor][k].vertex[ind1 + 2]);
							recRooms[floor][k].vertex.RemoveAt(ind1 + 2);
							recRooms[floor][k].vertex.RemoveAt(ind1);
							recRooms[floor][k].vertex.Insert(ind1, v2);
							recRooms[floor][k].vertex.Insert(ind1 + 2, v1);
						}
						break;
					}
					else if (nbs.Count == 6) // three vertex --> one
					{
						List<int> ps = new List<int>(), delv = new List<int>();
						for (p = 0; p < nbs.Count / 2; p++)
						{
							ps.Add(nbs[p * 2]);
							delv.Add(nbs[p * 2 + 1]);
						}
						Sort(delv);
						for (p = delv.Count - 1; p >= 0; p--)
							recRooms[floor][k].vertex.RemoveAt(delv[p]);
						v1 = new Vector2d();
						for (p = 0; p < 4; p++)
						{
							if (ps.IndexOf(p) == -1)
							{
								v1 = new Vector2d(laps[p]);
								break;
							}
						}
						recRooms[floor][k].vertex.Insert(delv[0], v1);
					}
					else
					{
						dist = 0;
						v1 = new Vector2d(recRooms[floor][k].vertex[nbs[1]]);
						v2 = new Vector2d(recRooms[floor][k].vertex[nbs[3]]);
						if (Math.Abs(v1.x - v2.x) < thresh)
						{
							if (Math.Abs(v1.x - x1) < thresh)
								dist = x2 - v1.x;
							else
								dist = x1 - v1.x;
							recRooms[floor][k].vertex[nbs[1]] += dist * new Vector2d(1, 0);
							recRooms[floor][k].vertex[nbs[3]] += dist * new Vector2d(1, 0);
							recRooms[floor][k].width += dist;
						}
						else
						{
							if (Math.Abs(v1.y - y1) < thresh)
								dist = y2 - v1.y;
							else
								dist = y1 - v1.y;
							recRooms[floor][k].vertex[nbs[1]] += dist * new Vector2d(0, 1);
							recRooms[floor][k].vertex[nbs[3]] += dist * new Vector2d(0, 1);
							recRooms[floor][k].length += dist;
						}
						break;
					}
				}
			}
			return (hasoverlap);
		}

		public void MergeRects(List<double> set, List<List<int>> nlaps, int floor)
		{
			int i, j, k, ni = 0, nj, p, q;
			double len, wid, min, max;
			bool merge = true;
			//Merge set if both length/width is smaller than threshold

			for (i = 0; i < set.Count - 4; i += 4)
			{
				merge = true;
				nj = ni + 1;
				len = set[i + 3] - set[i + 2];
				wid = set[i + 1] - set[i];
				if (nlaps != null)
				{
					for (p = 0; p < nlaps[ni].Count; p++)
					{
						q = nlaps[ni][p];
						double length = (recRooms[floor][q].vertex[0] - recRooms[floor][q].vertex[1]).Length();
						if (Math.Abs(len - length) < thresh)
							continue;
						length = (recRooms[floor][q].vertex[0] - recRooms[floor][q].vertex[3]).Length();
						if (Math.Abs(wid - length) < thresh)
							continue;
					}
				}
				for (j = i + 4; j < set.Count; j += 4)
				{
					len = set[j + 3] - set[j + 2];
					wid = set[j + 1] - set[j];
					// vertical merge
					if (Math.Abs(set[i] - set[j]) < thresh &&
						Math.Abs(set[i + 1] - set[j + 1]) < thresh &&
						(Math.Abs(set[i + 3] - set[j + 2]) < thresh ||
						 Math.Abs(set[i + 2] - set[j + 3]) < thresh))
					{
						// if the length equals to the wall length, no merge
						if (nlaps != null)
						{
							for (p = 0; p < nlaps[nj].Count; p++)
							{
								q = nlaps[nj][p];
								double length = (recRooms[floor][q].vertex[0] - recRooms[floor][q].vertex[1]).Length();
								if (Math.Abs(len - length) < thresh)
								{
									merge = false;
									break;
								}
							}
						}
						if (!merge) continue;
						min = Math.Min(set[i + 2], set[j + 2]);
						max = Math.Max(set[i + 3], set[j + 3]);
						set[i + 2] = min;
						set[i + 3] = max;
						for (k = 3; k >= 0; k--)
							set.RemoveAt(j + k);
						j -= 4;
						ni++;
					}
					else if (Math.Abs(set[i + 2] - set[j + 2]) < thresh &&
						Math.Abs(set[i + 3] - set[j + 3]) < thresh &&
						(Math.Abs(set[i + 1] - set[j]) < thresh ||
						Math.Abs(set[i] - set[j + 1]) < thresh))
						// horizontal merge
					{
						if (nlaps != null)
						{
							for (p = 0; p < nlaps[nj].Count; p++)
							{
								q = nlaps[nj][p];
								double length = (recRooms[floor][q].vertex[0] - recRooms[floor][q].vertex[3]).Length();
								if (Math.Abs(len - length) < thresh)
								{
									merge = false;
									break;
								}
							}
						}
						if (!merge) continue;
						min = Math.Min(set[i], set[j]);
						max = Math.Max(set[i + 1], set[j + 1]);
						set[i] = min;
						set[i + 1] = max;
						for (k = 3; k >= 0; k--)
							set.RemoveAt(j + k);
						j -= 4;
						ni++;
					}
					nj++;
				}
				ni++;
			}
		}

		public void ResetVertex(int floor, int ind)
		{
			recRooms[floor][ind].vertex = new List<Vector2d>();
			recRooms[floor][ind].vertex.Add(new Vector2d());
			recRooms[floor][ind].vertex[0] = new Vector2d(recRooms[floor][ind].center.x + recRooms[floor][ind].width / 2, recRooms[floor][ind].center.y - recRooms[floor][ind].length / 2);
			recRooms[floor][ind].vertex.Add(new Vector2d());
			recRooms[floor][ind].vertex[1] = new Vector2d(recRooms[floor][ind].center.x + recRooms[floor][ind].width / 2, recRooms[floor][ind].center.y + recRooms[floor][ind].length / 2);
			recRooms[floor][ind].vertex.Add(new Vector2d());
			recRooms[floor][ind].vertex[2] = new Vector2d(recRooms[floor][ind].center.x - recRooms[floor][ind].width / 2, recRooms[floor][ind].center.y + recRooms[floor][ind].length / 2);
			recRooms[floor][ind].vertex.Add(new Vector2d());
			recRooms[floor][ind].vertex[3] = new Vector2d(recRooms[floor][ind].center.x - recRooms[floor][ind].width / 2, recRooms[floor][ind].center.y - recRooms[floor][ind].length / 2);
		}

		public bool Contain(List<double> nums, double num)
		{
			bool con = false;
			double thresh = 1e-3;
			for (int i = 0; i < nums.Count; i++)
				if (Math.Abs(nums[i] - num) < thresh)
				{
					con = true;
					break;
				}
			return (con);
		}

		public int InsidePoly(int floor, int ind1, int ind2)
		{
			int i, ind = -1;
			Vector2d v;

			for (i = 0; i < recRooms[floor][ind2].vertex.Count; i++)
			{
				v = recRooms[floor][ind2].vertex[i];
				if (PointInPoly(v, recRooms[floor][ind1].vertex))
				{
					ind = i;
					break;
				}
			}
			return(ind);
		}

		public List<int> Neighbors(int floor, int r, int dir)
		{
			// dir = 0: x+ dir
			// dir = 1: x- dir
			// dir = 2: y+ dir
			// dir = 3: y- dir
			List<int> nbs = new List<int>();
			bool conn;

			for (int i = 0; i < recRooms[floor].Count; i++)
			{
				if (i == r) continue;
				conn = ConnectedRoom(floor, r, i, dir);
				if (conn)
					nbs.Add(i);
			}
			return (nbs);
		}

		public bool ConnectedRoom(int floor, int ind1, int ind2, int dir)
		{
			// ind2 at the expected direction of ind1
			bool c = false, conn = false;
			int i, j;

			for (i = 0; i < recRooms[floor][ind1].vertex.Count; i++)
			{
				if (dir == 0 || dir == 1) // X-dir
				{
					if (Math.Abs(recRooms[floor][ind1].vertex[i].y -
						recRooms[floor][ind1].vertex[(i + 1) % recRooms[floor][ind1].vertex.Count].y) < thresh)
						continue;
				}
				else
				{
					if (Math.Abs(recRooms[floor][ind1].vertex[i].x -
						recRooms[floor][ind1].vertex[(i + 1) % recRooms[floor][ind1].vertex.Count].x) < thresh)
						continue;
				}
				for (j = 0; j < recRooms[floor][ind2].vertex.Count; j++)
				{
					if (Point_In_Line(recRooms[floor][ind1].vertex[i], recRooms[floor][ind1].vertex[(i + 1) % recRooms[floor][ind1].vertex.Count],
						recRooms[floor][ind2].vertex[j]))
					{
						c = true;
						break;
					}
					if (Point_On_Line(recRooms[floor][ind1].vertex[i], recRooms[floor][ind1].vertex[(i + 1) % recRooms[floor][ind1].vertex.Count],
						recRooms[floor][ind2].vertex[j]) &&
						Point_On_Line(recRooms[floor][ind1].vertex[i], recRooms[floor][ind1].vertex[(i + 1) % recRooms[floor][ind1].vertex.Count],
						recRooms[floor][ind2].vertex[(j + 1) % recRooms[floor][ind2].vertex.Count]))
					{
						c = true;
						break;
					}
				}
				if (c)
				{
					switch (dir)
					{
						case 0:
							if (recRooms[floor][ind2].center.x > recRooms[floor][ind1].center.x)
								conn = true;
							break;
						case 1:
							if (recRooms[floor][ind2].center.x < recRooms[floor][ind1].center.x)
								conn = true;
							break;
						case 2:
							if (recRooms[floor][ind2].center.y > recRooms[floor][ind1].center.y)
								conn = true;
							break;
						case 3:
							if (recRooms[floor][ind2].center.y < recRooms[floor][ind1].center.y)
								conn = true;
							break;
					}
				}
			}
			// check ind1 on ind2
			for (i = 0; i < recRooms[floor][ind2].vertex.Count; i++)
			{
				if (dir == 0 || dir == 1) // X-dir
				{
					if (Math.Abs(recRooms[floor][ind2].vertex[i].y -
						recRooms[floor][ind2].vertex[(i + 1) % recRooms[floor][ind2].vertex.Count].y) < thresh)
						continue;
				}
				else 
				{
					if (Math.Abs(recRooms[floor][ind2].vertex[i].x -
						recRooms[floor][ind2].vertex[(i + 1) % recRooms[floor][ind2].vertex.Count].x) < thresh)
						continue;
				}
				for (j = 0; j < recRooms[floor][ind1].vertex.Count; j++)
				{
					if (Point_In_Line(recRooms[floor][ind2].vertex[i], recRooms[floor][ind2].vertex[(i + 1) % recRooms[floor][ind2].vertex.Count],
						recRooms[floor][ind1].vertex[j]))
					{
						c = true;
						break;
					}
					if (Point_On_Line(recRooms[floor][ind2].vertex[i], recRooms[floor][ind2].vertex[(i + 1) % recRooms[floor][ind2].vertex.Count],
						recRooms[floor][ind1].vertex[j]) &&
						Point_On_Line(recRooms[floor][ind2].vertex[i], recRooms[floor][ind2].vertex[(i + 1) % recRooms[floor][ind2].vertex.Count],
						recRooms[floor][ind1].vertex[(j + 1) % recRooms[floor][ind1].vertex.Count]))
					{
						c = true;
						break;
					}
				}
				if (c)
				{
					switch (dir)
					{
						case 0:
							if (recRooms[floor][ind2].center.x > recRooms[floor][ind1].center.x)
								conn = true;
							break;
						case 1:
							if (recRooms[floor][ind2].center.x < recRooms[floor][ind1].center.x)
								conn = true;
							break;
						case 2:
							if (recRooms[floor][ind2].center.y > recRooms[floor][ind1].center.y)
								conn = true;
							break;
						case 3:
							if (recRooms[floor][ind2].center.y < recRooms[floor][ind1].center.y)
								conn = true;
							break;
					}
				}
			}
			return (conn);
		}

		public double[] ApproxRect(int floor, int r)
		{
			// The approximate rectangle of a room
			int i;
			double[] lw = new double[4]; // cx, cy, len, wid
			double edgelen, max = double.MinValue, min = double.MaxValue, minx = double.MaxValue, miny = double.MaxValue;
			int xy = -1, mxy = -1, mixy = -1;
			double area = PolygonArea(recRooms[floor][r].vertices);
			Vector2d vc = new Vector2d();
			List<Vector2d> verts = new List<Vector2d>();

			for (i = 0; i < recRooms[floor][r].vertices.Count; i++)
				verts.Add(new Vector2d(recRooms[floor][r].vertices[i].Coord.x, recRooms[floor][r].vertices[i].Coord.y));

			for (i = 0; i < verts.Count; i++)
			{
				if (Math.Abs(verts[i].x - verts[(i + 1) % verts.Count].x) < thresh)
				{
					edgelen = Math.Abs(verts[i].y - verts[(i + 1) % verts.Count].y);
					xy = 0;
				}
				else
				{
					edgelen = Math.Abs(verts[i].x - verts[(i + 1) % verts.Count].x);
					xy = 1;
				}
				if (max < edgelen)
				{
					max = edgelen;
					mxy = xy;
				}
				if (min > edgelen)
				{
					min = edgelen;
					mixy = xy;
				}
				if (minx > verts[i].x)
					minx = verts[i].x;
				if (miny > verts[i].y)
					miny = verts[i].y;
			}

			lw[0] = vc.x;
			lw[1] = vc.y;
			if (mxy == 0)
			{
				lw[2] = max;
				//lw[2] = (max + min) / 2;
				lw[3] = area / max;
			}
			else
			{
				lw[3] = max;
				//lw[3] = (max + min) / 2;
				lw[2] = area / max;
			}
			
			recRooms[floor][r].length = lw[2];
			recRooms[floor][r].width = lw[3];
			recRooms[floor][r].center = new Vector2d(minx + 0.5 * lw[3], miny + 0.5 * lw[2]);
			double ad = lw[2] * lw[3] - recRooms[floor][r].area;
			Console.WriteLine(ad);
			return (lw);
		}

		int mxy = -1;
		Vertex orv0 = new Vertex(0, 0, 0), orv1 = new Vertex(0, 0, 0);
		List<Vector2d> mwc = new List<Vector2d>();
		List<int> wallinds = new List<int>();
		double currmove = 0;
		double[] currLimits = new double[4];
		public bool isOpt = true;
		private void MoveWall(int up)
		{
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			int i, j;
			int indF = moveWall[0], indW = moveWall[1];
			MouseMovePos(XYPlane);
			if (moveWall[0] == -1 || moveWall[1] == -1) return;
			activeFloor = moveWall[0];
			if (times == 0)	// Record the initial locations
			{
				relatedVerts = new List<Vertex>();
				deleteVertex = new List<int>();
				wallinds = new List<int>();
				wallinds = GetRelatedWall(moveWall, walls[indF][indW].type);
				orv0 = new Vertex(walls[indF][indW].endPoints[0]);
				orv1 = new Vertex(walls[indF][indW].endPoints[1]);

				currLimits[0] = double.MaxValue;
				currLimits[1] = double.MaxValue;
				currLimits[2] = double.MinValue;
				currLimits[3] = double.MinValue;
				// the direction from move wall to the center of the room affected 
				// a move wall segment will only affect two rooms
				mwc = new List<Vector2d>();
				for (int p = 0; p < walls[indF][indW].roomIndices.Count; p++)
				{
					int r = walls[indF][indW].roomIndices[p];
					if (Math.Abs(walls[indF][indW].endPoints[0].Coord.x - walls[indF][indW].endPoints[1].Coord.x) < thresh)
					{
						mxy = 0;
						mwc.Add(new Vector2d(walls[indF][indW].endPoints[0].Coord.x - recRooms[indF][r].center.x, 0));
						mwc[mwc.Count - 1] = mwc[mwc.Count - 1].Normalize();
						// In case the wall is pulled over the other one
						for (int q = 0; q < recRooms[indF][r].wallIndices.Count; q++)
						{
							int w = recRooms[indF][r].wallIndices[q];
							if (w == indW || Math.Abs(walls[indF][w].direction.x) > thresh) continue;

							if (walls[indF][w].endPoints[0].Coord.x > walls[indF][indW].endPoints[0].Coord.x)
								currLimits[2] = Math.Max(currLimits[2], walls[indF][w].endPoints[0].Coord.x - minLenOfWall );
							if (walls[indF][w].endPoints[0].Coord.x < walls[indF][indW].endPoints[0].Coord.x)
								currLimits[0] = Math.Min(currLimits[0], walls[indF][w].endPoints[0].Coord.x + minLenOfWall );
						}
					}
					else
					{
						mxy = 1;
						mwc.Add(new Vector2d(0, walls[indF][indW].endPoints[0].Coord.y - recRooms[indF][r].center.y));
						mwc[mwc.Count - 1] = mwc[mwc.Count - 1].Normalize();
						for (int q = 0; q < recRooms[indF][r].wallIndices.Count; q++)
						{
							int w = recRooms[indF][r].wallIndices[q];
							if (w == indW || Math.Abs(walls[indF][w].direction.y) > thresh) continue;

							if (walls[indF][w].endPoints[0].Coord.y > walls[indF][indW].endPoints[0].Coord.y)
								currLimits[3] = Math.Max(currLimits[3], walls[indF][w].endPoints[0].Coord.y - minLenOfWall);
							if (walls[indF][w].endPoints[0].Coord.y < walls[indF][indW].endPoints[0].Coord.y)
								currLimits[1] = Math.Min(currLimits[1], walls[indF][w].endPoints[0].Coord.y + minLenOfWall );
						}
					}
				}
				// for boundary walls
				if (currLimits[0] == double.MaxValue)
					currLimits[0] = double.MinValue;
				if (currLimits[2] == double.MinValue)
					currLimits[2] = double.MaxValue;
				if (currLimits[1] == double.MaxValue)
					currLimits[1] = double.MinValue;
				if (currLimits[3] == double.MinValue)
					currLimits[3] = double.MaxValue;
				if (isOpt)
				{
					currLimits[0] = Math.Max(blimits[indF, 0], currLimits[0]);
					currLimits[2] = Math.Min(blimits[indF, 2], currLimits[2]);
					currLimits[1] = Math.Max(blimits[indF, 1], currLimits[1]);
					currLimits[3] = Math.Min(blimits[indF, 3], currLimits[3]);
				}
				currLimits[0] = Math.Max(walls[indF][indW].endPoints[0].Coord.x - minLenOfWall, currLimits[0]);
				currLimits[2] = Math.Min(walls[indF][indW].endPoints[0].Coord.x + minLenOfWall, currLimits[2]);
				currLimits[1] = Math.Max(walls[indF][indW].endPoints[0].Coord.y - minLenOfWall, currLimits[1]);
				currLimits[3] = Math.Min(walls[indF][indW].endPoints[0].Coord.y + minLenOfWall, currLimits[3]);

				GetNeigborsOnAFloor(indF);
				lastMove = new Vector3d(0, 0, 0);
				Vector3d t;
				t_vertices = new List<Vector3d>();
				t_endPoints = new List<Vector3d>();
				for (i = 0; i < this.walls[indF][indW].cuboid.vertices.Count; ++i)
				{
					t = this.walls[indF][indW].cuboid.vertices[i].Coord;
					t_vertices.Add(new Vector3d(t.x, t.y, t.z));
				}
				t = this.walls[indF][indW].endPoints[0].Coord;
				t_endPoints.Add(new Vector3d(t.x, t.y, t.z));
				t = this.walls[indF][indW].endPoints[1].Coord;
				t_endPoints.Add(new Vector3d(t.x, t.y, t.z));
				times++;
			}

			// boundary check & 
			
			Vector3d mouseMove, currMouseMove;
			mouseMove = releasePoint - clickPoint;
			double moveDist = mouseMove.Length();
			if (!lastMove.IsNull())
			{
				currMouseMove = releasePoint - lastMove;
				if (mxy == 0)
					currmove = Math.Abs(releasePoint.x - lastMove.x);
				else
					currmove = Math.Abs(releasePoint.y - lastMove.y);
			}
			else
			{
				currMouseMove = releasePoint - clickPoint;
				if (mxy == 0)
					currmove = Math.Abs(releasePoint.x - clickPoint.x);
				else
					currmove = Math.Abs(releasePoint.y - clickPoint.y);
			}

			lastMove = releasePoint;
			mouseMove.z = 0.0;
			Vector3d direction = new Vector3d();
			if (walls[indF][indW].label == 1)
			{
				direction = walls[indF][indW].normal;
				if (currMouseMove.Normalize().Dot(direction) < 0)
					direction = -1.0 * direction;
			}

			Vector3d ori = new Vector3d(walls[indF][indW].endPoints[0].Coord);
			this.walls[indF][indW].endPoints[0].Coord += currmove * direction;
			this.walls[indF][indW].endPoints[1].Coord += currmove * direction;
			for (i = 0; i < walls[indF][indW].cuboid.vertices.Count; i++)
				this.walls[indF][indW].cuboid.vertices[i].Coord += currmove * direction;
			Vector3d pull = new Vector3d();

			if ((mxy == 0 && (walls[indF][indW].endPoints[0].Coord.x > currLimits[2] || walls[indF][indW].endPoints[0].Coord.x < currLimits[0])) ||
					(mxy == 1 && (walls[indF][indW].endPoints[0].Coord.y > currLimits[3] || walls[indF][indW].endPoints[0].Coord.y < currLimits[1])))
			{
				if (mxy == 0 && walls[indF][indW].endPoints[0].Coord.x > currLimits[2])
				{
					pull = new Vector3d(1, 0, 0) * (currLimits[2] - walls[indF][indW].endPoints[0].Coord.x);
					walls[indF][indW].endPoints[0].Coord.x = currLimits[2];
					walls[indF][indW].endPoints[1].Coord.x = currLimits[2];

				}
				else if (mxy == 0 && walls[indF][indW].endPoints[0].Coord.x < currLimits[0])
				{
					pull = new Vector3d(1, 0, 0) * (currLimits[0] - walls[indF][indW].endPoints[0].Coord.x);
					walls[indF][indW].endPoints[0].Coord.x = currLimits[0];
					walls[indF][indW].endPoints[1].Coord.x = currLimits[0];
				}
				else if (mxy == 1 && walls[indF][indW].endPoints[0].Coord.y > currLimits[3])
				{
					pull = new Vector3d(0, 1, 0) * (currLimits[3] - walls[indF][indW].endPoints[0].Coord.y);
					walls[indF][indW].endPoints[0].Coord.y = currLimits[3];
					walls[indF][indW].endPoints[1].Coord.y = currLimits[3];
				}
				else
				{
					pull = new Vector3d(0, 1, 0) * (currLimits[1] - walls[indF][indW].endPoints[0].Coord.y);
					walls[indF][indW].endPoints[0].Coord.y = currLimits[1];
					walls[indF][indW].endPoints[1].Coord.y = currLimits[1];
				}
			}
			for (i = 0; i < walls[indF][indW].cuboid.vertices.Count; i++)
				this.walls[indF][indW].cuboid.vertices[i].Coord += pull;
			
			if (isOpt)
			{
				double thismove = (walls[indF][indW].endPoints[0].Coord - ori).Length();
				currmove = thismove;
				
				direction = (walls[indF][indW].endPoints[0].Coord - ori).Normalize();
				double tarea = (walls[indF][indW].endPoints[0].Coord - walls[indF][indW].endPoints[1].Coord).Length() * thismove;
				double tlen, diff;
				for (i = 0; i < walls[indF][indW].roomIndices.Count; i++)
				{
					int r = walls[indF][indW].roomIndices[i];
					Vector2d dir = new Vector2d(direction.x, direction.y);// new Vector2d(direction.x, direction.y);
					double rarea = recRooms[indF][r].width * recRooms[indF][r].length;
					if (mwc[i].Dot(dir) > 0)
					{
						rarea += tarea;
					}
					else
					{
						rarea -= tarea;
					}
					if (mxy == 0)
					{
						tlen = rarea / recRooms[indF][r].length;
						diff = tlen - recRooms[indF][r].width;
						recRooms[indF][r].width = tlen;
						recRooms[indF][r].center.x += diff / 2;
					}
					else
					{
						tlen = rarea / recRooms[indF][r].width;
						diff = tlen - recRooms[indF][r].length;
						recRooms[indF][r].length = tlen;
						recRooms[indF][r].center.y += diff / 2;
					}
					recRooms[indF][r].area = rarea;
				}
			}

			// when release the mouse (onMounseUp), snap the wall if possible
			if (up == 1)
			{
				double dist = SnapWall(indF, indW);
				Vector3d dir;
				if (Math.Abs(dist) > thresh)
				{
					dir = new Vector3d(Math.Abs(direction.x), Math.Abs(direction.y), 0);
					for (i = 0; i < walls[indF][indW].cuboid.vertices.Count; i++)
					{
						walls[indF][indW].cuboid.vertices[i].Coord += dist * dir;
						if (i < walls[indF][indW].cuboid.vertices.Count / 2)
							walls[indF][indW].cuboid.planeVertices[i].Coord = walls[indF][indW].cuboid.vertices[i].Coord;
					}
				}
				double newv = 0;
				if (Math.Abs(direction.y) < thresh)
					newv = walls[indF][indW].endPoints[0].Coord.x;
				else
					newv = walls[indF][indW].endPoints[0].Coord.y;
				dir = (walls[indF][indW].endPoints[0].Coord - t_endPoints[0]).Normalize();
				for (i = 0; i < wallinds.Count; i += 2)
					UpdateRelatedWall(wallinds[i], wallinds[i + 1], dir, newv);
				FindRoomVertices(indF);
			}

			if (walls[indF][indW].doors != null)
			{
				for (i = 0; i < walls[indF][indW].doors.Count; i++)
				{
					Door door = walls[indF][indW].doors[i];
					for (j = 0; j < door.cuboid.vertices.Count; j++)
						door.cuboid.vertices[j].Coord += currmove * direction;
					door.cuboid.endPoints[0].Coord += currmove * direction;
					door.cuboid.endPoints[1].Coord += currmove * direction;
					door.UpdateDoor(mat);
				}
			}
		}

		public double SnapWall(int indF, int indW)
		{
			// If a wall is moved close to another wall, snap it
			// dist could be >0 or < 0
			int i, j = -1;
			double dist = 0, min = double.MaxValue, snap = 0.04;
			for (i = 0; i < walls[indF].Count; i++)
			{
				if (i == indW) continue;
				if (walls[indF][i].type == walls[indF][indW].type || walls[indF][i].type == Math.Abs(walls[indF][indW].type - 2))
				{
					if (walls[indF][indW].type == 0 || walls[indF][indW].type == 2)
					{
						dist = Math.Abs(walls[indF][i].endPoints[0].Coord.x - walls[indF][indW].endPoints[0].Coord.x);
						if (dist < min)
						{
							min = dist;
							j = i;
						}
					}
					else
					{
						dist = Math.Abs(walls[indF][i].endPoints[0].Coord.y - walls[indF][indW].endPoints[0].Coord.y);
						if (dist < min)
						{
							min = dist;
							j = i;
						}
					}
				}
			}

			dist = 0;
			if (min <= snap)
			{
				if (walls[indF][indW].type == 0 || walls[indF][indW].type == 2)
				{
					dist = walls[indF][j].endPoints[0].Coord.x - walls[indF][indW].endPoints[0].Coord.x;
					walls[indF][indW].endPoints[0].Coord.x = walls[indF][j].endPoints[0].Coord.x;
					walls[indF][indW].endPoints[1].Coord.x = walls[indF][j].endPoints[0].Coord.x;
				}
				else
				{
					dist = walls[indF][j].endPoints[0].Coord.y - walls[indF][indW].endPoints[0].Coord.y;
					walls[indF][indW].endPoints[0].Coord.y = walls[indF][j].endPoints[0].Coord.y;
					walls[indF][indW].endPoints[1].Coord.y = walls[indF][j].endPoints[0].Coord.y;
				}
			}
			return (dist);
		}

		List<Vertex> relatedVerts;
		List<int> deleteVertex;
		public void ReBuildWalls(int floor)
		{
			int i, j, k, t;
			List<int> indrs = new List<int>(), toAssign = new List<int>();
			if (moveWall[0] == -1 || moveWall[1] == -1) return;
			Vertex v0, v1;
			bool e0, e1;
			bool ex0 = false, ex1 = false; // Judge the original endpoints belong to some wall or not
			for (i = 0; i < walls[activeFloor].Count; i++)
			{
				if ((walls[activeFloor][i].endPoints[0].Coord - orv0.Coord).Length() < thresh ||
					(walls[activeFloor][i].endPoints[1].Coord - orv0.Coord).Length() < thresh)
				{
					relatedVerts.Add(new Vertex(orv0));
					ex0 = true;
					// the original endpoint 0 is pending --- it may cause miss connectiions between the new end point 0
				}
				else if ((walls[activeFloor][i].endPoints[0].Coord - orv1.Coord).Length() < thresh ||
					(walls[activeFloor][i].endPoints[1].Coord - orv1.Coord).Length() < thresh)
				{
					relatedVerts.Add(new Vertex(orv1));
					ex1 = true;
				}
			}
			relatedVerts.Add(new Vertex(walls[activeFloor][moveWall[1]].endPoints[0]));
			relatedVerts.Add(new Vertex(walls[activeFloor][moveWall[1]].endPoints[1]));

			// Delete walls[activeFloor] length = 0
			for (i = 0; i < walls[activeFloor].Count; i++)
			{
				if ((walls[activeFloor][i].endPoints[0].Coord - walls[activeFloor][i].endPoints[1].Coord).Length() < thresh)
					deleteVertex.Add(i);
			}

			InverseSort(deleteVertex);
			Distinct(deleteVertex);
			if (deleteVertex.Count > 0)
			{
				// Remove the walls[activeFloor] have been reversed
				for (i = 0; i < deleteVertex.Count; i++)
				{
					k = deleteVertex[i];
					walls[activeFloor].RemoveAt(k);
					if (moveWall[1] > k) moveWall[1]--;
					for (j = 0; j < recRooms[activeFloor].Count; j++)
					{
						recRooms[activeFloor][j].wallIndices.Remove(k);
						for (t = 0; t < recRooms[activeFloor][j].wallIndices.Count; t++)
							if (recRooms[activeFloor][j].wallIndices[t] > k)
								recRooms[activeFloor][j].wallIndices[t]--;
					}
				}
			}
			int indi = -1;
			for (i = 0; i < walls[activeFloor].Count; i++)
			{
				if (indwalls != null && indwalls.Count > 0 && (i == indwalls[0] || i == indwalls[1]))
					continue;
				v0 = walls[activeFloor][i].endPoints[0];
				v1 = walls[activeFloor][i].endPoints[1];
				indi = i;
				for (j = 0; j < relatedVerts.Count; j++)
				{
					v0 = walls[activeFloor][i].endPoints[0];
					v1 = walls[activeFloor][i].endPoints[1];
					// A translated endpoint split the wall into two walls[activeFloor]
					if (Point_In_Line(v0.Coord, v1.Coord, relatedVerts[j].Coord))
					{
						// Record the original wall belongs
						// Pay attention to the wall label!!!
						indrs = Intersect(walls[activeFloor][i].roomIndices, walls[activeFloor][moveWall[1]].roomIndices);
						walls[activeFloor].RemoveAt(i);
						for (t = 0; t < recRooms[activeFloor].Count; t++)
							recRooms[activeFloor][t].wallIndices.Remove(i);
						walls[activeFloor].Insert(i, new Wall(new Vertex(v0), new Vertex(relatedVerts[j])));
						walls[activeFloor][i].roomIndices = new List<int>();
						//RoomBelong(i);
						toAssign.Add(i);
						for (t = 0; t < indrs.Count; t++)
							walls[activeFloor][i].roomIndices.Add(indrs[t]);
						FindRoomWalls(activeFloor);

						walls[activeFloor].Insert(i + 1, new Wall(new Vertex(relatedVerts[j]), new Vertex(v1)));
						walls[activeFloor][i + 1].roomIndices = new List<int>();
						for (int p = 0; p < recRooms[activeFloor].Count; p++)
						{
							for (int q = 0; q < recRooms[activeFloor][p].wallIndices.Count; q++)
								if (recRooms[activeFloor][p].wallIndices[q] > i)
									recRooms[activeFloor][p].wallIndices[q]++;
						}
						for (t = 0; t < indrs.Count; t++)
							walls[activeFloor][i + 1].roomIndices.Add(indrs[t]);
						//RoomBelong(i + 1);
						toAssign.Add(i + 1);
						FindRoomWalls(activeFloor);
						if (i < moveWall[1]) moveWall[1]++;
						walls[activeFloor][moveWall[1]].special[0] = false;
						indi = i + 1;
					}
				}
			}
			for (i = 0; i < toAssign.Count; i++)
				RoomBelong(toAssign[i]);
			// Update the wall indices and room indices
			FindRoomWalls(activeFloor);
			bool[] edge = { false, false };
			bool e01, e11;
			for (i = 0; i < walls[activeFloor].Count; i++)
			{
				e0 = Point_On_Line(walls[activeFloor][i].endPoints[0].Coord,
					walls[activeFloor][i].endPoints[1].Coord, orv0.Coord);
				e01 = Point_On_Line(walls[activeFloor][i].endPoints[0].Coord,
					walls[activeFloor][i].endPoints[1].Coord, walls[activeFloor][moveWall[1]].endPoints[0].Coord);
				if (e0 && e01)
					edge[0] = true; // the wall already connected
				e1 = Point_On_Line(walls[activeFloor][i].endPoints[0].Coord,
					walls[activeFloor][i].endPoints[1].Coord, orv1.Coord);
				e11 = Point_On_Line(walls[activeFloor][i].endPoints[0].Coord,
					walls[activeFloor][i].endPoints[1].Coord, walls[activeFloor][moveWall[1]].endPoints[1].Coord);
				if (e1 && e11)
					edge[1] = true;
			}
			if (ex0 && !edge[0]) // Miss connection
			{
				walls[activeFloor].Insert(moveWall[1] + 1, new Wall(new Vertex(orv0), new Vertex(walls[activeFloor][moveWall[1]].endPoints[0].Coord)));
				walls[activeFloor][moveWall[1] + 1].special[1] = true;
				walls[activeFloor][moveWall[1] + 1].roomIndices = new List<int>();
				for (int p = 0; p < recRooms[activeFloor].Count; p++)
				{
					for (int q = 0; q < recRooms[activeFloor][p].wallIndices.Count; q++)
						if (recRooms[activeFloor][p].wallIndices[q] > moveWall[1])
							recRooms[activeFloor][p].wallIndices[q]++;
				}
				RoomBelong(moveWall[1] + 1);
				FindRoomWalls(activeFloor);
			}

			if (ex1 && !edge[1]) // Miss connection
			{
				walls[activeFloor].Insert(moveWall[1] + 1, new Wall(new Vertex(walls[activeFloor][moveWall[1]].endPoints[1].Coord), new Vertex(orv1)));
				walls[activeFloor][moveWall[1] + 1].special[1] = true;
				walls[activeFloor][moveWall[1] + 1].special[0] = true;
				walls[activeFloor][moveWall[1] + 1].roomIndices = new List<int>();
				for (int p = 0; p < recRooms[activeFloor].Count; p++)
				{
					for (int q = 0; q < recRooms[activeFloor][p].wallIndices.Count; q++)
						if (recRooms[activeFloor][p].wallIndices[q] > moveWall[1])
							recRooms[activeFloor][p].wallIndices[q]++;
				}
				RoomBelong(moveWall[1] + 1);
				FindRoomWalls(activeFloor);
			}

			List<int> toMerge = new List<int>();
			bool back = false;
			// Delete overlapping walls
			for (i = 0; i < walls[activeFloor].Count - 1; i++)
			{
				if (back)
				{
					i--;
					back = false;
				}
				for (j = i + 1; j < walls[activeFloor].Count; j++)
				{
					if (Point_On_Line(walls[activeFloor][j].endPoints[0].Coord, walls[activeFloor][j].endPoints[1].Coord,
					walls[activeFloor][i].endPoints[0].Coord) && Point_On_Line(walls[activeFloor][j].endPoints[0].Coord,
					walls[activeFloor][j].endPoints[1].Coord, walls[activeFloor][i].endPoints[1].Coord))
					{
						// exactly overlap
						if (Math.Abs((walls[activeFloor][i].endPoints[0].Coord - walls[activeFloor][i].endPoints[1].Coord).Length() -
							(walls[activeFloor][j].endPoints[0].Coord - walls[activeFloor][j].endPoints[1].Coord).Length()) < thresh)
						{
							for (int p = 0; p < recRooms[activeFloor].Count; p++)
							{
								for (int q = 0; q < recRooms[activeFloor][p].wallIndices.Count; q++)
								{
									int iw = recRooms[activeFloor][p].wallIndices[q];
									if (iw > j)
										recRooms[activeFloor][p].wallIndices[q]--;
								}
								recRooms[activeFloor][p].wallIndices.Remove(j);
							}
							walls[activeFloor].RemoveAt(j);
							j--;
						}
						else // wall j cover i
						{
							if ((walls[activeFloor][j].endPoints[0].Coord - walls[activeFloor][i].endPoints[0].Coord).Length() < thresh)
								walls[activeFloor][j].endPoints[0] = new Vertex(walls[activeFloor][i].endPoints[1]);
							else if ((walls[activeFloor][j].endPoints[0].Coord - walls[activeFloor][i].endPoints[1].Coord).Length() < thresh)
								walls[activeFloor][j].endPoints[0] = new Vertex(walls[activeFloor][i].endPoints[0]);
							else if ((walls[activeFloor][j].endPoints[1].Coord - walls[activeFloor][i].endPoints[0].Coord).Length() < thresh)
								walls[activeFloor][j].endPoints[1] = new Vertex(walls[activeFloor][i].endPoints[1]);
							else if ((walls[activeFloor][j].endPoints[1].Coord - walls[activeFloor][i].endPoints[1].Coord).Length() < thresh)
								walls[activeFloor][j].endPoints[1] = new Vertex(walls[activeFloor][i].endPoints[0]);
						}
					}
					else if (Point_On_Line(walls[activeFloor][i].endPoints[0].Coord, walls[activeFloor][i].endPoints[1].Coord,
						walls[activeFloor][j].endPoints[0].Coord) && Point_On_Line(walls[activeFloor][i].endPoints[0].Coord,
						walls[activeFloor][i].endPoints[1].Coord, walls[activeFloor][j].endPoints[1].Coord))
					{
						// if wall i repeat wall j, alreay handled in the "if"
						if ((walls[activeFloor][i].endPoints[0].Coord - walls[activeFloor][j].endPoints[0].Coord).Length() < thresh)
							walls[activeFloor][i].endPoints[0] = new Vertex(walls[activeFloor][j].endPoints[1]);
						else if ((walls[activeFloor][i].endPoints[0].Coord - walls[activeFloor][j].endPoints[1].Coord).Length() < thresh)
							walls[activeFloor][i].endPoints[0] = new Vertex(walls[activeFloor][j].endPoints[0]);
						else if ((walls[activeFloor][i].endPoints[1].Coord - walls[activeFloor][j].endPoints[0].Coord).Length() < thresh)
							walls[activeFloor][i].endPoints[1] = new Vertex(walls[activeFloor][j].endPoints[1]);
						else if ((walls[activeFloor][i].endPoints[1].Coord - walls[activeFloor][j].endPoints[1].Coord).Length() < thresh)
							walls[activeFloor][i].endPoints[1] = new Vertex(walls[activeFloor][j].endPoints[0]);
						back = true;
					}
					else if ((walls[activeFloor][i].endPoints[0].Coord - walls[activeFloor][j].endPoints[1].Coord).Length() < thresh ||
						(walls[activeFloor][i].endPoints[1].Coord - walls[activeFloor][j].endPoints[0].Coord).Length() < thresh ||
						(walls[activeFloor][i].endPoints[0].Coord - walls[activeFloor][j].endPoints[0].Coord).Length() < thresh ||
						(walls[activeFloor][i].endPoints[1].Coord - walls[activeFloor][j].endPoints[1].Coord).Length() < thresh)
					{
						toMerge.Add(i);
						toMerge.Add(j);
					}
				}
			}
			// Merge walls
			for (i = 0; i < toMerge.Count - 1; i += 2)
			{ }

			moveCuboid[0] = -1;
			moveCuboid[1] = -1;
			moveWall[0] = -1;
			moveWall[1] = -1;
			
			cuboids[activeFloor] = new List<Cuboid>();
			newed = new int[walls[activeFloor].Count * 2];
			for (i = 0; i < walls[activeFloor].Count; i++)
				walls[activeFloor][i].GetNormal();
			for (i = 0; i < walls[activeFloor].Count; i++)
				NewVertices(activeFloor, i);
			AddConnection(activeFloor, true);
			FindRoomVertices(activeFloor);
			//OrderRoomVertices(activeFloor);
		}

		public void OrderRoomVertices(int activeF)
		{
			// Calculste the area of each room
			Area = 0;
			int i, j, k, e;
			Vector3d v, v0;
			List<int> vinds;
			double thrh = 1e-3;
			bool found = false;
			for (i = 0; i < recRooms[activeF].Count; i++)
			{
				vinds = new List<int>();
				k = 0;
				vinds.Add(recRooms[activeF][i].wallIndices[0]);
				if (walls[activeF][vinds[0]].endPoints[0].Coord.x < walls[activeF][vinds[0]].endPoints[1].Coord.x + thresh &&
					walls[activeF][vinds[0]].endPoints[0].Coord.y < walls[activeF][vinds[0]].endPoints[1].Coord.y + thresh)
					e = 0;
				else e = 1;
				for (j = 1; j < recRooms[activeF][i].wallIndices.Count; j++)
				{
					vinds.Add(recRooms[activeF][i].wallIndices[j]);
					if (walls[activeF][vinds[j]].endPoints[0].Coord.x < walls[activeF][vinds[k]].endPoints[e].Coord.x + thresh &&
						walls[activeF][vinds[j]].endPoints[0].Coord.y < walls[activeF][vinds[k]].endPoints[e].Coord.y + thresh &&
						(walls[activeF][vinds[j]].type == 0 || walls[activeF][vinds[j]].type == 2))
					{
						k = j;
						e = 0;
					}
					else if (walls[activeF][vinds[j]].endPoints[1].Coord.x <= walls[activeF][vinds[k]].endPoints[e].Coord.x + thresh &&
						walls[activeF][vinds[j]].endPoints[1].Coord.y <= walls[activeF][vinds[k]].endPoints[e].Coord.y + thresh &&
						(walls[activeF][vinds[j]].type == 0 || walls[activeF][vinds[j]].type == 2))
					{
						k = j;
						e = 1;
					}
				}
				recRooms[activeF][i].vertices = new List<Vertex>();
				recRooms[activeF][i].vertices.Add(walls[activeF][vinds[k]].endPoints[e]);
				recRooms[activeF][i].vertices.Add(walls[activeF][vinds[k]].endPoints[1-e]);
				vinds.RemoveAt(k);
				// Find the vertices in clockwise/counter-clockwise order
				v0 = new Vector3d(0,0,0);
				v0 += recRooms[activeF][i].vertices[0].Coord;
				v0 += recRooms[activeF][i].vertices[1].Coord;
				while (vinds.Count > 1)
				{
					found = false;
					v = recRooms[activeF][i].vertices[recRooms[activeF][i].vertices.Count - 1].Coord;
					for (j = 0; j < vinds.Count; j++)
					{
						if ((v - walls[activeF][vinds[j]].endPoints[0].Coord).Length() < thrh)
						{
							walls[activeF][vinds[j]].endPoints[0].Coord.x = v.x;
							walls[activeF][vinds[j]].endPoints[0].Coord.y = v.y;
							recRooms[activeF][i].vertices.Add(walls[activeF][vinds[j]].endPoints[1]);
							v0 += walls[activeF][vinds[j]].endPoints[1].Coord;
							vinds.RemoveAt(j);
							found = true;
							break;
						}
						else if ((v - walls[activeF][vinds[j]].endPoints[1].Coord).Length() < thrh)
						{
							walls[activeF][vinds[j]].endPoints[1].Coord.x = v.x;
							walls[activeF][vinds[j]].endPoints[1].Coord.y = v.y;
							recRooms[activeF][i].vertices.Add(walls[activeF][vinds[j]].endPoints[0]);
							v0 += walls[activeF][vinds[j]].endPoints[0].Coord;
							vinds.RemoveAt(j);
							found = true;
							break;
						}
					}

					if (!found)
						break;
				}
				Area += PolygonArea(recRooms[activeF][i].vertices);
				Vector3d vc = v0 / recRooms[activeF][i].vertices.Count;
				recRooms[activeF][i].center = new Vector2d(vc.x, vc.y);
			}
		}

		public void FindRoomVertices(int activeF)
		{
			// Calculste the area of each room
			Area = 0;
			int i, j, k;
			List<int> vinds;
			for (i = 0; i < recRooms[activeF].Count; i++)
			{
				vinds = new List<int>();
				k = 0;
				vinds.Add(recRooms[activeF][i].wallIndices[0]);
				recRooms[activeF][i].vertices = new List<Vertex>();
				for (j = 0; j < recRooms[activeF][i].wallIndices.Count; j++)
				{
					k = recRooms[activeF][i].wallIndices[j];
					recRooms[activeF][i].vertices.Add(walls[activeF][k].endPoints[0]);
					recRooms[activeF][i].vertices.Add(walls[activeF][k].endPoints[1]);
				}
			}
			for (i = 0; i < recRooms[activeF].Count; i++)
				Distinct(recRooms[activeF][i].vertices);
			OrderRoomVertices(activeF);
		}

		public void RoomBelong(int ind)
		{
			// Find out wall ind belong to which recRooms
			bool e0, e1;
			int i, j, k;
			for (i = 0; i < recRooms[activeFloor].Count; i++)
			{
				e0 = false;
				e1 = false;
				for (j = 0; j < recRooms[activeFloor][i].wallIndices.Count; j++)
				{
					k = recRooms[activeFloor][i].wallIndices[j];
					if (Point_On_Line(walls[activeFloor][k].endPoints[0].Coord, walls[activeFloor][k].endPoints[1].Coord,
						walls[activeFloor][ind].endPoints[0].Coord))
						e0 = true;
					if (Point_On_Line(walls[activeFloor][k].endPoints[0].Coord, walls[activeFloor][k].endPoints[1].Coord,
						walls[activeFloor][ind].endPoints[1].Coord))
						e1 = true;
				}
				if (e0 && e1)
					walls[activeFloor][ind].roomIndices.Add(i);
			}
		}

		public void SnapWall(int floor)
		{
			// If a wall is moved close to another wall, snap it
			int i, j, k, t, xy = -1, ind1 = -1, ind2 = -1, ind;
			double diff1, diff2, value, snap = wallThickness * 2, thresh = 1e-3;
			bool snapped;
			List<int> changed = new List<int>();
			List<Vector2d> verts = new List<Vector2d>();
			Vector2d v = new Vector2d();

			for (i = 0; i < recRooms[floor].Count - 1; i++)
			{
				for (j = i + 1; j < recRooms[floor].Count; j++)
				{
					if (changed.IndexOf(j) != -1) continue;
					snapped = false;
					for (k = 0; k < recRooms[floor][i].vertex.Count; k++)
					{
						for (t = 0; t < recRooms[floor][j].vertex.Count; t++)
						{
							diff2 = (recRooms[floor][j].vertex[t] - recRooms[floor][i].vertex[k]).Length();
							// The distance needs to be shorter than snap value but larger than 0
							if (diff2 > snap || diff2 < thresh) continue;
							diff1 = Math.Abs(recRooms[floor][i].vertex[k].y - recRooms[floor][j].vertex[t].y);
							diff2 = Math.Abs(recRooms[floor][i].vertex[k].x - recRooms[floor][j].vertex[t].x);
							if (diff1 < thresh)
								xy = 0;
							else
								xy = 1;
							snapped = true;
							ind1 = k; ind2 = t;
							snapped = true;
							break;
						}
						if (snapped) break;
					}
					if (snapped)
					{
						//if (OnBoundary(floor, recRooms[floor][j].vertex[ind2],
						//        recRooms[floor][j].vertex[(ind2 + 1) % recRooms[floor][j].vertex.Count]) ||
						//        OnBoundary(floor, recRooms[floor][j].vertex[ind2],
						//        recRooms[floor][j].vertex[(ind2 - 1 + recRooms[floor][j].vertex.Count) % recRooms[floor][j].vertex.Count]))
						//    continue;

						changed.Add(j);
						v = new Vector2d(recRooms[floor][j].vertex[ind2]);
						if (xy == 0) // Move in x dir
						{
							value = recRooms[floor][i].vertex[ind1].x;
							//recRooms[floor][j].center.x += 0.5 * (value - v.x);
							recRooms[floor][j].vertex[ind2] = new Vector2d(value,
								recRooms[floor][j].vertex[ind2].y);
							ind = (ind2 + 1) % recRooms[floor][j].vertex.Count;
							if (Math.Abs(recRooms[floor][j].vertex[ind].x - v.x) > thresh)
								ind = (ind2 - 1 + recRooms[floor][j].vertex.Count) % recRooms[floor][j].vertex.Count;
							recRooms[floor][j].vertex[ind] = new Vector2d(value, recRooms[floor][j].vertex[ind].y);
							for (int p = 0; p < recRooms[floor].Count; p++)
							{
								if (p == i || p == j || changed.IndexOf(p) != -1) continue;
								for (int q = 0; q < recRooms[floor][p].vertex.Count; q++)
									if (Math.Abs(recRooms[floor][p].vertex[q].x - v.x) < thresh)
									{
										changed.Add(p);
										//recRooms[floor][p].center.x += 0.5 * (value - v.x);
										recRooms[floor][p].vertex[q] = new Vector2d(value, recRooms[floor][p].vertex[q].y);
										ind = (q + 1) % recRooms[floor][p].vertex.Count;
										if (Math.Abs(recRooms[floor][p].vertex[ind].x - v.x) > thresh)
											ind = (q - 1 + recRooms[floor][p].vertex.Count) % recRooms[floor][p].vertex.Count;
										recRooms[floor][p].vertex[ind] = new Vector2d(value, recRooms[floor][p].vertex[ind].y);
										break;
									}
							}
						}
						else
						{
							value = recRooms[floor][i].vertex[ind1].y;
							//recRooms[floor][j].center.y += 0.5 * (value - v.y);
							recRooms[floor][j].vertex[ind2] = new Vector2d(recRooms[floor][j].vertex[ind2].x, value);
							ind = (ind2 + 1) % recRooms[floor][j].vertex.Count;
							if (Math.Abs(recRooms[floor][j].vertex[ind].y - v.y) > thresh)
								ind = (ind2 - 1 + recRooms[floor][j].vertex.Count) % recRooms[floor][j].vertex.Count;
							recRooms[floor][j].vertex[ind] = new Vector2d(recRooms[floor][j].vertex[ind].x, value);
							for (int p = 0; p < recRooms[floor].Count; p++)
							{
								if (p == i || p == j || changed.IndexOf(p) != -1) continue;
								for (int q = 0; q < recRooms[floor][p].vertex.Count; q++)
									if (Math.Abs(recRooms[floor][p].vertex[q].y - v.y) < thresh)
									{
										changed.Add(p);
										//recRooms[floor][p].center.y += 0.5 * (value - v.y);
										recRooms[floor][p].vertex[q] = new Vector2d(recRooms[floor][p].vertex[q].x, value);
										ind = (q + 1) % recRooms[floor][p].vertex.Count;
										if (Math.Abs(recRooms[floor][p].vertex[ind].y - v.y) > thresh)
											ind = (q - 1 + recRooms[floor][p].vertex.Count) % recRooms[floor][p].vertex.Count;
										recRooms[floor][p].vertex[ind] = new Vector2d(recRooms[floor][p].vertex[ind].x, value);
										break;
									}
							}
						}
					}
				}
			}
			// Delete repeat vertex
			for (i = 0; i < recRooms[floor].Count; i++)
			{
				if (recRooms[floor][i].vertex.Count <= 4) continue;
				for (j = 0; j < recRooms[floor][i].vertex.Count - 1; j++)
				{
					if ((recRooms[floor][i].vertex[j] - recRooms[floor][i].vertex[j + 1]).Length() < thresh)
					{
						recRooms[floor][i].vertex.RemoveAt(j + 1);
						recRooms[floor][i].vertex.RemoveAt(j);
					}
				}
			}
		}

		Room StairRoom = new Room();
		double lastMoveDist = 0;
		private void MoveCuboid()
		{
			int indF, indC, indW, stair = -1;
			bool onBound, isStair = false;
			Vector2d v1, v2;
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			if (moveCuboid[0] == -1 || moveCuboid[1] == -1) return;
			indF = moveCuboid[0];
			indC = moveCuboid[1];
			if (cuboids[indF][indC].label == 1)	//move wall
			{
				moveWall[0] = indF;
				moveWall[1] = cuboids[indF][indC].wallIndex;
				indW = moveWall[1];
				v1 = new Vector2d(walls[indF][indW].endPoints[0].Coord.x, walls[indF][indW].endPoints[0].Coord.y);
				v2 = new Vector2d(walls[indF][indW].endPoints[1].Coord.x, walls[indF][indW].endPoints[1].Coord.y);
				onBound = OnBoundary(activeFloor, v1, v2);
				for (int i = 0; i < recRooms[indF].Count; i++)
					if (recRooms[indF][i].name == "Stair")
					{
						stair = i;
						break;
					}
				if (walls[indF][moveWall[1]].roomIndices.IndexOf(stair) != -1)
					isStair = true;
				if (isOpt)
				{
					if (onBound || isStair)
					{
						moveWall = new int[2] { -1, -1 };
						moveCuboid = new int[2] { -1, -1 };
						//if (isStair)
						//    MessageBox.Show("Stairs cannot be moved.");
						//else if (onBound)
						//    MessageBox.Show("Boundary is fixed.");
						isMouseDown = false;
						return;
					}
				}
				MoveWall(0);
			}
			else //move door
			{
				int ind = cuboids[indF][indC].index;
				Vector3d direction = doors[indF][ind].cuboid.direction;
				MouseMovePos(walls[indF][doors[indF][ind].belong].cuboid.planes[0]);
				Vector3d mouseMove = releasePoint - clickPoint;

				if (mouseMove.Dot(doors[indF][ind].cuboid.direction) <= 0)
					direction = -1 * direction;
				double moveDist = mouseMove.Dot(direction) - lastMoveDist;
				lastMoveDist = mouseMove.Dot(direction);
				for (int i = 0; i < 2; i++)
					doors[indF][ind].cuboid.endPoints[i].Coord += moveDist * direction;
				doors[indF][ind].UpdateDoor(mat);
			}
		}

		

		public void AddDoor(int floor, int w, int wd, double wid)
		{
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			Door door;
			double dist, width = 0;
			double len = (walls[floor][w].endPoints[1].Coord - walls[floor][w].endPoints[0].Coord).Length();
			if (wid > 0)
				width = wid;
			if (wd == 1) // door
			{
				dist = wallThickness * 2;
				door = new Door(dist, walls[floor][w], w, doors.Count, mat, width);
			}
			else
			{
				dist = len / 2 - 0.025;
				//door = new Door(dist, walls[floor][w], w, doors.Count, mat, Math.Min(0.1, len * 0.4), slabHeight, slabHeight);
				door = new Door(dist, walls[floor][w], w, doors.Count, mat, Math.Min(0.1, len * 0.4), 0.12, 0.12);
			}
			door.index = doors[floor].Count;
			door.cuboid.wallIndex = w;
			//door.texture = LoadImage(2);
			doors[floor].Add(door);
			cuboids[floor].Add(door.cuboid);
			cuboids[floor][cuboids[floor].Count - 1].index = doors[floor].Count - 1;
			walls[floor][w].doors.Add(door);
		}

		public void AddDoor_5(int floor, int w, int wd, double wid)
		{
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			Door door;
			double dist, width = 0;
			double len = (walls[floor][w].endPoints[1].Coord - walls[floor][w].endPoints[0].Coord).Length();
			if (wid > 0)
				width = wid;

				dist = wallThickness * 8;
				door = new Door(dist, walls[floor][w], w, doors.Count, mat, width, 1);

			door.cuboid.wallIndex = w;
			//door.texture = LoadImage(2);
			doors[floor].Add(door);
			cuboids[floor].Add(door.cuboid);
			cuboids[floor][cuboids[floor].Count - 1].index = doors[floor].Count - 1;
			walls[floor][w].doors.Add(door);
		}

		public void AddConnection(int floor, bool isFirstcall)
		{
			// Add door or open wall according to user definition
			int i, j, k, w, r;
			List<int> inters, wins = new List<int>();
			double dist;
			bool hasdoor;
			Vector2d v1, v2;

			doors[floor] = new List<Door>();
			for (i = 0; i < walls[floor].Count; i++)
			{
				walls[floor][i].doors = new List<Door>();
				walls[floor][i].open = false;
			}

			if (isFirstcall)
			{
				for (j = 0; j < recRooms[floor].Count; j++)
				{
					if (String.Compare(recRooms[floor][j].name, "Living") == 0)
					{
						for (i = 0; i < recRooms[floor].Count; i++)
						{
							if (String.Compare(recRooms[floor][i].name, "Dining") == 0 || //(not for layout_01)
								String.Compare(recRooms[floor][i].name, "Stair") == 0 ||
								recRooms[floor][i].name.Substring(0, 4) == "Hall" ||
								(LayoutInd > 3 && recRooms[floor][i].name == "Entry"))
							{
								if (recRooms[floor][j].conns.IndexOf(i) == -1)
								{
									recRooms[floor][j].conns.Add(i);
									recRooms[floor][j].types.Add(2);
									recRooms[floor][i].conns.Add(j);
									recRooms[floor][i].types.Add(2);
								}
							}
						}
						break;
					}
				}
			}
			for (j = 0; j < recRooms[floor].Count; j++)
			{
				for (k = 0; k < recRooms[floor][j].conns.Count; k++)
				{
					r = recRooms[floor][j].conns[k];
					if (r < j) continue;
					inters = Intersect(recRooms[floor][j].wallIndices, recRooms[floor][r].wallIndices);
					if (inters.Count == 0) continue;
					w = -1;
					for (int p = 0; p < inters.Count; p++)
					{
						w = inters[p];
						if ((walls[floor][w].endPoints[1].Coord - walls[floor][w].endPoints[0].Coord).Length() > 0.12)
							break;
					}
					if (recRooms[floor][j].types[k] == 1) //door
					{
						AddDoor(floor, w, 1, 0);
					}
					else if (recRooms[floor][j].types[k] == 2 && isFirstcall) //Open wall
					{
						for (int t = 0; t < inters.Count; t++)
							walls[floor][inters[t]].open = true;
					}
				}
			}

			for (j = 0; j < recRooms[floor].Count; j++)
			{
				if (recRooms[floor][j].name == "Garage")
				{
					for (k = 0; k < recRooms[floor][j].wallIndices.Count; k++)
					{
						r = recRooms[floor][j].wallIndices[k];
						if (walls[floor][r].roomIndices.Count == 1)
						{
							double width = (walls[floor][r].endPoints[0].Coord - walls[floor][r].endPoints[1].Coord).Length();
							width *= 0.6;
							AddDoor(floor, r, 1, width);
							break;
						}
					}
				}
			}
			// Open walls 
			if (isFirstcall)
			{
				for (j = 0; j < recRooms[floor].Count; j++)
				{
					if (recRooms[floor][j].name == "Entry" ||
						recRooms[floor][j].name == "Porch") 
					{
						for (k = 0; k < recRooms[floor][j].wallIndices.Count; k++)
						{
							r = recRooms[floor][j].wallIndices[k];
							if (walls[floor][r].roomIndices.Count == 1)
							{
								walls[floor][r].open = true;
							}
						}
					}
				}

				// Check closed rooms
				List<int> ind;
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					if (LayoutInd > 3 && recRooms[floor][i].name == "Stair") continue;
					hasdoor = false;
					ind = new List<int>();
					for (j = 0; j < recRooms[floor][i].wallIndices.Count; j++)
					{
						k = recRooms[floor][i].wallIndices[j];
						if (walls[floor][k].open || walls[floor][k].doors.Count > 0)
						{
							hasdoor = true;
							break;
						}
						else // Add the room vertices that are neighbors of i
						{
							for (int t = 0; t < walls[floor][k].roomIndices.Count; t++)
							{
								int wr = walls[floor][k].roomIndices[t];
								if (wr == i) continue;
								if (recRooms[floor][wr].name.Substring(0, 4) == "Hall" ||
									String.Compare(recRooms[floor][wr].name, "Living") == 0)
								{
									ind.Add(wr);
									ind.Add(k);
								}
							}
						}
					}

					// Get a door
					if (!hasdoor)
					{
						int wd = -1, ww = -1;
						if (ind.Count > 0)
						{
							wd = ind[0];
							ww = ind[1];
						}
						else
						{
							for (j = 0; j < recRooms[floor][i].wallIndices.Count; j++)
							{
								k = recRooms[floor][i].wallIndices[j];
								if (walls[floor][k].roomIndices.Count > 1)
								{
									int h = -1;
									for (h = 0; h < walls[floor][k].roomIndices.Count; h++)
										if (walls[floor][k].roomIndices[h] != i)
											break;
									if (h < walls[floor][k].roomIndices.Count)
									{
										wd = walls[floor][k].roomIndices[h];
										ww = k;
									}
								}
							}
						}
						if (ww == -1)
							continue;
						if (recRooms[floor][i].name == "Foyer" ||
							recRooms[floor][i].name.Substring(0, 4) == "Hall" ||
							recRooms[floor][i].name == "Stair")
						{
							walls[floor][ww].open = true;
							recRooms[floor][i].conns.Add(wd);
							recRooms[floor][i].types.Add(2);
							recRooms[floor][wd].conns.Add(i);
							recRooms[floor][wd].types.Add(2);
						}
						else
						{
							dist = wallThickness * 2;// (walls[floor][ww].endPoints[1].Coord - walls[floor][ww].endPoints[0].Coord).Length() / 2 - 0.025;
							AddDoor(floor, ww, 1, 0);
							recRooms[floor][i].conns.Add(wd);
							recRooms[floor][i].types.Add(1);
							recRooms[floor][wd].conns.Add(i);
							recRooms[floor][wd].types.Add(1);
						}
					}
				}

				for (i = 0; i < recRooms[floor].Count; i++)
				{
					if (recRooms[floor][i].name == "Foyer")
					{
						for (j = 0; j < recRooms[floor][i].wallIndices.Count; j++)
						{
							k = recRooms[floor][i].wallIndices[j];
							if (walls[floor][k].doors.Count > 0 ||
								walls[floor][k].roomIndices.Count == 1)
								continue;
							for (int t = 0; t < walls[floor][k].roomIndices.Count; t++)
							{
								int wr = walls[floor][k].roomIndices[t];
								if (wr == i) continue;
								if (recRooms[floor][wr].name == "Living" ||
									recRooms[floor][wr].name == "Dining" ||
									recRooms[floor][wr].name.Substring(0, 4) == "Hall")
								{

									walls[floor][k].open = true;
									recRooms[floor][i].conns.Add(wr);
									recRooms[floor][i].types.Add(2);

									break;
								}
							}
						}
						break;
					}
				}
				for (j = 0; j < recRooms[floor].Count; j++)
				{
					if (String.Compare(recRooms[floor][j].name, "Balcony") == 0)
					{
						for (k = 0; k < recRooms[floor][j].wallIndices.Count; k++)
						{
							r = recRooms[floor][j].wallIndices[k];
							if (walls[floor][r].roomIndices.Count == 1)
								walls[floor][r].open = true;
						}
					}
				}
			}
			// Add windows
			FindBoundRoom(floor);
			for (i = 0; i < bdInds.Length; i++)
			{
				for (j = 0; j < bdInds[i].Count; j++)
				{
					k = bdInds[i][j];
					if (recRooms[floor][k].name.Substring(0, 4) == "Hall" ||
						recRooms[floor][k].name == "Stair" ||
						wins.IndexOf(k) != -1)
						continue;
					for (int t = 0; t < recRooms[floor][k].wallIndices.Count; t++)
					{
						w = recRooms[floor][k].wallIndices[t];
						if (walls[floor][w].open ||
							(walls[floor][w].doors != null && walls[floor][w].doors.Count > 0))
							continue; // For space like Entry
						v1 = new Vector2d(walls[floor][w].endPoints[0].Coord.x,
							walls[floor][w].endPoints[0].Coord.y);
						v2 = new Vector2d(walls[floor][w].endPoints[1].Coord.x,
							walls[floor][w].endPoints[1].Coord.y);
						if (Point_On_Line(bounds[floor][i], bounds[floor][(i + 1) % bounds[floor].Count], v1) &&
							Point_On_Line(bounds[floor][i], bounds[floor][(i + 1) % bounds[floor].Count], v2))
						{
							AddDoor(floor, w, 2, 0);
							wins.Add(k);
							break;
						}
					}
				}
			}
		}

		public void ConnectionRepeatCheck()
		{
			int floor, i, j, k, r, ind;
			List<int> wdoors;
			int[] labels;
			for (floor = 0; floor < numFloor; floor++)
			{
				for (i = 0; i < recRooms[floor].Count; i++)
				{
					if (recRooms[floor][i].name.Substring(0, 4) == "Hall" ||
						recRooms[floor][i].name == "Living" ||
						recRooms[floor][i].name == "Entry" ||
						recRooms[floor][i].name == "Porch" ||
						recRooms[floor][i].name == "Foyer")
						continue;
					// open wall check
					bool open = false;
					for (j = 0; j < recRooms[floor][i].wallIndices.Count; j++)
					{
						k = recRooms[floor][i].wallIndices[j];
						if (walls[floor][k].open)
						{
							open = true;
							break;
						}
					}
					if (open)
					{
						for (j = 0; j < recRooms[floor][i].wallIndices.Count; j++)
						{
							k = recRooms[floor][i].wallIndices[j];
							if (walls[floor][k].roomIndices.Count < 2 || walls[floor][k].doors.Count == 0)
								continue;
							if (walls[floor][k].doors[0].elevation > thresh) continue;
							for (int p = 0; p < walls[floor][k].roomIndices.Count; p++)
							{
								r = walls[floor][k].roomIndices[p];
								if (recRooms[floor][r].name.Substring(0, 4) == "Hall" ||
									recRooms[floor][r].name == "Living" ||
									recRooms[floor][r].name == "Entry")
								{
									ind = walls[floor][k].doors[0].index;
									cuboids[floor].RemoveAt(walls[floor].Count + ind);
									doors[floor].RemoveAt(ind);
									for (int q = 0; q < doors[floor].Count; q++)
										if (doors[floor][q].index > ind)
											doors[floor][q].index--;
									walls[floor][k].doors = new List<Door>();
									break;
								}
							}
						}
					}
					
					wdoors = new List<int>();
					for (j = 0; j < recRooms[floor][i].wallIndices.Count; j++)
					{
						k = recRooms[floor][i].wallIndices[j];
						if (walls[floor][k].roomIndices.Count < 2 || walls[floor][k].doors.Count == 0)
							continue;
						if (walls[floor][k].doors[0].elevation < thresh) //door not window
							wdoors.Add(k);
					}
					if (wdoors.Count < 2) continue;
					labels = new int[wdoors.Count];
					// Delete redundant doors
					for (j = 0; j < wdoors.Count; j++)
					{
						labels[j] = 0;
						for (k = 0; k < walls[floor][wdoors[j]].roomIndices.Count; k++)
						{
							r = walls[floor][wdoors[j]].roomIndices[k];
							if (r == i)	continue;
							if (recRooms[floor][r].name.Substring(0, 4) == "Hall")
							{
								labels[j] = 1;
								break;
							}
						}
					}
					for (j = 0; j < wdoors.Count; j++)
					{
						bool del = true;
						if (labels[j] == 0)
						{
							// check whther the other room only has one door
							for (int p = 0; p < walls[floor][wdoors[j]].roomIndices.Count; p++)
							{
								r = walls[floor][wdoors[j]].roomIndices[p];
								if (r == i) continue;
								int d = 0;
								for (int q = 0; q < recRooms[floor][r].wallIndices.Count; q++)
								{
									int w = recRooms[floor][r].wallIndices[q];
									if (w == wdoors[j]) continue;
									if (walls[floor][w].doors.Count > 0 && walls[floor][w].doors[0].elevation < thresh)
										d++;
								}
								if (d == 0) // the door can not be deleted
								{
									del = false;
									break;
								}
							}
							if (!del) continue;
							ind = walls[floor][wdoors[j]].doors[0].index;
							cuboids[floor].RemoveAt(walls[floor].Count + ind);
							doors[floor].RemoveAt(ind);
							for (int p = 0; p < doors[floor].Count; p++)
								if (doors[floor][p].index > ind)
									doors[floor][p].index--;
							walls[floor][wdoors[j]].doors = new List<Door>();
						}
					}
					if (Sum(labels) == labels.Length) // all open to Hall
					{
						for (j = 0; j < wdoors.Count - 1; j++)
						{
							ind = walls[floor][wdoors[j]].doors[0].index;
							cuboids[floor].RemoveAt(walls[floor].Count + ind);
							doors[floor].RemoveAt(ind);
							for (int p = 0; p < doors[floor].Count; p++)
								if (doors[floor][p].index > ind)
									doors[floor][p].index--;
							walls[floor][wdoors[j]].doors = new List<Door>();
						}
					}
				}
			}
		}

		public void FindRoomWalls(int floor)
		{
			// Collect from room indices of walls[floor] 
			int i, j;
			for (i = 0; i < walls[floor].Count; i++)
				Distinct(walls[floor][i].roomIndices);
			for (i = 0; i < walls[floor].Count; i++)
				for (j = 0; j < walls[floor][i].roomIndices.Count; j++)
					recRooms[floor][walls[floor][i].roomIndices[j]].wallIndices.Add(i);

			for (i = 0; i < recRooms[floor].Count; i++)
				Distinct(recRooms[floor][i].wallIndices);
		}

		public bool OnBoundary(int indF, Vector2d v1, Vector2d v2)
		{
			bool onb = false;
			for (int i = 0; i < bounds[indF].Count; i ++)
			{
				if (Point_On_Line(bounds[indF][i], bounds[indF][(i + 1)%bounds[indF].Count], v1) &&
					Point_On_Line(bounds[indF][i], bounds[indF][(i + 1) % bounds[indF].Count], v2))
				{
					onb = true;
					break;
				}
			}
			return (onb);
		}

		public bool OnBoundaryInd(int indF, int ind, Vector2d v1, Vector2d v2)
		{
			bool onb = false;
			int i = ind, j = (ind + 1) % bounds[indF].Count;

			if (Point_On_Line(bounds[indF][i], bounds[indF][(j) % bounds[indF].Count], v1) &&
				Point_On_Line(bounds[indF][i], bounds[indF][(j) % bounds[indF].Count], v2))
				onb = true;
			return (onb);
		}

		public void UpdateVertex(int floor, int r)
		{
			double len = recRooms[floor][r].length / 2,
				wid = recRooms[floor][r].width / 2;
			recRooms[floor][r].vertex = new List<Vector2d>();
			recRooms[floor][r].vertex.Add(new Vector2d());
			recRooms[floor][r].vertex[0] = new Vector2d(recRooms[floor][r].center.x + wid, recRooms[floor][r].center.y - len);
			recRooms[floor][r].vertex.Add(new Vector2d());
			recRooms[floor][r].vertex[1] = new Vector2d(recRooms[floor][r].center.x + wid, recRooms[floor][r].center.y + len);
			recRooms[floor][r].vertex.Add(new Vector2d());
			recRooms[floor][r].vertex[2] = new Vector2d(recRooms[floor][r].center.x - wid, recRooms[floor][r].center.y + len);
			recRooms[floor][r].vertex.Add(new Vector2d());
			recRooms[floor][r].vertex[3] = new Vector2d(recRooms[floor][r].center.x - wid, recRooms[floor][r].center.y - len);
		}

		//////////////////////////////////Build Room---LH//////////////////////////////////////////////
        List<List<Wall>> walls;//,tWalls; // For initialize multiple floors of walls
        List<List<Cuboid>> cuboids;//, tCuboids;
        List<List<Door>> doors;//, tDoors;
        // For computer generated layout
        public int numFloor;
        int activeFloor = -1, drawFloor = -1, currFloor = -1;
		public double thresh = 1e-6, lastcost;
		public double Area;
        List<double> oriArea = new List<double>();
		public double[] blocks = new double[3];
		public double slabHeight;
		public bool slabdefined = false;

		public void UpdateRelatedWall(int ind, int p, Vector3d direction, double newv)
		{
			// ind: index of wall
			// p: 0/1 -- which endpoint
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			int xy = -1;
			Vector3d translate = new Vector3d(direction);
			if (Math.Abs(direction.y) < thresh) // y = 0
				xy = 0;
			else if (Math.Abs(direction.x) < thresh) // x = 0
				xy = 1;
			if (xy == -1) return;
			if (xy == 0)
			{
				translate *= newv - walls[activeFloor][ind].endPoints[p].Coord.x;
				walls[activeFloor][ind].endPoints[p].Coord.x = newv;
			}
			else
			{
				translate *= newv - walls[activeFloor][ind].endPoints[p].Coord.y;
				walls[activeFloor][ind].endPoints[p].Coord.y = newv;
			}
			for (int i = 0; i < walls[activeFloor][ind].cuboid.planeVertices.Count; i++)
				walls[activeFloor][ind].cuboid.planeVertices[i].Coord += translate;

			walls[activeFloor][ind].cuboid.UpdateUpperVertices();
			walls[activeFloor][ind].cuboid.UpdateCurrVertices(mat);
			walls[activeFloor][ind].cuboid.UpdatePlane();
			if ((walls[activeFloor][ind].endPoints[1].Coord - walls[activeFloor][ind].endPoints[0].Coord).Dot(walls[activeFloor][ind].direction) < thresh)
			{
				// a wall is reversed
				if (deleteVertex.IndexOf(ind) == -1)
				{
					relatedVerts.Add(walls[activeFloor][ind].endPoints[1-p]); // The other endpoint may form a new wall with one of endpoints
					deleteVertex.Add(ind); // To be deleted when mouse up in case it moved back again
				}
			}
			else if (deleteVertex.IndexOf(ind) != -1)
			{
				relatedVerts.RemoveAt(deleteVertex.IndexOf(ind));
				deleteVertex.Remove(ind);
			}
		}	

		public int FindVertexIndex(List<Vector3d> vertices, Vector3d v)
		{
			// Find out the index of current vertex as a variable for optimization
			int index = -1;
			for (int i = 0; i < vertices.Count; i++)
			{
				if ((v - vertices[i]).Length() < thresh)
				{
					index = i;
					break;
				}
			}
			return (index);
		}

		public void BuildWallWithoutOverlap()
		{
			int i, f;
			for (f = 0; f < numFloor; f++)
			{
				cuboids[f] = new List<Cuboid>();
				newed = new int[walls[f].Count * 2];
				for (i = 0; i < walls[f].Count; i++)
					NewVertices(f, i);
			}
		}

		List<Pair<Vector3d, int>> jointVertices;
		public void DeleteOpenWalls()
		{
			int i, j, k, f;
			List<Cuboid> cudoors = new List<Cuboid>();
			jointVertices = new List<Pair<Vector3d, int>>();
			// Delete open walls to facilitate casting other walls
			for (f = 0; f < numFloor; f++)
			{
				for (i = 0; i < walls[f].Count; i++)
				{
					if (!walls[f][i].open) continue;
					jointVertices.Add(new Pair<Vector3d, int>(new Vector3d(walls[f][i].endPoints[0].Coord), f));
					jointVertices.Add(new Pair<Vector3d, int>(new Vector3d(walls[f][i].endPoints[1].Coord), f));
					for (j = 0; j < recRooms[f].Count; j++)
					{
						recRooms[f][j].wallIndices.Remove(i);
						for (k = 0; k < recRooms[f][j].wallIndices.Count; k++)
							if (recRooms[f][j].wallIndices[k] > i)
								recRooms[f][j].wallIndices[k]--;
					}
					walls[f].RemoveAt(i);
					i--;
				}
				for (i = walls[f].Count; i < cuboids[f].Count; i++)
					cudoors.Add(cuboids[f][i]);
				cuboids[f] = new List<Cuboid>();
				newed = new int[walls[f].Count * 2];
				for (i = 0; i < walls[f].Count; i++)
					NewVertices(f, i);
				for (i = 0; i < cudoors.Count; i++)
					cuboids[f].Add(cudoors[i]);

				if (sideWallPlanes == null || sideWallPlanes.Count < f + 1)
				{
					sideWallPlanes.Add(new List<Plane>());
					AddSideWallPlanes(f);
				}
			}
		}

		public void AddSideWallPlanes(int f)
		{
			sideWallPlanes[f] = new List<Plane>();
			for (int i = 0; i < walls[f].Count; i++)
			{
				List<Vertex> vertices;
				if (walls[f][i].vertexType[0] == 3)
				{
					vertices = new List<Vertex>();
					vertices.Add(new Vertex(walls[f][i].cuboid.planeVertices[0]));
					vertices.Add(new Vertex(walls[f][i].cuboid.planeVertices[1]));
					vertices.Add(new Vertex(walls[f][i].cuboid.planeVertices[1].Coord + new Vector3d(0, 0, 1) * walls[f][i].cuboid.height));
					vertices.Add(new Vertex(walls[f][i].cuboid.planeVertices[0].Coord + new Vector3d(0, 0, 1) * walls[f][i].cuboid.height));
					sideWallPlanes[f].Add(new Plane(vertices, vertices));
					sideWallPlanes[f][sideWallPlanes[f].Count - 1].wallInd = i;
				}
				else if (walls[f][i].vertexType[1] == 3)
				{
					vertices = new List<Vertex>();
					int cnt = walls[f][i].cuboid.planeVertices.Count;
					vertices.Add(new Vertex(walls[f][i].cuboid.planeVertices[cnt - 2]));
					vertices.Add(new Vertex(walls[f][i].cuboid.planeVertices[cnt - 1]));
					vertices.Add(new Vertex(walls[f][i].cuboid.planeVertices[cnt - 1].Coord + new Vector3d(0, 0, 1) * walls[f][i].cuboid.height));
					vertices.Add(new Vertex(walls[f][i].cuboid.planeVertices[cnt - 2].Coord + new Vector3d(0, 0, 1) * walls[f][i].cuboid.height));
					sideWallPlanes[f].Add(new Plane(vertices, vertices));
					sideWallPlanes[f][sideWallPlanes[f].Count - 1].wallInd = i;
				}
			}

		}
		int[] vertexType = new int[2];
		int[] newed; // 0: haven't been renewed; 1: renewed; 3: not renewed, but 3 vertices
		// Decide the vertices of the polygon of the wall
		public void NewVertices(int floor, int i)
		{
			// After build/rebuild, reconstruct the cuboids[floor] 
			// vertextype == 1: 2 polygonal vertices at the endpoint
			Matrix4d mat = this.currTransformation == null ? Matrix4d.IdentityMatrix() : this.currTransformation;
			Vertex p0, p1;
			double dist = wallThickness / 2;
			int[] ind0, ind1;
			List<Vertex> verts;
			p0 = walls[floor][i].endPoints[0];
			p1 = walls[floor][i].endPoints[1];
			ind0 = SearchNeighors(floor, p0, walls[floor][i].type, walls[floor][i].normal, p1);
			ind1 = SearchNeighors(floor, p1, walls[floor][i].type, walls[floor][i].normal, p0);
			// Construct a new wall
			verts = RenewWall(floor, walls[floor][i].type, ind0, p0, ind1, p1, dist, i);
			newed[i * 2] = 1;
			newed[i * 2 + 1] = 1;
			walls[floor][i].cuboid = new Cuboid(1, i, p0, p1, verts, wallThickness, wallHeight, wallElevation,
			walls[floor][i].direction, walls[floor][i].normal, mat);
			walls[floor][i].cuboid.wallIndex = i;
			cuboids[floor].Add(walls[floor][i].cuboid);
			walls[floor][i].cuboid.UpdateCurrVertices(mat);
		}

		// For NewVertices
		public int[] SearchNeighors(int floor, Vertex v, int type, Vector3d normal, Vertex v0)
		{
			// Find out whether the vertex v is already used in some walls[floor]
			int[] indices = new int[9];
			// indices[0..2] == 
			// 0: vertical to current wall, but outside of current room
			// 1: vertical to current wall, but inside of current room
			// 2: parallel to current wall (co-linear), outside of current room
			// indices[3..5] represents which walls[floor]
			// indices[6..8] represents which endpoint
			int i;
			for (i = 0; i < walls[floor].Count; i++)
			{
				if ((walls[floor][i].endPoints[0].Coord - walls[floor][i].endPoints[1].Coord).Length() < thresh)
					continue;
				if (((walls[floor][i].endPoints[0].Coord - v.Coord).Length() < thresh
					&& (walls[floor][i].endPoints[1].Coord - v0.Coord).Length() < thresh) ||
					((walls[floor][i].endPoints[0].Coord - v0.Coord).Length() < thresh
					&& (walls[floor][i].endPoints[1].Coord - v.Coord).Length() < thresh))
					continue;
				if (walls[floor][i].direction.Cross(normal).Length() < thresh) // perpendicular
				{
					if ((walls[floor][i].endPoints[0].Coord - v.Coord).Length() < thresh && (
					 (type == 0 && walls[floor][i].endPoints[1].Coord.x > v.Coord.x) ||
					 (type == 2 && walls[floor][i].endPoints[1].Coord.x < v.Coord.x) ||
					 (type == 1 && walls[floor][i].endPoints[1].Coord.y < v.Coord.y) ||
					 (type == 3 && walls[floor][i].endPoints[1].Coord.y > v.Coord.y)) )
					{
						indices[0] = 1;
						indices[3] = i;
						indices[6] = 0;
						continue;
					}
					else if ( (walls[floor][i].endPoints[1].Coord - v.Coord).Length() < thresh && (
					 (type == 0 && walls[floor][i].endPoints[0].Coord.x > v.Coord.x) ||
					 (type == 2 && walls[floor][i].endPoints[0].Coord.x < v.Coord.x) ||
					 (type == 1 && walls[floor][i].endPoints[0].Coord.y < v.Coord.y) ||
					 (type == 3 && walls[floor][i].endPoints[0].Coord.y > v.Coord.y)))
					{
						indices[0] = 1;
						indices[3] = i;
						indices[6] = 1;
						continue;
					}
					else if ((walls[floor][i].endPoints[0].Coord - v.Coord).Length() < thresh && (
					 (type == 0 && walls[floor][i].endPoints[1].Coord.x < v.Coord.x) ||
					 (type == 2 && walls[floor][i].endPoints[1].Coord.x > v.Coord.x) ||
					 (type == 1 && walls[floor][i].endPoints[1].Coord.y > v.Coord.y) ||
					 (type == 3 && walls[floor][i].endPoints[1].Coord.y < v.Coord.y)) )
					{
						indices[1] = 1;
						indices[4] = i;
						indices[7] = 0;
						continue;
					}
					else if ((walls[floor][i].endPoints[1].Coord - v.Coord).Length() < thresh && (
					 (type == 0 && walls[floor][i].endPoints[0].Coord.x < v.Coord.x) ||
					 (type == 2 && walls[floor][i].endPoints[0].Coord.x > v.Coord.x) ||
					 (type == 1 && walls[floor][i].endPoints[0].Coord.y > v.Coord.y) ||
					 (type == 3 && walls[floor][i].endPoints[0].Coord.y < v.Coord.y)))
					{
						indices[1] = 1;
						indices[4] = i;
						indices[7] = 1;
						continue;
					}
				}
				// co-linear but different side of the intersection point
				else if (((walls[floor][i].endPoints[0].Coord - v.Coord).Length() < thresh &&
					!Point_In_Line(v.Coord, v0.Coord, walls[floor][i].endPoints[1].Coord)))
				{
					indices[2] = 1;
					indices[5] = i;
					indices[8] = 0;
					continue;
				}
				else if (((walls[floor][i].endPoints[1].Coord - v.Coord).Length() < thresh &&
					!Point_In_Line(v.Coord, v0.Coord, walls[floor][i].endPoints[0].Coord)))
				{
					indices[2] = 1;
					indices[5] = i;
					indices[8] = 1;
					continue;
				}
			}
			return (indices);
		}

		public List<int> SearchFormedWallVertices(Vertex v, int type, Vertex v0, int e)
		{
			// Find the vertices
			List<int> indices = new List<int>();
			if (walls[currFloor] == null || walls[currFloor].Count == 0) return (indices);

			for (int i = 0; i < walls[currFloor].Count; i++)
			{
				//if (indices.Count > 0)
				//{
					if (Point_In_Line(v.Coord, v0.Coord, walls[currFloor][i].endPoints[0].Coord) &&
						(Point_In_Line(v.Coord, v0.Coord, walls[currFloor][i].endPoints[1].Coord)))
					{
						indices.Add(i);
						continue;
					}
				//}
				if (((walls[currFloor][i].endPoints[0].Coord - v.Coord).Length() < thresh
					&& (walls[currFloor][i].endPoints[1].Coord - v0.Coord).Length() < thresh) ||
					((walls[currFloor][i].endPoints[0].Coord - v0.Coord).Length() < thresh
					&& (walls[currFloor][i].endPoints[1].Coord - v.Coord).Length() < thresh))
				{
					indices.Add(i);
					if (e == 0) formed0 = true;
					else formed1 = true;
					continue;
				}
				if (((walls[currFloor][i].endPoints[0].Coord - v.Coord).Length() < thresh &&
					 (Point_In_Line(v.Coord, v0.Coord, walls[currFloor][i].endPoints[1].Coord))) ||
					  ((walls[currFloor][i].endPoints[1].Coord - v.Coord).Length() < thresh &&
					 (Point_In_Line(v.Coord, v0.Coord, walls[currFloor][i].endPoints[0].Coord))))
				{
					indices.Add(i);
					if (e == 0) formed0 = true;
					else formed1 = true;
				}
			}
			return (indices);
		}

		public List<Vertex> RenewWall(int floor, int type, int[] ind0, Vertex p0, int[] ind1, Vertex p1, double dist, int nw)
		{
			List<Vertex> verts = new List<Vertex>();
			int numN0 = ind0[0] + ind0[1] + ind0[2],
				numN1 = ind1[0] + ind1[1] + ind1[2];
			int w1, w2, e1, e2, count;
			// Consider vertices at endpoint 0
			if (newed[nw * 2] == 1) // if the endpoint has already renewed
			{
				verts.Add(walls[floor][nw].cuboid.planeVertices[0]);
				verts.Add(walls[floor][nw].cuboid.planeVertices[1]);
				if (walls[floor][nw].vertexType[0] == 2)
					verts.Add(walls[floor][nw].cuboid.planeVertices[2]);
			}
			else
			{
				// One neighbor
				if (numN0 == 1 && newed[nw * 2] != 3)
				{
					walls[floor][nw].vertexType[0] = 1;
					if (ind0[0] == 1)
					{
						verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
						verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
						w1 = ind0[3];
						e1 = ind0[6];
					}
					else if (ind0[1] == 1)
					{
						verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
						verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
						w1 = ind0[4];
						e1 = ind0[7];
					}
					else
					{
						verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal));
						verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal));
						w1 = ind0[5];
						e1 = ind0[8];
					}
					// Form vertices for the neighbor
					if (e1 == 0)
					{
						newed[w1 * 2] = 1;
						if (walls[floor][w1].vertexType[0] == 2)
							walls[floor][w1].cuboid.planeVertices.RemoveAt(0);
						if (ind0[0] == 1)
						{
							//if (walls[floor][w1].cuboid.planeVertices.Count == 0)
							//{
								walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(verts[0]));
								walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(verts[1]));
							//}
							//else
							//{
							//    walls[floor][w1].cuboid.planeVertices[0] = new Vertex(verts[1]);
							//    walls[floor][w1].cuboid.planeVertices[1] = new Vertex(verts[0]);
							//}
						}
						else
						{
							//if (walls[floor][w1].cuboid.planeVertices.Count == 0)
							//{
								walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(verts[0]));
								walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(verts[1]));
							//}
							//else
							//{
							//    walls[floor][w1].cuboid.planeVertices[0] = new Vertex(verts[1]);
							//    walls[floor][w1].cuboid.planeVertices[1] = new Vertex(verts[0]);
							//}
						}
						walls[floor][w1].vertexType[0] = 1;
					}
					else
					{
						newed[w1 * 2 + 1] = 1;
						count = walls[floor][w1].cuboid.planeVertices.Count;
						if (walls[floor][w1].vertexType[1] == 2)
						{
							walls[floor][w1].cuboid.planeVertices.RemoveAt(count - 1);
							count--;
						}
						if (walls[floor][w1].cuboid.planeVertices.Count < 4)
						{
							walls[floor][w1].cuboid.planeVertices.Add(new Vertex(verts[1]));
							walls[floor][w1].cuboid.planeVertices.Add(new Vertex(verts[0]));
						}
						else
						{
							walls[floor][w1].cuboid.planeVertices[count - 2] = new Vertex(verts[1]);
							walls[floor][w1].cuboid.planeVertices[count - 1] = new Vertex(verts[0]);
						}
						walls[floor][w1].vertexType[1] = 1;
					}
				}
				// Two neighbors	
				else if (numN0 == 2 && newed[nw * 2] != 3)
				{
					if (ind0[0] == 1 && ind0[1] == 1)
					{
						walls[floor][nw].vertexType[0] = 2;
						verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
						verts.Add(new Vertex(p0));
						verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
						w1 = ind0[3]; w2 = ind0[4];
						e1 = ind0[6]; e2 = ind0[7];
						newed[w1 * 2 + e1] = 1;
						newed[w2 * 2 + e2] = 3;
						if (e1 == 0)
						{
							if (walls[floor][w1].vertexType[0] == 2)
								walls[floor][w1].cuboid.planeVertices.RemoveAt(0);
							//if (walls[floor][w1].cuboid.planeVertices.Count == 0)
							//{
								walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(p0.Coord + dist * walls[floor][w1].normal + dist * walls[floor][w1].direction));
								walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(p0.Coord - dist * walls[floor][w1].normal - dist * walls[floor][w1].direction));
							//}
							//else
							//{
							//    walls[floor][w1].cuboid.planeVertices[0] = new Vertex(p0.Coord - dist * walls[floor][w1].normal - dist * walls[floor][w1].direction);
							//    walls[floor][w1].cuboid.planeVertices[1] = new Vertex(p0.Coord + dist * walls[floor][w1].normal + dist * walls[floor][w1].direction);
							//}
							walls[floor][w1].vertexType[0] = 1;
						}
						else
						{
							count = walls[floor][w1].cuboid.planeVertices.Count;
							if (walls[floor][w1].vertexType[1] == 2)
							{
								walls[floor][w1].cuboid.planeVertices.RemoveAt(count - 1);
								count--;
							}
							if (walls[floor][w1].cuboid.planeVertices.Count < 4)
							{
								walls[floor][w1].cuboid.planeVertices.Add(new Vertex(p0.Coord + dist * walls[floor][w1].normal + dist * walls[floor][w1].direction));
								walls[floor][w1].cuboid.planeVertices.Add(new Vertex(p0.Coord - dist * walls[floor][w1].normal - dist * walls[floor][w1].direction));
							}
							else
							{
								walls[floor][w1].cuboid.planeVertices[count - 2] = new Vertex(p0.Coord + dist * walls[floor][w1].normal + dist * walls[floor][w1].direction);
								walls[floor][w1].cuboid.planeVertices[count - 1] = new Vertex(p0.Coord - dist * walls[floor][w1].normal - dist * walls[floor][w1].direction);
							}
							walls[floor][w1].vertexType[1] = 1;
						}
					}
					else
					{
						walls[floor][nw].vertexType[0] = 1;
						if (ind0[0] == 1)
						{
							verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
							verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
							w1 = ind0[3]; w2 = ind0[5];
							e1 = ind0[6]; e2 = ind0[8];
						}
						else
						{
							verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
							verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
							w1 = ind0[4]; w2 = ind0[5];
							e1 = ind0[7]; e2 = ind0[8];
						}
						newed[w1 * 2 + e1] = 3;
						newed[w2 * 2 + e2] = 3;
					}
				}
				// Three neighbors
				else if (numN0 == 3 || newed[nw * 2] == 3)	// if 4 walls[floor] are intersected, the vertices comform to the same rule	
				{
					walls[floor][nw].vertexType[0] = 2;
					verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
					verts.Add(new Vertex(p0));
					verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
				}
				else if (numN0 == 0) // no neighbor
				{
					walls[floor][nw].vertexType[0] = 3;
					verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
					verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
				}
			}
			// Consider vertices at endpoint 1
			if (newed[nw * 2 + 1] == 1)
			{
				count = walls[floor][nw].cuboid.planeVertices.Count;
				if (walls[floor][nw].vertexType[1] == 2)
					verts.Add(walls[floor][nw].cuboid.planeVertices[count - 3]);
				verts.Add(walls[floor][nw].cuboid.planeVertices[count - 2]);
				verts.Add(walls[floor][nw].cuboid.planeVertices[count - 1]);
			}
			else
			{
				// One neighbor
				if (numN1 == 1 && newed[nw * 2 + 1] != 3)			
				{
					walls[floor][nw].vertexType[1] = 1;
					if (ind1[0] == 1)
					{
						verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
						verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
						w1 = ind1[3];
						e1 = ind1[6];
					}
					else if (ind1[1] == 1)
					{
						verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
						verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
						w1 = ind1[4];
						e1 = ind1[7];
					}
					else
					{
						verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal));
						verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal));
						w1 = ind1[5];
						e1 = ind1[8];
					}
					// Form vertices for the neighbor
					if (e1 == 0)
					{
						newed[w1 * 2] = 1;
						if (walls[floor][w1].vertexType[0] == 2)
							walls[floor][w1].cuboid.planeVertices.RemoveAt(0);
						if (walls[floor][w1].cuboid.planeVertices.Count == 0)
						{
							walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(verts[verts.Count - 2]));
							walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(verts[verts.Count - 1]));
						}
						else
						{
							walls[floor][w1].cuboid.planeVertices[0] = new Vertex(verts[verts.Count - 1]);
							walls[floor][w1].cuboid.planeVertices[1] = new Vertex(verts[verts.Count - 2]);
						}
						walls[floor][w1].vertexType[0] = 1;
					}
					else
					{
						newed[w1 * 2 + 1] = 1;
						count = walls[floor][w1].cuboid.planeVertices.Count;
						if (walls[floor][w1].vertexType[1] == 2)
						{
							walls[floor][w1].cuboid.planeVertices.RemoveAt(count - 1);
							count--;
						}
						if (walls[floor][w1].cuboid.planeVertices.Count < 4)
						{
							walls[floor][w1].cuboid.planeVertices.Add(new Vertex(verts[verts.Count - 1]));
							walls[floor][w1].cuboid.planeVertices.Add(new Vertex(verts[verts.Count - 2]));
						}
						else
						{
							walls[floor][w1].cuboid.planeVertices[count - 2] = new Vertex(verts[verts.Count - 1]);
							walls[floor][w1].cuboid.planeVertices[count - 1] = new Vertex(verts[verts.Count - 2]);
						}
						walls[floor][w1].vertexType[1] = 1;
					}
				}
				// Two neighbors	
				else if (numN1 == 2 && newed[nw * 2 + 1] != 3)			
				{
					if (ind1[0] == 1 && ind1[1] == 1)
					{
						walls[floor][nw].vertexType[1] = 2;
						verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
						verts.Add(new Vertex(p1));
						verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
						w1 = ind1[3]; w2 = ind1[4];
						e1 = ind1[6]; e2 = ind1[7];
						newed[w1 * 2 + e1] = 1;
						newed[w2 * 2 + e2] = 3;
						if (e1 == 0)
						{
							if (walls[floor][w1].vertexType[0] == 2)
								walls[floor][w1].cuboid.planeVertices.RemoveAt(0);
							if (walls[floor][w1].cuboid.planeVertices.Count == 0)
							{
								walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(p1.Coord + dist * walls[floor][w1].normal - dist * walls[floor][w1].direction));
								walls[floor][w1].cuboid.planeVertices.Insert(0, new Vertex(p1.Coord - dist * walls[floor][w1].normal + dist * walls[floor][w1].direction));
							}
							else
							{
								walls[floor][w1].cuboid.planeVertices[0] = new Vertex(p1.Coord - dist * walls[floor][w1].normal + dist * walls[floor][w1].direction);
								walls[floor][w1].cuboid.planeVertices[1] = new Vertex(p1.Coord + dist * walls[floor][w1].normal - dist * walls[floor][w1].direction);
							}
							walls[floor][w1].vertexType[0] = 1;
						}
						else
						{
							count = walls[floor][w1].cuboid.planeVertices.Count;
							if (walls[floor][w1].vertexType[1] == 2)
							{
								walls[floor][w1].cuboid.planeVertices.RemoveAt(count - 1);
								count--;
							}
							if (walls[floor][w1].cuboid.planeVertices.Count < 4)
							{
								walls[floor][w1].cuboid.planeVertices.Add(new Vertex(p1.Coord + dist * walls[floor][w1].normal - dist * walls[floor][w1].direction));
								walls[floor][w1].cuboid.planeVertices.Add(new Vertex(p1.Coord - dist * walls[floor][w1].normal + dist * walls[floor][w1].direction));
							}
							else
							{
								walls[floor][w1].cuboid.planeVertices[count - 2] = new Vertex(p1.Coord + dist * walls[floor][w1].normal - dist * walls[floor][w1].direction);
								walls[floor][w1].cuboid.planeVertices[count - 1] = new Vertex(p1.Coord - dist * walls[floor][w1].normal + dist * walls[floor][w1].direction);
							}
							walls[floor][w1].vertexType[1] = 1;
						}
					}
					else
					{
						walls[floor][nw].vertexType[1] = 1;
						if (ind1[0] == 1)
						{
							verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
							verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
							w1 = ind1[3]; w2 = ind1[5];
							e1 = ind1[6]; e2 = ind1[8];
						}
						else
						{
							verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
							verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
							w1 = ind1[4]; w2 = ind1[5];
							e1 = ind1[7]; e2 = ind1[8];
						}
						newed[w1 * 2 + e1] = 3;
						newed[w2 * 2 + e2] = 3;
					}
				}
				// Three neighbors
				else if (numN1 == 3 || newed[nw * 2 + 1] == 3)		
				{
					walls[floor][nw].vertexType[1] = 2;
					verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
					verts.Add(new Vertex(p1));
					verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
				}
				else if (numN1 == 0)
				{
					walls[floor][nw].vertexType[1] = 3;
					verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
					verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
				}
			}

			return (verts);
		}

		public List<Vertex> NewWall(int floor, Vertex p0, Vertex p1, double dist, int nw)
		{
			List<Vertex> verts = new List<Vertex>();

			verts.Add(new Vertex(p0.Coord - dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
			verts.Add(new Vertex(p0.Coord + dist * walls[floor][nw].normal - dist * walls[floor][nw].direction));
			vertexType[0] = 1;

			verts.Add(new Vertex(p1.Coord + dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
			verts.Add(new Vertex(p1.Coord - dist * walls[floor][nw].normal + dist * walls[floor][nw].direction));
			vertexType[1] = 1;

			return (verts);
		}

        ////////// Basic Functions/////////

		public void Export_building(String layoutName)
		{
			// Export the layout as an .obj file
			StreamWriter swlt;
			int i, j, n, f, k, p, q, fd, split;
			int numv = 0;
			int[] nv = new int[4], snv = new int[4], dwind = new int[4]; // for side wall 
			List<int> indf = new List<int>(), sindf, invindf; // At most 6 vertices on a face
			List<List<int>> faces = new List<List<int>>();
			List<Vector3d> vertices = new List<Vector3d>();
			Vector2d v1, v2, v3, v4;
			String floorname;

			for (f = 0; f < numFloor; f++)
			{
				if (numFloor > 1)
				{
					int index = layoutName.LastIndexOf('.');
					floorname = layoutName.Substring(0, index) + "_f" + f.ToString() + layoutName.Substring(index, layoutName.Length - index);
					swlt = new StreamWriter(floorname);
					faces = new List<List<int>>();
					vertices = new List<Vector3d>();
					numv = 0;
				}
				else
					swlt = new StreamWriter(layoutName);
				fd = -1; split = -1;
				// vertex index
				for (i = 0; i < walls[f].Count; i++)
				{
					n = walls[f][i].cuboid.planeVertices.Count;
					// The face on the ground
					nv[0] = numv + 1; // for inside wall
					indf = new List<int>();
					for (j = 0; j < n; j++)
					{
						vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[j].Coord));
						numv++; // Total number of vertices
						indf.Add(numv); // Index from 1
					}
					faces.Add(indf);
					if (walls[f][i].doors.Count > 0 && walls[f][i].doors[0].cuboid.elevation < thresh)
					{
						dwind = new int[n];
						fd = faces.Count - 1;
						k = 0;
						for (j = n - 1; j >= 0; j--)
							dwind[k++] = numv - j;
					}
					nv[1] = nv[0] + n;
					nv[2] = numv + n;
					nv[3] = numv;

					// The offset face of the ground
					if (walls[f][i].cuboid.vertices.Count != 2 * n)
					{
						Console.Write("Vertex number error!");
						break;
					}
					indf = new List<int>();
					for (j = n; j < walls[f][i].cuboid.vertices.Count; j++)
					{
						vertices.Add(new Vector3d(walls[f][i].cuboid.vertices[j].Coord));
						numv++;
						indf.Add(numv);
					}
					//faces.Add(indf);
					invindf = InverseOrder(indf);
					faces.Add(invindf);

					// Find the vertices for faces vertical to the ground
					int lb1 = -1, lb2 = -1;
					indf = new List<int>();
					if (n == 4)
					{
						for (j = 0; j < n; j++)
							indf.Add(nv[j]);
						lb1 = 0;
						lb2 = 1;
						split = 2;
					}
					else if (n == 6)
					{
						split = 3; // for ground wall face with door
						if (walls[f][i].doors.Count == 0)
						{
							v3 = new Vector2d(walls[f][i].endPoints[0].Coord.x, walls[f][i].endPoints[0].Coord.y);
							v4 = new Vector2d(walls[f][i].endPoints[1].Coord.x, walls[f][i].endPoints[1].Coord.y);
							p = -1; q = -1;
							if (Math.Abs(walls[f][i].cuboid.endPoints[0].Coord.x -
								walls[f][i].cuboid.endPoints[1].Coord.x) < thresh)
							{
								for (j = 0; j < n - 1; j++)
								{
									v1 = (new Vector2d(walls[f][i].cuboid.vertices[j].Coord.x,
										walls[f][i].cuboid.vertices[j].Coord.y));
									for (k = j + 1; k < n; k++)
									{
										v2 = (new Vector2d(walls[f][i].cuboid.vertices[k].Coord.x,
										walls[f][i].cuboid.vertices[k].Coord.y));
										if (Math.Abs((v3 - v4).Cross((v1 - v2))) < thresh &&
											Math.Abs(v1.x - v3.x) > thresh)
										{
											p = j; q = k;
											break;
										}
									}
									if (p != -1 && q != -1)
									{
										indf.Add(numv - 2 * n + p + 1);
										indf.Add(numv - n + p + 1);
										indf.Add(numv - n + q + 1);
										indf.Add(numv - 2 * n + q + 1);
										p = -1; q = -1;
										continue;
									}
								}
							}
							else
							{
								for (j = 0; j < n - 1; j++)
								{
									v1 = (new Vector2d(walls[f][i].cuboid.vertices[j].Coord.x,
										walls[f][i].cuboid.vertices[j].Coord.y));
									for (k = j + 1; k < n; k++)
									{
										v2 = (new Vector2d(walls[f][i].cuboid.vertices[k].Coord.x,
										walls[f][i].cuboid.vertices[k].Coord.y));
										if (Math.Abs((v3 - v4).Cross((v1 - v2))) < thresh &&
											Math.Abs(v1.y - v3.y) > thresh)
										{
											p = j; q = k;
											break;
										}
									}
									if (p != -1 && q != -1)
									{
										indf.Add(numv - 2 * n + p + 1);
										indf.Add(numv - n + p + 1);
										indf.Add(numv - n + q + 1);
										indf.Add(numv - 2 * n + q + 1);
										p = -1; q = -1;
										continue;
									}
								}
							}
						}
						else // n=6 && has door
						{
							lb1 = 0; p = 0; q = 5;
							indf.Add(numv - 2 * n + p + 1);
							indf.Add(numv - n + p + 1);
							indf.Add(numv - n + q + 1);
							indf.Add(numv - 2 * n + q + 1);

							lb2 = 2; p = 2; q = 3;
							indf.Add(numv - 2 * n + p + 1);
							indf.Add(numv - n + p + 1);
							indf.Add(numv - n + q + 1);
							indf.Add(numv - 2 * n + q + 1);
						}
					}
					else
					{
						v3 = new Vector2d(walls[f][i].endPoints[0].Coord.x, walls[f][i].endPoints[0].Coord.y);
						v4 = new Vector2d(walls[f][i].endPoints[1].Coord.x, walls[f][i].endPoints[1].Coord.y);
						p = -1; q = -1;
						for (j = 0; j < n - 1; j++)
						{
							v1 = (new Vector2d(walls[f][i].cuboid.vertices[j].Coord.x,
								walls[f][i].cuboid.vertices[j].Coord.y));
							for (k = j + 1; k < n; k++)
							{
								v2 = (new Vector2d(walls[f][i].cuboid.vertices[k].Coord.x,
								walls[f][i].cuboid.vertices[k].Coord.y));
								if (Math.Abs((v3 - v4).Cross((v1 - v2))) < thresh)
								{
									p = j; q = k;
									break;
								}
							}
							if (p != -1 && q != -1)
							{
								indf.Add(numv - 2 * n + p + 1);
								indf.Add(numv - n + p + 1);
								indf.Add(numv - n + q + 1);
								indf.Add(numv - 2 * n + q + 1);
								if (lb1 != -1)
								{
									lb2 = p;
									split = q;
								}
								else
									lb1 = p;
								p = -1; q = -1;
								continue;
							}
						}
					}
					// if there is a door, there are 8 vertices: inverse of 凹
					// if a window, split to 4 faces
					int[] doorind = new int[8];
					if (walls[f][i].doors != null && walls[f][i].doors.Count > 0)
					{
						if (walls[f][i].doors[0].cuboid.elevation > thresh) // window
						{
							sindf = new List<int>();
							sindf.Add(indf[0]);
							sindf.Add(indf[1]);
							if (Math.Abs(walls[f][i].cuboid.endPoints[0].Coord.x -
								walls[f][i].cuboid.endPoints[1].Coord.x) < thresh)
							{
								vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[lb1].Coord.x,
									walls[f][i].doors[0].cuboid.vertices[0].Coord.y,
									walls[f][i].cuboid.height + walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[0] = numv;
								vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[lb1].Coord.x,
									walls[f][i].doors[0].cuboid.vertices[0].Coord.y,
									walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[1] = numv;
								//faces.Add(sindf);
								invindf = InverseOrder(sindf);
								faces.Add(invindf);

								sindf = new List<int>();
								vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[lb1].Coord.x,
									walls[f][i].doors[0].cuboid.vertices[3].Coord.y,
									walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[2] = numv;
								vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[lb1].Coord.x,
									walls[f][i].doors[0].cuboid.vertices[3].Coord.y,
									walls[f][i].cuboid.height + walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[3] = numv;
								sindf.Add(indf[2]);
								sindf.Add(indf[3]);
								//faces.Add(sindf);
								invindf = InverseOrder(sindf);
								faces.Add(invindf);
							}
							else
							{
								vertices.Add(new Vector3d(walls[f][i].doors[0].cuboid.vertices[0].Coord.x,
									walls[f][i].cuboid.planeVertices[lb1].Coord.y,
									walls[f][i].cuboid.height + walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[0] = numv;
								vertices.Add(new Vector3d(walls[f][i].doors[0].cuboid.vertices[0].Coord.x,
									walls[f][i].cuboid.planeVertices[lb1].Coord.y,
									walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[1] = numv;
								//faces.Add(sindf);
								invindf = InverseOrder(sindf);
								faces.Add(invindf);

								sindf = new List<int>();
								vertices.Add(new Vector3d(walls[f][i].doors[0].cuboid.vertices[3].Coord.x,
									walls[f][i].cuboid.planeVertices[lb1].Coord.y,
									walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[2] = numv;
								vertices.Add(new Vector3d(walls[f][i].doors[0].cuboid.vertices[3].Coord.x,
									walls[f][i].cuboid.planeVertices[lb1].Coord.y,
									walls[f][i].cuboid.elevation + walls[f][i].cuboid.height));
								sindf.Add(++numv);
								snv[3] = numv;
								sindf.Add(indf[2]);
								sindf.Add(indf[3]);
								//faces.Add(sindf);
								invindf = InverseOrder(sindf);
								faces.Add(invindf);
							}
							sindf = new List<int>();
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[4].Coord);
							indf.Add(++numv);
							sindf.Add(numv);
							doorind[4] = numv;
							sindf.Add(snv[0]);
							sindf.Add(snv[3]);
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[7].Coord);
							indf.Add(++numv);
							doorind[7] = numv;
							sindf.Add(numv);
							//faces.Add(sindf);
							invindf = InverseOrder(sindf);
							faces.Add(invindf);

							sindf = new List<int>();
							sindf.Add(snv[1]);
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[0].Coord);
							indf.Add(++numv);
							sindf.Add(numv);
							doorind[0] = numv;
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[3].Coord);
							indf.Add(++numv);
							sindf.Add(numv);
							doorind[3] = numv;
							sindf.Add(snv[2]);
							//faces.Add(sindf);
							invindf = InverseOrder(sindf);
							faces.Add(invindf);
						}
						else // door
						{
							if (n == 4)
							{
								vertices.Add(walls[f][i].doors[0].cuboid.vertices[3].Coord);
								indf.Add(++numv);
								doorind[3] = numv;
								vertices.Add(walls[f][i].doors[0].cuboid.vertices[7].Coord);
								indf.Add(++numv);
								doorind[7] = numv;
								vertices.Add(walls[f][i].doors[0].cuboid.vertices[4].Coord);
								indf.Add(++numv);
								doorind[4] = numv;
								vertices.Add(walls[f][i].doors[0].cuboid.vertices[0].Coord);
								indf.Add(++numv);
								doorind[0] = numv;
								//faces.Add(indf);
								invindf = InverseOrder(indf);
								faces.Add(invindf);
							}
							else
							{
								sindf = new List<int>();
								for (j = 0; j < 4; j++)
									sindf.Add(indf[j]);
								vertices.Add(walls[f][i].doors[0].cuboid.vertices[3].Coord);
								sindf.Add(++numv);
								doorind[3] = numv;
								vertices.Add(walls[f][i].doors[0].cuboid.vertices[7].Coord);
								sindf.Add(++numv);
								doorind[7] = numv;
								vertices.Add(walls[f][i].doors[0].cuboid.vertices[4].Coord);
								sindf.Add(++numv);
								doorind[4] = numv;
								vertices.Add(walls[f][i].doors[0].cuboid.vertices[0].Coord);
								sindf.Add(++numv);
								doorind[0] = numv;
								//faces.Add(sindf);
								invindf = InverseOrder(sindf);
								faces.Add(invindf);
							}
						}
					}
					else
					{
						if (n != 4)
						{
							sindf = new List<int>();
							for (j = 0; j < 4; j++)
								sindf.Add(indf[j]);
							//faces.Add(sindf);
							invindf = InverseOrder(sindf);
							faces.Add(invindf);
						}
						else
						{
							//faces.Add(indf);
							invindf = InverseOrder(indf);
							faces.Add(invindf);
						}
					}

					// outside wall

					if (n == 4)
					{
						indf = new List<int>();
						// vertextype = 3: the wall without any neighbor
						if (walls[f][i].vertexType[0] == 1 || walls[f][i].vertexType[0] == 3)
							nv[0] += 1;
						else
							nv[0] += 2;
						if (walls[f][i].vertexType[1] == 1 || walls[f][i].vertexType[1] == 3)
							nv[3] -= 1;
						else
							nv[3] -= 2;
						nv[1] = nv[0] + n;
						nv[2] = nv[3] + n;
						for (j = 0; j < 4; j++)
							indf.Add(nv[j]);
					}
					else
					{
						for (j = 0; j < 4; j++)
							indf.RemoveAt(0);
					}
					if (walls[f][i].doors != null && walls[f][i].doors.Count > 0)
					{
						if (walls[f][i].doors[0].cuboid.elevation > thresh) // window
						{
							sindf = new List<int>();
							sindf.Add(indf[0]);
							sindf.Add(indf[1]);
							if (Math.Abs(walls[f][i].cuboid.endPoints[0].Coord.x -
								walls[f][i].cuboid.endPoints[1].Coord.x) < thresh)
							{
								vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[lb2].Coord.x,
									walls[f][i].doors[0].cuboid.vertices[1].Coord.y,
									walls[f][i].cuboid.height + walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[0] = numv;
								vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[lb2].Coord.x,
									walls[f][i].doors[0].cuboid.vertices[1].Coord.y,
									walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[1] = numv;
								faces.Add(sindf);

								sindf = new List<int>();
								vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[lb2].Coord.x,
									walls[f][i].doors[0].cuboid.vertices[2].Coord.y,
									walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[2] = numv;
								vertices.Add(new Vector3d(walls[f][i].cuboid.planeVertices[lb2].Coord.x,
									walls[f][i].doors[0].cuboid.vertices[2].Coord.y,
									walls[f][i].cuboid.height + walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[3] = numv;
								sindf.Add(indf[2]);
								sindf.Add(indf[3]);
								faces.Add(sindf);
							}
							else
							{
								vertices.Add(new Vector3d(walls[f][i].doors[0].cuboid.vertices[1].Coord.x,
									walls[f][i].cuboid.planeVertices[lb2].Coord.y,
									walls[f][i].cuboid.height + walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[0] = numv;
								vertices.Add(new Vector3d(walls[f][i].doors[0].cuboid.vertices[1].Coord.x,
									walls[f][i].cuboid.planeVertices[lb2].Coord.y,
									walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[1] = numv;
								faces.Add(sindf);

								sindf = new List<int>();
								vertices.Add(new Vector3d(walls[f][i].doors[0].cuboid.vertices[2].Coord.x,
									walls[f][i].cuboid.planeVertices[lb2].Coord.y,
									walls[f][i].cuboid.elevation));
								sindf.Add(++numv);
								snv[2] = numv;
								vertices.Add(new Vector3d(walls[f][i].doors[0].cuboid.vertices[2].Coord.x,
									walls[f][i].cuboid.planeVertices[lb2].Coord.y,
									walls[f][i].cuboid.elevation + walls[f][i].cuboid.height));
								sindf.Add(++numv);
								snv[3] = numv;
								sindf.Add(indf[2]);
								sindf.Add(indf[3]);
								faces.Add(sindf);
							}
							sindf = new List<int>();
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[5].Coord);
							indf.Add(++numv);
							sindf.Add(numv);
							doorind[5] = numv;
							sindf.Add(snv[0]);
							sindf.Add(snv[3]);
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[6].Coord);
							indf.Add(++numv);
							sindf.Add(numv);
							doorind[6] = numv;
							faces.Add(sindf);

							sindf = new List<int>();
							sindf.Add(snv[1]);
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[1].Coord);
							indf.Add(++numv);
							sindf.Add(numv);
							doorind[1] = numv;
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[2].Coord);
							indf.Add(++numv);
							sindf.Add(numv);
							sindf.Add(snv[2]);
							doorind[2] = numv;
							faces.Add(sindf);
						}
						else	// door
						{
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[2].Coord);
							indf.Add(++numv);
							doorind[2] = numv;
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[6].Coord);
							indf.Add(++numv);
							doorind[6] = numv;
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[5].Coord);
							indf.Add(++numv);
							doorind[5] = numv;
							vertices.Add(walls[f][i].doors[0].cuboid.vertices[1].Coord);
							indf.Add(++numv);
							doorind[1] = numv;
							faces.Add(indf);
						}
					}
					else 
						faces.Add(indf);
					// the other four faces of window, and 3 faces of door
					if (walls[f][i].doors.Count > 0)
					{
						indf = new List<int>();
						indf.Add(doorind[0]);
						indf.Add(doorind[1]);
						indf.Add(doorind[5]);
						indf.Add(doorind[4]);
						faces.Add(indf);
						indf = new List<int>();
						//indf.Add(doorind[3]);
						//indf.Add(doorind[2]);
						//indf.Add(doorind[6]);
						//indf.Add(doorind[7]);
						indf.Add(doorind[3]);
						indf.Add(doorind[7]);
						indf.Add(doorind[6]);
						indf.Add(doorind[2]);
						faces.Add(indf);
						indf = new List<int>();
						indf.Add(doorind[4]);
						indf.Add(doorind[5]);
						indf.Add(doorind[6]);
						indf.Add(doorind[7]);
						faces.Add(indf);
						if (walls[f][i].doors[0].cuboid.elevation > thresh) // window
						{
							indf = new List<int>();
							//indf.Add(doorind[0]);
							//indf.Add(doorind[1]);
							//indf.Add(doorind[2]);
							//indf.Add(doorind[3]);
							indf.Add(doorind[0]);
							indf.Add(doorind[3]);
							indf.Add(doorind[2]);
							indf.Add(doorind[1]);
							faces.Add(indf);
						}
						else // for doors, delete ground wall face, replaced by two
						{
							if(fd != -1)
								faces.RemoveAt(fd);
							indf = new List<int>();
							for (j = 0; j < split; j++)
								indf.Add(dwind[j]);
							indf.Add(doorind[1]);
							indf.Add(doorind[0]);
							faces.Add(indf);
							indf = new List<int>();
							indf.Add(doorind[3]);
							indf.Add(doorind[2]);
							for (j = split; j < n; j++)
								indf.Add(dwind[j]);
							faces.Add(indf);
						}
					}
				}
				
				if (sideWallPlanes[f].Count > 0)
				{
					for (i = 0; i < sideWallPlanes[f].Count; i++)
					{
						indf = new List<int>();
						for (j = 0; j < sideWallPlanes[f][i].vertices.Count; j++)
						{
							vertices.Add(sideWallPlanes[f][i].vertices[j].Coord);
							++numv;
							indf.Add(numv);
						}
						//faces.Add(indf);
						invindf = InverseOrder(indf);
						faces.Add(invindf);
					}
				}
				// Vertices index
				for (i = 0; i < vertices.Count; i++)
				{
					vertices[i] *= 100;
					swlt.Write("v ");
					swlt.Write(vertices[i].x.ToString() + " ");
					swlt.Write(vertices[i].y.ToString() + " ");
					swlt.Write(vertices[i].z.ToString() + " ");
					swlt.WriteLine();
				}
				// Face index
				for (i = 0; i < faces.Count; i++)
				{
					swlt.Write("f ");
					for (j = 0; j < faces[i].Count; j++)
						swlt.Write(faces[i][j].ToString() + " ");
					swlt.WriteLine();
				}
				swlt.Close();
			}
		}

		public void Export_Cast(String filename)
		{
			// Export concrete as an .obj file
			StreamWriter sw;
			int i, j, k, n, f;
			int numv = 0;
			int[] nv = new int[4]; // for side wall 
			List<int> indf = new List<int>(), invindf; // At most 6 vertices on a face
			List<List<int>> faces = new List<List<int>>();
			List<Vector3d> vertices = new List<Vector3d>();
			Vector3d v;

			//numFloor = 1;
			for (f = 0; f < numFloor; f++)
			{
				if (numFloor > 1)
				{
					int index = filename.LastIndexOf('.');
					String floorname = filename.Substring(0, index) + "_f" + f.ToString() + filename.Substring(index, filename.Length - index);
					sw = new StreamWriter(floorname);
					faces = new List<List<int>>();
					vertices = new List<Vector3d>();
					numv = 0;
				}
				else
					sw = new StreamWriter(filename);
				for (i = 0; i < walls[f].Count; i++)
				{
					for (j = 0; j < walls[f][i].casts.Count; j++)
					{
						indf = new List<int>();
						for (k = 0; k < 4; k++)
						{
							v = walls[f][i].casts[j].vertices[k].Coord;
							n = FindVertexIndex(vertices, v);
							if (n == -1)
							{
								vertices.Add(new Vector3d(v));
								indf.Add(++numv);
							}
							else
								indf.Add(n + 1);
						}
						if (j < walls[f][i].num_front_cast) // for inverse side faces
						{
							invindf = InverseOrder(indf);
							faces.Add(invindf);
						}
						else
							faces.Add(indf);
					}
				}

				// Vertices index
				for (i = 0; i < vertices.Count; i++)
				{
					vertices[i] *= 100;
					sw.Write("v ");
					sw.Write(vertices[i].x.ToString() + " ");
					sw.Write(vertices[i].y.ToString() + " ");
					sw.Write(vertices[i].z.ToString() + " ");
					sw.WriteLine();
				}
				// Face index
				for (i = 0; i < faces.Count; i++)
				{
					sw.Write("f ");
					for (j = 0; j < faces[i].Count; j++)
						sw.Write(faces[i][j].ToString() + " ");
					sw.WriteLine();
				}
				sw.Close();
			}	
		}

		public void Export_Walltype(String filename)
		{
			//Export space division
			StreamWriter sw = new StreamWriter(filename);
			int f, i, j;
			Vector2d v1 = new Vector2d(), v3 = new Vector2d();

			//// Four types //////
			//i = 400; j = 400;
			//List<List<int>> verts = new List<List<int>>(){new List<int>() { 10, 10, 160, 10, 170, 20, 160, 30,  30, 30},
			//                            new List<int>() { 200, 10, 350, 10, 360, 20, 350, 30, 200, 30, 190, 20},
			//                        new List<int>() { 10, 50, 170, 50, 150, 70, 30, 70},
			//                        new List<int>() { 200, 50, 340, 50, 360, 70, 220, 70}};
			//sw.WriteLine("<</PageSize[" + i.ToString() + " " + j.ToString() + "]>>setpagedevice");
			//sw.WriteLine("0 0 translate");
			//for (j = 0; j < verts.Count; j++)
			//{
			//    verts[j][1] += 150;
			//    sw.Write(verts[j][0].ToString() + " ");
			//    sw.Write(verts[j][1].ToString() + " ");
			//    sw.WriteLine("moveto ");
			//    for (i = 2; i < verts[j].Count; i += 2)
			//    {
			//        verts[j][i+1] += 150;
			//        sw.Write(verts[j][i].ToString() + " ");
			//        sw.Write(verts[j][i + 1].ToString() + " ");
			//        sw.WriteLine("lineto ");
			//    }
			//    sw.WriteLine("closepath");
			//    sw.WriteLine("2 setlinecap");
			//    sw.WriteLine("stroke");
			//}
			//sw.Close();
			//return;
			//// Four types //////
			// rooms
			//numFloor = 1;
			cuboids[0] = new List<Cuboid>();
			newed = new int[walls[0].Count * 2];
			for (i = 0; i < walls[0].Count; i++)
				NewVertices(0, i);
			this.Refresh();

			for (f = 0; f < numFloor; f++)
			{
				int wid = (int)(Math.Abs(bounds[f][0].x - bounds[f][3].x) * 200) + 10,
					height = (int)(Math.Abs(bounds[f][0].y - bounds[f][1].y) * 200) + 10;
				v3.x = -(int)(bounds[f][3].x * 200 - 5);
				v3.y = -(int)(bounds[f][3].y * 200 - 5);
				sw.WriteLine("<</PageSize[" + wid.ToString() + " " + height.ToString() + "]>>setpagedevice");
				sw.WriteLine("0 0 translate");
				for (i = 0; i < walls[f].Count; i++)
				{
					sw.WriteLine("newpath");
					
					v1 = new Vector2d(walls[f][i].cuboid.planeVertices[0].Coord.x,
							walls[f][i].cuboid.planeVertices[0].Coord.y);
					v1 *= 200;
					v1 += v3;
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					for (j = 1; j < walls[f][i].cuboid.planeVertices.Count; j++)
					{
						v1 = new Vector2d(walls[f][i].cuboid.planeVertices[j].Coord.x, 
							walls[f][i].cuboid.planeVertices[j].Coord.y);
						v1 *= 200;
						v1 += v3;
						sw.Write(v1.x.ToString() + " ");
						sw.Write(v1.y.ToString() + " ");
						sw.WriteLine("lineto ");
					}
					sw.WriteLine("closepath");
					sw.WriteLine("2 setlinecap");
					sw.WriteLine("stroke");
				}
			}
			sw.WriteLine();
			sw.WriteLine("showpage");
			sw.Close();
		}

		public void Export_div(String filename)
		{
			//Export space division
			StreamWriter sw;
			int f, i, j;
			double r, g, b;
			Vector2d v1 = new Vector2d(), v2 = new Vector2d(), v3 = new Vector2d();

			// rooms
			//numFloor = 1;
			for (f = 0; f < numFloor; f++)
			{
				if (numFloor > 1)
				{
					int ind = filename.LastIndexOf('.');
					String floorname = filename.Substring(0, ind) + "_f" + f.ToString() + filename.Substring(ind, filename.Length - ind);
					sw = new StreamWriter(floorname);
				}
				else
					sw = new StreamWriter(filename);

				int wid = (int)(Math.Abs(bounds[f][0].x - bounds[f][3].x) * 200) + 20,
					height = (int)(Math.Abs(bounds[f][0].y - bounds[f][1].y) * 200) + 100;
				v3.x = -(int)(bounds[f][3].x * 200 - 10);
				v3.y = -(int)(bounds[f][3].y * 200 - 5);
				sw.WriteLine("<</PageSize[" + wid.ToString() + " " + height.ToString() + "]>>setpagedevice");
				sw.WriteLine("0 50 translate");
				for (i = 0; i < recRooms[f].Count; i++)
				{
					sw.WriteLine("newpath");
					v1 = recRooms[f][i].vertex[0];
					v1 *= 200;
					v1 += v3;
					v2 = recRooms[f][i].vertex[1];
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					for (j = 1; j < recRooms[f][i].vertex.Count; j++)
					{
						v1 = recRooms[f][i].vertex[j];
						v1 *= 200;
						v1 += v3;
						v2 = recRooms[f][i].vertex[(j + 1) % recRooms[f][i].vertex.Count];
						sw.Write(v1.x.ToString() + " ");
						sw.Write(v1.y.ToString() + " ");
						sw.WriteLine("lineto ");
					}
					sw.WriteLine("closepath");
					sw.WriteLine("gsave");
					sw.WriteLine("1 setlinewidth");
					sw.WriteLine("stroke");
					// Text
					r = 0; g = 0; b = 0;
					String str = recRooms[f][i].name;
					j = recRooms[f][i].vertex.Count - 1;
					v1 = new Vector2d(recRooms[f][i].vertex[j].x + recRooms[f][i].width / 4,
						recRooms[f][i].vertex[j].y + recRooms[f][i].length / 2);
					if (str == "Hall_2")
					{
						v1 = new Vector2d(recRooms[f][i].vertex[j].x + recRooms[f][i].width / 12,
						recRooms[f][i].vertex[j].y + recRooms[f][i].length / 2);
						//str = "Hall2";
					}
					v1 *= 200;
					v1 += v3;
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("/Courier-Bold findfont");
					sw.WriteLine("6 scalefont");
					sw.WriteLine("setfont");
					sw.WriteLine("newpath");
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					sw.Write("(");

					if (str == "Utility") str = "UTIL";
					if (str == "Laundry") str = "LAUN";
					sw.WriteLine("{0}{1}", str, ") show");
					sw.WriteLine("0.5 setlinewidth");
					sw.WriteLine("stroke");
				}
				sw.WriteLine();
				sw.WriteLine("showpage");
				sw.Close();
			}
		}

		public void Export_Graph(String filename)
		{
			StreamWriter sw;
			int f, i, j;
			double r, g, b, minx, miny, maxx, maxy;
			Vector2d v1 = new Vector2d(), v2 = new Vector2d();
			String floorname;

			for (f = 0; f < numFloor; f++)
			{
				minx = double.MaxValue; miny = double.MaxValue;
				maxx = double.MinValue; maxy = double.MinValue;
				for (i = 0; i < graph[f].Count; i++)
					for (j = 0; j < graph[f][i].vertices.Count(); j++)
					{
						if (minx > graph[f][i].vertices[j].x)
							minx = graph[f][i].vertices[j].x;
						if (miny > graph[f][i].vertices[j].y)
							miny = graph[f][i].vertices[j].y;
						if (maxx < graph[f][i].vertices[j].x)
							maxx = graph[f][i].vertices[j].x;
						if (maxy < graph[f][i].vertices[j].y)
							maxy = graph[f][i].vertices[j].y;
					}
				int wid = (int)((maxx - minx) * 100) + 10,
					height = (int)((maxy - miny) * 100) + 10;
				v2.x = -(int)(minx * 100 - 5);
				v2.y = -(int)(miny * 100 - 5);
				if (numFloor > 1)
				{
					int index = filename.LastIndexOf('.');
					floorname = filename.Substring(0, index) + "_0" + f.ToString() + filename.Substring(index, filename.Length - index);
					sw = new StreamWriter(floorname);
				}
				else
					sw = new StreamWriter(filename);
				sw.WriteLine("<</PageSize[" + wid.ToString() + " " + height.ToString() + "]>>setpagedevice");
				sw.WriteLine("0 0 translate");

				for (j = 0; j < arrows[f].Count; j += 2)
				{
					v1 = new Vector2d(arrows[f][j]);
					v1 *= 100;
					v1 += v2;
					sw.WriteLine("newpath");
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.Write("moveto ");
					v1 = new Vector2d(arrows[f][j + 1]);
					v1 *= 100;
					v1 += v2;
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.Write("lineto ");
					sw.WriteLine("0.5 setlinewidth");
					sw.WriteLine("stroke");
				}

				for (i = 0; i < graph[f].Count; i++)
				{
					sw.WriteLine("newpath");
					v1 = graph[f][i].vertices[0];
					v1 *= 100;
					v1 += v2;
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					for (j = 1; j < graph[f][i].vertices.Count(); j++)
					{
						v1 = graph[f][i].vertices[j];
						v1 *= 100;
						v1 += v2;
						sw.Write(v1.x.ToString() + " ");
						sw.Write(v1.y.ToString() + " ");
						sw.WriteLine("lineto ");
					}
					sw.WriteLine("closepath");
					sw.WriteLine("gsave");
					r = 1; g = 1; b = 1;
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("fill");
					sw.WriteLine("grestore");
					r = 0; g = 0; b = 0;
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("0.5 setlinewidth");
					sw.WriteLine("stroke");
					// Text
					r = 0; g = 0; b = 0;
					String str = recRooms[f][i].name;
					j = 3;
					v1 = new Vector2d(graph[f][i].center);
					v1 *= 100;
					v1 += v2;
					v1.x -= 4.5;
					v1.y -= 1;
					if (str == "Utility") str = "UTIL"; 
					if (str == "Laundry") str = "LAUN"; 
					
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("/Courier-Bold findfont");
					sw.WriteLine("2.5 scalefont");
					sw.WriteLine("setfont");
					sw.WriteLine("newpath");
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					sw.Write("(");

					
					sw.WriteLine("{0}{1}", str, ") show");
					sw.WriteLine("0.5 setlinewidth");
					sw.WriteLine("stroke");
				}
				sw.Close();
			}
		}

		public void Export_PS(String filename)
		{
			// Save wall structures to .pdf file 
			StreamWriter sw;
			int f, i, j, n;
			double r, g, b;
			Vector2d v1 = new Vector2d(), v2 = new Vector2d();

			// The space division
			int index = filename.LastIndexOf('.');
			String name = filename.Substring(0, index) + "_div" + filename.Substring(index, filename.Length - index);
			Export_div(name);

			//return;

			name = filename.Substring(0, index) + "_room" + filename.Substring(index, filename.Length - index);
			filename = name;
			
			//sw = new StreamWriter(filename);

			// walls
			//numFloor = 1;
			for (f = 0; f < numFloor; f++)
			{
				if (numFloor > 1)
				{
					int ind = filename.LastIndexOf('.');
					String floorname = filename.Substring(0, ind) + "_f" + f.ToString() + filename.Substring(ind, filename.Length - ind);
					sw = new StreamWriter(floorname);
				}
				else
					sw = new StreamWriter(filename);

				int wid = (int)(Math.Abs(bounds[f][0].x - bounds[f][3].x) * 200) + 10,
					height = (int)(Math.Abs(bounds[f][0].y - bounds[f][1].y) * 200) + 10;
				v2.x = -(int)(bounds[f][3].x * 200 - 5);
				v2.y = -(int)(bounds[f][3].y * 200 - 5);
				sw.WriteLine("<</PageSize[" + wid.ToString() + " " + height.ToString() + "]>>setpagedevice");
				sw.WriteLine("0 0 translate");
				// Openwall
				for (i = 0; i < walls[f].Count; i++)
				{
					if (!walls[f][i].open) continue;
					n = walls[f][i].cuboid.vertices.Count / 2;
					sw.WriteLine("newpath");
					v1 = (new Vector2d(walls[f][i].cuboid.vertices[0].Coord.x,
							walls[f][i].cuboid.vertices[0].Coord.y));
					v1 *= 200;
					v1 += v2;
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					for (j = 1; j < n; j++)
					{
						v1 = (new Vector2d(walls[f][i].cuboid.vertices[j].Coord.x,
							walls[f][i].cuboid.vertices[j].Coord.y));
						v1 *= 200;
						v1 += v2;
						sw.Write(v1.x.ToString() + " ");
						sw.Write(v1.y.ToString() + " ");
						sw.WriteLine("lineto ");
					}
					sw.WriteLine("closepath");
					sw.WriteLine("gsave");
					r = 0.9; g = 0.9; b = 0.9;
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("fill");
					sw.WriteLine("grestore");
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("1 setlinewidth");
					sw.WriteLine("stroke");
				}		
				// Normal wall
				List<Vector2d> vertices;
				for (i = 0; i < walls[f].Count; i++)
				{
					if (walls[f][i].open) continue;
					//if (i == 21) continue;
					n = walls[f][i].cuboid.vertices.Count / 2;
					/////// Avoid rounding error leading to irregular figure ///////
					n = 4;
					vertices = new List<Vector2d>();
					Vector3d vd = new Vector3d(walls[f][i].cuboid.endPoints[0].Coord - walls[f][i].cuboid.direction * wallThickness / 2
						- walls[f][i].cuboid.normal * wallThickness / 2);
					vertices.Add(new Vector2d( vd.x, vd.y ));
					vd = new Vector3d(walls[f][i].cuboid.endPoints[0].Coord - walls[f][i].cuboid.direction * wallThickness / 2
						+ walls[f][i].cuboid.normal * wallThickness / 2);
					vertices.Add(new Vector2d(vd.x, vd.y));
					vd = new Vector3d(walls[f][i].cuboid.endPoints[1].Coord + walls[f][i].cuboid.direction * wallThickness / 2 
						+ walls[f][i].cuboid.normal * wallThickness / 2);
					vertices.Add(new Vector2d(vd.x, vd.y));
					vd = new Vector3d(walls[f][i].cuboid.endPoints[1].Coord + walls[f][i].cuboid.direction * wallThickness / 2 
						- walls[f][i].cuboid.normal * wallThickness / 2);
					vertices.Add(new Vector2d(vd.x, vd.y));
					/////// Avoid rounding error leading to irregular figure ///////
					sw.WriteLine("newpath");
					//v1 = (new Vector2d(walls[f][i].cuboid.vertices[0].Coord.x,
					//        walls[f][i].cuboid.vertices[0].Coord.y));
					v1 = vertices[0];
					v1 *= 200;
					v1 += v2;
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					for (j = 1; j < n; j++)
					{
						//v1 = (new Vector2d(walls[f][i].cuboid.vertices[j].Coord.x,
						//    walls[f][i].cuboid.vertices[j].Coord.y));
						v1 = vertices[j];
						v1 *= 200;
						v1 += v2;
						sw.Write(v1.x.ToString() + " ");
						sw.Write(v1.y.ToString() + " ");
						sw.WriteLine("lineto ");
					}
					sw.WriteLine("closepath");
					sw.WriteLine("gsave");
					r = 0; g = 0; b = 0;
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor"); // fill color
					sw.WriteLine("fill");
					sw.WriteLine("grestore");
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor"); // boundary line color
					sw.WriteLine("1 setlinewidth");
					sw.WriteLine("stroke");
				}
				///////// For output figure
				//i = 21;
				//n = walls[f][i].cuboid.vertices.Count / 2;
				//sw.WriteLine("newpath");
				//v1 = (new Vector2d(walls[f][i].cuboid.vertices[0].Coord.x,
				//        walls[f][i].cuboid.vertices[0].Coord.y));
				//v1 *= 200;
				//v1 += v2;
				//v1.x -= 50;
				//sw.Write(v1.x.ToString() + " ");
				//sw.Write(v1.y.ToString() + " ");
				//sw.WriteLine("moveto ");
				//for (j = 1; j < n; j++)
				//{
				//    v1 = (new Vector2d(walls[f][i].cuboid.vertices[j].Coord.x,
				//        walls[f][i].cuboid.vertices[j].Coord.y));
				//    v1 *= 200;
				//    v1 += v2;
				//    v1.x -= 50;
				//    sw.Write(v1.x.ToString() + " ");
				//    sw.Write(v1.y.ToString() + " ");
				//    sw.WriteLine("lineto ");
				//}
				//sw.WriteLine("closepath");
				//sw.WriteLine("gsave");
				//r = 0.78; g = 0.88; b = 1;
				//sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor"); // fill color
				//sw.WriteLine("fill");
				//sw.WriteLine("grestore");
				//sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor"); // boundary line color
				//sw.WriteLine("1 setlinewidth");
				//sw.WriteLine("stroke");
				///////// For output figure
				sw.WriteLine("% Doors and Windows");
				// doors
				for (i = 0; i < doors[f].Count; i++)
				{
					v1 = new Vector2d(doors[f][i].cuboid.vertices[0].Coord.x,
							doors[f][i].cuboid.vertices[0].Coord.y);
					v1 *= 200;
					v1 += v2;
					//if (doors[f][i].cuboid.wallIndex == 21)
					//    v1.x -= 50;
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					for (j = 1; j < 4; j++)
					{
						v1 = new Vector2d(doors[f][i].cuboid.vertices[j].Coord.x,
							doors[f][i].cuboid.vertices[j].Coord.y);
						v1 *= 200;
						v1 += v2;
						//if (doors[f][i].cuboid.wallIndex == 21)
						//    v1.x -= 50;
						sw.Write(v1.x.ToString() + " ");
						sw.Write(v1.y.ToString() + " ");
						sw.WriteLine("lineto ");
					}
					sw.WriteLine("closepath");
					sw.WriteLine("gsave");
					if (doors[f][i].elevation > thresh)
					{
						r = 1; g = 0.8; b = 0.9;
					}
					else
					{
						r = 0.6; g = 0.8; b = 0.8;
					}
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("fill");
					sw.WriteLine("grestore");
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("1 setlinewidth");
					sw.WriteLine("stroke");
				}
				// Text
				r = 0; g = 0; b = 0;
				for (i = 0; i < recRooms[f].Count; i++)
				{
					String str = recRooms[f][i].name;
					j = recRooms[f][i].vertex.Count - 1;
					v1 = new Vector2d(recRooms[f][i].vertex[j].x + recRooms[f][i].width / 4,
						recRooms[f][i].vertex[j].y + recRooms[f][i].length / 2);
					if (str == "Hall_2")
					{
						v1 = new Vector2d(recRooms[f][i].vertex[j].x + recRooms[f][i].width / 6,
						recRooms[f][i].vertex[j].y + recRooms[f][i].length / 2);
						//str = "Hall2";
					}
					v1 *= 200;
					v1 += v2;
					v1.x -= 3;
					sw.WriteLine("{0} {1} {2} {3}", r.ToString(), g.ToString(), b.ToString(), "setrgbcolor");
					sw.WriteLine("/Courier-Bold findfont");
					sw.WriteLine("6 scalefont");
					sw.WriteLine("setfont");
					sw.WriteLine("newpath");
					sw.Write(v1.x.ToString() + " ");
					sw.Write(v1.y.ToString() + " ");
					sw.WriteLine("moveto ");
					sw.Write("(");
					if (str == "Utility") str = "UTIL";
					if (str == "Laundry") str = "LAUN";
					sw.WriteLine("{0}{1}", str, ") show");
					sw.WriteLine("0.5 setlinewidth");
					sw.WriteLine("stroke");
				}
				sw.WriteLine();
				sw.WriteLine("showpage");
				sw.Close();
			}
		}

		public bool Contain(List<Vertex> verts, Vertex v)
        {
            bool con = false;
            for (int i = 0; i < verts.Count; i++)
                if ((v.Coord - verts[i].Coord).Length() < thresh)
                {
                    con = true;
                    break;
                }
            return (con);
        }
		public void Distinct(List<Vertex> pnts)
		{
			for (int i = 0; i < pnts.Count - 1; i++)
				for (int j = i + 1; j < pnts.Count; j++)
					if ((pnts[j].Coord - pnts[i].Coord).Length() < thresh)
					{
						pnts.RemoveAt(j);
						j--;
					}
		}
        public void Distinct(List<Vector3d> pnts)
        {
            for (int i = 0; i < pnts.Count - 1; i++)
                for (int j = i + 1; j < pnts.Count; j++)
                    if ((pnts[j] - pnts[i]).Length() < thresh)
                    {
                        pnts.RemoveAt(j);
                        j--;
                    }
        }
        public void Distinct(List<int> arr)
        {
            int i, j;
            for (i = 0; i < arr.Count; i++)
            {
                for (j = i + 1; j < arr.Count; j++)
                    if (arr[j] == arr[i])
                    {
                        arr.RemoveAt(j);
                        j--;
                    }
            }
        }
        public void Distinct(List<double> arr)
        {
            // Distince Set
            int i, j;
            for (i = 0; i < arr.Count; i++)
            {
                for (j = i + 1; j < arr.Count; j++)
                    if (Math.Abs(arr[j] - arr[i]) < thresh)
                    {
                        arr.RemoveAt(j);
                        j--;
                    }
            }
        }
        public List<int> Intersect(List<int> array1, List<int> array2)
        {
            // Intersection 
            int i, j;
            List<int> arr = new List<int>();
            for (i = 0; i < array1.Count; i++)
            {
                for (j = 0; j < array2.Count; j++)
                    if (array2[j] == array1[i])
                    {
                        arr.Add(array1[i]);
                        break;
                    }
            }
            Distinct(arr);
            return (arr);
        }

        public Array ConvertArrayType(double[] arr)
        {
            Array array = new double[arr.Length];
            for (int i = 0; i < arr.Length; i++)
                array.SetValue(arr[i], i);
            return (array);
        }
        public Array ConvertArrayType(double[,] arr)
        {
            int row = arr.GetLength(0), col = arr.GetLength(1);
            Array array = new double[row, col];
            for (int i = 0; i < row; i++)
                for (int j = 0; j < col; j++)
                    array.SetValue(arr[i, j], i, j);
            return (array);
        }

        public bool Point_In_Line(Vector2d v1, Vector2d v2, Vector2d v)
        {
            // Point on the line but not the endpoints
            bool inline = false;
            if (Math.Abs(v1.x - v2.x) < thresh) // Y
            {
                if (Math.Abs(v.x - v1.x) < thresh)
                {
                    if (v.y < Math.Max(v1.y, v2.y) - thresh && v.y > Math.Min(v1.y, v2.y) + thresh)
                        inline = true;
                }
            }
            else if (Math.Abs(v1.y - v2.y) < thresh) // X
            {
                if (Math.Abs(v.y - v1.y) < thresh)
                {
                    if (v.x < Math.Max(v1.x, v2.x) - thresh && v.x > Math.Min(v1.x, v2.x) + thresh)
                        inline = true;
                }
            }
            return (inline);
        }
		public bool Point_In_Line(Vector3d v1, Vector3d v2, Vector3d v)
		{
            // Point on the line but not the endpoints
			bool inline = false;
			if (Math.Abs(v1.x - v2.x) < thresh) // Y
			{
				if (Math.Abs(v.x - v1.x) < thresh)
				{
					if (v.y < Math.Max(v1.y, v2.y) - thresh && v.y > Math.Min(v1.y, v2.y) + thresh)
						inline = true;
				}
			}
			else if (Math.Abs(v1.y - v2.y) < thresh) // X
			{
				if (Math.Abs(v.y - v1.y) < thresh)
				{
					if (v.x < Math.Max(v1.x, v2.x) - thresh && v.x > Math.Min(v1.x, v2.x) + thresh)
						inline = true;
				}
			}
			return (inline);
		}

		public bool Point_On_Line(Vector2d v1, Vector2d v2, Vector2d v)
		{
			bool online = false;
			double thresh = 1e-3;
			if ((v - v1).Length() < thresh || (v - v2).Length() < thresh)
				return (true);
			if (Math.Abs(v1.x - v2.x) < thresh) // Y
			{
				if (Math.Abs(v.x - v1.x) < thresh)
				{
					if (v.y < Math.Max(v1.y, v2.y)+thresh && v.y > Math.Min(v1.y, v2.y) - thresh)
						online = true;
				}
			}
			else if (Math.Abs(v1.y - v2.y) < thresh) // X
			{
				if (Math.Abs(v.y - v1.y) < thresh)
				{
                    if (v.x < Math.Max(v1.x, v2.x) + thresh && v.x > Math.Min(v1.x, v2.x) - thresh)
						online = true;
				}
			}
			return (online);
		}
		public bool Point_On_Line(Vector3d v1, Vector3d v2, Vector3d v)
		{
			bool online = false;
			double thresh = 1e-4;
			if ((v - v1).Length() < thresh || (v - v2).Length() < thresh)
				return (true);
			if (Math.Abs(v1.x - v2.x) < thresh) // Y
			{
				if (Math.Abs(v.x - v1.x) < thresh)
				{
					if (v.y < Math.Max(v1.y, v2.y) && v.y > Math.Min(v1.y, v2.y))
						online = true;
				}
			}
			else if (Math.Abs(v1.y - v2.y) < thresh) // X
			{
				if (Math.Abs(v.y - v1.y) < thresh)
				{
					if (v.x < Math.Max(v1.x, v2.x) && v.x > Math.Min(v1.x, v2.x))
						online = true;
				}
			}
			return (online);
		}

		public Vector3d CalNormal(Vertex v1, Vertex v2)
		{
			Vector3d normal;
			// Make the normal always point to the inside of a polygon
			if (Math.Abs(v1.Coord.x - v2.Coord.x) < 1e-6)
			{
				if (v2.Coord.y > v1.Coord.y)
					normal = new Vector3d(-1, 0, 0);
				else
					normal = new Vector3d(1, 0, 0);
			}
			else
			{
				if (v2.Coord.x > v1.Coord.x)
					normal = new Vector3d(0, 1, 0);
				else
					normal = new Vector3d(0, -1, 0);
			}
			normal.Normalize();
			return (normal);
		}
		public int Sum(int[] arr)
		{
			int sum = 0;
			for (int i = 0; i < arr.Length; i++)
				sum += arr[i];
			return (sum);
		}
		public double Sum(double[] arr)
		{
			double sum = 0;
			for (int i = 0; i < arr.Length; i++)
				sum += arr[i];
			return (sum);
		}
		public int WallType(Vertex p0, Vertex p1)
		{
 			// Four types of walls[activeFloor] (clockwise): 口
			// from left vertical wall to below horizontal wall
			int type = -1;
			if (Math.Abs(p0.Coord.x - p1.Coord.x) < thresh)
			{
				if (p0.Coord.y < p1.Coord.y)
					type = 0;
				else if (p0.Coord.y > p1.Coord.y)
					type = 2;
			}
			else if (Math.Abs(p0.Coord.y - p1.Coord.y) < thresh)
			{
				if (p0.Coord.x < p1.Coord.x)
					type = 1;
				else if (p0.Coord.x > p1.Coord.x)
					type = 3;
			}
			return (type);
		}

        // Calculate the area of a polygon (ordered)
        public double PolygonArea(List<PointF> pnts)
        {
            double area = 0;
            int i = 0, j;
            for (i = 0; i < pnts.Count; i++)
            {
                j = (i + 1) % pnts.Count;
                area += pnts[i].X * pnts[j].Y - pnts[i].Y * pnts[j].X;
            }
			area = Math.Abs(area);
            area /= 2;
			//area *= 100;
            return (area);
        }

        public double PolygonArea(List<Vector2d> pnts)
        {
            double area = 0;
            int i = 0, j;
            for (i = 0; i < pnts.Count; i++)
            {
                j = (i + 1) % pnts.Count;
                area += pnts[i].x * pnts[j].y - pnts[i].y * pnts[j].x;
            }
			area = Math.Abs(area);
            area /= 2;
            //area *= 100;
			
            return (area);
        }

		public double PolygonArea(List<Vertex> pnts)
		{
            double area = 0;
            int i = 0, j;
            for (i = 0; i < pnts.Count; i++)
            {
                j = (i + 1) % pnts.Count;
                area += pnts[i].Coord.x * pnts[j].Coord.y - pnts[i].Coord.y * pnts[j].Coord.x;
            }
			area = Math.Abs(area);
            area /= 2;
            //area *= 100;
            return (area);
		}

		public void InverseSort(List<int> list)
		{
			int max;
			for (int i = 0; i < list.Count - 1; i++)
			{
				max = i;
				for (int j = i + 1; j < list.Count; j++)
				{
					if (list[j] > list[max])
						max = j;
				}
				int t = list[max];
				list[max] = list[i];
				list[i] = t;
			}
		}

        public void Sort(List<int> list)
        {
            int min;
            for (int i = 0; i < list.Count - 1; i++)
            {
                min = i;
                for (int j = i + 1; j < list.Count; j++)
                {
                    if (list[j] < list[min])
                        min = j;
                }
                int t = list[min];
                list[min] = list[i];
                list[i] = t;
            }
        }

		public double[] Sort(double[] list)
		{
			int min;
			for (int i = 0; i < list.Length - 1; i++)
			{
				min = i;
				for (int j = i + 1; j < list.Length; j++)
				{
					if (list[j] < list[min])
						min = j;
				}
				double t = list[min];
				list[min] = list[i];
				list[i] = t;
			}
			return (list);
		}

		public List<double> Sort(List<double> arr)
		{
			int min;
			for (int i = 0; i < arr.Count - 1; i++)
			{
				min = i;
				for (int j = i + 1; j < arr.Count; j++)
				{
					if (arr[j] < arr[min])
						min = j;
				}
				double t = arr[min];
				arr[min] = arr[i];
				arr[i] = t;
			}
			return (arr);
		}

		public List<int> InverseOrder(List<int> elems)
		{
			List<int> invelems = new List<int>();
			for (int i = elems.Count - 1; i >= 0; i--)
				invelems.Add(elems[i]);
			return (invelems);
		}

		public List<int> GetRelatedWall(int[] inds, int type)
		{
			// Indices of related walls and which endpoints
			List<int> wallInds = new List<int>();
			int i, ind = inds[1];
			for (i = 0; i < walls[activeFloor].Count; i++) // Floor ind[0]
			{
                Distinct(walls[activeFloor][i].roomIndices);
				if (i == ind) continue;
				// only the walls[activeFloor] that are perpendicular to the active wall and intersect with one endpoint
				// need to be translated
				if (Math.Abs(walls[activeFloor][i].direction.Dot(walls[activeFloor][ind].direction)) < thresh)
				{
					if ((walls[activeFloor][i].endPoints[0].Coord - walls[activeFloor][ind].endPoints[0].Coord).Length() < thresh ||
						(walls[activeFloor][i].endPoints[0].Coord - walls[activeFloor][ind].endPoints[1].Coord).Length() < thresh)
					{
						wallInds.Add(i);
						wallInds.Add(0);
					}
					else if ((walls[activeFloor][i].endPoints[1].Coord - walls[activeFloor][ind].endPoints[0].Coord).Length() < thresh ||
						(walls[activeFloor][i].endPoints[1].Coord - walls[activeFloor][ind].endPoints[1].Coord).Length() < thresh)
					{
						wallInds.Add(i);
						wallInds.Add(1);
					}
				}
			}
			return (wallInds);
		}

		private static bool PointInPoly(Vector2d p, List<Vector3d> poly)
		{
			bool c = false;
			int num = poly.Count;
			for (int i = 0, j = num - 1; i < num; j = i++)
			{
				if (((poly[i].y > p.y) != (poly[j].y > p.y)) &&
						(p.x < (poly[j].x - poly[i].x) * (p.y - poly[i].y)
						/ (poly[j].y - poly[i].y) + poly[i].x))
					c = !c;
			}
			return c;
		}

        private static bool PointInPoly(Vector2d p, List<Vector2d> poly)
        {
            bool c = false;
            int num = poly.Count;
            for (int i = 0, j = num - 1; i < num; j = i++)
            {
                if (((poly[i].y > p.y ) != (poly[j].y > p.y)) &&
                        (p.x < (poly[j].x - poly[i].x) * (p.y - poly[i].y)
                        / (poly[j].y - poly[i].y) + poly[i].x ))
                    c = !c;
            }
            return c;
        }

		private bool PointInsidePoly(Vector2d p, List<Vector2d> poly)
		{
			// Edge points are not included
			bool c = false;
			int i,j, num = poly.Count;
			for (i = 0, j = num - 1; i < num; j = i++)
			{
				if (((poly[i].y > p.y) != (poly[j].y > p.y)) &&
						(p.x < (poly[j].x - poly[i].x) * (p.y - poly[i].y)
						/ (poly[j].y - poly[i].y) + poly[i].x))
					c = !c;
			}
			if (!c)	return c;
			// Check whether it is on the edge
			for (i = 0; i < poly.Count; i++)
			{
				if (Point_On_Line(poly[i], poly[(i + 1) % poly.Count], p))
				{
					c = false;
					break;
				}
			}
			return c;
		}

		public void Clear()
		{
			walls = new List<List<Wall>>();
			cuboids = new List<List<Cuboid>>();
			doors = new List<List<Door>>();
		}
		////////// Basic Functions/////////

		// -------------- Drawing -----------------------------//

		public void Print2DText(Vector2d pos, string s)
		{
			// Print text box
			Gl.glColor3ub(0, 0, 0);
			Gl.glRasterPos2d(pos.x, pos.y);
			Gl.glPushAttrib(Gl.GL_LIST_BIT);					// Pushes The Display List Bits
			Gl.glListBase(this.fontBase);						// Sets The Base Character to 32
			Gl.glCallLists(s.Length, Gl.GL_UNSIGNED_SHORT, s);	// Draws The Display List Text
			Gl.glPopAttrib();									// Pops The Display List Bits
		}

		public void Print3DText(Vector3d pos, string s)
		{
			// Print text box
			Gl.glColor3ub(0, 0, 0);
			Gl.glRasterPos3d(pos.x, pos.y, pos.z);
			Gl.glPushAttrib(Gl.GL_LIST_BIT);					// Pushes The Display List Bits
			Gl.glListBase(this.fontBase);						// Sets The Base Character to 32
			Gl.glCallLists(s.Length, Gl.GL_UNSIGNED_SHORT, s);	// Draws The Display List Text
			Gl.glPopAttrib();									// Pops The Display List Bits
		}
		// ------------------- 2D Drawing-------------------//

		private void DrawWall(int ind, int color)
		{
			// 2D
			float[] colors = { 0.0f, 0.0f, 0.0f, 0.8f },
				hlcolors = { 0.8f, 1.0f, 0.6f, 0.8f };
			Plane plane = walls[drawFloor][ind].cuboid.planes[0];
			String s;
			
			s = ind.ToString();

			Gl.glEnable(Gl.GL_BLEND);
			Gl.glBlendFunc(Gl.GL_SRC_ALPHA, Gl.GL_ONE_MINUS_SRC_ALPHA);
			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINES);
			Gl.glDisable(Gl.GL_CULL_FACE);
			Gl.glEnable(Gl.GL_NORMALIZE);
			if (color == 0)
				Gl.glColor4f(colors[0], colors[1], colors[2], colors[3]);
			else
				Gl.glColor4f(hlcolors[0], hlcolors[1], hlcolors[2], hlcolors[3]); //highlight wall
			if (walls[drawFloor][ind].open)
				Gl.glColor4f(colors[0], colors[1], colors[2], 0.3f);
			Gl.glBegin(Gl.GL_POLYGON);
			for (int j = 0; j < plane.vertices.Count; j++)
				Gl.glVertex3d(plane.vertices[j].Coord.x,
				plane.vertices[j].Coord.y,
				plane.vertices[j].Coord.z);
			Gl.glEnd();

			//Vector2d v;
			//v = new Vector2d((walls[drawFloor][ind].cuboid.endPoints[0].Coord.x +
			//    walls[drawFloor][ind].cuboid.endPoints[1].Coord.x) / 2,
			//    (walls[drawFloor][ind].cuboid.endPoints[0].Coord.y +
			//    walls[drawFloor][ind].cuboid.endPoints[1].Coord.y) / 2);
			////Vector3d v3;
			////v3 = walls[drawFloor][ind].cuboid.endPoints[1].Coord - walls[drawFloor][ind].cuboid.endPoints[0].Coord;
			////v3 = v3.Normalize();
			////v += new Vector2d(v3.x, v3.y) * wallThickness * 2;
			//Print2DText(v, s);

			Gl.glEnable(Gl.GL_CULL_FACE);
			Gl.glDisable(Gl.GL_NORMALIZE);
			Gl.glDisable(Gl.GL_BLEND);
		}

		public void DrawSlabLabel()
		{
			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glPushMatrix();
			Gl.glLoadIdentity();
			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			Gl.glPushMatrix();
			Gl.glLoadIdentity();

			double x = bounds[0][3].x - 0.3,
				y = bounds[0][3].y + 0.8;
			double len = 0.06, wid = 0.12, interv = 0.01, value;
			Vector2d v;
			int i, j;
			double[,] cr = new double[4, 2] { { x, y }, { x, y + len }, { x - wid, y + len }, { x - wid, y } };
			String str = "";
			for (i = 0; i < blocks.Length + 3; i++)
			{
				Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);
				Gl.glColor4ub(slabcolors[i, 0], slabcolors[i, 1], slabcolors[i, 2], slabcolors[i, 3]);
				Gl.glBegin(Gl.GL_POLYGON);
				for (j = 0; j < 4; j++)
					Gl.glVertex2d(cr[j, 0], cr[j, 1]);
				Gl.glEnd();
				v = new Vector2d(x + interv, y);
				if (i < blocks.Length)
				{
					value = slabHeight * 10;
					str = value.ToString() + "m * ";
					value = blocks[i] * 10;
					str += value.ToString() + "m (";
					str += types[i].ToString();
					str += ")";
				}
				if (i == blocks.Length)
				{
					str = "cut block (";
					str += ncuts.ToString();
					str += ")";
				}
				else if (i == blocks.Length + 1)
				{
					str = "block-door (";
					str += totald.ToString();
					str += ")";
				}
				else if (i == blocks.Length + 2)
				{
					str = "block-window (";
					str += totalw.ToString();
					str += ")";
				}
				Print2DText(v, str);

				y -= interv * 10;
				cr = new double[4, 2] { { x, y }, { x, y + len }, { x - wid, y + len }, { x - wid, y } };
			}

			Gl.glMatrixMode(Gl.GL_PROJECTION);
			Gl.glPopMatrix();
			Gl.glMatrixMode(Gl.GL_MODELVIEW);
			Gl.glPopMatrix();
		}

		public int colorindex = 0;
		private void DrawCuboid(int label, bool highlight, Cuboid cuboid)
		{
			float[] colors = { 0.0f, 0.0f, 0.0f, 0.8f };
			float[,] colours = { {0.8f, 0.6f, 0.8f},
								 {0.6f, 0.8f, 0.8f},
							     {0.6f, 0.6f, 0.8f}};
			if (label == 2)
			{
				colors[0] = 0.6f;
				colors[1] = 0.8f;
				colors[2] = 0.8f;
				if (cuboid.elevation > thresh)
				{
					colors[0] = 1f;
					colors[1] = 0.8f;
					colors[2] = 0.9f;
				}
			}
			if (label == 3)
			{
				colors[0] = colours[colorindex%3, 0];
				colors[1] = colours[colorindex%3, 1];
				colors[2] = colours[colorindex%3, 2];
				colorindex++;
			}
			Gl.glEnable(Gl.GL_BLEND);
			Gl.glBlendFunc(Gl.GL_SRC_ALPHA, Gl.GL_ONE_MINUS_SRC_ALPHA);
			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINES);
			Gl.glDisable(Gl.GL_CULL_FACE);
			Gl.glEnable(Gl.GL_NORMALIZE);
			Gl.glEnable(Gl.GL_LINE_SMOOTH);
			if (highlight)
				Gl.glColor4ub(228, 26, 28, 220);
			else
				Gl.glColor4f(colors[0], colors[1], colors[2], colors[3]);
			Gl.glBegin(Gl.GL_POLYGON);
			for (int i = 0; i < cuboid.planes[0].vertices.Count; i++)
				Gl.glVertex2d(cuboid.planes[0].vertices[i].Coord.x,
					cuboid.planes[0].vertices[i].Coord.y);
			Gl.glEnd();
			Gl.glDisable(Gl.GL_LINE_SMOOTH);
			Gl.glDisable(Gl.GL_NORMALIZE);
			Gl.glDisable(Gl.GL_BLEND);
			Gl.glEnable(Gl.GL_CULL_FACE);
		}
		// ------------------- 2D Drawing-------------------//

		private void DrawInit()
		{
			Vector2d loc;
			int i, j;

			//for (i = 0; i < drawBounds.Count; i++)
			//{
			//    Gl.glLineWidth(6.0f);
			//    Gl.glColor4ub(143, 188, 143, 255);
			//    for (j = 0; j < drawBounds[i].Count; j++)
			//    {
			//        Gl.glBegin(Gl.GL_LINES);
			//        Gl.glVertex2d(drawBounds[i][j].x, drawBounds[i][j].y);
			//        Gl.glVertex2d(drawBounds[i][(j + 1) % drawBounds[i].Count].x, drawBounds[i][(j + 1) % drawBounds[i].Count].y);
			//        Gl.glEnd();
			//    }
			//}
			if ((walls == null || walls.Count == 0) && dispRoom)
			{
				Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINE);
				Gl.glLineWidth(2.0f);
				for (int f = 0; f < recRooms.Count; f++)
				{
					for (i = 0; i < recRooms[f].Count; i++)
					{
						Gl.glColor3f(0.0f, 0.0f, 0.0f);
						//if (optimization)
						//    Gl.glColor3f((float)(i * 0.05), (float)(i * 0.05), (float)(i * 0.05));
						Gl.glBegin(Gl.GL_POLYGON);
						for (j = 0; j < recRooms[f][i].vertex.Count; j++)
						{
							Gl.glVertex2d(recRooms[f][i].vertex[j].x, recRooms[f][i].vertex[j].y);
						}
						Gl.glEnd();
					}
				}
				Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);
			}

			// Switch rooms
			if (toswitch[0, 0] != -1)
				DrawRoom(toswitch[0, 0], toswitch[0, 1]);
			if (toswitch[1, 0] != -1)
				DrawRoom(toswitch[1, 0], toswitch[1, 1]);

			if (dispRoom)
			{
				for (int f = 0; f < drawBounds.Count; f++)
				{
					for (i = 0; i < recRooms[f].Count; i++)
					{
						loc = new Vector2d(recRooms[f][i].center - new Vector2d(0.05, 0));
						Print2DText(loc, recRooms[f][i].name);
					}
				}
			}
			//////////////// Draw connectivity graph //////////////// 
			if (!dispRoom)
			{
				if (arrows != null && arrows.Count > 0)
				{
					Gl.glLineWidth(1.5f);
					for (int f = 0; f < arrows.Count; f++)
					{
						for (i = 0; i < arrows[f].Count; i += 2)
						{
							Gl.glColor3f(0.0f, 0.0f, 0.0f);
							Gl.glBegin(Gl.GL_LINES);
							Gl.glVertex2d(arrows[f][i].x, arrows[f][i].y);
							Gl.glVertex2d(arrows[f][i + 1].x, arrows[f][i + 1].y);
							Gl.glEnd();
						}
					}
				}


				if (newArrows != null && newArrows.Count > 0)
				{
					Gl.glLineWidth(1.5f);
					for (int f = 0; f < newArrows.Count; f++)
					{
						for (i = 0; i < newArrows[f].Count; i += 2)
						{
							Gl.glColor3f(0.0f, 0.0f, 1.0f);
							Gl.glBegin(Gl.GL_LINES);
							Gl.glVertex2d(newArrows[f][i].x, newArrows[f][i].y);
							Gl.glVertex2d(newArrows[f][i + 1].x, newArrows[f][i + 1].y);
							Gl.glEnd();
						}
					}
					for (int f = 0; f < missArrows.Count; f++)
					{
						for (i = 0; i < missArrows[f].Count; i += 2)
						{
							Gl.glColor3f(1.0f, 0.0f, 0.0f);
							Gl.glBegin(Gl.GL_LINES);
							Gl.glVertex2d(missArrows[f][i].x, missArrows[f][i].y);
							Gl.glVertex2d(missArrows[f][i + 1].x, missArrows[f][i + 1].y);
							Gl.glEnd();
						}
					}
				}


				if (graph != null && graph.Count > 0)
				{
					Gl.glLineWidth(2.0f);
					for (int f = 0; f < graph.Count; f++)
					{
						for (i = 0; i < graph[f].Count; i++)
						{
							Gl.glColor3f(1.0f, 1.0f, 1.0f);
							Gl.glBegin(Gl.GL_POLYGON);
							for (j = 0; j < graph[f][i].vertices.Length; j++)
								Gl.glVertex2d(graph[f][i].vertices[j].x, graph[f][i].vertices[j].y);
							Gl.glEnd();
						}
					}
					Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINE);
					for (int f = 0; f < graph.Count; f++)
					{
						for (i = 0; i < graph[f].Count; i++)
						{
							Gl.glColor3f(0.0f, 0.0f, 0.0f);
							Gl.glBegin(Gl.GL_POLYGON);
							for (j = 0; j < graph[f][i].vertices.Length; j++)
								Gl.glVertex2d(graph[f][i].vertices[j].x, graph[f][i].vertices[j].y);
							Gl.glEnd();

							//if (test == 1) ---- // initiallayout --> refresh() error! (remove room)
							//double t = recRooms[f][i].name.Length / 2 * 0.01;
							//loc = new Vector2d(graph[f][i].center - new Vector2d(1, 0) * t);
							//Print2DText(loc, recRooms[f][i].name);
						}
					}
					Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);
					for (int f = 0; f < graph.Count; f++)
					{
						for (i = 0; i < graph[f].Count; i++)
						{
							Gl.glColor3f(1f, 1f, 1f);
							Gl.glBegin(Gl.GL_POLYGON);
							for (j = 0; j < graph[f][i].vertices.Length; j++)
								Gl.glVertex2d(graph[f][i].vertices[j].x, graph[f][i].vertices[j].y);
							Gl.glEnd();

							//if (test == 1) ---- // initiallayout --> refresh() error! (remove room)
							double t = recRooms[f][i].name.Length / 2 * 0.01;
							loc = new Vector2d(graph[f][i].center - new Vector2d(1, 0) * t);
							Print2DText(loc, recRooms[f][i].name);
						}
					}
				}
			}
			//////////////// Draw connectivity graph ////////////////

			// Clip polygon
			if (clipPolys.Count > 0)
			{
				Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINE);
				Gl.glLineWidth(2.0f);
				for (i = 0; i < clipPolys.Count; i++)
				{
					Gl.glColor3f(1.0f, 0.0f, 0.0f);
					Gl.glBegin(Gl.GL_LINES);
					for (j = 0; j < clipPolys[i].Count; j++)
					{
						Gl.glVertex2d((double)clipPolys[i][j].X / 100, (double)clipPolys[i][j].Y / 100);
						Gl.glVertex2d((double)clipPolys[i][(j + 1) % clipPolys[i].Count].X / 100, (double)clipPolys[i][(j + 1) % clipPolys[i].Count].Y / 100);
					}
					Gl.glEnd();
				}
				Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);
			}
		}

		private void DrawRoom(int f, int r)
		{
			//Gl.glMatrixMode(Gl.GL_PROJECTION);
			//Gl.glPushMatrix();
			//Gl.glLoadIdentity();
			////Glu.gluOrtho2D(0, this.Width, 0, this.Height);
			//Gl.glMatrixMode(Gl.GL_MODELVIEW);
			//Gl.glPushMatrix();
			//Gl.glLoadIdentity();
			//Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);

			Gl.glColor4f(0.7f, 0.88f, 1f, 0.5f);
			Gl.glBegin(Gl.GL_POLYGON);
			//for (int i = 0; i < recRooms[f][r].vertices.Count; i++)
			//    Gl.glVertex2d(recRooms[f][r].vertices[i].Coord.x, recRooms[f][r].vertices[i].Coord.y);
			for (int i = recRooms[f][r].vertices.Count - 1; i >= 0; i--)
				Gl.glVertex2d(recRooms[f][r].vertices[i].Coord.x, recRooms[f][r].vertices[i].Coord.y);
			Gl.glEnd();

			//Gl.glMatrixMode(Gl.GL_PROJECTION);
			//Gl.glPopMatrix();
			//Gl.glMatrixMode(Gl.GL_MODELVIEW);
			//Gl.glPopMatrix();
		}

		byte[,] slabcolors = new byte[6, 4] {{253, 205, 172, 255}, { 203, 213, 232, 255 }, 
		{ 244, 202, 228, 255 },  { 228, 26, 28, 255 }, { 179, 226, 205, 255 }, {230, 245, 201, 255} };
		private void DrawWall(int label, int floor, int ind, int cind, int color) //3D
		{
			// label = 1: wall;
			// label = 2: door;
			// ind: index of wall/door
			// color = 0: original color;
			// color = 1: highlight color
			float[] colors = { 0.8f, 1.0f, 0.6f, 0.8f };
			Plane plane;

			bool withdepthtest = true;
			Gl.glShadeModel(Gl.GL_FLAT);

			//	Gl.glClearDepth(1.0f);							// Depth buffer setup 
			if (!withdepthtest)
			{
				Gl.glDisable(Gl.GL_DEPTH_TEST);
			}
			else
			{
				Gl.glEnable(Gl.GL_DEPTH_TEST);					// Enables Depth Testing             
				Gl.glDepthFunc(Gl.GL_LEQUAL);					// The Type Of Depth Test To Do     
			}
			Gl.glHint(Gl.GL_PERSPECTIVE_CORRECTION_HINT, Gl.GL_NICEST);     /* Really Nice Perspective Calculations */
			Gl.glDisable(Gl.GL_CULL_FACE);

			Gl.glEnable(Gl.GL_BLEND);
			Gl.glBlendFunc(Gl.GL_SRC_ALPHA, Gl.GL_ONE_MINUS_SRC_ALPHA);
			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_FILL);
			Gl.glColor4ub(190, 186, 218, 220);
			//Gl.glColor4ub(0, 0, 0, 255);

			if (label == 1) // Draw walls[floor] of a room
			{
				if (!cast3D)
				{
					for (int i = 0; i < walls[floor][ind].cuboid.planes.Count; i++)
					{
						plane = walls[floor][ind].cuboid.planes[i];
						Gl.glBegin(Gl.GL_POLYGON);
						for (int j = 0; j < plane.vertices.Count; j++)
							Gl.glVertex3d(plane.vertices[j].Coord.x,
							plane.vertices[j].Coord.y,
							plane.vertices[j].Coord.z);
						Gl.glEnd();
					}
					if (walls[floor][ind].casts != null)
					{
						Gl.glDepthRange(0f, 0.9998f);
						for (int i = 0; i < walls[floor][ind].casts.Count; i++)
						{
							plane = walls[floor][ind].casts[i];
							Gl.glColor4ub(255, 185, 15, 255); //Color of casts
							if (plane.castVisit)
								Gl.glColor4ub(137, 104, 205, 255);
							Gl.glBegin(Gl.GL_POLYGON);
							for (int j = 0; j < plane.vertices.Count; j++)
								Gl.glVertex3d(plane.vertices[j].Coord.x,
								plane.vertices[j].Coord.y,
								plane.vertices[j].Coord.z);
							Gl.glEnd();
						}
						Gl.glDepthRange(0.0f, 1.0f);
						int num = 4;
						// Draw boudaries
						Gl.glDepthRange(0f, 0.9997f);
						Gl.glEnable(Gl.GL_LINE_SMOOTH);
						Gl.glHint(Gl.GL_LINE_SMOOTH_HINT, Gl.GL_NICEST);
						for (int j = 0; j < walls[floor][ind].casts.Count; j++)
						{
							plane = walls[floor][ind].casts[j];
							Gl.glLineWidth(2.0f);
							Gl.glBegin(Gl.GL_LINES);
							Gl.glColor4f(0.0f, 0.0f, 0.0f, 1.0f);
							for (int i = 0; i < 4; i++)
							{
								Gl.glVertex3d(plane.vertices[i].Coord.x,
									plane.vertices[i].Coord.y,
									plane.vertices[i].Coord.z);
								Gl.glVertex3d(plane.vertices[(i + 1) % num].Coord.x,
									plane.vertices[(i + 1) % num].Coord.y,
									plane.vertices[(i + 1) % num].Coord.z);
							}
							Gl.glEnd();
						}
						Gl.glDepthRange(0.0f, 1.0f);
					}
				}
				// Draw 3D concretes
				if (cast3D && walls[floor][ind].casts != null)
				{
					for (int i = 0; i < walls[floor][ind].castPlanes.Count; i++)
					{
						if (walls[floor][ind].castType[i] == 0)
							Gl.glColor4ub(slabcolors[0,0], slabcolors[0,1], slabcolors[0,2], slabcolors[0,3]); //Color of casts
						else if (walls[floor][ind].castType[i] == 1)
							Gl.glColor4ub(slabcolors[1, 0], slabcolors[1, 1], slabcolors[1, 2], slabcolors[1, 3]);
						else if (walls[floor][ind].castType[i] == 2)
							Gl.glColor4ub(slabcolors[2, 0], slabcolors[2, 1], slabcolors[2, 2], slabcolors[2, 3]);
						else if (walls[floor][ind].castType[i] == 3) // door & window
						{
							Gl.glColor4ub(slabcolors[4, 0], slabcolors[4, 1], slabcolors[4, 2], slabcolors[4, 3]);
							if (walls[floor][ind].doors.Count > 0 && walls[floor][ind].doors[0].elevation > thresh)
								Gl.glColor4ub(slabcolors[5, 0], slabcolors[5, 1], slabcolors[5, 2], slabcolors[5, 3]);
						}
						else
							Gl.glColor4ub(slabcolors[3, 0], slabcolors[3, 1], slabcolors[3, 2], slabcolors[3, 3]);
							
						for (int k = 0; k < walls[floor][ind].castPlanes[i].Count; k++)
						{
							plane = walls[floor][ind].castPlanes[i][k];
							Gl.glBegin(Gl.GL_POLYGON);
							for (int j = 0; j < plane.vertices.Count; j++)
								Gl.glVertex3d(plane.vertices[j].Coord.x,
								plane.vertices[j].Coord.y,
								plane.vertices[j].Coord.z);
							Gl.glEnd();
						}
						for (int k = 0; k < walls[floor][ind].castPlanes[i].Count; k++)
						{
							plane = walls[floor][ind].castPlanes[i][k];
							Gl.glLineWidth(2.0f);
							Gl.glBegin(Gl.GL_LINES);
							Gl.glColor4f(0.0f, 0.0f, 0.0f, 1.0f);
							int num = 4;
							for (int j = 0; j < 4; j++)
							{
								Gl.glVertex3d(plane.vertices[j].Coord.x,
									plane.vertices[j].Coord.y,
									plane.vertices[j].Coord.z);
								Gl.glVertex3d(plane.vertices[(j + 1) % num].Coord.x,
									plane.vertices[(j + 1) % num].Coord.y,
									plane.vertices[(j + 1) % num].Coord.z);
							}
							Gl.glEnd();
						}
					}
					for (int i = walls[floor][ind].num_front_cast; i < walls[floor][ind].casts.Count; i++)
					{
						plane = walls[floor][ind].casts[i];
						Gl.glColor4ub(230, 245, 201, 255);
						Gl.glBegin(Gl.GL_POLYGON);
						for (int j = 0; j < plane.vertices.Count; j++)
							Gl.glVertex3d(plane.vertices[j].Coord.x,
							plane.vertices[j].Coord.y,
							plane.vertices[j].Coord.z);
						Gl.glEnd();
						// Draw boudaries
						Gl.glLineWidth(2.0f);
						Gl.glBegin(Gl.GL_LINES);
						Gl.glColor4f(0.0f, 0.0f, 0.0f, 1.0f);
						for (int t = 0; t < 4; t++)
						{
							Gl.glVertex3d(plane.vertices[t].Coord.x,
								plane.vertices[t].Coord.y,
								plane.vertices[t].Coord.z);
							Gl.glVertex3d(plane.vertices[(t + 1) % 4].Coord.x,
								plane.vertices[(t + 1) % 4].Coord.y,
								plane.vertices[(t + 1) % 4].Coord.z);
						}
						Gl.glEnd();
					}
				}
			}
			else if (label == 2) // Draw Independent/Single wall
			{
				for (int i = 0; i < walls[floor][ind].cuboid.planes.Count; i++)
				{
					plane = walls[floor][ind].cuboid.planes[i];
					if (color == 0)
						Gl.glColor4f(plane.color[0], plane.color[1], plane.color[2], plane.color[3]);
					else
						Gl.glColor4f(colors[0], colors[1], colors[2], colors[3]); //highlight wall
					Gl.glBegin(Gl.GL_POLYGON);
					for (int j = 0; j < plane.vertices.Count; j++)
						Gl.glVertex3d(plane.vertices[j].Coord.x,
						plane.vertices[j].Coord.y,
						plane.vertices[j].Coord.z);
					Gl.glEnd();
				}
			}
			else if (label == 3) // Draw door
			{
				if (!cast3D)
				{
					Gl.glDepthRange(0f, 0.9999f);
					for (int i = 0; i < doors[floor][ind].cuboid.planes.Count; i++)
					{
						plane = doors[floor][ind].cuboid.planes[i];
						if (doors[floor][ind].elevation > 0)
							Gl.glColor4ub(251, 180, 174, 220);
						else
							Gl.glColor4ub(204, 235, 197, 220);
						Gl.glBegin(Gl.GL_POLYGON);
						for (int j = 0; j < plane.vertices.Count; j++)
							Gl.glVertex3d(plane.vertices[j].Coord.x,
							plane.vertices[j].Coord.y,
							plane.vertices[j].Coord.z);
						Gl.glEnd();
					}
					Gl.glDepthRange(0f, 1f);
				}
			}

			if (label == 4)
			{
				Gl.glDepthRange(0f, 0.9998f);
				plane = walls[floor][ind].casts[cind];
				Gl.glColor4ub(255, 62, 150, 255); //Color of casts
				Gl.glBegin(Gl.GL_POLYGON);
				for (int j = 0; j < plane.vertices.Count; j++)
					Gl.glVertex3d(plane.vertices[j].Coord.x,
					plane.vertices[j].Coord.y,
					plane.vertices[j].Coord.z);
				Gl.glEnd();
				Gl.glDepthRange(0.0f, 1.0f);
			}

			Gl.glDisable(Gl.GL_BLEND);
			Gl.glEnable(Gl.GL_CULL_FACE);
			Gl.glEnable(Gl.GL_DEPTH_TEST);
			Gl.glDepthFunc(Gl.GL_LEQUAL);

		}
		private void DrawTexture()
		{
			Plane plane;
			Gl.glEnable(Gl.GL_BLEND);
			Gl.glBlendFunc(Gl.GL_SRC_ALPHA, Gl.GL_ONE_MINUS_SRC_ALPHA);
			Gl.glPolygonMode(Gl.GL_FRONT_AND_BACK, Gl.GL_LINES);
			Gl.glDisable(Gl.GL_CULL_FACE);
			Gl.glEnable(Gl.GL_NORMALIZE);
			for (int ind = 0; ind < walls[activeFloor].Count; ind++)
			{
				Gl.glEnable(Gl.GL_TEXTURE_2D);
				Gl.glBindTexture(Gl.GL_TEXTURE_2D, walls[activeFloor][ind].texture);
				Gl.glColor4f(1, 1, 1, 1);
				Gl.glBegin(Gl.GL_QUADS);
				plane = walls[activeFloor][ind].cuboid.planes[3];
				Gl.glTexCoord2d(1, 1);
				Gl.glVertex3d(plane.vertices[0].Coord.x,
					plane.vertices[0].Coord.y,
					plane.vertices[0].Coord.z);
				Gl.glTexCoord2d(0, 1);
				Gl.glVertex3d(plane.vertices[1].Coord.x,
						plane.vertices[1].Coord.y,
						plane.vertices[1].Coord.z);
				Gl.glTexCoord2d(0, 0);
				Gl.glVertex3d(plane.vertices[2].Coord.x,
						plane.vertices[2].Coord.y,
						plane.vertices[2].Coord.z);
				Gl.glTexCoord2d(1, 0);
				Gl.glVertex3d(plane.vertices[3].Coord.x,
						plane.vertices[3].Coord.y,
						plane.vertices[3].Coord.z);
				Gl.glEnd();
				Gl.glDisable(Gl.GL_TEXTURE_2D);

				if (walls[activeFloor][ind].casts.Count > 0)
				{
					for (int i = 0; i < walls[activeFloor][ind].casts.Count; i++)
					{
						Gl.glEnable(Gl.GL_TEXTURE_2D);
						Gl.glBindTexture(Gl.GL_TEXTURE_2D, walls[activeFloor][ind].casts[i].texture);
						Gl.glColor4f(1, 1, 1, 1);
						Gl.glBegin(Gl.GL_QUADS);
						plane = walls[activeFloor][ind].casts[i];
						Gl.glTexCoord2d(1, 1);
						Gl.glVertex3d(plane.vertices[0].Coord.x,
							plane.vertices[0].Coord.y,
							plane.vertices[0].Coord.z);
						Gl.glTexCoord2d(0, 1);
						Gl.glVertex3d(plane.vertices[1].Coord.x,
								plane.vertices[1].Coord.y,
								plane.vertices[1].Coord.z);
						Gl.glTexCoord2d(0, 0);
						Gl.glVertex3d(plane.vertices[2].Coord.x,
								plane.vertices[2].Coord.y,
								plane.vertices[2].Coord.z);
						Gl.glTexCoord2d(1, 0);
						Gl.glVertex3d(plane.vertices[3].Coord.x,
								plane.vertices[3].Coord.y,
								plane.vertices[3].Coord.z);
						Gl.glEnd();
						Gl.glDisable(Gl.GL_TEXTURE_2D);
					}
				}
			}

			for (int ind = 0; ind < doors.Count; ind++)
			{
				Gl.glEnable(Gl.GL_TEXTURE_2D);
				Gl.glBindTexture(Gl.GL_TEXTURE_2D, doors[activeFloor][ind].texture);
				Gl.glColor4f(1, 1, 1, 1);
				Gl.glBegin(Gl.GL_QUADS);
				plane = doors[activeFloor][ind].cuboid.planes[3];
				Gl.glTexCoord2d(1, 1);
				Gl.glVertex3d(plane.vertices[0].Coord.x,
					plane.vertices[0].Coord.y,
					plane.vertices[0].Coord.z);
				Gl.glTexCoord2d(0, 1);
				Gl.glVertex3d(plane.vertices[1].Coord.x,
						plane.vertices[1].Coord.y,
						plane.vertices[1].Coord.z);
				Gl.glTexCoord2d(0, 0);
				Gl.glVertex3d(plane.vertices[2].Coord.x,
						plane.vertices[2].Coord.y,
						plane.vertices[2].Coord.z);
				Gl.glTexCoord2d(1, 0);
				Gl.glVertex3d(plane.vertices[3].Coord.x,
						plane.vertices[3].Coord.y,
						plane.vertices[3].Coord.z);
				Gl.glEnd();
				Gl.glDisable(Gl.GL_TEXTURE_2D);
			}
			Gl.glEnable(Gl.GL_CULL_FACE);
			Gl.glDisable(Gl.GL_NORMALIZE);
			Gl.glDisable(Gl.GL_BLEND);
		}

		public void InitBorder()
		{
			double zCoord = 0.0d;
			Vector3d[] border = new Vector3d[4];
			double[,] borderCoord = { {0.0d, 0.0d, zCoord },
			                  { 0.0d, 2.0d, zCoord },
			                  { 2.0d, 2.0d, zCoord },
			                  { 2.0d, 0.0d, zCoord } };
			List<Vertex3> vertices = new List<Vertex3>();
			for (int i = 0; i < 4; i++)
			{
				border[i].x = borderCoord[i, 0];
				border[i].y = borderCoord[i, 1];
				border[i].z = borderCoord[i, 2];
				vertices.Add(new Vertex3(border[i]));
			}

			List<Vertex3> currVertices = new List<Vertex3>();
			for (int i = 0; i < vertices.Count; i++)
				currVertices.Add(new Vertex3(vertices[i].vertexCoord));

			List<Vertex> planeVertices = new List<Vertex>();
			List<Vertex> planeCurrVert = new List<Vertex>();
			for (int i = 0; i < 4; i++)
			{
				planeVertices.Add(new Vertex(border[i].x, border[i].y, border[i].z));
				planeCurrVert.Add(new Vertex(border[i].x, border[i].y, border[i].z));
			}

			XYPlane = new Plane(planeVertices, planeCurrVert);
		}
	}
}
