//---------------------------------------------------------------------------------------------------------------------------------------------------
// <copyright file="Statics.cs" company="DarkWynter Studios">
//     Copyright (C)2007 DarkWynter Studios.  All rights reserved.
// </copyright>
//---------------------------------------------------------------------------------------------------------------------------------------------------
// {Contact : darkwynter.com for licensing information
//---------------------------------------------------------------------------------------------------------------------------------------------------

namespace DarkWynter.Game.GameObjects
{
    #region Using Statements
    using System;
    using System.Collections.Generic;
    using Microsoft.Xna.Framework;
    using Microsoft.Xna.Framework.Graphics;
    using System.Xml;
    using DarkWynter.Engine.Globals;
    using DarkWynter.Engine.ObjectLib;
    using DarkWynter.Stream;
    using DarkWynter.Engine.Init;
    using DarkWynter.Engine.GameObjects;
    #endregion


    /// <summary>
    /// Basic Terrain Object
    /// </summary>
    public class Terrain : GameObject
    {
        // Diffuse cofactor
        private float[] materialDiffuse = { 0.5f, 0.5f, 0.5f, 1.0f };

        // Specular cofactor
        private float[] materialSpecular = { 0.1f, 0.1f, 0.1f, 1.0f };

        /// <summary>
        /// Height Map of the terrain
        /// </summary>
        public Texture2D heightMapTexture;
        /// <summary>
        /// Normal map of the terrain
        /// </summary>
        public Texture2D normalMapTexture;

        /// <summary>
        /// Minimum height of the terrain (from XML)
        /// </summary>
        public float terrainScaleMinY;
        /// <summary>
        /// Maximum height of the terrain (from XML)
        /// </summary>
        public float terrainScaleMaxY;

        private VertexPositionNormal[] vertices;

        // Terrain scaling factor
        private float terrainHeightScale;

        // Terrain modification distance limit
        private bool terrainChanged = false;

        /// <summary>
        /// Amount of rebound force applied by the terrain on a player falling on the terrain
        /// </summary>
        public float playerSurfaceTension = 2000.0f;
        /// <summary>
        /// Amount of rebound force applied by the terrain on a prop falling on the terrain
        /// </summary>
        public float propSurfaceTension = 200.0f;
        /// <summary>
        /// Amount of rebound force applied by the terrain on a particle falling on the terrain
        /// </summary>
        public float particleSurfaceTension = 600.0f;
        private float MaxHeight = 3;
        private float MinHeight= -1.5f;
        private string heightMapFile;
        private string normalMapFile;

        private Vector4[] normalData;
        private Vector4[] heightData;

        private int terrainClippingRate;
        private int modelScale = 32;

        private TerrainNormalStuff terrainNormalGen;

        //public bool lightUpTerrainMod = true;
        //public Vector3 terrainLookAtPoint = new Vector3();
        //public float terrainModRange = 0;

        /// <summary>
        /// Constructor
        /// </summary>
        public Terrain(Load gameObjectLoader)
            : base(gameObjectLoader)
        {
            mass.objectType = Enums_Engine.ObjectType.TERRAIN;
            mass.gameObjectPointer = this;
            mass.mass = 1.0f;
            mass.staticFrictionCoefficient = 0.6f;

            draw = new Draw();
            if(Statics_Engine.TerrainSettings.terrainInstanced)
            {
                draw.drawMethod = Enums_Stream.DrawMethod.Terrain_Draw;
                if (Statics_Engine.SystemSettings.enableTerrainBumpMapping)
                {
                    draw.technique = "TerrainInstanced";
                }
                else
                {
                    draw.technique = "TerrainNoBumpsInstanced";
                }
                draw.vertexDeclaration = new VertexDeclaration(Statics_Stream.RenderSettings.graphics.GraphicsDevice, draw.FBTDElements);
                draw.instanceDataFBTD = new VertexFogBinormalTangentDepth[64];
                draw.vertexBuffer = new VertexBuffer(Statics_Stream.RenderSettings.graphics.GraphicsDevice,
                             typeof(VertexFogBinormalTangentDepth),
                             draw.instanceDataFBTD.Length,
                             BufferUsage.WriteOnly);
            }
            else
            {
                draw.drawMethod = Enums_Stream.DrawMethod.BasicGameObject_Draw;
                if (Statics_Engine.SystemSettings.enableTerrainBumpMapping)
                {
                    draw.technique = "TerrainShaderMain";
                }
                else
                {
                    draw.technique = "TerrainNoBumpsShaderMain";
                }
            }

            drawAIVision = new Draw(Enums_Stream.DrawMethod.Terrain_Draw, "AI_Vision_TerrainShaderMain");
            drawAIVision.vertexDeclaration = new VertexDeclaration(Statics_Stream.RenderSettings.graphics.GraphicsDevice, drawAIVision.FBTDElements);
            drawAIVision.instanceDataFBTD = new VertexFogBinormalTangentDepth[64];
        }

