-- /startup  (central mapper autostart, no blit, in-situ commands, nil-safe)
-- CC:Tweaked + Advanced Peripherals

-- ======= CONFIG =======
local PLAYER_NAME   = "Robathome"
local MONITOR_NAME  = nil            -- nil = auto-find joined advanced monitor wall
local DETECTOR_NAME = nil            -- nil = auto-find player detector
local SAMPLE_SEC    = 0.2
local VOXEL         = 1
local RADIUS        = 2
local MAX_POINTS    = 120000
local BG_COL        = colors.black
local FG_COL        = colors.white
local SCALE         = 1.5            -- 0.5 .. 5.0
-- ======================

-- ===== Utilities =====
local function N(v,def) v = tonumber(v); if v==nil then return def end; return v end
local function D(v) return math.floor(N(v,0)+0.5) end
local function clamp(v,a,b) if v<a then return a elseif v>b then return b else return v end end

-- Open any modems
rednet.close()
for _,side in ipairs(rs.getSides()) do
  if peripheral.getType(side)=="modem" then rednet.open(side) end
end

-- Peripherals
local mon = MONITOR_NAME and peripheral.wrap(MONITOR_NAME) or peripheral.find("monitor")
assert(mon and peripheral.getType(mon)=="monitor","Monitor not found")
mon.setTextScale(SCALE)
mon.setBackgroundColor(BG_COL)
mon.setTextColor(FG_COL)
mon.clear()

local pd = DETECTOR_NAME and peripheral.wrap(DETECTOR_NAME) or peripheral.find("playerDetector")
assert(pd,"Advanced Peripherals Player Detector not found")

-- GPS fallback
local function gpsLocate() local x,y,z=gps.locate(2,false); return x,y,z end

-- Screen
local W,H = mon.getSize()
local function recalcLayout()
  W,H = mon.getSize()
end
recalcLayout()
local ROW_STATUS = H           -- bottom
local ROW_CMD    = H-1         -- second-last
local RENDER_MAX_Y = H-2       -- leave bottom two rows free

-- Camera
local cam = { yaw=math.rad(45), pitch=math.rad(25), dist=28, cx=0, cy=0, cz=0, panx=0, pany=0, panz=0, mouseRotate=false }

-- Tracking toggle
local tracking_enabled = true

