/** @file
 * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
 * @author Daniel CHAPPUIS
 * @author Edouard DUPIN
 * @copyright 2010-2016, Daniel Chappuis
 * @copyright 2017, Edouard DUPIN
 * @license MPL v2.0 (see license file)
 */
#pragma once

#include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/engine/ConstraintSolver.hpp>

namespace ephysics {
	/**
	 * This structure is used to gather the information needed to create a slider
	 * joint. This structure will be used to create the actual slider joint.
	 */
	struct SliderJointInfo : public JointInfo {
	
		public :
			vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
			vec3 sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
			boolean isLimitEnabled; //!< True if the slider limits are enabled
			boolean isMotorEnabled; //!< True if the slider motor is enabled
			float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
			float maxTranslationLimit; //!< Maximum allowed translation if limits are enabled
			float motorSpeed; //!< Motor speed
			float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
			/** 
			 * @brief Constructor without limits and without motor
			 * @param[in] rigidBody1 The first body of the joint
			 * @param[in] rigidBody2 The second body of the joint
			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
			 * @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
			 */
			SliderJointInfo(RigidBody* rigidBody1,
			                RigidBody* rigidBody2,
			                 vec3 initAnchorPointWorldSpace,
			                 vec3 initSliderAxisWorldSpace):
			  JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
			  sliderAxisWorldSpace(initSliderAxisWorldSpace),
			  isLimitEnabled(false),
			  isMotorEnabled(false),
			  minTranslationLimit(-1.0),
			  maxTranslationLimit(1.0),
			  motorSpeed(0),
			  maxMotorForce(0) {
				
			}
			/**
			 * @brief Constructor with limits and no motor
			 * @param[in] rigidBody1 The first body of the joint
			 * @param[in] rigidBody2 The second body of the joint
			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
			 * @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
			 * @param[in] initMinTranslationLimit The initial minimum translation limit (in meters)
			 * @param[in] initMaxTranslationLimit The initial maximum translation limit (in meters)
			 */
			SliderJointInfo(RigidBody* rigidBody1,
			                RigidBody* rigidBody2,
			                 vec3 initAnchorPointWorldSpace,
			                 vec3 initSliderAxisWorldSpace,
			                float initMinTranslationLimit,
			                float initMaxTranslationLimit):
			  JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
			  sliderAxisWorldSpace(initSliderAxisWorldSpace),
			  isLimitEnabled(true),
			  isMotorEnabled(false),
			  minTranslationLimit(initMinTranslationLimit),
			  maxTranslationLimit(initMaxTranslationLimit),
			  motorSpeed(0),
			  maxMotorForce(0) {
				
			}
			/**
			 * @brief Constructor with limits and motor
			 * @param[in] rigidBody1 The first body of the joint
			 * @param[in] rigidBody2 The second body of the joint
			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
			 * @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
			 * @param[in] initMinTranslationLimit The initial minimum translation limit (in meters)
			 * @param[in] initMaxTranslationLimit The initial maximum translation limit (in meters)
			 * @param[in] initMotorSpeed The initial speed of the joint motor (in meters per second)
			 * @param[in] initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters)
			 */
			SliderJointInfo(RigidBody* rigidBody1,
			                RigidBody* rigidBody2,
			                 vec3 initAnchorPointWorldSpace,
			                 vec3 initSliderAxisWorldSpace,
			                float initMinTranslationLimit,
			                float initMaxTranslationLimit,
			                float initMotorSpeed,
			                float initMaxMotorForce):
			  JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
			  sliderAxisWorldSpace(initSliderAxisWorldSpace),
			  isLimitEnabled(true),
			  isMotorEnabled(true),
			  minTranslationLimit(initMinTranslationLimit),
			  maxTranslationLimit(initMaxTranslationLimit),
			  motorSpeed(initMotorSpeed),
			  maxMotorForce(initMaxMotorForce) {
				
			}
	};
	
