
.namespace RagDoll

g_Bodies = {} --chaque element de bodies est un tableau avec un .edges et un .vertices contenant les index

g_Vertices = {}
g_Edges = {}
g_BodyCount = 1
g_VertexCount = 1
g_EdgeCount = 1

g_CollisionInfo = {Depth = 0 , Normal = {},E = 0 , V = 0} --info sur les collision en cours, pareil ce sont les INDEX que l'on stocke

g_dragging = false
g_curdragvertex = 0

g_mousepos = {}

GRAVITY = nil
TIMESTEP = 0.3
NBITERATIONS = 10

COEFFROTTEMENT = 0.995 
INVERTCOEFFROTTEMENT = 1 - COEFFROTTEMENT

function OnEnable() --initiliasiation des constantes
	GRAVITY = Vector:new(0,0.5)
	ConsoleText0.LineSpacing = - 20
	
	self.Flags_AcceptAllInput = true
	OPInput.RegisterListener(self , EVENT_MOUSE_MOVE+EVENT_MOUSE_LUP+EVENT_MOUSE_LDOWN + EVENT_MOUSE_RDOWN + EVENT_MOUSE_MDOWN)
	
	g_mousepos = Vector:new(0,0)
end

function  OnMouseMDown(x,y)
	CreateHumanBody(x,y,200)
end

function OnMouseRDown(x,y)
	--on ajoute un bloc
	CreateBox(x,y,200,200)
end

function OnMouseLDown(x,y)
	--on déplace un bloc
	g_curdragvertex = FindNearestVertex(x,y)
		
	if g_curdragvertex > 0 then
		g_dragging = true
	end

end

function OnMouseLUp(x,y)
	g_dragging = false
end

function OnMouseMove(x,y)
	if g_dragging then
		g_mousepos.x = x
		g_mousepos.y = y
	end
end

function UpdateDragMouse()
	--OPLog.Log(tostring(g_curdragvertex).." "..tostring(g_mousepos))
	g_Vertices[g_curdragvertex].Position.x = g_mousepos.x
	g_Vertices[g_curdragvertex].Position.y = g_mousepos.y
end

function AddBody( Body ) 
	AssertPopup(type(Body) == "table" ,"prob param")
	g_Bodies[ g_BodyCount ] = Body
	g_BodyCount = g_BodyCount + 1
	return g_BodyCount -1
end

function AddVertex(  V ) 
	AssertPopup(type(V) == "table" ,"prob param")
	g_Vertices[ g_VertexCount ] = V
	g_VertexCount = g_VertexCount +1
	return g_VertexCount - 1
end

function AddEdge( E ) 
	AssertPopup(type(E) == "table" ,"prob param")
	g_Edges[ g_EdgeCount ] = E
	g_EdgeCount = g_EdgeCount +1
	return g_EdgeCount - 1
end

--Sets the force on each vertex to the gravity force. You could of course apply other forces like magnetism etc.
--inutilie de le faire à chaque update
function UpdateForces() 
	for I,vertex in pairs(g_Vertices) do
		vertex.Acceleration = GRAVITY --vecteur 2d
	end
end

function UpdateVerlet() --Updates the vertex position as explained in the article
	for I,V in pairs(g_Vertices) do
		local Temp = V.Position
		V.Position = V.Position + V.Position - V.OldPosition + V.Acceleration:scale(TIMESTEP*TIMESTEP)
		--on rajoute les frottement ici
		--V.OldPosition = Temp
		--TODO, détecter quand il y a vraiment contact plutôt
		V.OldPosition.x = Temp.x*COEFFROTTEMENT + V.Position.x*INVERTCOEFFROTTEMENT
		V.OldPosition.y = Temp.y
	end
end

