

local ents 						= ents
local util						= util
local SERVER 					= SERVER
local CLIENT 					= CLIENT
local table 					= table
local tostring 					= tostring
local Vector 					= Vector
local pairs 					= pairs
local math 						= math
local duplicator 				= duplicator
local numpad 					= numpad
local SetPhysConstraintSystem 	= SetPhysConstraintSystem
local NULL 						= NULL
local CreateConVar 				= CreateConVar
local GetConVarNumber			= GetConVarNumber
local gamemode					= gamemode
local GetWorldEntity			= GetWorldEntity
local SOLID_NONE				= SOLID_NONE
local Msg						= Msg
local PrintTable					= PrintTable


if ( SERVER ) then

	// If you're a server admin and you want your physics to spazz out less you can
	// use the convar. The higher you set it the more accurate physics will be.
	// This is set to 2 by default, since we are a physics mod.

	CreateConVar( "gmod_physiterations", "2", { FCVAR_REPLICATED, FCVAR_ARCHIVE } )
	
end


module("constraint")

// Clients don't need this module.
if ( CLIENT ) then return end


// ! NOTE !
// The constraint systems don't get deleted yet. Don't do anything about it yet.
// ! NOTE !

// I think 128 constraints is around the max that causes the crash
// So at this number we'll refuse to add more to the system
local MAX_CONSTRAINTS_PER_SYSTEM 	= 100 

local function CreateConstraintSystem()

		local iterations = GetConVarNumber( "gmod_physiterations" )
		
		local System = ents.Create("phys_constraintsystem")
			System:SetKeyValue( "additionaliterations", iterations )
		System:Spawn()
		System:Activate()
		
		return System
		
end

/*----------------------------------------------------------------------
	FindOrCreateConstraintSystem
	
	Takes 2 entities. If the entities don't have a constraint system
	associated with them it creates one and associates it with them.
	
	It then returns the constraint system
----------------------------------------------------------------------*/
local function FindOrCreateConstraintSystem( Ent1, Ent2 )

	local System = nil
	
	Ent2 = Ent2 or Ent1

	// Does Ent1 have a constraint system?
	if ( !Ent1:IsWorld() && Ent1:GetTable().ConstraintSystem && Ent1:GetTable().ConstraintSystem:IsValid() ) then 
		System = Ent1:GetTable().ConstraintSystem
	end
	
	// Don't add to this system - we have too many constraints on it already.
	if ( System && System:IsValid() && System:GetVar( "constraints", 0 ) > MAX_CONSTRAINTS_PER_SYSTEM ) then System = nil end
	
	// Does Ent2 have a constraint system?
	if ( !System && !Ent2:IsWorld() && Ent2:GetTable().ConstraintSystem && Ent2:GetTable().ConstraintSystem:IsValid() ) then 
		System = Ent2:GetTable().ConstraintSystem
	end
	
	// Don't add to this system - we have too many constraints on it already.
	if ( System && System:IsValid() && System:GetVar( "constraints", 0 ) > MAX_CONSTRAINTS_PER_SYSTEM ) then System = nil end
	
	// No constraint system yet (Or they're both full) - make a new one
	if ( !System || !System:IsValid() ) then
	
		//Msg("New Constrant System\n")
		System = CreateConstraintSystem()
		
	end
	
	Ent1:GetTable().ConstraintSystem = System
	Ent2:GetTable().ConstraintSystem = System
	
	local ConstraintNum = System:GetVar( "constraints", 0 )
	System:SetVar( "constraints", ConstraintNum + 1 )
	
	//Msg("System has "..tostring( System:GetVar( "constraints", 0 ) ).." constraints\n")
	
	return System

end


/*----------------------------------------------------------------------
	onStartConstraint( Ent1, Ent2 )
	Should be called before creating a constraint
----------------------------------------------------------------------*/
local function onStartConstraint( Ent1, Ent2 )

	// Get constraint system
	local system = FindOrCreateConstraintSystem( Ent1, Ent2 )
	
	// Any constraints called after this call will use this system
	SetPhysConstraintSystem( system )

end

/*----------------------------------------------------------------------
	onFinishConstraint( Ent1, Ent2 )
	Should be called before creating a constraint
----------------------------------------------------------------------*/
local function onFinishConstraint( Ent1, Ent2 )

	// Turn off constraint system override
	SetPhysConstraintSystem( NULL )

end

local function SetPhysicsCollisions( Ent, b )

	if (!Ent || !Ent:IsValid() || !Ent:GetPhysicsObject()) then return end
	
	Ent:GetPhysicsObject():EnableCollisions( b )

end

/*----------------------------------------------------------------------
	RemoveConstraints( Ent, Type )
	Removes all constraints of type from entity
----------------------------------------------------------------------*/
function RemoveConstraints( Ent, Type )

	if ( !Ent:GetTable().Constraints ) then return end
	
	local c = Ent:GetTable().Constraints
	i = 0

	for k, v in pairs( c ) do
	
		if ( !v:IsValid() ) then
		
			c[ k ] = nil
	
		elseif (  v:GetTable().Type == Type ) then
		
			// Make sure physics collisions are on!
			// If we don't the unconstrained objects will fall through the world forever.
			SetPhysicsCollisions( v:GetTable().Ent1, true )
			SetPhysicsCollisions( v:GetTable().Ent2, true )
		
			c[ k ] = nil
			v:Remove()			
			
			i = i + 1
		end
		
	end

	if (table.Count(c) == 0) then
		Ent:GetTable().Constraints		= nil
	end

	local  bool = i != 0
	return bool, i

end


