---

-- ============================================================
-- VTOL SERVER  (server.lua)
-- Peripherals: Create_RotationSpeedController, redstone_link_bridge
-- CC:Sable API: sublevel, aero
-- Communicates with GUI via rednet (protocol "vtol")
-- ============================================================

-- ── Частоты редстоун-линков ──────────────────────────────────
local FREQ = {
  RIGHT_FWD  = { "minecraft:purple_glazed_terracotta", "minecraft:red_glazed_terracotta"   },
  RIGHT_BACK = { "minecraft:purple_glazed_terracotta", "minecraft:blue_glazed_terracotta"  },
  LEFT_FWD   = { "minecraft:purple_glazed_terracotta", "minecraft:yellow_glazed_terracotta"},
  LEFT_BACK  = { "minecraft:purple_glazed_terracotta", "minecraft:lime_glazed_terracotta"  },
}

-- ── Константы ────────────────────────────────────────────────
local RPM_MIN        = 1
local RPM_MAX        = 256
local RPM_HOVER      = 128     -- стартовый RPM для зависания
local LINK_ON        = 15
local LINK_OFF       = 0

local ALT_KP         = 4.0    -- пропорц. высота
local ALT_KD         = 2.0    -- дифф.   высота
local NAV_KP         = 0.25   -- пропорц. навигация (в RPM-единицах на блок)
local NAV_KD         = 0.5
local NAV_YAW_KP     = 1.8    -- поворот к цели
local NAV_YAW_KD     = 0.8
local NAV_ARRIVE_R   = 3.0    -- радиус «прибыл»
local LOOP_DT        = 0.1    -- секунды

-- ── Состояние ────────────────────────────────────────────────
local state = {
  mode       = "HOLD",    -- HOLD | NAV | MANUAL
  rpm        = RPM_HOVER,
  thrust     = { rf=0, rb=0, lf=0, lb=0 },  -- 0 или 15
  target     = { x=0, y=0, z=0 },
  status     = "Инициализация...",
  gui_id     = nil,
}

local altPrev_err = 0
local navPrev_ex  = 0
local navPrev_ez  = 0
local yawPrev_err = 0

-- ── Peripherals ──────────────────────────────────────────────
local rsc  = peripheral.find("Create_RotationSpeedController")
    or error("Не найден Create_RotationSpeedController")
local rlb  = peripheral.find("redstone_link_bridge")
    or error("Не найден redstone_link_bridge")
local modem = peripheral.find("modem") or error("Не найден модем")
rednet.open(peripheral.getName(modem))

-- ── Вспомогательные ─────────────────────────────────────────
local function clampRPM(v) return math.max(RPM_MIN, math.min(RPM_MAX, math.floor(v+0.5))) end
local function sign(v) return v > 0 and 1 or (v < 0 and -1 or 0) end

local function setRPM(r)
  state.rpm = clampRPM(r)
  rsc.setTargetSpeed(state.rpm)
end

local function sendLinks()
  local t = state.thrust
  rlb.sendLinkSignal(FREQ.RIGHT_FWD[1],  FREQ.RIGHT_FWD[2],  t.rf)
  rlb.sendLinkSignal(FREQ.RIGHT_BACK[1], FREQ.RIGHT_BACK[2], t.rb)
  rlb.sendLinkSignal(FREQ.LEFT_FWD[1],   FREQ.LEFT_FWD[2],   t.lf)
  rlb.sendLinkSignal(FREQ.LEFT_BACK[1],  FREQ.LEFT_BACK[2],  t.lb)
end

local function allStop()
  state.thrust = {rf=0, rb=0, lf=0, lb=0}
  sendLinks()
end

-- Получить позу (x,y,z) и рыскание из sublevel
local function getPose()
  local ok, pose = pcall(sublevel.getLogicalPose)
  if not ok or not pose then return nil end
  -- pose.position = {x,y,z}, pose.orientation = кватернион {w,x,y,z}
  local p = pose.position
  -- Yaw из кватерниона: atan2(2*(w*y - x*z), 1-2*(y^2+z^2))   [вокруг оси Y Minecraft]
  local q = pose.orientation
  local yaw = math.atan2(2*(q.w*q.y - q.x*q.z), 1 - 2*(q.y*q.y + q.z*q.z))
  return p.x, p.y, p.z, yaw
end

local function getVel()
  local ok, v = pcall(sublevel.getVelocity)
  if not ok then return 0,0,0 end
  return v.x, v.y, v.z
end

-- ── Трансляция состояния GUI ─────────────────────────────────
local function broadcast()
  if not state.gui_id then return end
  local px,py,pz,yaw = getPose()
  local packet = {
    type   = "STATE",
    mode   = state.mode,
    rpm    = state.rpm,
    thrust = state.thrust,
    target = state.target,
    pos    = px and {x=px,y=py,z=pz} or nil,
    yaw    = yaw,
    status = state.status,
    rpmMax = rsc.getTargetSpeed and rsc.getTargetSpeed() or state.rpm,
  }
  rednet.send(state.gui_id, packet, "vtol")
end

-- ── Логика режимов ───────────────────────────────────────────

-- HOLD: держать текущую высоту PD-регулятором
local holdY = nil
local function tickHold()
  local _,py,_,_ = getPose()
  if not py then state.status="Нет данных позиции"; return end
  if not holdY then holdY = py end

  local _,vy,_ = getVel()
  local err = holdY - py
  local derr = -vy  -- дериватив = минус вертикальная скорость
  local corr = ALT_KP*err + ALT_KD*derr
  altPrev_err = err

  setRPM(state.rpm + corr)
  allStop()
  state.status = string.format("HOLD  Y=%.1f цель=%.1f Δ=%.2f", py, holdY, err)
