# (1) Read a tps file type and parse it into landmark and outline data
# (2) Plot the data.
# (3) center and scale landmark data.
# (4) rotate to best fit and plot landmakr data.
# (5) afine transform and plot landmark data.
# (6) introduce the identify points interactive function.
# (7) introduce splining.
# (8) Save the AVG file.
# (9) Install browse() to allow object inspection at set points.
# (10) Retrieve image file names from tps file to append to data.


 require(MASS)       # includes ginv() function
 require(session)    # includes texteval() function
 library(clim.pact)
# Function Deffinition Section

# trace of a matrix function
tr<- function(X) {
dim<- attributes(X)[[1]][1]
V<- 0*(1:dim)
for (i in 1:dim) { V[i]<- X[i,i] }
 out<- sum(V)
  }

# pause function
pause<- function() {
cat("Push return to continue!")
indata<- readLines(con=stdin(), n=1, ok = TRUE)
  }

# Ask function
ask<- function(text) {
cat(text,": ", sep="")
indata<- readLines(con=stdin(), n=1, ok = TRUE)
                     }

# Make P-inverse matrix
PImat<- function(n) {
I<- matrix(1/n, ncol=n, nrow=n)
I<- diag(n)-I
         }

# End of Function Deffinition Section
 idyes = FALSE     # turn off identify points function  #40
 plotyes = FALSE
cat("Directory of tps files \n")   
lsout<- system("ls *.tps", TRUE, TRUE)
print(lsout)
file= ask("Enter a tps file name")
# file = "sneathd.tps"
nskip<- 0
nrec<-0
skount<- 0
cat("", file="recnames")
# plot it
     op <- par(mfrow = c(2, 3), # 2 x 3 pictures on one plot
               pty = "s")       # square plotting region,
junk<- readLines(file,  n = -1, ok = TRUE)
filength<- length(junk)
cat("File lines =", filength, "\n")
while (nskip < filength) {
skount<- skount + 1
rm(junk)
if(identical(plotyes, TRUE)) { cat("nskip(1) =", nskip, "of", filength, "\n")}
# Read the file header to find out landmark number lm
junk<- scan(file, skip=nskip, nlines = 1, what = "c", quiet=TRUE)
if ( !identical((1*grep("lm=", junk)), 0) ) {
 junk<- sub("lm=", "LM=", junk) }
out<- strsplit(junk, 'LM=')
lk<- out[[1]][2]
rm(junk)
out<- texteval(paste("lk<-", lk))
rm(out)
nskip<- nskip+1
# Read the file for lk lines of data landmarks.
Data<- matrix(scan(file, skip = nskip, nlines = lk, quiet=TRUE), ncol = lk, nrow = 2)
if (identical(skount, 1)) { Darray<- array(0, c(500, lk, 2))}
nskip<- nskip+lk
if(identical(plotyes, TRUE)) { cat("nskip(2) =", nskip, "\n") }
#                                                     }
# Read the file for image file and skip it
#   
junk<- scan(file, skip=nskip, nlines = 1, what = "c", quiet=TRUE)
if ( !identical((1*grep("image=", junk)), 0) ) {
 junk<- sub("image=", "IMAGE=", junk) }
if(identical(plotyes, TRUE)) { cat(junk, "\n")}     #80
if (!identical((1*grep("IMAGE=", junk)),0) ) {nskip<- nskip +1
                                              nrec<- nrec+1 
if(identical(plotyes, TRUE)) { cat("nskip(3) =", nskip, "\n")
                                cat(junk, "\n")}}
junk<- strsplit(junk, 'IMAGE=')
junk<- junk[[1]][2]
cat(nrec, junk, "\n")    # Trying to get file names
cat(junk,"\n", file="recnames", append=TRUE)

rm(junk) 
#
# Read the file for file id name
# line 54
junk<- scan(file, skip = nskip, nlines = 1, what="c", quiet=TRUE)
if ( !identical((1*grep("id=", junk)), 0) ) {
 junk<- sub("id=", "ID=", junk) }
out<- strsplit(junk, 'ID=')
nam<- out[[1]][2]
rm(out, junk)
if(identical(plotyes, TRUE)) { cat("name:", nam, "\n")
plot(Data[1,], Data[2,], main = nam)
                             }
Darray[skount, ,]<- t(Data)
nskip<- nskip+1
if(identical(plotyes, TRUE)) { cat("nskip(4) =", nskip, "\n") }
 if (!identical((filength-nskip),0)) {
#
# read one line to find number of discrete outline data curves 
# 69                                                     }
# Read the file for image file and skip it
  junk<- scan(file, skip=nskip, nlines = 1, what = "c", quiet=TRUE)
if ( !identical((1*grep("CURVES=", junk)), 0) ) {
 junk<- sub("CURVES=", "curves=", junk) }

  if (!identical((regexpr("curves=", junk)[1] + 1), 0)) { 
    rm(junk)
    junk<- scan(file, skip = nskip, nlines = 1, what="c", quiet=TRUE)
    if ( !identical((1*grep("CURVES=", junk)), 0) ) {
     junk<- sub("CURVES=", "curves=", junk) }
     nskip<- nskip+1
     cat("nskip(5) =", nskip, "\n")
     out<- strsplit(junk, 'curves=')
     nc<- out[[1]]
     cat("Number of outline curves:", nc, "\n")
     rm(junk)
  junk<- texteval(paste("nc<-", nc))      #120
# loop to read nc outline curves 
   for (i in 1:nc) {
    junk<- scan(file, skip = nskip, nlines = 1, what="c", quiet=TRUE, quiet=TRUE)
    nskip<- nskip+1
if ( !identical((1*grep("POINTS=", junk)), 0) ) {
 junk<- sub("POINTS=", "points=", junk) }
 cat("nskip(6) =", nskip, "\n")
    junk<- strsplit(junk[1], 'points=')
    no<- junk[[1]][2]
    rm(junk)
    junk<- texteval(no<- paste("no<-", no))
    Outlin<- matrix(scan(file, skip = nskip, nlines = no, quiet=TRUE), ncol = no, nrow = 2)
    lines(Outlin[1,],Outlin[2,]) 
    nskip<- nskip+no
if(identical(plotyes, TRUE)) {
 cat("nskip(7) =", nskip, "\n")
                              }
    rm(no, nc, junk, i)
                      }
                        } # end of outline points 
#    rm(no, nc, junk, i)
                 }
}
recnams<- readLines("recnames",  n = -1, ok = TRUE)
nnams<- length(recnams)
cat("recnams has", nnams, "names \n")

