using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VisiMath.Collisions;

namespace VisiMath.Geometry
{
    /// <summary>
    /// Axis alligned box
    /// </summary>
    public class AABox : BaseGeometry, IVolumetric, IConvexObject, Collisions.ISpatialIndex
    {
        /// <summary>
        /// Minimal values for the corner
        /// </summary>
        public Vector3 Minimum;
        /// <summary>
        /// Maximal values for the opposite corner
        /// </summary>
        public Vector3 Maximum;

        /// <summary>
        /// Returns halfextents from the center of the box
        /// </summary>
        public Vector3 HalfExtents
        {
            get
            {
                #region inline return 0.5f * Maximum-Minimum
                Vector3 rval = Maximum;
                rval.Substract(ref Minimum);
                rval.Mul(0.5f);
                return rval;
                #endregion
            }
        }

        /// <summary>
        /// Center of the object
        /// </summary>        
        public override Vector3 Center
        {
            get
            {
                #region inline return 0.5 * Maximum + Minimum
                return new Vector3((Maximum.X + Minimum.X) * 0.5f, (Maximum.Y + Minimum.Y) * 0.5f, (Maximum.Z + Minimum.Z) * 0.5f);
                #endregion
            }
        }


        /// <summary>
        /// Default constructor, expects user to set minimum and maximum to correct values
        /// </summary>
        public AABox()
        {
        }

        /// <summary>
        /// XML reader constructor
        /// </summary>
        /// <param name="reader">XML reader</param>
        public AABox(System.Xml.XmlReader reader)
            : base()
        {
            ReadXml(reader);
        }

        /// <summary>
        /// Constructs aabox from minimal and maximal corner
        /// </summary>
        /// <param name="min">minimal corner</param>
        /// <param name="max">maximal corner</param>
        public AABox(Vector3 min, Vector3 max) : this(ref min, ref max) {}


        /// <summary>
        /// Constructs aabox from minimal and maximal corner
        /// </summary>
        /// <param name="min">minimal corner</param>
        /// <param name="max">maximal corner</param>
        public AABox(ref Vector3 min, ref Vector3 max)
        {
            System.Diagnostics.Debug.Assert(min.X < max.X);
            System.Diagnostics.Debug.Assert(min.Y < max.Y);
            System.Diagnostics.Debug.Assert(min.Z < max.Z);
            Minimum = min;
            Maximum = max;
        }

        public AABox(float x, float y, float z)
        {            
            x = 0.5f*x;
            y = 0.5f*y;
            z = 0.5f*z;
            Minimum = new Vector3(-x, -y, -z);
            Maximum = new Vector3(x, y, z);
        }

        /// <summary>
        /// Prefers textured vertex
        /// </summary>
        public override Type PreferredVertexType
        {
            get { return typeof(TexturedVertex); }
        }

        /// <summary>
        /// Is object geometric renderable primitive from enumaration PrimitiveTypes?
        /// </summary>
        public override bool IsPrimitive
        {
            get { return false; }
        }


        /// <summary>
        /// Parametric box is not a container
        /// </summary>
        public override bool IsContainer
        {
            get { return false; }
        }

        /// <summary>
        /// Checks if specified position is inside the object
        /// </summary>
        /// <param name="pos">Position to check</param>
        /// <returns>True if vector is inside the object</returns>
        public override bool ContainsPosition(Vector3 pos)
        {
            for (int i = 0; i < 3; i++)
            {
                if ((pos[i] > Maximum[i]) || (pos[i] < Minimum[i]))
                    return false;
            }
            return true;
        }