function UpdateEdges() 
	--ne pas oublier que les edge ne contiennent que les indices des vertex
	for indedge,edge in pairs (g_Edges) do
		local V1V2 = g_Vertices[edge.V2].Position - g_Vertices[edge.V1].Position --Calculate the vector between the two vertices

		local V1V2Length = V1V2:norm() --Calculate the current distance
		local Diff       = V1V2Length - edge.Length --Calculate the difference from the original length
		
		V1V2 = V1V2:normalize()

		g_Vertices[edge.V1].Position = g_Vertices[edge.V1].Position + V1V2:scale(Diff*0.5) --Push both vertices apart by half of the difference respectively so the distance between them equals the original length
		g_Vertices[edge.V2].Position = g_Vertices[edge.V2].Position - V1V2:scale(Diff*0.5)
	end
end


function IterateCollisions() 
	--pas de assertpopup dans les fonctions critiques en update
	for I = 1,NBITERATIONS do --Repeat this a few times to give more exact results
		--A small 'hack' that keeps the vertices inside the screen. You could of course implement static objects and create
		--four to serve as screen boundaries, but the max/min method is faster
		for T, vertex in pairs(g_Vertices) do
			local Pos = vertex.Position
			Pos.x = math.max( math.min( Pos.x, WIDTH_SCREEN  ), 0.0 )
			Pos.y = math.max( math.min( Pos.y, HEIGHT_SCREEN ), 0.0 )
		end

		UpdateEdges() --Edge correction step

		for j,body in pairs(g_Bodies) do
			CalculateBodyCenter(j) --Recalculate the center
		end

		for indbody1, B1 in pairs(g_Bodies) do --Iterate trough all bodies, c'est du n² y a sans doute moyen d'optimiser
			for indbody2, B2 in pairs(g_Bodies) do
				if(indbody1 ~= indbody2 ) then
					if( BodiesOverlap( indbody1 , indbody2) ) then--Test the bounding boxes
						--OPLog.Log("overlap !!")
						if( DetectCollision( indbody1, indbody2) ) then --If there is a collision, respond to it
							ProcessCollision()
						end
					end
				end
			end
		end
		
	end
end


function DetectCollision( indbody1, indbody2 ) 
	local MinDistance = 10000.0 --Initialize the length of the collision vector to a relatively large value
	local curindedge
	for I = 1, table.getn(g_Bodies[indbody1].edges) + table.getn(g_Bodies[indbody2].edges) do --Just a fancy way of iterating through all of the edges of both bodies at once
		if( I <= table.getn(g_Bodies[indbody1].edges)  ) then
			curindedge = g_Bodies[indbody1].edges[I]
		else
			curindedge = g_Bodies[indbody2].edges[I - table.getn(g_Bodies[indbody1].edges)]
		end
			--E = B2->g_Edges[ I - B1->g_EdgeCount ]

		--This will skip edges that lie totally inside the bodies, as they don't matter.
		--The boundary flag has to be set manually and defaults to true
		if(g_Edges[curindedge].Boundary) then			
			local Axis = Vector:new( g_Vertices[g_Edges[curindedge].V1].Position.y - g_Vertices[g_Edges[curindedge].V2].Position.y, 
									g_Vertices[g_Edges[curindedge].V2].Position.x - g_Vertices[g_Edges[curindedge].V1].Position.x ) --Calculate the perpendicular to this edge and normalize it
			Axis = Axis:normalize()

			local MinA, MinB, MaxA, MaxB --Project both bodies onto the perpendicular
			MinA,MaxA = ProjectBodyToAxis(indbody1,Axis)
			MinB,MaxB = ProjectBodyToAxis(indbody2, Axis)

			local Distance = IntervalDistance( MinA, MaxA, MinB, MaxB ) --Calculate the distance between the two intervals

			if( Distance > 0.0 ) then --If the intervals don't overlap, return, since there is no collision
				return false
			elseif( math.abs( Distance ) < MinDistance ) then
				MinDistance = math.abs( Distance )

				g_CollisionInfo.Normal = Axis --Save collision information for later
				g_CollisionInfo.E      = curindedge   --Store the edge index, as it is the collision edge
			end		
		end
	end

	g_CollisionInfo.Depth = MinDistance

	if( g_Edges[g_CollisionInfo.E].Parent ~= indbody2 ) then  --Ensure that the body containing the collision edge lies in B2 and the one conatining the collision vertex in B1
		local temp = indbody2
		indbody2 = indbody1
		indbody1 = temp
	end

	local Sign 
	if (( g_CollisionInfo.Normal*( g_Bodies[indbody1].Center - g_Bodies[indbody2].Center ) )  < 0) then --This is needed to make sure that the collision normal is pointing at B1
		Sign = -1
	else
		Sign = 1
	end	
	--Remember that the line equation is N*( R - R0 ). We choose B2->Center as R0 the normal N is given by the collision normal

	if( Sign ~= 1 ) then
		g_CollisionInfo.Normal = -g_CollisionInfo.Normal --Revert the collision normal if it points away from B1
	end
	
	local CollisionVector = g_CollisionInfo.Normal:scale(g_CollisionInfo.Depth)

	local SmallestD = 10000.0 --Initialize the smallest distance to a large value
	for I,indvertex in pairs(g_Bodies[indbody1].vertices) do 
		local Distance = g_CollisionInfo.Normal*( g_Vertices[indvertex].Position - g_Bodies[indbody2].Center ) --Measure the distance of the vertex from the line using the line equation

		if( Distance < SmallestD ) then --If the measured distance is smaller than the smallest distance reported so far, set the smallest distance and the collision vertex
			SmallestD = Distance
			g_CollisionInfo.V = indvertex --indice du vertex
		end
	end

	--OPLog.Log("collision ! "..GlobalFunctions.Display(g_CollisionInfo))
	return true --There is no separating axis. Report a collision!