/*----------------------------------------------------------------------
	RemoveConstraints( Ent )
	Removes all constraints from entity
----------------------------------------------------------------------*/
function RemoveAll( Ent )

	if ( !Ent:GetTable().Constraints ) then return end
	
	local c = Ent:GetTable().Constraints
	local i = 0
	for k, v in pairs( c ) do
	
		if ( !v:IsValid() ) then
		
			c[ k ] = nil
	
		else
		
			// Make sure physics collisions are on!
			// If we don't the unconstrained objects will fall through the world forever.
			SetPhysicsCollisions( v:GetTable().Ent1, true )
			SetPhysicsCollisions( v:GetTable().Ent2, true )
		
			c[ k ] = nil
			v:Remove()
			i = i + 1
		end
		
	end
	Ent:GetTable().Constraints		= nil
	
	local  bool = i != 0
	return bool, i

end

/*----------------------------------------------------------------------
	Find( Ent1, Ent2, Type, Bone1, Bone2 )
	Removes all constraints from entity
----------------------------------------------------------------------*/
function Find( Ent1, Ent2, Type, Bone1, Bone2  )

	if ( !Ent1:GetTable().Constraints ) then return end
	
	local c = Ent1:GetTable().Constraints

	for k, v in pairs( Ent1:GetTable().Constraints ) do
	
		if ( v:IsValid() ) then
	
			local CTab = v:GetTable()
			
			if ( CTab.Type == Type && CTab.Ent1 == Ent1 && CTab.Ent2 == Ent2 && CTab.Bone1 == Bone1 && CTab.Bone2 == Bone2 ) then
				return v
			end
			
			if ( CTab.Type == Type && CTab.Ent2 == Ent1 && CTab.Ent1 == Ent2 && CTab.Bone2 == Bone1 && CTab.Bone1 == Bone2 ) then
				return v
			end

		end
		
	end

	return nil

end

/*----------------------------------------------------------------------
	CanConstrain( Ent, Bone )
	Returns false if we shouldn't be constraining this entity
----------------------------------------------------------------------*/
function CanConstrain( Ent, Bone )

	if ( !Ent )	then return false end
	if ( !Ent:IsWorld() && !Ent:IsValid() )	then return false end
	if ( !Ent:GetPhysicsObjectNum( Bone ):IsValid() )	then return false end
	
	return true

end

/*----------------------------------------------------------------------
	CalcElasticConsts( ... )
	This attempts to scale the elastic constraints such as the winch
	to keep a stable but responsive constraint..
----------------------------------------------------------------------*/
local function CalcElasticConsts(Phys1, Phys2, Ent1, Ent2)
	
	local minMass = 0;
	
	if ( Ent1:IsWorld() ) then minMass = Phys2:GetMass() end
	if ( Ent2:IsWorld() ) then minMass = Phys1:GetMass() end
	
	if ( minMass == 0 ) then
		minMass = math.min( Phys1:GetMass(), Phys2:GetMass() )
	end
	
	return minMass * 50, 100
	
end


/*----------------------------------------------------------------------
	CreateKeyframeRope( ... )
	Creates a rope without any constraint
----------------------------------------------------------------------*/
function CreateKeyframeRope( Pos, width, material, Constraint, Ent1, LPos1, Bone1, Ent2, LPos2, Bone2, kv )

	// No rope if 0 or minus
	if ( width < 1 ) then return nil end
	
	// Clamp the rope to a sensible width
	width = math.Clamp( width, 1, 100 )

	local rope = ents.Create( "keyframe_rope" )
		
		rope:SetPos( Pos )
		rope:SetKeyValue( "Width", 			width )
		if (material) then rope:SetKeyValue( "RopeMaterial", material ) end
		
		// Attachment point 1
		rope:SetEntity( "StartEntity", 		Ent1 )
		rope:SetKeyValue( "StartOffset", 	tostring(LPos1) )
		rope:SetKeyValue( "StartBone", 		Bone1 )
		
		// Attachment point 2
		rope:SetEntity( "EndEntity", 		Ent2 )
		rope:SetKeyValue( "EndOffset", 		tostring(LPos2) )
		rope:SetKeyValue( "EndBone", 		Bone2 )
			
		if (kv ) then
			for k, v in pairs( kv ) do
			
				rope:SetKeyValue( k, tostring(v) )
		
			end
		end
			
		rope:Spawn()
		rope:Activate()
		
	// Delete the rope if the attachments get killed
	Ent1:DeleteOnRemove( rope )
	Ent2:DeleteOnRemove( rope )
	if ( Constraint ) then Constraint:DeleteOnRemove( rope ) end
		
	return rope

end

/*----------------------------------------------------------------------
	AddConstraintTable( Ent, constraint )
	Stores info about the constraints on the entity's table
----------------------------------------------------------------------*/
function AddConstraintTable(  Ent, constraint, Ent2, Ent3, Ent4 )

	if ( !constraint || !constraint:IsValid() ) then return end

	if ( Ent:IsValid() ) then

		Ent:GetTable().Constraints = Ent:GetTable().Constraints or {}
		table.insert( Ent:GetTable().Constraints, constraint )
		Ent:DeleteOnRemove( constraint )
	
	end
	
	if (Ent2 && Ent2!=Ent) then
		AddConstraintTable( Ent2, constraint, Ent3, Ent4 )
	end

