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

namespace DarkWynterEngine.GameObjects
{
    #region Using Statements
    using System;
    using Microsoft.Xna.Framework;
    using Microsoft.Xna.Framework.Graphics;
    using System.Xml;

    using Globals;
    using ObjectLib;
    #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 VertexFogBinormalTangentWeight[] instanceData;
        private VertexBuffer instanceBuffer;

        // 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 float[,] vertexHeightData;
        private Vector4[] normalData;
        private Vector4[] heightData;

        private int terrainClippingRate;
        private int modelScale = 32;

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

        // Tells the GPU how to format the streams
        private VertexElement[] Elements = new VertexElement[]
        {
            new VertexElement(0, 0, VertexElementFormat.Vector3, VertexElementMethod.Default, VertexElementUsage.Position,  0),
            new VertexElement(0, sizeof(float) * 3, VertexElementFormat.Vector3, VertexElementMethod.Default, VertexElementUsage.Normal,  0),
            new VertexElement(0, sizeof(float) * 6, VertexElementFormat.Vector2, VertexElementMethod.Default, VertexElementUsage.TextureCoordinate, 0),
            new VertexElement(1, 0, VertexElementFormat.Vector4, VertexElementMethod.Default, VertexElementUsage.Fog, 0),
            new VertexElement(1, sizeof(float) * 4, VertexElementFormat.Vector4, VertexElementMethod.Default, VertexElementUsage.Binormal, 0),
            new VertexElement(1, sizeof(float) * 8, VertexElementFormat.Vector4, VertexElementMethod.Default, VertexElementUsage.Tangent, 0),
            new VertexElement(1, sizeof(float) * 12, VertexElementFormat.Vector4, VertexElementMethod.Default, VertexElementUsage.BlendWeight, 0),
        };

        /// <summary>
        /// Constructor
        /// </summary>
        public Terrain()
        {
            mass.objectType = Enums.ObjectType.TERRAIN;
            mass.gameObjectPointer = this;
            mass.mass = 1.0f;
            mass.staticFrictionCoefficient = 0.6f;

            vertexDeclaration = new VertexDeclaration(Statics.SystemSettings.graphics.GraphicsDevice, Elements);
            instanceData = new VertexFogBinormalTangentWeight[64];
            instanceBuffer = new VertexBuffer(Statics.SystemSettings.graphics.GraphicsDevice,
                                              typeof(VertexFogBinormalTangentWeight),
                                              instanceData.Length,
                                              BufferUsage.None);
        }