        /* GameObject Interface Methods
        *  
        */
        /// <summary>
        /// Load terrain info from XML
        /// </summary>
        /// <param name="objectLibrary">ObjectLibrary</param>
        /// <returns>True if load was successful</returns>
        public override bool Load(ObjectLibrary objectLibrary)
        {
            this.name = load.node.Attributes["name"].Value;

            // Height Map and Normal Maps
            heightMapFile = load.node.Attributes["heightmap"].Value;
            heightMapTexture = Statics_Engine.SystemSettings.content.Load<Texture2D>(heightMapFile);

            ShaderParameters.DrawFX.heightMapTexture.SetValue(heightMapTexture);

            // Set terrain Y scaling
            terrainScaleMinY = float.Parse(load.node.Attributes["terrainMinScaleY"].Value);
            terrainScaleMaxY = float.Parse(load.node.Attributes["terrainMaxScaleY"].Value);

            terrainScaleMinY *= heightMapTexture.Width * Statics_Engine.TerrainSettings.terrainScaleFactor;
            terrainScaleMaxY *= heightMapTexture.Width * Statics_Engine.TerrainSettings.terrainScaleFactor;

            ShaderParameters.DrawFX.worldMinY.SetValue(terrainScaleMinY);
            ShaderParameters.DrawFX.worldMaxY.SetValue(terrainScaleMaxY);
            terrainHeightScale = terrainScaleMaxY - terrainScaleMinY;

            ShaderParameters.TerrainNormalFX.terrainHeightScale.SetValue(terrainHeightScale);
            ShaderParameters.TerrainNormalFX.terrainScaleMinY.SetValue(terrainScaleMinY);
            ShaderParameters.TerrainNormalFX.invMapWidth.SetValue(1.0f / (float)heightMapTexture.Width);

            // TerrainLOD will Snap to new coordinates this many times across the map
            terrainClippingRate = int.Parse(load.node.Attributes["TerrainClippingRate"].Value);
            mass.dynamicFrictionCoefficient = Statics_Engine.LevelSettings.TERRAIN_FRICTION;

            ShaderParameters.DrawFX.terrainScaleFactor.SetValue(Statics_Engine.TerrainSettings.terrainScaleFactor);

            // Load Multi-texturing graphics
            draw.textureList.Clear();
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureLow"].Value));
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureMid"].Value));
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureHigh"].Value));
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureSlope"].Value));
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureBumpLow"].Value));
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureBumpMid"].Value));
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureBumpHigh"].Value));
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureBumpSlope"].Value));
            draw.textureList.Add(Statics_Engine.SystemSettings.content.Load<Texture2D>(load.node.Attributes["textureTerrainMod"].Value));

            ShaderParameters.DrawFX.terrainModTexture.SetValue(draw.textureList[8]);

            // Create Terrain Mesh for TerrainLOD Algorithm
            if (Statics_Engine.TerrainSettings.terrainInstanced)
            {
                draw.model = Statics_Engine.SystemSettings.content.Load<Model>("Content/_models/Terrain_Instanced");
            }
            else
            {
                draw.model = Statics_Engine.SystemSettings.content.Load<Model>("Content/_models/TerrainMesh2");
            }
            foreach (ModelMesh mesh in draw.model.Meshes)
            {
                for (int i = 0; i < mesh.MeshParts.Count; i++)
                {
                    // Add effect to mesh
                    mesh.MeshParts[i].Effect = ShaderParameters.DrawFX.effect;
                }
            }

            drawAIVision.model = Statics_Engine.SystemSettings.content.Load<Model>("Content/_models/Terrain_Instanced_AIVision");
            foreach (ModelMesh mesh in drawAIVision.model.Meshes)
            {
                for (int i = 0; i < mesh.MeshParts.Count; i++)
                {
                    // Add effect to mesh
                    mesh.MeshParts[i].Effect = ShaderParameters.DrawFX.effect;
                }
            }

            // Generate Normal map and store height values
            CreateTerrainNNormalMap();

            return true;
        }
        private void CreateTerrainNNormalMap()
        {
            // Initialize normal map and terrain normal generator
            terrainNormalGen = new TerrainNormalStuff(heightMapTexture.Width, heightMapTexture.Height);
            normalMapTexture = new Texture2D(Statics_Stream.RenderSettings.graphics.GraphicsDevice,
                                             heightMapTexture.Width,
                                             heightMapTexture.Height,
                                             1,
                                             TextureUsage.None,
                                             SurfaceFormat.Vector4);
    
            // Set size variables
            Statics_Engine.TerrainSettings.vertexMapSize = heightMapTexture.Height;
            ShaderParameters.DrawFX.terrainMapSize.SetValue(Statics_Engine.TerrainSettings.vertexMapSize);
            ShaderParameters.DrawFX.mapSizeXScaleFactor.SetValue(Statics_Engine.TerrainSettings.terrainScaleFactor * Statics_Engine.TerrainSettings.vertexMapSize);
            Statics_Engine.TerrainSettings.collisionMapSize = Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.terrainScaleFactor;

            // Create Vertex and Collision Height Maps
            float vertexHeightData;

            // Set texture step
            int index = 0;
            float textureStep = 1.0f / (float)Statics_Engine.TerrainSettings.vertexMapSize;

            // Set up color array
            heightData = new Vector4[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];
            heightMapTexture.GetData<Vector4>(heightData, 0, Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize);

            // Call function to generate the terrain normals
            terrainNormalGen.GenerateTerrainNormals(ref normalMapTexture, heightMapTexture);

            normalData = new Vector4[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];
            normalMapTexture.GetData<Vector4>(normalData);

            // Create Vertices
            vertices = new VertexPositionNormal[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];
            for (int x = 0; x < Statics_Engine.TerrainSettings.vertexMapSize; x++)
            {
                for (int z = 0; z < Statics_Engine.TerrainSettings.vertexMapSize; z++)
                {
                    vertexHeightData = ((float)heightData[index].X * terrainHeightScale) + terrainScaleMinY;
                    vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position = new Vector3(x, vertexHeightData, z);
                    vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal = new Vector3(normalData[index].X,
                                                                                                        normalData[index].Y,
                                                                                                        normalData[index].Z);
                    index++;
                }
            }
        }

        /// <summary>
        /// Update to see if the terrain has been modified
        /// </summary>
        /// <param name="objectLibrary">ObjectLibrary</param>
        public override void Update(ref ObjectLibrary objectLibrary)
        {
            if (terrainChanged)
            {
                terrainChanged = false;
                ShaderParameters.DrawFX.heightMapTexture.SetValue(heightMapTexture);
            }
        }

        /// <summary>
        /// Draw the level selection screen in the Menu
        /// </summary>
        public void DrawLevelSelectionScreen()
        {
            //------------------------------------------------------------------------------------------------
            // NOTE: This will not work in it's current state... This code is old and hasn't been used in ages
            //------------------------------------------------------------------------------------------------

            // Set this object's global transformation matrix
            Matrix matrix = Matrix.CreateScale(new Vector3(Statics_Engine.TerrainSettings.terrainScaleFactor, 1, Statics_Engine.TerrainSettings.terrainScaleFactor)) *
                            Matrix.CreateTranslation(new Vector3(-255.0f, 0.0f, 255.0f)) *
                            Matrix.CreateRotationY(-(float)Math.PI / 2.0f);

            ShaderParameters.DrawFX.World.SetValue(matrix);

            // Lighting
            ShaderParameters.DrawFX.shininess.SetValue(7000.0f);
            ShaderParameters.DrawFX.materialDiffuse.SetValue(materialDiffuse);
            ShaderParameters.DrawFX.materialSpecular.SetValue(materialSpecular);

            // Texture
            ShaderParameters.DrawFX.modelTexture1.SetValue(draw.textureList[0]);
            ShaderParameters.DrawFX.modelTexture2.SetValue(draw.textureList[1]);
            ShaderParameters.DrawFX.bumpTexture1.SetValue(draw.textureList[4]);
            ShaderParameters.DrawFX.bumpTexture2.SetValue(draw.textureList[5]);

            ShaderParameters.DrawFX.terrainModEnabled.SetValue(false);
            ShaderParameters.DrawFX.playerPosition.SetValue(Vector3.Zero);

            //base.DrawTriStrips("TerrainNoBumpsShaderMain");
        }

        /// <summary>
        /// General draw function
        /// </summary>
        public override Draw Draw()
        {
            if (Statics_Engine.TerrainSettings.terrainInstanced)
            {
                return DrawInstanced();
            }
            else
            {
                return DrawSimple();
            }
        }

