-- This is paid software, do not distribute, Rule 6.5

-- Settings, change them for different effects
local ownerUserName	= "jasper_185" -- If set uses getMetaByName instead of getMetaOwner when getMetaOwner isn't available
local maxSpeed = 1	-- The speed of the last slot, note: it is impossible to go over speed 4, and trying to go over that limit will cause your movement direction to not sync up with your actual velocity "pushing" you towards the diagonals
local launchMult = 1	-- Higher values = stronger launch, may cause overshoot if too high
local baseSlotSpeed = 1	-- The base speed you got at, if set to 0 your slot 0 will not let you move at all, should be 0 if not using a keyboard module (WHY ARE YOU NOT USING IT? ITS AWESOME AND CHEAP)
local speedPower = 0	-- The power the speed is raised to (0 = constant speed, 1 = linear speed increase from each slot, 2 = exponential speed increase)
local sprintSpeedIncrease = 4	-- Whenever we are sprinting, how much faster do we go?
local gpsHover = true -- Do we use the GPS to stay still in the air while we are not moving?
local gpsStrength = 0.025	-- How strong is the GPS correction? (really doesn't need to be strong, as base hover is already really good and instead just makes it less stable)
local pitchChangesForward = false-- If you are looking up and pressing forward do you go up or do you go horizontal?
local doubleTapUpToToggle = 0.3 --t Whats the duration of time to toggle flight by spamming the up key? set to nil or to a negative number to disable and use a dedicated toggle key instead
local lagCompensation = true	-- Do we try calculate the amount of lag we have, and try to compensate for that?
local smoothing = 0.65	-- How smooth is the flight? 0 = no smoothing, 1 = so smooth you will never start moving, 0.9 = pretty smooth
local autoDisengageOnGroundTouch = true	-- Do we disable flight once we touch the ground?

-- Still settings, but more technical
local gravityCancelation = 0.08	-- How strong gravity is, too low and you will fall down, too high and you will shoot up (The constant I for the Y PID)
local drag = 0.98	-- What is the drag
local waterGravityCancelation = 0.01	-- Water version of the above
local PStrength = 1	-- Strenght of P in PD settings
local DStrength = 1	-- Strenght of D in PD settings
local rateLimitAtPower = 1.5	-- Going above this power rate limits us
local yMult = 0.5	-- Launching in the Y dir is less strong
local elytraMult = 0.4	-- While elytra flying all dirs are less strong
local maxXZRatio = 0.8	-- How much X and Z axes can be reduced for more power in the Y axis before stopping, lowering this too much will give bad controll when going at high speeds, but having it too high will make hoving slightly less stable

-- Keybinds
local forwardKey = keys.w;
local backKey = keys.s;
local leftKey = keys.a;
local rightKey = keys.d;
local upKey = keys.space;
local downKey = keys.leftShift;
local SprintKey = keys.r;	-- Lets you hold this key to enable sprint mode, so it also lets you go at sprint speed backwards
local toggleFlightKey = keys.g;	-- Only used when doubleTapSpaceToToggle is set to false

-- Internal code, don't change unless you know what you are doing!
local modules = peripheral.wrap("back")
local hasKeyboard = modules.hasModule("plethora:keyboard")

local pressedKeys = {}

-- Global for isActive
local flightActive = false
local lastGroundTouch = os.clock()
local function isFlightActiveRaw(meta)
	if hasKeyboard then
		if autoDisengageOnGroundTouch then
			if lastGroundTouch + 0.5 < os.clock() and not meta.isAirborne then
				flightActive = false
			end

			if not meta.isAirborne then
				lastGroundTouch = os.clock()
			end
		end
		return flightActive	-- Toggled by keyboard module
	end

	-- Enable flight when sneaking and having a decent Y vel so it doesn't activate when just holding shift on a block
	if meta.isSneaking then
		if not flightActive then
			flightActive = math.abs(meta.motionY) > 0.2
		end
	else
		flightActive = false
	end
	return flightActive
end

-- Calculates your desired speed
local isSprinting = false
local function getSpeedRaw(meta)
	local selectedSlot = meta.heldItemSlot

	local speed = math.pow((selectedSlot + baseSlotSpeed) / (9 + baseSlotSpeed), speedPower) * maxSpeed;
	if meta.isSprinting or pressedKeys[SprintKey] then
		isSprinting = true
	elseif not pressedKeys[forwardKey] and not pressedKeys[backKey] and not pressedKeys[leftKey] and not pressedKeys[rightKey] and not pressedKeys[upKey] and not pressedKeys[downKey] then	-- Not great, but oh well
		isSprinting = false
	end

	if isSprinting then
		speed = speed * sprintSpeedIncrease
	end

	return speed
end

-- Calculates your desired movement direction
local gpsX, gpsY, gpsZ
local function getDesiredDirRaw(meta)
	-- If you want a goto function, you can calculate the direction to the next waypoint and return that here for example

	if not hasKeyboard then
		local pitch = -math.rad(meta.pitch)
		local yaw = math.rad(meta.yaw)

		local dx = -math.sin(yaw) * math.cos(pitch)
		local dy = math.sin(pitch)
		local dz = math.cos(yaw) * math.cos(pitch)

		return dx, dy, dz
	else
		local pitch = -math.rad(meta.pitch)
		if not pitchChangesForward then pitch = 0 end

		local yaw = math.rad(meta.yaw)

		local fx = -math.sin(yaw) * math.cos(pitch)
		local fy = math.sin(pitch)
		local fz = math.cos(yaw) * math.cos(pitch)

		local sx = math.cos(yaw)
		local sz = math.sin(yaw)

		local forward = (pressedKeys[forwardKey] and 1 or 0) + (pressedKeys[backKey] and -1 or 0)
		local left = (pressedKeys[leftKey] and 1 or 0) + (pressedKeys[rightKey] and -1 or 0)
		local up = (pressedKeys[upKey] and 1 or 0) + (pressedKeys[downKey] and -1 or 0)

		return fx * forward + sx * left,
			fy * forward + up,
			fz * forward + sz * left
	end
end

-- Below is the actual math / logic, i don't recommend changing this

-- Overrides
local defaultIsFlightActiveOverride = function(defaultFlightActive, meta) return defaultFlightActive end
local defaultGetSpeedOverride = function(defaultSpeed, meta) return defaultSpeed end
local defaultGetDesiredDirOverride = function(defaultDirX, defaultDirY, defaultDirZ, meta) return defaultDirX, defaultDirY, defaultDirZ end
local defaultMotionOverride = function(defaultMotionX, defaultMotionY, defaultMotionZ, meta) return defaultMotionX, defaultMotionY, defaultMotionZ end

local isFlightActiveOverride = defaultIsFlightActiveOverride
local getSpeedOverride = defaultGetSpeedOverride
local getDesiredDirOverride = defaultGetDesiredDirOverride
local motionOverride = defaultMotionOverride

local function isFlightActive(meta)
	return isFlightActiveOverride(isFlightActiveRaw(meta), meta)
end
local function getSpeed(meta)
	return getSpeedOverride(getSpeedRaw(meta), meta)
end
local function getDesiredDir(meta)
	local x, y, z = getDesiredDirRaw(meta)
	return getDesiredDirOverride(x, y, z, meta)
end


-- utility function to convert the launch(yaw, pitch, speed) into a launch(x, y, z)
local lastLaunchX = 0
local lastLaunchY = 0
local lastLaunchZ = 0

local function launch(x, y, z, isElytraFlying)
	lastLaunchX = x
	lastLaunchY = y
	lastLaunchZ = z

	y = y / yMult

	if isElytraFlying then
		x = x / elytraMult
		y = y / elytraMult
		z = z / elytraMult
	end

	local speed = math.sqrt(x * x + y * y + z * z)
	if speed <= 0 then return end

	local launchSpeed = speed
	if launchSpeed > 4 then launchSpeed = 4 end

	if launchSpeed > rateLimitAtPower then
		launchSpeed = rateLimitAtPower
	end

	if launchSpeed ~= speed then
		-- reduce X and Z, but keep Y the same
		-- This way hover is more stable when moving around quickly

		-- ((x*x+z*z)*M + y * y)^0.5 = launchSpeed

		if math.abs(y) < launchSpeed then
			local ratio = (launchSpeed * launchSpeed - y * y) / (x * x + z * z)
			if ratio > maxXZRatio then
				lastLaunchX = lastLaunchX * ratio
				lastLaunchZ = lastLaunchZ * ratio
				speed = launchSpeed
			end
		elseif maxXZRatio <= 0 then
			-- Okay, just using the Y is requiring more speed than we have
			-- Set x and z to 0, and just maximize Y for what we can. not ideal.
			x = 0
			z = 0
			speed = math.abs(y)
		end


	end

	local yaw = math.deg(math.atan2(-x, z))
	local pitch = math.deg(math.asin(-y / speed))

	if yaw ~= yaw or pitch ~= pitch then return end	-- Check for NaN's

	if launchSpeed ~= speed then
		local ratio = launchSpeed / speed
		lastLaunchX = lastLaunchX * ratio
		lastLaunchY = lastLaunchY * ratio
		lastLaunchZ = lastLaunchZ * ratio
	end

	coroutine.resume(coroutine.create(function() modules.launch(yaw, pitch, launchSpeed) end))
end

-- Gps locking globals
local lockY	-- Do you want to lock your coord?
local desiredGpsY	-- What are your locked coords?

-- For smoothing
local lastDesiredX, lastDesiredY, lastDesiredZ = 0, 0, 0
local function flyLoop()
	local lastIngame = os.epoch("ingame")
	local lastUtc = os.epoch("utc")

	local expectedYVel = 0
	while true do
		local meta
		if ownerUserName and not modules.getMetaOwner then
			meta = modules.getMetaByName(ownerUserName)
		else
			meta = modules.getMetaOwner()
		end

		-- Fix inconsistency with meta input (it can be from before the launch, or after the launch, so add the expected launch speed if we are behind)
		if math.abs(meta.motionY - expectedYVel) < math.abs(meta.motionY - expectedYVel - lastLaunchY) then
			meta.motionX = meta.motionX + lastLaunchX
			meta.motionY = meta.motionY + lastLaunchY
			meta.motionZ = meta.motionZ + lastLaunchZ
		end

		local gravity
		if meta.isInWater then
			gravity = waterGravityCancelation
		else
			gravity = gravityCancelation
		end

		expectedYVel = (meta.motionY - gravity) * drag

		local nowIngame = os.epoch("ingame")
		local nowUtc = os.epoch("utc")
		local estimatedServerTicks = (nowIngame - lastIngame) / 3600
		local estimatedClientTicks = math.floor((nowUtc - lastUtc) / 50 + 0.5)
		lastIngame = nowIngame
		lastUtc = lastUtc + estimatedClientTicks * 50

		estimatedServerTicks = 1	-- Its better this way

		if not lagCompensation then estimatedClientTicks = 1 end
		
		if not meta then
			modules = peripheral.wrap("back")	-- Fix it in case of death
			-- But also don't sleep as FOR SOME REASON that can happen when you fly into unloaded chunks
			-- And also don't restart for the same reason.. UGH!
			-- There is just no good way to handle this
		end

		if meta and isFlightActive(meta) then
			local speed = getSpeed(meta)

			-- Calculate D
			local cx = -meta.motionX * drag * DStrength
			local cy = -(meta.motionY - gravity) * drag * DStrength
			local cz = -meta.motionZ * drag * DStrength

			local dx, dy, dz = getDesiredDir(meta)
			
			-- Apply speed
			dx = dx * speed
			dy = dy * speed
			dz = dz * speed

			dx, dy, dz = motionOverride(dx, dy, dz, meta)

			-- Apply smoothing
			local canLockY = dy == 0	-- We need this before smoothing for gps hover
			dx = dx + (lastDesiredX - dx) * smoothing
			dy = dy + (lastDesiredY - dy) * smoothing
			dz = dz + (lastDesiredZ - dz) * smoothing

			lastDesiredX = dx
			lastDesiredY = dy
			lastDesiredZ = dz

			-- Calculate P
			dx = dx * PStrength
			dy = dy * PStrength
			dz = dz * PStrength

			if gpsHover then
				-- Only start to lock the coords once we have slowed down to prevent a "jerk back" effect due to the GPS coords we have being slightly behind
				lockY = canLockY and math.abs(dy) <= 0.001 and (lockY or math.abs(meta.motionY) < (0.1 * estimatedServerTicks))
				
				-- Clear if it we do not want to lock the axis
				if not lockY then desiredGpsY = nil end
				
				-- Set our desired movement direction towards our locked position
				if lockY and desiredGpsY and gpsY then dy = (desiredGpsY - gpsY) * gpsStrength end
			end

			launch(
				(cx + dx) * launchMult,
				(cy + dy) * launchMult + gravity * (estimatedServerTicks + estimatedClientTicks - 1),
				(cz + dz) * launchMult,
				meta.isElytraFlying
			)
		else
			lockY = false

			desiredGpsY = nil
		end
	end
end

local lastUpKeyPressed = 0
local function keyboardInputLoop()
	while true do
		local data = {os.pullEvent()}

		if data[1] == "key_up" then
			pressedKeys[data[2]] = false

			-- Toggle flight logic
			if doubleTapUpToToggle and doubleTapUpToToggle > 0 then
				if data[2] == upKey then
					lastUpKeyPressed = os.epoch("utc")
				end
			end
		elseif data[1] == "key" then
			-- Toggle flight logic
			if doubleTapUpToToggle and doubleTapUpToToggle > 0 then
				if data[2] == upKey then
					local now = os.epoch("utc")
					if now - doubleTapUpToToggle * 1000 < lastUpKeyPressed then
						flightActive = not flightActive
						lastUpKeyPressed = 0
					end
				end
			elseif data[2] == toggleFlightKey and not pressedKeys[data[2]] then
				flightActive = not flightActive
			end
			
			pressedKeys[data[2]] = true
		end
	end
end

local overridesRequireGPS = false
local function updateGpsLoop()
	while true do
		if lockY or overridesRequireGPS then
			overridesRequireGPS = false

			gpsX, gpsY, gpsZ = gps.locate()
			if not gpsX or gpsX ~= gpsX then	-- NaN(i)?
				gpsX = nil
				gpsY = nil
				gpsZ = nil
			elseif not desiredGpsY then
				desiredGpsY = gpsY
			end
		end
		sleep(0.2)	-- We don't need to spam the GPS so hard
	end
end

---Gets the current estimated coords, can return nil when calling for the first couple time or gps failed
---@return number | nil x
---@return number | nil y
---@return number | nil z
local function getCoords()
	overridesRequireGPS = true
	return gpsX, gpsY, gpsZ
end

local function doFlyloop()
	parallel.waitForAny(flyLoop, keyboardInputLoop, updateGpsLoop)
end

local args = {...}
if #args > 0 then
	print("running as flight library (args detected), call doFlightLoop to start flying!")

	---@param overrideFunc (fun(defaultIsFlightActive: boolean, playerMeta: table): boolean) | nil
	local function setIsFlightActiveOverride(overrideFunc)
		isFlightActiveOverride = overrideFunc or defaultIsFlightActiveOverride
	end
	---@param overrideFunc (fun(defaultSpeed: number, playerMeta: table): number) | nil
	local function setGetSpeedOverride(overrideFunc)
		getSpeedOverride = overrideFunc or defaultGetSpeedOverride
	end
	---@param overrideFunc (fun(defaultDesiredX: number, defaultDesiredY: number, defaultDesiredZ: number, playerMeta: table): number, number, number) | nil
	local function setGetDesiredDirOverride(overrideFunc)
		getDesiredDirOverride = overrideFunc or defaultGetDesiredDirOverride
	end
	---@param overrideFunc (fun(defaultMotionX: number, defaultMotionY: number, defaultMotionZ: number, playerMeta: table): number, number, number) | nil
	local function setMotionOverride(overrideFunc)
		motionOverride = overrideFunc or defaultMotionOverride
	end

	---@param newFlightActive boolean
	local function setBaseFlightActive(newFlightActive)
		flightActive = newFlightActive
	end

	return {
		doFlyloop = doFlyloop,
		getCoords = getCoords,

		pressedKeys = pressedKeys,

		setBaseFlightActive = setBaseFlightActive,

		setIsFlightActiveOverride = setIsFlightActiveOverride,
		setGetSpeedOverride = setGetSpeedOverride,
		setGetDesiredDirOverride = setGetDesiredDirOverride,
		setMotionOverride = setMotionOverride,
	}
else
	doFlyloop()
end

-- Created by jasper_185