end



function ProcessCollision() 
	indvertex1 = g_Edges[g_CollisionInfo.E].V1
	indvertex2 = g_Edges[g_CollisionInfo.E].V2

	local CollisionVector = g_CollisionInfo.Normal:scale(g_CollisionInfo.Depth)

	local T
	if( math.abs( g_Vertices[indvertex1].Position.x - g_Vertices[indvertex2].Position.x ) > math.abs( g_Vertices[indvertex1].Position.y - g_Vertices[indvertex2].Position.y ) ) then
		T = ( g_Vertices[g_CollisionInfo.V].Position.x - CollisionVector.x - g_Vertices[indvertex1].Position.x )/(  g_Vertices[indvertex2].Position.x - g_Vertices[indvertex1].Position.x )
	else
		T = ( g_Vertices[g_CollisionInfo.V].Position.y - CollisionVector.y - g_Vertices[indvertex1].Position.y )/(  g_Vertices[indvertex2].Position.y - g_Vertices[indvertex1].Position.y )
	end

	local Lambda = 1.0/( T*T + ( 1 - T )*( 1 - T ) )

	g_Vertices[indvertex1].Position = g_Vertices[indvertex1].Position - CollisionVector:scale(( 1 - T )*0.5*Lambda)
	g_Vertices[indvertex2].Position = g_Vertices[indvertex2].Position  -CollisionVector:scale( T  *0.5*Lambda)
	
	g_Vertices[g_CollisionInfo.V].Position = g_Vertices[g_CollisionInfo.V].Position + CollisionVector:scale(0.5)
end


function IntervalDistance(  MinA,  MaxA,  MinB,  MaxB ) 
	if( MinA < MinB ) then
		return MinB - MaxA
	else
		return MinA - MaxB
	end
end


function BodiesOverlap( indbody1,  indbody2 )  --Simple bounding box overlapping test
	return ( g_Bodies[indbody1].MinX <= g_Bodies[indbody2].MaxX ) and ( g_Bodies[indbody1].MinY <= g_Bodies[indbody2].MaxY ) and 
		   ( g_Bodies[indbody1].MaxX >= g_Bodies[indbody2].MinX ) and ( g_Bodies[indbody2].MaxY >= g_Bodies[indbody1].MinY )
