
require("Actors/AI/PID")

function Create(self)
	---------------- AI variables start ----------------
	self.StableTimer = Timer()
	self.StuckTimer = Timer()
	self.DoorTimer = Timer()
	
	self.ObstacleTimer = Timer()
	self.ObstacleTimer:SetSimTimeLimitMS(100)
	
	self.PlayerInterferedTimer = Timer()
	self.PlayerInterferedTimer:SetSimTimeLimitMS(500)
	
	self.DeliveryState = ACraft.FALL
	self.LastAIMode = Actor.AIMODE_NONE
	self.Controller = self:GetController()
	self.groundDist = self.Radius / 1.35
	
	self.LZpos = SceneMan:MovePointToGround(self.Pos, self.groundDist, 9)
	self.velIntegrator = 0
	
	if self.AIMode == Actor.AIMODE_DELIVER and self:IsInventoryEmpty() then
		self.AIMode = Actor.AIMODE_STAY	-- stop the craft from returning to orbit immediately
	end
	
	function self:MoveLZ()
		local FuturePos = self.Pos + self.Vel*7
		
		-- Make sure FuturePos is inside the scene
		if FuturePos.X > SceneMan.SceneWidth then
			if SceneMan.SceneWrapsX then
				FuturePos.X = FuturePos.X - SceneMan.SceneWidth
			else
				FuturePos.X = SceneMan.SceneWidth - self.Radius
			end
		elseif FuturePos.X < 0 then
			if SceneMan.SceneWrapsX then
				FuturePos.X = FuturePos.X + SceneMan.SceneWidth
			else
				FuturePos.X = self.Radius
			end
		end
		
		if self.DeliveryState == ACraft.LAUNCH then
			self.LZpos.X = FuturePos.X
		else
			local TestPos = Vector(FuturePos.X, math.min(FuturePos.Y, self.Pos.Y))
			self.LZpos = SceneMan:MovePointToGround(TestPos, self.groundDist, 5)
		end
	end
	
	-- The controllers
	self.AngPID = RegulatorPID:New{p=2.7, i=0.01, d=0.9, last_input=self.RotAngle, filter_leak=0.6, integral_max=150}
	self.XposPID = RegulatorPID:New{p=0.2, d=0.5, filter_leak=0.6, integral_max=100}
	self.YposPID = RegulatorPID:New{p=0.012, i=0.07, d=4, last_input=self.LZpos.Y, filter_leak=0.5, integral_max=30}
	
	---------------- AI variables end ----------------
end