        /// <summary>
        /// Check for intersection and fills intersection info if intersection is closer to the origin of the ray then value before
        /// </summary>
        /// <param name="inters">Data structure with intersection info</param>
        /// <param name="obj">Ray object</param>
        /// <returns>True if there was an intersection and it was closer then previous stored</returns>
        public override bool GetIntersectionInfo(Ray obj, ref IIntersectionInfo inters)
        {
            float dist;
            Vector3 intpoint;

            if (MathTools.IntersectionRayAABox(ref obj.Origin, ref obj.Direction, ref Minimum, ref Maximum, out dist, out intpoint))
            {
                if (dist < inters.Distance)
                {
                    inters.Distance = dist;
                    inters.IntersectionPoint = intpoint;
                    inters.GeoObject = this;
                    return true;
                }
            }
            return false;
        }

        /// <summary>
        /// Preferred primitive is triangle list
        /// </summary>
        public override PrimitiveTypes PreferredPrimitive
        {
            get { return PrimitiveTypes.TriangleList; }
        }


        /// <summary>
        /// Convert object to list of points. Quality and vertex limits not used
        /// </summary>
        /// <param name="vertextype">Required type of the vertex</param>
        public override Point3DList ToPoints(Type vertextype, float quality = 0, int vertexlimit = 0)
        {
            var rval = VertexFactory.CreateArray(vertextype, 8);
            
            IVertex iv = VertexFactory.CreateNewVertex(vertextype);

            Vector3[] corners = GetCorners();

            for (int i = 0; i < 8; i++)
            {
                iv.Position = corners[i];
                rval[i] = iv;
            }

            return new Point3DList(rval);
        }

        /// <summary>
        /// Returns closest distance from the specified point to object
        /// </summary>
        /// <param name="pos">position</param>
        /// <returns>Closest distance to the object</returns>
        public override float Distance(ref Vector3 pos)
        {
            if (ContainsPosition(pos)) return 0;

            float rval = 0;

            for (int i = 0; i < 3; i++)
            {
                float v = pos[i];
                float vv = v - Minimum[i];
                if (vv < 0)
                    rval += (-vv);
                vv = Maximum[i] - v;
                if (vv < 0)
                    rval += (-vv);
            }
            return rval;

        }

        /// <summary>
        /// Returns closest distance to the ray
        /// </summary>
        /// <param name="ray">Ray to check distance to</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public override float Distance(Ray ray, bool frontOnly = false)
        {
            Vector3[] quads = GetQuads();

            float dist = float.NaN;
            
            for (int i = 0; i < 6; i++)
            {
                float newdst = MathTools.DistanceRayQuad(
                    ref ray.Origin, ref ray.Direction, 
                    ref quads[i * 4 + 0], ref quads[i * 4 + 1], ref quads[i * 4 + 2], ref quads[i * 4 + 3], frontOnly);

                if ((newdst != float.NaN) && (dist != float.NaN) && (newdst < dist))
                    dist = newdst;
            }
            return dist;
        }

        /// <summary>
        /// Returns closest distance to the line segment. Generic, from center, OVERRIDE!
        /// </summary>
        ///<param name="pos1">Start of the line</param>
        ///<param name="pos2">end of the line</param>
        /// <returns>Closest distance</returns>
        public override float DistanceLineSegment(ref Vector3 pos1, ref Vector3 pos2)
        {
            Vector3[] quads = GetQuads();
            //OPTIMIZE!!!

            float dist = float.MaxValue;

            for (int i = 0; i < 6; i++)
            {
                float newdst = MathTools.DistanceLineSegmentQuad(ref pos1, ref pos2, 
                    ref quads[i * 4 + 0], ref quads[i * 4 + 1], ref quads[i * 4 + 2], ref quads[i * 4 + 3]);

                if (newdst < dist)
                    dist = newdst;
            }
            return dist;
        }

