# Kompenzacija propelerjev
kompenzacija = 0.5

# Kako dolgo cakam med ukazi (ms)?
cakaj = 3000

# Kako pogosto grem kar ravno (ms)?
kar_naprej_cas = 1.5 * 60 * 1000

zadnji = 0

def nic()
  print('ja, nic')
end
tasmota.add_cmd('KarNaprej', nic)

def ugasni_levo()
  tasmota.set_power(0, false)
end

def ugasni_desno()
  tasmota.set_power(1, false)
end

def posodobi_zadnji(cas)
  zadnji = tasmota.millis() + cas
  tasmota.cmd('KarNaprej')
end

# payload = kako dolgo naj bo vklopljen, v milisekundah
def pojdi_levo(cmd, idx, payload, payload_json)
  if tasmota.millis() < zadnji + cakaj return nil end

  tasmota.set_power(0, true)

  var cas = int(int(payload) * kompenzacija)
  tasmota.set_timer(cas, ugasni_levo)

  posodobi_zadnji(cas)
  #kar_naprej()
end

# payload = kako dolgo naj bo vklopljen, v milisekundah
def pojdi_desno(cmd, idx, payload, payload_json)
  if tasmota.millis() < zadnji + cakaj return nil end

  tasmota.set_power(1, true)

  var cas = int(payload)
  tasmota.set_timer(cas, ugasni_desno)

  posodobi_zadnji(cas)
end

def ugasni_vse()
  ugasni_levo()
  ugasni_desno()
end
  
# payload = kako dolgo naj bo vklopljen, v milisekundah
def pojdi_ravno(cmd, idx, payload, payload_json)
  if tasmota.millis() < zadnji + cakaj return nil end
  
  tasmota.set_power(0, true)
  tasmota.set_power(1, true)

  #tasmota.set_timer(int(payload), ugasni_vse)
  # Kompenziramo razliko v motorjih
  var cas = int(payload) * 2
  tasmota.set_timer(int(cas * kompenzacija), ugasni_levo)
  tasmota.set_timer(cas, ugasni_desno)

  posodobi_zadnji(cas)
end

def auto_naprej()
  pojdi_ravno(nil, nil, 1000)
end

def kar_naprej()
  tasmota.remove_timer("auto_naprej")
  tasmota.set_timer(kar_naprej_cas, auto_naprej, "auto_naprej")
end

tasmota.add_cmd('PojdiLevo', pojdi_levo)
tasmota.add_cmd('PojdiDesno', pojdi_desno)
tasmota.add_cmd('PojdiRavno', pojdi_ravno)
tasmota.add_cmd('UgasniVse', ugasni_vse)
tasmota.add_cmd('KarNaprej', kar_naprej)

def inicializiraj()
    tasmota.cmd('WcInit')
end
tasmota.set_timer(5000, inicializiraj)