function UpdateAI(self)
	if self.PlayerInterferedTimer:IsPastSimTimeLimit() then
		self.StuckTimer:Reset()
		self:MoveLZ()
	end
	
	self.PlayerInterferedTimer:Reset()
	
	if self.AIMode ~= self.LastAIMode then
		self.LastAIMode = self.AIMode

		if self.AIMode == Actor.AIMODE_RETURN then
			self.DeliveryState = ACraft.LAUNCH
			self.LZpos.Y = -10000	-- Go to orbit
			self:MoveLZ()
		else
			self.DeliveryState = ACraft.FALL
			self:MoveLZ()
		end
	end
	
	-- Reset StableTimer if not in a stable and upright state
	self.velIntegrator = self.velIntegrator * 0.8 + self.Vel.Magnitude * 0.2
	if self.velIntegrator > 3 or math.abs(self.AngularVel) > 0.75 then
		self.StableTimer:Reset()
	else
		self.LZpos.X = self.Pos.X
	end
	
	-- Delivery Sequence logic
	if self.DeliveryState == ACraft.FALL then		
		self.checkLZ = not self.checkLZ	-- Only check every second frame
		if self.checkLZ then
			-- Move Landing Zone closer to the rocket if it is about to collide with something
			if (self.LZpos.Y-self.Pos.Y) > self.Radius then
				local Speed = (self.Vel + Vector(0,4))*15
				if SceneMan:CastObstacleRay(self.Pos, Speed, Vector(), Vector(), self.ID, self.IgnoresWhichTeam, 0, 10) > -1 then
					self:MoveLZ()
				end
			end
		else
			-- Check for something in the way of our descent, and move to the side to avoid it
			if self.ObstacleTimer:IsPastSimTimeLimit() then
				self.ObstacleTimer:Reset()
				
				local Trace = Vector(self.Vel.X+RangeRand(-1,1), math.max(self.Vel.Y,2)) * 40
				local obstID = SceneMan:CastMORay(self.Pos, Trace, self.ID, self.IgnoresWhichTeam, 0, false, 5)
				if obstID < 255 then
					local MO = MovableMan:GetMOFromID(MovableMan:GetRootMOID(obstID))
					if MO.ClassName == "ACDropShip" or MO.ClassName == "ACRocket" then
						self.LZpos:SetXY(MO.Pos.X+MO.Radius*3, math.max(self.Pos.Y-200,0))
						self.ObstacleTimer:SetSimTimeLimitMS(750)
						self.obstacle = true
					elseif MovableMan:IsActor(MO) and MO.Team == self.Team then
						local newLZx = MO.Pos.X+MO.Diameter+self.Diameter
						
						-- Make sure newLZx is inside the scene
						if newLZx > SceneMan.SceneWidth then
							if SceneMan.SceneWrapsX then
								newLZx = newLZx - SceneMan.SceneWidth
							else
								newLZx = MO.Pos.X - (MO.Diameter + self.Diameter)
							end
						end
						
						self.LZpos:SetXY(newLZx, math.max(MO.Pos.Y-200,0))
					end
				elseif self.obstacle then
					self.obstacle = false
					self:MoveLZ()
					self.ObstacleTimer:SetSimTimeLimitMS(100)
				end
			end
		end
		
		if self.AIMode == Actor.AIMODE_DELIVER and self:IsInventoryEmpty() then
			self.DeliveryState = ACraft.LAUNCH	-- Don't descend if we have nothing to deliver
			self.LZpos.Y = -10000	-- Go to orbit
		else
			if self.StableTimer:IsPastSimMS(500) then	-- Move LZ if stable
				self.LZpos = SceneMan:MovePointToGround(self.Pos, self.groundDist, 6)
			end
			
			if self.AIMode ~= Actor.AIMODE_STAY then
				local dist = SceneMan:ShortestDistance(self.Pos, self.LZpos, false).Magnitude
				if dist < 25 then	-- If we passed the check, start unloading
					self.DeliveryState = ACraft.UNLOAD
				end
			end
		end
	elseif self.DeliveryState == ACraft.UNLOAD then
		if self:IsInventoryEmpty() and self.AIMode ~= Actor.AIMODE_STAY then	-- Return to orbit if empty
			if self.DoorTimer:IsPastSimMS(750) then	-- Pause before returning to orbit
				self.DeliveryState = ACraft.LAUNCH
				self.LZpos.Y = -10000	-- Go to orbit
				if self.HatchState == ACraft.OPEN then
					self:CloseHatch()
				end
			end
		elseif self.StableTimer:IsPastSimMS(400) and self.HatchState == ACraft.CLOSED then
			self:OpenHatch()
			self.DoorTimer:Reset()
		end
	elseif self.DeliveryState == ACraft.LAUNCH then
		-- Check for something in the way of our ascent, and move to the side to avoid it
		if self.ObstacleTimer:IsPastSimTimeLimit() then
			self.ObstacleTimer:Reset()
			
			local Trace = Vector(self.Vel.X+RangeRand(-1,1), math.min(self.Vel.Y,-1)) * 50
			local obstID = SceneMan:CastMORay(self.Pos, Trace, self.ID, self.IgnoresWhichTeam, 0, false, 5)
			if obstID < 255 then
				local MO = MovableMan:GetMOFromID(MovableMan:GetRootMOID(obstID))
				if MO.ClassName == "ACDropShip" or MO.ClassName == "ACRocket" then
					self.LZpos:SetXY(MO.Pos.X-(MO.Diameter+self.Diameter), self.Pos.Y+self.Vel.Y)
					self.ObstacleTimer:SetSimTimeLimitMS(750)
					self.obstacle = true
				end
			elseif self.obstacle then
				self.obstacle = false
				self.LZpos.Y = -10000	-- Go to orbit
				self:MoveLZ()
				self.ObstacleTimer:SetSimTimeLimitMS(100)
			else
				self:MoveLZ()	-- move the exit point closer towards us
			end
		end
	end
	
	-- Control up/down movement
	if self.DeliveryState ~= ACraft.UNLOAD then
		--local limit = self.Mass * -0.0157 + 29	-- Descend slower when carrying a heavy cargo
		local change = self.YposPID:Update(-(self.LZpos.Y-(self.Pos.Y+self.Vel.Y)), 0)
		if math.abs(self.RotAngle) < 0.9 then
			if self.DeliveryState == ACraft.LAUNCH and change > 7 then
				self.burstUp = nil
				self.Controller:SetState(Controller.MOVE_UP, true)	-- Don't burst when returning to orbit
			else
				if change > 11 and not self.burstUp then
					self.burstUp = math.max(15-change, 4) -- Wait n frames until next burst (lower -> better control)
				elseif change < -20 then
					self.burstUp = nil
					self.Controller:SetState(Controller.MOVE_DOWN, true)
				end
			end
		elseif self.RotAngle > 2.14 and self.RotAngle < 4.14 then	-- Upside down
			self.Controller:SetState(Controller.MOVE_DOWN, true)
		end
	end
	
	-- Control right/left movement (the rocket will move sideways if rotated to the side)
	local dist = SceneMan:ShortestDistance(self.Pos+self.Vel*20, self.LZpos, false).X
	local change = self.XposPID:Update(dist, 0)
	local targetAng = 0
	if self.Vel.Y > 0 then
		if change < -4 then
			targetAng = -math.max(change/40, -0.5)
		elseif change > 4 then
			targetAng = -math.min(change/40, 0.5)
		end
	else
		if change > 4 then
			targetAng = -math.max(change/40, -0.5)
		elseif change < -4 then
			targetAng = -math.min(change/40, 0.5)
		end
	end
	
	-- Control angle
	change = self.AngPID:Update(self.RotAngle+self.AngularVel, targetAng)
	if change > 1.1 and not self.burstRight then
		self.burstRight = math.max(5-change, 2) -- Wait n frames until next burst (lower -> better control)
	elseif change < -1.1 and not self.burstLeft then
		self.burstLeft = math.max(5+change, 2)
	end
	
	-- Trigger bursts
	if self.burstRight then
		self.burstRight = self.burstRight - 1
		if self.burstRight < 0 then
			self.Controller:SetState(Controller.MOVE_RIGHT, true)
			if self.burstRight < -4 then	-- Fire for -n frames (higher -> better control)
				self.burstRight = nil	-- Allow a new burst
			end
		end
	end
	
	if self.burstLeft then
		self.burstLeft = self.burstLeft - 1
		if self.burstLeft < 0 then
			self.Controller:SetState(Controller.MOVE_LEFT, true)
			if self.burstLeft < -4 then
				self.burstLeft = nil
			end
		end
	end
	
	if self.burstUp then
		self.burstUp = self.burstUp - 1
		if self.burstUp < 0 then
			self.Controller:SetState(Controller.MOVE_UP, true)
			if self.burstUp < -12 then
				self.burstUp = nil
			end
		end
	end
	
	-- If we are hopelessly stuck, self destruct
	if self.Vel.Largest > 3 or self.AIMode == Actor.AIMODE_STAY then
		self.StuckTimer:Reset()
	elseif self.AIMode == Actor.AIMODE_SCUTTLE or self.StuckTimer:IsPastSimMS(40000) then
		self:GibThis()
	end
end