end

	
/*----------------------------------------------------------------------
	Weld( ... )
	Creates a solid weld constraint
----------------------------------------------------------------------*/
function Weld( Ent1, Ent2, Bone1, Bone2, forcelimit, nocollide )
	
	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	if ( Find( Ent1, Ent2, "Weld", Bone1, Bone2 ) ) then 
	
		// A weld already exists between these two physics objects.
		// There's totally no point in re-creating it. It doesn't make 
		// the weld any stronger - that's just an urban ledgend.
		return false
	
	end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 ) 
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2 )

	if (Ent1 == Ent2 && Bone1 == Bone2) then return false end
	
	onStartConstraint( Ent1, Ent2 )

		// Create the constraint 
		local constraint = ents.Create("phys_constraint")
			if (forcelimit)	then constraint:SetKeyValue( "forcelimit", forcelimit )	  end
			if (nocollide) && Bone1 == 0 && Bone2 == 0	then constraint:SetKeyValue( "spawnflags", 1 )			  end
			constraint:SetPhysConstraintObjects( Phys2, Phys1 )
		constraint:Spawn()
		constraint:Activate()

	// If either object is a ragdoll then don't use the spawnflag method to nocollide
	// as it doesn't take into accound phys bones
	if (nocollide && ( Bone1 != 0 || Bone2 != 0 )) then

		local nocollide = NoCollide( Ent1, Ent2, Bone1, Bone2 )
		constraint.DeleteOnRemove( nocollide )
	end

	onFinishConstraint( Ent1, Ent2 )
	AddConstraintTable( Ent1, constraint, Ent2 )

	// Make a constraints table
	local ctable  = 
		{
			Type  = "Weld",
			Ent1  = Ent1, 	Ent2  = Ent2,
			Bone1 = Bone1,	Bone2 = Bone2,
			forcelimit	= forcelimit,
			nocollide	= nocollide
		}

	constraint:SetTable( ctable )
	
	
	Phys1:Wake()
	Phys2:Wake()


	return constraint
	
end

duplicator.RegisterConstraint( "Weld", Weld, "Ent1", "Ent2", "Bone1", "Bone2", "forcelimit", "nocollide" )


/*----------------------------------------------------------------------
	Rope( ... )
	Creates a rope constraint - with rope!
----------------------------------------------------------------------*/
function Rope( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, length, addlength, forcelimit, width, material, rigid )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end

	local Phys1 		= Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 		= Ent2:GetPhysicsObjectNum( Bone2 )
	local WPos1 		= Phys1:LocalToWorld( LPos1 )
	local WPos2 		= Phys2:LocalToWorld( LPos2 )
	local addlength		= addlength or 0
	local constraint	= nil

	length = length + addlength - (WPos1-WPos2):Length()
	
	// Make Constraint
	if ( Phys1 != Phys2 ) then
			
		onStartConstraint( Ent1, Ent2 )

			// Create the constraint
			constraint = ents.Create("phys_lengthconstraint")
				constraint:SetPos( WPos1 )		
				constraint:SetKeyValue( "attachpoint", tostring(WPos2) )
				constraint:SetKeyValue( "minlength", "0.0" )
				if (length)	 		then constraint:SetKeyValue( "addlength", length ) end
				if (forcelimit)	 	then constraint:SetKeyValue( "forcelimit", forcelimit )	  end
				if (rigid) 			then constraint:SetKeyValue( "spawnflags", 2 ) end
				constraint:SetPhysConstraintObjects( Phys1, Phys2 )
			constraint:Spawn()
			constraint:Activate()

		onFinishConstraint( Ent1, Ent2 )

	end

	// Make Rope
	local kv = 
	{ 
		Slack 		= length, 
		Collide 	= 1
	}
	if ( rigid ) then kv.Type = 2 end
	
	local rope = CreateKeyframeRope( WPos1, width, material, constraint, Ent1, LPos1, Bone1, Ent2, LPos2, Bone2, kv )

	if !constraint then constraint, rope = rope, nil end

	local ctable = 
		{
			Type 		= "Rope",
			Ent1 		= Ent1,		Ent2 		= Ent2,
			Bone1 		= Bone1,	Bone2 		= Bone2,
			LPos1 		= LPos1,	LPos2 		= LPos2,
			length 		= (WPos1 - WPos2):Length(),
			addlength 	= addlength,
			forcelimit 	= forcelimit,
			width 		= width,
			material 	= material,
			rigid		= rigid
		}
	constraint:SetTable( ctable )

	AddConstraintTable( Ent1, constraint, Ent2 )

	return constraint, rope
	
end

duplicator.RegisterConstraint( "Rope", Rope, "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "length", "addlength", "forcelimit", "width", "material", "rigid" )

/*----------------------------------------------------------------------
	Elastic( ... )
	Creates an elastic constraint
----------------------------------------------------------------------*/
function Elastic( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, constant, damping, rdamping, material, width, stretchonly )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end

	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2)
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos2 = Phys2:LocalToWorld( LPos2 )

	local constraint 	= nil

	// Make Constraint
	if ( Phys1 != Phys2 ) then
	
		onStartConstraint( Ent1, Ent2 )
		
			constraint = ents.Create("phys_spring")
			
				constraint:SetPos( WPos1 )		
				constraint:SetKeyValue( "springaxis", tostring(WPos2) )
				constraint:SetKeyValue( "constant", constant )
				constraint:SetKeyValue( "damping", damping )
				constraint:SetKeyValue( "relativedamping", rdamping )
				constraint:SetPhysConstraintObjects( Phys1, Phys2 )
				if (stretchonly == 1 || stretchonly == true) then
					constraint:SetKeyValue( "spawnflags", 1 )					
				end
	
			constraint:Spawn()
			constraint:Activate()
		
		onFinishConstraint( Ent1, Ent2 )
		AddConstraintTable( Ent1, constraint, Ent2 )
		
		local ctable = 
		{
			Type = "Elastic",

			Ent1	= Ent1,
			Ent2	= Ent2,
			Bone1	= Bone1,
			Bone2	= Bone2,
			LPos1	= LPos1,
			LPos2	= LPos2,

			constant	= constant,
			damping		= damping,
			rdamping	= rdamping,
			material	= material,
			width		= width,
			length  	= (WPos1-WPos2):Length(),
			stretchonly = stretchonly,
		}

		constraint:SetTable( ctable )
	
	end

	// Make Rope
	local kv = 
	{ 
		Collide 	= 1,
		Type 		= 0
	}
	
	local rope = CreateKeyframeRope( WPos1, width, material, constraint, Ent1, LPos1, Bone1, Ent2, LPos2, Bone2, kv )
	

	return constraint, rope