        /// <summary>
        /// Returns closest distance to the triangle.
        /// </summary>
        /// <param name="pos1">First corner</param>
        /// <param name="pos2">Second corner</param>
        /// <param name="pos3">Third corner</param>
        /// <returns>Distance</returns>
        public override float DistanceTriangle(ref Vector3 pos1, ref Vector3 pos2, ref Vector3 pos3)
        {
            //OPTIMIZE!!!            

            Vector3[] quads = GetQuads();
            float dist = float.MaxValue;

            for (int i = 0; i < 6; i++)
            {
                float newdst = MathTools.DistanceTriangleQuad(ref pos1, ref pos2, ref pos3,
                    ref quads[i * 4 + 0], ref quads[i * 4 + 1], ref quads[i * 4 + 2], ref quads[i * 4 + 3]);

                if (newdst < dist)
                    dist = newdst;
            }
            return dist;

        }

        /// <summary>
        /// Returns closest distance to the sphere
        /// </summary>
        /// <param name="center">center of the sphere</param>
        /// <param name="radius">radius of the sphere</param>
        /// <returns>Closest distance</returns>
        public override float DistanceSphere(ref Vector3 center, float radius)
        {
            Vector3[] corners = GetCorners();
            float dst = float.MaxValue;
            for (int i = 0; i < corners.Length; i++)
            {
                float newdst = MathTools.DistanceVectorSphere(ref center, radius, ref corners[i]);
                if (newdst < dst)
                    dst = newdst;
            }
            return dst;       
        }

        private Vector3 CornerChooser(int n)
        {
            switch (n)
            {
                case 0:
                    return Minimum;
                case 1:
                    return new Vector3(Maximum.X, Minimum.Y, Minimum.Z);
                case 2:
                    return new Vector3(Minimum.X, Maximum.Y, Minimum.Z);
                case 3:
                    return new Vector3(Maximum.X, Maximum.Y, Minimum.Z);
                case 4:
                    return new Vector3(Minimum.X, Minimum.Y, Maximum.Z);
                case 5:
                    return new Vector3(Maximum.X, Minimum.Y, Maximum.Z);
                case 6:
                    return new Vector3(Minimum.X, Maximum.Y, Maximum.Z);
                default:
                    return new Vector3(Maximum.X, Maximum.Y, Maximum.Z);
            }
        }

        private Vector3 CornerChooser2(int n)
        {
            return new Vector3(
                ((n & 1) != 0) ? Maximum.X : Minimum.X,
                ((n & 2)!=0) ? Maximum.Y : Minimum.Y,
                ((n & 4)!=0) ? Maximum.Z : Minimum.Z);

        }

        /// <summary>
        /// Checks for intersection between object and moving sphere
        /// </summary>
        ///<param name="radius">Radius of the sphere</param>
        /// <param name="maxdistance">Maximal distance to travel</param>
        /// <param name="ray">Ray describing movement of the sphere</param>
        /// <returns>Distance to the intersection or -1 if no intersection exists</returns>
        public override float IntersectsMoving(float radius, Ray ray, float maxdistance)
        {
            Vector3 newmin = Minimum; newmin.Substract(radius, radius, radius);
            Vector3 newmax = Maximum; newmax.Add(radius, radius, radius);
        
            float t;
            Vector3 p;

            //quick check with enlarged box
            if (!MathTools.IntersectionRayAABox(ref ray.Origin, ref ray.Direction, ref newmin, ref newmax, out t, out p))
                return -1;
            if (t > maxdistance)
                return -1;

            int u =0, v =0;

            if (p.X < Minimum.X) u |= 1;
            if (p.X > Maximum.X) v |= 1;
            if (p.Y < Minimum.Y) u |= 2;
            if (p.Y > Maximum.Y) v |= 2;
            if (p.Z < Minimum.Z) u |= 4;
            if (p.Z > Maximum.Z) v |= 4;

            int mask = u + v;

            Vector3 target = Vector3.MoveInDirection(ref ray.Origin, ref ray.Direction, t);

            Vector3 corner1, corner2;

            if (mask == 7)
            {
                float tmin = float.MaxValue;
                corner1 = CornerChooser2(v);
                corner2 = CornerChooser2(v ^ 1);
                if (MathTools.IntersectionSegmentCapsule(ref ray.Origin, ref target, ref corner1, ref corner2, radius, out t))
                    tmin = MathTools.Min(tmin, t);
                corner2 = CornerChooser2(v ^ 2);
                if (MathTools.IntersectionSegmentCapsule(ref ray.Origin, ref target, ref corner1, ref corner2, radius, out t))
                    tmin = MathTools.Min(tmin, t);
                corner2 = CornerChooser2(v ^ 4);
                if (MathTools.IntersectionSegmentCapsule(ref ray.Origin, ref target, ref corner1, ref corner2, radius, out t))
                    tmin = MathTools.Min(tmin, t);

                if (tmin == float.MaxValue)
                    return -1;
                return t;
                //all bits set, intersection point is vertex
            }

            if ((mask & (mask - 1)) == 0)
            {
                //intersection point is the face of the box
                return t;
            }

            //intersection point is edge
            corner1 = CornerChooser2(u ^7);
            corner2 = CornerChooser2(v);
            if (MathTools.IntersectionSegmentCapsule(ref ray.Origin, ref target, ref corner1, ref corner2, radius, out t))
                return t;
            return -1;
        }




