###FIRST WE DEFINE CONSTANTS NEEDED FOR TO ESTIMATE DE SEs
Window <- function(x)(1-x^3)^3
TT <- 5001
N <- (TT-1)/2
waux <-  apply(as.array(c(N:0,1:N)/N),1,Window)
aux <- c(1:TT)/TT - .5/TT
W0 <- mean(waux)
W1 <-  mean(aux*waux)
W2 <-  mean(aux^2*waux)
U0 <-  mean(waux^2)
U1 <-  mean(aux*waux^2)
U2 <-  mean(aux^2*waux^2)
a0 <- (W0*W2-W1^2)^{-2}
a1 <- (U0*U2 - U1^2)
a2 <- W0^(-2)*(W0*U1 - W1*U0)^2
b0 <- (U2*W0^2 - 2*W1*U1*W0+U0*W1^2)
b1 <-  (U2*W1^2 - 2*W2*U1*W1+U0*W2^2)
 
C0 <- a0*b0
C1 <- U0/W0^2
C2 <- a0*b1
C3 <- a0*W1/(W0^2)*(W0^2*W1*U2 - W1^3*U0 - 2*W0^2*W2*U1 + 2*W0*W1*W2*U0)
C4 <- a0*(W0*W1*U2 + W1*W2*U0 - U1*(W1^2  + W0*W2))
rm(a0,a1,a2,b0,b1,b2)


##THIS FILE IS WRITTEN FOR 5 harmonics.
WS <- 6*(48) +1   ##  THIS IS THE WINDOW SIZE
RATE <- 1 ## THE RATE is measurments taken per unit of time
U <- (WS-1)/2 ### half the window size to add to each side of t_0
l1 <- 1/RATE*2*pi  #Initial value of the frequency
jump <- 1 ## after estimating t_0, we wil estimate t_0 + jump
tolerance <- 0.005 ### the tolerance in the minimization algorithm
#
N <- length(y)
o <- seq((U+1),(N-U),jump)
NH <- 5
res <-  apply(as.array(o),1,function(t0){
  cat(t0," ") 
  tt <- TIME[(t0-U):(t0+U)]
  wtt <- (tt - min(tt))/(diff(range(tt))) - 0.5#used for window calc
  w <- apply(as.array(2*abs(wtt)),1,function(x){(1-x^3)^3})
  yy <- y[(t0-U):(t0+U)]
  MEAN <- mean(yy)
  yy <- yy - MEAN
  data.object <-  data.frame(yy=w*yy,tt=tt,w=w)
  parameters(data.object) <- list(l1=l1)
  aux <- my.nls(yy~cbind(w,w*cos(l1*1*tt),w*sin(l1*1*tt),
		      w*cos(l1*2*tt),w*sin(l1*2*tt),
		      w*cos(l1*3*tt),w*sin(l1*3*tt),
		      w*cos(l1*4*tt),w*sin(l1*4*tt),
		      w*cos(l1*5*tt),w*sin(l1*5*tt)),
	     data=data.object,algorithm="plinear",
	     start=parameters(data.object),trace=TRACE,
	     control=nls.control(maxiter=150,tolerance=tolerance))
  AMP <- sqrt(aux$param[seq(3,2*NH+2,2)]^2+aux$param[seq(4,2*NH+2,2)]^2)
  sigma <- sum(aux$resid^2)/sum(w^2) 
  AMP.se <- sqrt((2*sigma*C1)/WS)
  se <- sqrt(2*sigma/(sum(c(1:NH)^2*(AMP^2)))*C0/WS^3)
  c(aux$parameters[1:2],se,AMP,AMP.se,aux$fitted[U+1]+MEAN,aux$res[U+1])
})


aux <- list(l1=res[1,]*RATE/2/pi,mean=res[2,],
	    se=res[3,]*RATE/2/pi,amp=res[4:(NH+3),],ampse=res[(NH+4),],
	    fitted=res[(NH+5),],resid=res[(NH+6),],time=o,rate=RATE)


###my.nls is like nls but doesnt stop if there is no convergence is just says 
###there was an error... see my.nls.S