# Kompenzacija propelerjev
var kompenzacija = 0.5

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

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

var zadnji = 0

def kar_naprej() end

tasmota.add_cmd('KarNaprej', kar_naprej)

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)
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 odkimaj()
  tasmota.set_power(2, false)
end
def pokimaj()
  tasmota.set_power(2, true)
  tasmota.set_timer(20, odkimaj)
end

def auto_naprej()
  if tasmota.millis() > zadnji + kar_naprej_cas * 0.9
    pokimaj()
    var n = tasmota.millis() % 3

    if n == 0
        pojdi_ravno(nil, nil, dolzina * 2)
    elif n == 1
        pojdi_levo(nil, nil, dolzina * 1.5)
    else
        pojdi_desno(nil, nil, dolzina)
    end
  end

  tasmota.set_timer(kar_naprej_cas, 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', auto_naprej)

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

tasmota.set_timer(kar_naprej_cas, auto_naprej)