# plot it     line 100
     op <- par(mfrow = c(1, 1), # 2 x 3 pictures on one plot
               pty = "m")       # m  for flexible plotting region,
Sarray<- Darray[1:skount,,]
rm(Darray)
Darray<- Sarray
rm(Sarray)
#if(identical(plotyes, TRUE)) {

# ************  imported    *** line 113  *****************************************
# ************    from      *******************************************************
# ************ getmosqNTS.R *******************************************************  #160
# (3) center and scale landmark data.
cat("Now center, scale and plot the landmarks. \n")

PI<- PImat(lk)
AVG<- matrix(0, nrow=lk, ncol= 2)

#
# Center landmarks
#

for (i in 1:skount) {
  X<- Darray[i,,]
# cat("Raw X =\n")
# Center landmarks
#    X'i = (I - P) Xi
# cat("Centered X = XP =\n")
  XP<- PI%*%X
# Scale and Plot landmarks
#    X'i = (I - P) Xi / si
#  cat("Computed si","\n")
# print( si<- (tr(XP%*%t(XP)))^0.5)
 si<- (tr(XP%*%t(XP)))^0.5
#
#       X = V' + cos(rho) Xc'
#
 Darray[i,,] <- XP/si
                 }
#
#  Plot Centered landmarks
#

# (4) rotate to best fit and plot landmark data.
# Take pairs and allign them on first subject
  HDarray<- 0*Darray
  HDarray[1,,] <- X1<- Darray[1,,]

for (i in 2:skount) {
   X2<- Darray[i,,]
  X2a<- svd(t(X1)%*%X2)
  X2<- X2 %*% X2a$v %*% diag(2) %*% t(X2a$u)
   HDarray[i,,]<- X2
                }
plot(HDarray[,,1], HDarray[,,2], main = paste("Landmarks Rotated and averaged (cross) for", file), type="n")
 for (i in 1:skount) {
  points(HDarray[i,,1], HDarray[i,,2], col="black", bg=colors()[1+i*3], pch = 21)
  AVG<-AVG + HDarray[i,,]
                     }
cat("Now plot the average. \n")

