---

-- cannon.lua for CC:Tweaked + CC:CBC
-- v6 — 3D closest-approach tracking (fixes close-range)
--
-- PHYSICS (CBC source):
--   Gravity: 0.05 | Drag: 0.99 multiplicative | Half-step
--
-- CONVENTIONS:
--   Yaw: atan2(dx, -dz) → 0° = -Z, 90° = +X
--   Pitch: 0° = up, 90° = horizontal (sent directly)
--   Mortar: 0.5-44° from vertical

local peripheral = peripheral
local rednet = rednet
local term = term
local io = io
local os = os
local math = math

-- ============ CONFIG ============
local BARREL_LEN = 5
local VELOCITY   = 8
local G          = 0.05
local DRAG       = 0.99
local MAX_TICKS  = 2000
local HIT_RADIUS = 15    -- max 3D distance to count as "hit"

-- ============ FIND MOUNT ============
local mount = nil
for _, name in ipairs(peripheral.getNames()) do
  if peripheral.getType(name) == "cannon_mount" then
    mount = peripheral.wrap(name)
    break
  end
end

if not mount then
  print("ERROR: Cannon Mount not found!")
  for _, name in ipairs(peripheral.getNames()) do
    print("  " .. name .. " = " .. peripheral.getType(name))
  end
  return
end

mount.setComputerControl(true)
print("Cannon Mount connected.")

-- ============ SIMULATE ============
-- Tracks minimum 3D distance to target across ENTIRE flight.
-- No XZ window — just pure closest approach in 3D.
local function simulate(x0, y0, z0, xt, yt, zt, barrelLen, vel, pitchV, dirX, dirZ)
  local pitchRad = math.rad(pitchV)

  local barrelH = barrelLen * math.sin(pitchRad)
  local barrelV = barrelLen * math.cos(pitchRad)
  local px = x0 + barrelH * dirX
  local py = y0 + barrelV
  local pz = z0 + barrelH * dirZ

  local vh = vel * math.sin(pitchRad)
  local vy = vel * math.cos(pitchRad)
  local vx = vh * dirX
  local vz = vh * dirZ

  local minDist = 9999
  local bestT = 0

  for t = 1, MAX_TICKS do
    local ovx, ovy, ovz = vx, vy, vz

    vy = vy - G
    vx = vx * DRAG
    vy = vy * DRAG
    vz = vz * DRAG

    px = px + (ovx + vx) / 2
    py = py + (ovy + vy) / 2
    pz = pz + (ovz + vz) / 2

    -- 3D distance to target
    local dx = px - xt
    local dy = py - yt
    local dz = pz - zt
    local dist = math.sqrt(dx*dx + dy*dy + dz*dz)

    if dist < minDist then
      minDist = dist
      bestT = t
    end

    -- Stop when shell well below target
    if py < yt - 30 then
      return minDist, bestT
    end
  end
  return minDist, bestT
end

-- ============ CALCULATE ============
local function calculate(x0, y0, z0, xt, yt, zt, barrelLen, vel)
  local dx = xt - x0
  local dz = zt - z0

  local apiYaw = math.deg(math.atan2(dx, -dz))
  if apiYaw < 0 then apiYaw = apiYaw + 360 end
  local yawRad = math.rad(apiYaw)
  local dirX = math.sin(yawRad)
  local dirZ = -math.cos(yawRad)

  local bestPitch = 0
  local bestError = 9999
  local bestTime = 0

  -- Phase 1: coarse search
  -- 0.5° steps for 0.5-10° (close range), 1° steps for 10-44°
  for pitch = 1, 88 do
    local pitchVal = pitch * 0.5  -- 0.5° steps
    if pitchVal > 44 then break end
    local err, time = simulate(x0, y0, z0, xt, yt, zt, barrelLen, vel, pitchVal, dirX, dirZ)
    if err < bestError then
      bestError = err
      bestPitch = pitchVal
      bestTime = time
    end
  end

  -- Phase 2: fine search (0.01° steps around best)
  if bestPitch > 0 then
    local lo = (bestPitch - 0.5) * 100
    local hi = (bestPitch + 0.5) * 100
    for p100 = lo, hi do
      local pitch = p100 / 100
      if pitch < 0.5 then pitch = 0.5 end
      if pitch > 44 then pitch = 44 end
      local err, time = simulate(x0, y0, z0, xt, yt, zt, barrelLen, vel, pitch, dirX, dirZ)
      if err < bestError then
        bestError = err
        bestPitch = pitch
        bestTime = time
      end
    end
  end

  return apiYaw, bestPitch, bestError, bestTime
end

-- ============ STATE ============
local lastYaw = nil
local lastPitch = nil

