#include "script/common.lua"
--handler for flight systems

--flight settings
function initFlightVar()
	speed = 0 --current speed 
	maxSpeed = tonumber(GetTagValue(flightSettings,"vne")) or 84 --vNE NEVER EXCEED
	minSpeed = tonumber(GetTagValue(flightSettings,"minspeed")) or 8 --slowest allowed speed
	fastSpeedThreshold = tonumber(GetTagValue(flightSettings,"fastthreshold")) or 33

	patrolheight = tonumber(GetTagValue(flightSettings,"patrolheight")) or 32 --patrol altitude, recommend altering in editor if you have multiple
	--entities in the map
	hoverheight = tonumber(GetTagValue(flightSettings,"hoverheight")) or 42 --hover altitude for vertical posture
	height = 30 --desired altitude
	toHeight = 0 --height to go to after accounting for ground

	turnRate = 0.03--for use in different modes
	patrolTurnRate = tonumber(GetTagValue(flightSettings,"patrolturnrate")) or 0.042 --target turnRate for forward flight
	hoverTurnRate = tonumber(GetTagValue(flightSettings,"hoverturnrate")) or 0.06 --target turnrate for hover
	hoverMinDist = tonumber(GetTagValue(flightSettings,"hovermin")) or 37
	hoverMaxDist = tonumber(GetTagValue(flightSettings,"hovermax")) or 59
end

function heightControl(controlTargetPos)
	--height control
	--computeSurroundingHeight()
	toHeight = height
	--rejectSelf()
	rejectAll()
	rejectChoppers()
	QueryRequire("large")
	local probe = VecCopy(controlTargetPos)
	probe[2] = 100
	local hit, dist = QueryRaycast(probe, Vec(0,-1,0), 100, 0)
	if hit then
		toHeight = toHeight + 100 - dist
	end
end
heightRaycastStep = {0.016,0} --[1] for the step, [2] for the actual time

function forwardFlightTailControl(dt)
	for i=1,#tailTable do
	if i == 1 then --behind the head
		--local behind = Vec(0,0,distance-3.6-speed*0.25*dt)
		local behind = Vec(0,0,distance-1.2-clamp(speed*0.26*dt,0,1)) --speed*dt should scale with VecLerp in update
		behind = VecAdd(headTransform.pos,TransformToParentVec(headTransform,behind))
		--tailTable[i][2].pos = VecCopy(behind) --enable to directly move the transform
		tailTable[i].targetPos = VecCopy(behind)
	else
		--local behind = Vec(0,0,distance-speed*0.25*dt)
		local behind = Vec(0,0,distance-clamp(speed*0.26*dt,0,1)) --optimize VERY carefully to minimize drift of the tail pieces
		behind = VecAdd(tailTable[i-1].transform.pos,TransformToParentVec(tailTable[i-1].transform,behind))
		--tailTable[i].transform.pos = VecCopy(behind) --enable to directly move the transform
		tailTable[i].targetPos = VecCopy(behind)
	end
	
	--tail rotation
	local behind = Vec(0,0,0)
	if i ==1 then
		behind = VecAdd(headTransform.pos, TransformToParentVec(headTransform,behind))
	else
		behind = Vec(0,0,0) --Where each tail unit is aimed at
		behind = VecAdd(tailTable[i-1].transform.pos, TransformToParentVec(tailTable[i-1].transform,behind))
	end
	tailTable[i].transform.rot = QuatLookAt(tailTable[i].transform.pos, behind)
	SetBodyTransform(tailTable[i].body,tailTable[i].transform)
	end
end


function patrolPointMove(dt)
	local radius = clamp(13+timeSinceChoosePatrol*2.5,10,30)
	if playerSeen then radius = 0 end
	if not patrolPointAngle then patrolPointAngle = 0 end
	patrolPointAngle = patrolPointAngle%360 + dt*15 --360 cause i dont want this to runaway
	local x = math.cos(math.rad(patrolPointAngle))*radius
	local z = math.sin(math.rad(patrolPointAngle))*radius
	targetPosOffset = VecAdd(targetPos,Vec(x,0,z))
	--distance control
	local toTarget = VecSub(targetPosOffset, headTargetPos)
	toTarget[2] = 0 --flatten
	local dis = VecLength(toTarget)
	local minDist = 15 --slow down here
	local toSpeed = clamp(minSpeed+(dis-minDist)/2,minSpeed,maxSpeed)
	if toSpeed > speed then
		speed = clamp(speed + 15*dt, minSpeed, maxSpeed)--throttle up
	else
		speed = clamp(speed - 15*dt, minSpeed, maxSpeed)--brakes
	end--]]