end


function OnUpdate(dt) 
	if g_dragging then
		UpdateDragMouse()
	end

	UpdateForces()
	UpdateVerlet()
	

	UpdateSprites()
	
	if OPCore.EditorMode() then
		DisplayDebug()
	end
	--pour l'instant on va juste afficher sans collision
	IterateCollisions()
end

function DisplayDebug()
	--ConsoleText0.Text = GlobalFunctions.Display(g_Bodies).."\r"
	--ConsoleText0.Text = ConsoleText0.Text..GlobalFunctions.Display(g_Vertices).."\r"
	--ConsoleText0.Text = ConsoleText0.Text..GlobalFunctions.Display(g_Edges)
end

function UpdateSprites()
	--OPLog.Log("___________update sprite _________")
	for i,edge in pairs(g_Edges) do
		--à optimiser par la suite
		local length = (g_Vertices[edge.V1].Position - g_Vertices[edge.V2].Position):norm()
		local scale = length / 200
		--TODO le scale bug pour l'instant
		edge.spr.Scale_X = scale
		edge.spr.Scale_Y = scale
		local angle = (g_Vertices[edge.V1].Position - g_Vertices[edge.V2].Position):getangle() + math.pi/2
		--OPLog.Log("angle" ..(angle/math.pi)*180 .. " "..tostring(g_Vertices[edge.V1].Position) .." " .. tostring(g_Vertices[edge.V2].Position).. " name "..edge.spr.Name)

		--edge.spr.Position_X = g_Vertices[edge.V1].Position.x
		--edge.spr.Position_Y = g_Vertices[edge.V1].Position.y
		--edge.spr.Rotation = angle
		RotateSprite(edge.spr,angle,g_Vertices[edge.V1].Position,Vector:new(0,0))
		edge.spr.Rotation = - edge.spr.Rotation -- TODO trés moche à revoir
	end
	
	for i,vertex in pairs(g_Vertices) do
		vertex.Position:set(vertex.nodespr)
	end
	--OPLog.Log("___________fin update sprite _________")
end

--on renvoit l'indice du vertex
function FindNearestVertex( X, Y )  --Helper function that finds the vertex laying closest to a given point
	local NearestVertex = 0
	local MinDistance = 10000.0

	local Coords = Vector:new(X,Y)

	for I, vertex in pairs(g_Vertices) do 
		local Distance = ( vertex.Position - Coords ):normsquare()

		if( Distance < MinDistance )  then
			NearestVertex = I
			MinDistance = Distance
		end
	end

	return NearestVertex
end



--IndBody est l'index dans g_Bodies
-- indV1 et indV2 sont les indices des verts dans g_Vertices
function AddEdgeToBodyByVertices(  IndBody,  indV1,  indV2, pBoundary )  --Constructor
	AssertPopup(type(indV1) == "number" and type(indV2) == "number" , "probparam")
	AssertPopup(type(IndBody) == "number" , "probparam")
	
	local edge = {}
	edge.V1 = indV1 --Set boundary vertices
	edge.V2 = indV2

	edge.Length   = ( g_Vertices[indV2].Position - g_Vertices[indV1].Position ):norm() -- Calculate the original length
	edge.Boundary = pBoundary

	edge.Parent = IndBody
	
	
	--création du sprite
	edge.spr = OPUnitFactory.CreateUnit("Sprite")
	edge.spr.Image = "media/common/graph/interface/InGame/redline.png"
	
	--on ajoute dans le monde "global"
	local ind = AddEdge(edge)
	
	--on ajoute au body précisé, l'index de l'edge
	table.insert(g_Bodies[IndBody].edges,ind)
	return ind
end

