--------------------------------------------------------------------
-- lib/navigation.lua : position GPS, estimation du cap, ralliement.
--
-- Le cap n'est pas mesurable directement (pas de boussole): il est
-- estime a partir du deplacement GPS quand la vitesse est
-- suffisante. Le ralliement avance donc "en aveugle" au depart puis
-- corrige le cap des que le drone bouge.
--------------------------------------------------------------------
local Navigation = {}

function Navigation.nouveau(conf, materiel, journal)
  local n = {}
  local posPrec, tPrec = nil, nil
  local cap = nil    -- radians, atan2(dx, dz) du deplacement

  function n.position()
    return materiel.lirePosition()
  end

  -- A appeler periodiquement: met a jour l'estimation du cap
  function n.actualiser()
    local x, y, z = materiel.lirePosition()
    if not x then return nil end
    local t = os.clock()
    if posPrec and t > tPrec then
      local dx, dz = x - posPrec.x, z - posPrec.z
      local dist = math.sqrt(dx * dx + dz * dz)
      local vitesse = dist / (t - tPrec)
      if vitesse >= conf.NAV.VITESSE_GPS_MIN then
        cap = math.atan2(dx, dz)
      end
    end
    posPrec, tPrec = { x = x, y = y, z = z }, t
    return x, y, z
  end

  -- Commandes propulseurs pour rallier (cibleX, cibleZ).
  -- Retourne avance, virage, distance (ou nil si pas de GPS).
  function n.rallier(cibleX, cibleZ)
    local x, y, z = n.actualiser()
    if not x then return nil end

    local dx, dz = cibleX - x, cibleZ - z
    local distance = math.sqrt(dx * dx + dz * dz)
    if distance <= conf.NAV.SEUIL_ARRIVEE then
      return 0, 0, distance
    end

    local capCible = math.atan2(dx, dz)
    local virage = 0
    if cap then
      local ecart = capCible - cap
      while ecart > math.pi do ecart = ecart - 2 * math.pi end
      while ecart < -math.pi do ecart = ecart + 2 * math.pi end
      -- proportionnel borne: plein virage au dela de 45 degres
      virage = math.max(-1, math.min(1, ecart / (math.pi / 4)))
    end

    -- avance reduite tant que le cap est incertain ou tres desaxe
    local avance = 0.8
    if cap == nil then
      avance = 0.4          -- demarrage: avancer pour estimer le cap
    elseif math.abs(virage) > 0.7 then
      avance = 0.2          -- virage serre: privilegier la rotation
    end

    return avance, virage, distance
  end

  function n.razCap()
    cap, posPrec, tPrec = nil, nil, nil
  end

  return n
end

return Navigation