-- Pick Up module for GameGuru
-- Written by Chris Stapleton
-- Contains helper functiond for the 'pickuppable' script
local U = require "scriptbank\\utillib"
local Q = require "scriptbank\\quatlib"
local P = require "scriptbank\\physlib"

local PU = { _G = _G }

-- import section: modules
local rad   = math.rad
local deg   = math.deg
local cos	= math.cos
local sin	= math.sin
local asin  = math.asin 
local abs	= math.abs
local atan  = math.atan2
local sqrt  = math.sqrt
local pi	= math.pi
local pairs = pairs

-- GG functions/Tables
local collsOff     = CollisionOff
local collsOn      = CollisionOn
local posObject    = PositionObject
local rotObject    = RotateObject
local rayCast      = IntersectAll
local getObjPosAng = GetObjectPosAng
local getTerHeight = GetTerrainHeight

-- include these for debugging
local Prompt  = Prompt
local PromptE = PromptLocal
local PromptD = PromptDuration

local gEnt = g_Entity

_ENV = PU 

local function timeNow() return _G.g_Time end
local function getPos3() return _G.g_PlayerPosX, _G.g_PlayerPosY, _G.g_PlayerPosZ end
--local function getAng3() return _G.g_PlayerAngX, _G.g_PlayerAngY, _G.g_PlayerAngZ end

local levelAng  = 20    -- number of degrees off horizontal still considered 
                        -- as 'level' for stacking purposes
local maxSize   = 400   -- default maximum size of pickuppable 

local pEnt       = {}
local dimensions = {}
local names = {}

function PU.getpEnt( e )
	return pEnt[ e ]
end

function PU.setpEnt( e, p )
	pEnt[ e ] = p
end

function PU.setdimensions( obj, dims )
	dimensions[ obj ] = dims
end

function PU.getdimensions( obj )
	return dimensions[ obj ]
end

function PU.setName( e, name )
	names[ e ] = name
end

function PU.getName( e )
	return names[ e ]
end
	
function PU.ObjectPickuppable( obj )
	if obj == nil or obj == 0 then return false end 
	
	for _,v in pairs( pEnt ) do
		if v.obj == obj then return true end
	end
	return false
end

----------------------------------------------------------
-- Function to reposition an object - angles in radians --
----------------------------------------------------------
function PU.rePositionEnt( e, obj, x, y, z, ax, ay, az )		
	collsOff( e )
	posObject( obj , x, y, z )
	rotObject( obj, deg( ax ), deg( ay ), deg( az ) )
	collsOn( e )
end

----------------------------------------------------------
-- Function to detect if player is on a pickuppable     --
----------------------------------------------------------
function PU.playerIsOn( p, ppx, ppy, ppz )
	return p.obj == rayCast( ppx, ppy, ppz, ppx, ppy - 200, ppz, 0 )
end

----------------------------------------------------------
-- Function to reposition a stack of objects            --
----------------------------------------------------------
function PU.repositionStack( p, nquat, nx, ny, nz, ppx, ppy, ppz )
	if p.stackList ~= nil then
		
		if ppy > nx + 20 or playerIsOn( p, ppx, ppy, ppz ) then
			return true
		end
		-- quaternion of base item 
		local xa, ya, za = Q.ToEuler( nquat )

		
		for k, v in pairs( p.stackList ) do
			
			-- calculate relative rotation
			local pquat = Q.Mul( nquat, v.q )
						
			-- rotate centre offsets to match new angle
			local xo, yo, zo = U.Rotate3D( v.x, v.y, v.z, xa, ya, za )

			local px, py, pz = nx + xo, ny + yo, nz + zo 
			
			local nxa, nya, nza = Q.ToEuler( pquat )
			
			PU.rePositionEnt( k, v.obj, px, py, pz, nxa, nya, nza )

			repositionStack( pEnt[ k ], pquat, px, py, pz, ppx, ppy, ppz )
			
			-- if carryingEnt == nil then return end
		end
	end
