-- Get objects from orxonox
thisscript:addObject("GenericNPC", "secondguard")
thisscript:addObject("GenericNPC", "guard")
thisscript:addObject("FPSPlayer", "Player")
thisscript:addObject("GenericNPC", "prisoner")

-- Global Variables
allOk = true
cellReached = false
guidePositionReached = false
prisonerReachedLock = false
hangarReached = false

height = 100

-- Returns the distance between (x1,x2,x3) and (y1,y2,y3)
function dist( x1,x2,x3, y1,y2,y3 )
return math.sqrt( (x1-y1)^2 + (x2-y2)^2 + (x3-y3)^2 )
end


function observeGuard()
guardX = guard:getAbsCoorX()
guardY = guard:getAbsCoorY()
guardZ = guard:getAbsCoorZ()

if dist( guardX,guardY,guardZ,35.48, 29, 191.28 ) < 30 then
allOk = false
end

end

function observePrisoner()

prisonerX = prisoner:getAbsCoorX()
prisonerY = prisoner:getAbsCoorY()
prisonerZ = prisoner:getAbsCoorZ()

if dist( prisonerX,prisonerY,prisonerZ,162.757919,49,363.489349 ) < 60 then
--io.write("Prisonner reached lock \n")
prisonerReachedLock = true
end

end

cellProgrammed = false
function goToCell(timestep)

if not cellProgrammed then
secondguard:walkTo( 442.173767, height, 176.429855)
secondguard:walkTo( 442.173767, height, 176.429855)
secondguard:walkTo( 436.727753, height, 188.191757)
secondguard:walkTo( 432.311584, height, 198.882462)
secondguard:walkTo( 426.079590, height, 213.968994)
secondguard:walkTo( 420.553986, height, 226.714996)
secondguard:walkTo( 412.681915, height, 235.053696)
secondguard:walkTo( 398.758545, height, 241.846924)
secondguard:walkTo( 384.690735, height, 242.851395)
secondguard:walkTo( 348.930481, height, 242.394958)
secondguard:walkTo( 336.467926, height, 241.985764)
secondguard:walkTo( 324.042999, height, 241.577774)
secondguard:walkTo( 303.851410, height, 240.914841)
secondguard:walkTo( 293.013977, height, 240.559006)
secondguard:walkTo( 276.207611, height, 240.007233)
secondguard:walkTo( 261.475800, height, 239.523544)
secondguard:walkTo( 239.185089, height, 238.791687)
secondguard:walkTo( 226.655655, height, 238.491974)
secondguard:walkTo( 211.996979, height, 238.851044)
secondguard:walkTo( 192.956741, height, 239.859207)
secondguard:walkTo( 178.496552, height, 241.852539)
secondguard:walkTo( 160.421860, height, 244.009827)
secondguard:walkTo( 145.371857, height, 244.946075)
secondguard:walkTo( 129.918198, height, 245.307037)
secondguard:walkTo( 113.065468, height, 245.108231)
secondguard:walkTo( 99.159073, height, 244.695694)
secondguard:walkTo( 82.069923, height, 244.188766)
secondguard:walkTo( 65.880531, height, 243.708527)
secondguard:walkTo( 48.133881, height, 243.182114)
secondguard:walkTo( 34.706280, height, 242.783813)
secondguard:walkTo( 20.682344, height, 242.367783)
secondguard:walkTo( 5.419819, height, 241.915070)
secondguard:walkTo( -10.556346, height, 241.058746)
secondguard:walkTo( -24.159513, height, 240.127090)
secondguard:walkTo( -33.819145, height, 239.995453)
secondguard:walkTo( -45.414162, height, 240.373322)
secondguard:walkTo( -60.228550, height, 240.856079)
secondguard:walkTo( -73.269897, height, 241.260895)
secondguard:walkTo( -86.128647, height, 240.457886)
secondguard:walkTo( -100.443581, height, 236.485718)
secondguard:walkTo( -112.346939, height, 233.967209)
secondguard:walkTo( -127.331635, height, 227.615112)
secondguard:walkTo( -135.153656, height, 215.476883)
secondguard:walkTo( -131.529984, height, 202.931015)
secondguard:walkTo( -122.671799, height, 193.644669)
secondguard:walkTo( -113.823517, height, 183.986633)
secondguard:walkTo( -104.089592, height, 170.659729)
cellProgrammed = true
end

