using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VisiMath;
using VisiMath.Geometry;
using VisimBox.Scene;
using VisimBox;
using VisimBox.Renderer;
using VisimBox.Renderer.RenderObjects;
using System.Diagnostics;
using VisimBox.Physics;
using System.Windows.Forms;

namespace MarsRover
{

    public static class TerrainConstructor
    {
        public static Node ConstructTerrain()
        {
            Node MarsTerrain = new Node();
            GeoHeightMap TerrainMap;
            int TerrainSize;




            Stopwatch watch = new Stopwatch();
            watch.Start();

            bool BleedBouldersToOne = false;

            int totalboulders = 50;
            int BoulderUniqueCount = totalboulders / 10 + 1;
            TerrainSize = 256;


            IndexedMeshBuilder<TexturedVertex> meshbuilder = null;

            if (BleedBouldersToOne)
            {
                meshbuilder = new IndexedMeshBuilder<TexturedVertex>(TerrainSize * TerrainSize + totalboulders * 500, 1, TerrainSize * TerrainSize * 4 + totalboulders * 1000);
            }


            Random rgen = new Random(10);

            IRenderMaterial material;
            Mesh mesh;

            MeshManager meshman = VisimEngine.Instance.Renderer.MeshMan;
            MaterialManager matman = VisimEngine.Instance.Renderer.MaterialMan;

            StandardColorValues colorvals = StandardColorValues.Default();
            colorvals.Ambient = colorvals.Diffuse = new ColorByte(255, 89, 10);

            material = matman.Add("MarsGround", "Textures/Ground/Mars.jpg", ref colorvals);

            GeoHeightMap marsmap = new GeoHeightMap(rgen, TerrainSize, TerrainSize);
            TerrainMap = marsmap;

            marsmap.Position = new Vector3(-TerrainSize / 2, -TerrainSize / 2, 0);
            marsmap.TextureScale = new Vector2(0.05f, 0.05f);

            double time1 = watch.ElapsedMilliseconds;

            IndexedMesh[] Boulders = new IndexedMesh[BoulderUniqueCount];
            AABox[] BoulderBounds = new AABox[BoulderUniqueCount];
            Mesh[] BoulderMesh = new Mesh[BoulderUniqueCount];
            float[] BoulderRads = new float[BoulderUniqueCount];


            float boulderradius = 3;

            for (int i = 0; i < BoulderUniqueCount; i++)
            {
                BoulderRads[i] = boulderradius * (float)rgen.NextDouble();

                ///bias for small boulders
                if (rgen.NextDouble() > 0.7f)
                    BoulderRads[i] += 0.5f;
                else BoulderRads[i] += 0.05f;


                float boulderq = 0.2f;

                if (BoulderRads[i] < 0.5f)
                    boulderq = 0.1f;

                if (BoulderRads[i] < 0.25f)
                    boulderq = 0.05f;

                Boulders[i] = GeoFactory.CreateBoulder(ColorByte.White, BoulderRads[i], boulderq, rgen);

                if (!BleedBouldersToOne)
                {
                    BoulderBounds[i] = GeoFactory.CreateBoundingBox(Boulders[i]);
                    BoulderMesh[i] = meshman.Create(Boulders[i]);
                    meshman.Add("boulder#" + i.ToString(), BoulderMesh[i]);
                }
            }

            double time2 = watch.ElapsedMilliseconds;

            for (int i = 0; i < totalboulders; i++)
            {
                int randomboulder = rgen.Next(BoulderUniqueCount);

                Vector2 mappos = new Vector2((float)rgen.NextDouble() * (TerrainSize - 5) + 1, (float)rgen.NextDouble() * (TerrainSize - 5) + 1);

                float Z = marsmap.Height[mappos.X, mappos.Y];

                Vector3 newposition = new Vector3(mappos.X - TerrainSize / 2, mappos.Y - TerrainSize / 2, Z);

                if (newposition.IsValid())
                {
                    newposition.Z += ((float)rgen.NextDouble()) * (BoulderRads[randomboulder]) * 0.5f;
                }
                else
                {
                    throw new ArgumentException();
                }

                Vector3 rotnorm = new Vector3(0.5f + (float)rgen.NextDouble(), 0.5f + (float)rgen.NextDouble(), 0.5f + (float)rgen.NextDouble());
                rotnorm.Normalize();

                Quaternion q = Quaternion.CreateFromAxisAngle(ref rotnorm, (float)rgen.NextDouble() * 3);

                Transformation bouldertrans = new Transformation();
                bouldertrans.Position = newposition;
                bouldertrans.Rotation = q;

                if (BleedBouldersToOne)
                {
                    meshbuilder.Add(Boulders[randomboulder], bouldertrans);
                }
                else
                {
                    //OctTreeNode.Add(Boulders[randomboulder], bouldertrans);                                       
                    Mesh msh = BoulderMesh[randomboulder];

                    GeometryNode bldnode = new GeometryNode(Boulders[randomboulder], BoulderMesh[randomboulder], material);
                    bldnode.Trans = bouldertrans;
                    bldnode.BoundingObject = new Sphere(new Vector3(), 1);
                    //bldnode.BoundingObject = BoulderBounds[randomboulder];

                    //NodeMesh bldnode = new NodeMesh(msh, material);
                    //bldnode.Trans = bouldertrans;
                    //bldnode.BoundingObject = BoulderBounds[randomboulder];


                    MarsTerrain.AddNode(bldnode, "boulder#" + i.ToString());
                }
            }

            double time3 = watch.ElapsedMilliseconds;

            IndexedMesh marsmapIM = marsmap.GenerateTriMesh();

            if (BleedBouldersToOne)
            {
                meshbuilder.Add(marsmapIM);
                marsmapIM = meshbuilder.GetMesh();
            }

            mesh = meshman.Create(marsmapIM);

            GeometryNode terrain = new GeometryNode(marsmapIM, material);
            StaticCollision terraincol = new StaticCollision();
            terraincol.Material = new PhysMaterial();
            terraincol.StaticGeometry = marsmapIM;
            terrain.AddNode(terraincol, "statcol");
            

            MarsTerrain.AddNode(terrain, "terrain");

            //build collisions, yay!



            /*
            if (BleedBouldersToOne)
            {
                TriangleList tris = marsmapIM.ToTriangles();
                //OctTreeNode.Add(tris);
            }
            */

            double time4 = watch.ElapsedMilliseconds;
            watch.Stop();

            string message = string.Format("Terrain built in {0}\nUnique boulders in {1}\nBoulder placement in {2}\nCollision build {3}\nTotal time {4}",
                time1 / 1000, (time2 - time1) / 1000, (time3 - time2) / 1000, (time4 - time3) / 1000, time4 / 1000);
            VisimBox.Console.WriteLine(message);

            string[] textures = new string[6]{"Textures/Skybox/Mars/desertplains_left.jpg", "Textures/Skybox/Mars/desertplains_right.jpg",
                "Textures/Skybox/Mars/desertplains_front.jpg", "Textures/Skybox/Mars/desertplains_back.jpg",
                "Textures/Skybox/Mars/desertplains_top.jpg", null};

            MarsTerrain.AddNode(new SkyBoxNode(null, textures, new Vector3(200, 200, 200)), "skybox");

            var light = VisimEngine.Instance.Renderer.LightMan.AddDirectional("light", new Vector3(0, 0, 1), new ColorByte(50, 50, 50), ColorByte.White);


            var lnode = new LightNode(light);
            lnode.Trans = new QTransformation();
            lnode.Trans.Move(new Vector3(0, 0, 100));
            lnode.Trans.Yaw(1.57f);
            //lnode.Trans.Pitch(-1.57f);
            //lnode.Trans.Pitch(1.57f);
            lnode.UpdateLight(0);

            MarsTerrain.AddNode(lnode, "lightnode");


            VisimEngine.Instance.Renderer.MainContext.Fog = true;
            //VisimEngine.Instance.Renderer.MainContext.FogColor = new ColorByte(255, 89, 10);
            VisimEngine.Instance.Renderer.MainContext.FogColor = new ColorByte(128, 45, 5);
            VisimEngine.Instance.Renderer.MainContext.FogDensity = 0.025f;          
            
            return MarsTerrain;
        }
    }


