/*
    public Vector3 ObstactleAvoidance()
    {
        float lookAhead = GetObstactleAvoidanceLookAheadDist();

        // Need to use a primitive collider, the object running the sweeptest can't use its mesh collidier
        // though the mesh colliders of the objects it tests can be used
      //  m_EntityRoot.GetCollidable().m_Collider.enabled = true;

        Vector3 result = Vector3.zero;
        RaycastHit sweepResult = new RaycastHit();
        if (m_Rigidbody.SweepTest(m_Rigidbody.velocity, out sweepResult, lookAhead))
        {
            GameObject rayHitObject = sweepResult.collider.gameObject;
            Collider rayHitCollider = rayHitObject.GetComponent<Collider>();

            float hitObjRadius = rayHitCollider.bounds.extents.magnitude;
            Vector2 hitObjPos = rayHitCollider.bounds.center; // world or local?

            Vector3 hitObjInMyLocalCoords = transform.worldToLocalMatrix.MultiplyPoint(hitObjPos);

            Vector3 posDiff = hitObjInMyLocalCoords - transform.localPosition;

            // The closer the object , the stronger the steering force
            float mult = 1.0f + (lookAhead - hitObjInMyLocalCoords.x) / lookAhead;
            Vector3 steeringForce = Vector3.zero;

            // Lateral force
            const float breakingWeight = 1.0f;
            steeringForce.y = (hitObjRadius - hitObjInMyLocalCoords.y) * mult;

            steeringForce.x = (hitObjRadius - hitObjInMyLocalCoords.x) * breakingWeight;

            result = transform.localToWorldMatrix.MultiplyPoint(steeringForce);
        }

    //    m_EntityRoot.GetCollidable().m_Collider.enabled = false;

        return result;
    }
*/

    public static Vector3 Wander(SteeringActor actor)
    {
        SteeringBehaviours.Forces attributes = actor.GetAttributes().Forces;
        SteeringBehaviours.Wander wander = actor.GetAttributes().Wander;

        wander.Target = 
            new Vector3(
                wander.Target.x + RandomClamped() * wander.Jitter,
                wander.Target.y + RandomClamped() * wander.Jitter);
        wander.Target.Normalize();

        wander.Target *= wander.Radius;

        return attributes.MaxSpeed * (wander.Target * wander.Distance);
    }
    public static Vector3 Evade(SteeringActor actor, SteeringActor pursuer)
    {
        SteeringBehaviours.Forces attributes = actor.GetAttributes().Forces;

        Vector3 toPursuer = pursuer.GetPosition() - actor.GetPosition();

        float lookAhreadTime = toPursuer.magnitude / (attributes.MaxSpeed + pursuer.GetVelocity().magnitude);
        return Flee(actor, pursuer.GetPosition() + (Vector3)pursuer.GetVelocity() * lookAhreadTime);
    }
    public static Vector3 Pursuit(SteeringActor actor, SteeringActor evader)
    {
        SteeringBehaviours.Forces attributes = actor.GetAttributes().Forces;
        Vector3 toEvader = evader.GetPosition() - actor.GetPosition();

        Vector3 thisHeading = actor.GetVelocity();
        thisHeading.Normalize();

        Vector3 evaderHeading = evader.GetVelocity();
        evaderHeading.Normalize();
        float relativeHeading = Vector3.Dot(thisHeading, evaderHeading);

        float tresholdAngleAcos = Mathf.Acos(attributes.PursuitThresholdAngle);
        if (Vector3.Dot(toEvader, thisHeading) > 0 && (relativeHeading < -tresholdAngleAcos))
        {
            return Seek(actor, evader.GetPosition());
        }

        // the look ahead time is proportional to the distance between the evader
        // and the pursuer; and is inversely proportional to the sum of the agents velocities
        float lookAheadTime = toEvader.magnitude / (attributes.MaxSpeed + evader.GetVelocity().magnitude);

        // Now seek to the predicted future position of the evader
        return Seek(actor, evader.GetPosition() + (Vector3) evader.GetVelocity() * lookAheadTime);
    }
    public static Vector3 Arrive(SteeringActor actor, Vector3 targetPos, SteeringBehaviours.Deceleration deceleration)
    {
        SteeringBehaviours.Forces attributes = actor.GetAttributes().Forces;

        Vector3 toTarget = targetPos - actor.GetPosition();

        float dist = toTarget.magnitude;
        if (dist <= 0)
        {
            return new Vector3();
        }

        float decelerationTweaker = 0.3f;
        float speed = dist / ((float)deceleration * decelerationTweaker);

        speed = Mathf.Min(speed, attributes.MaxSpeed);

        Vector3 desiredVelocity = toTarget * speed / dist;

        return desiredVelocity;// - GetVelocity();
    }
    public static Vector3 Flee(SteeringActor actor, Vector3 targetPos)
    {
        SteeringBehaviours.Forces attributes = actor.GetAttributes().Forces;

        Vector3 fromTarget = actor.GetPosition() - targetPos;
        fromTarget.Normalize();
        Vector3 desiredVelocity = fromTarget * attributes.MaxSpeed;
        return desiredVelocity;// - GetVelocity();
    }
    public static Vector3 Seek(SteeringActor actor, Vector3 targetPos)
    {
        SteeringBehaviours.Forces attributes = actor.GetAttributes().Forces;

        Vector3 toTarget = (targetPos - actor.GetPosition());
        toTarget.Normalize();
        Vector3 desiredVelocity = toTarget * attributes.MaxSpeed;
        return desiredVelocity;// - GetVelocity();
    }