using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Brains.Framework.Utility;
using Brains.Framework.Grouping;


namespace Brains.Framework.Locomotion
{
    /// <summary>
    /// This class is currently W.I.P
    /// </summary>
    /// <remarks></remarks>
    public class LocomtionSteering:LocomotionController
    {
        private static Random rand = new Random();
        private Vector2 _wanderTarget;

        public const float WanderRadius = 20;
        public const float WanderDistance = 30;
        public const float WanderJitter = 80;
         
        public enum Deceleration
        {
            Fast,
            Normal,
            Slow
        }

        [Flags()]
        public enum BehaviorType
        {
            None = 0x00000,
            Seek = 0x00002,
            Flee= 0x00004,
            Arrive= 0x00008,
            Wander = 0x00010,
            Cohesion= 0x00020,
            Seperation= 0x00040,
            Alignment = 0x00080,
            //ObstacleAvoidance= 0x00100,
            //WallAvoidance = 0x00200,
            //FollowPath = 0x00400,
            Pursuit= 0x00800,
            Evade = 0x01000,
            //Interpose= 0x02000,
            //Hide= 0x04000,
            //Flock = 0x08000,
            OffsetPursuit= 0x10000,
        }

        public void TurnAllOff()
        {
            _behaviorFlags = BehaviorType.None;
        }
        
        public void TurnOnArrive(Vector2 targetPosition)
        {
            TurnOn(BehaviorType.Arrive);
            SeekPos = targetPosition;
        }
        
        public void TurnOffArrive()
        {
            TurnOff(BehaviorType.Arrive);
        }

        public void TurnOnSeek(Vector2 targetPosition)
        {
            SeekPos = targetPosition;
            TurnOn(BehaviorType.Seek);
        }
        
        public void TurnOffSeek()
        {
            TurnOff(BehaviorType.Seek);
        }
        
        public void TurnOnWander()
        {
            TurnOn(BehaviorType.Wander);
        }
        
        public void TurnOffWander()
        {
            TurnOff(BehaviorType.Wander);
        }

        public void TurnOnSeperation()
        {
            TurnOn(BehaviorType.Seperation);
        }
        public void TurnOffSeperation()
        {
            TurnOff(BehaviorType.Seperation);
        }
        public void TurnOnCohesion()
        {
            TurnOn(BehaviorType.Cohesion);
        }
        public void TurnOffCohesion()
        {
            TurnOff(BehaviorType.Cohesion);
        }
        public void TurnOnAlignment()
        {
            TurnOn(BehaviorType.Alignment);
        }
        public void TurnOffAlignment()
        {
            TurnOff(BehaviorType.Alignment);
        }

        public void TurnOnOffsetPursuit(Vector2 seek)
        {

        }
        
        public void TurnOffOffsetPursuit()
        {

        }
        
        public void TurnOnPursuit(Vector2 seek)
        {

        }
        
        public void TurnOffPursuit()
        {

        }
        
        public void TurnOnFlee(Vector2 seek)
        {

        }
        
        public void TurnOffFlee()
        {

        }
      
        private Vector2 _steeringForce;
        private float ObstacleAvoidanceWeight=10;
        private Vector2 SteeringForce = Vector2.Zero;
        private Vector2 Heading;
        private float MaxForce = 2;
        private Vector2 Velocity;
        private BehaviorType _behaviorFlags;


        public float SeekWeight = 1;
        public float ArriveWeight= 1;
        public float WanderWeight= 1;
        public float SeperationWeight= 2;
        public float AlignmentWeight= 1;
        public float CohesionWeight = 1;

        public Vector2 SeekPos { get; set; }
        public Deceleration DecelerationSpeed { get; set; }
        public LocomtionSteering()
        {
            DecelerationSpeed = Deceleration.Normal;
        }
        float timeElapsed;
        public override void Update(Microsoft.Xna.Framework.GameTime gameTime)
        {
            timeElapsed = gameTime.GetElapsed();
            
            SteeringUpdate(gameTime.GetElapsed());
        }

        private void SteeringUpdate(float elapsed)
        {
            Vector2 OldPos = Owner.Position;
            SteeringForce = Vector2.Zero;

            SteeringForce = Calculate();
            Vector2 acceleration = SteeringForce / 1;
            Velocity = Vector2.Add(Velocity, acceleration);//* elapsed);
            Velocity = Truncate(Velocity, MaxSpeed);

            if (Velocity.LengthSquared() > 0.00000001)
            {
                SetHeading(Vector2.Normalize(Velocity));
            }
            Owner.Position += Velocity * elapsed;
            if(Heading!=Vector2.Zero)
                Owner.Orientation = Heading;// (float)Math.Atan2(Heading.Y, Heading.X);
        }

        private void SetHeading(Vector2 new_heading)
        {
            Heading= new_heading;
            //Right = Perp(_heading);
        }
        