    enum RobotStateEnum { Manual, Auto, Braking, Turning };

    public class MarsTerrain : Node, GeoOctTree
    {
        public GeoHeightMap TerrainMap;
        public int TerrainSize = 256;
        public int BoulderUniqueCount = 10;
        public int TotalBoulders = 100;
        RobotStateEnum RobotState = RobotStateEnum.Manual;
        Random rgn = new Random();

        bool automatic = false;

        Sphere[] CollSpheres;
        
        
        public VisimBox.Physics.PhysicalScene PhysScene;


        public MarsTerrain(int size, int total, int unique, bool auto)
        {
            TerrainSize = size;
            BoulderUniqueCount = unique;
            TotalBoulders = total;
            automatic = auto;
            if (auto)
                RobotState = RobotStateEnum.Auto;
            Construct();
        }

        public MarsTerrain() : this(256, 100, 10, false)
        {            
        }

        public Rover RoverNode;

        protected void Construct()
        {                        
            Stopwatch watch = new Stopwatch();
            watch.Start();
            
            bool BleedBouldersToOne = false;
            
            OctTreeNode = new GeoOctTreeNode(this, null, new Vector3(-TerrainSize, -TerrainSize, -TerrainSize), TerrainSize * 2);


            IndexedMeshBuilder<TexturedVertex> meshbuilder = null;

            if (BleedBouldersToOne)
            {
                meshbuilder = new IndexedMeshBuilder<TexturedVertex>(TerrainSize * TerrainSize + TotalBoulders * 500, 1, TerrainSize * TerrainSize * 4 + TotalBoulders * 1000);
            }

            
            PhysScene = new VisimBox.Physics.PhysicalScene();
            
            //VisibleHierarchy = false;            
            Random rgen = new Random();

            IRenderMaterial material;
            Mesh mesh;


            MeshManager meshman = VisimEngine.Instance.Renderer.MeshMan;
            MaterialManager matman = VisimEngine.Instance.Renderer.MaterialMan;

            StandardColorValues colorvals = StandardColorValues.Default();
            colorvals.Ambient = colorvals.Diffuse = new ColorByte(255, 89, 10);
            
            material = matman.Add("MarsGround", "Textures/Ground/Mars.jpg", ref colorvals);
            

            GeoHeightMap marsmap = new GeoHeightMap(rgen, TerrainSize, TerrainSize);
            TerrainMap = marsmap;

            marsmap.Position = new Vector3(-TerrainSize / 2, -TerrainSize / 2, 0);
            marsmap.TextureScale = new Vector2(0.05f, 0.05f);
           
            double time1 = watch.ElapsedMilliseconds;

            CollSpheres = new Sphere[TotalBoulders];
            IndexedMesh[] Boulders = new IndexedMesh[BoulderUniqueCount];
            AABox[] BoulderBounds = new AABox[BoulderUniqueCount];
            Mesh[] BoulderMesh = new Mesh[BoulderUniqueCount];
            float[] BoulderRads = new float[BoulderUniqueCount];


            float boulderradius = 3;

            for (int i = 0; i < BoulderUniqueCount; i++)
            {
                BoulderRads[i] = boulderradius * (float)rgen.NextDouble();

                ///bias for small boulders
                if (rgen.NextDouble() > 0.7f)
                    BoulderRads[i]+=0.5f;
                else BoulderRads[i] += 0.05f;


                float boulderq = 0.2f;

                if (BoulderRads[i] < 0.5f)
                    boulderq = 0.1f;

                if (BoulderRads[i] < 0.25f)
                    boulderq = 0.05f;

                Boulders[i] = GeoFactory.CreateBoulder(ColorByte.White, BoulderRads[i], boulderq, rgen);

                if (!BleedBouldersToOne)
                {
                    BoulderBounds[i] = GeoFactory.CreateBoundingBox(Boulders[i]);
                    BoulderMesh[i] = meshman.Create(Boulders[i]);
                    meshman.Add("boulder#" + i.ToString(), BoulderMesh[i]);
                }
            }

            double time2 = watch.ElapsedMilliseconds;

            //colorvals.Ambient = colorvals.Diffuse = ColorByte.White;
            //material = matman.Add("BoulderTex", "Textures/Ground/Mars.jpg", ref colorvals);


            for (int i = 0; i < TotalBoulders; i++)
            {
                int randomboulder = rgen.Next(BoulderUniqueCount);
                
                Vector2 mappos = new Vector2((float)rgen.NextDouble() * (TerrainSize - 5) + 1, (float)rgen.NextDouble() * (TerrainSize - 5) + 1);

                float Z = marsmap.Height[mappos.X, mappos.Y];

                Vector3 newposition = new Vector3(mappos.X - TerrainSize / 2, mappos.Y - TerrainSize / 2, Z);
        
                if (newposition.IsValid())
                {
                    newposition.Z += ((float)rgen.NextDouble()) * (BoulderRads[randomboulder]) * 0.5f;
                }
                else
                {
                    throw new ArgumentException();
                }

                CollSpheres[i] = new Sphere(newposition, BoulderRads[randomboulder]);


                Vector3 rotnorm = new Vector3(0.5f + (float)rgen.NextDouble(), 0.5f + (float)rgen.NextDouble(), 0.5f + (float)rgen.NextDouble());
                rotnorm.Normalize();

                Quaternion q = Quaternion.CreateFromAxisAngle(ref rotnorm, (float)rgen.NextDouble() * 3);

                Transformation bouldertrans = new Transformation();
                bouldertrans.Position = newposition;
                bouldertrans.Rotation = q;
                
                if (BleedBouldersToOne)
                {
                    meshbuilder.Add(Boulders[randomboulder], bouldertrans);
                }
                else
                {
                    OctTreeNode.Add(Boulders[randomboulder], bouldertrans);
                    Mesh msh = BoulderMesh[randomboulder];
                    NodeMesh bldnode = new NodeMesh(msh, material);
                    bldnode.Trans = bouldertrans;
                    bldnode.BoundingObject = BoulderBounds[randomboulder];
                    AddNode(bldnode, "boulder#" + i.ToString());
                }
            }
            
            double time3 = watch.ElapsedMilliseconds;

            IndexedMesh marsmapIM = marsmap.GenerateTriMesh();
            
            //if (!BleedBouldersToOne)
                //OctTreeNode.Add(marsmapIM);

            if (BleedBouldersToOne)
            {
                meshbuilder.Add(marsmapIM);
                marsmapIM = meshbuilder.GetMesh();
            }

            mesh = meshman.Create(marsmapIM);
            meshman.Add("marsmap", mesh);
            TerrainNode TerrainMesh = new TerrainNode(mesh, material, marsmap, TerrainSize);
            AddNode(TerrainMesh, "terrain");

            //build collisions, yay!




            if (BleedBouldersToOne)
            {
                TriangleList tris = marsmapIM.ToTriangles();
                //OctTreeNode.Add(tris);
            }


            double time4 = watch.ElapsedMilliseconds;
            watch.Stop();

            string message = string.Format("Terrain built in {0}\nUnique boulders in {1}\nBoulder placement in {2}\nCollision build {3}\nTotal time {4}",
                time1 / 1000, (time2 - time1) / 1000, (time3 - time2) / 1000, (time4-time3)/1000, time4 / 1000);
            VisimBox.Console.MessageBox(message);


            IndexedMesh imsphere = GeoFactory.CreateSphere<TexturedVertex>(new Vector3(0), 1.0f, out BounceSphere, 0.5f);
            
            
            SphereMat = material;   //no point in adding reference?
            SphereMesh = meshman.Create(imsphere);
            meshman.Add("spheremesh", SphereMesh);
            
            Vector3 pos = new Vector3(10, 0, 5 + marsmap.Height[10 + TerrainSize / 2, TerrainSize / 2]);
            //CreateSphere(pos, new Vector3(0));

            RoverNode = new Rover();

            AddNode(RoverNode, "Rover");

            pos = new Vector3(0, 0, marsmap.Height[TerrainSize / 2, TerrainSize / 2]);
            RoverNode.Trans.Position = pos;


            //PhysScene.RegisterBody(rov);
            //PhysScene.RegisterContactGenerator(rov);


            string[] textures = new string[6]{"Textures/Skybox/Mars/desertplains_left.jpg", "Textures/Skybox/Mars/desertplains_right.jpg",
                "Textures/Skybox/Mars/desertplains_front.jpg", "Textures/Skybox/Mars/desertplains_back.jpg",
                "Textures/Skybox/Mars/desertplains_top.jpg", null};

            AddNode(new SkyBoxNode(null, textures, new Vector3(200, 200, 200)), "skybox");

            //var light = VisimEngine.Instance.Renderer.LightMan.AddSpot("light", new Vector3(0, 0, 0), new Vector3(1, 0, 0), 45, new ColorByte(50, 50, 50), ColorByte.White, ColorByte.White);
            //var light = VisimEngine.Instance.Renderer.LightMan.AddPositional("light", new Vector3(0, 0, 0), new ColorByte(50, 50, 50), ColorByte.White, new ColorByte(50, 50, 50));
            var light = VisimEngine.Instance.Renderer.LightMan.AddDirectional("light", new Vector3(0, 0, 1), new ColorByte(50, 50, 50), ColorByte.White);


            var lnode = new LightNode(light);
            lnode.Trans = new QTransformation();
            lnode.Trans.Yaw(1.57f);
            //lnode.Trans.Pitch(-1.57f);
            //lnode.Trans.Pitch(1.57f);
            lnode.UpdateLight(0);

            AddNode(lnode, "lightnode");


            VisimEngine.Instance.Renderer.MainContext.Fog = true;
            //VisimEngine.Instance.Renderer.MainContext.FogColor = new ColorByte(255, 89, 10);
            VisimEngine.Instance.Renderer.MainContext.FogColor = new ColorByte(128, 45, 5);
            VisimEngine.Instance.Renderer.MainContext.FogDensity = 0.025f;

            VisimEngine.Instance.Renderer.MainContext.MainCamera = new FPSCamera();

            AttachCode();
        }


