Unfortunately it's exam time so I don't have the time to provide a working script but you should do the following:

• create a VECTOR array with the coods of your waypoints
• fill the nodes[][] array with computed values (e.g. differences in height and/or distance)
• run the algorithm
• pass through the predecessor array, beginning with predecessor[target]
• let your player follow the found nodes in the predecessor array