end
function forwardWingMode()
	for w=1, #leftWingTable do
		local leftOffset = leftWingTable[w].offset
		local leftSweep = QuatCopy(leftWingTable[w].sweep)
		if speed > fastSpeedThreshold then
			leftSweep = QuatRotateQuat(leftSweep,QuatEuler(0,0,leftWingTable[w].fasthedral))
		end
		leftWingControl(w,leftOffset, leftSweep)
	end
	for w=1, #rightWingTable do
		local rightOffset = rightWingTable[w].offset
		local rightSweep = QuatCopy(rightWingTable[w].sweep)
		if speed > fastSpeedThreshold then
			rightSweep = QuatRotateQuat(rightSweep,QuatEuler(0,0,rightWingTable[w].fasthedral))
		end
		rightWingControl(w,rightOffset, rightSweep)
	end
end
function patrolDir()
--height control
	computeSurroundingHeight(35,headTargetPos,5)
	computeSurroundingHeight(37,headTargetPos,5)
	computeSurroundingHeight(35,headTransform.pos,5)
	computeSurroundingHeight(0,headTargetPos,height)
	computeSurroundingHeight(3,headTargetPos,height)
	computeSurroundingHeight(3,headTransform.pos,height)
	computeSurroundingHeight(4,headTransform.pos,height)
	toHeight = averageSurroundingHeight
	
	--look direction determinant
	local toPoint = VecNormalize(VecSub(targetPosOffset, headTargetPos))
	local lookPoint = VecAdd(headTargetPos, toPoint)
	lookPoint[2] = toHeight --+math.sin(GetTime()*1.2)*13--hold altitude
	local rot = QuatLookAt(headTargetPos, lookPoint)
	headTargetRot = rot
	headLookRot = QuatCopy(headTransform.rot)
end

function alertMove(dt)
	--movement
	local hoverPos = VecCopy(targetPos) --need distance here...
	local toTarget = VecSub(hoverPos, tailTable[1].targetPos)
	toTarget[2] = 0 --flatten
	local dis = VecLength(Vec(toTarget[1],0,toTarget[3]))
	local maxDist = hoverMaxDist --hover within bound
	local minDist = hoverMinDist --back up to here
	
	--movement controlled by neck unit, tailTable[1]
	toTarget = VecNormalize(toTarget)
	if dis > maxDist then
		toSpeed = dis- maxDist
		toSpeed = clamp(toSpeed,-maxSpeed,maxSpeed)
		if toSpeed > speed then 
			speed = speed+9*dt
		else
			speed = speed-9*dt
		end	
		toTarget = VecNormalize(toTarget)
		tailTable[1].targetPos = VecAdd(tailTable[1].targetPos, VecScale(toTarget, speed*dt))
	elseif dis <= minDist then
		toSpeed = minDist - dis
		toSpeed = clamp(toSpeed,-maxSpeed,maxSpeed)
		if toSpeed > speed then 
			speed = speed+9*dt
		else
			speed = speed-9*dt
		end
		toTarget = VecNormalize(toTarget)		
		tailTable[1].targetPos = VecSub(tailTable[1].targetPos, VecScale(toTarget, speed*dt))
	else
		toSpeed = 0
		speed = clamp(speed,-13,13)
		if toSpeed > speed then 
			speed = speed+9*dt
			tailTable[1].targetPos = VecAdd(tailTable[1].targetPos, VecScale(VecNormalize(toTarget), speed*dt))
		else
			speed = speed-9*dt
			tailTable[1].targetPos = VecAdd(tailTable[1].targetPos, VecScale(VecNormalize(toTarget), speed*dt))
		end
		
	end--]]