        protected void AttachCode()
        {
            System.Windows.Forms.Form frm = VisimEngine.Instance.Renderer.MainContext.Settings.Window as System.Windows.Forms.Form;
            frm.MouseMove += new System.Windows.Forms.MouseEventHandler(frm_MouseMove);
            frm.KeyDown += new System.Windows.Forms.KeyEventHandler(frm_KeyDown);
            frm.KeyUp += new System.Windows.Forms.KeyEventHandler(frm_KeyUp);
            frm.KeyPress += new KeyPressEventHandler(frm_KeyPress);

            VisimBox.VisimEngine.Instance.Renderer.MainContext.FrameEnd += new RendererEvent(MainContext_FrameEnd);
        }

        void MainContext_FrameEnd(object sender, RendererEventArgs e)
        {
            e.RC.Start2DRender();
            e.RC.DrawText(0, 0, "Mars Rover Simulator. Pres H for help");
            e.RC.Stop2DRender();
        }

        void frm_KeyPress(object sender, KeyPressEventArgs e)
        {
            switch (e.KeyChar)
            {
                case ' ':
                    //switch manual / auto
                    if (RobotState == RobotStateEnum.Manual)
                    {
                        RobotState = RobotStateEnum.Auto;
                    }
                    else
                        RobotState = RobotStateEnum.Manual;
                    break;
                case 'c':
                    //switch free camera, linked
                    freecamera = !freecamera;
                    break;
                case 'h':
                    //help
                    MessageBox.Show("Use WASD keys to steer robot or move camera\nUse Space to switch betweeen automatic and manual mode\nUse C to switch between tied and free camera", "MarsRobot help");
                    break;
            }
        }