        private Vector2 Truncate(Vector2 vec, float max)
        {
            Vector2 __v = vec;
            if (vec.Length() > max)
            {
                __v = Vector2.Normalize(vec);
                __v = Vector2.Multiply(__v, max);
            }
            return __v;
        }

        private Vector2 Calculate()
        {
            _steeringForce = Vector2.Zero;
            
            _steeringForce = CalculatePrioritized();
             
            return _steeringForce;
        }

        private bool On(BehaviorType type)
        {
            return (_behaviorFlags & type) == type;
        }
        
        private void TurnOn(BehaviorType bt)
        {
            if(!On(bt))
                _behaviorFlags |= bt;
        }

        private void TurnOff(BehaviorType bt)
        {
            if(On(bt))
                _behaviorFlags ^= bt;
        }
        
        private Vector2 CalculatePrioritized()
        {
            
            Vector2 force = Vector2.Zero;

            if (On(BehaviorType.Seperation))
            {
                force = Separation() * SeperationWeight;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }
            if (On(BehaviorType.Alignment))
            {
                force = Alignment() * AlignmentWeight;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }
            if (On(BehaviorType.Cohesion))
            {
                force = Cohesion() * CohesionWeight;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }

            if (On(BehaviorType.Seek))
            {
                force = Seek(SeekPos);
                force = Vector2.Multiply(force, SeekWeight);

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }



            if (On(BehaviorType.Arrive))
            {
                force = Arrive(SeekPos, DecelerationSpeed) * ArriveWeight;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }
            
            if (On(BehaviorType.Wander))
            {
                force = Wander() * WanderWeight;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }
         
            return _steeringForce;
        }

        private Vector2 Cohesion()
        {
            if (Owner.GetLabel(AIConsts.ISGROUPMEMBER) == 0)
                return Vector2.Zero;
            Group g = Owner.ParentWorld.GetGroup(Owner.GetLabel(AIConsts.GROUPID));

            //first find the center of mass of all the agents
            Vector2 CenterOfMass=Vector2.Zero;
            Vector2 SteeringForce=Vector2.Zero;

            int NeighborCount = 0;
            List<Agent> _agents = Owner.GetNearbyAgents(100);
            //iterate through the neighbors and sum up all the position vectors
            for (int a = 0; a < g.Agents.Count; ++a)
            {
                //make sure *this* agent isn't included in the calculations and that
                //the agent being examined is a neighbor
                if ((g.Agents[a] != Owner))// && neighbors[a]->IsTagged())
                {
                    CenterOfMass += g.Agents[a].Position;

                    ++NeighborCount;
                }
            }

            if (NeighborCount > 0)
            {
                //the center of mass is the average of the sum of positions
                CenterOfMass /= (float)NeighborCount;

                //now seek toward that position
                SteeringForce = Seek(CenterOfMass);
            }

            return SteeringForce;

        }

        private Vector2 Alignment()
        {
            if (Owner.GetLabel(AIConsts.ISGROUPMEMBER) == 0)
                return Vector2.Zero;
            Group g = Owner.ParentWorld.GetGroup(Owner.GetLabel(AIConsts.GROUPID));


            //used to record the average heading of the neighbors
            Vector2 AverageHeading = Vector2.Zero;

            //used to count the number of vehicles in the neighborhood
            int NeighborCount = 0;

            //iterate through all the tagged vehicles and sum their heading vectors
            for (int a = 0; a < g.Agents.Count; ++a)
            {
                //make sure *this* agent isn't included in the calculations and that
                //the agent being examined is close enough
                if ((g.Agents[a] != Owner))// && g.Agents[a]->IsTagged)
                {
                    AverageHeading += g.Agents[a].Orientation;

                    ++NeighborCount;
                }
            }

            //if the neighborhood contained one or more vehicles, average their
            //heading vectors.
            if (NeighborCount > 0)
            {
                AverageHeading /= (float)NeighborCount;
                AverageHeading -= Owner.Orientation;
            }

            return AverageHeading;

        }

        private Vector2 Separation()
        {
            if (Owner.GetLabel(AIConsts.ISGROUPMEMBER) == 0)
                return Vector2.Zero;
            Group g = Owner.ParentWorld.GetGroup(Owner.GetLabel(AIConsts.GROUPID));

            Vector2 SteeringForce = Vector2.Zero;
            
            for (int a = 0; a < g.Agents.Count; ++a)
            {
                //make sure this agent isn't included in the calculations and that
                //the agent being examined is close enough.
                if ((g.Agents[a] != Owner)) // && g.Agents[a]->IsTagged())
                {
                    Vector2 ToAgent = Owner.Position - g.Agents[a].Position;
                    if (ToAgent == Vector2.Zero)
                        continue;
                    //scale the force inversely proportional to the agent's distance
                    //from its neighbor.
                    SteeringForce +=Vector2.Normalize(ToAgent) / ToAgent.Length();
                }
            }

            return SteeringForce;

        }

        
        