        private Draw DrawSimple()
        {
            // Snap grid to nearest 32 boundary
            int snapX = (int)((Statics_Engine.PlayerSettings.playerPosition.X / Statics_Engine.TerrainSettings.terrainScaleFactor - 255.0f) / 4.0f);
            int snapZ = (int)((Statics_Engine.PlayerSettings.playerPosition.Z / Statics_Engine.TerrainSettings.terrainScaleFactor - 255.0f) / 4.0f);
            snapX *= 4;
            snapZ *= 4;

            // Set this object's global transformation matrix
            draw.matrix = Matrix.CreateRotationY(-(float)Math.PI / 2.0f) *
                            Matrix.CreateTranslation(new Vector3(snapX, 0.0f, snapZ)) *
                            Matrix.CreateScale(new Vector3(Statics_Engine.TerrainSettings.terrainScaleFactor, 1.0f, Statics_Engine.TerrainSettings.terrainScaleFactor));
            
            // Lighting
            ShaderParameters.DrawFX.shininess.SetValue(7000.0f);
            ShaderParameters.DrawFX.materialDiffuse.SetValue(materialDiffuse);
            ShaderParameters.DrawFX.materialSpecular.SetValue(materialSpecular);

            // Texture
            ShaderParameters.DrawFX.modelTexture1.SetValue(draw.textureList[0]);
            ShaderParameters.DrawFX.modelTexture2.SetValue(draw.textureList[1]);
            ShaderParameters.DrawFX.modelTexture3.SetValue(draw.textureList[2]);
            ShaderParameters.DrawFX.modelTexture4.SetValue(draw.textureList[3]);
            ShaderParameters.DrawFX.bumpTexture1.SetValue(draw.textureList[4]);
            ShaderParameters.DrawFX.bumpTexture2.SetValue(draw.textureList[5]);
            ShaderParameters.DrawFX.bumpTexture3.SetValue(draw.textureList[6]);
            ShaderParameters.DrawFX.bumpTexture4.SetValue(draw.textureList[7]);

            ShaderParameters.DrawFX.terrainModEnabled.SetValue(Statics_Engine.TerrainSettings.terrainModEnabled);
            ShaderParameters.DrawFX.PlaneRayIntersectionPoint.SetValue(Statics_Engine.TerrainSettings.terrainLookAtPoint);
            ShaderParameters.DrawFX.TerrainModRange.SetValue((Statics_Engine.TerrainSettings.terrainModRange + 1) * Statics_Engine.TerrainSettings.terrainScaleFactor);

            return draw;
        }

        private Draw DrawInstanced()
        {
            // Tile the terrain map around the player's current position
            TileMap(Statics_Engine.PlayerSettings.playerPosition);

            // Lighting
            ShaderParameters.DrawFX.shininess.SetValue(7000.0f);
            ShaderParameters.DrawFX.materialDiffuse.SetValue(materialDiffuse);
            ShaderParameters.DrawFX.materialSpecular.SetValue(materialSpecular);

            // Texture
            ShaderParameters.DrawFX.modelTexture1.SetValue(draw.textureList[0]);
            ShaderParameters.DrawFX.modelTexture2.SetValue(draw.textureList[1]);
            ShaderParameters.DrawFX.modelTexture3.SetValue(draw.textureList[2]);
            ShaderParameters.DrawFX.modelTexture4.SetValue(draw.textureList[3]);
            ShaderParameters.DrawFX.bumpTexture1.SetValue(draw.textureList[4]);
            ShaderParameters.DrawFX.bumpTexture2.SetValue(draw.textureList[5]);
            ShaderParameters.DrawFX.bumpTexture3.SetValue(draw.textureList[6]);
            ShaderParameters.DrawFX.bumpTexture4.SetValue(draw.textureList[7]);

            ShaderParameters.DrawFX.terrainModEnabled.SetValue(Statics_Engine.TerrainSettings.terrainModEnabled);
            ShaderParameters.DrawFX.PlaneRayIntersectionPoint.SetValue(Statics_Engine.TerrainSettings.terrainLookAtPoint);
            ShaderParameters.DrawFX.TerrainModRange.SetValue((Statics_Engine.TerrainSettings.terrainModRange + 1) * Statics_Engine.TerrainSettings.terrainScaleFactor);

            //draw.DoDraw();

            return draw;
        }
        /// <summary>
        /// Draw function for AI-Vision
        /// </summary>
        /// <param name="currentAIPosition">Current AI's position</param>
        public void DrawAIVision(Vector3 currentAIPosition)
        {
            GraphicsDevice gd = Statics_Stream.RenderSettings.graphics.GraphicsDevice;

            // Tile the terrain map around the ai's current position
            TileMap(currentAIPosition);

            //drawAIVision.DoDraw();
        }

        private void TileMap(Vector3 playerPosition)
        {
            // Snap grid to nearest Clip boundary, relative to the player's location in vertexMapSpace
            int snapX = (int)((playerPosition.X / Statics_Engine.TerrainSettings.terrainScaleFactor - Statics_Engine.TerrainSettings.vertexMapSize)
                        / terrainClippingRate);
            int snapZ = (int)((playerPosition.Z / Statics_Engine.TerrainSettings.terrainScaleFactor - Statics_Engine.TerrainSettings.vertexMapSize)
                        / terrainClippingRate);

            float ratio = (float)Statics_Engine.TerrainSettings.terrainScaleFactor / modelScale;

            // Scale back to collisionMapSize, having rounded off the Clip amount
            snapX *= terrainClippingRate;
            snapZ *= terrainClippingRate;

            // Set this object's World transformation matrix
            Matrix matrix;

            int coverEntireMap = Statics_Engine.TerrainSettings.vertexMapSize / modelScale;
            int start = coverEntireMap - 4;
            int end = coverEntireMap + 4;

            int index = 0;
            for (int x = start; x < end; x++)
            {
                int xPos = (x * modelScale) + snapX;
                for (int z = start; z < end; z++)
                {
                    int zPos = (z * modelScale) + snapZ;

                    matrix = Matrix.CreateTranslation(new Vector3(xPos, 0.0f, zPos)) *
                             Matrix.CreateScale(new Vector3(modelScale * ratio, 1.0f, modelScale * ratio));

                    draw.instanceDataFBTD[index].Fog = new Vector4(matrix.M11, matrix.M12, matrix.M13, matrix.M14);
                    draw.instanceDataFBTD[index].Binormal = new Vector4(matrix.M21, matrix.M22, matrix.M23, matrix.M24);
                    draw.instanceDataFBTD[index].Tangent = new Vector4(matrix.M31, matrix.M32, matrix.M33, matrix.M34);
                    draw.instanceDataFBTD[index].Depth = new Vector4(matrix.M41, matrix.M42, matrix.M43, matrix.M44);
                    index++;
                }
            }
        }