        void frm_KeyUp(object sender, System.Windows.Forms.KeyEventArgs e)
        {
            UpdateKeyboard(e, false);
        }

        void frm_KeyDown(object sender, System.Windows.Forms.KeyEventArgs e)
        {
            UpdateKeyboard(e, true);
        }

        private void UpdateKeyboard(System.Windows.Forms.KeyEventArgs e, bool down)
        {
            switch (e.KeyCode)
            {
                case Keys.A:
                    keyA = down;
                    break;
                case Keys.W:
                    keyW = down;
                    break;
                case Keys.S:
                    keyS = down;
                    break;
                case Keys.D:
                    keyD = down;
                    break;
                case Keys.F:
                    keyF = down;
                    break;
                case Keys.R:
                    keyR = down;
                    break;
                case Keys.Up:
                    keyUp = down;
                    break;
                case Keys.Left:
                    keyLeft = down;
                    break;
                case Keys.Right:
                    keyRight = down;
                    break;
                case Keys.Down:
                    keyDown = down;
                    break;
            }
        }


        int mousex, mousey;

        bool keyW = false;
        bool keyA = false;
        bool keyS = false;
        bool keyD = false;
        bool keyR = false;
        bool keyF = false;
        bool keyUp = false;
        bool keyLeft = false;
        bool keyRight = false;
        bool keyDown = false;

