# (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.
# (11) Produce a plot that identifies the order of the individual landmarks by color and connects
#      their centroids with a thick red line. 


 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 
                 }
}
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)) {
plot(Darray[,,1], Darray[,,2], main = paste("Raw data plotted for", file), type ="n")
for (i in 1:skount) {
  points(Darray[i,,1], Darray[i,,2], col="black", bg=colors()[1 +i*3], pch = 21)
}

# }

  browser()

# ************  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
#
op<- par( pty = "m")
plot(Darray[,,1], Darray[,,2], main = paste("Translated and scaled", file), type="n")

for (i in 1:skount) {
  points(Darray[i,,1], Darray[i,,2], col="black", bg=colors()[1+i*3], pch = 21)
                 }
cat("Now allign files on the first specimen. \n")
  cat("Function bowser() is allowing you to execute R functions to explore the objects at point 1 \n")
  browser()
#  pause()                         #200

# (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
                     }
plot(AFarray[,,1], AFarray[,,2], main = paste("Landmarks Afine fit for", file), type="n")
 for (i in 1:skount) {
  points(AFarray[i,,1], AFarray[i,,2], col="black", bg=colors()[1+i*3], pch = 21)
                     }
  points(AVG[,1], AVG[,2], col="red", pch = 3, cex = 3)
   if (identical(idyes, TRUE)) {identify(AVG[,1], AVG[,2], col="red", offset=1, cex = 2)}

# (7) introduce splining .
# Spline calculations

  cat("Function bowser() is allowing you to execute R functions to explore the objects at point 4 \n")
  browser()

#  pause()
SParray<- 0*HDarray
plot(AFarray[,,1], AFarray[,,2], main = paste("Landmarks Rohlf Spline fit for", file), type="n")
  points(AVG[,1], AVG[,2], col="red", pch = 1, cex = 3)

 for (ispec in 1:skount) {
# Compute the pXp K matrix of Rohlf (=Pk matrix of Bookstein) on AVG
# p = lk, the number of landmarks.
 r<- matrix(0,ncol=lk,nrow=lk)
 z<- r
 P<- matrix(1,ncol=3, nrow=lk)
 V<- P

X2<- HDarray[ispec,,]
for (i in 1:lk) {for (j in 1:lk) {
  if (!identical(i,j)) 
 #  {r[i,j]<- ((AVG[i,1]-AVG[j,1])^2 + (AVG[i,2]-AVG[j,2])^2)^0.5
    {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)
                       }
    }}
KI<- ginv(z)
V[,2:3]<- AVG
P[,2:3]<- X2
Q<- t(V)%*% KI %*% P %*% ginv(t(P)%*% KI %*% P)
 T<-Q[2:3,1]
 H<-Q[2:3,2:3]
X2p<- t(T + H%*% t(X2))
points(X2p[,1], X2p[,2], col="blue", bg=colors()[1 +ispec*3], pch = 21, cex = 0.8)
SParray[ispec,,]<- X2p
                   }  # end of plotting Rohlf splined data
  points(AVG[,1], AVG[,2], col="red", pch = 1, cex = 4)

  cat("Function bowser() is allowing you to execute R functions to explore the objects at point 5 \n")
  browser()
  
  rm(Q, T, H)
#  pause()

# 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)

 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)
                       }
    }}
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] 

 points(X2p[,1], X2p[,2], col="blue", bg=colors()[1 +ispec*3], pch = 21, cex = 0.8)
   }
  points(AVG[,1], AVG[,2], col="red", pch = 1, cex = 4)
#  pause()

  browser()

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

# Use Booksteins 1991 Equations section 2.2.4 page 33.
plot(AFarray[,,1], AFarray[,,2], main = paste("Colored order of landmarks for", skount, "sets from", file), type="n")
  points(AVG[,1], AVG[,2], col="red", pch = 1, cex = 3)
#  ****************************
dcrit <- 0.41   # more than 0.40   less than 0.5
#  ****************************
 for (ispec in 1:skount) {
 L<- matrix(0,ncol=lk+3,nrow=lk+3)

X2<- HDarray[ispec,,]
dist<-0
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)
                        }
                 }
                 
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,9)], 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")}
HDarray[ispec,,]<- X2p
   }
  points(AVG[,1], AVG[,2], col="red", pch = 1, cex = 4)
  lines(AVG[,1], AVG[,2], col="red", lwd = 4)

par(op)

## Now design a file saving routine for AFarray[i,j,l].
#filnam<- strsplit(file, '.tps')
junk<- strsplit(file, '.tps')
dtafilnam<- paste(junk[[1]], '.dta', sep="")
angfilnam<- paste(junk[[1]], '.ang', sep="")

#cat("31 \n", file= filnam)
cat("", file= dtafilnam)
for (i in 1:skount) {cat(recnams[i],sep = " ", file= dtafilnam, append= TRUE)
                     for (j in 1:lk) {cat(AFarray[i,j,1]," ", AFarray[i,j,2], " ", file= dtafilnam, append= TRUE)}
                     cat("\n", file= dtafilnam, append= TRUE)
                    }
                    
cat("", file= angfilnam)
cat("filnam ", file= angfilnam, append=TRUE)
for (j in 1:lk) {cat("X",j," ", "Y",j, " ",sep="", file= angfilnam, append= TRUE)}
cat("\n", file= angfilnam, append=TRUE)
for (i in 1:skount) {cat(recnams[i],sep = " ", file= angfilnam, append= TRUE)
                     for (j in 1:lk) {cat(HDarray[i,j,1]," ", HDarray[i,j,2], " ", file= angfilnam, append= TRUE)}
                     cat("\n", file= angfilnam, append= TRUE)
                    }
                    
rm(Q, Y, W, op, Linver, Wout) # end