using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VisimBox.Physics;
using VisimBox.Scene;
using VisiMath.Geometry;
using VisiMath;
using VisiMath.Collisions;


namespace MarsRover
{
    public class Rover : Node
    {

        public Rover()
        {
            NodeFactory.Settings.DefaultScale = 0.01f;
            NodeFactory.Settings.DefaultTrans = new QTransformation();
            NodeFactory.Settings.DefaultTrans.Roll(-MathTools.HalfPi);
            NodeFactory.Settings.DefaultTrans.Yaw(MathTools.HalfPi);
            //NodeFactory.Settings.DefaultTrans.Pitch(MathTools.HalfPi);


            var node = NodeFactory.ImportNodes("models/MarsRover.obj");
            AddNode(node, "RoverBody");

            this.Trans = new QTransformation();
        }

    }
    
    class RoverPhys : RigidBody
    {
        AABox body;
        Vector3[] WheelPoints = new Vector3[4];

        List<CollisionInfo> MyList;

        public RoverPhys(MarsTerrain world)
            : base(world)
        {
            float mass = 20;

            body = new AABox(new Vector3(-1, -0.5f, -0.3f), new Vector3(1, 0.5f, 0.3f));

            WheelPoints[0] = new Vector3(-0.8f, -0.5f, -0.3f);
            WheelPoints[1] = new Vector3(-0.8f, +0.5f, -0.3f);
            WheelPoints[2] = new Vector3(+0.8f, -0.5f, -0.3f);
            WheelPoints[3] = new Vector3(+0.8f, +0.5f, -0.3f);

            PhysicalBody = body;
            InvertedMass = 1/mass;
            //Acceleration = new Vector3(0, 0, -4);
            AngularDamping = 0.2f;
            InertiaTensor = Matrix3.CreateInertiaCuboid(2, 1, 0.6f, mass);
            MyList = new List<CollisionInfo>(20);
            for (int i = 0; i < 20; i++)
                MyList.Add(new CollisionInfo());

            var im = body.ToIndexedMesh(typeof(NormalVertex));

            var mesh = VisimBox.VisimEngine.Instance.Renderer.MeshMan.Create(im);
            VisimBox.VisimEngine.Instance.Renderer.MeshMan.Add("rovermesh", mesh);


            var mat = VisimBox.VisimEngine.Instance.Renderer.MaterialMan.Add("daka", VisimBox.Renderer.StandardColorValues.Default());

            NodeMesh nm = new NodeMesh(mesh, mat);

            AddNode(nm);

            float mapx = world.TerrainSize / 2;
            float mapy = world.TerrainSize / 2;

            float Z = world.TerrainMap.Height[mapx, mapy] + 2;

            Trans.Position = new Vector3(0, 0, Z);
        }

        Ray WheelRay = new Ray();
        Vector3 DownDir = new Vector3(0, 0, -1);

        public override bool PhysicalTimeTick(double time)
        {
            AddForce(new Vector3((float)time, 0, 0));
            return base.PhysicalTimeTick(time);
        }

        public override int AddContact(List<PhysicalContact> List, int index)
        {
            int total = 0;
            
            //check wheels first with heightmap only

            Matrix4 mymat = Trans.TransMatrix;
            MarsTerrain boss = (MarsTerrain)WorldNode;
            GeoOctTreeNode octree = boss.TopNode;

            for (int i = 0; i < 4; i++)
            {
                if (index == List.Count)
                    return total;

                WheelRay.FromDirection(WheelPoints[i], DownDir);
                WheelRay.Transform(ref mymat);

                float dst = octree.RayCast(WheelRay.Origin, WheelRay.Direction);

                if (dst < 0.3f)
                {
                    List[index].ContactNormal = WheelRay.Direction;
                    List[index].ContactPoint1 = WheelRay.Origin;
                    List[index].ContactPoint1.AddScaled(ref WheelRay.Direction, dst);
                    List[index].Penetration = (0.3f - dst);
                    List[index].SetBodyData(this, null, 0.5f, 0.1f);
                    index++;
                    total++;
                }
            }

            return total;
        }
    
    }
}
