rm(list=ls())

Nqds = 50
L = 10
s = c(0,1:Nqds)*L
F = 13.8379
q = (-1)^((1:Nqds)+1)/F
d = rnorm(Nqds,0,1)*0.75

trk = function(V,d){
   i = 0
   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
}

x  = 0
xp = 0
V = c(x,xp)  # values at s=0
V = trk(V,d)
xt  = V[,1]  # position through the line
plot(s, xt,typ="l",ylim=c(-1,1)*20,col="red",lwd=4)
abline(h=0,col="blue")

# draw another nearby trajectory
x  =  rnorm(1,0, 2)
xp = (rnorm(1,0, 2) + 2.5*x)/60
V = c(x,xp)  # values at s=0
V = trk(V,d)
xt  = V[,1]  # position through the line
points(s, xt,typ="l",lty=2)

# draw another nearby trajectory
x  =  rnorm(1,0, 2)
xp = (rnorm(1,0, 2) + 2.5*x)/60
V = c(x,xp)  # values at s=0
V = trk(V,d)
xt  = V[,1]  # position through the line
points(s, xt,typ="l",lty=2)

# draw another nearby trajectory
x  =  rnorm(1,0, 2)
xp = (rnorm(1,0, 2) + 2.5*x)/60
V = c(x,xp)  # values at s=0
V = trk(V,d)
xt  = V[,1]  # position through the line
points(s, xt,typ="l",lty=2)

# draw a series of nearby trajectories
Ntr = 100
i = 0
while(i<Ntr){
   i = i+1
   x  =  rnorm(1,0, 2)
   xp = (rnorm(1,0, 2) + 2.5*x)/60
   V = c(x,xp)  # values at s=0
   V = trk(V,d)
   xt  = V[,1]  # position through the line
   points(s, xt,typ="l",lty=2)
}


# Introduce various sets of random errors;
# look at final trajectory

drms = 0.5
d = rnorm(Nqds,0,drms)

V = c(0,0)  # values at s=0
V = trk(V,d)
xt  = V[,1]  # position through the line
plot(s, xt,typ="l",ylim=c(-1,1)*20,
   xlab="path length [m]",ylab="x [mm]")
abline(h=0,col="blue")

# change the quad offsets and track again
Ntrials = 1000
xtL = c(1:Ntrials)*0
i = 1
while(i<Ntrials){
   i = i+1
   d = rnorm(Nqds,0,drms)
   V = c(0,0)  # values at s=0
   V = trk(V,d)
   xt  = V[,1]  # position through the line
   points(s, xt,typ="l",lty=2)
   xtL[i] = xt[length(xt)]
}

beta = 2*F*sqrt((1+L/2/F)/(1-L/2/F))
sf = beta*drms/F/2
curve( 3*sf*sqrt(x/L),add=TRUE,col="red",lwd=3)
curve(-3*sf*sqrt(x/L),add=TRUE,col="red",lwd=3)

hist(xtL,breaks=25)