end
function hoverTailControl(dt)
	--tail control, hovering
	for i=2,#tailTable do
	if i == 1 then --behind the head
		local behind = Vec(0,-1,distance+3)
		behind = TransformToParentPoint(headTransform,behind)
		--tailTable[i].transform.pos = VecCopy(behind) --enable to directly move the transform
		tailTable[i].targetPos = VecLerp(behind)
	elseif i == clavicleNum then --first winged unit
		local behind = Vec(0,-4.6,distance-0.8)
		behind = TransformToParentPoint(tailTable[i-1].transform,behind)
		--tailTable[i].transform.pos = VecCopy(behind) --enable to directly move the transform
		tailTable[i].targetPos = VecCopy(behind)
	elseif i > clavicleNum and i <(clavicleNum+#leftWingTable) then --next winged units
		local behind = Vec(0,math.cos(i*4-8.8)*3,distance)
		behind = TransformToParentPoint(tailTable[i-1].transform,behind)
		--tailTable[i][2].pos = VecCopy(behind) --enable to directly move the transform
		tailTable[i].targetPos = VecCopy(behind)
	elseif i >=(clavicleNum+#leftWingTable) and i <(clavicleNum+#leftWingTable+2) then
		local behind = Vec(math.sin(GetTime()*3.5)*0.47, math.cos(i*0.7-3.7)*3.3,distance) --rest of the tail behind the wings
		behind = TransformToParentPoint(tailTable[i-1].transform,behind)
		--tailTable[i].transform.pos = VecCopy(behind) --enable to directly move the transform
		tailTable[i].targetPos = VecCopy(behind)
	else
		local behind = Vec(0,math.cos(i*0.8-5.3)*2,distance) --rest of the tail behind the wings
		behind = TransformToParentPoint(tailTable[i-1].transform,behind)
		--tailTable[i].transform.pos = VecCopy(behind) --enable to directly move the transform
		tailTable[i].targetPos = VecCopy(behind)
	end--]]
	
	--tail rotation
	local behind = Vec(0,0,0)
	if i ==1 then
	behind = VecAdd(headTransform.pos, TransformToParentVec(headTransform,behind))
	else
	behind = Vec(0,0,0)
	behind = VecAdd(tailTable[i-1].transform.pos, TransformToParentVec(tailTable[i-1].transform,behind))
	end
	tailTable[i].transform.rot = QuatLookAt(tailTable[i].transform.pos, behind)
	SetBodyTransform(tailTable[i].body,tailTable[i].transform)
	end
end
function hoverWingMode()
--wing animations
	--oh god it's total tag hell in the editor
	for w=1, #leftWingTable do
		local leftOffset = leftWingTable[w].offset
		local leftSweep = GetTagValue(leftWingTable[w].body,"sweep")
		leftSweep = tagToVec(leftSweep)--get xyz for QuatEuler
		pivot = leftWingTable[w].hoverpivot

		leftSweep = QuatEuler(leftSweep[1]+pivot,leftSweep[2],leftSweep[3])		
		
		leftWingControl(w,leftOffset, leftSweep)
	end
	for w=1, #rightWingTable do
		local rightOffset = rightWingTable[w].offset
		local rightSweep = GetTagValue(rightWingTable[w].body,"sweep")
		rightSweep = tagToVec(rightSweep)--get xyz for QuatEuler
		pivot = rightWingTable[w].hoverpivot
		
		rightSweep = QuatEuler(rightSweep[1]+pivot,rightSweep[2],rightSweep[3])
		
		rightWingControl(w,rightOffset, rightSweep)
	end
end
function hoverDir(dt)
--look direction determinant
	headTargetPos = TransformToParentPoint(tailTable[1].transform,Vec(0,0,-5))
	local toPoint = VecNormalize(VecSub(targetPos, headTargetPos))
	local lookPoint = VecAdd(headTargetPos, VecSub(targetPos, headTargetPos))
	
	local radius = clamp(timeSinceChoosePatrol*1.6,0,10)
	if not lookAngle then lookAngle = 0; lookAngleY = 0 end
	lookAngle = lookAngle%360 + dt
	aimAngle = lookAngle
	lookAngleY = lookAngleY%360 + 1.7*dt
	aimAngleY = lookAngleY --hope this forces the searchlight to align with the head direction
	local x = math.cos(lookAngle) * radius
	local z = math.sin(lookAngleY) * radius
	lookPoint = VecAdd(lookPoint,Vec(x,0,z))
	local rot = QuatLookAt(headTargetPos, lookPoint)
	headTargetRot = rot
	
	local neckToPoint = VecNormalize(VecSub(targetPos,tailTable[1].targetPos))
	neckToPoint[2] = 0 --keep neck flat
	local neckLookPoint = VecAdd(tailTable[1].targetPos,neckToPoint)
	local neckRot = QuatLookAt(tailTable[1].targetPos,neckLookPoint)
	tailTable[1].targetRot = neckRot 
