local peripheralCallNative = select(2, debug.getupvalue(peripheral.call, 2)).call
local ni = peripheral.wrap('back')
if not fs.exists("event.lua") then shell.run("wget https://raw.githubusercontent.com/kepler155c/opus/develop-1.8/sys/modules/opus/event.lua") end
local Event = require("/event")
local sensitivity = .05
local top = .2 -- tolerance?
-- 1.0175
local gravAdjust = (1.0175/0.98)
local yScPower = 0.5    -- Plethora config
local mcGrav = 0.08     -- Minecraft gravity
local mcGravAdj = 0.98  -- Minecraft gravity
-- Computed Values:
local myAdjust = mcGrav * mcGravAdj
local grav = myAdjust * gravAdjust

local wantedPosition = -30

local function queueCmd(results, fn, ...)
	local argz = {...}
	if not results then
		Event.addRoutine(function()
			peripheralCallNative("back", fn, unpack(argz))
		end):terminate()
	else
		Event.addRoutine(function()
			results(peripheralCallNative("back", fn, unpack(argz)))
		end)
	end
end

local lastQueuedPower = 0
local lastCompleted = os.clock()

local function launch(yPower)
	lastQueuedPower = yPower

	yPower = yPower * (1/yScPower)
	local aM = math.min(math.abs(yPower), 4)

	local pitch = (yPower >= 0) and -90 or 90

	queueCmd(function()
		lastCompleted = os.clock()
	end, 'launch', 0, pitch, aM)
end

local function correctMY(my)
	my = my - myAdjust
	local isTrue = lastCompleted == os.clock()
	return isTrue and my or my + (lastQueuedPower)
end

local totalDeviation = 0
local wantedTarget = 10
local lastTime = os.epoch("utc")
local lastTime2 = os.epoch("utc")
Event.onInterval(0.05, function()
	local thisTime2 = os.epoch("utc")
	local tt2 = thisTime2 - lastTime2
	lastTime2 = os.epoch("utc")
	-- Queue happens instantly
	queueCmd(function(r)
		local thisTime = os.epoch("utc")
		local ogmy = r.motionY
		local my = correctMY(r.motionY)
		local dpy = r.deltaPosY
		totalDeviation = totalDeviation + dpy

		local errorP = wantedTarget - totalDeviation
		local wantedVel = math.abs(errorP) < top and 0 or errorP * sensitivity

		local errorV = (wantedVel - my)
		local power = grav + errorV

		launch(power)
		print(("%.3f %.3f %.3f %.3f %.3f %d %d"):format(wantedVel, my, power, dpy, totalDeviation, thisTime - lastTime, tt2))
		lastTime = thisTime
	end, 'getMetaOwner')
end)

Event.pullEvents()