        #region Conversions

        public Vector3[] GetCorners()
        {
            float minx = Minimum.X;
            float maxx = Maximum.X;
            float miny = Minimum.Y;
            float maxy = Maximum.Y;
            float minz = Minimum.Z;
            float maxz = Maximum.Z;

            Vector3[] rval = new Vector3[8];

            rval[0] = new Vector3(minx, miny, minz);
            rval[1] = new Vector3(maxx, miny, minz);
            rval[2] = new Vector3(maxx, maxy, minz);
            rval[3] = new Vector3(minx, maxy, minz);
            rval[4] = new Vector3(minx, miny, maxz);
            rval[5] = new Vector3(maxx, miny, maxz);
            rval[6] = new Vector3(maxx, maxy, maxz);
            rval[7] = new Vector3(minx, maxy, maxz);
            return rval;
        }

        public Vector3[] GetQuads()
        {
            Vector3[] corners = GetCorners();
            Vector3[] rval = new Vector3[24];

            rval[0] = new Vector3(corners[3]);
            rval[1] = new Vector3(corners[2]);
            rval[2] = new Vector3(corners[1]);
            rval[3] = new Vector3(corners[0]);

            rval[4] = new Vector3(corners[4]);
            rval[5] = new Vector3(corners[5]);
            rval[6] = new Vector3(corners[6]);
            rval[7] = new Vector3(corners[7]);

            rval[8] = new Vector3(corners[7]);
            rval[9] = new Vector3(corners[3]);
            rval[10] = new Vector3(corners[0]);
            rval[11] = new Vector3(corners[4]);

            rval[12] = new Vector3(corners[2]);
            rval[13] = new Vector3(corners[6]);
            rval[14] = new Vector3(corners[5]);
            rval[15] = new Vector3(corners[1]);

            rval[16] = new Vector3(corners[0]);
            rval[17] = new Vector3(corners[1]);
            rval[18] = new Vector3(corners[5]);
            rval[19] = new Vector3(corners[4]);

            rval[20] = new Vector3(corners[7]);
            rval[21] = new Vector3(corners[6]);
            rval[22] = new Vector3(corners[2]);
            rval[23] = new Vector3(corners[3]);

            return rval;
        }


        public QuadList ToQuadList<VertexT>()
            where VertexT : IVertex
        {
            return ToQuadList(typeof(VertexT));
        }

