library(fda)
library(ggplot2)
library(plyr)


################################################################################
###                      FUNCTIONAL REGISTRATION                            ####
################################################################################

# Berkeley Growth

girls <- growth$hgtf
age <- growth$age
n.t <- length(age)


growbasis <- create.bspline.basis(breaks = age, norder = 6)
Lfdobj <- int2Lfd(3)          #  penalize curvature of acceleration
lambda <- 10^(-0.5)  #  smoothing parameter
cvecf <- matrix(0, growbasis$nbasis, ncol(girls))
dimnames(cvecf) <- list(growbasis$names, dimnames(girls)[[2]])

growfd0 <- fd(cvecf, growbasis)
growfdPar <- fdPar(growfd0, Lfdobj, lambda)

growthMon <- smooth.monotone(age, girls, growfdPar)
girlshatfd <- growthMon$yhatfd

# hgtfhatfd = girlshatfd

accelfdUN <- deriv.fd(girlshatfd, 2)
accelmeanfdUN <- mean.fd(accelfdUN)

# plot curves

age.spec <- seq(1, 18, length.out = 100)
x11()
plot(girlshatfd[1:10], lty = 1, lwd = 2,
     xlab = "Age [years]", ylab = "Height [cm]")

# ggplot

# age.spec <- seq(1, 18, length.out = 100)
# df <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, girlshatfd)), girl = factor(rep(1:54, each = length(age.spec))))
# 
# x11()
# p0 <- ggplot(df[1:1000,], aes(x = Age, y = Height, colour = girl, group = girl))
# p0 <- p0 + geom_line()
# p0 <- p0 + labs (x = "Age [years]", y = "Height [cm]")
# p0 <- p0 + theme_bw()
# p0 <- p0 + guides(colour = FALSE)
# p0

#  plot unregistered acceleration curves

plot(accelfdUN[1:10], xlim = c(1,18), ylim = c(-4,3), lty = 1, lwd = 2,
     cex = 2, xlab = "Age", ylab= "Acceleration (cm/yr/yr)")

# ggplot

# df.accel <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, accelfdUN)), girl = factor(rep(1:54, each = length(age.spec))))
# df.accel.mean <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, accelmeanfdUN)), girl = "mean")
# 
# x11()
# p0 <- ggplot(df.accel[1:1000,], aes(x = Age, y = Height, colour = girl, group = girl))
# p0 <- p0 + geom_hline(yintercept = 0, linetype = "dashed")
# p0 <- p0 + geom_line()
# p0 <- p0 + geom_line(data = df.accel.mean, linetype = "dashed", colour = "black")
# p0 <- p0 + labs (x = "Age [years]", y = "Height Acceleration [cm/year/year]")
# p0 <- p0 + theme_bw() + ylim(-4,2)
# p0 <- p0 + guides(colour = FALSE)
# p0

#############################
# warping
#############################

#  This is a MANUAL PGS spurt identification procedure requiring
#  a mouse click at the point where the acceleration curve
#  crosses the zero axis with a negative slope during puberty.
#  Here we do this only for the first 10 children.

children <- 1:10

PGSctr <- rep(0,length(children))
for (icase in children) {
  accveci <- eval.fd(age.spec, accelfdUN[icase])
  plot(age.spec,accveci,"l", ylim = c(-6,4),
       xlab = "Year", ylab = "Height Accel.",
       main = paste("Case",icase))
  lines(c(1,18),c(0,0),lty=2)
  PGSctr[icase] <- locator(1)$x
  # **** CLICK ON EACH ACCELERATION CURVE WHERE IT
  # **** CROSSES ZERO WITH NEGATIVE SLOPE DURING PUBERTY
}

#  This is an automatic PGS spurt identification procedure.
#  A mouse click advances the plot to the next case.
#  Compute PGS mid point for landmark registration.
#  Downward crossings are computed within the limits defined
#  by INDEX.  Each of the crossings within this interval
#  are plotted.  The estimated PGS center is plotted as a vertical line.

#  The choice of range of argument values (6--18) to consider
#  for a potential mid PGS location is determined by previous
#  analyses, where they have a mean of about 12 and a s.d. of 1.