function AddVertexToBodyByPosition( indbody ,  PosX, PosY ) 
	AssertPopup( type(indbody) == "number" and type(PosX) == "number" and type(PosY) == "number" ,"prob params")
	
	local vertex = {}
	
	vertex.Position    = Vector:new( PosX, PosY )
	vertex.OldPosition = Vector:new( PosX, PosY )

	vertex.Parent = indbody
	vertex.nodespr = OPUnitFactory.CreateUnit("Sprite")
	vertex.nodespr.Image = "media/common/graph/interface/InGame/bluespot.png"
	vertex.nodespr.Layer = LayerMenu
	
	
	local ind = AddVertex(vertex) --rajout au "world"
	AddVertexToBody(indbody,ind) --rajout au body
	
	return ind
end

function AddBodyToWorld()  --Constructor
	local body = {edges = {} , vertices = {}, Center = Vector:new(0,0), MinX = 0,MinY = 0,MaxX = 0, MaxY = 0 }
	return AddBody(body) --Add body to the physics simulator
end

function AddEdgeToBody(indbody,  indedge ) 
	AssertPopup(type(indbody) == "number" and type(indedge) == "number" ,"prob params")
	
	AssertPopup(type(g_Bodies[indbody]) == "table" and type(g_Edges[indedge]) == "table" , "prob indice non valides")
	table.insert(g_Bodies[IndBody].edges,indedge)
end

function AddVertexToBody(indbody , indvertex ) 

	AssertPopup(type(indbody) == "number" and type(indvertex) == "number" ,"prob params")
	AssertPopup(type(g_Bodies[indbody]) == "table" and type(g_Vertices[indvertex]) == "table" , "prob indice non valides")
	table.insert(g_Bodies[indbody].vertices,indvertex)
end


function ProjectBodyToAxis(indbody, Axis) 
	local DotP = Axis*g_Vertices[g_Bodies[indbody].vertices[1]].Position --produit scalaire
	local Min = DotP	--Set the minimum and maximum values to the projection of the first vertex
	local Max = DotP

	for I, indvertex in pairs(g_Bodies[indbody].vertices) do 
		DotP = Axis*g_Vertices[indvertex].Position --Project the rest of the vertices onto the axis and extend the interval to the left/right if necessary

		Min = math.min( DotP, Min )
		Max = math.max( DotP, Max )
	end
	
	return Min,Max
end



function CalculateBodyCenter(indbody)  --Recalculates the center of mass and the bounding box
	g_Bodies[indbody].Center = Vector:new( 0.0, 0.0 )
	g_Bodies[indbody].MinX =  10000.0
	g_Bodies[indbody].MinY =  10000.0
	g_Bodies[indbody].MaxX = -10000.0
	g_Bodies[indbody].MaxY = -10000.0

	for i, indvertex in pairs(g_Bodies[indbody].vertices ) do
		g_Bodies[indbody].Center = g_Bodies[indbody].Center + g_Vertices[ indvertex].Position

		g_Bodies[indbody].MinX = math.min( g_Bodies[indbody].MinX, g_Vertices[ indvertex ].Position.x )
		g_Bodies[indbody].MinY = math.min( g_Bodies[indbody].MinY, g_Vertices[ indvertex ].Position.y )
		g_Bodies[indbody].MaxX = math.max( g_Bodies[indbody].MaxX, g_Vertices[ indvertex ].Position.x )
		g_Bodies[indbody].MaxY = math.max( g_Bodies[indbody].MaxX, g_Vertices[ indvertex ].Position.y )
	end

	g_Bodies[indbody].Center = g_Bodies[indbody].Center:scale(1/ table.getn(g_Bodies[indbody].vertices ))
end




