/*
      Copyright (C) 2011 Bepu Entertainment LLC.

      This software source code is provided 'as-is', without 
      any express or implied warranty.  In no event will the authors be held 
      liable for any damages arising from the use of this software.

      Permission is granted to anyone to use this software for any purpose,
      including commercial applications, and to alter it and redistribute it
      freely, subject to the following restrictions:

      1. The origin of this software must not be misrepresented; you must not
         claim that you wrote the original software. If you use this software
         in a product, an acknowledgment in the product documentation would be
         appreciated but is not required.
      2. Altered source versions must be plainly marked as such, and must not be
         misrepresented as being the original software.
      3. This notice may not be removed or altered from any source distribution.

    Contact us at:
    contact@bepu-games.com
 */

using BEPUphysics;
using BEPUphysics.Entities;
using Microsoft.Xna.Framework;
using BEPUphysics.MathExtensions;
using BEPUphysics.Constraints;

namespace BEPUphysicsDemos.SampleCode
{
    /// <summary>
    /// Grabs an entity at a specified location and applies corrective impulses to keep the grabbed location near the goal location.
    /// </summary>
    public class MotorizedGrabSpring : Updateable
    {

        SingleEntityLinearMotor linearMotor;
        SingleEntityAngularMotor angularMotor;


        /// <summary>
        /// Constructs a grab constraint.
        /// </summary>
        public MotorizedGrabSpring()
        {
            //Note that when the motor is created using the empty constructor, 
            //it starts deactivated.  This prevents explosions from attempting
            //to update it without being configured.
            linearMotor = new SingleEntityLinearMotor();
            angularMotor = new SingleEntityAngularMotor();
            linearMotor.Settings.Mode = MotorMode.Servomechanism;
            angularMotor.Settings.Mode = MotorMode.Servomechanism;
            //You can configure the stiffness and damping of the corrective springs like so.
            //For this example, the motors will be just be the nearly rigid default.
            linearMotor.Settings.Servo.SpringSettings.StiffnessConstant *= .1f;
            linearMotor.Settings.Servo.SpringSettings.DampingConstant *= .01f;
            //angularMotor.Settings.Servo.SpringSettings.StiffnessConstant = someStiffnessConstant;
            //angularMotor.Settings.Servo.SpringSettings.DampingConstant = someDampingConstant;
            //An unlimited motor will gladly push the entity through other objects.
            //Putting a limit on the strength of the motor will prevent it from doing so.
            linearMotor.Settings.MaximumForce = 10000;

            //The stiffness, damping, and maximum force could be assigned during setup if the motor
            //needs to behave similarly for entities of varying masses.  When using a fixed configuration,
            //the grabspring will behave weakly when trying to move extremely heavy objects, while staying
            //very responsive for sufficiently light objects.

            IsUpdating = false;
        }

        /// <summary>
        /// Gets the grabbed entity.
        /// </summary>
        public Entity Entity
        {
            get
            {
                return linearMotor.Entity;
            }
            private set
            {
                if (linearMotor.Entity != value) //Don't bother changing the entity if it is the same.
                {
                    //The motors can only be on while the entity isn't null.
                    if (value != null)
                    {
                        linearMotor.Entity = value;
                        angularMotor.Entity = value;

                        linearMotor.IsActive = true;
                        angularMotor.IsActive = true;
                        IsUpdating = true;
                    }
                    else
                    {
                        //In v0.14.3, SingleEntity constraints don't take kindly to being given null as an entity.
                        //Instead, give it a "stand-in" entity that it can hold on to while disabled.
                        //The constraint should never be active while holding the world entity.
                        linearMotor.Entity = Constraint.WorldEntity;
                        angularMotor.Entity = Constraint.WorldEntity;

                        linearMotor.IsActive = false;
                        angularMotor.IsActive = false;
                        IsUpdating = false;
                    }
                }
            }
        }

        /// <summary>
        /// Gets the location that the entity will be pulled towards.
        /// </summary>
        public Vector3 GoalPosition
        {
            get
            {
                return linearMotor.Settings.Servo.Goal;
            }
            set
            {
                linearMotor.Settings.Servo.Goal = value;
            }
        }

        /// <summary>
        /// Gets the offset from the entity to the grabbed location in its local space.
        /// </summary>
        public Vector3 LocalOffset
        {
            get
            {
                return linearMotor.LocalPoint;
            }
            private set
            {
                linearMotor.LocalPoint = value;
            }
        }

        /// <summary>
        /// Gets the last updated position of the grab location on the surface of the entity.
        /// </summary>
        public Vector3 GrabbedPosition { get; private set; }

        /// <summary>
        /// Gets whether or not the grabber is currently grabbing something.
        /// </summary>
        public bool IsGrabbing
        {
            get
            {
                return Entity != null;
            }
        }

        /// <summary>
        /// Reinitializes the grabbing constraint with new information.
        /// </summary>
        /// <param name="e">Entity to grab.</param>
        /// <param name="grabLocation">Location on the entity being grabbed in world space.</param>
        public void Setup(Entity e, Vector3 grabLocation)
        {
            Entity = e;
            LocalOffset = Vector3.Transform(grabLocation - e.InternalCenterOfMass, Quaternion.Conjugate(e.OrientationQuaternion));
            angularMotor.Settings.Servo.Goal = e.OrientationQuaternion;
            GoalPosition = grabLocation;
        }

        /// <summary>
        /// Releases the entity being held by the grab spring.
        /// </summary>
        public void Release()
        {
            //In v0.14.3, changing the constraint's involved entities will not 
            //wake up the entities that were in the constraint prior to the change.
            Entity.IsActive = true;
            Entity = null;
        }

        /// <summary>
        /// Updates the grab constraint's grab position after the end of a frame.
        /// </summary>
        /// <param name="dt">Time since last frame in simulation seconds.</param>
        public override void UpdateAtEndOfFrame(float dt)
        {
            //Since the grabbed position is usually examined graphically, 
            //it's good to use the interpolated positions in case the 
            //engine is using internal time stepping and interpolation.
            GrabbedPosition = Vector3.Transform(LocalOffset, Entity.OrientationMatrix) + Entity.CenterPosition;
        }

        public override void OnAdditionToSpace(Space space)
        {
            space.Add(linearMotor);
            space.Add(angularMotor);
        }

        public override void OnRemovalFromSpace()
        {
            space.Remove(linearMotor);
            space.Remove(angularMotor);
        }
    }
}