#  We compute landmarks for all 54 children

ncasef <- dim(girls)[2]

index <- 1:102  #  wide limits
nindex <- length(index)
ageval <- seq(8.5, 15, len = nindex)
PGSctr <- rep(0, ncasef)
op <- par(ask = TRUE)
for (icase in 1:ncasef) {
  accveci <- eval.fd(ageval, accelfdUN[icase])
  aup <- accveci[2:nindex]
  adn <- accveci[1:(nindex - 1)]
  indx <- (1:102)[adn*aup < 0 & adn > 0]
  plot(ageval[2:nindex], aup, "p",
       xlim = c(7.9, 18), ylim = c(-6, 4))
  lines(c(8,18), c(0,0), lty=2)
  for (j in 1:length(indx)) {
    indxj <- indx[j]
    aupj  <- aup[indxj]
    adnj  <- adn[indxj]
    agej  <- ageval[indxj] + 0.1*(adnj/(adnj-aupj))
    if (j == length(indx)) {
      PGSctr[icase] = agej
      lines(c(agej, agej), c(-4,4), lty = 1)
    } else {
      lines(c(agej, agej), c(-4,4), lty = 3)
    }
  }
  title(paste('Case ',icase))
  # ****** CLICK ON EACH PLOT TO ADVANCE TO THE NEXT
  # ****** {par(ask=TRUE)}
}
par(op)

#  We use the minimal basis function sufficient to fit 3 points
#  remember that the first coefficient is set to 0, so there
#  are three free coefficients, and the data are two boundary
#  values plus one interior knot.
#  Suffix LM means "Landmark-registered".

PGSctrmean <- mean(PGSctr)

#  Define the basis for the function W(t).

wbasisLM <- create.bspline.basis(c(1,18), norder = 3, breaks = c(1, PGSctrmean, 18))
WfdLM <- fd(matrix(0, 4, 1), wbasisLM)
WfdParLM <- fdPar(WfdLM, 1, 1e-12)

#  Carry out landmark registration.

regListLM <- landmarkreg(accelfdUN, PGSctr, PGSctrmean,
                        WfdParLM, monwrd = TRUE)

accelfdLM <- regListLM$regfd     # aligned curves
accelmeanfdLM <- mean.fd(accelfdLM)

warpfdLM <- regListLM$warpfd
warpmatLM <- eval.fd(age.spec, warpfdLM)

#  plot registered curves

plot(accelfdLM, xlim = c(1,18), ylim = c(-4,3), lty = 1, lwd = 1,
     cex = 2, xlab = "Age", ylab = "Acceleration (cm/yr/yr)")
lines(accelmeanfdLM, col = 1, lwd = 2, lty = 2)
lines(c(PGSctrmean,PGSctrmean), c(-4,3), lty = 2, lwd = 1.5)

# ggplot

# df.accel$Registered <- "Observed"
# df.accel.mean$Registered <- "Observed"
# 
# df.accel.reg <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, accelfdLM)), girl = factor(rep(1:54, each = length(age.spec))),
#                            Registered = "Aligned")
# df.accel.mean.reg <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, accelmeanfdLM)), girl = "mean2", Registered = "Aligned")
# 
# x11()
# p0 <- ggplot(df.accel.reg[1:1000,], aes(x = Age, y = Height, colour = girl, group = girl))
# p0 <- p0 + geom_hline(yintercept = 0, linetype = "dashed")
# p0 <- p0 + geom_line()
# p0 <- p0 + geom_line(data = df.accel.mean.reg, linetype = "dashed", colour = "black")
# p0 <- p0 + geom_vline(xintercept = PGSctrmean, linetype = "dashed")
# p0 <- p0 + labs (x = "Age [years]", y = "Height Acceleration [cm/year/year]")
# p0 <- p0 + theme_bw() + ylim(-4,2)
# p0 <- p0 + guides(colour = FALSE)
# p0


# Both registered + unregistered