function CreateBox(  X, Y, Width,  Height )  --Creates a box primitive
	AssertPopup( type(X) == "number" and type(Y) == "number" and type(Width) == "number" and type(Height) == "number" ,"prob params")
	
	local indbody = AddBodyToWorld()
	--on fonctionne avec les indices
	local ind1 = AddVertexToBodyByPosition(indbody,X,Y)
	local ind2 = AddVertexToBodyByPosition(indbody,X + Width,Y)
	local ind3 = AddVertexToBodyByPosition(indbody,X + Width,Y + Height)
	local ind4 = AddVertexToBodyByPosition(indbody,X,Y + Height)
	

	AddEdgeToBodyByVertices(indbody , ind1,ind2,true)
	AddEdgeToBodyByVertices(indbody , ind2,ind3,true)
	AddEdgeToBodyByVertices(indbody , ind3,ind4,true)
	AddEdgeToBodyByVertices(indbody , ind4,ind1,true)
	
	AddEdgeToBodyByVertices(indbody , ind1,ind3,false)
	AddEdgeToBodyByVertices(indbody , ind2,ind4,false)
end

function CreateHumanBody()
	local indbody = AddBodyToWorld()
	
	--tête
	local ind1 = AddVertexToBodyByPosition(indbody,120,0)
	local ind2 = AddVertexToBodyByPosition(indbody,160,0)
	local ind3 = AddVertexToBodyByPosition(indbody,120,80)
	local ind4 = AddVertexToBodyByPosition(indbody,160,80)
		
	--corps	
	local ind5 = AddVertexToBodyByPosition(indbody,140,80)
	local ind6 = AddVertexToBodyByPosition(indbody,80,120)
	local ind7 = AddVertexToBodyByPosition(indbody,200,120)
	local ind8 = AddVertexToBodyByPosition(indbody,140,120)
	local ind11 = AddVertexToBodyByPosition(indbody,80,240)
	local ind12 = AddVertexToBodyByPosition(indbody,200,240)
	
	--jambes,bras
	local ind9 = AddVertexToBodyByPosition(indbody,0,120)
	local ind10 = AddVertexToBodyByPosition(indbody,280,120)
	local ind13 = AddVertexToBodyByPosition(indbody,80,320)
	local ind14 = AddVertexToBodyByPosition(indbody,200,320)
	
	AddEdgeToBodyByVertices(indbody , ind1,ind2,true)
	AddEdgeToBodyByVertices(indbody , ind2,ind4,true)
	AddEdgeToBodyByVertices(indbody , ind4,ind5,true)
	AddEdgeToBodyByVertices(indbody , ind5,ind3,true)
	AddEdgeToBodyByVertices(indbody , ind3,ind1,true)
	AddEdgeToBodyByVertices(indbody , ind1,ind4,false)
	AddEdgeToBodyByVertices(indbody , ind3,ind2,false)
	
	AddEdgeToBodyByVertices(indbody , ind5,ind8,false)
	AddEdgeToBodyByVertices(indbody , ind6,ind8,true)
	AddEdgeToBodyByVertices(indbody , ind8,ind7,true)
	AddEdgeToBodyByVertices(indbody , ind7,ind12,true)
	AddEdgeToBodyByVertices(indbody , ind12,ind11,true)
	AddEdgeToBodyByVertices(indbody , ind11,ind6,true)
	AddEdgeToBodyByVertices(indbody , ind6,ind12,false)
	AddEdgeToBodyByVertices(indbody , ind11,ind7,false)
	
	AddEdgeToBodyByVertices(indbody , ind9,ind6,true)
	AddEdgeToBodyByVertices(indbody , ind7,ind10,true)
	AddEdgeToBodyByVertices(indbody , ind11,ind13,true)
	AddEdgeToBodyByVertices(indbody , ind12,ind14,true)
	
	
	
	
end

function CreateSimpleArticulation(X,Y,Width)
	local indbody = AddBodyToWorld()
	--on fonctionne avec les indices
	local ind1 = AddVertexToBodyByPosition(indbody,X,Y)
	local ind2 = AddVertexToBodyByPosition(indbody,X + Width,Y)
	local ind3 = AddVertexToBodyByPosition(indbody,X + 2*Width,Y)
	
	AddEdgeToBodyByVertices(indbody , ind1,ind2,true)
	AddEdgeToBodyByVertices(indbody , ind2,ind3,true)
end