end

duplicator.RegisterConstraint("Elastic", Elastic, "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "constant", "damping", "rdamping", "material", "width", "stretchonly")


/*----------------------------------------------------------------------
	Keepupright( ... )
	Creates a KeepUpright constraint
----------------------------------------------------------------------*/
function Keepupright( Ent, Ang, Bone, angularlimit )

	if ( !CanConstrain( Ent, Bone ) ) then return false end
	if ( Ent:GetClass() != "prop_physics" && Ent:GetClass() != "prop_ragdoll" ) then return false end
	if ( !angularlimit or angularlimit < 0 ) then return end
	
	local Phys = Ent:GetPhysicsObjectNum(Bone)

	// Remove any KU's already on entity
	RemoveConstraints( Ent, "Keepupright" )
	
	onStartConstraint( Ent )

		local constraint = ents.Create( "phys_keepupright" )
			constraint:SetAngles( Ang )
			constraint:SetKeyValue( "angularlimit", angularlimit )
			constraint:SetPhysConstraintObjects( Phys, Phys )
		constraint:Spawn()
		constraint:Activate()
	
	onFinishConstraint( Ent )
	AddConstraintTable( Ent, constraint )
	
	local ctable = 
	{
		Type 			= "Keepupright",
		Ent 			= Ent,
		Ang 			= Ang,
		Bone 			= Bone,
		angularlimit 	= angularlimit
	}
	constraint:SetTable( ctable )
	
	return constraint
	
end

duplicator.RegisterConstraint( "Keepupright", Keepupright, "Ent", "Ang", "Bone", "angularlimit" )


function CreateStaticAnchorPoint( Pos )

	// Creates an invisible frozen, not interactive prop.
	
	Anchor = ents.Create( "prop_physics" )
		Anchor:SetPos( Pos )
		Anchor:SetModel( "models/Combine_Helicopter/helicopter_bomb01.mdl" ) 
		Anchor:DrawShadow( false )
		Anchor:SetNoDraw( true )
	Anchor:Spawn()
	Anchor:Activate()
	
	// Non solid - we can't allow the physgun to 
	// pick this up or the illusion is ruined!
	Anchor:SetSolid( SOLID_NONE )
	
	local Phys = Anchor:GetPhysicsObject()
	if ( Phys ) then
		Phys:EnableMotion( false )
		Phys:EnableCollisions( false )
	end
	
	return Anchor, Phys, 0, Vector(0,0,0)

end


/*----------------------------------------------------------------------
	Slider( ... )
	Creates a slider constraint
----------------------------------------------------------------------*/
function Slider( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, width )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2)
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos2 = Phys2:LocalToWorld( LPos2 )
	local StaticAnchor = nil

	// Make Constraint
	if ( Phys1 == Phys2 ) then return end
	
	// Make Rope
	local kv = 
	{ 
		Collide 	= 0,
		Type 		= 2,
		Subdiv		= 1,
	}
	
	local rope = CreateKeyframeRope( WPos1, width, material, constraint, Ent1, LPos1, Bone1, Ent2, LPos2, Bone2, kv )
	
	
	// Start World Hack.
	// Attaching a slider to the world makes it really sucks so we make
	// a prop and attach to that.
	
	if ( Ent1:IsWorld() ) then
		
		Ent1, Phys1, Bone1, LPos1 = CreateStaticAnchorPoint( WPos1 );
		StaticAnchor = Ent1
	
	end
	
	if ( Ent2:IsWorld() ) then
		
		Ent2, Phys2, Bone2, LPos2 = CreateStaticAnchorPoint( WPos2 );
		StaticAnchor = Ent2
	
	end
	
	// End World Hack.
	
	onStartConstraint( Ent1, Ent2 )

		local constraint = ents.Create("phys_slideconstraint")
			constraint:SetPos( WPos1 )
			constraint:SetKeyValue( "slideaxis", tostring(WPos2) ) 
			constraint:SetPhysConstraintObjects( Phys1, Phys2 )
		constraint:Spawn()
		constraint:Activate()
		
	onFinishConstraint( Ent1, Ent2 )
	AddConstraintTable( Ent1, constraint, Ent2 )
	
	// If we have a static anchor - delete it when we die.
	if ( StaticAnchor ) then
	
		constraint:DeleteOnRemove( StaticAnchor )
	
	end
	
	local ctable = 
	{
		Type 	= "Slider",
		Ent1  	= Ent1,
		Ent2  	= Ent2,
		Bone1 	= Bone1,
		Bone2 	= Bone2,
		LPos1 	= LPos1,
		LPos2 	= LPos2,
		width 	= width
	}

	constraint:SetTable( ctable )	
	
	return constraint, rope
	