        /// <summary>
        /// Procedurally let the user modify the terrain height values 
        /// </summary>
        /// <param name="modFactor">Amount by which to modify the terrain</param>
        /// <param name="lookAtPoint">Target point on the terrain</param>
        /// <param name="modRadius">Radius of modification</param>
        public void ModifyTerrain(float modFactor, Vector3 lookAtPoint, int manhattanRadius, Enums_Engine.TerrainModType type)
        {
            int minX = (int)lookAtPoint.X - manhattanRadius;
            int maxX = (int)lookAtPoint.X + manhattanRadius;
            int minZ = (int)lookAtPoint.Z - manhattanRadius;
            int maxZ = (int)lookAtPoint.Z + manhattanRadius;

            if (maxX >= Statics_Engine.TerrainSettings.vertexMapSize)
            {
                maxX = Statics_Engine.TerrainSettings.vertexMapSize - 2;
            }

            if (minX < 0)
            {
                minX = 0;
            }

            if (maxZ >= Statics_Engine.TerrainSettings.vertexMapSize)
            {
                maxZ = Statics_Engine.TerrainSettings.vertexMapSize - 2;
            }

            if (minZ < 0)
            {
                minZ = 0;
            }

            Rectangle heightRect = new Rectangle(minZ, minX, maxZ - minZ + 1, maxX - minX + 1);

            List<Vector4> newHeightList = new List<Vector4>();

            // The Tip
            for (int x = minX; x <= maxX; x++)
            {
                for (int z = minZ; z <= maxZ; z++)
                {
                    if (x == minX || x == maxX - 1 || z == minZ || z == maxZ - 1)
                    {
                        vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal = new Vector3();
                    }

                    if (type == Enums_Engine.TerrainModType.ADDITIVE)
                    {
                        heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize] += new Vector4(modFactor / 25.5f);
                    }
                    if (type == Enums_Engine.TerrainModType.EQUALTO)
                    {
                        heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize] = new Vector4(modFactor / 25.5f);
                    }

                    
                    // Clamp height values so they don't exceed range..
                    if (heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize].X > 3.0f)
                    {
                        heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize].X = 3.0f;
                    }
                    if (heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Y > 3.0f)
                    {
                        heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Y = 3.0f;
                    }
                    if (heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Z > 3.0f)
                    {
                        heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Z = 3.0f;
                    }
                    if (heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize].W > 3.0f)
                    {
                        heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize].W = 3.0f;
                    }

                    newHeightList.Add(heightData[z + x * Statics_Engine.TerrainSettings.vertexMapSize]);
                    vertices[(z + 1) + x * Statics_Engine.TerrainSettings.vertexMapSize].Position.Y =
                            (newHeightList[newHeightList.Count - 1].X * terrainHeightScale) + terrainScaleMinY;
                }
            }

            if (newHeightList.Count > 0)
            {
                // Set new data
                Vector4[] newHeightData = new Vector4[newHeightList.Count];
                newHeightList.CopyTo(newHeightData);
                newHeightList.Clear();
                try
                {
                    heightMapTexture.SetData<Vector4>(0, heightRect, newHeightData, 0, newHeightData.Length, SetDataOptions.Discard);
                }
                catch (Exception e)
                {
                    //Console.Write(e.ToString());
                }
            }

            // Get grid round point to adjust normals
            minX -= 1;
            maxX += 1;
            minZ -= 1;
            maxZ += 1;

            // Check grid bounds
            if (maxX >= Statics_Engine.TerrainSettings.vertexMapSize)
            {
                maxX = Statics_Engine.TerrainSettings.vertexMapSize - 2;
            }

            if (minX < 0)
            {
                minX = 0;
            }

            if (maxZ >= Statics_Engine.TerrainSettings.vertexMapSize)
            {
                maxZ = Statics_Engine.TerrainSettings.vertexMapSize - 2;
            }

            if (minZ < 0)
            {
                minZ = 0;
            }

            // Calculate normal
            Vector3 edge1, edge2, normal;
            for (int x = minX; x < maxX; x++)
            {
                for (int z = minZ; z < maxZ; z++)
                {
                    if (x == minX || x == maxX - 1 || z == minZ || z == maxZ - 1)
                    {
                        edge1 = vertices[(z + 1) + x * Statics_Engine.TerrainSettings.vertexMapSize].Position - 
                                vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position;
                        edge2 = vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position - 
                                vertices[(z + 1) + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Position;
                        normal = Vector3.Cross(edge2, edge1);       // normal = Vector3.Cross(edge1, edge2); --> To play the NAN-mine game:-)
                        normal.Normalize();

                        vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                        vertices[(z + 1) + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                        vertices[(z + 1) + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;

                        edge1 = vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position - 
                                vertices[z + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Position;
                        edge2 = vertices[z + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Position - 
                                vertices[(z + 1) + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Position;
                        normal = Vector3.Cross(edge2, edge1);       // normal = Vector3.Cross(edge1, edge2); --> To play the NAN-mine game:-)
                        normal.Normalize();

                        vertices[z + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                        vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                        vertices[(z + 1) + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                    }
                }
            }

            // Normalize
            for (int x = minX; x <= maxX; x++)
            {
                for (int z = minZ; z <= maxZ; z++)
                {
                    if (x == minX || x == maxX - 1 || z == minZ || z == maxZ - 1)
                    {
                        vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal.Normalize();
                    }
                }
            }

            //Rectangle normalRect = new Rectangle(minX, minZ, maxX - minX + 1, maxZ - minZ + 1);
            //normalMapTexture.SetData<Vector4>(0, normalRect, normalData, 0, normalData.Length, SetDataOptions.Discard);

            terrainChanged = true;
        }

        /// <summary>
        /// Returns the normal of the terrain between point (x, z) and the next point that the object is going to land on
        /// based on the velocity
        /// </summary>
        /// <param name="x">X</param>
        /// <param name="z">Z</param>
        /// <param name="velocity">Object's velocity</param>
        /// <returns>Normal of the terrain</returns>
        public Vector3 GetTerrainNormal(float x, float z, Vector3 velocity)
        {
            int floorX = (int)x;
            int floorZ = (int)z;

            int ceilX = 0;
            int ceilZ = 0;

            velocity.Normalize();

            if (velocity.X < 0)
            {
                if (x > 0)
                {
                    floorX = (int)(x - 1);
                }
                else
                {
                    floorX = 0;
                }

                if (x < Statics_Engine.TerrainSettings.vertexMapSize - 1)
                {
                    ceilX = (int)(x + 1);
                }
                else
                {
                    ceilX = Statics_Engine.TerrainSettings.vertexMapSize - 1;
                }
            }
            else //other way
            {
                if (x < Statics_Engine.TerrainSettings.vertexMapSize - 1)
                {
                    floorX = (int)(x + 1);
                }
                else
                {
                    floorX = Statics_Engine.TerrainSettings.vertexMapSize - 1;
                }

                if (x > 0)
                {
                    ceilX = (int)(x - 1);
                }
                else
                {
                    ceilX = 0;
                }
            }

            if (velocity.Z < 0)
            {
                if (z > 0)
                {
                    floorZ = (int)(z - 1);
                }
                else
                {
                    floorZ = 0;
                }

                if (z < Statics_Engine.TerrainSettings.vertexMapSize - 1)
                {
                    ceilZ = (int)(z + 1);
                }
                else
                {
                    ceilZ = Statics_Engine.TerrainSettings.vertexMapSize - 1;
                }
            }
            else //other way
            {
                if (z < Statics_Engine.TerrainSettings.vertexMapSize - 1)
                {
                    floorZ = (int)(z + 1);
                }
                else
                {
                    floorZ = Statics_Engine.TerrainSettings.vertexMapSize - 1;
                }

                if (z > 0)
                {
                    ceilZ = (int)(z - 1);
                }
                else
                {
                    ceilZ = 0;
                }
            }

            Vector3 A = new Vector3(floorX, GetTerrainHeight(floorX, floorZ), floorZ);
            Vector3 B = new Vector3(ceilX, GetTerrainHeight(ceilX, ceilZ), ceilZ);
            Vector3 C;
            if (GetTerrainHeight(floorX, ceilZ) > GetTerrainHeight(ceilX, floorZ))
            {
                C = new Vector3(floorX, GetTerrainHeight(floorX, ceilZ), ceilZ);
            }
            else
            {
                C = new Vector3(ceilX, GetTerrainHeight(ceilX, floorZ), floorZ);
            }

            Vector3 vec1 = B - A;
            Vector3 vec2 = C - A;

            Vector3 result = Vector3.Cross(vec1, vec2);
            result.Normalize();
            if (float.IsNaN(result.X) || float.IsNaN(result.Y) || float.IsNaN(result.Z))
            {
                result = Vector3.Zero;
            }
            if (result.Y < 0)
            {
                //can never be pointing down!!!!
                result = -result;
            }
            return result;
        }
        /// <summary>
        /// Returns the height of the terrain at a particular x, z point
        /// </summary>
        /// <param name="x">X</param>
        /// <param name="z">Z</param>
        /// <returns>Height of the terrain</returns>
        public float GetTerrainHeight(float x, float z)
        {
            int floorX = (int)x;
            int floorZ = (int)z;

            int ceilX = Statics_Engine.TerrainSettings.vertexMapSize - 1;
            int ceilZ = Statics_Engine.TerrainSettings.vertexMapSize - 1;

            // Check if X is less than our map size
            if (x < ceilX)
            {
                ceilX = (int)(x + 1);
            }

            // Check if Z is less than our map size
            if (z < ceilZ)
            {
                ceilZ = (int)(z + 1);
            }

            // Get the fractional components
            float fracX = x - (float)floorX;
            float fracZ = z - (float)floorZ;

            // Get height value using X to lerp
            float lerpX = MathHelper.Lerp(heightData[floorZ + floorX * Statics_Engine.TerrainSettings.vertexMapSize].X,
                                          heightData[ceilZ + ceilX * Statics_Engine.TerrainSettings.vertexMapSize].X,
                                          fracX);

            // Get height value using Z to lerp
            float lerpZ = MathHelper.Lerp(heightData[floorZ + floorX * Statics_Engine.TerrainSettings.vertexMapSize].X,
                                          heightData[ceilZ + ceilX * Statics_Engine.TerrainSettings.vertexMapSize].X,
                                          fracZ);

            float diff;
            // Get the difference between the two fractions
            float diffXZ = (x - (float)floorX) - (z - (float)floorZ);

            if (diffXZ > 0)
            {
                // X fraction is greater
                diff = 1 - diffXZ;
            }
            else if (diffXZ < 0)
            {
                // Z fraction is greater
                diff = 1 + diffXZ;
            }
            else
            {
                // They are equal
                diff = 0.5f;
            }

            return (MathHelper.Lerp(lerpX, lerpZ, diff) * terrainHeightScale) + terrainScaleMinY;
        } 
    }
}

// ======================= OLD TERRAIN CODE =======================
/*
        private void CreateTerrainNNormalMap()
        {
            normalMapTexture = new Texture2D(Statics_Stream.RenderSettings.graphics.GraphicsDevice, 
                                             heightMapTexture.Width, 
                                             heightMapTexture.Height, 
                                             0, 
                                             TextureUsage.AutoGenerateMipMap, 
                                             SurfaceFormat.Vector4);

            // Set size variables
            Statics_Engine.TerrainSettings.vertexMapSize = heightMapTexture.Height;
            ShaderParameters.DrawFX.terrainMapSize.SetValue(Statics_Engine.TerrainSettings.vertexMapSize);
            ShaderParameters.DrawFX.mapSizeXScaleFactor.SetValue(Statics_Engine.TerrainSettings.terrainScaleFactor * Statics_Engine.TerrainSettings.vertexMapSize);
            Statics_Engine.TerrainSettings.collisionMapSize = Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.terrainScaleFactor;

            // Create Vertex and Collision Height Maps
            vertexHeightData = null;
            vertexHeightData = new float[Statics_Engine.TerrainSettings.vertexMapSize, Statics_Engine.TerrainSettings.vertexMapSize];
            GC.Collect();

            // Set texture step
            int index = 0;
            float textureStep = 1.0f / (float)Statics_Engine.TerrainSettings.vertexMapSize;

            // Set up color array
            heightData = new Vector4[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];
            heightMapTexture.GetData<Vector4>(heightData, 0, Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize);

            normalData = new Vector4[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];

            // Create Vertices
            vertices = new VertexPositionNormalTexture[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];
            for (int x = 0; x < Statics_Engine.TerrainSettings.vertexMapSize; x++)
            {
                for (int z = 0; z < Statics_Engine.TerrainSettings.vertexMapSize; z++)
                {
                    vertexHeightData[x, z] = ((float)heightData[index].X * terrainHeightScale) + terrainScaleMinY;
                    vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position = new Vector3(x, vertexHeightData[x, z], z);
                    index++;
                }
            }

            Vector3 normal = new Vector3();
            Vector3 edge1 = new Vector3();
            Vector3 edge2 = new Vector3();

            // Stitch the verteces together into triangles. Two triangles per square
            for (int x = 0; x < Statics_Engine.TerrainSettings.vertexMapSize - 1; x++)
            {
                for (int z = 0; z < Statics_Engine.TerrainSettings.vertexMapSize - 1; z++)
                {
                    edge1 = vertices[(z + 1) + x * Statics_Engine.TerrainSettings.vertexMapSize].Position - 
                            vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position;
                    edge2 = vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position - 
                            vertices[(z + 1) + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Position;
                    normal = Vector3.Cross(edge2, edge1);
                    normal.Normalize();

                    vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                    vertices[(z + 1) + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                    vertices[(z + 1) + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;

                    edge1 = vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position - 
                            vertices[z + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Position;
                    edge2 = vertices[z + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Position - 
                            vertices[(z + 1) + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Position;
                    normal = Vector3.Cross(edge2, edge1);
                    normal.Normalize();

                    vertices[z + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                    vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                    vertices[(z + 1) + (x + 1) * Statics_Engine.TerrainSettings.vertexMapSize].Normal += normal;
                }
            }

            // Normalize vertex normals
            for (int i = 0; i < Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize; i++)
            {
                vertices[i].Normal.Normalize();
                normalData[i] = new Vector4(vertices[i].Normal, 1.0f);
            }

            // Set the data
            normalMapTexture.SetData<Vector4>(normalData);
            normalMapTexture.Save("terrainNormalMap.dds", ImageFileFormat.Dds);
        }

        private void CreateTerrain()
        {
            // Set size variables
            Statics_Engine.TerrainSettings.vertexMapSize = heightMapTexture.Height;
            ShaderParameters.DrawFX.terrainMapSize.SetValue(Statics_Engine.TerrainSettings.vertexMapSize);
            ShaderParameters.DrawFX.mapSizeXScaleFactor.SetValue(Statics_Engine.TerrainSettings.terrainScaleFactor * Statics_Engine.TerrainSettings.vertexMapSize);
            Statics_Engine.TerrainSettings.collisionMapSize = Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.terrainScaleFactor;

            // Create Vertex and Collision Height Maps
            float vertexHeightData;

            // Set texture step
            int index = 0;
            float textureStep = 1.0f / (float)Statics_Engine.TerrainSettings.vertexMapSize;

            // Set up color array
            heightData = new Vector4[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];
            heightMapTexture.GetData<Vector4>(heightData, 0, Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize);

            normalData = new Vector4[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];
            normalMapTexture.GetData<Vector4>(normalData, 0, Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize);

            // Create Vertices
            vertices = new VertexPositionNormalTexture[Statics_Engine.TerrainSettings.vertexMapSize * Statics_Engine.TerrainSettings.vertexMapSize];
            for (int x = 0; x < Statics_Engine.TerrainSettings.vertexMapSize; x++)
            {
                for (int z = 0; z < Statics_Engine.TerrainSettings.vertexMapSize; z++)
                {
                    vertexHeightData = ((float)heightData[index].X * terrainHeightScale) + terrainScaleMinY;
                    vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Position = new Vector3(x, vertexHeightData, z);
                    vertices[z + x * Statics_Engine.TerrainSettings.vertexMapSize].Normal = Vector3.Normalize(new Vector3(normalData[index].X, 
                                                                                                                          normalData[index].Y, 
                                                                                                                          normalData[index].Z));
                    index++;
                }
            }
        }

        public void DrawRotated(ModelManager modelManager, string technique)
        {
            // Snap grid to nearest Clip boundary, relative to the player's location in vertexMapSpace
            int snapX = (int)((Statics.PlayerSettings.playerPosition.X / Statics.TerrainSettings.terrainScaleFactor - Statics.TerrainSettings.vertexMapSize) / TerrainClippingRate);
            int snapZ = (int)((Statics.PlayerSettings.playerPosition.Y / Statics.TerrainSettings.terrainScaleFactor - Statics.TerrainSettings.vertexMapSize) / TerrainClippingRate);

            // Scale back to collisionMapSize, having rounded off the Clip amount
            snapX *= TerrainClippingRate;
            snapZ *= TerrainClippingRate;

            float xRot, zRot;

            Vector3 xVector = new Vector3(Statics.PlayerSettings.playerLookVector.X, 0.0f, 0.0f);
            Vector3 zVector = new Vector3(0.0f, 0.0f, Statics.PlayerSettings.playerLookVector.Y);

            xRot = (float)Math.Acos(Vector3.Dot(xVector, Vector3.UnitX) / (xVector.Length() * Vector3.UnitX.Length()));

            if (float.IsNaN(xRot))
            {
                xRot = 0.0f;
            }

            zRot = (float)Math.Acos(Vector3.Dot(zVector, Vector3.UnitZ) / (zVector.Length() * Vector3.UnitZ.Length()));

            if (float.IsNaN(zRot))
            {
                zRot = 0.0f;
            }

            // Set this object's global transformation matrix
            Matrix matrix = Matrix.CreateTranslation(new Vector3(snapX, 0.0f, snapZ)) *
                            Matrix.CreateScale(new Vector3(Statics.TerrainSettings.terrainScaleFactor, 1.0f, Statics.TerrainSettings.terrainScaleFactor));
            //*
            //Matrix.CreateRotationX(xRot) *
            //Matrix.CreateRotationZ(zRot);

            // Set light shading parameters
            ShaderParameters.DrawFX.World.SetValue(matrix);

            // Lighting
            ShaderParameters.DrawFX.shininess.SetValue(7000.0f);
            ShaderParameters.DrawFX.materialDiffuse.SetValue(materialDiffuse);
            ShaderParameters.DrawFX.materialSpecular.SetValue(materialSpecular);

            // Texture
            ShaderParameters.DrawFX.modelTexture1.SetValue(textureList[0]);
            ShaderParameters.DrawFX.modelTexture2.SetValue(textureList[1]);
            ShaderParameters.DrawFX.bumpTexture1.SetValue(textureList[4]);
            ShaderParameters.DrawFX.bumpTexture2.SetValue(textureList[5]);
            ShaderParameters.DrawFX.modelTexture3.SetValue(textureList[2]);
            ShaderParameters.DrawFX.modelTexture4.SetValue(textureList[3]);
            ShaderParameters.DrawFX.bumpTexture3.SetValue(textureList[6]);
            ShaderParameters.DrawFX.bumpTexture4.SetValue(textureList[7]);

            ShaderParameters.DrawFX.terrainModEnabled.SetValue(Statics.TerrainSettings.terrainModEnabled);
            ShaderParameters.PlaneRayIntersectionPoint.SetValue(Statics.TerrainSettings.terrainLookAtPoint);
            ShaderParameters.TerrainModRange.SetValue((Statics.TerrainSettings.terrainModRange + 1) * Statics.TerrainSettings.terrainScaleFactor);

            ShaderParameters.DrawFX.playerPosition.SetValue(Statics.PlayerSettings.playerPosition);

            if (Statics.SystemSettings.enableTerrainBumpMapping)
            {
                base.DrawTriStrips(model, "TerrainShaderMain");
            }
            else
            {
                base.DrawTriStrips(model, "TerrainNoBumpsShaderMain");
            }
        }
        public void OldDraw(ModelManager modelManager, string technique)
        {
            // Snap grid to nearest Clip boundary, relative to the player's location in vertexMapSpace
            int snapX = (int)((Statics.PlayerSettings.playerPosition.X / Statics.TerrainSettings.terrainScaleFactor - Statics.TerrainSettings.vertexMapSize) / TerrainClippingRate);
            int snapZ = (int)((Statics.PlayerSettings.playerPosition.Y / Statics.TerrainSettings.terrainScaleFactor - Statics.TerrainSettings.vertexMapSize) / TerrainClippingRate);

            // Scale back to collisionMapSize, having rounded off the Clip amount
            snapX *= TerrainClippingRate;
            snapZ *= TerrainClippingRate;


            // Set this object's global transformation matrix
            Matrix matrix = Matrix.CreateRotationY(-(float)Math.PI / 2.0f) *
                            Matrix.CreateTranslation(new Vector3(snapX, 0.0f, snapZ)) *
                            Matrix.CreateScale(new Vector3(Statics.TerrainSettings.terrainScaleFactor, 1.0f, Statics.TerrainSettings.terrainScaleFactor));


            // Set light shading parameters
            ShaderParameters.DrawFX.World.SetValue(matrix);

            // Lighting
            ShaderParameters.DrawFX.shininess.SetValue(7000.0f);
            ShaderParameters.DrawFX.materialDiffuse.SetValue(materialDiffuse);
            ShaderParameters.DrawFX.materialSpecular.SetValue(materialSpecular);

            // Texture
            ShaderParameters.DrawFX.modelTexture1.SetValue(textureList[0]);
            ShaderParameters.DrawFX.modelTexture2.SetValue(textureList[1]);
            ShaderParameters.DrawFX.bumpTexture1.SetValue(textureList[4]);
            ShaderParameters.DrawFX.bumpTexture2.SetValue(textureList[5]);
            ShaderParameters.DrawFX.modelTexture3.SetValue(textureList[2]);
            ShaderParameters.DrawFX.modelTexture4.SetValue(textureList[3]);
            ShaderParameters.DrawFX.bumpTexture3.SetValue(textureList[6]);
            ShaderParameters.DrawFX.bumpTexture4.SetValue(textureList[7]);

            ShaderParameters.DrawFX.terrainModEnabled.SetValue(Statics.TerrainSettings.terrainModEnabled);
            ShaderParameters.PlaneRayIntersectionPoint.SetValue(Statics.TerrainSettings.terrainLookAtPoint);
            ShaderParameters.TerrainModRange.SetValue((Statics.TerrainSettings.terrainModRange + 1) * Statics.TerrainSettings.terrainScaleFactor);

            ShaderParameters.DrawFX.playerPosition.SetValue(Statics.PlayerSettings.playerPosition );

            base.DrawTriStrips(model, technique);
        }

// Procedurally generate terrain from heightmap and scale
private void CreateTerrain_CPU(GraphicsDeviceManager graphics)
{
    // Set size variables
    vertexMapSize = heightMapTexture.Height;
    collisionMapSize = vertexMapSize * terrainScaleFactor;

    // Create Vertex and Collision Height Maps
    vertexHeightData = new float[vertexMapSize, vertexMapSize];
    collisionHeightData = new float[collisionMapSize, collisionMapSize];

    // Set texture step
    int index = 0;
    float textureStep = 1.0f / (float)vertexMapSize;

    // Set up color array
    Color[] data = new Color[vertexMapSize * vertexMapSize];
    heightMapTexture.GetData<Color>(data);

    // Create Vertex Declaration and Buffer
    vertexDeclaration = new VertexDeclaration(graphics.GraphicsDevice,
                                              VertexPositionNormalTexture.VertexElements);
    vertexBuffer = new VertexBuffer(graphics.GraphicsDevice,
                                    VertexPositionNormalTexture.SizeInBytes * vertexMapSize * vertexMapSize,
                                    ResourceUsage.Dynamic,
                                    ResourceManagementMode.Manual);


    // Create Vertecies
    vertices = new VertexPositionNormalTexture[vertexMapSize * vertexMapSize];
    for (int x = 0; x < vertexMapSize; x++)
    {
        for (int z = 0; z < vertexMapSize; z++)
        {
            vertexHeightData[x, z] = (float)data[index].R * terrainHeightScale;
            vertices[z + x * vertexMapSize].Position = new Vector3(x, vertexHeightData[x, z], z);
            vertices[z + x * vertexMapSize].Normal = new Vector3(0, 0, 0);
            vertices[z + x * vertexMapSize].TextureCoordinate = new Vector2(x * textureStep, z * textureStep);
            index++;

            // Fill collision array with known height values from vertices
            collisionHeightData[x * terrainScaleFactor, z * terrainScaleFactor] = vertexHeightData[x, z];
        }
    }

    // Holds the order in which triangles are created from vertex buffer
    meshIndexBuffer = new IndexBuffer(graphics.GraphicsDevice,
                                      sizeof(int) * ((vertexMapSize - 1) * (vertexMapSize - 1) * 6),
                                      ResourceUsage.Dynamic,
                                      ResourceManagementMode.Manual,
                                      IndexElementSize.ThirtyTwoBits);

    meshIndices = new int[(vertexMapSize - 1) * (vertexMapSize - 1) * 6];
    Vector3 normal = new Vector3();
    Vector3 edge1 = new Vector3();
    Vector3 edge2 = new Vector3();

    // Stitch the verteces together into triangles. Two triangles per square
    for (int x = 0; x < vertexMapSize - 1; x++)
    {
        for (int z = 0; z < vertexMapSize - 1; z++)
        {
            meshIndices[(z + x * (vertexMapSize - 1)) * 6] = (z + x * vertexMapSize);
            meshIndices[(z + x * (vertexMapSize - 1)) * 6 + 1] = ((z + 1) + x * vertexMapSize);
            meshIndices[(z + x * (vertexMapSize - 1)) * 6 + 2] = ((z + 1) + (x + 1) * vertexMapSize);

            edge1 = vertices[(z + 1) + x * vertexMapSize].Position - vertices[z + x * vertexMapSize].Position;
            edge2 = vertices[z + x * vertexMapSize].Position - vertices[(z + 1) + (x + 1) * vertexMapSize].Position;
            normal = Vector3.Cross(edge2, edge1);
            normal.Normalize();

            vertices[z + x * vertexMapSize].Normal += normal;
            vertices[(z + 1) + x * vertexMapSize].Normal += normal;
            vertices[(z + 1) + (x + 1) * vertexMapSize].Normal += normal;

            meshIndices[(z + x * (vertexMapSize - 1)) * 6 + 3] = (z + (x + 1) * vertexMapSize);
            meshIndices[(z + x * (vertexMapSize - 1)) * 6 + 4] = (z + x * vertexMapSize);
            meshIndices[(z + x * (vertexMapSize - 1)) * 6 + 5] = ((z + 1) + (x + 1) * vertexMapSize);

            edge1 = vertices[z + x * vertexMapSize].Position - vertices[z + (x + 1) * vertexMapSize].Position;
            edge2 = vertices[z + (x + 1) * vertexMapSize].Position - vertices[(z + 1) + (x + 1) * vertexMapSize].Position;
            normal = Vector3.Cross(edge2, edge1);
            normal.Normalize();

            vertices[z + (x + 1) * vertexMapSize].Normal += normal;
            vertices[z + x * vertexMapSize].Normal += normal;
            vertices[(z + 1) + (x + 1) * vertexMapSize].Normal += normal;
        }
    }

    // Normalize vertex normals
    for (int i = 0; i < vertexMapSize * vertexMapSize; i++)
    {
        vertices[i].Normal.Normalize();
    }

    // Set the data
    vertexBuffer.SetData<VertexPositionNormalTexture>(vertices);
    meshIndexBuffer.SetData<int>(meshIndices);
}
public void GenerateCollisionMap()
{
    // Interpolate the missing values between scaled vertex values for smoother collision go through each block 
    for (int blockX = 0; blockX < vertexMapSize; blockX++)
    {
        for (int blockZ = 0; blockZ < vertexMapSize; blockZ++)
        {
            float diffx = 0;
            float diffz = 0;

            // This crap is b/c we need to avoid array out of bounds exception
            if ((blockX != (vertexMapSize - 1)) && (blockZ != (vertexMapSize - 1)))
            {
                diffx = vertexHeightData[blockX + 1, blockZ] - vertexHeightData[blockX, blockZ];
                diffz = vertexHeightData[blockX, blockZ + 1] - vertexHeightData[blockX, blockZ];
            }
            else if ((blockX == (vertexMapSize - 1)) && (blockZ == (vertexMapSize - 1)))
            {
                // Leave 0
            }
            else if (blockX == vertexMapSize - 1)
            {
                // Calculate difference between height values from base point to next point in array
                diffz = vertexHeightData[blockX, blockZ + 1] - vertexHeightData[blockX, blockZ];
            }
            else if (blockZ == vertexMapSize - 1)
            {
                // Calculate difference between height values from base point to next point in array
                diffx = vertexHeightData[blockX + 1, blockZ] - vertexHeightData[blockX, blockZ];
            }

            // Moving on, For each empty block in array
            for (int x = 0; x < terrainScaleFactor; x++)
            {
                for (int z = 0; z < terrainScaleFactor; z++)
                {
                    // Distance from base position to current collision point
                    float distx = 1.0f * x / terrainScaleFactor;
                    float distz = 1.0f * z / terrainScaleFactor;

                    // Calc height of new value
                    float interpValueX = vertexHeightData[blockX, blockZ] + distx * diffx;
                    float interpValueZ = vertexHeightData[blockX, blockZ] + distz * diffz;

                    collisionHeightData[blockX * terrainScaleFactor + x,
                                        blockZ * terrainScaleFactor + z] = (interpValueX + interpValueZ) / 2;
                }
            }
        }
    }
}
private void UpdateCollisionMap(int minX, int maxX, int minZ, int maxZ)
{
    for (int blockX = minX; blockX < maxX; blockX++)
    {
        for (int blockZ = minZ; blockZ < maxZ; blockZ++)
        {
            float diffx = 0;
            float diffz = 0;

            // Calculate difference between height values from base point to next point in array
            diffx = vertices[blockZ + (blockX + 1) * vertexMapSize].Position.Y - vertices[blockZ + blockX * vertexMapSize].Position.Y;
            diffz = vertices[(blockZ + 1) + blockX * vertexMapSize].Position.Y - vertices[blockZ + blockX * vertexMapSize].Position.Y;

            // Moving on, For each empty block in array
            for (int x = 0; x < terrainScaleFactor; x++)
            {
                for (int z = 0; z < terrainScaleFactor; z++)
                {
                    // Distance from base position to current collision point
                    float distx = 1.0f * x / terrainScaleFactor;
                    float distz = 1.0f * z / terrainScaleFactor;

                    // Calc height of new value
                    float interpValueX = vertices[blockZ + blockX * vertexMapSize].Position.Y + distx * diffx;
                    float interpValueZ = vertices[blockZ + blockX * vertexMapSize].Position.Y + distz * diffz;

                    //collisionHeightData[blockX * terrainScaleFactor + x,
                    //                    blockZ * terrainScaleFactor + z] = (interpValueX + interpValueZ) / 2;
                }
            }
        }
    }
}

        public void ModifyTerrain(float modFactor, Vector3 lookAtPoint)
        {
            // TERRAIN OBJECT
            Vector3 normal = new Vector3();
            Vector3 edge1 = new Vector3();
            Vector3 edge2 = new Vector3();

            int minX, maxX, minZ, maxZ;

            // Change the height value and renormalize vertex normals in surrounding area
            if (lookAtPoint.X >= 0 && lookAtPoint.Z >= 0)
            {
                vertices[((int)lookAtPoint.Z + (int)lookAtPoint.X * vertexMapSize)].Position.Y += 10 * modFactor;

                vertices[((int)lookAtPoint.Z + (int)lookAtPoint.X * vertexMapSize)].Normal = new Vector3();

                minX = (int)lookAtPoint.X - 1;
                maxX = (int)lookAtPoint.X + 1;
                minZ = (int)lookAtPoint.Z - 1;
                maxZ = (int)lookAtPoint.Z + 1;

                if (maxX >= vertexMapSize)
                {
                    maxX = vertexMapSize - 2;
                }

                if (minX < 0)
                {
                    minX = 0;
                }

                if (maxZ >= vertexMapSize)
                {
                    maxZ = vertexMapSize - 2;
                }

                if (minZ < 0)
                {
                    minZ = 0;
                }

                //UpdateCollisionMap(minX, maxX, minZ, maxZ);

                // Renormalize normals
                for (int x = minX; x < maxX; x++)
                {
                    for (int z = minZ; z < maxZ; z++)
                    {
                        if ((x > lookAtPoint.X && z < lookAtPoint.Z) || (x < lookAtPoint.X && z > lookAtPoint.Z))
                        {
                            continue;
                        }

                        edge1 = vertices[(z + 1) + x * vertexMapSize].Position - vertices[z + x * vertexMapSize].Position;
                        edge2 = vertices[z + x * vertexMapSize].Position - vertices[(z + 1) + (x + 1) * vertexMapSize].Position;
                        normal = Vector3.Cross(edge2, edge1);       // normal = Vector3.Cross(edge1, edge2); --> To play the NAN-mine game:-)
                        normal.Normalize();

                        vertices[z + x * vertexMapSize].Normal += normal;
                        vertices[(z + 1) + x * vertexMapSize].Normal += normal;
                        vertices[(z + 1) + (x + 1) * vertexMapSize].Normal += normal;

                        edge1 = vertices[z + x * vertexMapSize].Position - vertices[z + (x + 1) * vertexMapSize].Position;
                        edge2 = vertices[z + (x + 1) * vertexMapSize].Position - vertices[(z + 1) + (x + 1) * vertexMapSize].Position;
                        normal = Vector3.Cross(edge2, edge1);       // normal = Vector3.Cross(edge1, edge2); --> To play the NAN-mine game:-)
                        normal.Normalize();

                        vertices[z + (x + 1) * vertexMapSize].Normal += normal;
                        vertices[z + x * vertexMapSize].Normal += normal;
                        vertices[(z + 1) + (x + 1) * vertexMapSize].Normal += normal;
                    }
                }

                for (int x = minX; x <= maxX; x++)
                {
                    for (int z = minZ; z <= maxZ; z++)
                    {
                        vertices[z + x * vertexMapSize].Normal.Normalize();
                    }
                }
                
                terrainChanged = true;
            }
        }
*/