local sides = peripheral.getNames()
local shipcores = {}
for _, side in pairs(sides) do
  if peripheral.getType(side) == "warpdriveShipController" then
    print("Wrapping " .. side)
    table.insert(shipcores, peripheral.wrap(side))

  end
end
local navdatafile = "navdata.txt"
local navdata={} -- format: {{dx,dy,dz},{dx,dy,dz},...} -- these are ship coords (forward, up, right)
for _, shipcore in pairs(shipcores) do
local initialx,initialy,initialz = shipcore.getLocalPosition()
--if not (shipcore.isInHyperspace() or shipcore.isInSpace()) then
 --   error("Ship needs to be in space or hyperspace!")
  --end
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
	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()

    function dst(x1,y1,z1,x2,y2,z2)
        local dx = x2-x1
        local dy = y2-y1
        local 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

    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
	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.")
    table.remove(navdata,1)

    if #navdata > 0 then
        local text = textutils.serialize(navdata)
        local h = fs.open(navdatafile, "w")
        h.write(text)
        h.close()
    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
    error("The ship did not jump.")
end