# df.both <- rbind(df.accel, df.accel.reg)
# df.both.mean <- rbind(df.accel.mean, df.accel.mean.reg)
# 
# df.both$Registered <- factor(df.both$Registered, levels = c("Observed", "Aligned"))
# df.both.mean$Registered <- factor(df.both.mean$Registered, levels = c("Observed", "Aligned"))
# 
# x11()
# p0 <- ggplot(subset(df.both, is.element(girl, as.character(1:10))), aes(x = Age, y = Height, colour = girl, group = girl))
# p0 <- p0 + geom_hline(yintercept = 0, linetype = "dashed")
# p0 <- p0 + geom_line()
# #p0 <- p0 + geom_vline(xintercept = PGSctrmean, linetype = "dashed")
# p0 <- p0 + geom_line(data = df.both.mean, linetype = "dashed", colour = "black")
# p0 <- p0 + labs (x = "Age [years]", y = "Height Acceleration [cm/year/year]")
# p0 <- p0 + theme_bw() + ylim(-4,2)
# p0 <- p0 + guides(colour = FALSE)
# p0 <- p0 + facet_wrap( ~ Registered)
# p0


# plot all 10 warping functions

plot(regListLM$warpfd[1:10])

# ggplot
# 
# df.warp <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, warpfdLM)), girl = factor(rep(1:54, each = length(age.spec))),
#                            Registered = "Aligned", Type = "Warping functions")
# df.both$Type <- "Result"
# 
# x11()
# p0 <- ggplot(subset(df.warp, is.element(girl, as.character(1:10))), aes(x = Age, y = Height, colour = girl, group = girl))
# p0 <- p0 + geom_line()
# p0 <- p0 + geom_abline(slope = 1, intercept = 0, linetype = "dashed")
# p0 <- p0 + geom_vline(xintercept = PGSctrmean, linetype = "dashed")
# p0 <- p0 + labs (x = "Age [years]", y = "Age [years]")
# p0 <- p0 + theme_bw()
# p0 <- p0 + guides(colour = FALSE)
# p0


#  plot warping functions for cases 3 and 7

op = par(mfrow=c(2,2))
plot(accelfdUN[3], xlim=c(1,18), ylim=c(-3,1.5), lty=1, lwd=2,
     xlab="", ylab="")
lines(accelfdLM[3], lty = 2, lwd = 2)
abline(v = PGSctrmean, lty = 2, lwd = 1)
plot(age.spec, warpmatLM[,3], "l", lty=1, lwd=2, col=1, cex=1.2,
     xlab="", ylab="")
lines(age.spec,  age.spec, lty=2, lwd=1.5)
lines(c(PGSctrmean,PGSctrmean), c(1,18), lty=2, lwd=1.5)
text(PGSctrmean+0.1, warpmatLM[61,3]+0.3, "o", lwd=2)

plot(accelfdUN[7], xlim=c(1,18), ylim=c(-3,1.5), lty=1, lwd=2,
     xlab="", ylab="")
lines(accelfdLM[7], lty = 2, lwd = 2)
abline(v = PGSctrmean, lty = 2, lwd = 1)
plot(age.spec, warpmatLM[,7], "l", lty=1, lwd=2, col=1, cex=1.2,
     xlab="", ylab="")
lines(c(PGSctrmean,PGSctrmean), c(1,18), lty=2, lwd=1.5)
lines(age.spec,  age.spec, lty=2, lwd=1.5)
text(PGSctrmean+0.1, warpmatLM[61,7]+0.2, "o", lwd=2)
par(op)

# ggplot

# upper

# x11()
# p0 <- ggplot(subset(df.both, is.element(girl, as.character(c(3,7)))), 
#              aes(x = Age, y = Height, colour = girl, group = interaction(girl, Registered), linetype = Registered))
# p0 <- p0 + geom_hline(yintercept = 0, linetype = "dashed")
# p0 <- p0 + geom_line()
# p0 <- p0 + geom_vline(xintercept = PGSctrmean, linetype = "dashed")
# p0 <- p0 + labs (x = "Age [years]", y = "Height Acceleration [cm/year/year]")
# p0 <- p0 + labs(linetype = "Result") + guides(colour  = FALSE)
# p0 <- p0 + facet_wrap(girl ~ ., ncol = 2, labeller = as_labeller(c("3" = "girl 3", "7" = "girl 7")))
# p0 <- p0 + theme_bw() + ylim(-4,2) + theme(legend.position = c(0.5, 0.9), 
#                                            legend.background = element_rect(size=0.5, linetype="solid", colour = "black"))
# p0