        void frm_MouseMove(object sender, System.Windows.Forms.MouseEventArgs e)               
        {
            int deltax = mousex - e.X;
            int deltay = mousey - e.Y;

            mousex = e.X;
            mousey = e.Y;

            if (e.Button == System.Windows.Forms.MouseButtons.Right)
            {
                var irc = VisimEngine.Instance.Renderer.MainContext;

                if (irc.MainCamera != null)
                {
                    irc.MainCamera.Yaw(VisiMath.MathTools.DegToRad(deltay));
                    irc.MainCamera.Pitch(VisiMath.MathTools.DegToRad(-deltax));
                }
            }
        }


        Sphere BounceSphere;
        Mesh SphereMesh;
        IRenderMaterial SphereMat;


        void CreateSphere(Vector3 pos, Vector3 vel)
        {
            PhysSphere ps = new PhysSphere(this, BounceSphere);
            ps.Velocity = vel;
            ps.Trans.Move(ref pos);

            NodeMesh ren = new NodeMesh(SphereMesh, SphereMat);
            ren.BoundingObject = BounceSphere;

            ps.AddNode(ren, "renderable");
            ps.Visible = true;

            AddNode(ps);
            PhysScene.RegisterBody(ps);
            PhysScene.RegisterContactGenerator(ps);
        }