end
duplicator.RegisterConstraint( "Slider", Slider, "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "width" )

/*----------------------------------------------------------------------
	Axis( ... )
	Creates an axis constraint
----------------------------------------------------------------------*/
function Axis( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, forcelimit, torquelimit, friction, nocollide )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2)
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos2 = Phys2:LocalToWorld( LPos2 )

	if ( Phys1 == Phys2 ) then return false end

	onStartConstraint( Ent1, Ent2 )
	
		local constraint = ents.Create("phys_hinge")
			constraint:SetPos( WPos1 )
			constraint:SetKeyValue( "hingeaxis", tostring( WPos2 ) ) 
			if (forcelimit && forcelimit > 0)	then constraint:SetKeyValue( "forcelimit", forcelimit )		 end
			if (torquelimit && torquelimit > 0)	then constraint:SetKeyValue( "torquelimit", torquelimit )	 end
			if (friction && friction > 0)		then constraint:SetKeyValue( "hingefriction", friction )	 end
			if (nocollide && nocollide > 0)		then constraint:SetKeyValue( "spawnflags", 1 )			 end
			constraint:SetPhysConstraintObjects( Phys1, Phys2 )
		constraint:Spawn()
		constraint:Activate()
	
	onFinishConstraint( Ent1, Ent2 )
	AddConstraintTable( Ent1, constraint, Ent2 )
	
	local ctable = {
		Type 			= "Axis",
		Ent1  			= Ent1,
		Ent2  			= Ent2,
		Bone1 			= Bone1,
		Bone2 			= Bone2,
		LPos1 			= LPos1,
		LPos2 			= LPos2,
		forcelimit  	= forcelimit, 
		torquelimit 	= torquelimit,
		friction    	= friction,
		nocollide   	= nocollide
	}

	constraint:SetTable( ctable )

	return constraint
	
end

duplicator.RegisterConstraint( "Axis", Axis, "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "forcelimit", "torquelimit", "friction", "nocollide" )


/*----------------------------------------------------------------------
	AdvBallsocket( ... )
	Creates an advanced ballsocket (ragdoll) constraint
----------------------------------------------------------------------*/
function AdvBallsocket(Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, forcelimit, torquelimit, xmin, ymin, zmin, xmax, ymax, zmax, xfric, yfric, zfric, onlyrotation, nocollide)

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2 )
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos2 = Phys2:LocalToWorld( LPos2 )

	if ( Phys1 == Phys2 ) then return false end
	
	// Make Constraint
	onStartConstraint( Ent1, Ent2 )
	
		local flags = 0
		if (onlyrotation && onlyrotation > 0) 	then flags = flags + 2 end
		if (nocollide && nocollide > 0) 		then flags = flags + 1 end
	
		local constraint = ents.Create("phys_ragdollconstraint")
			constraint:SetPos( WPos1 )
			constraint:SetKeyValue( "xmin", xmin )
			constraint:SetKeyValue( "xmax", xmax )
			constraint:SetKeyValue( "ymin", ymin )
			constraint:SetKeyValue( "ymax", ymax )
			constraint:SetKeyValue( "zmin", zmin )
			constraint:SetKeyValue( "zmax", zmax )
			if (xfric && xfric > 0) then constraint:SetKeyValue( "xfriction", xfric ) end
			if (yfric && yfric > 0) then constraint:SetKeyValue( "yfriction", yfric ) end
			if (zfric && zfric > 0) then constraint:SetKeyValue( "zfriction", zfric ) end
			if (forcelimit && forcelimit > 0) then constraint:SetKeyValue( "forcelimit", forcelimit ) end
			if (torquelimit && torquelimit > 0) then constraint:SetKeyValue( "torquelimit", torquelimit ) end
			constraint:SetKeyValue("spawnflags", flags)
			constraint:SetPhysConstraintObjects( Phys1, Phys2 )
		constraint:Spawn()
		constraint:Activate()
	
	onFinishConstraint( Ent1, Ent2 )
	AddConstraintTable( Ent1, constraint, Ent2 )
	
	local ctable = 
	{
		Type 			= "AdvBallsocket",
		Ent1  			= Ent1,
		Ent2 			= Ent2,
		Bone1 			= Bone1,
		Bone2 			= Bone2,
		LPos1 			= LPos1,
		LPos2 			= LPos2,
		forcelimit  	= forcelimit, 
		torquelimit 	= torquelimit,
		xmin 			= xmin,
		ymin 			= ymin,
		zmin 			= zmin,
		xmax 			= xmax,
		ymax 			= ymax,
		zmax 			= zmax,
		xfric 			= xfric,
		yfric 			= yfric,
		zfric 			= zfric,
		onlyrotation 	= onlyrotation,
		nocollide   	= nocollide
	}

	constraint:SetTable( ctable )

	return constraint
	
end

duplicator.RegisterConstraint( "AdvBallsocket", AdvBallsocket, "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "forcelimit", "torquelimit", "xmin", "ymin", "zmin", "xmax", "ymax", "zmax", "xfric", "yfric", "zfric", "onlyrotation", "nocollide")


/*----------------------------------------------------------------------
	NoCollide( ... )
	Creates an nocollide `constraint'
----------------------------------------------------------------------*/
function NoCollide( Ent1, Ent2, Bone1, Bone2 )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2 )

	if ( Phys1 == Phys2 ) then return false end

	if ( Find( Ent1, Ent2, "NoCollide", Bone1, Bone2 ) ) then 

		return false
	
	end

	// Make Constraint
	onStartConstraint( Ent1, Ent2 )
	
		local constraint = ents.Create("logic_collision_pair")
			constraint:SetKeyValue( "startdisabled", 1 )
			constraint:SetPhysConstraintObjects( Phys1, Phys2 )
		constraint:Spawn()
		constraint:Activate()
		constraint:Input( "DisableCollisions", nil, nil, nil )
	
	onFinishConstraint( Ent1, Ent2 )
	AddConstraintTable( Ent1, constraint, Ent2 )
	
	local ctable  = 
	{
		Type  = "NoCollide",
		Ent1  = Ent1,
		Ent2  = Ent2,
		Bone1 = Bone1,
		Bone2 = Bone2,
	}

	constraint:SetTable( ctable )

	return constraint