        public virtual QuadList ToQuadList(Type VertexType)
        {
            Vector3[] quads = GetQuads();

            IVertexArray varray = VertexFactory.CreateArray(VertexType, 24);


            bool normals = false;
            if (VertexFactory.HasNormal(VertexType))
                normals = true;
            bool txcoords = VertexFactory.HasTexCoord(VertexType);

            for (int i =0; i < 24; i++)
            {
                IVertex ver = VertexFactory.CreateNewVertex(VertexType);
                if (normals)
                {
                    Vector3 norm;
                    if (i < 4)
                        norm = new Vector3(0,0,-1);
                    else if (i < 8)
                        norm = new Vector3(0,0, 1);
                    else if (i < 12)
                        norm = new Vector3(-1,0, 0);
                    else if (i < 16)
                        norm = new Vector3(1,0, 0);
                    else if (i < 20)
                        norm = new Vector3(0,-1, 0);
                    else 
                        norm = new Vector3(0,1, 0);
                    ver.Normal = norm;
                }

                if (txcoords)
                {
                    int j = i % 4;

                    switch (j)
                    {
                        case 0:
                            ver.TexCoord2 = new Vector2(0, 0);
                            break;
                        case 1:
                            ver.TexCoord2 = new Vector2(1, 0);
                            break;
                        case 2:
                            ver.TexCoord2 = new Vector2(1, 1);
                            break;
                        case 3:
                            ver.TexCoord2 = new Vector2(0, 1);
                            break;
                    }
                }

                ver.Position = quads[i];
                varray[i] = ver;

            }
            return new QuadList(varray);
        }

        /// <summary>
        /// Convert object to list of lines. Quality and vertex limits not used
        /// </summary>
        public override LineList ToLines(Type vertextype, float quality = 0, int vertexlimit = 0)
        {
            return ToQuadList(vertextype).ToLines();
        }

        /// <summary>
        /// Convert object to list of triangles. Quality and vertex limits not used
        /// </summary>
        public override TriangleList ToTriangles(Type vertextype, float quality = 0, int vertexlimit = 0)
        {
            return ToQuadList(vertextype).ToTriangles();
        }

        /// <summary>
        /// Convert object to indexed list of triangles. Quality and vertex limits not used
        /// </summary>
        public override IndexedMesh ToIndexedMesh(Type vertextype, float quality = 0, int vertexlimit = 0)
        {
            return ToQuadList(vertextype).ToIndexedMesh();
        }

        #endregion

        /// <summary>
        /// Volume of the object
        /// </summary>
        public float Volume
        {
            get
            {
                    Vector3 boxval = Maximum - Minimum;
                    return boxval.X * boxval.Y * boxval.Z;
            }
        }

        //private Vector3[] SupportCorners;

        /// <summary>
        /// Gets supporting vertex in local coordinates
        /// </summary>
        /// <param name="vertex">Save vertex</param>
        /// <param name="direction">Direction vector</param>
        public void GetLocalSupportingVertex(ref Vector3 direction, out Vector3 vertex)
        {
           /* if (SupportCorners == null)
                SupportCorners = GetCorners();

            float max = float.NegativeInfinity;
            vertex = new Vector3();

            //note bruteforce implementation, use hill climbing for fully formed convex hulls

            for (int i = 0; i < SupportCorners.Length; i++)
            {
                float dot = SupportCorners[i].Dot(ref direction);
                if (dot > max)
                {
                    max = dot;
                    vertex = SupportCorners[i];
                }
            }
            */

            
            if (direction.X <= 0)
                vertex.X = Minimum.X;
            else vertex.X = Maximum.X;
            if (direction.Y <= 0)
                vertex.Y = Minimum.Y;
            else
                vertex.Y = Maximum.Y;
            if (direction.Z <= 0)
                vertex.Z = Minimum.Z;
            else
                vertex.Z = Maximum.Z;
        }

        /// <summary>
        /// Returns implicit (math expression) or explicit (vertex based) type of convex object
        /// </summary>
        public ConvexObjectType BaseType { get { return ConvexObjectType.Explicit; } }

        /// <summary>
        /// Returns number of vertices, valid for explicit object types
        /// </summary>
        public int VertexCount { get { return 8; } }

