@name Waypoint Racer
@inputs X Y Z On XYZ:vector N Save A D
@outputs  Save1 M N1 N2 N F:vector X Y Z XYZ:vector AngVel
@persist K Xd Yd Zd P1 P2 P3 N1 N2 Save1
if (On) {
interval(1)
K=10
#if (Save & ~Save) {Save1 = !Save1}
Save1 = 1
if (Save1 & N!=0) {N1 = N} 
if (Save1) {N2 = N1} else {N2 = N}
M=entity(N2):mass()
#X = XYZ:x()
#Y = XYZ:y()
#Z = XYZ:z()

Xd = X-entity(N2):pos():x()
Yd = Y-entity(N2):pos():y()
Zd = Z-entity(N2):pos():z()+40

P1 = (K*Xd-entity(N2):vel():x())*M
P2 = (K*Yd-entity(N2):vel():y())*M
P3 = (K*Zd-entity(N2):vel():z())*M
F = vec(P1,P2,P3)
entity(N2):applyForce(vec(P1,P2,P3))
AngVel = entity(N2):angVel():yaw()
Yaw = (A-D)+0-AngVel
applyAngVel(ang(0,Yaw,0))
XYZ = vec(X,Y,Z)
}