if secondguard:finalGoalReached() then
return true
end

return false
end

guideProgrammed = false
function goToGuidePosition(timestep)

if not guideProgrammed then
secondguard:walkTo( -104.757011, height, 171.200729)
secondguard:walkTo( -100.266739, height, 171.345627)
secondguard:walkTo( -91.152046, height, 171.639709)
secondguard:walkTo( -85.456375, height, 171.823486)
secondguard:walkTo( -74.048538, height, 172.191589)
secondguard:walkTo( -53.574173, height, 172.852219)
secondguard:walkTo( -43.275124, height, 173.184540)
secondguard:walkTo( -35.155544, height, 173.446518)
secondguard:walkTo( -31.645050, height, 173.559784)
secondguard:walkTo( -23.426588, height, 173.824966)
secondguard:walkTo( -10.576803, height, 174.239578)
secondguard:walkTo( -2.389911, height, 174.503754)
secondguard:walkTo( 6.950986, height, 174.805176)
secondguard:walkTo( 16.313608, height, 175.161179)
secondguard:walkTo( 23.271458, height, 175.547226)
secondguard:walkTo( 31.248058, height, 176.199890)
secondguard:walkTo( 43.694778, height, 177.753479)
secondguard:walkTo( 50.441612, height, 178.595612)
secondguard:walkTo( 71.374535, height, 181.208389)
secondguard:walkTo( 82.251022, height, 181.855988)
secondguard:walkTo( 91.975792, height, 182.434998)
secondguard:walkTo( 101.695015, height, 183.013672)
secondguard:walkTo( 110.377205, height, 183.530609)
secondguard:walkTo( 122.299400, height, 184.240448)
secondguard:walkTo( 129.848495, height, 184.689911)
secondguard:walkTo( 141.687805, height, 185.394821)
secondguard:walkTo( 150.166702, height, 186.776016)
secondguard:walkTo( 155.342255, height, 189.285080)
secondguard:walkTo( 162.508133, height, 192.849976)
secondguard:walkTo( 171.747696, height, 197.075134)
secondguard:walkTo( 177.846390, height, 199.864029)
secondguard:walkTo( 182.707596, height, 204.381516)
secondguard:walkTo( 182.707596, height, 204.381516)
secondguard:walkTo( 187.317139, height, 211.966385)
secondguard:walkTo( 193.191528, height, 221.632446)
secondguard:walkTo( 197.856018, height, 229.307724)
secondguard:walkTo( 198.350906, height, 232.634857)
secondguard:walkTo( 198.427155, height, 243.656113)

guideProgrammed = true
end

if secondguard:finalGoalReached() then
return true
end

return false
end

hangarProgrammed = false
function goToHangar(timestep)