end

-- NAV: лететь к target.x/y/z
local function tickNav()
  local px,py,pz,yaw = getPose()
  if not px then state.status="Нет данных позиции"; return end
  local tx,ty,tz = state.target.x, state.target.y, state.target.z

  -- Высота
  local _,vy,_ = getVel()
  local ey = ty - py
  local corrAlt = ALT_KP*ey + ALT_KD*(-vy)
  setRPM(state.rpm + corrAlt)

  -- Горизонталь
  local dx = tx - px
  local dz = tz - pz
  local dist = math.sqrt(dx*dx + dz*dz)

  if dist < NAV_ARRIVE_R then
    allStop()
    holdY = py
    state.status = string.format("ARRIVED  dist=%.1f", dist)
    return
  end

  -- Нужный курс (yaw) на цель
  local targetYaw = math.atan2(dx, dz)   -- MC: +Z = север, atan2(x,z) = heading
  local yawErr = targetYaw - (yaw or 0)
  -- Нормализация [-π, π]
  yawErr = (yawErr + math.pi) % (2*math.pi) - math.pi

  local vx,_,vz = getVel()
  -- Поступательная ошибка вдоль нужного направления
  local ex = dx/dist   -- единичный вектор
  local ez = dz/dist
  local speed = vx*ex + vz*ez

  local fwdCmd  = NAV_KP*dist  + NAV_KD*(-speed)
  local yawCmd  = NAV_YAW_KP*yawErr + NAV_YAW_KD*(-yawPrev_err)
  yawPrev_err   = yawErr

  -- Маппинг команд в линки
  -- fwdCmd > 0 → вперёд (RF+LF), < 0 → назад (RB+LB)
  -- yawCmd > 0 → по часовой (LF+RB), < 0 → против (RF+LB)
  local newThrust = {rf=0, rb=0, lf=0, lb=0}
  if fwdCmd > 0.5 then
    newThrust.rf = LINK_ON; newThrust.lf = LINK_ON
    newThrust.rb = LINK_OFF; newThrust.lb = LINK_OFF
  elseif fwdCmd < -0.5 then
    newThrust.rb = LINK_ON; newThrust.lb = LINK_ON
    newThrust.rf = LINK_OFF; newThrust.lf = LINK_OFF
  end

  -- Поворот перекрывает одну сторону
  if math.abs(yawErr) > 0.1 then
    if yawErr > 0 then
      -- вправо: LF + RB
      newThrust.lf = LINK_ON; newThrust.rb = LINK_ON
      newThrust.rf = LINK_OFF; newThrust.lb = LINK_OFF
    else
      -- влево: RF + LB
      newThrust.rf = LINK_ON; newThrust.lb = LINK_ON
      newThrust.lf = LINK_OFF; newThrust.rb = LINK_OFF
    end
  end

  state.thrust = newThrust
  sendLinks()
  state.status = string.format("NAV dist=%.1f yaw=%.1f°", dist, math.deg(yawErr))
end

-- MANUAL: GUI сам шлёт thrust-команды, сервер только слушает и применяет
local function tickManual()
  sendLinks()  -- применяем то, что было установлено командой
  state.status = "MANUAL"
end

-- ── Обработка команд от GUI ──────────────────────────────────
local function handleCmd(id, msg)
  state.gui_id = id
  local cmd = msg.cmd

  if cmd == "PING" then
    broadcast(); return
  end

  if cmd == "SET_MODE" then
    state.mode = msg.mode
    if msg.mode == "HOLD" then
      local _,py = getPose(); holdY = py
      allStop()
    elseif msg.mode == "MANUAL" then
      allStop()
    elseif msg.mode == "NAV" then
      yawPrev_err = 0
    end
    state.status = "Режим: "..state.mode

  elseif cmd == "SET_TARGET" then
    state.target = { x=msg.x, y=msg.y, z=msg.z }
    state.status = string.format("Цель: %.0f %.0f %.0f", msg.x, msg.y, msg.z)

  elseif cmd == "SET_RPM" then
    setRPM(msg.rpm)

  elseif cmd == "THRUST" then
    -- MANUAL thrust override
    state.thrust = msg.thrust
    sendLinks()

  elseif cmd == "HOLD_HERE" then
    local _,py = getPose(); holdY = py
    state.mode = "HOLD"
    allStop()
    state.status = "HOLD на Y="..(holdY and string.format("%.1f",holdY) or "?")

  elseif cmd == "EMERGENCY_STOP" then
    state.mode = "HOLD"
    allStop()
    setRPM(RPM_HOVER)
    holdY = nil
    state.status = "!! АВАРИЙНАЯ ОСТАНОВКА !!"
  end
end

-- ── Главный цикл ─────────────────────────────────────────────
print("[SERVER] Запущен. Ожидание GUI...")
local tickTimer = os.startTimer(LOOP_DT)

while true do
  local ev, p1, p2, p3 = os.pullEvent()

  if ev == "rednet_message" then
    local senderId, msg = p1, p2
    if type(msg) == "table" then
      handleCmd(senderId, msg)
    end

  elseif ev == "timer" and p1 == tickTimer then
    if state.mode == "HOLD" then
      tickHold()
    elseif state.mode == "NAV" then
      tickNav()
    elseif state.mode == "MANUAL" then
      tickManual()
    end
    broadcast()
    tickTimer = os.startTimer(LOOP_DT)
  end
end