-- ===== Status line (bottom row reserved) =====
local status_phase, status_data, status_dirty = "INIT", {}, true
local function setStatus(phase, data) status_phase=phase or status_phase; status_data=data or status_data; status_dirty=true end
local function fmtStatus()
  local parts={("phase:%s"):format(status_phase), ("track:%s"):format(tracking_enabled and "on" or "off")}
  for k,v in pairs(status_data) do parts[#parts+1]=("%s=%s"):format(k,tostring(v)) end
  local s=table.concat(parts,"  ")
  if #s>N(W,0) then s=s:sub(1,N(W,0)) end
  return s
end
local function drawStatus()
  if not status_dirty then return end
  mon.setBackgroundColor(colors.gray); mon.setTextColor(colors.black)
  mon.setCursorPos(1,N(ROW_STATUS,1)); mon.write(("%-"..N(W,1).."s"):format(fmtStatus()))
  mon.setBackgroundColor(BG_COL); mon.setTextColor(FG_COL)
  status_dirty=false
end

-- ===== Command line (H-1) =====
local cmd_text, cmd_dirty = "", true
local function drawCmd()
  if not cmd_dirty then return end
  mon.setBackgroundColor(colors.blue); mon.setTextColor(colors.white)
  mon.setCursorPos(1,N(ROW_CMD,1)); mon.write(("%-"..N(W,1).."s"):format((":%s"):format(cmd_text)))
  mon.setBackgroundColor(BG_COL); mon.setTextColor(FG_COL)
  cmd_dirty=false
end
local function clearCmd()
  cmd_text=""; cmd_dirty=true; drawCmd()
end

-- ===== Voxel store =====
local vox, order = {}, {}
local function key(ix,iy,iz) return ix..":"..iy..":"..iz end
local function setVoxel(ix,iy,iz)
  ix,iy,iz = D(ix),D(iy),D(iz)
  local k=key(ix,iy,iz)
  if not vox[k] then
    vox[k]=true; order[#order+1]=k
    if #order>MAX_POINTS then local old=table.remove(order,1); vox[old]=nil end
  end
end
local function markFreeSphere(px,py,pz,rad)
  px,py,pz,rad = D(px),D(py),D(pz),N(rad,0)
  for dx=-rad,rad do
    for dy=-rad,rad do
      for dz=-rad,rad do
        if dx*dx+dy*dy+dz*dz<=rad*rad then setVoxel(px+dx,py+dy,pz+dz) end
      end
    end
  end
end

-- Highlights (drawn last)
local highlights = {}  -- { {x=..,y=..,z=..,r=..,color=..}, ... }
local function addHighlight(x,y,z,r,c)
  highlights[#highlights+1]={x=N(x,0),y=N(y,0),z=N(z,0),r=N(r,0),color=c or colors.red}
  setStatus("CMD",{added="highlight"})
end
local function clearHighlights()
  highlights = {}
  setStatus("CMD",{highlights="cleared"})
end

-- ===== Math / projection =====
local function rotatePoint(x,y,z,yaw,pitch)
  x,y,z = N(x,0),N(y,0),N(z,0)
  local cy,sy=math.cos(N(yaw,0)),math.sin(N(yaw,0))
  local cp,sp=math.cos(N(pitch,0)),math.sin(N(pitch,0))
  local xr =  cy*x + sy*z
  local zr = -sy*x + cy*z
  local yr =  cp*y - sp*zr
  zr =  sp*y + cp*zr
  return xr,yr,zr
end

local function project(ix,iy,iz)
  local x=(N(ix,0)*VOXEL - N(cam.cx,0))+N(cam.panx,0)
  local y=(N(iy,0)*VOXEL - N(cam.cy,0))+N(cam.pany,0)
  local z=(N(iz,0)*VOXEL - N(cam.cz,0))+N(cam.panz,0)
  local xr,yr,zr=rotatePoint(x,y,z,cam.yaw,cam.pitch)
  local f,d=30,N(cam.dist,30)
  local denom=zr+d; if denom<=0.2 then return nil end
  local sx=D(W/2 + (xr*f)/denom)
  local sy=D(H/2 - (yr*f)/denom)
  return sx,sy,denom
end

local function clearMon()
  mon.setBackgroundColor(BG_COL); mon.setTextColor(FG_COL); mon.clear()
  drawCmd(); drawStatus()
end

-- Pixel fill without blit
local function plot(x,y,c)
  x,y = D(x),D(y)
  if x>=1 and x<=N(W,1) and y>=1 and y<=N(RENDER_MAX_Y,1) then
    mon.setCursorPos(x,y)
    mon.setBackgroundColor(c or FG_COL)
    mon.write(" ")
    mon.setBackgroundColor(BG_COL)
  end
end

local function colorByDepth(den)
  den=N(den,0)
  if den < 12 then return colors.white end
  if den < 24 then return colors.lightGray end
  if den < 48 then return colors.gray end
  return colors.darkGray
end

local function drawHUD(points,fps)
  points = N(points,0); fps = N(fps,0)
  local yaw   = D(math.deg(N(cam.yaw,0)))
  local pitch = D(math.deg(N(cam.pitch,0)))
  local dist  = D(N(cam.dist,0))
  local cx,cy,cz = D(cam.cx), D(cam.cy), D(cam.cz)
  mon.setCursorPos(1,1)
  mon.setTextColor(colors.yellow)
  mon.setBackgroundColor(BG_COL)
  local hud = ("pts:%d  fps:%.1f  yaw:%d  pitch:%d  zoom:%d  cx:%d cy:%d cz:%d")
              :format(points,fps,yaw,pitch,dist,cx,cy,cz)
  if #hud>N(W,0) then hud=hud:sub(1,N(W,0)) end
  mon.write(("%-"..N(W,1).."s"):format(hud))
end

-- render: guard bad keys and nil projects
local function render(fps)
  clearMon()
  setStatus("RENDER",{voxels=#order})
  local count=0
  local bucketNear,bucketFar={},{}

  for k,_ in pairs(vox) do
    local ix,iy,iz = k:match("(-?%d+):(-?%d+):(-?%d+)")
    if ix and iy and iz then
      ix,iy,iz = tonumber(ix),tonumber(iy),tonumber(iz)
      local sx,sy,den = project(ix,iy,iz)
      if sx and sy and den then
        if den<25 then bucketNear[#bucketNear+1]={sx,sy,den}
        else bucketFar[#bucketFar+1]={sx,sy,den} end
      end
    else
      vox[k] = nil
    end
  end

  local function drawB(b)
    for i=1,#b do
      local sx,sy,den = b[i][1],b[i][2],b[i][3]
      if sx and sy then plot(sx,sy,colorByDepth(den or 0)); count=count+1 end
    end
  end
  drawB(bucketFar); drawB(bucketNear)

  -- Draw highlights last
  for _,h in ipairs(highlights) do
    local r=N(h.r,0)
    for dx=-r,r do for dy=-r,r do for dz=-r,r do
      if dx*dx+dy*dy+dz*dz <= r*r then
        local sx,sy,den = project(h.x+dx,h.y+dy,h.z+dz)
        if sx then plot(sx,sy,h.color or colors.red) end
      end
    end end end
  end

  drawHUD(count, fps or 0)
  drawCmd(); drawStatus()
end

-- ===== Command parsing =====
local function split(s) local t={} for w in tostring(s):gmatch("%S+") do t[#t+1]=w end; return t end
local function toNum(s) return tonumber(s) end

local function execCommand(line)
  local t=split(line)
  if #t==0 then return end
  local cmd=t[1]:lower()

  if cmd=="help" or cmd=="?" then
    setStatus("CMD",{help="track on|off; highlight x y z [r]; clear highlights; cam yaw|pitch|dist N; cam set yaw pitch dist; center x y z"})
    return

  elseif cmd=="track" and t[2] then
    local v=t[2]:lower()
    if v=="on" then tracking_enabled=true elseif v=="off" then tracking_enabled=false else return setStatus("CMD",{error="track on/off"}) end
    setStatus("CMD",{track=tracking_enabled and "on" or "off"}); return

  elseif cmd=="highlight" and t[2] and t[3] and t[4] then
    local x,y,z=toNum(t[2]),toNum(t[3]),toNum(t[4]); if not(x and y and z) then return setStatus("CMD",{error="coords"}) end
    local r=toNum(t[5]) or 0
    addHighlight(x,y,z,r, colors.red)
    return

  elseif cmd=="clear" and t[2] and t[2]:lower()=="highlights" then
    clearHighlights(); return

  elseif cmd=="cam" and t[2] then
    local sub=t[2]:lower()
    if sub=="yaw" and t[3] then cam.yaw=math.rad(clamp(toNum(t[3]) or 0,-180,180)); setStatus("CMD",{yaw=D(math.deg(cam.yaw))})
    elseif sub=="pitch" and t[3] then cam.pitch=math.rad(clamp(toNum(t[3]) or 0,-80,80)); setStatus("CMD",{pitch=D(math.deg(cam.pitch))})
    elseif sub=="dist" and t[3] then cam.dist=clamp(toNum(t[3]) or cam.dist,8,200); setStatus("CMD",{dist=D(cam.dist)})
    elseif sub=="set" and t[3] and t[4] and t[5] then
      cam.yaw=math.rad(clamp(toNum(t[3]) or 45,-180,180))
      cam.pitch=math.rad(clamp(toNum(t[4]) or 25,-80,80))
      cam.dist=clamp(toNum(t[5]) or 48,8,200)
      setStatus("CMD",{yaw=D(math.deg(cam.yaw)),pitch=D(math.deg(cam.pitch)),dist=D(cam.dist)})
    else
      setStatus("CMD",{error="cam yaw|pitch|dist N  / cam set yaw pitch dist"})
    end
    return

  elseif (cmd=="center" or cmd=="lookat") and t[2] and t[3] and t[4] then
    local x,y,z=toNum(t[2]),toNum(t[3]),toNum(t[4]); if not(x and y and z) then return setStatus("CMD",{error="center x y z"}) end
    cam.cx,cam.cy,cam.cz=x,y,z
    setStatus("CMD",{center=("%d,%d,%d"):format(D(x),D(y),D(z))})
    return
  end

  setStatus("CMD",{error="unknown; try :help"})
end

-- ===== Command-mode prompt (triggered by ':' key) =====
local function commandPrompt()
  cmd_text=""; cmd_dirty=true; drawCmd()
  while true do
    local e={os.pullEvent()}
    if e[1]=="char" then
      local ch=e[2]
      if #cmd_text < math.max(1,N(W,1)-2) then cmd_text=cmd_text..ch; cmd_dirty=true; drawCmd() end
    elseif e[1]=="key" then
      local k=e[2]
      if k==keys.enter then
        local line=cmd_text
        clearCmd()
        execCommand(line)
        return
      elseif k==keys.backspace then
        cmd_text=cmd_text:sub(1,math.max(0,#cmd_text-1)); cmd_dirty=true; drawCmd()
      elseif k==keys.escape then
        clearCmd(); setStatus("CMD",{cancel="ok"}); return
      end
    elseif e[1]=="monitor_resize" then
      recalcLayout(); ROW_STATUS=H; ROW_CMD=H-1; RENDER_MAX_Y=H-2
      drawCmd(); drawStatus()
    end
  end
end

-- ===== Input loop =====
local function inputLoop()
  while true do
    local e={os.pullEvent()}
    if e[1]=="char" then
      if e[2]==":" then setStatus("CMD",{mode="prompt"}); commandPrompt() end
    elseif e[1]=="key" then
      local k=e[2]
      if k==keys.q then return end
      if k==keys.left then cam.yaw=cam.yaw-math.rad(5) end
      if k==keys.right then cam.yaw=cam.yaw+math.rad(5) end
      if k==keys.up then cam.pitch=clamp(cam.pitch-math.rad(5),math.rad(-80),math.rad(80)) end
      if k==keys.down then cam.pitch=clamp(cam.pitch+math.rad(5),math.rad(-80),math.rad(80)) end
      if k==keys.leftBracket then cam.dist=clamp(cam.dist-2,8,200) end
      if k==keys.rightBracket then cam.dist=clamp(cam.dist+2,8,200) end
      if k==keys.w then cam.pany=cam.pany+1 end
      if k==keys.s then cam.pany=cam.pany-1 end
      if k==keys.a then cam.panx=cam.panx-1 end
      if k==keys.d then cam.panx=cam.panx+1 end
      if k==keys.r then cam.panz=cam.panz+1 end
      if k==keys.f then cam.panz=cam.panz-1 end
      if k==keys.m then cam.mouseRotate=not cam.mouseRotate end
      setStatus("INPUT",{key=k})
    elseif e[1]=="mouse_drag" and cam.mouseRotate then
      local dx,dy=e[3],e[4]
      cam.yaw=cam.yaw+dx*0.01
      cam.pitch=clamp(cam.pitch+dy*0.01,math.rad(-80),math.rad(80))
      setStatus("INPUT",{drag=("%d,%d"):format(D(dx),D(dy))})
    elseif e[1]=="monitor_touch" then
      local _side,x,y=e[2],e[3],e[4]
      if y==ROW_STATUS then
        cam.mouseRotate=not cam.mouseRotate
        setStatus("TOUCH",{toggle_mouseRotate=cam.mouseRotate})
      else
        local third = math.floor(N(W,1)/3)
        if x<=third then
          cam.yaw=cam.yaw-math.rad(10); setStatus("TOUCH",{action="yaw -10"})
        elseif x>2*third then
          cam.yaw=cam.yaw+math.rad(10); setStatus("TOUCH",{action="yaw +10"})
        else
          if y < math.floor(N(RENDER_MAX_Y,1)/2) then
            cam.pitch=clamp(cam.pitch-math.rad(10),math.rad(-80),math.rad(80)); setStatus("TOUCH",{action="pitch -10"})
          else
            cam.pitch=clamp(cam.pitch+math.rad(10),math.rad(-80),math.rad(80)); setStatus("TOUCH",{action="pitch +10"})
          end
        end
      end
    elseif e[1]=="monitor_resize" then
      recalcLayout(); ROW_STATUS=H; ROW_CMD=H-1; RENDER_MAX_Y=H-2
      mon.setTextScale(SCALE)
      setStatus("RESIZE",{w=W,h=H,scale=SCALE})
      clearMon()
    end
  end
end

-- ===== Sampling =====
local function roundDiv(v,step) return math.floor((N(v,0) + (v and v>=0 and step/2 or -step/2))/step) end
local function sampleLoop()
  local last=os.clock()
  while true do
    local x,y,z
    if tracking_enabled then
      setStatus("SAMPLE",{src="detector"})
      local pos=pd.getPlayerPos and pd.getPlayerPos(PLAYER_NAME)
      if type(pos)=="table" and pos.x then
        x,y,z=pos.x,pos.y,pos.z
      else
        setStatus("SAMPLE",{src="gps"})
        x,y,z=gpsLocate()
      end
    else
      setStatus("IDLE",{track="off"})
    end

    if x and y and z then
      setStatus("UPDATE",{x=D(x),y=D(y),z=D(z)})
      cam.cx = cam.cx + (x - cam.cx)*0.3
      cam.cy = cam.cy + (y - cam.cy)*0.3
      cam.cz = cam.cz + (z - cam.cz)*0.3
      local ix,iy,iz=roundDiv(x,VOXEL),roundDiv(y,VOXEL),roundDiv(z,VOXEL)
      markFreeSphere(ix,iy,iz,RADIUS)
    end

    local now=os.clock(); local dt=now-last; last=now
    render(1/math.max(dt,1e-3))
    sleep(SAMPLE_SEC)
  end
end

-- ===== Startup =====
term.redirect(mon)
mon.setBackgroundColor(BG_COL); mon.setTextColor(FG_COL); mon.clear()
mon.setCursorPos(1,2); mon.setTextColor(FG_COL); mon.write("Mapper autostart. Q quits.")
mon.setCursorPos(1,3); mon.write("Arrows rotate  [ ] zoom  WASD pan  R/F elevate  M mouse-rot")
mon.setCursorPos(1,4); mon.write("Type ':' for commands. ':help' for syntax.")
setStatus("INIT",{scale=SCALE}); drawCmd(); drawStatus()

parallel.waitForAny(sampleLoop, inputLoop)
term.redirect(term.native())