if not hangarProgrammed and prisonerReachedLock then
secondguard:walkTo( 194.153641, height, 244.749863)
secondguard:walkTo( 185.976517, height, 243.876404)
secondguard:walkTo( 179.645737, height, 243.505066)
secondguard:walkTo( 169.583008, height, 244.959457)
secondguard:walkTo( 161.454269, height, 249.422363)
secondguard:walkTo( 158.488998, height, 260.951874)
secondguard:walkTo( 158.228958, height, 272.121582)
secondguard:walkTo( 158.326004, height, 288.892853)
secondguard:walkTo( 158.389069, height, 299.792603)
secondguard:walkTo( 158.437561, height, 308.174896)
secondguard:walkTo( 158.479034, height, 315.341095)
secondguard:walkTo( 158.542801, height, 326.362518)
secondguard:walkTo( 158.621414, height, 339.950256)
secondguard:walkTo( 158.664566, height, 347.408417)
secondguard:walkTo( 158.737900, height, 360.080902)
secondguard:walkTo( 158.795013, height, 369.950531)
secondguard:walkTo( 158.857086, height, 380.676056)
secondguard:walkTo( 158.935440, height, 394.219177)
secondguard:walkTo( 158.984039, height, 402.618896)
secondguard:walkTo( 159.035522, height, 411.518219)
secondguard:walkTo( 159.089432, height, 420.837036)
secondguard:walkTo( 159.143021, height, 430.104523)
secondguard:walkTo( 159.203644, height, 440.579224)
secondguard:walkTo( 159.257263, height, 449.841522)
secondguard:walkTo( 159.304810, height, 458.057617)
secondguard:walkTo( 159.351440, height, 466.113068)
secondguard:walkTo( 160.060013, height, 482.411865)
secondguard:walkTo( 161.352600, height, 488.302185)
secondguard:walkTo( 174.254333, height, 503.439209)
secondguard:walkTo( 181.668213, height, 506.381195)
secondguard:walkTo( 189.177277, height, 508.729156)
secondguard:walkTo( 213.465332, height, 512.184326)
secondguard:walkTo( 223.635010, height, 512.920715)
secondguard:walkTo( 233.668503, height, 513.374512)
secondguard:walkTo( 448.702118, height, 512.187744)
secondguard:walkTo( 477.701050, height, 511.587463)
secondguard:walkTo( 525.645752, height, 511.139526)
secondguard:walkTo( 559.871216, height, 510.819763)
secondguard:walkTo( 596.801941, height, 510.632080)
secondguard:walkTo( 640.160767, height, 510.906891)
secondguard:walkTo( 684.745239, height, 510.429138)
secondguard:walkTo( 706.457214, height, 508.596039)
secondguard:walkTo( 718.750916, height, 507.558044)
secondguard:walkTo( 728.147827, height, 506.649780)
secondguard:walkTo( 743.581970, height, 502.185822)
secondguard:walkTo( 752.977234, height, 495.788635)
secondguard:walkTo( 759.201721, height, 483.407104)
secondguard:walkTo( 761.196167, height, 461.130798)
secondguard:walkTo( 762.335449, height, 398.166901)
secondguard:walkTo( 762.908691, height, 366.482208)
secondguard:walkTo( 763.506409, height, 333.456848)
secondguard:walkTo( 764.330750, height, 287.894409)
secondguard:walkTo( 767.909790, height, 194.910492)
secondguard:walkTo( 770.649231, height, 122.865265)
secondguard:walkTo( 770.102112, height, 44.244534)
secondguard:walkTo( 767.269775, height, -37.871273)
secondguard:walkTo( 763.989197, height, -132.996460)
secondguard:walkTo( 755.274231, height, -217.520859)
secondguard:walkTo( 739.636230, height, -307.869354)
secondguard:walkTo( 722.792725, height, -390.075226)
secondguard:walkTo( 702.582275, height, -475.155273)
secondguard:walkTo( 687.325012, height, -551.699036)
secondguard:walkTo( 681.873169, height, -628.924316)
secondguard:walkTo( 679.997437, height, -708.013794)
secondguard:walkTo( 681.080933, height, -772.108887)
secondguard:walkTo( 681.686035, height, -845.449524)
secondguard:walkTo( 682.188538, height, -906.434998)
secondguard:walkTo( 682.669800, height, -964.887085)
secondguard:walkTo( 684.829895, height, -1023.870117)
secondguard:walkTo( 690.728516, height, -1069.400146)
secondguard:walkTo( 695.965027, height, -1097.353149)
secondguard:walkTo( 701.128540, height, -1118.496216)
secondguard:walkTo( 708.865234, height, -1144.444214)
secondguard:walkTo( 724.753845, height, -1192.741699)
secondguard:walkTo( 737.665771, height, -1230.398560)
secondguard:walkTo( 741.520081, height, -1241.704712)
secondguard:walkTo( 756.537842, height, -1287.260376)
secondguard:walkTo( 764.046631, height, -1310.927002)
secondguard:walkTo( 769.402771, height, -1342.806152)
secondguard:walkTo( 772.158691, height, -1363.218750)
secondguard:walkTo( 772.599426, height, -1374.571899)
secondguard:walkTo( 771.977844, height, -1390.778320)
secondguard:walkTo( 771.576355, height, -1401.246826)
secondguard:walkTo( 770.982666, height, -1423.905273)
secondguard:walkTo( 770.952637, height, -1429.344360)
secondguard:walkTo( 770.912476, height, -1436.589966)
secondguard:walkTo( 770.880249, height, -1442.399536)
secondguard:walkTo( 770.847107, height, -1448.385376)
secondguard:walkTo( 770.785339, height, -1454.349365)
secondguard:walkTo( 768.858459, height, -1464.382080)
secondguard:walkTo( 767.639709, height, -1469.028809)
secondguard:walkTo( 765.099487, height, -1476.370605)
secondguard:walkTo( 762.003601, height, -1484.767456)
secondguard:walkTo( 758.787231, height, -1492.394287)
secondguard:walkTo( 755.135986, height, -1496.482788)
secondguard:walkTo( 752.631165, height, -1498.550049)
secondguard:walkTo( 746.021912, height, -1502.547485)
secondguard:walkTo( 739.376099, height, -1505.427734)
secondguard:walkTo( 731.429199, height, -1507.691162)
secondguard:walkTo( 719.202393, height, -1510.812622)
secondguard:walkTo( 716.329712, height, -1511.546021)
secondguard:walkTo( 711.301514, height, -1512.674438)
secondguard:walkTo( 702.414124, height, -1514.126099)
secondguard:walkTo( 695.724304, height, -1515.173218)
secondguard:walkTo( 576.522278, height, -1521.146362)
secondguard:walkTo( 542.552246, height, -1522.204712)
secondguard:walkTo( 499.907288, height, -1524.441772)
secondguard:walkTo( 461.101288, height, -1525.987915)
secondguard:walkTo( 417.154602, height, -1526.579102)
secondguard:walkTo( 367.752686, height, -1527.168091)
secondguard:walkTo( 322.209106, height, -1527.709473)
secondguard:walkTo( 273.998047, height, -1528.283447)
secondguard:walkTo( 234.330612, height, -1528.756348)
secondguard:walkTo( 188.730209, height, -1529.299438)
secondguard:walkTo( 140.078659, height, -1529.880249)
secondguard:walkTo( 105.882103, height, -1530.287109)
secondguard:walkTo( 71.860947, height, -1532.856079)
secondguard:walkTo( 39.777569, height, -1541.485596)
secondguard:walkTo( 13.919314, height, -1565.188110)
secondguard:walkTo( -0.632493, height, -1604.602783)
secondguard:walkTo( -2.103141, height, -1639.236694)
secondguard:walkTo( 0.498238, height, -1674.846558)
secondguard:walkTo( 2.861493, height, -1705.131958)

hangarProgrammed = true
end

guardX = secondguard:getAbsCoorX()
guardY = secondguard:getAbsCoorY()
guardZ = secondguard:getAbsCoorZ()

if dist( guardX,guardY,guardZ, 2.861493, 29, -1705.131958) < 60 then
return true
end

return false

end

time = 2
function tick(timestep)
--io.write("Secondguard called \n")
 
observePrisoner()
observeGuard()

if not allOk then
cellReached = goToCell(timestep)
end

--do talking
if cellReached then
time = time - timestep
end


if time < 0 then

if cellReached and not guidePositionReached then
guidePositionReached = goToGuidePosition(timestep)
end

if guidePositionReached and not hangarReached then
hangarReached = goToHangar(timestep)
end

if hangarReached then
return true
end


end


return false
end