end
duplicator.RegisterConstraint( "NoCollide", NoCollide, "Ent1", "Ent2", "Bone1", "Bone2" )


/*----------------------------------------------------------------------
	MotorControl( pl, motor, onoff, dir )
	Numpad controls for the motor constraints
----------------------------------------------------------------------*/
local function MotorControl( pl, motor, onoff, dir )

	if (!motor:IsValid()) then return false end

	local activate = false
	
	if (motor:GetTable().toggle == 1) then
		
		// Toggle mode, only do something when the key is pressed
		// if the motor is off, turn it on, and vice-versa.
		// This only happens if the same key as the current
		// direction is pressed, otherwise the direction is changed
		// with the motor being left on.
		
		if (onoff) then
		
			if (motor:GetTable().direction == dir or !motor:GetTable().is_on) then
			
				// Direction is the same, Activate if the motor is off
				// Deactivate if the motor is on.

				motor:GetTable().is_on = !motor:GetTable().is_on

				activate = motor:GetTable().is_on

			else
			
				// Change of direction, make sure it's activated
			
				activate = true
				
			end
			
		else
		
			return
			
		end
		
	else
	
		// normal mode: activate is based on the key status
		// (down = on, up = off)
		
		activate = onoff
		
	end
	
	if (activate) then
	
		motor:Fire( "Activate", "", 0 )		// Turn on the motor
		motor:Fire( "Scale", dir, 0)		// This makes the direction change
		
	else
		motor:Fire( "Deactivate", "", 0 )	// Turn off the motor
	end
	
	motor:GetTable().direction = dir
	
	return true
	
end

numpad.Register( "MotorControl", MotorControl )

/*----------------------------------------------------------------------
	Motor( ... )
	Creates a motor constraint
----------------------------------------------------------------------*/
function Motor( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, friction, torque, forcetime, nocollide, toggle, pl, forcelimit, numpadkey_fwd, numpadkey_bwd)

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	// Get information we're about to use
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2)
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos2 = Phys2:LocalToWorld( LPos2 )
	
	if ( Phys1 == Phys2 ) then return false end

	local axis = Axis( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, 0, 0, friction, nocollide )

	// Why do we do this? - Garry 15:59 27/07/2006
	axis:SetTable( {} )

	// Create the constraint 
	onStartConstraint( Ent1, Ent2 )
	
		local constraint = ents.Create("phys_torque")
			constraint:SetPos( WPos1 )
			constraint:SetKeyValue( "axis", tostring( WPos2 ) ) 
			constraint:SetKeyValue( "force", torque )
			constraint:SetKeyValue( "forcetime", forcetime )
			constraint:SetKeyValue( "spawnflags", 4 )
			constraint:SetPhysConstraintObjects( Phys1, Phys1 )
		constraint:Spawn()
		constraint:Activate()

	onFinishConstraint( Ent1, Ent2 )
	AddConstraintTable( Ent1, constraint, Ent2 )
	
	local ctable = 
	{
		Type 		= "Motor",
		Ent1  		= Ent1,
		Ent2  		= Ent2,
		Bone1 		= Bone1,
		Bone2 		= Bone2,
		LPos1 		= LPos1,
		LPos2 		= LPos2,
		friction 	= friction,
		torque 		= torque,
		forcetime 	= forcetime,
		nocollide 	= nocollide,
		toggle 		= toggle,
		pl 		= pl,
		forcelimit 	= forcelimit,
		forcescale 	= 0,
		direction 	= 1,
		is_on		= false,
		numpadkey_fwd 	= numpadkey_fwd,
		numpadkey_bwd 	= numpadkey_bwd
	}

	constraint:SetTable( ctable )

	if (numpadkey_fwd) then
	
		numpad.OnDown( 	 pl, 	numpadkey_fwd, 	"MotorControl", 	constraint,		true,	1 )
		numpad.OnUp( 	 pl, 	numpadkey_fwd, 	"MotorControl", 	constraint,		false,	1 )
	end
		
	if (numpadkey_bwd) then

		numpad.OnDown( 	 pl, 	numpadkey_bwd, 	"MotorControl", 	constraint,		true,	-1 )
		numpad.OnUp( 	 pl, 	numpadkey_bwd, 	"MotorControl", 	constraint,		false,	-1 )

	end
	
	return constraint, axis
	
