/*
      Copyright (C) 2010 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.Entities;
using Microsoft.Xna.Framework;
using System.Collections.Generic;
using BEPUphysics;
using BEPUphysics.Constraints;

namespace BEPUphysicsDemos.Demos
{
    /// <summary>
    /// Demo showing a wall of blocks stacked up.
    /// </summary>
    public class TestDemo : StandardDemo
    {

        float MaxAngle = -MathHelper.PiOver2;
        RevoluteMotor TopMotor;
        RevoluteMotor BottomMotor;

        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public TestDemo(DemosGame game)
            : base(game)
        {
            var palm = new Box(new Vector3(0, 0, 0), 1.5f, 1.5f, .5f, 10);
            
            var constraints = new List<SolverUpdateable>();

            float bottomHeight = 1f;
            float topHeight = 1f;
            float xOffset = .5f;
            float width = .5f;
            float depth = .5f;

            #region Bottom Part
            //
            float YOffset = ((Box)palm).HalfHeight; // Move to top of palm.
            YOffset += bottomHeight / 2; // Move up to center pos of bottom part.

            Vector3 offset = new Vector3(xOffset, YOffset, 0);

            var Bottom = new Box(palm.CenterPosition + offset, width, bottomHeight, depth, 0.1f);

            var BottomPalmJoint = new RevoluteJoint(palm, Bottom, Bottom.CenterPosition - new Vector3(0, Bottom.HalfHeight, 0), Vector3.UnitX);

            BottomPalmJoint.Limit.MinimumAngle = -MathHelper.PiOver2;
            BottomPalmJoint.Limit.MaximumAngle = 0;
            BottomPalmJoint.Limit.IsActive = true;

            BottomMotor = BottomPalmJoint.Motor;


            BottomMotor.IsActive = true;
            BottomMotor.Settings.Mode = MotorMode.Servomechanism;
            BottomMotor.Settings.Servo.Goal = -MathHelper.PiOver4;

            constraints.Add(BottomPalmJoint);
            #endregion

            #region Top Part
            //
            YOffset = bottomHeight / 2;  // Move to top of bottom part.
            YOffset += topHeight / 2; // Move to center pos of top part.

            offset = new Vector3(0, YOffset, 0);

            var Top = new Box(Bottom.CenterPosition + offset, width, topHeight, depth, 0.1f);

            var TopBottomJoint = new RevoluteJoint(Bottom, Top, Top.CenterPosition - new Vector3(0, Top.HalfHeight, 0), Vector3.UnitX);

            TopBottomJoint.Limit.MinimumAngle = -MathHelper.PiOver2;
            TopBottomJoint.Limit.MaximumAngle = 0;
            TopBottomJoint.Limit.IsActive = true;

            TopMotor = TopBottomJoint.Motor;

            TopMotor.IsActive = true;
            TopMotor.Settings.Mode = MotorMode.Servomechanism;
            TopMotor.Settings.Servo.Goal = -MathHelper.PiOver4;        

            constraints.Add(TopBottomJoint);
            #endregion

            #region Collision Rules
            Top.CollisionRules.SpecificEntities.Add(Bottom, CollisionRule.NoPair);
            Top.CollisionRules.SpecificEntities.Add(palm, CollisionRule.NoPair);
            Bottom.CollisionRules.SpecificEntities.Add(palm, CollisionRule.NoPair);
            #endregion

            Top.CollisionMargin = 0.001f;
            Bottom.CollisionMargin = 0.001f;

            foreach (SolverUpdateable s in constraints)
            {
                Space.Add(s);
            }
            Space.Add(palm);
            Space.Add(Top);
            Space.Add(Bottom);

            Space.Add(new Box(new Vector3(0, -.5f, 0), 50f, 1, 50f));
            game.Camera.Position = new Vector3(0, 6, 15);
        }

        /// <summary>
        /// Gets the name of the simulation.
        /// </summary>
        public override string Name
        {
            get { return "Wall"; }
        }

        public override void Update(float dt)
        {
            if (Game.KeyboardInput.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.Up))
            {
                TopMotor.Settings.Servo.Goal = MathHelper.Max(TopMotor.Settings.Servo.Goal - 1 * dt, MaxAngle);
                BottomMotor.Settings.Servo.Goal = MathHelper.Max(BottomMotor.Settings.Servo.Goal - 1 * dt, MaxAngle);
            }

            if (Game.KeyboardInput.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.Down))
            {
                TopMotor.Settings.Servo.Goal = MathHelper.Min(TopMotor.Settings.Servo.Goal + 1 * dt, 0);
                BottomMotor.Settings.Servo.Goal = MathHelper.Min(BottomMotor.Settings.Servo.Goal + 1 * dt, 0);
            }
            base.Update(dt);
        }
    }
}