sleep(15)
local name = "<@>" -- Discord ID
local uri = "https://discord" -- Discord Uri
local lever = "front" -- format: front,back,left,right,top,bottom ONLY
local failping = 1 -- format: Will ping on Failed node execution 0 = Dont Ping, 1 = Ping
local navdatafile = ".navdata"
local navbackup = ".navbackup"
local navdata={} -- format: {{dx,dy,dz},{dx,dy,dz},...} -- these are ship coords (forward, up, right)
local sides = peripheral.getNames()
local shipcores = {}
while true do
    on = redstone.getAnalogInput(lever)

    if on > 6 then
        print("AutoPilot: Disabled")
        sleep(3)
    elseif on < 5 then
        for _, side in pairs(sides) do
            if peripheral.getType(side) == "warpdriveShipController" then
                print("Wrapping " .. side)
                table.insert(shipcores, peripheral.wrap(side))

            end
        end
        for _, shipcore in pairs(shipcores) do
            local initialx,initialy,initialz = shipcore.getLocalPosition()
            print("We are currently in "..(shipcore.isInHyperspace() and "HYPERSPACE"or"SPACE").." at "..initialx.." "..initialy.." "..initialz)
        end
        if fs.exists(navdatafile) then
            local h = fs.open(navdatafile, "r")
            local text = h.readAll()
            navdata = textutils.unserialize(text)
            h.close()
            print("Waiting 5 seconds before starting node execution...")
            sleep(5)
        end

        if #navdata <= 0 then -- if navdata wasnt loaded
            print("Enter target pos: x y z")
            local input = read()
            local txs,tys,tzs=input:gmatch("(%-?%d+) (%-?%d+) (%-?%d+)")()
            print("'"..txs.."'")
            print("'"..tys.."'")
            print("'"..tzs.."'")
            local tx = tonumber(txs)
            local ty = tonumber(tys)
            local tz = tonumber(tzs)
            if tx == nil or ty == nil or tz == "" then
                error("Please use the proper number format.")
            end
            for _, shipcore in pairs(shipcores) do
                initialx,initialy,initialz = shipcore.getLocalPosition()
                local ship_len_back,ship_len_left,ship_len_down=shipcore.dim_negative()
                local ship_len_front,ship_len_right,ship_len_up=shipcore.dim_positive()

                function dst(x1,y1,z1,x2,y2,z2)
                    dx = x2-x1
                    dy = y2-y1
                    dz = z2-z1
                    return math.sqrt(dx*dx+dy*dy+dz*dz)
                end --dst

                if ty-ship_len_down <= 0 or ty+ship_len_up >= 256 then
                    error("Target y position needs to be inside "..ship_len_down.." and "..256-ship_len_up..".")
                end
            end
            print("Do you want to stay in hyperspace after jumping? (Y/n)")
            local input2 = read()
            local stayInHyper = input2~="n" and input2~="N"
            print("----------------------------------")
            print("Is this information correct?")
            print("Target coords: ", tx,ty,tz)
            print("Target dimension: "..(stayInHyper and "HYPERSPACE" or "SPACE"))
            print("Total distance: "..math.floor(dst(initialx,initialy,initialz,tx,ty,tz)).." blocks")
            print("----------------------------------")
            for i=1,0,-1 do
                term.setCursorPos(1,select(2,term.getCursorPos()))
                term.write("Please double check the info..."..i.."   ")
                sleep(1)
            end
            term.clearLine()
            print()
            print("Enter 'Y' or press ENTER to engage navigation.")
            local input3 = read()
            if input3 ~= "Y" and input3 ~="y" and input3 ~="" then
                print("Aborting nagivation.")
                return
            end
            print("Computing navigation steps...")
            for _, shipcore in pairs(shipcores) do
                local initialx,initialy,initialz = shipcore.getLocalPosition()
                local ship_len_back,ship_len_left,ship_len_down=shipcore.dim_negative()
                local ship_len_front,ship_len_right,ship_len_up=shipcore.dim_positive()
                local shipdeltafront, shipdeltaright
                local shipdeltaup = ty-initialy
                local orix,_,oriz = shipcore.getOrientation()

                if orix == 1 then
                    shipdeltafront = tx-initialx
                    shipdeltaright = tz-initialz
                elseif orix == -1 then
                    shipdeltafront = -tx+initialx
                    shipdeltaright = -tz+initialz
                elseif oriz == 1 then
                    shipdeltafront = tz-initialz
                    shipdeltaright = -tx+initialx
                elseif oriz == -1 then
                    shipdeltafront = -tz+initialz
                    shipdeltaright = tx-initialx
                else
                    error("Unexpected ship orientation!")
                end

                print("We need to move "..shipdeltafront.." block(s) forward, "..shipdeltaup.." block(s) upward and "..shipdeltaright.." block(s) to the right.")
                local minforward = ship_len_front+ship_len_back
                local minup = ship_len_up+ship_len_down
                local minright = ship_len_right+ship_len_left
                for _, shipcore in pairs(shipcores) do
                    shipcore.command("MANUAL",false)
                    local success, maxdist = shipcore.getMaxJumpDistance()
                    if not success then error("unable to get the ships max jump distance: "..maxdist) end
                    if maxdist <= 0 then error("max jump distance is zero") end
                    print("Max jump length = "..maxdist)
                    function computeMove(mindist,remaining,ignoreconstraint) -- returns a move to make along that axis
                        if remaining == 0 then return 0 end

                        local remsign = (remaining < 0) and -1 or 1

                        if math.abs(remaining) < mindist and not ignoreconstraint then -- if the move would be impossible because it is too short, move further away
                            return -remsign * mindist
                        end
                        return remsign * math.min(math.abs(remaining),maxdist)
                    end
                end
                repeat
                    local move = {}
                    move[2] = computeMove(minup+1,shipdeltaup,true) --y
                    shipdeltaup = shipdeltaup - move[2]

                    move[1] = computeMove(minforward+1,shipdeltafront,math.abs(move[2]) > minup) --x

                    if not (math.abs(move[2]) > minup) and  shipdeltafront == 0 and shipdeltaright == 0 and shipdeltaup ~= 0 and move[1] == 0 then
                        move[1] = minforward+1
                    end

                    shipdeltafront = shipdeltafront - move[1]
                    move[3] = computeMove(minright+1,shipdeltaright, math.abs(move[2]) > minup or math.abs(move[1]) > minforward) --z
                    shipdeltaright = shipdeltaright - move[3]
                    navdata[#navdata+1] = move
                    print("Computed move: #"..#navdata..": "..move[1].." block(s) forward, "..move[2].." block(s) upward and "..move[3].." block(s) to the right.")
                    print("Remaining: "..shipdeltafront..":"..shipdeltaup..":"..shipdeltaright)

                until shipdeltafront == 0 and shipdeltaup == 0 and shipdeltaright == 0
                print("Navigational path plotted using "..#navdata.." jump(s).")
            end
        end
        for i=1,#navdata do
            local move = navdata[i]

            print("Executing next node... There are "..#navdata.." node(s) left to execute.")
            http.post(uri,"{\"content\":\"```yaml\\nAutopilot: Executing next node There are "..#navdata.." node(s) left to execute.```\"}",{['content-type']="application/json"})
            table.remove(navdata,1)
            if fs.exists(navbackup) then
                fs.delete(navbackup)
            end
            if #navdata > 0 then
                local text = textutils.serialize(navdata)
                local h = fs.open(navdatafile, "w")
                h.write(text)
                h.close()
                fs.copy(navdatafile, navbackup)
            else
                fs.delete(navdatafile)
            end
            for _, shipcore in pairs(shipcores) do
                shipcore.command("MANUAL", false)
                shipcore.movement(move[1],move[2],move[3])
                shipcore.rotationSteps(0)
                --shipcore.command("MANUAL", true)
            end
            for i=60,0,-1 do
                term.setCursorPos(1,select(2,term.getCursorPos()))
                term.write("Waiting for the ship to jump..."..i.."   ")
                sleep(1)
            end
            number = #navdata + 1
            http.post(uri,"{\"content\":\"```yaml\\nAutoPilot: Failed Executing node "..number..".```\"}",{['content-type']="application/json"})
            fs.delete(navdatafile)
            fs.copy(navbackup, navdatafile)
            print("The ship did not jump.")
            if failping >= 0 then
                http.post(uri,"{\"content\":\""..name.."\"}",{['content-type']="application/json"})
            end
            sleep(3)
            os.reboot()
        end
    end
end