end
function hoverHeightControl(dt)
--height control
	computeSurroundingHeight(25)--head scan
	computeSurroundingHeight(23,headTransform.pos,height)
	computeSurroundingHeight(30,tailTable[5].transform.pos,height)--midsection scan
	computeSurroundingHeight(33,tailTable[5].transform.pos)
	computeSurroundingHeight(23,tailTable[1].targetPos,height) --neck unit scan
	computeSurroundingHeight(30,tailTable[1].targetPos)
	computeSurroundingHeight(25,tailTable[1].transform.pos,height)
	computeSurroundingHeight(0,tailTable[1].targetPos,height)--proxy for standard height check
	computeSurroundingHeight(2,tailTable[1].targetPos,height)
	computeSurroundingHeight(3,tailTable[1].targetPos,height)
	toHeight = averageSurroundingHeight

	tailTable[1].targetPos[2] = averageSurroundingHeight
end
function basiliscoHoverDodge(directed) --flash step
	local shiftVec = VecNormalize(Vec(rndFloat(-1,1),0,0))
	shiftVec[3] = rndFloat(-0.66,0.66)
	shiftVec = VecNormalize(shiftVec)
	shiftVec = VecScale(shiftVec,math.random(21,69))
	shiftVec[2] = math.random(-15,37)
	
	if not directed then
		shiftVec = VecNormalize(Vec(rndFloat(-1,1),0,rndFloat(-1,1)))
		shiftVec = VecScale(shiftVec, 23)
	end
	if directed then 
		tailTable[1].targetPos = VecAdd(tailTable[1].targetPos,TransformToParentVec(tailTable[1].transform,shiftVec)) --relative to fwd
	else
		tailTable[1].targetPos = VecAdd(tailTable[1].targetPos,shiftVec) --in world space
	end
	timeSinceChoosePatrol = 0 --refresh alertness
end

function strafeDir()
	computeSurroundingHeight(35,headTargetPos,5)
	computeSurroundingHeight(37,headTargetPos,5)
	computeSurroundingHeight(35,headTransform.pos,5)
	computeSurroundingHeight(0,headTargetPos,patrolheight)
	computeSurroundingHeight(3,headTargetPos,patrolheight)
	computeSurroundingHeight(3,headTransform.pos,patrolheight)
	computeSurroundingHeight(4,headTransform.pos,patrolheight)
	toHeight = math.max(averageSurroundingHeight,300)
	
	--look direction determinant
	local toPoint = nil
	local targetPosShift = VecCopy(targetPos)
	targetPosShift[2] = targetPosShift[2]+1.83 --how tall you are
	if not strafeStart then
		toPoint = VecCopy(strafeStartVec)
	elseif strafeEnd then
		toPoint = VecNormalize(VecSub(strafeEndVec,headTargetPos))
	else
		toPoint = VecNormalize(VecSub(targetPosShift, headTargetPos))
	end
	local lookPoint = VecAdd(headTargetPos, toPoint)
	if not strafeStart then lookPoint[2] = toHeight end
	local rot = QuatLookAt(headTargetPos, lookPoint)
	headTargetRot = rot
	headLookRot = QuatCopy(headTransform.rot)
end
--wing function for flight modules to call

function leftWingControl (w, leftOffset, leftSweep) 
	leftWingTable[w].targetRot = QuatSlerp(leftWingTable[w].targetRot, leftSweep, 0.08)
	leftWingTable[w].transform.rot = leftWingTable[w].targetRot
	
	leftWingTable[w].targetPos = VecLerp(leftWingTable[w].targetPos,leftOffset,0.08)
	leftWingTable[w].transform.pos = leftWingTable[w].targetPos
	local mount = tailTable[leftWingTable[w].tailNum].transform
	SetBodyTransform(leftWingTable[w].body,TransformToParentTransform(mount,leftWingTable[w].transform))--moves left wing