# bottom

# x11()
# p0 <- ggplot(subset(df.warp, is.element(girl, as.character(c(3, 7)))), aes(x = Age, y = Height, colour = girl, group = girl))
# p0 <- p0 + geom_line()
# p0 <- p0 + geom_abline(slope = 1, intercept = 0, linetype = "dashed")
# p0 <- p0 + geom_vline(xintercept = PGSctrmean, linetype = "dashed")
# p0 <- p0 + labs (x = "Age [years]", y = "Age [years]")
# p0 <- p0 + theme_bw()
# p0 <- p0 + guides(colour = FALSE)
# p0 <- p0 + facet_wrap(girl ~ ., ncol = 2, labeller = as_labeller(c("3" = "girl 3", "7" = "girl 7")))
# p0


#  Comparing unregistered to landmark registered curves

AmpPhasList <- AmpPhaseDecomp(accelfdUN, accelfdLM, warpfdLM, c(3,18))
MS.amp <- AmpPhasList$MS.amp
MS.pha <- AmpPhasList$MS.pha
RSQRLM <- AmpPhasList$RSQR  #the squared correlation measure of the proportion of the total variation that is due to phase variation.
CLM <- AmpPhasList$C   # a constant required for the decomposition. Its value is one if the derivatives the warping functions are independent of the squared registered functions.

print(paste("Total MS =",     round(MS.amp+MS.pha,2),
            "Amplitude MS =", round(MS.amp,2),
            "Phase MS =",     round(MS.pha,2)))

#  [1] "Total MS = 7.06 Amplitude MS = 2.12 Phase MS = 4.95"

print(paste("R-squared =", round(RSQRLM,3), ",  C =", round(CLM,3)))

#  "R-squared = 0.7 ,  C = 0.984"

#########################
# Continuous registration
#########################

#  Set up a cubic spline basis for continuous registration

nwbasisCR <- 15
norderCR <- 5
wbasisCR <- create.bspline.basis(c(1, 18), nwbasisCR, norderCR)
Wfd0CR <- fd(matrix(0, nwbasisCR, ncasef), wbasisCR)
lambdaCR <- 1
WfdParCR <- fdPar(Wfd0CR, 1, lambdaCR)

#  carry out the registration

registerlistCR <- register.fd(accelmeanfdLM, accelfdLM, WfdParCR)

accelfdCR <- registerlistCR$regfd
warpfdCR <- registerlistCR$warpfd
WfdCR <- registerlistCR$Wfd

accelmeanfdCR <- mean.fd(accelfdCR)

#  plot landmark and continuously registered curves for the
#  first 10 children

accelmeanfdLM10 <- mean.fd(accelfdLM[children])
accelmeanfdCR10 <- mean.fd(accelfdCR[children])

op <- par(mfrow=c(1, 2))
plot(accelfdLM[children], xlim=c(1,18), ylim=c(-3,1.5), lty=1, lwd=1,
     cex=2, xlab="Age (Years)", ylab="Acceleration (cm/yr/yr)", main = "Landmark registration")
lines(accelmeanfdLM10, col=1, lwd=2, lty=2)
lines(c(PGSctrmean,PGSctrmean), c(-3,1.5), lty=2, lwd=1.5)
plot(accelfdCR[children], xlim=c(1,18), ylim=c(-3,1.5), lty=1, lwd=1,
     cex=2, xlab="Age (Years)", ylab="Acceleration (cm/yr/yr)", main = "Continuous registration")
lines(accelmeanfdCR10, col=1, lwd=2, lty=2)
lines(c(PGSctrmean,PGSctrmean), c(-3,1.5), lty=2, lwd=1.5)
par(op)


# ggplot

