robot = require("robot")
component = require("component")
StartPositionX , StartPositionY , StartPositionZ = component.navigation.getPosition() -- where the robot is when program started
DistX = "NCY"
DistY = "NCY"--Not Calibrated Yet...
DistZ = "NCY"
Down = false
while true do
 CurrentPositionX , CurrentPositionY , CurrentPositionZ = component.navigation.getPosition() -- Where The Robot Currently Is
 print("Current Position: ["..CurrentPositionX..", "..CurrentPositionY..", "..CurrentPositionZ.."]")
end

function CheckDirection(Value1 , Value2 , Value3) -- Value1 : (Changing Value) , Value2 : (Value That Gets Compared To Value) , Value3 : X , Y , or Z
    print("Checking Direction")
 local loop = true
 if Value3 == "X" then
  while loop == true do
   robot.forward()
   Value1 = CurrentPositionX --To check if the dir is correct (E(+) , W(-))
   if Value1 ~= Value2 then
    loop = false
    return true
   elseif Value == Value2 then
    robot.back()
    robot.turnLeft()
   end
  end
 elseif Value3 == "Y" then
  while loop == true do
   robot.up()
   Value1 = CurrentPositionY --To check if the dir is correct (Up Or Down)
   if Value1 ~= Value2 then
    loop = false
    return true
   elseif Value == Value2 then
    Down = true
    robot.down()
   end
  end
 elseif Value3 == "Z" then
  while loop == true do
   robot.forward()
   Value1 = CurrentPositionZ --To check if the dir is correct (N(-) , S(+))
   if Value1 ~= Value2 then
    loop = false
    return true
   elseif Value == Value2 then
    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
  break
 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
  break
 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
  break
 end
end

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