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
{
    class PhysSphere : RigidBody
    {
        Sphere sphere;

        List<CollisionInfo> MyList;

        public PhysSphere(MarsTerrain world, Sphere sph)
            : base(world)
        {
            float mass = 1;
            PhysicalBody = sph;
            sphere = sph;
            InvertedMass = 1/mass;
            Acceleration = new Vector3(0, 0, -4);
            //InertiaTensor = Matrix3.Identitity;
            AngularDamping = 0.2f;
            InertiaTensor = Matrix3.CreateInertiaSphere(sph.Radius, mass);
            MyList = new List<CollisionInfo>(20);
            for (int i = 0; i < 20; i++)
                MyList.Add(new CollisionInfo());
       
        }

        public override int AddContact(List<PhysicalContact> List, int index)
        {
            bool collidewithhmap = false;

            if (_sleeping)
                return 0;


            if (index == List.Capacity-1)
                return 0;

            PhysicalContact cont = List[index];

            Vector3 mypos = Position;

            MarsTerrain boss = (MarsTerrain)WorldNode;


            if (collidewithhmap)
            {

                float mapx = boss.TerrainSize / 2 + mypos.X;
                float mapy = boss.TerrainSize / 2 + mypos.Y;

                float Z = boss.TerrainMap.Height[mapx, mapy];

                if (Z > (mypos.Z - sphere.Radius))
                {
                    cont.SetBodyData(this, null, 0.5f, 0.1f);
                    cont.ContactPoint1 = new Vector3(mypos.X, mypos.Y, Z);

                    float Z1 = boss.TerrainMap.Height[mapx + 0.5f, mapy];
                    float Z2 = boss.TerrainMap.Height[mapx, mapy + 0.5f];

                    cont.ContactNormal = MathTools.CalcTriNormal(new Vector3(mypos.X, mypos.Y, Z),
                                new Vector3(mypos.X + 0.5f, mypos.Y, Z1),
                                new Vector3(mypos.X, mypos.Y + 0.5f, Z2));

                    //cont.ContactNormal = Vector3.UnitZ;
                    cont.Penetration = Z - mypos.Z + sphere.Radius;
                    return 1;
                }
                //return 0;
            }
            //else
            {
                //first from topnode...

                GeoOctTreeNode octree = boss.TopNode;

                //unfinished

                //octree=octree.GetNode(ref mypos, sphere.Radius, false, 0, false, 0);

                //now get collisions with sphere...

                int total = List.Count - index;
                if (total > 20)
                    total = 20;
                int rval = octree.AddContactCollision(ref mypos, sphere.Radius, MyList, 0, total);

                for (int i = 0; i < rval; i++)
                {
                    List[index].ContactNormal = MyList[i].ContactNormal;
                    List[index].ContactPoint1 = MyList[i].ContactPoint1;
                    List[index].Penetration = MyList[i].Penetration;
                    List[index].SetBodyData(this, null, 0.5f, 0.1f);
                    index++;
                }
                return rval;
            }
        }
    }
}