	/**
	 * @brief This class represents a slider joint. This joint has a one degree of freedom.
	 * It only allows relative translation of the bodies along a single direction and no
	 * rotation.
	 */
	class SliderJoint: public Joint {
		private:
			static  float BETA; //!< Beta value for the position correction bias factor
			vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
			vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
			vec3 this.sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
			etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
			etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates)
			etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
			vec3 this.N1; //!< First vector orthogonal to the slider axis local-space of body 1
			vec3 this.N2; //!< Second vector orthogonal to the slider axis and this.N1 in local-space of body 1
			vec3 this.R1; //!< Vector r1 in world-space coordinates
			vec3 this.R2; //!< Vector r2 in world-space coordinates
			vec3 this.R2CrossN1; //!< Cross product of r2 and n1
			vec3 this.R2CrossN2; //!< Cross product of r2 and n2
			vec3 this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis
			vec3 this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
			vec3 this.R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2
			vec3 this.R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
			vec2 this.bTranslation; //!< Bias of the 2 translation raints
			vec3 this.bRotation; //!< Bias of the 3 rotation raints
			float this.bLowerLimit; //!< Bias of the lower limit raint
			float this.bUpperLimit; //!< Bias of the upper limit raint
			etk::Matrix2x2 this.inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
			etk::Matrix3x3 this.inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
			float this.inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix)
			float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
			vec2 this.impulseTranslation; //!< Accumulated impulse for the 2 translation raints
			vec3 this.impulseRotation; //!< Accumulated impulse for the 3 rotation raints
			float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
			float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
			float this.impulseMotor; //!< Accumulated impulse for the motor
			boolean this.isLimitEnabled; //!< True if the slider limits are enabled
			boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled
			vec3 this.sliderAxisWorld; //!< Slider axis in world-space coordinates
			float this.lowerLimit; //!< Lower limit (minimum translation distance)
			float this.upperLimit; //!< Upper limit (maximum translation distance)
			boolean this.isLowerLimitViolated; //!< True if the lower limit is violated
			boolean this.isUpperLimitViolated; //!< True if the upper limit is violated
			float this.motorSpeed; //!< Motor speed (in m/s)
			float this.maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
			/// Private copy-ructor
			SliderJoint( SliderJoint raint);
			/// Private assignment operator
			SliderJoint operator=( SliderJoint raint);
			/// Reset the limits
			void resetLimits();
			sizet getSizeInBytes()  override;
			void initBeforeSolve( ConstraintSolverData raintSolverData) override;
			void warmstart( ConstraintSolverData raintSolverData) override;
			void solveVelocityConstraint( ConstraintSolverData raintSolverData) override;
			void solvePositionConstraint( ConstraintSolverData raintSolverData) override;
		public :
			/// Constructor
			SliderJoint( SliderJointInfo jointInfo);
			/// Destructor
			virtual ~SliderJoint();
			/// Return true if the limits or the joint are enabled
			boolean isLimitEnabled() ;
			/// Return true if the motor of the joint is enabled
			boolean isMotorEnabled() ;
			/// Enable/Disable the limits of the joint
			void enableLimit(boolean isLimitEnabled);
			/// Enable/Disable the motor of the joint
			void enableMotor(boolean isMotorEnabled);
			/// Return the current translation value of the joint
			float getTranslation() ;
			/// Return the minimum translation limit
			float getMinTranslationLimit() ;
			/// Set the minimum translation limit
			void setMinTranslationLimit(float lowerLimit);
			/// Return the maximum translation limit
			float getMaxTranslationLimit() ;
			/// Set the maximum translation limit
			void setMaxTranslationLimit(float upperLimit);
			/// Return the motor speed
			float getMotorSpeed() ;
			/// Set the motor speed
			void setMotorSpeed(float motorSpeed);
			/// Return the maximum motor force
			float getMaxMotorForce() ;
			/// Set the maximum motor force
			void setMaxMotorForce(float maxMotorForce);
			/// Return the intensity of the current force applied for the joint motor
			float getMotorForce(float timeStep) ;
	};


}