robot = require("robot")
component = require("component")

function getPosition()
    local x,y,z = component.navigation.getPosition()
    if x == nil or y == nil or z == nil then
     print("Error: out of bounds, can not locate self...")
     os.exit()
    end
    return x,y,z
   end

StartPositionX , StartPositionY , StartPositionZ = component.navigation.getPosition() -- where the robot is when program started
if getPosition then
    CurrentPositionX , CurrentPositionY , CurrentPositionZ = component.navigation.getPosition()
    print("Current Pos: [X: "..CurrentPositionX.." Y: "..CurrentPositionY.." Z: "..CurrentPositionZ .."]")
end
DistX = "NCY"
DistY = "NCY"--Not Calibrated Yet...
DistZ = "NCY"
Down = false

--function pointToDirection(dir)
 --while nav.getFacing() ~= dir do robot.turnLeft() end
--end

function getPosition()
local   x,y,z = component.navigation.getPosition()
 if x == nil or y == nil or z == nil then
  print("Error: out of bounds, can not locate self...")
  os.exit()
 end
end

function CheckDirection(Value1 , Value2 , Value3) -- Value1 : (Changing Value) , Value2 : (Value That Gets Compared To Value) , Value3 : X , Y , or Z
 print("Checking Direction: "..Value3)
 local loop = true



 if Value3 == "X" then
  while loop == true do
    print("Checking...")
  print(Value1 , Value2)
  robot.forward()
  CurrentPositionX , CurrentPositionY , CurrentPositionZ = getPosition() -- Where The Robot Currently Is
   Value1 = CurrentPositionX --To check if the dir is correct (E(+) , W(-))
   if Value1 ~= Value2 then
    if Value1 < Value2 then -- Robot got closer to origin
     print("Got Closer To Origin")
     loop = false
     return true
    elseif Value1 > Value2 then -- Robot got farther from origin
    print("Moving Back Wrong Dir")
    robot.back()
     robot.turnLeft()
    end
   elseif Value1 == Value2 then
    print("Moving Back Wrong Dir")
    robot.back()
    robot.turnLeft()
   end
  end


 elseif Value3 == "Y" then
  while loop == true do
    print("Checking...")
   robot.up()
  CurrentPositionX , CurrentPositionY , CurrentPositionZ = getPosition() -- Where The Robot Currently Is
   Value1 = CurrentPositionY --To check if the dir is correct (Up Or Down)
   if Value1 < Value2 then  -- Robot got closer to origin
    loop = false
    return true
   elseif Value1 > Value2 then-- Robot got farther from origin
    Down = true
    robot.down()
   end
  end


 elseif Value3 == "Z" then
  while loop == true do
    print("Checking...")
   robot.forward()
  CurrentPositionX , CurrentPositionY , CurrentPositionZ = getPosition() -- Where The Robot Currently Is
   Value1 = CurrentPositionZ --To check if the dir is correct (N(-) , S(+))
   if Value1 ~= Value2 then
    if Value1 < Value2 then -- Robot got closer to origin
     loop = false
     return true
    elseif Value1 > Value2 then-- Robot got farther from origin
    print("Moving Back Wrong Dir")
    robot.back()
     robot.turnLeft()
    end
   elseif Value1 == Value2 then
    print("Moving Back Wrong Dir")
    robot.back()
    robot.turnLeft()
   end
  end
 end
end

function Goto_Origin_X(X)
 print("Going to X")
 if CheckDirection(CurrentPositionX , StartPositionX , "X") then
  DistX = CurrentPositionX - StartPositionX
  print("DistanceX: "..DistX)
  for Count = 0 , DistX do
   if robot.detect() == false then
    robot.forward()
   elseif robot.detect() == true then
    robot.up()
   end
  end
 end
end

function Goto_Origin_Y(Y)
 print("Going To Y")
 if CheckDirection(CurrentPositionY , StartPositionY , "Y") then
  DistY = CurrentPositionY - StartPositionY
  print("DistanceY: "..DistY)
  for Count = 0 , DistY do
   if robot.detect() == false then
    if Down == false then
     robot.up()
    else
     robot.down()
    end
   elseif robot.detect() == true then
    robot.up()
   end
  end
 end
end

function Goto_Origin_Z(Z)
 print("Going To Z")
 if CheckDirection(CurrentPositionZ , StartPositionZ , "Z") then
  DistZ = CurrentPositionZ - StartPositionZ
  print("DistanceZ: "..DistZ)
  for Count = 0 , DistZ do
   if robot.detect() == false then
    robot.forward()
   elseif robot.detect() == true then
    robot.up()
   end
  end
 end
end

Goto_Origin_X(CurrentPositionX)
Goto_Origin_Y(CurrentPositionY)
Goto_Origin_Z(CurrentPositionZ)