-- ============ MAIN ============
while true do
  term.clear()
  term.setCursorPos(1, 1)
  print("=== CBC Auto-Artillery (v6) ===")
  print("Barrel: " .. BARREL_LEN .. " | Vel: " .. VELOCITY .. " b/t")
  print("Drag: " .. DRAG .. " | Gravity: " .. G)
  print("Pitch: 0=up, 90=horizon | Yaw: 0=-Z, 90=+X")
  print("Search: 0.5-44 deg | 3D closest approach")
  print("")
  print("1 - Fire at target (X Y Z)")
  print("2 - Rednet mode")
  print("f - Fire at last target")
  print("b - Set barrel length")
  print("v - Set velocity")
  print("d - Diagnostics")
  print("t - Test angle (no fire)")
  print("q - Quit")
  io.write("> ")
  local input = io.read()

  if input == "q" then break end

  if input == "b" then
    io.write("Barrel length: ")
    local v = tonumber(io.read())
    if v then BARREL_LEN = v end
    print("Set to " .. BARREL_LEN)
    os.sleep(1)

  elseif input == "v" then
    io.write("Velocity (blocks/tick): ")
    local v = tonumber(io.read())
    if v then VELOCITY = v end
    print("Set to " .. VELOCITY)
    os.sleep(1)

  elseif input == "d" then
    print("\n--- Mount Info ---")
    local info = mount.getInfo()
    if info then
      for k, v in pairs(info) do
        print("  " .. tostring(k) .. " = " .. tostring(v))
      end
    else
      print("  getInfo() = nil")
    end
    print("------------------")
    os.sleep(5)

  elseif input == "t" then
    io.write("Yaw: ")
    local y = tonumber(io.read())
    io.write("Pitch (0=up, 90=horizon): ")
    local p = tonumber(io.read())
    if y and p then
      mount.setTargetAngles(y, p)
      os.sleep(3)
      local info = mount.getInfo()
      print("Current: yaw=" .. tostring(info.yaw) .. " pitch=" .. tostring(info.pitch))
    end
    os.sleep(3)

  elseif input == "f" then
    if lastYaw and lastPitch then
      print("Firing at yaw=" .. lastYaw .. " pitch=" .. lastPitch)
      mount.setTargetAngles(lastYaw, lastPitch)
      os.sleep(3)
      mount.fire(true)
      print("Shot fired!")
    else
      print("No target yet. Use 1 first.")
    end
    os.sleep(3)

  elseif input == "1" then
    io.write("Target X: ") local tx = tonumber(io.read())
    io.write("Target Y: ") local ty = tonumber(io.read())
    io.write("Target Z: ") local tz = tonumber(io.read())

    if tx and ty and tz then
      local info = mount.getInfo()
      local mx, my, mz = info.x, info.y, info.z
      local dist = math.sqrt((tx-mx)^2 + (tz-mz)^2)
      print(string.format("Distance: %.1f blocks (horizontal)", dist))
      print("Calculating...")

      local apiYaw, apiPitch, err, time = calculate(mx, my, mz, tx, ty, tz, BARREL_LEN, VELOCITY)

      if apiPitch > 0 and err < HIT_RADIUS then
        print(string.format("Yaw:   %.2f deg", apiYaw))
        print(string.format("Pitch: %.2f deg (0=up, 90=horizon)", apiPitch))
        print(string.format("3D error: %.2f blocks | Time: %d ticks (%.1fs)", err, time, time / 20))

        lastYaw = apiYaw
        lastPitch = apiPitch

        mount.setTargetAngles(apiYaw, apiPitch)
        print("Waiting for rotation...")
        os.sleep(3)
        mount.fire(true)
        print("Shot fired!")
      else
        print("FAILED: Target out of range.")
        print(string.format("Best 3D error: %.1f blocks", err))
        print("Try: decrease velocity (v) for close targets.")
      end
    else
      print("Invalid coordinates.")
    end
    os.sleep(3)

  elseif input == "2" then
    print("Rednet mode. Listening...")
    rednet.open("back")
    while true do
      local event, id, msg = os.pullEvent("rednet_message")
      if type(msg) == "table" and msg.x and msg.y and msg.z then
        print("Target: " .. msg.x .. ", " .. msg.y .. ", " .. msg.z)
        local info = mount.getInfo()
        local mx, my, mz = info.x, info.y, info.z
        local apiYaw, apiPitch, err, time = calculate(mx, my, mz, msg.x, msg.y, msg.z, BARREL_LEN, VELOCITY)

        if apiPitch > 0 and err < HIT_RADIUS then
          lastYaw = apiYaw
          lastPitch = apiPitch
          mount.setTargetAngles(apiYaw, apiPitch)
          os.sleep(3)
          mount.fire(true)
          rednet.send(id, "OK")
          print("Fired. OK sent.")
        else
          rednet.send(id, "FAIL")
          print("Out of range. FAIL sent.")
        end
      end
    end
  end
end