        /// <summary>
        /// Returns vertex with specified index, valid for explicit object types
        /// </summary>
        /// <param name="index">index of vertex</param>
        /// <param name="vertex">Vector to save vertex</param>
        public void GetVertex(int index, out Vector3 vertex)
        {
            vertex = CornerChooser(index);
        }


        #region SpatialIndex
        /// <summary>
        /// Returns spatial index as box
        /// </summary>
        public AABox AsBox 
        {
            get
            {
                return this;
            }
        }
        /// <summary>
        /// Returns spatial index as sphere
        /// </summary>
        public Sphere AsSphere 
        {
            get
            {
                float rad = MathTools.Max(HalfExtents);
                rad = MathTools.Sqrt(2 * rad*rad);  //note save constant sqrt(2) somewhere?
                return new Sphere(this.Center, rad);
            }
        }
        /// <summary>
        /// returns spatial index as point (center)
        /// </summary>
        public Vector3 AsPoint { get { return Center; } }

        /// <summary>
        /// return true if index contains (intersects) another index
        /// </summary>
        /// <param name="index">Second spatial index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(ref ISpatialIndex index)
        {
            AABox box = index as AABox;
            if (box != null)
                return Contains(box);
            Sphere sph = index as Sphere;
            if (sph != null)
                return Contains(sph);
            Point3D point = index as Point3D;
            if (point != null)
                return Contains(point);
            LineSegment3D line = index as LineSegment3D;
            if (line != null)
                return Contains(line);

            return Contains(index as IConvexObject);
        }

        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Vector3 index)
        {
            return Contains(ref index);
        }
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(ref Vector3 index)
        { 
            return ContainsPosition(index);
        }
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Point3D index)
        {
            return Contains(index.GetPosition(0));
        }

        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if intersection exists</returns>
        public bool ContainsLine(Vector3 line1, Vector3 line2)
        {
            return ContainsLine(ref line1, ref line2);
        }
        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if intersection exists</returns>
        public bool ContainsLine(ref Vector3 line1, ref Vector3 line2)
        {
            return ContainsPosition(line1) || ContainsPosition(line2);
        }
        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="index">line index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(LineSegment3D line)
        {
            return ContainsLine(line.GetPosition(0), line.GetPosition(1));
        }

        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Vector3 center, float radius)
        {
            return Contains(ref center, radius);
        }
        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(ref Vector3 center, float radius)
        {
            for (int i = 0; i < 3; i++)
            {
                if ((center[i] - radius <= Minimum[i]) || (center[i] + radius >= Maximum[i]))
                    return false;
            }
            return true;
        }
        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="sphere">sphere index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Sphere sphere)
        {
            return Contains(ref sphere._Center, sphere.Radius);
        }

        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if intersection exists</returns>
        public bool ContainsBox(Vector3 min, Vector3 max)
        {
            return ContainsBox(ref min, ref max);
        }
        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if intersection exists</returns>
        public bool ContainsBox(ref Vector3 min, ref Vector3 max)
        {
            for (int i = 0; i < 3; i++)
            {
                if ((max[i] < Minimum[i]) || (min[i] > Maximum[i]))
                    return false;
            }
            return true;
        }
        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="box">AABox</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(AABox box)
        {
            return ContainsBox(ref box.Minimum, ref box.Maximum);
        }

        /// <summary>
        /// Return true if index contains generic convex object
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(IConvexObject convex)
        { 
            int vcount = convex.VertexCount;
            for (int i = 0; i < vcount; i++)
            {
                Vector3 vec;
                convex.GetVertex(i, out vec);
                if (ContainsPosition(vec))
                    return true;
            }
            return false;
        }


        /// <summary>
        /// return true if index fully contains another index
        /// </summary>
        /// <param name="index">Second spatial index</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(ref ISpatialIndex index)
        {
            AABox box = index as AABox;
            if (box != null)
                return ContainsFully(box);
            Sphere sph = index as Sphere;
            if (sph != null)
                return ContainsFully(sph);
            Point3D point = index as Point3D;
            if (point != null)
                return Contains(point);
            LineSegment3D line = index as LineSegment3D;
            if (line != null)
                return ContainsFully(line);

            return ContainsFully(index as IConvexObject);
        }

        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFullyLine(Vector3 line1, Vector3 line2)
        {
            return ContainsFullyLine(ref line1, ref line2);
        }
        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFullyLine(ref Vector3 line1, ref Vector3 line2)
        {
            return (ContainsPosition(line1) && ContainsPosition(line2));
        }
        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line">Line segment</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(LineSegment3D line)
        {
            return ContainsFullyLine(line.GetPosition(0), line.GetPosition(1));
        }

        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(Vector3 center, float radius)
        {
            return ContainsFully(ref center, radius);
        }
        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(ref Vector3 center, float radius)
        { 
            for (int i = 0; i < 3; i++)
            {
                if ((center[i] - radius < Minimum[i]) || (center[i] + radius > Maximum[i]))
                    return false;
            }
            return true;
        }
        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="sphere">sphere index</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(Sphere sphere)
        {
            return ContainsFully(ref sphere._Center, sphere.Radius);
        }

        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFullyBox(Vector3 min, Vector3 max)
        {
            return ContainsFullyBox(ref min, ref max);
        }
        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFullyBox(ref Vector3 min, ref Vector3 max)
        {
            for (int i = 0; i < 3; i++)
            {
                if ((Minimum[i] > min[i]) || (Maximum[i] < max[i]))
                    return false;
            }
            return true;
        }
        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="box">AABox</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(AABox box)
        {
            return ContainsFullyBox(ref box.Minimum, ref box.Maximum);
        }

        /// <summary>
        /// Return true if index FULLY contains generic convex object
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if object is fully contained in index</returns>
        public bool ContainsFully(IConvexObject convex)
        {
            int vcount = convex.VertexCount;
            for (int i = 0; i < vcount; i++)
            {
                Vector3 vec;
                convex.GetVertex(i, out vec);
                if (!ContainsPosition(vec))
                    return false;
            }
            return true;
        }        




        #endregion

        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public override void ReadXml(System.Xml.XmlReader reader)
        {
            if (reader["type"] != this.GetType().Name)
                throw new ArgumentException("IGeometry:ReadXML - wrong xml node type!");

            while (reader.Read())
            {
                if (reader.IsStartElement())
                {
                    if (reader.Name == "Minimum")
                    {
                        Minimum.ReadXml(reader);
                    }
                    else if (reader.Name == "Maximum")
                    {
                        Maximum.ReadXml(reader);
                    }
                }
                else break;
            }
        }


        /// <summary>
        /// Serializes object to XML
        /// </summary>
        /// <param name="writer">XML writer to save values</param>
        public override void WriteXml(System.Xml.XmlWriter writer)
        {
            writer.WriteAttributeString("type", this.GetType().Name);
            writer.WriteStartElement("Minimum");
            Minimum.WriteXml(writer);
            writer.WriteEndElement();
            writer.WriteStartElement("Maximum");
            Maximum.WriteXml(writer);
            writer.WriteEndElement();
        }



        /// <summary>
        /// Constructs new aabox for array of vertices
        /// </summary>
        /// <param name="vertices"></param>
        /// <returns></returns>
        public static AABox ConstructFromPositions(ICollection<Vector3> vertices)
        {
            Vector3 min = new Vector3(float.MaxValue);
            Vector3 max = new Vector3(float.MinValue);

            foreach (Vector3 vec in vertices)
            {
                Vector3 vv = vec;
                min = MathTools.Min(ref min, ref vv);
                max = MathTools.Max(ref max, ref vv);
            }
            return new AABox(min, max);
        }

        /// <summary>
        /// Returns copy of the geometric data
        /// </summary>
        /// <returns>Full copy of the geometry</returns>
        public override IGeometry GetCopy()
        {
            return new AABox(this.Minimum, this.Maximum);
        }

    }
}