end

function rightWingControl(w, rightOffset, rightSweep)
	rightWingTable[w].targetRot = QuatSlerp(rightWingTable[w].targetRot, rightSweep, 0.08)
	rightWingTable[w].transform.rot = rightWingTable[w].targetRot
	
	rightWingTable[w].targetPos = VecLerp(rightWingTable[w].targetPos,rightOffset,0.08)
	rightWingTable[w].transform.pos = rightWingTable[w].targetPos
	local mount = tailTable[rightWingTable[w].tailNum].transform
	SetBodyTransform(rightWingTable[w].body,TransformToParentTransform(mount,rightWingTable[w].transform)) -- moves right wing
end


function enginePlacement(dt)
	engineLLocalTransform.rot = QuatEuler(engineLrot[1],engineLrot[2],engineLrot[3])
	engineRLocalTransform.rot = QuatEuler(engineRrot[1],engineRrot[2],engineRrot[3])
	SetBodyTransform(engineL, TransformToParentTransform(tailTable[engineLtailNum].transform,engineLLocalTransform))
	SetBodyTransform(engineR, TransformToParentTransform(tailTable[engineRtailNum].transform,engineRLocalTransform))
end

function cordiumParticle()--set up engine particles
	ParticleReset()
	ParticleTile(4)
	ParticleRadius(0.1)
	ParticleStretch(0)	
	ParticleEmissive(3,0.14,"easeout")
	ParticleColor(cordiumR,cordiumG,cordiumB)
	ParticleGravity(rndFloat(-3,2))
	ParticleSticky(0,1)
	ParticleDrag(rndFloat(0.1,0.15))
	ParticleCollide(1,1,"constant",0.02)
end
function wingthrusters() --spews cordium from the engines
	cordiumParticle()
	
	for w, wing in ipairs(leftWingTable) do
		local t = tailTable[wing.tailNum].transform --transform of tail unit
		local minbb = Vec(-9.5,-1,5)--minimum of bounding box
		local maxbb = Vec(-20,1,7.7)--maximum of bounding box
		local emitter = Vec(rndFloat(minbb[1],maxbb[1]), rndFloat(minbb[2],maxbb[2]),rndFloat(minbb[3],maxbb[3]))
		local s = wing.transform --start
		s = TransformToParentTransform(t,s)
		local thrustpower = -speed+34+rndFloat(0,15)
		local thrustdir = TransformToParentVec(s,Vec(rndFloat(-3,3),0,thrustpower*60*GetTimeStep()))
		SpawnParticle(TransformToParentPoint(s,emitter),thrustdir,rndFloat(1,3))
	end
	
	for w, wing in ipairs(rightWingTable) do
		local t = tailTable[wing.tailNum].transform --transform of tail unit
		local minbb = Vec(9.5,-1,5)--minimum of bounding box
		local maxbb = Vec(20,1,7.7)--maximum of bounding box
		local emitter = Vec(rndFloat(minbb[1],maxbb[1]), rndFloat(minbb[2],maxbb[2]),rndFloat(minbb[3],maxbb[3]))
		local s = wing.transform --start
		s = TransformToParentTransform(t,s)
		local thrustpower = -speed+34+rndFloat(0,15)
		local thrustdir = TransformToParentVec(s,Vec(rndFloat(-3,3),0,thrustpower*60*GetTimeStep()))
		SpawnParticle(TransformToParentPoint(s,emitter),thrustdir,rndFloat(1,3))
	end
	