end

local function isLevelIsh( xa, ya, za )  -- Euler in degrees, returns true/false
	local maxAng =  180 - levelAng
	local minAng = -180 + levelAng
	
	return ( xa < minAng or xa > maxAng or 
	         ( xa < levelAng and xa > -levelAng ) ) and
		   ( za < minAng or za > maxAng or 
	         ( za < levelAng and za > -levelAng ) ) 
end 

-- given angles and origin position return centre position
local function getCentre( p, x, y, z, xa, ya, za )
	-- first rotate offsets 
	local dims = dimensions[ p.obj ]
	local xo, yo, zo = U.Rotate3D( dims.cx, dims.cy, dims.cz, xa, ya, za )

	-- now calculate centre of object
	return x + xo, y + yo, z + zo
end

local function checkIfSatOn( pb, bpx, bpy, bpz, bax, bay, baz,
                             pt, tpx, tpy, tpz, tax, tay, taz )
		
	-- can't be stacked if its origin is lower than the base
	-- objects origin no matter what the relative orientations are
	if tpy < bpy then return false end
	
	-- send a ray trace straight down from the centre of the top object

	-- first calculate centre of object
	local tcx, tcy, tcz = getCentre( pt, tpx, tpy, tpz, tax, tay, taz )
	
	local lowestY = tcy - ( dimensions[ pt.obj ].h / 2 + 5 )
	
	-- now cast rays from around centre point to try and find the base object
	local bObj = rayCast( tcx, tcy, tcz, tcx, lowestY , tcz, pt.obj )
	
	if bObj ~= pb.obj then
		bObj = rayCast( tcx + 1, tcy, tcz + 1, tcx + 1, lowestY, tcz + 1, pt.obj )
	end
	if bObj ~= pb.obj then
		bObj = rayCast( tcx + 1, tcy, tcz - 1, tcx + 1, lowestY, tcz - 1, pt.obj )
	end
	if bObj ~= pb.obj then
		bObj = rayCast( tcx - 1, tcy, tcz + 1, tcx + 1, lowestY, tcz + 1, pt.obj )
	end
	if bObj ~= pb.obj then
		bObj = rayCast( tcx - 1, tcy, tcz - 1, tcx + 1, lowestY, tcz - 1, pt.obj )
	end
	
	if bObj ~= pb.obj then return false end
	
	local q1 = Q.Conjugate( Q.FromEuler( bax, bay, baz ) )

	local xo, yo, zo = U.Rotate3D( tpx - bpx, tpy - bpy, tpz - bpz, Q.ToEuler( q1 ) )
	
	return true, xo, yo, zo, Q.Mul( q1 , Q.FromEuler( tax, tay, taz ) )
end

function PU.buildStackList( p, base_obj )
	-- basically we have to find all pickuppable objects
	-- that are sitting on this one and bung them in a 
	-- separate list, storing their positions relative to
	-- the base entity and rotations relative to it
	-- we have to do this before we first move the object
	-- so we can also move all the others at the same time
	local stacked = "No"
	
	-- first check if object is roughly level
	local pbx, pby, pbz, abx, aby, abz = getObjPosAng( p.obj )

	-- for now only allow stacking on 'level(ish)' object
	if p.obj == base_obj and not isLevelIsh( abx, aby, abz ) then 
		p.stackList = nil
		return 
	end
	
	-- roughly level so lets find out who is sat on us; 
	-- start by creating an empty list, we can delete
	-- it if we don't find any 'stackable' items
	p.stackList = {}
	
	for k, v in pairs( pEnt ) do
		-- don't process ourselves!
		if v ~= p then
			-- only process entities within range
			local tpx, tpy, tpz, tax, tay, taz = getObjPosAng( v.obj )
			
			if U.CloserThan( tpx, tpy, tpz, pbx, pby, pbz, maxSize * 2 ) then
			
				local isOn, ox, oy, oz, q = checkIfSatOn( p, pbx, pby, pbz, rad(abx), rad(aby), rad(abz),
                                                          v, tpx, tpy, tpz, rad(tax), rad(tay), rad(taz), 
													      dimensions[ k ] );
				if isOn then 
					-- offset values are from the centres not origin!
					p.stackList[ k ] = { obj = v.obj, x = ox, y = oy, z = oz, q = q }
					stacked = "Yes"				
				end
			end
		end
	end	
	
	if stacked == "Yes" then
		for k, v in pairs( p.stackList ) do
			-- now recurse
			buildStackList( pEnt[ k ] )
		end
	else -- didn't find any so delete the stack 
		p.stackList = nil
	end
