dofile("timer.lua")

function logbase(val,base)
    return math.log(val)/math.log(base)
end

local chargeSide = "back"
local running = false

local cvts = {peripheral.wrap("AdvancedGears_4"),peripheral.wrap("AdvancedGears_5")}
local extractor = peripheral.wrap("Extractor_1")
local coil = peripheral.wrap("AdvancedGears_2")

timer.add(timer.new("stage",0,nil,false))
timer.add(timer.new("charge",0,nil,false))
timer.add(timer.new("wait",5,nil,false))

local activeStage = nil

local stages = {}
stages[1] = {base=900,scale=60,speed=32768,slots={0},cvts={1,8},coil={4096,4096}}
stages[2] = {base=400,scale=20,speed=1048576,slots={1,4},cvts={32,8},coil={256,4096}}
stages[3] = {base=600,scale=30,speed=1048576,slots={2,5},cvts={32,8},coil={256,4096}}
stages[4] = {base=1200,scale=80,speed=32768,slots={3,6},cvts={1,8},coil={2048,4096}}

--CALC TIMES
function calcStageTime()
    local itemTime = math.max(1,stages[activeStage].base-stages[activeStage].scale*logbase(stages[activeStage].speed,2)+0.05)
    local totalTime = (stageItemCount(activeStage) * itemTime)/20 + 0.4

    timer.get("stage"):setCap(totalTime,true)
    timer.get("stage"):start()
end

function calcChargeTime()
    timer.get("charge"):stop()

    local e = coil.getEnergy()
    if(e>=200000000000) then return 0 end
    local de = 200000000000-e

    local t = math.ceil(de / (1024*1024))
    timer.get("charge"):setCap(t,true)
    if(t>0) then
        timer.get("charge"):start()
    end
end

--CHARGING
function setCharging(setting)
    rs.setOutput(chargeSide,setting)
end

--COILS

function setCoilRedstone(setting)
    rs.setOutput("left",setting)
end

--INVENTORIES
function slotItemCount(slot)
    local _,_,am = extractor.getSlot(slot)
    return am or 0
end

function stageItemCount(stage)
    local total = 0
    for i,v in ipairs(stages[stage].slots) do
        total = total + slotItemCount(v)
    end
    return total
end

function slotHasItems(slot)
    return extractor.getSlot(slot) ~= nil
end

function stageHasItems(stage)
    for i,v in ipairs(stages[stage].slots) do
        if(slotHasItems(v)) then return true end
    end
    return false
end

--STAGING
function setStage()
    activeStage = nil
    for i=4,1,-1 do
        if(stageHasItems(i)) then
            activeStage = i
            break
        end
    end
end

function setCVTs()
    for i,v in ipairs(stages[activeStage].cvts) do
        cvts[i].setRatio(v)
    end
end

function setCoil()
    coil.setTorque(stages[activeStage].coil[1])
    coil.setSpeed(stages[activeStage].coil[2])
end

function startProcessing()
    setStage()
    if(activeStage == nil) then return false end
    timer.get("wait"):stop()
    running = true
    setCoil()
    setCVTs()
    calcStageTime()
    setCharging(false)
    setCoilRedstone(true)
    return true
end

function stopProcessing()
    running = false
    print("Stopping Processing")
    setCoilRedstone(false)
    calcChargeTime()
    setCharging(timer.get("charge"):isRunning())
    timer.get("wait"):start()
end

function updateTimers(delta)
    local timers = timer.update(delta)
    term.clear()
    term.setCursorPos(1,1)
    print("Charge Time: "..timer.get("charge"):getLeft())
    print("Stage Time: "..timer.get("stage"):getLeft())

    local sleepTime = 0.05
    if(timer.get("wait"):isRunning())  then sleepTime=5 end

    return timers,sleepTime
end
function exec()
    while true do

        if not startProcessing() then
            if(running) then stopProcessing() end
            calcChargeTime()
            setCharging(true)
            timer.get("wait"):start()
            --sleep(5)
        end
        local pre = os.time()
        while true do

            local delta = (os.time()-pre)*50
            if(os.time()<pre) then delta = (24-pre+os.time())*50 end
            local timers,sleepTime = updateTimers(delta)
            if(timers.stage) then
                print("Switching Stage")
                break
            end
            if(timers.charge) then
                print("Stopping Charging")
                setCharging(false)
            end
            if(timers.wait) then
                --print("ping!")
                if startProcessing() then
                    sleepTime = 0.05
                else
                    timer.get("wait"):start()
                end
            end
            pre = os.time()
            sleep(sleepTime)
        end
    end
end

local res,err = pcall(exec)

for k,v in pairs(rs.getSides()) do
    rs.setOutput(v,false)
end
print(err)