        double elapsedtime = 0;
        float turndir = 1;

        Sphere RoverSphere = new Sphere(new Vector3(), 1);


        bool freecamera = false;        

        public override bool TimeTick(double time)
        {
            //check keyboard
            bool moved = false;

            if (freecamera)
            {

                Vector3 move = new Vector3();
                //float val = movementspeed / irc.CurrentFPS;
                float val = 0.1f;                
                if (keyW)
                {
                    move.Add(val, 0, 0);
                    moved = true;
                }
                if (keyS)
                {
                    move.Add(-val, 0, 0);
                    moved = true;
                }
                if (keyA)
                {
                    move.Add(0, -val, 0);
                    moved = true;
                }
                if (keyD)
                {
                    move.Add(0, val, 0);
                    moved = true;
                }
                if (keyR)
                {
                    move.Add(0, 0, val);
                    moved = true;
                }
                if (keyF)
                {
                    move.Add(0, 0, -val);
                    moved = true;
                }
                if (moved)
                {
                    VisimEngine.Instance.Renderer.MainContext.MainCamera.MoveLocal(ref move);
                }
            }



            if (RobotState == RobotStateEnum.Manual)
            {
                if (!freecamera)
                {

                    if (keyA)
                    {
                        RoverNode.Trans.Pitch(MathTools.DegToRad(-1));
                        VisimEngine.Instance.Renderer.MainContext.MainCamera.Pitch(MathTools.DegToRad(-1));
                    }
                    else if (keyD)
                    {
                        RoverNode.Trans.Pitch(MathTools.DegToRad(+1));
                        VisimEngine.Instance.Renderer.MainContext.MainCamera.Pitch(MathTools.DegToRad(+1));
                    }
                }
            }

            Vector3 movepos = new Vector3(0.1f, 0, 0);
            moved = false;

            switch (RobotState)
            {
                case RobotStateEnum.Manual:
                    if (!freecamera)
                    {
                        if (keyW)
                        {
                            movepos = new Vector3(0.1f, 0, 0);
                            moved = true;
                        }
                        else if (keyS)
                        {
                            moved = true;
                            movepos = new Vector3(-0.1f, 0, 0);
                        }
                    }
                    break;
                case RobotStateEnum.Auto:
                    movepos = new Vector3(0.1f, 0, 0);
                    moved = true;
                    break;
                case RobotStateEnum.Braking:
                    moved = true;
                    movepos = new Vector3(-0.1f, 0, 0);
                    break;
            }           

            if (moved)
            {
                Vector3 original = RoverNode.Trans.Position;

                RoverNode.Trans.MoveLocal(ref movepos);

                Vector3 newpos = RoverNode.Trans.Position;

                Vector2 mappos = new Vector2(newpos.X + TerrainSize / 2, newpos.Y + TerrainSize / 2);

                newpos.Z = TerrainMap.Height[mappos.X, mappos.Y];

                //check for collision

                bool moveisvalid = true;

                RoverSphere._Center = newpos;

                for (int i = 0; i < CollSpheres.Length; i++)
                {
                    float dst = newpos.DistanceSquared(CollSpheres[i]._Center);

                        float sqrad = (1 + CollSpheres[i].Radius) * (1 + CollSpheres[i].Radius);
                        if (dst < sqrad)
                        {
                            moveisvalid = false;
                            break;
                        }
                }

                float limit = (TerrainSize - 3) / 2;
                if ((newpos.X > limit) || (newpos.Y > limit) || (newpos.X < -limit) || (newpos.Y < -limit))
                {
                    moveisvalid = false;
                }


                if (moveisvalid)
                {
                    RoverNode.Trans.Position = newpos;
                }
                else
                {
                    RoverNode.Trans.Position = original;

                    switch (RobotState)
                    {
                        case RobotStateEnum.Braking:
                            break;
                        case RobotStateEnum.Auto:
                            RobotState = RobotStateEnum.Braking;
                            break;
                    }
                }
            }

            if (RobotState == RobotStateEnum.Braking)
            {
                elapsedtime += time;
                if (elapsedtime > 1)
                {
                    elapsedtime = rgn.NextDouble()+0.5f;
                    RobotState = RobotStateEnum.Turning;
                    if (rgn.Next(2) == 0)
                        turndir = -1;
                    else turndir = 1;
                }
            }

            if (RobotState == RobotStateEnum.Turning)
            {
                elapsedtime -= time;
                RoverNode.Trans.Pitch(MathTools.DegToRad(turndir));
                
                if (!freecamera)
                    VisimEngine.Instance.Renderer.MainContext.MainCamera.Pitch(MathTools.DegToRad(turndir));

                if (elapsedtime < 0)
                {
                    elapsedtime = 0;
                    RobotState = RobotStateEnum.Auto;
                }
            }

            if (!freecamera)
            {
                Vector3 roverpos = RoverNode.Trans.Position + RoverNode.Trans.Rotation * new Vector3(-4, 0, 0);
                roverpos.Z += 3;
                VisimEngine.Instance.Renderer.MainContext.MainCamera.Position = roverpos;
            }

            
            return base.TimeTick(time);
        }

        #region GeoOctTree Members

        GeoOctTreeNode OctTreeNode = null;

        public GeoOctTreeNode TopNode
        {
            get
            {
                return OctTreeNode;
            }
            set
            {
                OctTreeNode = value;
            }
        }

        #endregion
    }

    public class TerrainNode : NodeMesh
    {
        GeoHeightMap MyMap;
        int TerrainSize;
        bool camera = true;
        
        public TerrainNode(Mesh mesh, IRenderMaterial mat, GeoHeightMap map, int terrainsize)
            : base(mesh, mat)
        {
            MyMap = map;
            TerrainSize = terrainsize;
        }

        public override bool TimeTick(double time)
        {
            if (!camera)
                return false;

            ICamera cam = VisimEngine.Instance.Renderer.MainContext.MainCamera;

            if (cam != null)
            {

                camera = false;

                Vector3 position = cam.Position;

                Vector2 mappos = new Vector2(position.X + TerrainSize / 2, position.Y + TerrainSize / 2);

                position.Z = MyMap.Height[mappos.X, mappos.Y] + 1.0f;

                cam.Position = position;
            }
            return true;
        }
    }



}