#  pause()
  AVG<- AVG/skount
  points(AVG[,1], AVG[,2], col="yellow", pch = 3, cex = 4)

 if (identical(idyes, TRUE)) { 
   cat("Use a left-mouse-click to identify the index of averages. \n End labeling with a right-mouse-click!\n")
   identify(AVG[,1], AVG[,2], col="red", offset=2)
                             }
  cat("Function bowser() is allowing you to execute R functions to explore the objects at point 3 \n")
  browser()

# *******************************************************************
# (5) afine transform and plot landmark data.
     op <- par(mfrow = c(1, 1), # 1 x 1 pictures on one plot
               pty = "m")       # rectangular plotting region
 AFarray<- 0*HDarray
 
 for (i in 1:skount) {
   X1<- HDarray[i,,]
   X2<- X1 %*% ginv(t(X1)%*%X1)%*%t(X1)%*%AVG
   AFarray[i,,]<- X2
                     }
# (7) introduce splining .
# Spline calculations

cat("Now plot each landmark with a different color! Is each color together? ", "\n")
rainboc<- c("red", "yellow", "orange", "green", "blue", "purple", "black")

# Use Booksteins 1991 Equations section 2.2.4 page 33.
plot(AFarray[,,1], AFarray[,,2], main = paste("Bookstein Spline fit (afine part) for", file), type="n")
  points(AVG[,1], AVG[,2], col="red", pch = 1, cex = 3)
#  ****************************
dcrit <- 0.4    # more than 0.38   less than 0.5
#  ****************************
 for (ispec in 1:skount) {
 L<- matrix(0,ncol=lk+3,nrow=lk+3)

X2<- HDarray[ispec,,]
for (i in 1:lk) {for (j in 1:lk)
  if (!identical(i,j)) {
    r[i,j]<- ((X2[i,1]-X2[j,1])^2 + (X2[i,2]-X2[j,2])^2)^0.5
    z[i,j]<- (r[i,j]^2)*log(r[i,j]^2)
                       }
#    dist<- dist + ((X2[i,1]-AVG[i,1])^2 + (X2[i,2]-AVG[i,2])^2)^0.5                        
                  }
                        
L[1:lk,1:lk]<- z
 Q<- matrix(1,ncol=3, nrow=lk)
 Y<- matrix(0,ncol=lk+3, nrow=1)
 W<- matrix(0,ncol=lk, nrow=2)

 Wall<- matrix(0,ncol=lk+3, nrow=2)

 Q[,2:3]<-X2
L[1:lk,lk+(1:3)]<- Q
L[lk+(1:3), 1:lk]<- t(Q)
 Linver<- ginv(L)
 Y[,1:lk]<- t(AVG[,1])
 Wout<- Linver%*% t(Y)
 W[1,]<- Wout[1:lk]
 Wall[1,]<- Wout  
 Y[,1:lk]<- t(AVG[,2])
 Wout<- Linver%*% t(Y)
 W[2,]<- Wout[1:lk]
 Wall[2,]<- Wout
  X2p[,1]<- Wall[1,lk+1]+  Wall[1,lk+2]* X2[,1] + Wall[1,lk+3]* X2[,2] 
  X2p[,2]<- Wall[2,lk+1]+  Wall[2,lk+2]* X2[,1] + Wall[2,lk+3]* X2[,2] 
dist<- 0.0
for (lki in 1:lk) {
 points(X2p[lki,1], X2p[lki,2], col=rainboc[mod(lki,7)], pch = 21, cex = 0.8)
   dist<- dist + ((X2p[lki,1]-AVG[lki,1])^2 + (X2[lki,2]-AVG[lki,2])^2)^0.5
                  }
if(dist > dcrit) {cat(recnams[ispec], "over \n")}
                  
   }
  
  points(AVG[,1], AVG[,2], col="red", pch = 1, cex = 4)
  
par(op)

## Now design a file saving routine for AFarray[i,j,l].
#filnam<- paste("Asiilid", ".dta", sep="")

#cat("31 \n", file= filnam)
#for (i in 1:31) { cat(attr(junk, "dimnames")[[1]][i]," ", file= filnam, append= TRUE)
#                  for (j in 1:31) {cat(AFarray[i,j]," ", file= filnam, append= TRUE)}
#                                   cat("\n", file= filnam, append= TRUE)}
#                }


rm(Q, Y, W, filength, nskip, op, resultText, Linver, Wout) # end