# (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 landmark data.
# (5) afine transform and plot landmark data.
# (6) introduce the identify points interactive function.
# (7) introduce splining .
# Derived from R-script getsplTPS.R
 require(MASS)       # includes ginv() function
 require(session)    # includes texteval() function
# Function Deffinition Section
Qwidth<- 8; Qheight<- 5
# 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)
  }

# 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
 plotyes = FALSE
cat("Directory of tps files \n")
print(dir(patt=".tps"))
# system('ls *.tps')
file= ask("Enter a tps file name")
# file = "sneathd.tps"
nskip<- 0
skount<- 0
# 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(250, 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")}
if (!identical((1*grep("IMAGE=", junk)),0) ) {nskip<- nskip +1
if(identical(plotyes, TRUE)) { cat("nskip(3) =", nskip, "\n")
                                cat(junk, "\n")}} 
rm(junk) 
#
# Read the file for file id name
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 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))
# 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)
                 }
}
# 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)
quartz(width=Qwidth, height=Qheight)

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

# ************  imported    *******************************************
# ************    from      *******************************************
# ************ getmosqNTS.R *******************************************
# (3) center and scale landmark data.
cat("Now center, scale and plot the landmarks. \n")
browser()
quartz(width=Qwidth, height=Qheight)

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

#
# Center landmarks
#

for (i in 1:skount) {
  X<- Darray[i,,]
  XP<- PI%*%X
 si<- (tr(XP%*%t(XP)))^0.5
#
 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")
browser()
quartz(width=Qwidth, height=Qheight)

# (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")
  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)
                             }
# *******************************************************************
# plot it
# (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
browser()
quartz(width=Qwidth, height=Qheight)
 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

browser()
quartz(width=Qwidth, height=Qheight)
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]<- ((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)
                       }
    }}
 # cat("Rohlf's K matrix\n")
 # print(z)
 # cat("K inverse matrix\n")
KI<- ginv(z)
 # print(KI)
 # V[,2:3]<- AVG
 # P[,2:3]<-X2
V[,2:3]<- AVG
P[,2:3]<- X2
Q<- t(V)%*% KI %*% P %*% ginv(t(P)%*% KI %*% P)

# cat("Q matrix of Rohlf", ispec,"\n")
# print(Q)

 T<-Q[2:3,1]
 H<-Q[2:3,2:3]
X2p<- t(T + H%*% t(X2))
#  browser()

#  points(X2[,1], X2[,2], col="green", pch = 2, cex = 2)
  points(X2p[,1], X2p[,2], col="blue", bg=colors()[1 +ispec*3], pch = 21, cex = 0.8)
SParray[ispec,,]<- X2p
# cat("Q matrix printed", ispec, "\n")

                   }  # end of plotting Rohlf splined data
  points(AVG[,1], AVG[,2], col="red", pch = 1, cex = 4)
# rm(Q, T, H)
  browser()

quartz(width=Qwidth, height=Qheight)
# 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
# cat("W matrix of Bookstein's\n")
  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)
par(op)

rm(filength, lk, nskip, op, file, resultText) # end