end
duplicator.RegisterConstraint( "Motor", Motor, "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "friction", "torque", "forcetime", "nocollide", "toggle", "pl", "forcelimit", "numpadkey_fwd", "numpadkey_bwd" )


/*----------------------------------------------------------------------
	Pulley( ... )
	Creates a pulley constraint
----------------------------------------------------------------------*/
function Pulley( Ent1, Ent4, Bone1, Bone4, LPos1, LPos4, WPos2, WPos3, forcelimit, rigid, width, material )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent4, Bone4 ) ) then return false end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys4 = Ent4:GetPhysicsObjectNum( Bone4 )
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos4 = Phys4:LocalToWorld( LPos4 )
	
	if ( Phys1 == Phys2 ) then return false end

	// Make Constraint
	onStartConstraint( Ent1, Ent4 )

		local constraint = ents.Create( "phys_pulleyconstraint" )
			constraint:SetPos( WPos2 )
			constraint:SetKeyValue( "position2", tostring( WPos3 ) )
			constraint:SetKeyValue( "ObjOffset1", tostring( LPos1 ) )
			constraint:SetKeyValue( "ObjOffset2", tostring( LPos4 ) )
			constraint:SetKeyValue( "forcelimit", forcelimit )
			constraint:SetKeyValue( "addlength", (WPos3-WPos4):Length() )
			if ( rigid ) then constraint:SetKeyValue( "spawnflags", 2 ) end
			constraint:SetPhysConstraintObjects( Phys1, Phys4 )
		constraint:Spawn()
		constraint:Activate()
		
	onFinishConstraint( Ent1, Ent4 )
	AddConstraintTable( Ent1, constraint, Ent4 )

	local ctable = 
	{
		Type 		= "Pulley",
		Ent1  		= Ent1,
		Ent4  		= Ent4,
		Bone1 		= Bone1,
		Bone4 		= Bone4,
		LPos1 		= LPos1,
		LPos4 		= LPos4,
		WPos2 		= WPos2,
		WPos3 		= WPos3,
		forcelimit 	= forcelimit,
		rigid 		= rigid,
		width 		= width,
		material 	= material
	}
	constraint:SetTable( ctable )
	
	// make Rope
	local World = GetWorldEntity()
	
	local kv = 
	{ 
		Collide 	= 1,
		Type 		= 2,
		Subdiv		= 1,
	}
	
	CreateKeyframeRope( WPos1, width, material, constraint, 	Ent1, LPos1, Bone1, 	World, WPos2, 0, 		kv )
	CreateKeyframeRope( WPos1, width, material, constraint, 	World, WPos3, 0, 		World, WPos2, 0, 		kv )
	CreateKeyframeRope( WPos1, width, material, constraint, 	World, WPos3, 0, 		Ent4, LPos4, Bone4, 	kv )

	return constraint
	
end

duplicator.RegisterConstraint( "Pulley", Pulley, "Ent1", "Ent4", "Bone1", "Bone4", "LPos1", "LPos4", "WPos2", "WPos3", "forcelimit", "rigid", "width", "material" )


/*----------------------------------------------------------------------
	Ballsocket( ... )
	Creates a Ballsocket constraint
----------------------------------------------------------------------*/
function Ballsocket( Ent1, Ent2, Bone1, Bone2, LPos, forcelimit, torquelimit, nocollide )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	// Get information we're about to use
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2 )
	local WPos = Phys2:LocalToWorld( LPos )
	
	if ( Phys1 == Phys2 ) then return false end

	onStartConstraint( Ent1, Ent2 )
	
		local constraint = ents.Create("phys_ballsocket")
			constraint:SetPos( WPos )
			if (forcelimit && forcelimit > 0)	then constraint:SetKeyValue( "forcelimit", forcelimit )				end
			if (torquelimit && torquelimit > 0)	then constraint:SetKeyValue( "torquelimit", torquelimit )			end
			if (nocollide && nocollide > 0)		then constraint:SetKeyValue( "spawnflags", 1 )						end
			constraint:SetPhysConstraintObjects( Phys1, Phys2 )
		constraint:Spawn()
		constraint:Activate()
	
	onFinishConstraint( Ent1, Ent2 )
	AddConstraintTable( Ent1, constraint, Ent2 )
	
	local ctable = 
	{
		Type = "Ballsocket",
		Ent1  = Ent1,
		Ent2  = Ent2,
		Bone1 = Bone1,
		Bone2 = Bone2,
		LPos = LPos,
		forcelimit  = forcelimit, 
		torquelimit = torquelimit,
		nocollide   = nocollide
	}

	constraint:SetTable( ctable )

	return constraint
	
end

duplicator.RegisterConstraint( "Ballsocket", Ballsocket, "Ent1", "Ent2", "Bone1", "Bone2", "LPos", "forcelimit", "torquelimit", "nocollide" )


/*----------------------------------------------------------------------
	Winch( ... )
	Creates a Winch constraint
----------------------------------------------------------------------*/
function Winch( pl, Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, width, fwd_bind, bwd_bind, fwd_speed, bwd_speed, material, toggle )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2)
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos2 = Phys2:LocalToWorld( LPos2 )
	
	if ( Phys1 == Phys2 ) then return false end
				
	local const, dampen = CalcElasticConsts( Phys1, Phys2, Ent1, Ent2 )

	local constraint, rope = Elastic( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, const, dampen, 0, material, width, true )
	
	if ( !constraint ) then return nil, rope end
	
	local ctable = 
	{
			Type 		= "Winch",
			pl			= pl,
			Ent1		= Ent1,
			Ent2		= Ent2,
			Bone1		= Bone1,
			Bone2		= Bone2,
			LPos1		= LPos1,
			LPos2		= LPos2,
			width		= width,
			fwd_bind	= fwd_bind,
			bwd_bind	= bwd_bind,
			fwd_speed	= fwd_speed,
			bwd_speed	= bwd_speed,
			material	= material,
			toggle 		= toggle,
	}
	constraint:SetTable( ctable )
	

	// Attach our Controller to the Elastic constraint
	local controller = ents.Create("gmod_winch_controller")
		controller:GetTable():SetConstraint( constraint )
		controller:GetTable():SetRope( rope )
	controller:Spawn()
	
	constraint:DeleteOnRemove( controller )
	Ent1:DeleteOnRemove( controller )
	Ent2:DeleteOnRemove( controller )

	if (toggle) then
	
		numpad.OnDown( 	 pl, 	fwd_bind, 	"WinchToggle", 	controller, 1 )
		numpad.OnDown( 	 pl, 	bwd_bind, 	"WinchToggle", 	controller, -1 )
		
	else
	
		numpad.OnDown( 	 pl, 	fwd_bind, 	"WinchOn", 		controller, 1 )
		numpad.OnUp( 	 pl, 	fwd_bind, 	"WinchOff", 	controller )
		numpad.OnDown( 	 pl, 	bwd_bind, 	"WinchOn", 		controller, -1 )
		numpad.OnUp( 	 pl, 	bwd_bind, 	"WinchOff", 	controller )
		
	end
				
	return constraint, rope, controller