# df.accel.conreg <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, accelfdCR)), girl = factor(rep(1:54, each = length(age.spec))),
#                            Registered = "Continuous Registration")
# df.accel.mean.conreg <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, accelmeanfdCR)), girl = "mean3", Registered = "Continuous Registration")
# 
# df.accel.reg$Registered <- "Landmark Registration"
# df.accel.mean.reg$Registered <- "Landmark Registration"
# 
# df.both.CR <- rbind(df.accel.reg, df.accel.conreg)
# df.both.mean.CR <- rbind(df.accel.mean.reg, df.accel.mean.conreg)
# 
# df.both.CR$Registered <- factor(df.both.CR$Registered, levels = c("Landmark Registration", "Continuous Registration"))
# df.both.mean.CR$Registered <- factor(df.both.mean.CR$Registered, levels = c("Landmark Registration", "Continuous Registration"))
# 
# x11()
# p0 <- ggplot(subset(df.both.CR, is.element(girl, as.character(1:10))), aes(x = Age, y = Height, colour = girl, group = girl))
# p0 <- p0 + geom_hline(yintercept = 0, linetype = "dashed")
# p0 <- p0 + geom_line()
# p0 <- p0 + geom_vline(xintercept = PGSctrmean, linetype = "dashed")
# p0 <- p0 + geom_line(data = df.both.mean.CR, linetype = "dashed", colour = "black")
# p0 <- p0 + labs (x = "Age [years]", y = "Height Acceleration [cm/year/year]")
# p0 <- p0 + theme_bw() + ylim(-4,2)
# p0 <- p0 + guides(colour = FALSE)
# p0 <- p0 + facet_wrap( ~ Registered)
# p0

# plot all 10 warping functions

plot(registerlistCR$warpfd[1:10])

# ggplot

# warpfdCR <- registerlistCR$warpfd
# warpmatCR <- eval.fd(age.spec, warpfdCR)
# 
# 
# df.warp.CR <- data.frame(Age = age.spec, Height = c(eval.fd(age.spec, warpfdCR)), girl = factor(rep(1:54, each = length(age.spec))),
#                       Registered = "Continuous Registration", Type = "Warping functions")
# df.both.CR$Type <- "Result"
# 
# x11()
# p0 <- ggplot(subset(df.warp.CR, is.element(girl, as.character(1:10))), aes(x = Age, y = Height, colour = girl, group = girl))
# p0 <- p0 + geom_line()
# p0 <- p0 + geom_abline(slope = 1, intercept = 0, linetype = "dashed")
# p0 <- p0 + labs (x = "Age [years]", y = "Age [years]")
# p0 <- p0 + theme_bw()
# p0 <- p0 + guides(colour = FALSE)
# p0


##
## A Decomposition into Amplitude and Phase Sums of Squares
##

#  Comparing landmark to continuously registered curves

AmpPhasList <- AmpPhaseDecomp(accelfdLM, accelfdCR, warpfdCR, c(3,18))
MS.amp <- AmpPhasList$MS.amp
MS.pha <- AmpPhasList$MS.pha
RSQRCR <- AmpPhasList$RSQR
CCR <- AmpPhasList$C

print(paste("Total MS =",     round(MS.amp+MS.pha,2),
            "Amplitude MS =", round(MS.amp,2),
            "Phase MS =",     round(MS.pha,2)))

#  "Total MS = 1.5 Amplitude MS = 1.6 Phase MS = -0.1"

print(paste("R-squared =", round(RSQRCR,3), ",  C =", round(CCR,3)))

#  "R-squared = -0.067 ,  C = 1.001"

########################
# Problems
######################

#  set up a fine mesh of t-values for plotting, and define mu and sigma

tvec <- seq(-5, 5, length.out = 201)
mu <- seq(-1, 1, length.out = 5)
sigma <- (1:5)/3

#  Here is function Dgauss that we use below to compute values
#  of the first derivative of a Gaussian density:

DGauss <- function(tvec, mu, sigma)
{
  var <- as.matrix(sigma)^2
  n <- length(tvec)
  m <- length(mu)
  if (length(sigma) != m)
    stop('MU and SIGMA not of same length')
  tvec <- as.matrix(tvec)
  mu <- as.matrix(mu)
  onesm <- matrix(1,1,m)
  onesn <- matrix(1,n,1)
  res <- tvec %*% onesm - onesn %*% t(mu)
  expon <- res^2/(2*onesn %*% t(var))
  DpG <- -res*exp(-expon)
  return(DpG)
}