end
function largeThrusters()
		cordiumParticle()
		local exhaustl = Vec(-0.7+rndFloat(-1.3,1.3),rndFloat(-0.5,0.5),8.8) 
		local exhaustr = Vec(0.7+rndFloat(-1.3,1.3),rndFloat(-0.5,0.5),8.8)
		local engineLTrans = TransformToParentTransform(tailTable[engineLtailNum].transform,engineLLocalTransform)
		local engineRTrans = TransformToParentTransform(tailTable[engineRtailNum].transform,engineRLocalTransform)
		
		local exhaustl = TransformToParentPoint(engineLTrans,exhaustl)
		local exhaustr = TransformToParentPoint(engineRTrans,exhaustr)
		
		SpawnParticle(exhaustl,TransformToParentVec(engineLTrans,Vec(rndFloat(-16,10),0,55*60*GetTimeStep())),rndFloat(1,3))
		SpawnParticle(exhaustr,TransformToParentVec(engineRTrans,Vec(rndFloat(-10,16),0,55*60*GetTimeStep())),rndFloat(1,3))
end

function shedding() --shedding flakes... of some kind from the body pieces
	ParticleReset()
	ParticleTile(6) --6 for squares
	ParticleRadius(0.1)
	ParticleAlpha(1,0,"easein")
	ParticleStretch(0)
	ParticleEmissive(4,0)
	ParticleColor(0.04,0.04,0.04,0,0,0,"easein")
	ParticleGravity(rndFloat(-0.78,-2))
	ParticleSticky(0,1)
	ParticleCollide(1,1,"constant",0.3)
	
	local emit = Vec(rndFloat(-3,3), rndFloat(-3,0),rndFloat(-3,3))
	local emitUnit = math.random(1,5) --shedding for wing units
	SpawnParticle(TransformToParentPoint(tailTable[emitUnit].transform,emit),Vec(),rndFloat(3,6))
end

--for update(dt)
function alicantoHeadUpdate(headtranspos,headTargetPos,headtransrot,headTargetRot,turnRate,dt)
	headtranspos = VecLerp(headtranspos, headTargetPos, 0.8)
	headtransrot = QuatSlerp(headtransrot, headTargetRot, turnRate*speed*dt)
	return headtranspos, headtransrot
end

function alicantoTailUpdate(tailtranspos, tailTargetPos,dt)
	tailMinLerp = 0
	hoverVertiVel = 0
	vertiVelMult = 4
	tailtranspos = VecLerp(tailtranspos,tailTargetPos, 0.26*speed*dt)
	return tailtranspos
end

function basiliscoHoverUpdate(dt)
	--control unit pos updater
	headLookRot = QuatSlerp(headLookRot, headTargetRot, turnRate)
	tailTable[1].transform.rot = QuatSlerp(tailTable[1].transform.rot,tailTable[1].targetRot,turnRate/5)
	
	--tail update
	if not hoverVertiVel then hoverVertiVel = 0 end --very crude graft code
	if not vertiVelMult then vertiVelMult = 4 end
	if not hoverDrag then hoverDrag = 0.98 end
	--tailMinLerp = clamp(tailMinLerp+dt/2,0,1) --smooth the transition of tail units to hover mode
	SetValue("tailMinLerp",1,"linear", 1)
	SetValue("vertiVelMult",1.3,"linear",0.64)
	SetValue("hoverDrag",0.98,"easeout",0.5)
	
	for i=1,#tailTable do
		if i ==1 then --neck unit
			local previousHeight = tailTable[i].transform.pos[2]
			tailTable[i].transform.pos = VecLerp(tailTable[i].transform.pos,tailTable[i].targetPos, clamp(0.9*speed*dt,0.22,1))
			tailTable[i].transform.pos[2] = previousHeight
			--smoother vertical velocity based on choppervel update code
			hoverVertiVel = hoverVertiVel+(tailTable[i].targetPos[2] - tailTable[i].transform.pos[2])*dt*vertiVelMult
			hoverVertiVel = hoverVertiVel * 0.98
			tailTable[i].transform.pos[2] = tailTable[i].transform.pos[2] +hoverVertiVel*dt
		elseif i >= clavicleNum and i <(clavicleNum+#leftWingTable) then --winged units
			tailTable[i].transform.pos = VecLerp(tailTable[i].transform.pos,tailTable[i].targetPos, clamp(1.3*speed*dt*tailMinLerp,9*dt,1))
		elseif i >=(clavicleNum+#leftWingTable) then
			tailTable[i].transform.pos = VecLerp(tailTable[i].transform.pos,tailTable[i].targetPos, 6*dt)
		end
	end
end