end

local function RotatePoint2D (x, y, Ang)  -- Ang in radians
	local Sa, Ca = sin( Ang ), cos( Ang )
	return x*Ca - y*Sa, x*Sa + y*Ca
end


-- this function checks if given x,z coordinates are overlapping a particular 
-- box taking into account rotation
local function isOverlapping( x, z, x2, z2, Ang, L, W )

	local HL, HW = L / 2, W / 2
	local DX, DZ = x - x2, z - z2

	local NDX, NDZ = RotatePoint2D (DX, DZ, rad( Ang ) )
		
	return ( NDZ > -HL and NDZ < HL and 
	         NDX > -HW and NDX < HW )
end

local function lowestSide( obj, x, y, z, xa, ya, za )
	local dims = getdimensions( obj )

	-- work out which way up the box is
	local nx, ny, nz = U.Rotate3D( 0, 1, 0, rad( xa ), rad( ya ), rad( za ) )
	if ny > 0.6 then
		-- box is upright so just return plain coords
		return x, y, z, dims.h
		
	elseif ny < - 0.6 then
		-- upside down so 'massage' y value
		return x, y - dims.h, z, dims.h
		
	else
		-- on one side
		return x + nx * dims.w / 2, y - dims.h / 2, z + nz * dims.l / 2, dims.h
	end
end

function PU.EntityOnPlatform( x, z, y, Ang, L, W, H ) 
	for k, v in pairs( pEnt ) do
		local nx, ny, nz, ax, ay, az = getObjPosAng( v.obj ) 
		local  px, py, pz, h = lowestSide( v.obj, nx, ny, nz, ax, ay, az )
		if py > y + H - 1 and py < y + H + 5 then
			if isOverlapping( px, pz, x, z, Ang, L, W ) then
				return k, names[ k ]
			end
		end
	end
end

function PU.MoveIfOn( x, z, y, Ang, L, W, H, xi, yi, zi, rQuat )
	for k, v in pairs( pEnt ) do
		local nx, ny, nz, ax, ay, az = getObjPosAng( v.obj )

		local  px, py, pz, h = lowestSide( v.obj, nx, ny, nz, ax, ay, az )
		if py > y + H - 1 and py < y + H + 15 then
			if isOverlapping( px, pz, x, z, Ang, L, W ) then
			
				if rQuat ~= nil then
					local quat = Q.FromEuler( rad( ax ), rad( ay ), rad( az ) )
					ax, ay, az = Q.ToEuler( Q.Mul( quat, rQuat ) )
				else
					ax, ay, az = rad( ax ), rad ( ay ), rad ( az )
				end	
				rePositionEnt( k, v.obj, nx + xi, ny + yi - 0.1, nz + zi, ax, ay, az )	
			end
		end
	end
end	

function PU.getRealWorldYangle( xa, ya, za )  -- Euler in degrees, returns radians 
	-- Tricky to explain but basically g_PlayerAngY is the real-world
	-- angle, i.e. 0-359 degrees, whereas the Euler angle for an entity
    -- returned by GetObjectPosAng is not.
	-- In order to get the 'real-world' equivalent we need to do some math.

	-- first rotate a unit 'facing forward' vector by the Euler angles
	local xv, _, zv = U.Rotate3D( 0, 0, 1,  rad( xa ), rad( ya ), rad ( za ) )
	
	-- now work out the angle from the x and z components of the result
	return atan( xv, zv )
