ApiMov

--- Apis para movimiento de turtles


-- VARIABLE GLOBALES

global_debug = 0

global_deltah=0
global_deltaw=0
global_deltad=0

global_orientacion = 0 -- Guarda global_orientacion respecto a origen. 0: frente, 1: derecha, 2: atras, 3: izquierda


-- Utilidad para printear con formato estilo C
printf = function(s,...)
           return io.write(s:format(...))
         end -- function



-- FUNCIONES NAVEGACION


function tLeft()
turtle.turnLeft()
global_orientacion = (global_orientacion-1) % 4
end
function tRight()
turtle.turnRight()
global_orientacion = (global_orientacion+1) % 4
end

function back()
  if global_debug == 1 then print("b") end

  if (not turtle.back()) then
     error("NO se logro romper bloque")
  end

  if (global_orientacion == 0) then
    global_deltad=global_deltad-1
  end
  if (global_orientacion == 1) then
    global_deltaw=global_deltaw-1
  end
  if (global_orientacion == 2) then
    global_deltad=global_deltad+1
  end
  if (global_orientacion == 3) then
    global_deltaw=global_deltaw+1
  end
end

function forw()
  if global_debug == 1 then print("f") end

  turtle.dig()
  if (not turtle.forward()) then
    os.sleep(0.2)
    turtle.dig()
    if (not turtle.forward()) then
     error("NO se logro romper bloque")
    end
  end

  if (global_orientacion == 0) then
    global_deltad=global_deltad+1
  end
  if (global_orientacion == 1) then
    global_deltaw=global_deltaw+1
  end
  if (global_orientacion == 2) then
    global_deltad=global_deltad-1
  end
  if (global_orientacion == 3) then
    global_deltaw=global_deltaw-1
  end

end

function up()
  if global_debug == 1 then print("up") end

  turtle.digUp()
  if (not turtle.up()) then
   error("NO se logro romper bloque")
  end

  global_deltah=global_deltah+1

end

function down()
  if global_debug == 1 then print("down") end

  turtle.digDown()
  if (not turtle.down()) then
   error("NO se logro romper bloque")
  end

  global_deltah=global_deltah-1

end


-- Funcion que retorna a origen dado desplazamiento global

function retorno()

    printf("El delta de posicion global_orientacion es  \n")
  	printf("DH: %d DW: %d DD: %d global_orientacion: %d \n",global_deltah,global_deltaw,global_deltad,global_orientacion)

    -- Ajusta global_orientacion
    local dh = global_deltah
    local dw = global_deltaw
    local dd = global_deltad
    -- Primero ajusta dH
    for i=1,math.abs(dh) do
      if (dh > 0) then
        down()
      else
        up()
      end
    end


    --Ajusta DW
    while (global_orientacion ~= 1) do
      tLeft()
    end

    for i=1,math.abs(dw) do
      if (dw > 0) then
        back()
      else
        forward()
      end
    end

    -- Ajusta dD
    while (global_orientacion ~= 0) do
      tLeft()
    end
    for i=1,math.abs(dd) do
      if (dd > 0) then
        back()
      else
        forward()
      end
    end


  printf("Posterior a relocacion GLOBALES \n")
  printf("DH: %d DW: %d DD: %d global_orientacion: %d \n",global_deltah,global_deltaw,global_deltad,global_orientacion)

end