        /* GameObject Interface Methods
        *  
        */
        /// <summary>
        /// Load terrain info from XML
        /// </summary>
        /// <param name="node">Terrain XML node</param>
        /// <param name="objectLibrary">ObjectLibrary</param>
        /// <returns>True if load was successful</returns>
        public override bool Load(XmlNode node, ObjectLibrary objectLibrary)
        {
            //base.Load(node, objectLibrary);

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

            // Height Map and Normal Maps
            heightMapFile = node.Attributes["heightmap"].Value;
            normalMapFile = node.Attributes["normalmap"].Value;
            heightMapTexture = Statics.SystemSettings.content.Load<Texture2D>(heightMapFile);
            ShaderParameters.heightMapTexture.SetValue(heightMapTexture);
            ShaderParameters.terrainNormalMapTexture.SetValue(normalMapTexture);

            // Set terrain x, z scaling
            //Statics.TerrainSettings.terrainScaleFactor = int.Parse(node.Attributes["scale"].Value);

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

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

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

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

            ShaderParameters.terrainModTexture.SetValue(textureList[8]);
            ShaderParameters.terrainScaleFactor.SetValue(Statics.TerrainSettings.terrainScaleFactor);

            mass.dynamicFrictionCoefficient = Statics.LevelSettings.TERRAIN_FRICTION;

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

            try
            {
                normalMapTexture = Statics.SystemSettings.content.Load<Texture2D>(normalMapFile);
                CreateTerrain();
            }
            catch
            {
                // Normal map does not exist so generate it
                CreateTerrainNNormalMap();
            }


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

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

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

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

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

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

        private void CreateTerrainNNormalMap()
        {
            normalMapTexture = heightMapTexture;

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

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

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

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

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

            // Create Vertices
            vertices = new VertexPositionNormalTexture[Statics.TerrainSettings.vertexMapSize * Statics.TerrainSettings.vertexMapSize];
            for (int x = 0; x < Statics.TerrainSettings.vertexMapSize; x++)
            {
                for (int z = 0; z < Statics.TerrainSettings.vertexMapSize; z++)
                {
                    vertexHeightData[x, z] = ((float)heightData[index].X * terrainHeightScale) + terrainScaleMinY;
                    vertices[z + x * Statics.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.TerrainSettings.vertexMapSize - 1; x++)
            {
                for (int z = 0; z < Statics.TerrainSettings.vertexMapSize - 1; z++)
                {
                    edge1 = vertices[(z + 1) + x * Statics.TerrainSettings.vertexMapSize].Position - vertices[z + x * Statics.TerrainSettings.vertexMapSize].Position;
                    edge2 = vertices[z + x * Statics.TerrainSettings.vertexMapSize].Position - vertices[(z + 1) + (x + 1) * Statics.TerrainSettings.vertexMapSize].Position;
                    normal = Vector3.Cross(edge2, edge1);
                    normal.Normalize();

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

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

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

            // Normalize vertex normals
            for (int i = 0; i < Statics.TerrainSettings.vertexMapSize * Statics.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);
        }

        /// <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;
                normalMapTexture.SetData<Vector4>(normalData);
                heightMapTexture.SetData<Vector4>(heightData);
                ShaderParameters.terrainNormalMapTexture.SetValue(normalMapTexture);
                ShaderParameters.heightMapTexture.SetValue(heightMapTexture);
            }
        }

        /// <summary>
        /// Draw the level selection screen in the Menu
        /// </summary>
        public void DrawLevelSelectionScreen()
        {
            // Set this object's global transformation matrix
            Matrix matrix = Matrix.CreateScale(new Vector3(Statics.TerrainSettings.terrainScaleFactor, 1, Statics.TerrainSettings.terrainScaleFactor)) *
                            Matrix.CreateTranslation(new Vector3(-255.0f, 0.0f, 255.0f)) *
                            Matrix.CreateRotationY(-(float)Math.PI / 2.0f);

            ShaderParameters.World.SetValue(matrix);

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

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

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

            base.DrawTriStrips(model, "TerrainNoBumpsShaderMain");
        }
        /// <summary>
        /// General draw function
        /// </summary>
        /// <param name="modelManager">Model Manager</param>
        /// <param name="technique">Draw technique to use</param>
        public override void Draw(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.Z / Statics.TerrainSettings.terrainScaleFactor - Statics.TerrainSettings.vertexMapSize) / terrainClippingRate);

            float ratio = (float)Statics.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.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));

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

            //instanceBuffer = new VertexBuffer(Statics.SystemSettings.graphics.GraphicsDevice,
            //                      typeof(VertexFogBinormalTangentWeight),
            //                      instanceData.Length,
            //                      BufferUsage.None);
            instanceBuffer.SetData<VertexFogBinormalTangentWeight>(instanceData);

            // Set the vertex declaration
            Statics.SystemSettings.graphics.GraphicsDevice.VertexDeclaration = vertexDeclaration;
            Statics.SystemSettings.graphics.GraphicsDevice.Vertices[0].SetFrequencyOfIndexData(instanceData.Length);

            // Tell the GPU how many times to run through the instance data and set the stream.
            Statics.SystemSettings.graphics.GraphicsDevice.Vertices[1].SetSource(instanceBuffer, 0, vertexDeclaration.GetVertexStrideSize(1));
            Statics.SystemSettings.graphics.GraphicsDevice.Vertices[1].SetFrequencyOfInstanceData(1);

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

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

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

            ShaderParameters.playerPosition.SetValue(new Vector2(Statics.PlayerSettings.playerPosition.X, Statics.PlayerSettings.playerPosition.Z));