end	

local SEPTT_timer = 0

function PU.stopEntsPunchingThroughTerrain( carryingEnt )
	-- or more accurately rescue them when they have been punched through
	local tn = timeNow()
	
	if tn > SEPTT_timer then
		-- check 5 times a second
		SEPTT_timer = tn + 200
		
		for k, v in pairs( pEnt ) do
			
			if k ~= carryingEnt then
				
				local  x, y, z, xa, ya, za = getObjPosAng( v.obj )
				
				-- check if the centre of the object is below terrain height
				local cx, cy, za = getCentre( v, x, y, z, xa, ya, za )
				
				local th = getTerHeight( x, z )
				
				if cy < th - 30 then
					ya = getRealWorldYangle( xa, ya, za )
					rePositionEnt( k, v.obj, x, th + 1, z, 0, ya, 0 )
				end
			end
		end
	end
end

function PU.clearStacks( p )
	if p.stackList ~= nil then
		for k, _ in pairs( p.stackList ) do
			clearStacks( pEnt[ k ] )
			pEnt[ k ].stackList = nil
		end
	end
end

function PU.stackWeight( p )
	local totWeight = 0
	
	if p.stackList ~= nil then
		for k, p in pairs( p.stackList ) do
			totWeight = totWeight + ( PU.getdimensions( p.obj ).m or 0 ) + stackWeight( pEnt[ k ] )
		end
	end
	return totWeight
end

function positionHandler( e, p, rq, numFrames )
	local ppx, ppy, ppz = getPos3()
	p.dropObject = false
	-- So we need this to run every frame and do the following:
	if PU.playerIsOn( p, ppx, ppy, ppz ) then
		p.dropObject = true
		return
	end
	
	-- calculate current object position and rotation : cp, cq
	local cpx, cpy, cpz, cax, cay, caz = getObjPosAng( p.obj )
	
	local cq = Q.FromEuler( rad( cax ), rad( cay ), rad( caz ) )
	
	-- calculate difference between cp and required position : rp
	local xtg, ytg, ztg = p.rx - cpx, p.ry - cpy, p.rz - cpz  
	
	-- if difference greater that some amount work out how far we need 
	-- to move and calculate a per frame vector to apply which will depend
	-- on maximum movement allowable
	
	local dtg = xtg*xtg + ytg*ytg + ztg*ztg 

	if dtg < 1 then 
		rePositionEnt( e, p.obj, p.rx, p.ry, p.rz, Q.ToEuler( rq ) )

		if repositionStack( p, rq, p.rx, p.ry, p.rz, ppx, ppy, ppz ) then
			pEnt[ carryingEnt ].dropObject = true
		end
	else
		
		xtg, ytg, ztg = xtg / numFrames, ytg / numFrames, ztg / numFrames
	
		-- calculate per frame rotation (SLERP) : sq (same rules as for position) 
		local sq = Q.NLerp( cq, rq, 1 / numFrames ) 
		
		local nx, ny, nz = cpx + xtg, cpy + ytg, cpz + ztg
		
		rePositionEnt( e, p.obj, nx, ny, nz, Q.ToEuler( sq ) )

		-- now position stacked ents
		if repositionStack( p, sq, nx, ny, nz, ppx, ppy, ppz ) then
			pEnt[ carryingEnt ].dropObject = true
		end
	end
end
-- user should call this on a regular basis to update pickuppables
function PU.periodicProcessing()
	-- First up check what pickuppables are sat on (or not) as the case may be
	
	
	-- next handle collisions

end

function PU.everyFrameProcessing()
	-- handle movement updates of carried item or player on item
	
end	


return PU