#  Data to be plotted in the left panel

DpGphase <- matrix(0, 201, 5)
for (i in 1:5){
  DpGphase[,i] <- DGauss(tvec + mu[i], 0, 1)
  DpGphaseMean <- apply(DpGphase, 1, mean)
}
#  Data to be plotted in the right panel

DpGampli <- matrix(0, 201, 5)
for (i in 1:5){
  DpGampli[,i] <- sigma[i]*DGauss(tvec, 0, 1)
  DpGampliMean <- apply(DpGampli, 1, mean)
}

op <- par(mfrow = c(1, 2), cex = 1, ask = FALSE)
#  top panel
matplot(tvec, DpGphase, "l", lwd = 1, col = 1, lty = 1,
        xlim = c(-5, 5), ylim = c(-0.8, 0.8),
        xlab = "", ylab = "")
lines(tvec, DpGphaseMean, col = 1, lty = 2, lwd = 4)
lines(c(-5, 5), c(0, 0), col = 1, lty = 3, lwd = 1)
# bottom panel
matplot(tvec, DpGampli, "l", lwd = 1, col = 1, lty = 1,
        xlim = c(-5, 5), ylim = c(-1.2, 1.2),
        xlab = "", ylab = "")
lines(tvec, DpGampliMean, col = 1, lty = 2, lwd = 4)
lines(c(-5, 5), c(0, 0), col = 1, lty = 3, lwd = 1)
par(op)

# ggplot
# df.example <- data.frame(x = tvec, y = c(DpGphase, DpGampli), sample = factor(rep(c(1:5, 1:5), each = length(tvec))),
#                          type = factor(rep(c("Phase variation", "Amplitude variation"), each = 5*length(tvec)), ordered = TRUE, levels = c("Phase variation", "Amplitude variation")))
# df.example.mean <- data.frame(x =tvec, y = c(DpGphaseMean, DpGampliMean), sample = "mean", 
#                               type = factor(rep(c("Phase variation", "Amplitude variation"), each = length(tvec)), ordered = TRUE, levels = c("Phase variation", "Amplitude variation")))
# 
# x11()
# p0 <- ggplot (data = df.example, aes (x = x, y = y, group = sample, colour = sample))
# p0 <- p0 + geom_hline (yintercept = 0, linetype = "dashed", colour = "black")
# p0 <- p0 + geom_line ()
# p0 <- p0 + geom_line (data = df.example.mean, colour = "black", linetype = "dashed")
# p0 <- p0 + labs (x = "", y = "")
# p0 <- p0 + facet_wrap( ~ type)
# p0 <- p0 + guides (colour = FALSE) 
# p0 <- p0 + theme_bw ()
# print (p0)

# EXAMPLE
# Do 3 steps of continuous registration on simulated data from example
# a) Do it for the case of Phase variation    
# b) Do it for the case of Amplitude variation

# setting to FD
pokbasis <- create.fourier.basis(rangeval = range(tvec), nbasis = 13)
pokfd <- Data2fd(tvec, DpGphase, pokbasis)
pokmeanfd <- mean.fd(pokfd)

df0 <- data.frame(t = tvec, y = c(DpGphase), sample = factor(rep(c(1:5), each = length(tvec))), Registered = "Unregistered")
df0.mean <- data.frame(t = tvec, y = DpGphaseMean, sample = "mean0", Registered = "Unregistered")

#  Set up a cubic spline basis for continuous registration

nwbasisPCR <- 15
norderPCR <- 5
wbasisPCR <- create.bspline.basis(c(-5, 5), nwbasisPCR, norderPCR)
Wfd0PCR <- fd(matrix(0, nwbasisPCR, 5), wbasisPCR)
lambdaPCR <- 1
WfdParPCR <- fdPar(Wfd0PCR, 1, lambdaPCR)

#  carry out the registration...


####################
### Pinch Force Data
####################

x11()
matplot (pinchtime, pinchraw, type = "l", lty = 1, cex=2,
         lwd = 1,  xlab = "Seconds", ylab = "Force (N)")

#x <- pinchtime
x <- 0:150
y <- pinchraw