            base.DrawInstancedTriStrips(model, "TerrainInstanced", vertexDeclaration.GetVertexStrideSize(0));
        }
        /// <summary>
        /// Draw function for AI-Vision
        /// </summary>
        /// <param name="currentAIPosition">Current AI's position</param>
        public void DrawAIVision(Vector2 currentAIPosition)
        {
            // Snap grid to nearest Clip boundary, relative to the player's location in vertexMapSpace
            int snapX = (int)((currentAIPosition.X / Statics.TerrainSettings.terrainScaleFactor - Statics.TerrainSettings.vertexMapSize) / terrainClippingRate);
            int snapZ = (int)((currentAIPosition.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;

            int index = 0;
            for (int x = 4; x < 12; x++)
            {
                int xPos = (x * 32) + snapX;
                for (int z = 4; z < 12; z++)
                {
                    int zPos = (z * 32) + snapZ;

                    matrix = Matrix.CreateTranslation(new Vector3(xPos, 0.0f, zPos)) *
                             Matrix.CreateScale(new Vector3(Statics.TerrainSettings.terrainScaleFactor, 1.0f, Statics.TerrainSettings.terrainScaleFactor));

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

            instanceBuffer = new VertexBuffer(Statics.SystemSettings.graphics.GraphicsDevice,
                                              typeof(VertexFogBinormalTangentWeight),
                                              instanceData.Length,
                                              BufferUsage.None);
            instanceBuffer.SetData<VertexFogBinormalTangentWeight>(instanceData);

            // Set the vertex declaration
            Statics.SystemSettings.graphics.GraphicsDevice.VertexDeclaration = vertexDeclaration;
            Statics.SystemSettings.graphics.GraphicsDevice.Vertices[0].SetFrequencyOfIndexData(instanceData.Length);

            // Tell the GPU how many times to run through the instance data and set the stream.
            Statics.SystemSettings.graphics.GraphicsDevice.Vertices[1].SetSource(instanceBuffer, 0, vertexDeclaration.GetVertexStrideSize(1));
            Statics.SystemSettings.graphics.GraphicsDevice.Vertices[1].SetFrequencyOfInstanceData(1);

            base.DrawInstancedTriStrips(model, "AI_Vision_TerrainShaderMain", vertexDeclaration.GetVertexStrideSize(0));
        }

        /// <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 modRadius)
        {
            int minX = (int)lookAtPoint.X - modRadius;
            int maxX = (int)lookAtPoint.X + modRadius;
            int minZ = (int)lookAtPoint.Z - modRadius;
            int maxZ = (int)lookAtPoint.Z + modRadius;

            int collisionMinX, collisionMaxX;
            int collisionMinZ, collisionMaxZ;

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

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

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

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

            // The Tip
            for (int x = minX; x <= maxX; x++)
            {
                for (int z = minZ; z <= maxZ; z++)
                {
                    if ((modFactor > 0 && heightData[z + x * Statics.TerrainSettings.vertexMapSize].Y < MaxHeight) ||
                        (modFactor < 0 && heightData[z + x * Statics.TerrainSettings.vertexMapSize].Y > MinHeight))
                    {
                        //vertices[z + x * vertexMapSize].Position.Y += 10 * modFactor;
                        if (x == minX || x == maxX - 1 || z == minZ || z == maxZ - 1)
                        {
                            vertices[z + x * Statics.TerrainSettings.vertexMapSize].Normal = new Vector3();
                        }

                        heightData[z + x * Statics.TerrainSettings.vertexMapSize] += new Vector4(modFactor / 25.5f);
                    }
                }
            }

            minX -= 1;
            maxX += 1;
            minZ -= 1;
            maxZ += 1;

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

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

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

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

            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.TerrainSettings.vertexMapSize].Position - vertices[z + x * Statics.TerrainSettings.vertexMapSize].Position;
                        edge2 = vertices[z + x * Statics.TerrainSettings.vertexMapSize].Position - vertices[(z + 1) + (x + 1) * Statics.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.TerrainSettings.vertexMapSize].Normal += normal;
                        vertices[(z + 1) + x * Statics.TerrainSettings.vertexMapSize].Normal += normal;
                        vertices[(z + 1) + (x + 1) * Statics.TerrainSettings.vertexMapSize].Normal += normal;

                        edge1 = vertices[z + x * Statics.TerrainSettings.vertexMapSize].Position - vertices[z + (x + 1) * Statics.TerrainSettings.vertexMapSize].Position;
                        edge2 = vertices[z + (x + 1) * Statics.TerrainSettings.vertexMapSize].Position - vertices[(z + 1) + (x + 1) * Statics.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.TerrainSettings.vertexMapSize].Normal += normal;
                        vertices[z + x * Statics.TerrainSettings.vertexMapSize].Normal += normal;
                        vertices[(z + 1) + (x + 1) * Statics.TerrainSettings.vertexMapSize].Normal += 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)
                    {
                        vertices[z + x * Statics.TerrainSettings.vertexMapSize].Normal.Normalize();
                        normalData[z + x * Statics.TerrainSettings.vertexMapSize] = new Vector4(vertices[z + x * Statics.TerrainSettings.vertexMapSize].Normal, 0.0f);
                    }
                }
            }

            //Recalculate collision map
            collisionMinX = (int)minX - 1;
            collisionMaxX = (int)maxX + 1;
            collisionMinZ = (int)minZ - 1;
            collisionMaxZ = (int)maxZ + 1;

            if (collisionMaxX >= Statics.TerrainSettings.vertexMapSize)
            {
                collisionMaxX = Statics.TerrainSettings.vertexMapSize - 2;
            }

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

            if (collisionMaxZ >= Statics.TerrainSettings.vertexMapSize)
            {
                collisionMaxZ = Statics.TerrainSettings.vertexMapSize - 2;
            }

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

            //UpdateCollisionMap(collisionMinX, collisionMaxX, collisionMinZ, collisionMaxZ);
            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.TerrainSettings.vertexMapSize - 1)
                {
                    ceilX = (int)(x + 1);
                }
                else
                {
                    ceilX = Statics.TerrainSettings.vertexMapSize - 1;
                }
            }
            else //other way
            {
                if (x < Statics.TerrainSettings.vertexMapSize - 1)
                {
                    floorX = (int)(x + 1);
                }
                else
                {
                    floorX = Statics.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.TerrainSettings.vertexMapSize - 1)
                {
                    ceilZ = (int)(z + 1);
                }
                else
                {
                    ceilZ = Statics.TerrainSettings.vertexMapSize - 1;
                }
            }
            else //other way
            {
                if (z < Statics.TerrainSettings.vertexMapSize - 1)
                {
                    floorZ = (int)(z + 1);
                }
                else
                {
                    floorZ = Statics.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.TerrainSettings.vertexMapSize - 1;
            int ceilZ = Statics.TerrainSettings.vertexMapSize - 1;

            if (x < Statics.TerrainSettings.vertexMapSize - 1)
            {
                ceilX = (int)(x + 1);
            }

            if (z < Statics.TerrainSettings.vertexMapSize - 1)
            {
                ceilZ = (int)(z + 1);
            }

            float lerpX = MathHelper.Lerp(heightData[floorZ + floorX * Statics.TerrainSettings.vertexMapSize].X,
                                          heightData[ceilZ + ceilX * Statics.TerrainSettings.vertexMapSize].X, 
                                          x - (float)floorX);

            float lerpZ = MathHelper.Lerp(heightData[floorZ + floorX * Statics.TerrainSettings.vertexMapSize].X,
                                          heightData[ceilZ + ceilX * Statics.TerrainSettings.vertexMapSize].X,
                                          z - (float)floorZ);

            float diff;

            if ((x - (float)floorX) - (z - (float)floorZ) > 0)
            {
                diff = 1 - ((x - (float)floorX) - (z - (float)floorZ));
            }
            else if ((x - (float)floorX) - (z - (float)floorZ) < 0)
            {
                diff = 1 + (x - (float)floorX) - (z - (float)floorZ);
            }
            else
            {
                diff = 0.5f;
            }

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

// ======================= OLD TERRAIN CODE =======================
/*
        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.World.SetValue(matrix);

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

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

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

            ShaderParameters.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.World.SetValue(matrix);

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

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

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

            ShaderParameters.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;
            }
        }
*/