rm(list=ls())

nu = 7.120926 # betatron tune
Nt = 28       # total no. turns to track

Nqds = 64
L = 30
s = c(0,1:Nqds)*L
F = L/sin(2*pi*nu/Nqds*2/2)/2
q = (-1)^((1:Nqds)+1)/F
d = c(1:Nqds)*0
d[25] = 0.5

trk = function(V0){
   i = 0
   V = V0
   x  = V0[1]
   xp = V0[2]
   while(i < Nqds){
      i = i+1
      xp = xp-x*q[i]  + d[i]*q[i]
      x  = x+xp*L
      V  = rbind(V,c(x,xp))
   }
   V
}

V0 = c(0,0)  # values at s=0
V = trk(V0)
xt  = V[,1]  # position through the line
plot(s, xt,typ="l",ylim=c(-1,1)*10,col="red",lwd=4)
abline(h=0,col="blue")
text(1800, 10, paste("nu =",round(nu,4)))
text(100, 10, paste("n =",1))
text(800, 10, paste("theta =", round(d[25]/F*1e3,2)," mr"))
u0 <- locator(1)

V0 = V[Nqds+1, ]
V = trk(V0)
xt  = V[,1]  # position through the line
points(s, xt,typ="l",lty=2)
text(100,10,paste("n =",1),col="white")
text(100,10,paste("n =",2),  col="black")
u0 <- locator(1)


# repeat for another Nt turns
i=2
while(i<Nt+2){
   tt=1; while(tt<2000000){tt=tt+1} # time delay
   i=i+1
   V0 = V[Nqds+1, ]
   V  = trk(V0)
   xt = V[,1]  # position through the line
   x0 = V[length(s),]
   points(s, xt,typ="l",lty=2)
   text(100,10,paste("n =",i-1),col="white")
   text(100,10,paste("n =",i),  col="black")
}
u0 <- locator(1)


# find the closed orbit and plot
ZZ = function(X){
	dV = trk(X)[Nqds+1,]-X
	sum(dV * dV)
}
Vopt = optim(c(0,0),ZZ)
Vclo = Vopt$par

V  = trk(Vclo)
xt = V[,1]  # position through the line
points(s, xt,typ="l",col="green",lwd=4)