        private bool AccumulateForce(ref Vector2 sf, Vector2 ForceToAdd)
        {
            //first calculate how much steering force we have left to use
            float MagnitudeSoFar = sf.Length();

            float magnitudeRemaining = MaxForce - MagnitudeSoFar;

            //return false if there is no more force left to use
            if (magnitudeRemaining <= 0.0) return false;

            //calculate the magnitude of the force we want to add
            float MagnitudeToAdd = ForceToAdd.Length();

            //now calculate how much of the force we can really add  
            if (MagnitudeToAdd > magnitudeRemaining)
            {
                MagnitudeToAdd = magnitudeRemaining;
            }

            //add it to the steering force
            if (ForceToAdd != Vector2.Zero)
                sf += (Vector2.Normalize(ForceToAdd) * MagnitudeToAdd);

            return true;
        }
      
        private Vector2 ObstacleAvoidance()
        {
            Vector2 _steer=Vector2.Zero;
            foreach (var item in Owner.ParentWorld.Map.ClusterGrid.Grids[0].Cells)
            {
                if (item.Type == 0)
                {
                    //Check feeler 1
                    Ray _ra = new Ray(Owner.Position.ToVector3(), Owner.Feelers[1].WorldDirection.ToVector3());
                    Vector3 tl = (item.Position - new Vector2(Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize / 2)).ToVector3();
                    BoundingBox _box = new BoundingBox(
                        tl,
                        tl + new Vector3(Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize,
                            Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize, 0));
                    float? inte = _ra.Intersects(_box);
                    if (inte.HasValue)
                    {
                        if (inte < Owner.Feelers[1].Length)
                        {
                            _steer = new Vector2(
                                Owner.Feelers[1].WorldDirection.Y, 
                                -Owner.Feelers[1].WorldDirection.X);                            
                        }
                    }
                    //Check feeler2

                     _ra = new Ray(Owner.Position.ToVector3(), Owner.Feelers[2].WorldDirection.ToVector3());
                     tl = (item.Position - new Vector2(Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize / 2)).ToVector3();
                     _box = new BoundingBox(
                        tl,
                        tl + new Vector3(Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize,
                            Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize, 0));
                    inte = _ra.Intersects(_box);
                    if (inte.HasValue)
                    {
                        if (inte < Owner.Feelers[1].Length)
                        {
                            _steer = new Vector2(-Owner.Feelers[2].WorldDirection.Y, Owner.Feelers[1].WorldDirection.X);
                        }
                    }
                }
            }

            return _steer;
        }
       
        public Vector2 Wander()
        {
            float JitterThisTimeSlice = WanderJitter * timeElapsed;

            //first, add a small random vector to the target's position (RandomClamped
            //returns a value between -1 and 1)
            _wanderTarget += new Vector2(RandomClamped() * JitterThisTimeSlice,
                                  RandomClamped() * JitterThisTimeSlice);

            if (_wanderTarget != Vector2.Zero) //reproject this new vector back on to a unit circle
                _wanderTarget.Normalize();

            //increase the length of the vector to the same as the radius
            //of the wander circle
            _wanderTarget *= WanderRadius;

            //move the target into a position WanderDist in front of the agent
            Vector2 _newtarget = _wanderTarget + new Vector2(WanderDistance, 0);

            //project the target into world space
            Vector2 _target = VectorUtil.PointToWorldSpace(_newtarget,
                                                 Owner.Orientation,
                                                 Owner.Position);

            return _target - Owner.Position;
            
        }

        public Vector2 Arrive(Vector2 targetPosition,Deceleration decelspeed)
        {
            Vector2 toTarget = targetPosition - Owner.Position;

            //calculate the distance to the target
            float dist = toTarget.Length();

            if (dist > 0)
            {
                //because Deceleration is enumerated as an int, this value is required
                //to provide fine tweaking of the deceleration..
                const float DecelerationTweaker = 0.3f;

                //calculate the speed required to reach the target given the desired
                //deceleration
                float speed = dist / ((float)decelspeed * DecelerationTweaker);

                //make sure the velocity does not exceed the max
                speed = Math.Min(speed, MaxSpeed);

                //from here proceed just like Seek except we don't need to normalize 
                //the ToTarget vector because we have already gone to the trouble
                //of calculating its length: dist. 
                Vector2 DesiredVelocity = toTarget * speed / dist;

                return (DesiredVelocity - Velocity);
            }

            return Vector2.Zero;
        }
        private Vector2 Seek(Vector2 seek)
        {
            Vector2 tpos = seek - Owner.Position;
            Vector2 DesiredVelocity = Vector2.Zero;
            if (tpos != Vector2.Zero)
                DesiredVelocity = Vector2.Normalize(seek - Owner.Position) * MaxSpeed;

            return (DesiredVelocity - Velocity);
        }

        float RandomClamped()
        {
            return rand.Next() - rand.Next();
        }

    }
}