end

duplicator.RegisterConstraint( "Winch", Winch, "pl", "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "width", "fwd_bind", "bwd_bind", "fwd_speed", "bwd_speed", "material", "toggle" )


/*----------------------------------------------------------------------
	Hydraulic( ... )
	Creates a Hydraulic constraint
----------------------------------------------------------------------*/
function Hydraulic( pl, Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, Length1, Length2, width, key, fixed, speed )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2 )
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos2 = Phys2:LocalToWorld( LPos2 )
	
	if ( Phys1 == Phys2 ) then return false end
	
	local const,dampn = CalcElasticConsts(Phys1, Phys2, Ent1, Ent2)

	local constraint,rope = Elastic( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, const, dampn, 0, material, width, false )
	local ctable = {
			Type = "Hydraulic",
			pl	= pl,
			Ent1	= Ent1,
			Ent2	= Ent2,
			Bone1	= Bone1,
			Bone2	= Bone2,
			LPos1	= LPos1,
			LPos2	= LPos2,
			Length1	= Length1,
			Length2	= Length2,
			width	= width,
			key		= key,
			fixed	= fixed,
			fwd_speed	= speed,
			bwd_speed	= speed,
			toggle = true,
	}
	constraint:SetTable( ctable )
	
	if (constraint && constraint != rope) then
	
		if fixed == 1 then 
			local slider = Slider( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, 0 )
			slider:SetTable( {} )
			constraint:DeleteOnRemove( slider )
		end
		
		local controller = ents.Create("gmod_winch_controller")
		if (Length2 > Length1) then
			controller:SetKeyValue("minlength", Length1)
			controller:SetKeyValue("maxlength", Length2)
		else
			controller:SetKeyValue("minlength", Length2)
			controller:SetKeyValue("maxlength", Length1)
		end
		controller:GetTable():SetConstraint( constraint )
		controller:Spawn()
			
		Ent1:DeleteOnRemove( controller )
		Ent2:DeleteOnRemove( controller )
		
		constraint:DeleteOnRemove( controller )

		numpad.OnDown( 	 pl, 	key, 	"HydraulicToggle", 	controller )

		return constraint,rope,controller,slider
	else
		return constraint,rope
	end

end

duplicator.RegisterConstraint( "Hydraulic", Hydraulic, "pl", "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "Length1", "Length2", "width", "key", "fixed", "fwd_speed" )


/*----------------------------------------------------------------------
	Hydraulic( ... )
	Creates a Hydraulic constraint
----------------------------------------------------------------------*/
function Muscle( pl, Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, Length1, Length2, width, key, fixed, period, amplitude )

	if ( !CanConstrain( Ent1, Bone1 ) ) then return false end
	if ( !CanConstrain( Ent2, Bone2 ) ) then return false end
	
	local Phys1 = Ent1:GetPhysicsObjectNum( Bone1 )
	local Phys2 = Ent2:GetPhysicsObjectNum( Bone2 )
	local WPos1 = Phys1:LocalToWorld( LPos1 )
	local WPos2 = Phys2:LocalToWorld( LPos2 )
	
	if ( Phys1 == Phys2 ) then return false end

	local const, dampn = CalcElasticConsts( Phys1, Phys2, Ent1, Ent2 )

	local constraint, rope = Elastic( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, const, dampn, 0, material, width, false )
	if (!constraint) then return false end
	
	local ctable = 
	{
			Type 		= "Muscle",
			pl		= pl,
			Ent1		= Ent1,
			Ent2		= Ent2,
			Bone1		= Bone1,
			Bone2		= Bone2,
			LPos1		= LPos1,
			LPos2		= LPos2,
			Length1		= Length1,
			Length2		= Length2,
			width		= width,
			key			= key,
			fixed		= fixed,
			period		= period,
			amplitude	= amplitude,
			toggle 		= true,
	}
	constraint:SetTable( ctable )
	
	if fixed == 1 then  
		slider = Slider( Ent1, Ent2, Bone1, Bone2, LPos1, LPos2, 0 )
		slider:SetTable( {} ) // ??
		constraint:DeleteOnRemove( slider )
	end
	
	local controller = ents.Create("gmod_winch_controller")
	if (Length2 > Length1) then
		controller:SetKeyValue("minlength", Length1)
		controller:SetKeyValue("maxlength", Length2)
	else
		controller:SetKeyValue("minlength", Length2)
		controller:SetKeyValue("maxlength", Length1)
	end
	controller:SetKeyValue("type", 1)
	controller:GetTable():SetConstraint( constraint )
	controller:Spawn()
		
	Ent1:DeleteOnRemove( controller )
	Ent2:DeleteOnRemove( controller )
	
	constraint:DeleteOnRemove( controller )

	numpad.OnDown( 	 pl, 	key, 	"MuscleToggle", 	controller )

	return constraint,rope,controller,slider

end
duplicator.RegisterConstraint( "Muscle", Muscle, "pl", "Ent1", "Ent2", "Bone1", "Bone2", "LPos1", "LPos2", "Length1", "Length2", "width", "key", "fixed", "period", "amplitude" )

